bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

[kdl_parser] Fix bug in importing com if inertia and link frames are different

This commit is contained in:
Silvio Traversaro 2014-03-26 10:06:20 +01:00 committed by Jackie Kay
parent 1e74e643e8
commit f3639205a0
1 changed files with 25 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
kdl_parser/src/kdl_parser.cpp
View File

@ -96,11 +96,34 @@ Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i) RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
{ {
Frame origin = toKdl(i->origin); Frame origin = toKdl(i->origin);
// kdl specifies the inertia in the reference frame of the link, the urdf specifies the inertia in the inertia reference frame
return origin.M * RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz)); // the mass is frame indipendent
double kdl_mass = i->mass;
// kdl and urdf both specify the com position in the reference frame of the link
Vector kdl_com = origin.p;
// kdl specifies the inertia matrix in the reference frame of the link,
// while the urdf specifies the inertia matrix in the inertia reference frame
RotationalInertia urdf_inertia =
RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
// Rotation operators are not defined for rotational inertia,
// so we use the RigidBodyInertia operators (with com = 0) as a workaround
RigidBodyInertia kdl_inertia_wrt_com_workaround =
origin.M *RigidBodyInertia(0, Vector::Zero(), urdf_inertia);
// Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
// while the getRotationalInertia method returns the 3d inertia wrt the frame origin
// (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
RotationalInertia kdl_inertia_wrt_com =
kdl_inertia_wrt_com_workaround.getRotationalInertia();
return RigidBodyInertia(kdl_mass,kdl_com,kdl_inertia_wrt_com);
} }
// recursive function to walk through tree // recursive function to walk through tree
bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree) bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
{ {