motionproperties.cpp
Go to the documentation of this file.
1 /******************************************************************************
2 * This file is part of the KDL project *
3 * *
4 * (C) 2004 Peter Soetens *
5 * 2010 Ruben Smits *
6 * 2010 Steven Bellens *
7 * peter.soetens@mech.kuleuven.be *
8 * ruben.smits@mech.kuleuven.be *
9 * steven.bellens@mech.kuleuven.be *
10 * Department of Mechanical Engineering, *
11 * Katholieke Universiteit Leuven, Belgium. *
12 * *
13 * You may redistribute this software and/or modify it under either the *
14 * terms of the GNU Lesser General Public License version 2.1 (LGPLv2.1 *
15 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>) or (at your *
16 * discretion) of the Modified BSD License: *
17 * Redistribution and use in source and binary forms, with or without *
18 * modification, are permitted provided that the following conditions *
19 * are met: *
20 * 1. Redistributions of source code must retain the above copyright *
21 * notice, this list of conditions and the following disclaimer. *
22 * 2. Redistributions in binary form must reproduce the above copyright *
23 * notice, this list of conditions and the following disclaimer in the *
24 * documentation and/or other materials provided with the distribution. *
25 * 3. The name of the author may not be used to endorse or promote *
26 * products derived from this software without specific prior written *
27 * permission. *
28 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR *
29 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED *
30 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
31 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,*
32 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
33 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS *
34 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) *
35 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, *
36 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING *
37 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE *
38 * POSSIBILITY OF SUCH DAMAGE. *
39 * *
40 *******************************************************************************/
41 
42 #include "motionproperties.hpp"
43 
44 #include <rtt/Property.hpp>
45 #include <rtt/PropertyBag.hpp>
46 #include <rtt/Logger.hpp>
47 
48 namespace RTT
49 {
50  using namespace std;
51 
63  {
68 
69  public:
70 
71  VectorDecomposer( const Vector& v);
72 
73  PropertyBag& result() { return resultBag; }
74  };
75 
81  {
82  const PropertyBag& bag;
83  public:
85  : bag(_bag)
86  {}
87 
88  bool getResult( Vector& res);
89  };
90 
92  : resultBag("KDL.Vector"), // bag_type
93  X("X","X Value", v[0]),
94  Y("Y","Y Value", v[1]),
95  Z("Z","Z Value", v[2])
96  {
100  }
101 
103  {
104  if ( bag.getType() == "MotCon::Vector" || bag.getType() == "KDL.Vector" )
105  {
106  Property<double>* px = dynamic_cast<Property<double>*>( bag.find("X") );
107  Property<double>* py = dynamic_cast<Property<double>*>( bag.find("Y") );
108  Property<double>* pz = dynamic_cast<Property<double>*>( bag.find("Z") );
109  // found it.
110  if ( px != 0 && py != 0 && pz != 0)
111  {
112  res = Vector( px->get(),py->get(),pz->get() );
113  return true;
114  } else {
115  std::string element = !px ? "X" : !py ? "Y" : "Z";
116  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Vector > "
117  << ": Missing element '" <<element<<"'." <<Logger::endl;
118  return false;
119  }
120  } else {
121  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Vector > "
122  << ": Expected type 'KDL.Vector', got type '"<< bag.getType() <<"'."
123  <<Logger::endl;
124  }
125  return false;
126  }
127 
128 
140  {
151  public:
152 
153  RotationDecomposer( const Rotation& r );
154 
155  PropertyBag& result() { return resultBag; }
156  };
157 
163  {
164  const PropertyBag& bag;
165  public:
167  : bag(_bag)
168  {}
169 
170  bool getResult( Rotation& res );
171  };
172 
174  : resultBag("KDL.Rotation"),
175  X_x("X_x","", r(0,0) ),
176  X_y("X_y","", r(1,0) ),
177  X_z("X_z","", r(2,0) ),
178  Y_x("Y_x","", r(0,1) ),
179  Y_y("Y_y","", r(1,1) ),
180  Y_z("Y_z","", r(2,1) ),
181  Z_x("Z_x","", r(0,2) ),
182  Z_y("Z_y","", r(1,2) ),
183  Z_z("Z_z","", r(2,2) )
184  {
194  }
195 
197  {
198  if ( bag.getType() == "MotCon::Rotation" || bag.getType() == "KDL.Rotation" )
199  {
200 
201  Property<double>* X_x = dynamic_cast<Property<double>*>( bag.find("X_x") );
202  Property<double>* X_y = dynamic_cast<Property<double>*>( bag.find("X_y") );
203  Property<double>* X_z = dynamic_cast<Property<double>*>( bag.find("X_z") );
204  Property<double>* Y_x = dynamic_cast<Property<double>*>( bag.find("Y_x") );
205  Property<double>* Y_y = dynamic_cast<Property<double>*>( bag.find("Y_y") );
206  Property<double>* Y_z = dynamic_cast<Property<double>*>( bag.find("Y_z") );
207  Property<double>* Z_x = dynamic_cast<Property<double>*>( bag.find("Z_x") );
208  Property<double>* Z_y = dynamic_cast<Property<double>*>( bag.find("Z_y") );
209  Property<double>* Z_z = dynamic_cast<Property<double>*>( bag.find("Z_z") );
210  // found it.
211  if ( X_x != 0 && X_y != 0 && X_z != 0 &&
212  Y_x != 0 && Y_y != 0 && Y_z != 0 &&
213  Z_x != 0 && Z_y != 0 && Z_z != 0 )
214  {
215  res = Rotation(
216  X_x->get(), Y_x->get(),Z_x->get(),
217  X_y->get(),Y_y->get(),Z_y->get(),
218  X_z->get(),Y_z->get(),Z_z->get()
219  );
220  return true;
221  }
222  }
223  return false;
224  }
225 
231  {
236 
237  public:
238 
239  EulerZYXDecomposer( const Rotation& r);
240 
241  PropertyBag& result() { return resultBag; }
242  };
243 
249  {
250  const PropertyBag& bag;
251  public:
253  : bag(_bag)
254  {}
255 
256  bool getResult( Rotation& res );
257  };
258 
260  : resultBag("KDL.Rotation"),
261  _a("alpha","First Rotate around the Z axis with alpha in radians" ),
262  _b("beta","Then Rotate around the new Y axis with beta in radians" ),
263  _g("gamma","Then Rotation around the new X axis with gamma in radians" )
264  {
265  r.GetEulerZYX(_a.set(), _b.set(), _g.set());
269  }
270 
272  {
273  if ( bag.getType() == "KDL.Rotation" || bag.getType() == "MotCon::Rotation" )
274  {
275 
276  // ZYX is deprecated, use alpha, beta, gamma. also alpha maps to Z and gamma to X !
277  Property<double>* _a = dynamic_cast<Property<double>*>( bag.find("alpha") );
278  if ( !_a)
279  _a = dynamic_cast<Property<double>*>( bag.find("Z") );
280  Property<double>* _b = dynamic_cast<Property<double>*>( bag.find("beta") );
281  if ( !_b)
282  _b = dynamic_cast<Property<double>*>( bag.find("Y") );
283  Property<double>* _g = dynamic_cast<Property<double>*>( bag.find("gamma") );
284  if ( !_g)
285  _g = dynamic_cast<Property<double>*>( bag.find("X") );
286 
287  // found it.
288  if ( _a != 0 && _b != 0 && _g != 0 )
289  {
290  res = Rotation::EulerZYX(_a->get(), _b->get(), _g->get() );
291  return true;
292  } else {
293  std::string element = !_a ? "alpha" : !_b ? "beta" : "gamma";
294  Logger::log() << Logger::Debug << "Aborting composition of (KDL.EulerZYX) Property< KDL.Rotation > "
295  << ": Missing element '" <<element<<"'." <<Logger::endl;
296  return false;
297  }
298  }
299  return false;
300  }
301 
307  {
312 
313  public:
314 
315  RPYDecomposer( const Rotation& r);
316 
317  PropertyBag& result() { return resultBag; }
318  };
319 
325  {
326  const PropertyBag& bag;
327  public:
328  RPYComposer(const PropertyBag& _bag )
329  : bag(_bag)
330  {}
331 
332  bool getResult( Rotation& res);
333  };
335  : resultBag("KDL.Rotation" ),
336  _r("R","First rotate around X with R(oll) in radians" ),
337  _p("P","Next rotate around old Y with P(itch) in radians" ),
338  _y("Y","Next rotate around old Z with Y(aw) in radians" )
339  {
340  r.GetRPY(_r.set(), _p.set(), _y.set());
344  }
345 
347  {
348  if ( bag.getType() == "KDL.Rotation" || bag.getType() == "MotCon::Rotation" )
349  {
350  Property<double>* _r = dynamic_cast<Property<double>*>( bag.find("R") );
351  Property<double>* _p = dynamic_cast<Property<double>*>( bag.find("P") );
352  Property<double>* _y = dynamic_cast<Property<double>*>( bag.find("Y") );
353 
354  // found it.
355  if ( _r != 0 && _p != 0 && _y != 0 )
356  {
357  res = Rotation::RPY(_r->get(), _p->get(), _y->get() );
358  return true;
359  } else {
360  std::string element = !_r ? "R" : !_p ? "P" : "Y";
361  Logger::log() << Logger::Debug << "Aborting composition of (KDL.RPY) Property< KDL.Rotation > "
362  << ": Missing element '" <<element<<"'." <<Logger::endl;
363  return false;
364  }
365  }
366  return false;
367  }
368 
369 
370  void decomposeProperty(const Vector &v, PropertyBag& targetbag)
371  {
372  // construct a property with same name and description, but containing a typed PropertyBag.
373  VectorDecomposer vco(v);
374  targetbag = vco.result();
375  }
376 
377  bool composeProperty(const PropertyBag& bag, Vector &v)
378  {
379  VectorComposer vas( bag );
380  return vas.getResult(v);
381  }
382 
383  void decomposeProperty(const Rotation &b, PropertyBag& targetbag)
384  {
385  // construct a property with same name and description, but containing a typed PropertyBag.
386 #ifdef ROTATION_PROPERTIES_EULER
387  EulerZYXDecomposer rot(b);
388 #else
389 # ifdef ROTATION_PROPERTIES_RPY
390  RPYDecomposer rot(b);
391 # else
392  RotationDecomposer rot(b);
393 # endif
394 #endif
395  targetbag = rot.result();
396  }
397 
398  bool composeProperty(const PropertyBag& bag, Rotation &r)
399  {
400  // try all three, see which one works, that one will fill in r.
401  RPYComposer rpyc(bag);
402  EulerZYXComposer eulc(bag);
403  RotationComposer rotc(bag);
404 
405  if ( rpyc.getResult( r ) || eulc.getResult( r ) || rotc.getResult( r ) )
406  return true;
407  else {
408  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Rotation > "
409  << ": Expected type 'KDL.Rotation', got type '"<< bag.getType() <<"'."
410  <<Logger::endl;
411  }
412  return false;
413  }
414 
415  void decomposeProperty(const Twist &t, PropertyBag& targetbag)
416  {
417  targetbag.setType("KDL.Twist"); // bag_type
418 
419  VectorDecomposer vel( t.vel );
420  VectorDecomposer rot( t.rot );
421 
422  targetbag.ownProperty( new Property<PropertyBag>("vel","Translational Velocity", vel.result() ) );
423  targetbag.ownProperty( new Property<PropertyBag>("rot","Rotational Velocity",rot.result() ));
424  }
425 
426  bool composeProperty(const PropertyBag& bag, Twist &t)
427  {
428  if ( bag.getType() == "KDL.Twist" || bag.getType() == "MotCon::Twist" )
429  {
430 
431  // pass the subbag to the vector Composers
432  Property<PropertyBag>* subbag = bag.getPropertyType<PropertyBag>("vel");
433  if (! subbag ) {
434  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Twist > "
435  << ": vel not found."
436  <<Logger::endl;
437  return false;
438  }
439  VectorComposer vas_vel( subbag->value() );
440 
441  subbag = bag.getPropertyType<PropertyBag>("rot");
442  if (! subbag ) {
443  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Twist > "
444  << ": rot not found."
445  <<Logger::endl;
446  return false;
447  }
448  VectorComposer vas_rot( subbag->value() );
449 
450  return vas_vel.getResult( t.vel ) && vas_rot.getResult( t.rot );
451  } else {
452  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Twist > "
453  << ": Expected type 'KDL.Twist', got type '"<< bag.getType() <<"'."
454  <<Logger::endl;
455  }
456  return false;
457  }
458 
459  void decomposeProperty(const Wrench &b, PropertyBag& targetbag)
460  {
461  // construct a property with same name and description, but containing a typed PropertyBag.
462  targetbag.setType("KDL.Wrench"); // bag_type
463 
464  VectorDecomposer force( b.force );
465  VectorDecomposer torque( b.torque );
466 
467  targetbag.ownProperty( new Property<PropertyBag>("force", "Axial Force", force.result() ) );
468  targetbag.ownProperty( new Property<PropertyBag>("torque", "Axial Torque", torque.result() ) );
469  }
470 
471  bool composeProperty(const PropertyBag& bag,Wrench &w)
472  {
473  if ( bag.getType() == "KDL.Wrench" || bag.getType() == "Motcon::Wrench")
474  {
475 
476  // pass this bag to the vector Composers
477  Property<PropertyBag>* subbag = bag.getPropertyType<PropertyBag>("force");
478  if (! subbag ) {
479  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Wrench > "
480  << ": force not found."
481  <<Logger::endl;
482  return false;
483  }
484  VectorComposer vas_force( subbag->value() );
485 
486  subbag = bag.getPropertyType<PropertyBag>("torque");
487  if (! subbag ) {
488  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Wrench > "
489  << ": torque not found."
490  <<Logger::endl;
491  return false;
492  }
493  VectorComposer vas_torque( subbag->value() );
494 
495  return vas_force.getResult( w.force ) && vas_torque.getResult( w.torque );
496  } else {
497  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Wrench > "
498  << ": Expected type 'KDL.Wrench', got type '"<< bag.getType() <<"'."
499  <<Logger::endl;
500  return false;
501  }
502  return false;
503  }
504 
505  void decomposeProperty(const KDL::Frame &f, PropertyBag& targetbag )
506  {
507  // construct a typed PropertyBag.
508  targetbag.setType("KDL.Frame");
509 
510  VectorDecomposer vel( f.p );
511 #ifdef ROTATION_PROPERTIES_EULER
512  EulerZYXDecomposer rot( f.M );
513 #else
514 # ifdef ROTATION_PROPERTIES_RPY
515  RPYDecomposer rot( f.M );
516 # else
517  RotationDecomposer rot( f.M );
518 # endif
519 #endif
520 
521  targetbag.ownProperty( new Property<PropertyBag>("p","", vel.result() ) );
522  targetbag.ownProperty( new Property<PropertyBag>("M","", rot.result() ) );
523  }
524 
525  bool composeProperty(const PropertyBag& f_bag, KDL::Frame &f)
526  {
527  if ( f_bag.getType() == "KDL.Frame" || f_bag.getType() == "MotCon::Frame" )
528  {
529  // pass this bag to the vector Composers
530  Property<PropertyBag>* subbag = f_bag.getPropertyType<PropertyBag>("p");
531  if (! subbag ) {
532  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > "
533  << ": p not found."
534  <<Logger::endl;
535  return false;
536  }
537  VectorComposer vas_pos( subbag->value() );
538 
539  subbag = f_bag.getPropertyType<PropertyBag>("M");
540  if (! subbag ) {
541  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > "
542  << ": M not found."
543  <<Logger::endl;
544  return false;
545  }
546  RPYComposer vas_rpy( subbag->value() );
547  EulerZYXComposer vas_eul( subbag->value() );
548  RotationComposer vas_rot( subbag->value() );
549  bool result = vas_pos.getResult( f.p );
550  if (!result )
551  {
552  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > "
553  << ": element 'p' has wrong format." <<Logger::endl;
554  return false;
555  }
556  result = vas_rpy.getResult( f.M) ||
557  vas_eul.getResult( f.M ) ||
558  vas_rot.getResult( f.M );
559  if (!result )
560  {
561  Logger::log()
562  << Logger::Error << "Aborting composition of Property< KDL.Frame > "
563  << ": Could not compose 'M' type 'KDL.Rotation', got type '"
564  << subbag->get().getType() <<"'."<<Logger::endl;
565  return false;
566  }
567  // OK: exit.
568  return true;
569  } else {
570  Logger::log() << Logger::Error << "Aborting composition of Property< KDL.Frame > "
571  << ": Expected type 'KDL.Frame', got type '"<< f_bag.getType() <<"'."
572  <<Logger::endl;
573  return false;
574  }
575  return false;
576  }
577 
578 } // MotionControl
Property< double > Z
bool getResult(Rotation &res)
bool getResult(Rotation &res)
DataSourceType get() const
Property< double > X
Property< T > * getPropertyType(const std::string &name) const
const PropertyBag & bag
const std::string & getType() const
virtual Property< T > * clone() const
RPYDecomposer(const Rotation &r)
Vector vel
bool composeProperty(const PropertyBag &bag, Joint &joint)
void GetEulerZYX(double &Alfa, double &Beta, double &Gamma) const
void setType(const std::string &newtype)
Property< double > Y
RotationDecomposer(const Rotation &r)
PropertyBag & result()
Vector torque
EulerZYXComposer(const PropertyBag &_bag)
Rotation M
reference_t value()
bool ownProperty(base::PropertyBase *p)
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Vector rot
static Rotation RPY(double roll, double pitch, double yaw)
const PropertyBag & bag
VectorDecomposer(const Vector &v)
static std::ostream & endl(std::ostream &__os)
void decomposeProperty(const Joint &joint, PropertyBag &targetbag)
bool getResult(Vector &res)
RPYComposer(const PropertyBag &_bag)
Property< double > _r
Property< double > _p
void GetRPY(double &roll, double &pitch, double &yaw) const
Vector force
const PropertyBag & bag
RotationComposer(const PropertyBag &_bag)
bool getResult(Rotation &res)
static Logger & log()
VectorComposer(const PropertyBag &_bag)
const PropertyBag & bag
reference_t set()
Property< double > _y
EulerZYXDecomposer(const Rotation &r)


kdl_typekit
Author(s): Steven Bellens, Ruben Smits
autogenerated on Wed Jul 3 2019 19:39:45