Your second application using semantic checking on geometric relations including coordinate checking

This tutorial assumes you have prepared a ROS package with name myApplication and that you have set your ROS_PACKAGE_PATH environment variable accordingly, as explained in this tutorial.

In this tutorial we first explain how you can create basic semantic objects (without coordinates but with coordinate checking) and perform semantic operations on them. We will show how you can create any of the supported geometric relations: position, orientation, pose, transational velocity, rotational velocity, twist, force, torque, and wrench.

Remark that the file resulting from following this tutorial is attached to this wiki page for completeness.

Prepare the main file

Prepare a mySecondApplication.cpp main file as explained in this tutorial.
Edit the C++ file with your favorite editor. For instance:

vim mySecondApplication.cpp

Include the necessary headers.

#include <Position/PositionCoordinatesSemantics.h>
#include <Orientation/OrientationCoordinatesSemantics.h>
#include <Pose/PoseCoordinatesSemantics.h>
#include <LinearVelocity/LinearVelocityCoordinatesSemantics.h>
#include <AngularVelocity/AngularVelocityCoordinatesSemantics.h>
#include <Twist/TwistCoordinatesSemantics.h>
#include <Force/ForceCoordinatesSemantics.h>
#include <Torque/TorqueCoordinatesSemantics.h>
#include <Wrench/WrenchCoordinatesSemantics.h>

Next we use the geometric_semantics namespace for convenience:

using namespace geometric_semantics;

Create a main program:

int main (int argc, const char* argv[])
{
// Here comes the code of our second application
}

Building your second application

To build you application you should edit the CMakeLists.txt file created in you application directory. Add the your C++ main file to be build as an executable adding the following line:

rosbuild_add_executable(mySecondApplication mySecondApplication.cpp)

Now you are ready to build, so type

rosmake myApplication

and the executable will be created in the bin directory.

To run the executable do:

bin/mySecondApplication

You will get the semantic output on your screen.

Creating the geometric relations coordinates semantics

We will start with creating the geometric relation coordinates semantics objects for the relation between body C with point a and orientation frame [e], and body D with point b and orientation frame [f], all expressed in coordinate frame [r]:

    // Creating the geometric relations coordinates semantics
    PositionCoordinatesSemantics position("a","C","b","D","r");
    OrientationCoordinatesSemantics orientation("e","C","f","D","r");
    PoseCoordinatesSemantics pose("a","e","C","b","f","D","r");
 
    LinearVelocityCoordinatesSemantics linearVelocity("a","C","D","r");
    AngularVelocityCoordinatesSemantics angularVelocity("C","D","r");
    TwistCoordinatesSemantics twist("a","C","D","r");
 
    TorqueCoordinatesSemantics torque("a","C","D","r");
    ForceCoordinatesSemantics force("C","D","r");
    WrenchCoordinatesSemantics wrench("a","C","D","r");

Doing semantic coordinate operations

We can for instance take the inverses of the created geometric relation coordinates semantics by:

    //Doing semantic operations with the geometric relations
    // inverting
    PositionCoordinatesSemantics positionInv = position.inverse();
    OrientationCoordinatesSemantics orientationInv = orientation.inverse();
    PoseCoordinatesSemantics poseInv = pose.inverse();
    LinearVelocityCoordinatesSemantics linearVelocityInv = linearVelocity.inverse();
    AngularVelocityCoordinatesSemantics angularVelocityInv = angularVelocity.inverse();
    TwistCoordinatesSemantics twistInv = twist.inverse();
    TorqueCoordinatesSemantics torqueInv = torque.inverse();
    ForceCoordinatesSemantics forceInv = force.inverse();
    WrenchCoordinatesSemantics wrenchInv = wrench.inverse();

And if we print the inverses, we will see they are semantically correct:

    std::cout << "-----------------------------------------" << std::endl;
    std::cout << "Inverses: " << std::endl;
    std::cout << "     " << positionInv << " is the inverse of " << position << std::endl;
    std::cout << "     " << orientationInv << " is the inverse of " << orientation << std::endl;
    std::cout << "     " << poseInv << " is the inverse of " << pose << std::endl;
    std::cout << "     " << linearVelocityInv << " is the inverse of " << linearVelocity << std::endl;
    std::cout << "     " << angularVelocityInv << " is the inverse of " << angularVelocity << std::endl;
    std::cout << "     " << twistInv << " is the inverse of " << twist << std::endl;
    std::cout << "     " << torqueInv << " is the inverse of " << torque << std::endl;
    std::cout << "     " << forceInv << " is the inverse of " << force << std::endl;
    std::cout << "     " << wrenchInv << " is the inverse of " << wrench << std::endl;

Now we can for instance compose the result with their inverses. Mind that the order of composition does not matter, since this is automatically derived from the semantic information in the objects.

    //Composing
    PositionCoordinatesSemantics positionComp = compose(position,positionInv);
    OrientationCoordinatesSemantics orientationComp = compose(orientation,orientationInv);
    PoseCoordinatesSemantics poseComp = compose(pose,poseInv);
    LinearVelocityCoordinatesSemantics linearVelocityComp = compose(linearVelocity,linearVelocityInv);
    AngularVelocityCoordinatesSemantics angularVelocityComp = compose(angularVelocity,angularVelocityInv);
    TwistCoordinatesSemantics twistComp = compose(twist,twistInv);
    TorqueCoordinatesSemantics torqueComp = compose(torque,torqueInv);
    ForceCoordinatesSemantics forceComp = compose(force,forceInv);
    WrenchCoordinatesSemantics wrenchComp = compose(wrench,wrenchInv);

If you execute the program you will get screen output on the semantic correctness (and mark: in this case also incorrectness) of the compositions (if not check the build flags of your geometric_semantics library as explained in the user guide. You can print and check the result of the composition using:

    std::cout << "-----------------------------------------" << std::endl;
    std::cout << "Composed objects: " << std::endl;
    std::cout << "     " << positionComp << " is the composition of " << position << " and " << positionInv << std::endl;
    std::cout << "     " << orientationComp << " is the composition of " << orientation << " and " << orientationInv <<  std::endl;
    std::cout << "     " << poseComp << " is the composition of " << pose <<  " and " << poseInv << std::endl;
    std::cout << "     " << linearVelocityComp << " is the composition of " << linearVelocity  << " and " << linearVelocityInv << std::endl;
    std::cout << "     " << angularVelocityComp << " is the composition of " << angularVelocity  << " and " << angularVelocityInv << std::endl;
    std::cout << "     " << twistComp << " is the composition of " << twist <<  " and " << twistInv << std::endl;
    std::cout << "     " << torqueComp << " is the composition of " << torque <<  " and " << torqueInv << std::endl;
    std::cout << "     " << forceComp << " is the composition of " << force <<  " and " << forceInv << std::endl;
    std::cout << "     " << wrenchComp << " is the composition of " << wrench <<  " and " << wrenchInv << std::endl;

Attachment	Size
mySecondApplication.cpp	4.72 KB