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);;

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