88 bool getResult(
Vector& res);
92 : resultBag(
"KDL.Vector"),
93 X(
"X",
"X Value", v[0]),
94 Y(
"Y",
"Y Value", v[1]),
95 Z(
"Z",
"Z Value", v[2])
104 if ( bag.getType() ==
"MotCon::Vector" || bag.getType() ==
"KDL.Vector" )
110 if ( px != 0 && py != 0 && pz != 0)
115 std::string element = !px ?
"X" : !py ?
"Y" :
"Z";
117 <<
": Missing element '" <<element<<
"'." <<
Logger::endl;
122 <<
": Expected type 'KDL.Vector', got type '"<< bag.getType() <<
"'." 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) )
198 if ( bag.getType() ==
"MotCon::Rotation" || bag.getType() ==
"KDL.Rotation" )
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 )
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" )
273 if ( bag.getType() ==
"KDL.Rotation" || bag.getType() ==
"MotCon::Rotation" )
288 if ( _a != 0 && _b != 0 && _g != 0 )
293 std::string element = !_a ?
"alpha" : !_b ?
"beta" :
"gamma";
295 <<
": Missing element '" <<element<<
"'." <<
Logger::endl;
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" )
348 if ( bag.getType() ==
"KDL.Rotation" || bag.getType() ==
"MotCon::Rotation" )
355 if ( _r != 0 && _p != 0 && _y != 0 )
360 std::string element = !_r ?
"R" : !_p ?
"P" :
"Y";
362 <<
": Missing element '" <<element<<
"'." <<
Logger::endl;
386 #ifdef ROTATION_PROPERTIES_EULER 389 # ifdef ROTATION_PROPERTIES_RPY 409 <<
": Expected type 'KDL.Rotation', got type '"<< bag.
getType() <<
"'." 417 targetbag.
setType(
"KDL.Twist");
428 if ( bag.
getType() ==
"KDL.Twist" || bag.
getType() ==
"MotCon::Twist" )
435 <<
": vel not found." 444 <<
": rot not found." 453 <<
": Expected type 'KDL.Twist', got type '"<< bag.
getType() <<
"'." 462 targetbag.
setType(
"KDL.Wrench");
473 if ( bag.
getType() ==
"KDL.Wrench" || bag.
getType() ==
"Motcon::Wrench")
480 <<
": force not found." 489 <<
": torque not found." 498 <<
": Expected type 'KDL.Wrench', got type '"<< bag.
getType() <<
"'." 508 targetbag.
setType(
"KDL.Frame");
511 #ifdef ROTATION_PROPERTIES_EULER 514 # ifdef ROTATION_PROPERTIES_RPY 527 if ( f_bag.
getType() ==
"KDL.Frame" || f_bag.
getType() ==
"MotCon::Frame" )
549 bool result = vas_pos.getResult( f.
p );
556 result = vas_rpy.getResult( f.
M) ||
557 vas_eul.getResult( f.
M ) ||
558 vas_rot.getResult( f.
M );
562 <<
Logger::Error <<
"Aborting composition of Property< KDL.Frame > " 563 <<
": Could not compose 'M' type 'KDL.Rotation', got type '" 571 <<
": Expected type 'KDL.Frame', got type '"<< f_bag.
getType() <<
"'."
bool getResult(Rotation &res)
bool getResult(Rotation &res)
DataSourceType get() const
Property< T > * getPropertyType(const std::string &name) const
const std::string & getType() const
virtual Property< T > * clone() const
RPYDecomposer(const Rotation &r)
bool composeProperty(const PropertyBag &bag, Joint &joint)
void GetEulerZYX(double &Alfa, double &Beta, double &Gamma) const
void setType(const std::string &newtype)
RotationDecomposer(const Rotation &r)
EulerZYXComposer(const PropertyBag &_bag)
bool ownProperty(base::PropertyBase *p)
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
static Rotation RPY(double roll, double pitch, double yaw)
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)
void GetRPY(double &roll, double &pitch, double &yaw) const
RotationComposer(const PropertyBag &_bag)
bool getResult(Rotation &res)
VectorComposer(const PropertyBag &_bag)
EulerZYXDecomposer(const Rotation &r)