00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042 #include "motionproperties.hpp"
00043
00044 #include <rtt/Property.hpp>
00045 #include <rtt/PropertyBag.hpp>
00046 #include <rtt/Logger.hpp>
00047
00048 namespace RTT
00049 {
00050 using namespace std;
00051
00062 class VectorDecomposer
00063 {
00064 PropertyBag resultBag;
00065 Property<double> X;
00066 Property<double> Y;
00067 Property<double> Z;
00068
00069 public:
00070
00071 VectorDecomposer( const Vector& v);
00072
00073 PropertyBag& result() { return resultBag; }
00074 };
00075
00080 class VectorComposer
00081 {
00082 const PropertyBag& bag;
00083 public:
00084 VectorComposer(const PropertyBag& _bag )
00085 : bag(_bag)
00086 {}
00087
00088 bool getResult( Vector& res);
00089 };
00090
00091 VectorDecomposer::VectorDecomposer( const Vector& v )
00092 : resultBag("KDL.Vector"),
00093 X("X","X Value", v[0]),
00094 Y("Y","Y Value", v[1]),
00095 Z("Z","Z Value", v[2])
00096 {
00097 resultBag.add(X.clone());
00098 resultBag.add(Y.clone());
00099 resultBag.add(Z.clone());
00100 }
00101
00102 bool VectorComposer::getResult(Vector& res)
00103 {
00104 if ( bag.getType() == "MotCon::Vector" || bag.getType() == "KDL.Vector" )
00105 {
00106 Property<double>* px = dynamic_cast<Property<double>*>( bag.find("X") );
00107 Property<double>* py = dynamic_cast<Property<double>*>( bag.find("Y") );
00108 Property<double>* pz = dynamic_cast<Property<double>*>( bag.find("Z") );
00109
00110 if ( px != 0 && py != 0 && pz != 0)
00111 {
00112 res = Vector( px->get(),py->get(),pz->get() );
00113 return true;
00114 } else {
00115 std::string element = !px ? "X" : !py ? "Y" : "Z";
00116 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Vector > "
00117 << ": Missing element '" <<element<<"'." <<Logger::endl;
00118 return false;
00119 }
00120 } else {
00121 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Vector > "
00122 << ": Expected type 'KDL.Vector', got type '"<< bag.getType() <<"'."
00123 <<Logger::endl;
00124 }
00125 return false;
00126 }
00127
00128
00139 class RotationDecomposer
00140 {
00141 PropertyBag resultBag;
00142 Property<double> X_x;
00143 Property<double> X_y;
00144 Property<double> X_z;
00145 Property<double> Y_x;
00146 Property<double> Y_y;
00147 Property<double> Y_z;
00148 Property<double> Z_x;
00149 Property<double> Z_y;
00150 Property<double> Z_z;
00151 public:
00152
00153 RotationDecomposer( const Rotation& r );
00154
00155 PropertyBag& result() { return resultBag; }
00156 };
00157
00162 class RotationComposer
00163 {
00164 const PropertyBag& bag;
00165 public:
00166 RotationComposer(const PropertyBag& _bag )
00167 : bag(_bag)
00168 {}
00169
00170 bool getResult( Rotation& res );
00171 };
00172
00173 RotationDecomposer::RotationDecomposer( const Rotation& r)
00174 : resultBag("KDL.Rotation"),
00175 X_x("X_x","", r(0,0) ),
00176 X_y("X_y","", r(1,0) ),
00177 X_z("X_z","", r(2,0) ),
00178 Y_x("Y_x","", r(0,1) ),
00179 Y_y("Y_y","", r(1,1) ),
00180 Y_z("Y_z","", r(2,1) ),
00181 Z_x("Z_x","", r(0,2) ),
00182 Z_y("Z_y","", r(1,2) ),
00183 Z_z("Z_z","", r(2,2) )
00184 {
00185 resultBag.add(X_x.clone());
00186 resultBag.add(X_y.clone());
00187 resultBag.add(X_z.clone());
00188 resultBag.add(Y_x.clone());
00189 resultBag.add(Y_y.clone());
00190 resultBag.add(Y_z.clone());
00191 resultBag.add(Z_x.clone());
00192 resultBag.add(Z_y.clone());
00193 resultBag.add(Z_z.clone());
00194 }
00195
00196 bool RotationComposer::getResult(Rotation& res)
00197 {
00198 if ( bag.getType() == "MotCon::Rotation" || bag.getType() == "KDL.Rotation" )
00199 {
00200
00201 Property<double>* X_x = dynamic_cast<Property<double>*>( bag.find("X_x") );
00202 Property<double>* X_y = dynamic_cast<Property<double>*>( bag.find("X_y") );
00203 Property<double>* X_z = dynamic_cast<Property<double>*>( bag.find("X_z") );
00204 Property<double>* Y_x = dynamic_cast<Property<double>*>( bag.find("Y_x") );
00205 Property<double>* Y_y = dynamic_cast<Property<double>*>( bag.find("Y_y") );
00206 Property<double>* Y_z = dynamic_cast<Property<double>*>( bag.find("Y_z") );
00207 Property<double>* Z_x = dynamic_cast<Property<double>*>( bag.find("Z_x") );
00208 Property<double>* Z_y = dynamic_cast<Property<double>*>( bag.find("Z_y") );
00209 Property<double>* Z_z = dynamic_cast<Property<double>*>( bag.find("Z_z") );
00210
00211 if ( X_x != 0 && X_y != 0 && X_z != 0 &&
00212 Y_x != 0 && Y_y != 0 && Y_z != 0 &&
00213 Z_x != 0 && Z_y != 0 && Z_z != 0 )
00214 {
00215 res = Rotation(
00216 X_x->get(), Y_x->get(),Z_x->get(),
00217 X_y->get(),Y_y->get(),Z_y->get(),
00218 X_z->get(),Y_z->get(),Z_z->get()
00219 );
00220 return true;
00221 }
00222 }
00223 return false;
00224 }
00225
00230 class EulerZYXDecomposer
00231 {
00232 PropertyBag resultBag;
00233 Property<double> _a;
00234 Property<double> _b;
00235 Property<double> _g;
00236
00237 public:
00238
00239 EulerZYXDecomposer( const Rotation& r);
00240
00241 PropertyBag& result() { return resultBag; }
00242 };
00243
00248 class EulerZYXComposer
00249 {
00250 const PropertyBag& bag;
00251 public:
00252 EulerZYXComposer(const PropertyBag& _bag )
00253 : bag(_bag)
00254 {}
00255
00256 bool getResult( Rotation& res );
00257 };
00258
00259 EulerZYXDecomposer::EulerZYXDecomposer( const Rotation& r)
00260 : resultBag("KDL.Rotation"),
00261 _a("alpha","First Rotate around the Z axis with alpha in radians" ),
00262 _b("beta","Then Rotate around the new Y axis with beta in radians" ),
00263 _g("gamma","Then Rotation around the new X axis with gamma in radians" )
00264 {
00265 r.GetEulerZYX(_a.set(), _b.set(), _g.set());
00266 resultBag.add(_a.clone());
00267 resultBag.add(_b.clone());
00268 resultBag.add(_g.clone());
00269 }
00270
00271 bool EulerZYXComposer::getResult(Rotation& res )
00272 {
00273 if ( bag.getType() == "KDL.Rotation" || bag.getType() == "MotCon::Rotation" )
00274 {
00275
00276
00277 Property<double>* _a = dynamic_cast<Property<double>*>( bag.find("alpha") );
00278 if ( !_a)
00279 _a = dynamic_cast<Property<double>*>( bag.find("Z") );
00280 Property<double>* _b = dynamic_cast<Property<double>*>( bag.find("beta") );
00281 if ( !_b)
00282 _b = dynamic_cast<Property<double>*>( bag.find("Y") );
00283 Property<double>* _g = dynamic_cast<Property<double>*>( bag.find("gamma") );
00284 if ( !_g)
00285 _g = dynamic_cast<Property<double>*>( bag.find("X") );
00286
00287
00288 if ( _a != 0 && _b != 0 && _g != 0 )
00289 {
00290 res = Rotation::EulerZYX(_a->get(), _b->get(), _g->get() );
00291 return true;
00292 } else {
00293 std::string element = !_a ? "alpha" : !_b ? "beta" : "gamma";
00294 Logger::log() << Logger::Debug << "Aborting composition of (KDL.EulerZYX) Property< KDL.Rotation > "
00295 << ": Missing element '" <<element<<"'." <<Logger::endl;
00296 return false;
00297 }
00298 }
00299 return false;
00300 }
00301
00306 class RPYDecomposer
00307 {
00308 PropertyBag resultBag;
00309 Property<double> _r;
00310 Property<double> _p;
00311 Property<double> _y;
00312
00313 public:
00314
00315 RPYDecomposer( const Rotation& r);
00316
00317 PropertyBag& result() { return resultBag; }
00318 };
00319
00324 class RPYComposer
00325 {
00326 const PropertyBag& bag;
00327 public:
00328 RPYComposer(const PropertyBag& _bag )
00329 : bag(_bag)
00330 {}
00331
00332 bool getResult( Rotation& res);
00333 };
00334 RPYDecomposer::RPYDecomposer( const Rotation& r)
00335 : resultBag("KDL.Rotation" ),
00336 _r("R","First rotate around X with R(oll) in radians" ),
00337 _p("P","Next rotate around old Y with P(itch) in radians" ),
00338 _y("Y","Next rotate around old Z with Y(aw) in radians" )
00339 {
00340 r.GetRPY(_r.set(), _p.set(), _y.set());
00341 resultBag.add(_r.clone());
00342 resultBag.add(_p.clone());
00343 resultBag.add(_y.clone());
00344 }
00345
00346 bool RPYComposer::getResult(Rotation& res)
00347 {
00348 if ( bag.getType() == "KDL.Rotation" || bag.getType() == "MotCon::Rotation" )
00349 {
00350 Property<double>* _r = dynamic_cast<Property<double>*>( bag.find("R") );
00351 Property<double>* _p = dynamic_cast<Property<double>*>( bag.find("P") );
00352 Property<double>* _y = dynamic_cast<Property<double>*>( bag.find("Y") );
00353
00354
00355 if ( _r != 0 && _p != 0 && _y != 0 )
00356 {
00357 res = Rotation::RPY(_r->get(), _p->get(), _y->get() );
00358 return true;
00359 } else {
00360 std::string element = !_r ? "R" : !_p ? "P" : "Y";
00361 Logger::log() << Logger::Debug << "Aborting composition of (KDL.RPY) Property< KDL.Rotation > "
00362 << ": Missing element '" <<element<<"'." <<Logger::endl;
00363 return false;
00364 }
00365 }
00366 return false;
00367 }
00368
00369
00370 void decomposeProperty(const Vector &v, PropertyBag& targetbag)
00371 {
00372
00373 VectorDecomposer vco(v);
00374 targetbag = vco.result();
00375 }
00376
00377 bool composeProperty(const PropertyBag& bag, Vector &v)
00378 {
00379 VectorComposer vas( bag );
00380 return vas.getResult(v);
00381 }
00382
00383 void decomposeProperty(const Rotation &b, PropertyBag& targetbag)
00384 {
00385
00386 #ifdef ROTATION_PROPERTIES_EULER
00387 EulerZYXDecomposer rot(b);
00388 #else
00389 # ifdef ROTATION_PROPERTIES_RPY
00390 RPYDecomposer rot(b);
00391 # else
00392 RotationDecomposer rot(b);
00393 # endif
00394 #endif
00395 targetbag = rot.result();
00396 }
00397
00398 bool composeProperty(const PropertyBag& bag, Rotation &r)
00399 {
00400
00401 RPYComposer rpyc(bag);
00402 EulerZYXComposer eulc(bag);
00403 RotationComposer rotc(bag);
00404
00405 if ( rpyc.getResult( r ) || eulc.getResult( r ) || rotc.getResult( r ) )
00406 return true;
00407 else {
00408 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Rotation > "
00409 << ": Expected type 'KDL.Rotation', got type '"<< bag.getType() <<"'."
00410 <<Logger::endl;
00411 }
00412 return false;
00413 }
00414
00415 void decomposeProperty(const Twist &t, PropertyBag& targetbag)
00416 {
00417 targetbag.setType("KDL.Twist");
00418
00419 VectorDecomposer vel( t.vel );
00420 VectorDecomposer rot( t.rot );
00421
00422 targetbag.add( new Property<PropertyBag>("vel","Translational Velocity", vel.result() ) );
00423 targetbag.add( new Property<PropertyBag>("rot","Rotational Velocity",rot.result() ));
00424 }
00425
00426 bool composeProperty(const PropertyBag& bag, Twist &t)
00427 {
00428 if ( bag.getType() == "KDL.Twist" || bag.getType() == "MotCon::Twist" )
00429 {
00430
00431
00432 Property<PropertyBag>* subbag = bag.getPropertyType<PropertyBag>("vel");
00433 if (! subbag ) {
00434 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Twist > "
00435 << ": vel not found."
00436 <<Logger::endl;
00437 return false;
00438 }
00439 VectorComposer vas_vel( subbag->value() );
00440
00441 subbag = bag.getPropertyType<PropertyBag>("rot");
00442 if (! subbag ) {
00443 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Twist > "
00444 << ": rot not found."
00445 <<Logger::endl;
00446 return false;
00447 }
00448 VectorComposer vas_rot( subbag->value() );
00449
00450 return vas_vel.getResult( t.vel ) && vas_rot.getResult( t.rot );
00451 } else {
00452 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Twist > "
00453 << ": Expected type 'KDL.Twist', got type '"<< bag.getType() <<"'."
00454 <<Logger::endl;
00455 }
00456 return false;
00457 }
00458
00459 void decomposeProperty(const Wrench &b, PropertyBag& targetbag)
00460 {
00461
00462 targetbag.setType("KDL.Wrench");
00463
00464 VectorDecomposer force( b.force );
00465 VectorDecomposer torque( b.torque );
00466
00467 targetbag.add( new Property<PropertyBag>("force", "Axial Force", force.result() ) );
00468 targetbag.add( new Property<PropertyBag>("torque", "Axial Torque", torque.result() ) );
00469 }
00470
00471 bool composeProperty(const PropertyBag& bag,Wrench &w)
00472 {
00473 if ( bag.getType() == "KDL.Wrench" || bag.getType() == "Motcon::Wrench")
00474 {
00475
00476
00477 Property<PropertyBag>* subbag = bag.getPropertyType<PropertyBag>("force");
00478 if (! subbag ) {
00479 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Wrench > "
00480 << ": force not found."
00481 <<Logger::endl;
00482 return false;
00483 }
00484 VectorComposer vas_force( subbag->value() );
00485
00486 subbag = bag.getPropertyType<PropertyBag>("torque");
00487 if (! subbag ) {
00488 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Wrench > "
00489 << ": torque not found."
00490 <<Logger::endl;
00491 return false;
00492 }
00493 VectorComposer vas_torque( subbag->value() );
00494
00495 return vas_force.getResult( w.force ) && vas_torque.getResult( w.torque );
00496 } else {
00497 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Wrench > "
00498 << ": Expected type 'KDL.Wrench', got type '"<< bag.getType() <<"'."
00499 <<Logger::endl;
00500 return false;
00501 }
00502 return false;
00503 }
00504
00505 void decomposeProperty(const KDL::Frame &f, PropertyBag& targetbag )
00506 {
00507
00508 targetbag.setType("KDL.Frame");
00509
00510 VectorDecomposer vel( f.p );
00511 #ifdef ROTATION_PROPERTIES_EULER
00512 EulerZYXDecomposer rot( f.M );
00513 #else
00514 # ifdef ROTATION_PROPERTIES_RPY
00515 RPYDecomposer rot( f.M );
00516 # else
00517 RotationDecomposer rot( f.M );
00518 # endif
00519 #endif
00520
00521 targetbag.add( new Property<PropertyBag>("p","", vel.result() ) );
00522 targetbag.add( new Property<PropertyBag>("M","", rot.result() ) );
00523 }
00524
00525 bool composeProperty(const PropertyBag& f_bag, KDL::Frame &f)
00526 {
00527 if ( f_bag.getType() == "KDL.Frame" || f_bag.getType() == "MotCon::Frame" )
00528 {
00529
00530 Property<PropertyBag>* subbag = f_bag.getPropertyType<PropertyBag>("p");
00531 if (! subbag ) {
00532 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > "
00533 << ": p not found."
00534 <<Logger::endl;
00535 return false;
00536 }
00537 VectorComposer vas_pos( subbag->value() );
00538
00539 subbag = f_bag.getPropertyType<PropertyBag>("M");
00540 if (! subbag ) {
00541 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > "
00542 << ": M not found."
00543 <<Logger::endl;
00544 return false;
00545 }
00546 RPYComposer vas_rpy( subbag->value() );
00547 EulerZYXComposer vas_eul( subbag->value() );
00548 RotationComposer vas_rot( subbag->value() );
00549 bool result = vas_pos.getResult( f.p );
00550 if (!result )
00551 {
00552 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > "
00553 << ": element 'p' has wrong format." <<Logger::endl;
00554 return false;
00555 }
00556 result = vas_rpy.getResult( f.M) ||
00557 vas_eul.getResult( f.M ) ||
00558 vas_rot.getResult( f.M );
00559 if (!result )
00560 {
00561 Logger::log()
00562 << Logger::Error << "Aborting composition of Property< KDL.Frame > "
00563 << ": Could not compose 'M' type 'KDL.Rotation', got type '"
00564 << subbag->get().getType() <<"'."<<Logger::endl;
00565 return false;
00566 }
00567
00568 return true;
00569 } else {
00570 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > "
00571 << ": Expected type 'KDL.Frame', got type '"<< f_bag.getType() <<"'."
00572 <<Logger::endl;
00573 return false;
00574 }
00575 return false;
00576 }
00577
00578 }