$search
00001 /****************************************************************************** 00002 * This file is part of the KDL project * 00003 * * 00004 * (C) 2004 Peter Soetens * 00005 * 2010 Ruben Smits * 00006 * 2010 Steven Bellens * 00007 * peter.soetens@mech.kuleuven.be * 00008 * ruben.smits@mech.kuleuven.be * 00009 * steven.bellens@mech.kuleuven.be * 00010 * Department of Mechanical Engineering, * 00011 * Katholieke Universiteit Leuven, Belgium. * 00012 * * 00013 * You may redistribute this software and/or modify it under either the * 00014 * terms of the GNU Lesser General Public License version 2.1 (LGPLv2.1 * 00015 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>) or (at your * 00016 * discretion) of the Modified BSD License: * 00017 * Redistribution and use in source and binary forms, with or without * 00018 * modification, are permitted provided that the following conditions * 00019 * are met: * 00020 * 1. Redistributions of source code must retain the above copyright * 00021 * notice, this list of conditions and the following disclaimer. * 00022 * 2. Redistributions in binary form must reproduce the above copyright * 00023 * notice, this list of conditions and the following disclaimer in the * 00024 * documentation and/or other materials provided with the distribution. * 00025 * 3. The name of the author may not be used to endorse or promote * 00026 * products derived from this software without specific prior written * 00027 * permission. * 00028 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR * 00029 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * 00030 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 00031 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,* 00032 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 00033 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * 00034 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * 00035 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, * 00036 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING * 00037 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * 00038 * POSSIBILITY OF SUCH DAMAGE. * 00039 * * 00040 *******************************************************************************/ 00041 00042 #include <rtt/Property.hpp> 00043 #include <rtt/PropertyBag.hpp> 00044 #include <rtt/base/PropertyIntrospection.hpp> 00045 #include <rtt/Logger.hpp> 00046 #include <sstream> 00047 00048 #include "motionproperties.hpp" 00049 //#include "kdltk.hpp" 00050 00051 namespace RTT 00052 { 00053 using namespace std; 00054 00055 00056 template class Property<KDL::Frame>; 00057 template class Property<KDL::Wrench>; 00058 template class Property<KDL::Twist>; 00059 template class Property<KDL::Rotation>; 00060 template class Property<KDL::Vector>; 00061 00072 class VectorDecomposer 00073 { 00074 PropertyBag resultBag; 00075 Property<double> X; 00076 Property<double> Y; 00077 Property<double> Z; 00078 00079 public: 00080 00081 VectorDecomposer( const Vector& v); 00082 00083 PropertyBag& result() { return resultBag; } 00084 }; 00085 00090 class VectorComposer 00091 { 00092 const PropertyBag& bag; 00093 public: 00094 VectorComposer(const PropertyBag& _bag ) 00095 : bag(_bag) 00096 {} 00097 00098 bool getResult( Vector& res); 00099 }; 00100 00101 VectorDecomposer::VectorDecomposer( const Vector& v ) 00102 : resultBag("KDL.Vector"), // bag_type 00103 X("X","X Value", v[0]), 00104 Y("Y","Y Value", v[1]), 00105 Z("Z","Z Value", v[2]) 00106 { 00107 resultBag.add(X.clone()); 00108 resultBag.add(Y.clone()); 00109 resultBag.add(Z.clone()); 00110 } 00111 00112 bool VectorComposer::getResult(Vector& res) 00113 { 00114 if ( bag.getType() == "MotCon::Vector" || bag.getType() == "KDL.Vector" ) 00115 { 00116 Property<double>* px = dynamic_cast<Property<double>*>( bag.find("X") ); 00117 Property<double>* py = dynamic_cast<Property<double>*>( bag.find("Y") ); 00118 Property<double>* pz = dynamic_cast<Property<double>*>( bag.find("Z") ); 00119 // found it. 00120 if ( px != 0 && py != 0 && pz != 0) 00121 { 00122 res = Vector( px->get(),py->get(),pz->get() ); 00123 return true; 00124 } else { 00125 std::string element = !px ? "X" : !py ? "Y" : "Z"; 00126 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Vector > " 00127 << ": Missing element '" <<element<<"'." <<Logger::endl; 00128 return false; 00129 } 00130 } else { 00131 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Vector > " 00132 << ": Expected type 'KDL.Vector', got type '"<< bag.getType() <<"'." 00133 <<Logger::endl; 00134 } 00135 return false; 00136 } 00137 00138 00149 class RotationDecomposer 00150 { 00151 PropertyBag resultBag; 00152 Property<double> X_x; 00153 Property<double> X_y; 00154 Property<double> X_z; 00155 Property<double> Y_x; 00156 Property<double> Y_y; 00157 Property<double> Y_z; 00158 Property<double> Z_x; 00159 Property<double> Z_y; 00160 Property<double> Z_z; 00161 public: 00162 00163 RotationDecomposer( const Rotation& r ); 00164 00165 PropertyBag& result() { return resultBag; } 00166 }; 00167 00172 class RotationComposer 00173 { 00174 const PropertyBag& bag; 00175 public: 00176 RotationComposer(const PropertyBag& _bag ) 00177 : bag(_bag) 00178 {} 00179 00180 bool getResult( Rotation& res ); 00181 }; 00182 00183 RotationDecomposer::RotationDecomposer( const Rotation& r) 00184 : resultBag("KDL.Rotation"), 00185 X_x("X_x","", r(0,0) ), 00186 X_y("X_y","", r(1,0) ), 00187 X_z("X_z","", r(2,0) ), 00188 Y_x("Y_x","", r(0,1) ), 00189 Y_y("Y_y","", r(1,1) ), 00190 Y_z("Y_z","", r(2,1) ), 00191 Z_x("Z_x","", r(0,2) ), 00192 Z_y("Z_y","", r(1,2) ), 00193 Z_z("Z_z","", r(2,2) ) 00194 { 00195 resultBag.add(X_x.clone()); 00196 resultBag.add(X_y.clone()); 00197 resultBag.add(X_z.clone()); 00198 resultBag.add(Y_x.clone()); 00199 resultBag.add(Y_y.clone()); 00200 resultBag.add(Y_z.clone()); 00201 resultBag.add(Z_x.clone()); 00202 resultBag.add(Z_y.clone()); 00203 resultBag.add(Z_z.clone()); 00204 } 00205 00206 bool RotationComposer::getResult(Rotation& res) 00207 { 00208 if ( bag.getType() == "MotCon::Rotation" || bag.getType() == "KDL.Rotation" ) 00209 { 00210 00211 Property<double>* X_x = dynamic_cast<Property<double>*>( bag.find("X_x") ); 00212 Property<double>* X_y = dynamic_cast<Property<double>*>( bag.find("X_y") ); 00213 Property<double>* X_z = dynamic_cast<Property<double>*>( bag.find("X_z") ); 00214 Property<double>* Y_x = dynamic_cast<Property<double>*>( bag.find("Y_x") ); 00215 Property<double>* Y_y = dynamic_cast<Property<double>*>( bag.find("Y_y") ); 00216 Property<double>* Y_z = dynamic_cast<Property<double>*>( bag.find("Y_z") ); 00217 Property<double>* Z_x = dynamic_cast<Property<double>*>( bag.find("Z_x") ); 00218 Property<double>* Z_y = dynamic_cast<Property<double>*>( bag.find("Z_y") ); 00219 Property<double>* Z_z = dynamic_cast<Property<double>*>( bag.find("Z_z") ); 00220 // found it. 00221 if ( X_x != 0 && X_y != 0 && X_z != 0 && 00222 Y_x != 0 && Y_y != 0 && Y_z != 0 && 00223 Z_x != 0 && Z_y != 0 && Z_z != 0 ) 00224 { 00225 res = Rotation( 00226 X_x->get(), Y_x->get(),Z_x->get(), 00227 X_y->get(),Y_y->get(),Z_y->get(), 00228 X_z->get(),Y_z->get(),Z_z->get() 00229 ); 00230 return true; 00231 } 00232 } 00233 return false; 00234 } 00235 00240 class EulerZYXDecomposer 00241 { 00242 PropertyBag resultBag; 00243 Property<double> _a; 00244 Property<double> _b; 00245 Property<double> _g; 00246 00247 public: 00248 00249 EulerZYXDecomposer( const Rotation& r); 00250 00251 PropertyBag& result() { return resultBag; } 00252 }; 00253 00258 class EulerZYXComposer 00259 { 00260 const PropertyBag& bag; 00261 public: 00262 EulerZYXComposer(const PropertyBag& _bag ) 00263 : bag(_bag) 00264 {} 00265 00266 bool getResult( Rotation& res ); 00267 }; 00268 00269 EulerZYXDecomposer::EulerZYXDecomposer( const Rotation& r) 00270 : resultBag("KDL.Rotation"), 00271 _a("alpha","First Rotate around the Z axis with alpha in radians" ), 00272 _b("beta","Then Rotate around the new Y axis with beta in radians" ), 00273 _g("gamma","Then Rotation around the new X axis with gamma in radians" ) 00274 { 00275 r.GetEulerZYX(_a.set(), _b.set(), _g.set()); 00276 resultBag.add(_a.clone()); 00277 resultBag.add(_b.clone()); 00278 resultBag.add(_g.clone()); 00279 } 00280 00281 bool EulerZYXComposer::getResult(Rotation& res ) 00282 { 00283 if ( bag.getType() == "KDL.Rotation" || bag.getType() == "MotCon::Rotation" ) 00284 { 00285 00286 // ZYX is deprecated, use alpha, beta, gamma. also alpha maps to Z and gamma to X ! 00287 Property<double>* _a = dynamic_cast<Property<double>*>( bag.find("alpha") ); 00288 if ( !_a) 00289 _a = dynamic_cast<Property<double>*>( bag.find("Z") ); 00290 Property<double>* _b = dynamic_cast<Property<double>*>( bag.find("beta") ); 00291 if ( !_b) 00292 _b = dynamic_cast<Property<double>*>( bag.find("Y") ); 00293 Property<double>* _g = dynamic_cast<Property<double>*>( bag.find("gamma") ); 00294 if ( !_g) 00295 _g = dynamic_cast<Property<double>*>( bag.find("X") ); 00296 00297 // found it. 00298 if ( _a != 0 && _b != 0 && _g != 0 ) 00299 { 00300 res = Rotation::EulerZYX(_a->get(), _b->get(), _g->get() ); 00301 return true; 00302 } else { 00303 std::string element = !_a ? "alpha" : !_b ? "beta" : "gamma"; 00304 Logger::log() << Logger::Error << "Aborting composition of (KDL.EulerZYX) Property< KDL.Rotation > " 00305 << ": Missing element '" <<element<<"'." <<Logger::endl; 00306 return false; 00307 } 00308 } 00309 return false; 00310 } 00311 00316 class RPYDecomposer 00317 { 00318 PropertyBag resultBag; 00319 Property<double> _r; 00320 Property<double> _p; 00321 Property<double> _y; 00322 00323 public: 00324 00325 RPYDecomposer( const Rotation& r); 00326 00327 PropertyBag& result() { return resultBag; } 00328 }; 00329 00334 class RPYComposer 00335 { 00336 const PropertyBag& bag; 00337 public: 00338 RPYComposer(const PropertyBag& _bag ) 00339 : bag(_bag) 00340 {} 00341 00342 bool getResult( Rotation& res); 00343 }; 00344 RPYDecomposer::RPYDecomposer( const Rotation& r) 00345 : resultBag("KDL.Rotation" ), 00346 _r("R","First rotate around X with R(oll) in radians" ), 00347 _p("P","Next rotate around old Y with P(itch) in radians" ), 00348 _y("Y","Next rotate around old Z with Y(aw) in radians" ) 00349 { 00350 r.GetRPY(_r.set(), _p.set(), _y.set()); 00351 resultBag.add(_r.clone()); 00352 resultBag.add(_p.clone()); 00353 resultBag.add(_y.clone()); 00354 } 00355 00356 bool RPYComposer::getResult(Rotation& res) 00357 { 00358 if ( bag.getType() == "KDL.Rotation" || bag.getType() == "MotCon::Rotation" ) 00359 { 00360 Property<double>* _r = dynamic_cast<Property<double>*>( bag.find("R") ); 00361 Property<double>* _p = dynamic_cast<Property<double>*>( bag.find("P") ); 00362 Property<double>* _y = dynamic_cast<Property<double>*>( bag.find("Y") ); 00363 00364 // found it. 00365 if ( _r != 0 && _p != 0 && _y != 0 ) 00366 { 00367 res = Rotation::RPY(_r->get(), _p->get(), _y->get() ); 00368 return true; 00369 } else { 00370 std::string element = !_r ? "R" : !_p ? "P" : "Y"; 00371 Logger::log() << Logger::Error << "Aborting composition of (KDL.RPY) Property< KDL.Rotation > " 00372 << ": Missing element '" <<element<<"'." <<Logger::endl; 00373 return false; 00374 } 00375 } 00376 return false; 00377 } 00378 00379 00380 void decomposeProperty(const Vector &v, PropertyBag& targetbag) 00381 { 00382 // construct a property with same name and description, but containing a typed PropertyBag. 00383 VectorDecomposer vco(v); 00384 targetbag = vco.result(); 00385 } 00386 00387 bool composeProperty(const PropertyBag& bag, Vector &v) 00388 { 00389 VectorComposer vas( bag ); 00390 return vas.getResult(v); 00391 } 00392 00393 void decomposeProperty(const Rotation &b, PropertyBag& targetbag) 00394 { 00395 // construct a property with same name and description, but containing a typed PropertyBag. 00396 #ifdef ROTATION_PROPERTIES_EULER 00397 EulerZYXDecomposer rot(b); 00398 #else 00399 # ifdef ROTATION_PROPERTIES_RPY 00400 RPYDecomposer rot(b); 00401 # else 00402 RotationDecomposer rot(b); 00403 # endif 00404 #endif 00405 targetbag = rot.result(); 00406 } 00407 00408 bool composeProperty(const PropertyBag& bag, Rotation &r) 00409 { 00410 // try all three, see which one works, that one will fill in r. 00411 RPYComposer rpyc(bag); 00412 EulerZYXComposer eulc(bag); 00413 RotationComposer rotc(bag); 00414 00415 if ( rpyc.getResult( r ) || eulc.getResult( r ) || rotc.getResult( r ) ) 00416 return true; 00417 else { 00418 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Rotation > " 00419 << ": Expected type 'KDL.Rotation', got type '"<< bag.getType() <<"'." 00420 <<Logger::endl; 00421 } 00422 return false; 00423 } 00424 00425 void decomposeProperty(const Twist &t, PropertyBag& targetbag) 00426 { 00427 targetbag.setType("KDL.Twist"); // bag_type 00428 00429 VectorDecomposer vel( t.vel ); 00430 VectorDecomposer rot( t.rot ); 00431 00432 targetbag.add( new Property<PropertyBag>("vel","Translational Velocity", vel.result() ) ); 00433 targetbag.add( new Property<PropertyBag>("rot","Rotational Velocity",rot.result() )); 00434 } 00435 00436 bool composeProperty(const PropertyBag& bag, Twist &t) 00437 { 00438 if ( bag.getType() == "KDL.Twist" || bag.getType() == "MotCon::Twist" ) 00439 { 00440 00441 // pass the subbag to the vector Composers 00442 Property<PropertyBag>* subbag = bag.getPropertyType<PropertyBag>("vel"); 00443 if (! subbag ) { 00444 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Twist > " 00445 << ": Trans_Vel not found." 00446 <<Logger::endl; 00447 return false; 00448 } 00449 VectorComposer vas_vel( subbag->value() ); 00450 00451 subbag = bag.getPropertyType<PropertyBag>("rot"); 00452 if (! subbag ) { 00453 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Twist > " 00454 << ": Rot_Vel not found." 00455 <<Logger::endl; 00456 return false; 00457 } 00458 VectorComposer vas_rot( subbag->value() ); 00459 00460 return vas_vel.getResult( t.vel ) && vas_rot.getResult( t.rot ); 00461 } else { 00462 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Twist > " 00463 << ": Expected type 'KDL.Twist', got type '"<< bag.getType() <<"'." 00464 <<Logger::endl; 00465 } 00466 return false; 00467 } 00468 00469 void decomposeProperty(const Wrench &b, PropertyBag& targetbag) 00470 { 00471 // construct a property with same name and description, but containing a typed PropertyBag. 00472 targetbag.setType("KDL.Wrench"); // bag_type 00473 00474 VectorDecomposer force( b.force ); 00475 VectorDecomposer torque( b.torque ); 00476 00477 targetbag.add( new Property<PropertyBag>("force", "Axial Force", force.result() ) ); 00478 targetbag.add( new Property<PropertyBag>("torque", "Axial Torque", torque.result() ) ); 00479 } 00480 00481 bool composeProperty(const PropertyBag& bag,Wrench &w) 00482 { 00483 if ( bag.getType() == "KDL.Wrench" || bag.getType() == "Motcon::Wrench") 00484 { 00485 00486 // pass this bag to the vector Composers 00487 Property<PropertyBag>* subbag = bag.getPropertyType<PropertyBag>("force"); 00488 if (! subbag ) { 00489 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Wrench > " 00490 << ": Force not found." 00491 <<Logger::endl; 00492 return false; 00493 } 00494 VectorComposer vas_force( subbag->value() ); 00495 00496 subbag = bag.getPropertyType<PropertyBag>("torque"); 00497 if (! subbag ) { 00498 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Wrench > " 00499 << ": Torque not found." 00500 <<Logger::endl; 00501 return false; 00502 } 00503 VectorComposer vas_torque( subbag->value() ); 00504 00505 return vas_force.getResult( w.force ) && vas_torque.getResult( w.torque ); 00506 } else { 00507 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Wrench > " 00508 << ": Expected type 'KDL.Wrench', got type '"<< bag.getType() <<"'." 00509 <<Logger::endl; 00510 return false; 00511 } 00512 return false; 00513 } 00514 00515 void decomposeProperty(const KDL::Frame &f, PropertyBag& targetbag ) 00516 { 00517 // construct a typed PropertyBag. 00518 targetbag.setType("KDL.Frame"); 00519 00520 VectorDecomposer vel( f.p ); 00521 #ifdef ROTATION_PROPERTIES_EULER 00522 EulerZYXDecomposer rot( f.M ); 00523 #else 00524 # ifdef ROTATION_PROPERTIES_RPY 00525 RPYDecomposer rot( f.M ); 00526 # else 00527 RotationDecomposer rot( f.M ); 00528 # endif 00529 #endif 00530 00531 targetbag.add( new Property<PropertyBag>("p","", vel.result() ) ); 00532 targetbag.add( new Property<PropertyBag>("M","", rot.result() ) ); 00533 } 00534 00535 bool composeProperty(const PropertyBag& f_bag, KDL::Frame &f) 00536 { 00537 if ( f_bag.getType() == "KDL.Frame" || f_bag.getType() == "MotCon::Frame" ) 00538 { 00539 // pass this bag to the vector Composers 00540 Property<PropertyBag>* subbag = f_bag.getPropertyType<PropertyBag>("p"); 00541 if (! subbag ) { 00542 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > " 00543 << ": Position not found." 00544 <<Logger::endl; 00545 return false; 00546 } 00547 VectorComposer vas_pos( subbag->value() ); 00548 00549 subbag = f_bag.getPropertyType<PropertyBag>("M"); 00550 if (! subbag ) { 00551 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > " 00552 << ": Rotation not found." 00553 <<Logger::endl; 00554 return false; 00555 } 00556 RPYComposer vas_rpy( subbag->value() ); 00557 EulerZYXComposer vas_eul( subbag->value() ); 00558 RotationComposer vas_rot( subbag->value() ); 00559 bool result = vas_pos.getResult( f.p ); 00560 if (!result ) 00561 { 00562 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > " 00563 << ": element 'Position' has wrong format." <<Logger::endl; 00564 return false; 00565 } 00566 result = vas_rpy.getResult( f.M) || 00567 vas_eul.getResult( f.M ) || 00568 vas_rot.getResult( f.M ); 00569 if (!result ) 00570 { 00571 Logger::log() 00572 << Logger::Error << "Aborting composition of Property< KDL.Frame > " 00573 << ": Could not compose 'Rotation' type 'KDL.Rotation','KDL.EulerZYX' or 'KDL.RPY', got type '" 00574 << subbag->get().getType() <<"'."<<Logger::endl; 00575 return false; 00576 } 00577 // OK: exit. 00578 return true; 00579 } else { 00580 Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > " 00581 << ": Expected type 'KDL.Frame', got type '"<< f_bag.getType() <<"'." 00582 <<Logger::endl; 00583 return false; 00584 } 00585 return false; 00586 } 00587 00588 } // MotionControl