Commit 26a86e3b authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Fix bug when base link is not in neutral position.

parent 463a9d65
......@@ -573,8 +573,7 @@ namespace hpp
MatrixHomogeneousType parentJointInWorld;
if (link == model_.getRoot ()) {
MatrixHomogeneousType result; result.setIdentity ();
return result;
parentJointInWorld.setIdentity ();
}
else {
parentJointInWorld =
......@@ -582,16 +581,14 @@ namespace hpp
->currentTransformation ();
}
// Denormalize orientation if this is an actuated joint.
if (link == model_.getRoot ())
{}
else
if (link->parent_joint->type == ::urdf::Joint::REVOLUTE
|| link->parent_joint->type == ::urdf::Joint::CONTINUOUS
|| link->parent_joint->type == ::urdf::Joint::PRISMATIC) {
MatrixHomogeneousType inverse =
normalizeFrameOrientation (link->parent_joint).inverse ();
parentJointInWorld = parentJointInWorld * inverse;
}
if (link->parent_joint
&& (link->parent_joint->type == ::urdf::Joint::REVOLUTE
|| link->parent_joint->type == ::urdf::Joint::CONTINUOUS
|| link->parent_joint->type == ::urdf::Joint::PRISMATIC)) {
MatrixHomogeneousType inverse =
normalizeFrameOrientation (link->parent_joint).inverse ();
parentJointInWorld = parentJointInWorld * inverse;
}
MatrixHomogeneousType position = parentJointInWorld *
linkPositionInParentJoint;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment