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 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 }

- 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.

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

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

// 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 |