00001 #include "kdlTypekit.hpp" 00002 00003 namespace KDL{ 00004 00005 Rotation Inverse(const Rotation& rot){ 00006 return rot.Inverse(); 00007 } 00008 00009 Frame Inverse(const Frame& f){ 00010 return f.Inverse(); 00011 } 00012 00013 bool KDLTypekitPlugin::loadOperators() 00014 { 00015 OperatorRepository::shared_ptr oreg = OperatorRepository::Instance(); 00016 00017 oreg->add( newBinaryOperator( "==", std::equal_to<Frame>() ) ); 00018 oreg->add( newBinaryOperator( "!=", std::not_equal_to<Frame>() ) ); 00019 oreg->add( newBinaryOperator( "==", std::equal_to<Vector>() ) ); 00020 oreg->add( newBinaryOperator( "!=", std::not_equal_to<Vector>() ) ); 00021 oreg->add( newBinaryOperator( "==", std::equal_to<Rotation>() ) ); 00022 oreg->add( newBinaryOperator( "!=", std::not_equal_to<Rotation>() ) ); 00023 oreg->add( newBinaryOperator( "==", std::equal_to<Wrench>() ) ); 00024 oreg->add( newBinaryOperator( "!=", std::not_equal_to<Wrench>() ) ); 00025 oreg->add( newBinaryOperator( "==", std::equal_to<Twist>() ) ); 00026 oreg->add( newBinaryOperator( "!=", std::not_equal_to<Twist>() ) ); 00027 oreg->add( newUnaryOperator( "-", std::negate<Vector>() ) ); 00028 oreg->add( newBinaryOperator( "*", std::multiplies<Vector>() ) ); 00029 oreg->add( newBinaryOperator( "*", std::multiplies<Frame>() ) ); 00030 oreg->add( newBinaryOperator( "*", std::multiplies<Rotation>() ) ); 00031 oreg->add( newBinaryOperator( "+", std::plus<Vector>() ) ); 00032 oreg->add( newBinaryOperator( "-", std::minus<Vector>() ) ); 00033 oreg->add( newBinaryOperator( "+", std::plus<Wrench>() ) ); 00034 oreg->add( newBinaryOperator( "-", std::minus<Wrench>() ) ); 00035 oreg->add( newBinaryOperator( "+", std::plus<Twist>() ) ); 00036 oreg->add( newBinaryOperator( "-", std::minus<Twist>() ) ); 00037 oreg->add( newBinaryOperator( "*", multiplies3<Vector,int, Vector>() ) ); 00038 oreg->add( newBinaryOperator( "*", multiplies3<Vector,Vector, int>() ) ); 00039 oreg->add( newBinaryOperator( "*", multiplies3<Vector,double, Vector>() ) ); 00040 oreg->add( newBinaryOperator( "*", multiplies3<Vector,Vector, double>() ) ); 00041 oreg->add( newBinaryOperator( "*", multiplies3<Wrench, Frame, Wrench>() ) ); 00042 oreg->add( newBinaryOperator( "*", multiplies3<Twist, Frame, Twist>() ) ); 00043 oreg->add( newBinaryOperator( "*", multiplies3<Vector, Frame, Vector>() ) ); 00044 oreg->add( newBinaryOperator( "*", multiplies3<Vector, Rotation, Vector>() ) ); 00045 oreg->add( newBinaryOperator( "/", divides3<Vector, Vector, double>() ) ); 00046 oreg->add( newBinaryOperator( "/", divides3<Wrench, Wrench, double>() ) ); 00047 oreg->add( newBinaryOperator( "/", divides3<Twist, Twist, double>() ) ); 00048 00049 RTT::Service::shared_ptr gs = RTT::internal::GlobalService::Instance(); 00050 00051 gs->provides("KDL")->provides("Vector")->addOperation("diff",(Vector (*) (const Vector&, const Vector&, double)) &diff).doc(""); 00052 gs->provides("KDL")->provides("Vector")->addOperation("addDelta",(Vector (*) (const Vector&, const Vector&, double)) &addDelta).doc(""); 00053 00054 gs->provides("KDL")->provides("Rotation")->addOperation("diff",(Vector (*) (const Rotation&, const Rotation&, double)) &diff).doc(""); 00055 gs->provides("KDL")->provides("Rotation")->addOperation("addDelta",(Rotation (*) (const Rotation&, const Vector&, double)) &addDelta).doc(""); 00056 gs->provides("KDL")->provides("Rotation")->addOperation("Inverse",(Rotation (*) (const Rotation&)) &Inverse).doc(""); 00057 gs->provides("KDL")->provides("Twist")->addOperation("diff",(Twist (*) (const Twist&, const Twist&, double)) &diff).doc(""); 00058 gs->provides("KDL")->provides("Wrench")->addOperation("diff",(Wrench (*) (const Wrench&, const Wrench&, double)) &diff).doc(""); 00059 00060 gs->provides("KDL")->provides("Frame")->addOperation("diff",(Twist (*) (const Frame&, const Frame&, double)) &diff) 00061 .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"); 00062 gs->provides("KDL")->provides("Frame")->addOperation("addDelta", (Frame (*) (const Frame&, const Twist&, double)) &addDelta) 00063 .doc("Constructs a frame that is obtained by: starting from frame f, apply twist t, during time d"); 00064 gs->provides("KDL")->provides("Frame")->addOperation("Inverse",(Frame (*) (const Frame&)) &Inverse).doc(""); 00065 return true; 00066 } 00067 }