00001 #include "kdlTypekit.hpp"
00002 namespace KDL{
00003
00004 Frame framevr( const Vector& v, const Rotation& r )
00005 {
00006 return Frame( r, v );
00007 };
00008 Frame framerv( const Rotation& r, const Vector& v )
00009 {
00010 return Frame( r, v );
00011 };
00012
00013
00014 Wrench wrenchft( const Vector& force, const Vector& torque )
00015 {
00016 return Wrench( force, torque );
00017 };
00018
00019 Twist twistvw( const Vector& trans, const Vector& rot )
00020 {
00021 return Twist( trans, rot );
00022 };
00023
00024 Vector vectorxyz( double a, double b, double c )
00025 {
00026 return Vector( a, b, c );
00027 };
00028
00029
00030
00031 template<class WT>
00032 struct wrenchtwist_index
00033 : public std::binary_function<WT,int,double>
00034 {
00035 double operator()( WT& w, int index) const
00036 {
00037 if (index > 5 || index <0)
00038 return 0.0;
00039 else
00040 return w[index];
00041 }
00042 };
00043
00044 struct vector_index
00045 : public std::binary_function<Vector, int, double>
00046 {
00047 double operator()( Vector& v, int index ) const
00048 {
00049 if (index > 2 || index <0)
00050 return 0.0;
00051 else
00052 return v[index];
00053 }
00054 };
00055
00056
00057 template< int I>
00058 struct vector_dot
00059 : public std::unary_function<Vector, double>
00060 {
00061 double operator()(const Vector& v ) const
00062 {
00063 return v[I];
00064 }
00065 };
00066
00067 struct twist_rot
00068 : public std::unary_function<Twist, Vector>
00069 {
00070 Vector operator()(const Twist& t ) const
00071 {
00072 return t.rot;
00073 }
00074 };
00075
00076 struct twist_vel
00077 : public std::unary_function<Twist, Vector>
00078 {
00079 Vector operator()(const Twist& t ) const
00080 {
00081 return t.vel;
00082 }
00083 };
00084
00085 struct wrench_torque
00086 : public std::unary_function<Wrench, Vector>
00087 {
00088 Vector operator()(const Wrench& t ) const
00089 {
00090 return t.torque;
00091 }
00092 };
00093
00094 struct wrench_force
00095 : public std::unary_function<Wrench, Vector>
00096 {
00097 Vector operator()(const Wrench& t ) const
00098 {
00099 return t.force;
00100 }
00101 };
00102
00103 struct frame_pos
00104 : public std::unary_function<Frame, Vector>
00105 {
00106 Vector operator()(const Frame& f ) const
00107 {
00108 return f.p;
00109 }
00110 };
00111
00112 struct frame_rot
00113 : public std::unary_function<Frame, Rotation>
00114 {
00115 Rotation operator()(const Frame& f ) const
00116 {
00117 return f.M;
00118 }
00119 };
00120
00121 struct frame_inv
00122 : public std::unary_function<Frame, Frame>
00123 {
00124 Frame operator()(const Frame& f ) const
00125 {
00126 return f.Inverse();
00127 }
00128 };
00129
00130 struct rotation_roll
00131 : public std::unary_function<Rotation, double>
00132 {
00133 double operator()(const Rotation& rot ) const
00134 {
00135 double r,p,y;
00136 rot.GetRPY(r,p,y);
00137 return r;
00138 }
00139 };
00140
00141 struct rotation_pitch
00142 : public std::unary_function<Rotation, double>
00143 {
00144 double operator()(const Rotation& rot ) const
00145 {
00146 double r,p,y;
00147 rot.GetRPY(r,p,y);
00148 return p;
00149 }
00150 };
00151
00152 struct rotation_yaw
00153 : public std::unary_function<Rotation, double>
00154 {
00155 double operator()(const Rotation& rot ) const
00156 {
00157 double r,p,y;
00158 rot.GetRPY(r,p,y);
00159 return y;
00160 }
00161 };
00162
00163 struct rotation_inv
00164 : public std::unary_function<Rotation, Rotation>
00165 {
00166 Rotation operator()(const Rotation& rot ) const
00167 {
00168 return rot.Inverse();
00169 }
00170 };
00171 bool KDLTypekitPlugin::loadConstructors()
00172 {
00173 TypeInfoRepository::shared_ptr ti = TypeInfoRepository::Instance();
00174
00175 ti->type("KDL.Vector")->addConstructor( newConstructor(&vectorxyz) );
00176 ti->type("KDL.Rotation")->addConstructor( newConstructor( ptr_fun( Rotation::RPY )) );
00177 ti->type("KDL.Frame")->addConstructor( newConstructor(&framerv) );
00178 ti->type("KDL.Frame")->addConstructor( newConstructor(&framevr) );
00179 ti->type("KDL.Wrench")->addConstructor( newConstructor(&wrenchft) );
00180 ti->type("KDL.Twist")->addConstructor( newConstructor(&twistvw) );
00181
00182 RTT::Service::shared_ptr gs = RTT::internal::GlobalService::Instance();
00183
00184 gs->provides("KDL")->provides("Rotation")->addOperation("RotX",&Rotation::RotX).doc("");
00185 gs->provides("KDL")->provides("Rotation")->addOperation("RotY",&Rotation::RotY).doc("");
00186 gs->provides("KDL")->provides("Rotation")->addOperation("RotZ",&Rotation::RotZ).doc("");
00187 gs->provides("KDL")->provides("Rotation")->addOperation("RPY",&Rotation::RPY).doc("");
00188 gs->provides("KDL")->provides("Rotation")->addOperation("EulerZYX",&Rotation::EulerZYX).doc("");
00189 gs->provides("KDL")->provides("Rotation")->addOperation("EulerZYZ",&Rotation::EulerZYZ).doc("");
00190 gs->provides("KDL")->provides("Rotation")->addOperation("Quaternion",&Rotation::Quaternion).doc("");
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212 return true;
00213 }
00214
00215 }