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

add regression test for parsing the inertia

This commit is contained in:
wim 2009-11-19 19:15:05 +00:00
parent c1ff68a28e
commit d4fa8bc8e3
2 changed files with 3 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
kdl_parser/src/kdl_parser.cpp
View File

@ -96,6 +96,7 @@ 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)); return origin.M * RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
} }

2
kdl_parser/test/test_kdl_parser.cpp
View File

@ -77,6 +77,8 @@ TEST_F(TestParser, test)
ASSERT_TRUE(my_tree.getSegment("world") == my_tree.getRootSegment()); ASSERT_TRUE(my_tree.getSegment("world") == my_tree.getRootSegment());
ASSERT_EQ(my_tree.getRootSegment()->second.children.size(), (unsigned int)1); ASSERT_EQ(my_tree.getRootSegment()->second.children.size(), (unsigned int)1);
ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment()); ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment());
ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 116.0);
ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 15.6107, 0.001);
SUCCEED(); SUCCEED();
} }