//Including the necessary headers
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
//Using the geometric_semantics and KDL namespace
using namespace geometric_semantics;
using namespace KDL;
// The main program
int main (int argc, const char* argv[])
{
// Here comes the code of our third application
// Creating the geometric relations
// a Position with a KDL::Vector
Vector coordinatesPosition(1,2,3);
Position position("a","C","b","D","r",coordinatesPosition);
// an Orientation with KDL::Rotation
Rotation coordinatesOrientation=Rotation::EulerZYX(M_PI/4,0,0);
Orientation orientation("e","C","f","D","f",coordinatesOrientation);
// a Pose with a KDL::Frame
KDL::Frame coordinatesPose(coordinatesOrientation,coordinatesPosition);
Pose pose1("a","e","C","b","f","D","f",coordinatesPose);
// a Pose as aggregation of a Position and a Orientation
Pose pose2(position,orientation);
// a LinearVelocity with a KDL::Vector
Vector coordinatesLinearVelocity(1,2,3);
LinearVelocity linearVelocity("a","C","D","r",coordinatesLinearVelocity);
// a AngularVelocity with a KDL::Vector
Vector coordinatesAngularVelocity(1,2,3);
AngularVelocity angularVelocity("C","D","r",coordinatesAngularVelocity);
// a Twist with a KDL::Twist
KDL::Twist coordinatesTwist(coordinatesLinearVelocity,coordinatesAngularVelocity);
geometric_semantics::Twist twist1("a","C","D","r",coordinatesTwist);
// a Twist of a LinearVelocity and a AngularVelocity
geometric_semantics::Twist twist2(linearVelocity,angularVelocity);
// a Torque with a KDL::Vector
Vector coordinatesTorque(1,2,3);
Torque torque("a","C","D","r",coordinatesTorque);
// a Force with a KDL::Vector
Vector coordinatesForce(1,2,3);
Force force("C","D","r",coordinatesForce);
// a Wrench with a KDL::Wrench
KDL::Wrench coordinatesWrench(coordinatesForce,coordinatesTorque);
geometric_semantics::Wrench wrench1("a","C","D","r",coordinatesWrench);
// a Wrench of a Force and a Torque
geometric_semantics::Wrench wrench2(torque,force);
//Doing operations with the geometric relations
// inverting
Position positionInv = position.inverse();
Orientation orientationInv = orientation.inverse();
Pose pose1Inv = pose1.inverse();
Pose pose2Inv = pose2.inverse();
LinearVelocity linearVelocityInv = linearVelocity.inverse();
AngularVelocity angularVelocityInv = angularVelocity.inverse();
geometric_semantics::Twist twist1Inv = twist1.inverse();
geometric_semantics::Twist twist2Inv = twist2.inverse();
Torque torqueInv = torque.inverse();
Force forceInv = force.inverse();
geometric_semantics::Wrench wrench1Inv = wrench1.inverse();
geometric_semantics::Wrench 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 positionComp = compose(position,positionInv);
Orientation orientationComp = compose(orientation,orientationInv);
Pose pose1Comp = compose(pose1,pose1Inv);
Pose pose2Comp = compose(pose2,pose2Inv);
LinearVelocity linearVelocityComp = compose(linearVelocity,linearVelocityInv);
AngularVelocity angularVelocityComp = compose(angularVelocity,angularVelocityInv);
geometric_semantics::Twist twist1Comp = compose(twist1,twist1Inv);
geometric_semantics::Twist twist2Comp = compose(twist2,twist2Inv);
Torque torqueComp = compose(torque,torqueInv);
Force forceComp = compose(force,forceInv);
geometric_semantics::Wrench wrench1Comp = compose(wrench1,wrench1Inv);
geometric_semantics::Wrench 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;
}