Tutorials

Setting up a package and the build system for your application

This tutorial explains (one possibility) to set up a build system for your application using the geometric_relations_semantics. The possibility we explain uses the ROS package and build infrastructure, and will therefore assume you have ROS installed and set up on your computer.

Create a new ROS package (in this case with name myApplication), with a dependency on the geometric_semantics library and for instance the geometric_semantics_kdl library:

roscreate-pkg myApplication geometric_semantics geometric_semantics_kdl

This will automatically create a directory with name myApplication and a basic build infrastructure (see the roscreate-pkg documentation)

Add the newly created directory to your ROS_PACKAGE_PATH environment variable:

export ROS_PACKAGE_PATH=myApplication:$ROS_PACKAGE_PATH

Your first application using semantic checking on geometric relations (without 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 and 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

Go to the directory of our first application using:

roscd myApplication

Create a main file (in this tutorial called myFirstApplication.cpp) in which we will put the code of our first application.

touch myFirstApplication.cpp

Edit the C++ file with your favorite editor. For instance:

vim myFirstApplication.cpp

Include the necessary headers.

#include <Position/PositionSemantics.h>
#include <Orientation/OrientationSemantics.h>
#include <Pose/PoseSemantics.h>
#include <LinearVelocity/LinearVelocitySemantics.h>
#include <AngularVelocity/AngularVelocitySemantics.h>
#include <Twist/TwistSemantics.h>
#include <Force/ForceSemantics.h>
#include <Torque/TorqueSemantics.h>
#include <Wrench/WrenchSemantics.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 first application
}

Building your first application

To build you application you should edit the CMakeLists.txt file created in your application directory.

vim CMakeLists.txt

Add the C++ main file to be build as an executable by adding the following line:

rosbuild_add_executable(myFirstApplication myFirstApplication.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/myFirstApplication

You will get the semantic output on your screen.

Creating the geometric relations semantics

We will start with creating the geometric relation 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]:

    // Creating the geometric relations semantics
    PositionSemantics position("a","C","b","D");
    OrientationSemantics orientation("e","C","f","D");
    PoseSemantics pose("a","e","C","b","f","D");
 
    LinearVelocitySemantics linearVelocity("a","C","D");
    AngularVelocitySemantics angularVelocity("C","D");
    TwistSemantics twist("a","C","D");
 
    TorqueSemantics torque("a","C","D");
    ForceSemantics force("C","D");
    WrenchSemantics wrench("a","C","D");

Doing semantic operations

We can for instance take the inverses of the created semantic geometric relations by:

    //Doing semantic operations with the geometric relations
    // inverting
    PositionSemantics positionInv = position.inverse();
    OrientationSemantics orientationInv = orientation.inverse();
    PoseSemantics poseInv = pose.inverse();
    LinearVelocitySemantics linearVelocityInv = linearVelocity.inverse();
    AngularVelocitySemantics angularVelocityInv = angularVelocity.inverse();
    TwistSemantics twistInv = twist.inverse();
    TorqueSemantics torqueInv = torque.inverse();
    ForceSemantics forceInv = force.inverse();
    WrenchSemantics 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
    PositionSemantics positionComp = compose(position,positionInv);
    OrientationSemantics orientationComp = compose(orientation,orientationInv);
    PoseSemantics poseComp = compose(pose,poseInv);
    LinearVelocitySemantics linearVelocityComp = compose(linearVelocity,linearVelocityInv);
    AngularVelocitySemantics angularVelocityComp = compose(angularVelocity,angularVelocityInv);
    TwistSemantics twistComp = compose(twist,twistInv);
    TorqueSemantics torqueComp = compose(torque,torqueInv);
    ForceSemantics forceComp = compose(force,forceInv);
    WrenchSemantics wrenchComp = compose(wrench,wrenchInv);

If you execute the program you will get screen output on the semantic correctness 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
myFirstApplication.cpp	4.28 KB

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

Your third application doing actual geometric calculations on top of the semantic 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 full geometric relation objects (with semantics and actual coordinate representation) and perform 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. To this end we will use the coordinate representations of the Orocos Kinematics and Dynamics Library. The semantic support on top of this geometry library is already provided by the geometric_semantics_kdl package.

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

Prepare the main file

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

vim myThirdApplication.cpp

Include the necessary headers

#include <Position/Position.h>
#include <Orientation/Orientation.h>
#include <Pose/Pose.h>
#include <LinearVelocity/LinearVelocity.h>
#include <AngularVelocity/AngularVelocity.h>
#include <Twist/Twist.h>
#include <Force/Force.h>
#include <Torque/Torque.h>
#include <Wrench/Wrench.h>
 
#include <Position/PositionCoordinatesKDL.h>
#include <Orientation/OrientationCoordinatesKDL.h>
#include <Pose/PoseCoordinatesKDL.h>
#include <LinearVelocity/LinearVelocityCoordinatesKDL.h>
#include <AngularVelocity/AngularVelocityCoordinatesKDL.h>
#include <Twist/TwistCoordinatesKDL.h>
#include <Force/ForceCoordinatesKDL.h>
#include <Torque/TorqueCoordinatesKDL.h>
#include <Wrench/WrenchCoordinatesKDL.h>
 
#include <kdl/frames.hpp>
#include <kdl/frames_io.hpp>

Next we use the geometric_semantics namespace and the KDL namespace for convenience:

using namespace geometric_semantics;
using namespace KDL;

Create a main program:

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

Building your Third 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(myThirdApplication myThirdApplication.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/myThirdApplication

You will get the semantic output on your screen.

Creating the geometric relations

We will start with creating the geometric relation 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], together with their coordinate representations using KDL types.

  // Creating the geometric relations 
 
    // a Position with a KDL::Vector
    Vector coordinatesPosition(1,2,3);
    Position<Vector> position("a","C","b","D","r",coordinatesPosition);
 
    // an Orientation with KDL::Rotation
    Rotation coordinatesOrientation=Rotation::EulerZYX(M_PI/4,0,0);
    Orientation<Rotation> orientation("e","C","f","D","f",coordinatesOrientation);
 
    // a Pose with a KDL::Frame
    KDL::Frame coordinatesPose(coordinatesOrientation,coordinatesPosition);
    Pose<KDL::Frame> pose1("a","e","C","b","f","D","f",coordinatesPose);
 
    // a Pose as aggregation of a Position and a Orientation
    Pose<Vector,Rotation> pose2(position,orientation);
 
    // a LinearVelocity with a KDL::Vector
    Vector coordinatesLinearVelocity(1,2,3);
    LinearVelocity<Vector> linearVelocity("a","C","D","r",coordinatesLinearVelocity);
 
    // a AngularVelocity with a KDL::Vector
    Vector coordinatesAngularVelocity(1,2,3);
    AngularVelocity<Vector> angularVelocity("C","D","r",coordinatesAngularVelocity);
 
    // a Twist with a KDL::Twist
    KDL::Twist coordinatesTwist(coordinatesLinearVelocity,coordinatesAngularVelocity);
    geometric_semantics::Twist<KDL::Twist> twist1("a","C","D","r",coordinatesTwist);
 
    // a Twist of a LinearVelocity and a AngularVelocity
    geometric_semantics::Twist<Vector,Vector> twist2(linearVelocity,angularVelocity);
 
    // a Torque with a KDL::Vector
    Vector coordinatesTorque(1,2,3);
    Torque<Vector> torque("a","C","D","r",coordinatesTorque);
 
    // a Force with a KDL::Vector
    Vector coordinatesForce(1,2,3);
    Force<Vector> force("C","D","r",coordinatesForce);
 
    // a Wrench with a KDL::Wrench
    KDL::Wrench coordinatesWrench(coordinatesForce,coordinatesTorque);
    geometric_semantics::Wrench<KDL::Wrench> wrench1("a","C","D","r",coordinatesWrench);
 
    // a Wrench of a Force and a Torque
    geometric_semantics::Wrench<KDL::Vector,KDL::Vector> wrench2(torque,force);

Doing geometric operations

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

    //Doing operations with the geometric relations
    // inverting
    Position<Vector> positionInv = position.inverse();
    Orientation<Rotation> orientationInv = orientation.inverse();
    Pose<KDL::Frame> pose1Inv = pose1.inverse();
    Pose<Vector,Rotation> pose2Inv = pose2.inverse();
    LinearVelocity<Vector> linearVelocityInv = linearVelocity.inverse();
    AngularVelocity<Vector> angularVelocityInv = angularVelocity.inverse();
    geometric_semantics::Twist<KDL::Twist> twist1Inv = twist1.inverse();
    geometric_semantics::Twist<Vector,Vector> twist2Inv = twist2.inverse();
    Torque<Vector> torqueInv = torque.inverse();
    Force<Vector> forceInv = force.inverse();
    geometric_semantics::Wrench<KDL::Wrench> wrench1Inv = wrench1.inverse();
    geometric_semantics::Wrench<Vector,Vector> wrench2Inv = wrench2.inverse();

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

    // print the inverses
    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 << "     " << pose1Inv << " is the inverse of " << pose1 << std::endl;
    std::cout << "     " << pose2Inv << " is the inverse of " << pose2 << std::endl;
    std::cout << "     " << linearVelocityInv << " is the inverse of " << linearVelocity << std::endl;
    std::cout << "     " << angularVelocityInv << " is the inverse of " << angularVelocity << std::endl;
    std::cout << "     " << twist1Inv << " is the inverse of " << twist1 << std::endl;
    std::cout << "     " << twist2Inv << " is the inverse of " << twist2 << std::endl;
    std::cout << "     " << torqueInv << " is the inverse of " << torque << std::endl;
    std::cout << "     " << forceInv << " is the inverse of " << force << std::endl;
    std::cout << "     " << wrench1Inv << " is the inverse of " << wrench1 << std::endl;
    std::cout << "     " << wrench2Inv << " is the inverse of " << wrench2 << 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
    Position<Vector> positionComp = compose(position,positionInv);
    Orientation<Rotation> orientationComp = compose(orientation,orientationInv);
    Pose<KDL::Frame> pose1Comp = compose(pose1,pose1Inv);
    Pose<Vector,Rotation> pose2Comp = compose(pose2,pose2Inv);
    LinearVelocity<Vector> linearVelocityComp = compose(linearVelocity,linearVelocityInv);
    AngularVelocity<Vector> angularVelocityComp = compose(angularVelocity,angularVelocityInv);
    geometric_semantics::Twist<KDL::Twist> twist1Comp = compose(twist1,twist1Inv);
    geometric_semantics::Twist<Vector,Vector> twist2Comp = compose(twist2,twist2Inv);
    Torque<Vector> torqueComp = compose(torque,torqueInv);
    Force<Vector> forceComp = compose(force,forceInv);
    geometric_semantics::Wrench<KDL::Wrench> wrench1Comp = compose(wrench1,wrench1Inv);
    geometric_semantics::Wrench<Vector,Vector> wrench2Comp = compose(wrench2,wrench2Inv);;

    // print the composed objects
    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 << "     " << pose1Comp << " is the composition of " << pose1 <<  " and " << pose1Inv << std::endl;
    std::cout << "     " << pose2Comp << " is the composition of " << pose2 <<  " and " << pose2Inv << 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 << "     " << twist1Comp << " is the composition of " << twist1 <<  " and " << twist1Inv << std::endl;
    std::cout << "     " << twist2Comp << " is the composition of " << twist2 <<  " and " << twist2Inv << 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 << "     " << wrench1Comp << " is the composition of " << wrench1 <<  " and " << wrench2Inv << std::endl;
    std::cout << "     " << wrench2Comp << " is the composition of " << wrench1 <<  " and " << wrench2Inv << std::endl;

Attachment	Size
myThirdApplication.cpp	7.63 KB

Some extra examples

In case you are looking for some extra examples you can have a look at the geometric_semantics_examples package. So far it already contains an example showing the advantage of using semantics when integrating twists, and when programming two position controlled robots.