$search
00001 #include "kdlTypekit.hpp" 00002 namespace KDL{ 00003 // CONSTRUCTORS 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 // INDEXING 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 // DOTTING 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 //ti->type("Frame[]")->addConstructor(newConstructor(stdvector_ctor<Frame>() ) ); 00193 //ti->type("Frame[]")->addConstructor(newConstructor(stdvector_ctor2<Frame>() ) ); 00194 //ti->type("Frame[]")->addConstructor(new StdVectorBuilder<Frame>() ); 00195 00196 //ti->type("Vector[]")->addConstructor(newConstructor(stdvector_ctor<Vector>() ) ); 00197 //ti->type("Vector[]")->addConstructor(newConstructor(stdvector_ctor2<Vector>() ) ); 00198 //ti->type("Vector[]")->addConstructor(new StdVectorBuilder<Vector>() ); 00199 00200 //ti->type("Rotation[]")->addConstructor(newConstructor(stdvector_ctor<Rotation>() ) ); 00201 //ti->type("Rotation[]")->addConstructor(newConstructor(stdvector_ctor2<Rotation>() ) ); 00202 //ti->type("Rotation[]")->addConstructor(new StdVectorBuilder<Rotation>() ); 00203 00204 //ti->type("Wrench[]")->addConstructor(newConstructor(stdvector_ctor<Wrench>() ) ); 00205 //ti->type("Wrench[]")->addConstructor(newConstructor(stdvector_ctor2<Wrench>() ) ); 00206 //ti->type("Wrench[]")->addConstructor(new StdVectorBuilder<Wrench>() ); 00207 00208 //ti->type("Twist[]")->addConstructor(newConstructor(stdvector_ctor<Twist>() ) ); 00209 //ti->type("Twist[]")->addConstructor(newConstructor(stdvector_ctor2<Twist>() ) ); 00210 //ti->type("Twist[]")->addConstructor(new StdVectorBuilder<Twist>() ); 00211 00212 return true; 00213 } 00214 00215 }