00001 #include "kdlTypekit.hpp" 00002 #include <kdl/frames.hpp> 00003 00004 namespace KDL{ 00005 00006 Rotation Inverse(const Rotation& rot){ 00007 return rot.Inverse(); 00008 } 00009 00010 Frame Inverse(const Frame& f){ 00011 return f.Inverse(); 00012 } 00013 00014 void GetQuaternion(const Rotation& rot,double& x, double& y, double& z, double& w){ 00015 rot.GetQuaternion(x,y,z,w); 00016 } 00017 00018 bool KDLTypekitPlugin::loadOperators() 00019 { 00020 OperatorRepository::shared_ptr oreg = OperatorRepository::Instance(); 00021 00022 oreg->add( newBinaryOperator( "==", std::equal_to<Frame>() ) ); 00023 oreg->add( newBinaryOperator( "!=", std::not_equal_to<Frame>() ) ); 00024 oreg->add( newBinaryOperator( "==", std::equal_to<Vector>() ) ); 00025 oreg->add( newBinaryOperator( "!=", std::not_equal_to<Vector>() ) ); 00026 oreg->add( newBinaryOperator( "==", std::equal_to<Rotation>() ) ); 00027 oreg->add( newBinaryOperator( "!=", std::not_equal_to<Rotation>() ) ); 00028 oreg->add( newBinaryOperator( "==", std::equal_to<Wrench>() ) ); 00029 oreg->add( newBinaryOperator( "!=", std::not_equal_to<Wrench>() ) ); 00030 oreg->add( newBinaryOperator( "==", std::equal_to<Twist>() ) ); 00031 oreg->add( newBinaryOperator( "!=", std::not_equal_to<Twist>() ) ); 00032 oreg->add( newUnaryOperator( "-", std::negate<Vector>() ) ); 00033 oreg->add( newBinaryOperator( "*", std::multiplies<Vector>() ) ); 00034 oreg->add( newBinaryOperator( "*", std::multiplies<Frame>() ) ); 00035 oreg->add( newBinaryOperator( "*", std::multiplies<Rotation>() ) ); 00036 oreg->add( newBinaryOperator( "+", std::plus<Vector>() ) ); 00037 oreg->add( newBinaryOperator( "-", std::minus<Vector>() ) ); 00038 oreg->add( newBinaryOperator( "+", std::plus<Wrench>() ) ); 00039 oreg->add( newBinaryOperator( "-", std::minus<Wrench>() ) ); 00040 oreg->add( newBinaryOperator( "+", std::plus<Twist>() ) ); 00041 oreg->add( newBinaryOperator( "-", std::minus<Twist>() ) ); 00042 oreg->add( newBinaryOperator( "*", multiplies3<Vector,int, Vector>() ) ); 00043 oreg->add( newBinaryOperator( "*", multiplies3<Vector,Vector, int>() ) ); 00044 oreg->add( newBinaryOperator( "*", multiplies3<Vector,double, Vector>() ) ); 00045 oreg->add( newBinaryOperator( "*", multiplies3<Vector,Vector, double>() ) ); 00046 oreg->add( newBinaryOperator( "*", multiplies3<Wrench, Frame, Wrench>() ) ); 00047 oreg->add( newBinaryOperator( "*", multiplies3<Twist, Frame, Twist>() ) ); 00048 oreg->add( newBinaryOperator( "*", multiplies3<Vector, Frame, Vector>() ) ); 00049 oreg->add( newBinaryOperator( "*", multiplies3<Vector, Rotation, Vector>() ) ); 00050 oreg->add( newBinaryOperator( "/", divides3<Vector, Vector, double>() ) ); 00051 oreg->add( newBinaryOperator( "/", divides3<Wrench, Wrench, double>() ) ); 00052 oreg->add( newBinaryOperator( "/", divides3<Twist, Twist, double>() ) ); 00053 00054 RTT::Service::shared_ptr gs = RTT::internal::GlobalService::Instance(); 00055 00056 gs->provides("KDL")->provides("Vector")->addOperation("diff",(Vector (*) (const Vector&, const Vector&, double)) &diff).doc(""); 00057 gs->provides("KDL")->provides("Vector")->addOperation("addDelta",(Vector (*) (const Vector&, const Vector&, double)) &addDelta).doc(""); 00058 00059 gs->provides("KDL")->provides("Rotation")->addOperation("diff",(Vector (*) (const Rotation&, const Rotation&, double)) &diff).doc(""); 00060 gs->provides("KDL")->provides("Rotation")->addOperation("addDelta",(Rotation (*) (const Rotation&, const Vector&, double)) &addDelta).doc(""); 00061 gs->provides("KDL")->provides("Rotation")->addOperation("Inverse",(Rotation (*) (const Rotation&)) &Inverse).doc(""); 00062 gs->provides("KDL")->provides("Rotation")->addOperation("GetQuaternion",(void (*)(const Rotation&, double&, double&, double&, double&)) &GetQuaternion).doc(""); 00063 00064 gs->provides("KDL")->provides("Twist")->addOperation("diff",(Twist (*) (const Twist&, const Twist&, double)) &diff).doc(""); 00065 gs->provides("KDL")->provides("Wrench")->addOperation("diff",(Wrench (*) (const Wrench&, const Wrench&, double)) &diff).doc(""); 00066 00067 gs->provides("KDL")->provides("Frame")->addOperation("diff",(Twist (*) (const Frame&, const Frame&, double)) &diff) 00068 .doc("Returns the twist that is needed to move from frame f1 to frame f2 in a time d. The resulting twist is represented in the same reference frame as f1 and f2, and has reference point at the origin of f1"); 00069 gs->provides("KDL")->provides("Frame")->addOperation("addDelta", (Frame (*) (const Frame&, const Twist&, double)) &addDelta) 00070 .doc("Constructs a frame that is obtained by: starting from frame f, apply twist t, during time d"); 00071 gs->provides("KDL")->provides("Frame")->addOperation("Inverse",(Frame (*) (const Frame&)) &Inverse).doc(""); 00072 return true; 00073 } 00074 }