From 848abcdd207ad49e8194f4d97ada229b1c1738ea Mon Sep 17 00:00:00 2001 From: rphilippsen Date: Fri, 30 Oct 2009 18:51:30 +0000 Subject: [PATCH] Bugfix: URDF inertia was not added to KDL::Segment, which ended up always having zero mass and inertia. (Yet another case of default ctor args standing in the way of development). --- kdl_parser/src/kdl_parser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp index 2478250..717661f 100644 --- a/kdl_parser/src/kdl_parser.cpp +++ b/kdl_parser/src/kdl_parser.cpp @@ -115,7 +115,7 @@ bool addChildrenToTree(boost::shared_ptr root, Tree& tree) Joint jnt = toKdl(root->parent_joint); // construct the kdl segment - Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform)); + Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert); // add segment to tree tree.addSegment(sgm, root->parent_joint->parent_link_name);