Remark that this wiki contains a summary of the theoretical article and the software article both published as a tutorial for IEEE Robotics and Automation Magazine:

The geometric relations semantics software (C++) implements the geometric relation semantics theory, hereby offering support for semantic checks for your rigid body relations calculations. This will avoid commonly made errors, and hence reduce application and, especially, system integration development time considerably. The proposed software is to our knowledge the first to offer a semantic interface for geometric operation software libraries.
The screenshot below shows the output of the semantic checks of the (wrong) composition of two positions and two orientations.
The goal of the software is to provide semantic checking for calculations with geometric relations between rigid bodies on top of existing geometric libraries, which are only working on specific coordinate representations. Since there are already a lot of libraries with good support for geometric calculations on specific coordinate representations (The Orocos Kinematics and Dynamics library, the ROS geometry library, boost, ...) we do not want to design yet another library but rather will extend these existing geometric libraries with semantic support. The effort to extend an existing geometric library with semantic support is very limited: it boils down to the implementation of about six function template specializations.
This wiki contains a summary of the article accepted as a tutorial for IEEE Robotics and Automation Magazine on the 4th June 2012.
Rigid bodies are essential primitives in the modelling of robotic devices, tasks and perception, starting with the basic geometric relations such as relative position, orientation, pose, translational velocity, rotational velocity, and twist. This wiki elaborates on the background and the software for the semantics underlying rigid body relationships. This wiki is based on the research of the KU Leuven robotics group, in this case mainly conducted by Tinne De Laet, to explain semantics of all coordinateinvariant properties and operations, and, more importantly, to document all the choices that are made in coordinate representations of these geometric relations. This resulted in a set of concrete suggestions for standardizing terminology and notation, and software with a fully unambiguous software interface, including automatic checks for semantic correctness of all geometric operations on rigidbody coordinate representations.
The geometric relations semantics software prevents commonly made errors in geometric rigidbody relations calculations like:
This wiki contains a summary of the article accepted as a tutorial for IEEE Robotics and Automation Magazine on the 4th June 2012.
A rigid body is an idealization of a solid body of infinite or finite size in which deformation is neglected. We often abbreviate “rigid body” to “body”, and denotes it by the symbol $\mathcal{A}$. A body in threedimensional space has six degrees of freedom: three degrees of freedom in translation and three in rotation. The subspace of all body motions that involve only changes in the orientation is often denoted by SO(3) (the Special Orthogonal group in threedimensional space). It forms a group under the operation of composition of relative motion. The space of all body motions, including translations, is denoted by SE(3) (the Special Euclidean group in threedimensional space).
A general sixdimensional displacement between two bodies is called a (relative) pose: it contains both the position and orientation. Remark that the position, orientation, and pose of a body are not absolute concepts, since they imply a second body with respect to which they are defined. Hence, only the relative position, orientation, and pose between two bodies are relevant geometric relations.
A general sixdimensional velocity between two bodies is called a (relative) twist: it contains both the rotational and the translational velocity. Similar to the position, orientation, and pose, the translational velocity, rotational velocity, and twist of a body are not absolute concepts, since they imply a second body with respect to which they are defined. Hence, only the relative translational velocity, rotational velocity, and twist between two bodies are relevant geometric relations.
When doing actual calculations with the geometric relations between rigid bodies, one has to use the coordinate representation of the geometric relations, and therefore has to choose a coordinate frame in which the coordinates are expressed in order to obtain numerical values for the geometric relations.
Each of these geometric primitives can be fixed to a body, which means that the geometric primitive coincides with the body not only instantaneously, but also over time. For the point $a$ and the body $\mathcal{C}$ for instance, this is written as $a\mathcal{C}$. The figure below presents the geometric primitives body, point, vector, orientation frame, and frame graphically.
The table below summarizes the semantics for the following geometric relations between rigid bodies: position, orientation, pose, translational velocity, rotational velocity, and twist.
The software implements the geometric relation semantics, hereby offering support for semantic checks for your rigid body relations. This will avoid commonly made errors, and hence reduce application (and, especially, system integration) development time considerably. The proposed software is to our knowledge the first to offer a semantic interface for geometric operation software libraries.
For the semantic checking, we created the (templated) geometric_semantics core library, providing all the necessary semantic support for geometric relations (relative positions, orientations, poses, translational velocities, rotational velocities, twists, forces, torques, and wrenches) and the operations on these geometric relations (composition, integration, inversion, ...).
If you want to perform actual geometric relation calculations, you will need particular coordinate representations (for instance a homogeneous transformation matrix for a pose) and a geometric library offering support for calculations on these coordinate representations (for instance multiplication of homogeneous transformation matrices). To this end, you can build your own library depending on the geometric_semantics core library in which you implement a limited number of functions, which make the connection between semantic operations (for instance composition) and actual coordinate representation calculations (for instance multiplication of homogeneous transformation matrices). We already provide support for two geometric libraries: the Orocos Kinematics and Dynamics library and the ROS geometry library, in the geometric_semantics_kdl and geometric_semantics_tf libraries, respectively.
Again, the template is the actual geometry (of an external library) you will use as a coordinate representation, for instance a KDL::Vector.
The above described design is illustrated by the figure below.
Remark that all four of the above 'levels' are of actual use:
The API is available at: http://people.mech.kuleuven.be/~tdelaet/geometric_relations_semantics/doc/.
This stack consists of following packages:
Each package contains the following subdirectories:
git clone https://gitlab.mech.kuleuven.be/robdsl/geometricrelationssemantics.git
cd geometric_relations_semantics
export ROS_PACKAGE_PATH=$PWD:$ROS_PACKAGE_PATH
rosdep install geometric_relations_semantics rosmake geometric_relations_semantics
roscd geometric_semantics make test
rosmake geometric_relations_semantics
rosmake PACKAGE_NAME
roscd geometric_semantics
make test
If you are looking for installation instructions you should read the quick start.
Here we will explain how you can use the geometric relations semantics in your application, in particular using the Orocos Kinematics and Dynamics library as a geometry library, supplemented with the semantic support.
roscreatepkg myApplication geometric_semantics_kdl
This will automatically create a directory with name myApplication and a basic build infrastructure (see the roscreatepkg documentation)
cd myApplication export ROS_PACKAGE_PATH=$PWD:$ROS_PACKAGE_PATH
roscd myApplication
touch myApplication.cpp
#include <Pose/Pose.h> #include <Pose/PoseCoordinatesKDL.h>
using namespace geometric_semantics; using namespace KDL;
Rotation coordinatesRotB2_B1=Rotation::EulerZYX(M_PI,0,0); Vector coordinatesPosB2_B1(2.2,0,0); KDL::Frame coordinatesFrameB2_B1(coordinatesRotB2_B1,coordinatesPosB2_B1)
Then use this KDL coordinates to create a PoseCoordinates object:
PoseCoordinates<KDL::Frame> poseCoordB2_B1(coordinatesFrameB2_B1);
Then create a Pose object using both the semantic information and the PoseCoordinates:
Pose<KDL::Frame> poseB2_B1("b2","b2","B2","b1","b1","B1","b1",poseCoordB2_B1);
Pose<KDL::Frame> poseB1_B2 = poseB2_B1.inverse()
rosbuild_add_executable(myApplication myApplication.cpp)
rosmake myApplication
and the executable will be created in the bin directory.
bin/myApplication
You will get the semantic output on your screen.
Imagine you have your own geometry library with support for geometric relation coordinate representations and calculations with these coordinate representations. You however would like to have semantic support on top of this geometry library. Probably the best thing to do in this case is to mimic our support for the Orocos Kinematics and Dynamics Library. To have a look at it do:
roscd geometric_semantics_kdl/
The possible semantic constraints are listed in the *Coordinates.h files in the geometric_semantics core library. So for instance for OrientationCoordinates we find there an enumeration of the different possible semantic constraints imposed by Orientation coordinate representations:
/** *\brief Constraints imposed by the orientation coordinate representation to the semantics */ enum Constraints{ noConstraints = 0x00, coordinateFrame_equals_referenceOrientationFrame = 0x01, // constraint that the orientation frame on the reference body has to be equal to the coordinate frame };
You should specify the constraint when writing the template specialization of the OrientationCoordinates<KDL::Rotation>:
// template specialization for KDL::Rotation template <> OrientationCoordinates<KDL::Rotation>::OrientationCoordinates(const KDL::Rotation& coordinates): data(coordinates), constraints(coordinateFrame_equals_referenceOrientationFrame){ };
The other function template specializations specify the actual coordinate calculations that have to be performed for semantic operations like inverse, changing the coordinate frame, changing the orientation frame, ... For instance, to specialize the inverse for KDL::Rotation coordinate representations:
template <> OrientationCoordinates<KDL::Rotation> OrientationCoordinates<KDL::Rotation>::inverse2Impl() const { return OrientationCoordinates<KDL::Rotation>(this>data.Inverse()); }
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.
roscreatepkg myApplication geometric_semantics geometric_semantics_kdl
This will automatically create a directory with name myApplication and a basic build infrastructure (see the roscreatepkg documentation)
export ROS_PACKAGE_PATH=myApplication:$ROS_PACKAGE_PATH
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.
roscd myApplication
touch myFirstApplication.cpp
vim myFirstApplication.cpp
#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>
using namespace geometric_semantics;
int main (int argc, const char* argv[]) { // Here comes the code of our first application }
vim CMakeLists.txt
rosbuild_add_executable(myFirstApplication myFirstApplication.cpp)
rosmake myApplication
and the executable will be created in the bin directory.
bin/myFirstApplication
You will get the semantic output on your screen.
// 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 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();
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;
//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);
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 
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.
vim mySecondApplication.cpp
#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>
using namespace geometric_semantics;
int main (int argc, const char* argv[]) { // Here comes the code of our second application }
rosbuild_add_executable(mySecondApplication mySecondApplication.cpp)
rosmake myApplication
and the executable will be created in the bin directory.
bin/mySecondApplication
You will get the semantic output on your screen.
// 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 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();
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;
//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);
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 
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.
vim myThirdApplication.cpp
#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>
using namespace geometric_semantics; using namespace KDL;
int main (int argc, const char* argv[]) { // Here goes the code of our third application }
rosbuild_add_executable(myThirdApplication myThirdApplication.cpp)
rosmake myApplication
and the executable will be created in the bin directory.
bin/myThirdApplication
You will get the semantic output on your screen.
// 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 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();
// 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;
//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);;
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:
// 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 
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.