bool loadOperators()
{
RTT::Service::shared_ptr gs = RTT::internal::GlobalService::Instance();
gs->provides("KDL")->addOperation("TwistToMsg",&tf::TwistKDLToMsg);
gs->provides("KDL")->addOperation("MsgToTwist",&tf::TwistMsgToKDL);
gs->provides("KDL")->addOperation("FrameToMsg",&tf::PoseKDLToMsg);
gs->provides("KDL")->addOperation("MsgToFrame",&tf::PoseKDLToMsg);
return true;
}
bool KDLTypekitPlugin::loadConstructors()
{
TypeInfoRepository::shared_ptr ti = TypeInfoRepository::Instance();
ti->type("KDL.Vector")->addConstructor( newConstructor(&vectorxyz) );
ti->type("KDL.Rotation")->addConstructor( newConstructor( ptr_fun( Rotation::RPY )) );
ti->type("KDL.Rotation")->addConstructor( newConstructor(&rotationAngleAxis) );
ti->type("KDL.Frame")->addConstructor( newConstructor(&framerv) );
ti->type("KDL.Frame")->addConstructor( newConstructor(&framevr) );
ti->type("KDL.Wrench")->addConstructor( newConstructor(&wrenchft) );
ti->type("KDL.Twist")->addConstructor( newConstructor(&twistvw) );
RTT::Service::shared_ptr gs = RTT::internal::GlobalService::Instance();
gs->provides("KDL")->provides("Rotation")->addOperation("RotX",&Rotation::RotX).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("RotY",&Rotation::RotY).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("RotZ",&Rotation::RotZ).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("RPY",&Rotation::RPY).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("EulerZYX",&Rotation::EulerZYX).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("EulerZYZ",&Rotation::EulerZYZ).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("Quaternion",&Rotation::Quaternion).doc("");
//ti->type("Frame[]")->addConstructor(newConstructor(stdvector_ctor<Frame>() ) );
//ti->type("Frame[]")->addConstructor(newConstructor(stdvector_ctor2<Frame>() ) );
//ti->type("Frame[]")->addConstructor(new StdVectorBuilder<Frame>() );
//ti->type("Vector[]")->addConstructor(newConstructor(stdvector_ctor<Vector>() ) );
//ti->type("Vector[]")->addConstructor(newConstructor(stdvector_ctor2<Vector>() ) );
//ti->type("Vector[]")->addConstructor(new StdVectorBuilder<Vector>() );
//ti->type("Rotation[]")->addConstructor(newConstructor(stdvector_ctor<Rotation>() ) );
//ti->type("Rotation[]")->addConstructor(newConstructor(stdvector_ctor2<Rotation>() ) );
//ti->type("Rotation[]")->addConstructor(new StdVectorBuilder<Rotation>() );
//ti->type("Wrench[]")->addConstructor(newConstructor(stdvector_ctor<Wrench>() ) );
//ti->type("Wrench[]")->addConstructor(newConstructor(stdvector_ctor2<Wrench>() ) );
//ti->type("Wrench[]")->addConstructor(new StdVectorBuilder<Wrench>() );
//ti->type("Twist[]")->addConstructor(newConstructor(stdvector_ctor<Twist>() ) );
//ti->type("Twist[]")->addConstructor(newConstructor(stdvector_ctor2<Twist>() ) );
//ti->type("Twist[]")->addConstructor(new StdVectorBuilder<Twist>() );
return true;
}
bool KDLTypekitPlugin::loadOperators()
{
OperatorRepository::shared_ptr oreg = OperatorRepository::Instance();
oreg->add( newBinaryOperator( "==", std::equal_to<Frame>() ) );
oreg->add( newBinaryOperator( "!=", std::not_equal_to<Frame>() ) );
oreg->add( newBinaryOperator( "==", std::equal_to<Vector>() ) );
oreg->add( newBinaryOperator( "!=", std::not_equal_to<Vector>() ) );
oreg->add( newBinaryOperator( "==", std::equal_to<Rotation>() ) );
oreg->add( newBinaryOperator( "!=", std::not_equal_to<Rotation>() ) );
oreg->add( newBinaryOperator( "==", std::equal_to<Wrench>() ) );
oreg->add( newBinaryOperator( "!=", std::not_equal_to<Wrench>() ) );
oreg->add( newBinaryOperator( "==", std::equal_to<Twist>() ) );
oreg->add( newBinaryOperator( "!=", std::not_equal_to<Twist>() ) );
oreg->add( newUnaryOperator( "-", std::negate<Vector>() ) );
oreg->add( newBinaryOperator( "*", std::multiplies<Vector>() ) );
oreg->add( newBinaryOperator( "*", std::multiplies<Frame>() ) );
oreg->add( newBinaryOperator( "*", std::multiplies<Rotation>() ) );
oreg->add( newBinaryOperator( "+", std::plus<Vector>() ) );
oreg->add( newBinaryOperator( "-", std::minus<Vector>() ) );
oreg->add( newBinaryOperator( "+", std::plus<Wrench>() ) );
oreg->add( newBinaryOperator( "-", std::minus<Wrench>() ) );
oreg->add( newBinaryOperator( "+", std::plus<Twist>() ) );
oreg->add( newBinaryOperator( "-", std::minus<Twist>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector,int, Vector>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector,Vector, int>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector,double, Vector>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector,Vector, double>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Wrench, Frame, Wrench>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Twist, Frame, Twist>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector, Frame, Vector>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector, Rotation, Vector>() ) );
oreg->add( newBinaryOperator( "/", divides3<Vector, Vector, double>() ) );
oreg->add( newBinaryOperator( "/", divides3<Wrench, Wrench, double>() ) );
oreg->add( newBinaryOperator( "/", divides3<Twist, Twist, double>() ) );
RTT::Service::shared_ptr gs = RTT::internal::GlobalService::Instance();
gs->provides("KDL")->provides("Vector")->addOperation("diff",(Vector (*) (const Vector&, const Vector&, double)) &diff).doc("");
gs->provides("KDL")->provides("Vector")->addOperation("addDelta",(Vector (*) (const Vector&, const Vector&, double)) &addDelta).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("diff",(Vector (*) (const Rotation&, const Rotation&, double)) &diff).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("addDelta",(Rotation (*) (const Rotation&, const Vector&, double)) &addDelta).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("Inverse",(Rotation (*) (const Rotation&)) &Inverse).doc("");
gs->provides("KDL")->provides("Rotation")->addOperation("GetQuaternion",(void (*)(const Rotation&, double&, double&, double&, double&)) &GetQuaternion).doc("");
gs->provides("KDL")->provides("Twist")->addOperation("diff",(Twist (*) (const Twist&, const Twist&, double)) &diff).doc("");
gs->provides("KDL")->provides("Wrench")->addOperation("diff",(Wrench (*) (const Wrench&, const Wrench&, double)) &diff).doc("");
gs->provides("KDL")->provides("Frame")->addOperation("diff",(Twist (*) (const Frame&, const Frame&, double)) &diff)
.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");
gs->provides("KDL")->provides("Frame")->addOperation("addDelta", (Frame (*) (const Frame&, const Twist&, double)) &addDelta)
.doc("Constructs a frame that is obtained by: starting from frame f, apply twist t, during time d");
gs->provides("KDL")->provides("Frame")->addOperation("Inverse",(Frame (*) (const Frame&)) &Inverse).doc("");
return true;
}