bugfix: set mass of root link

rock-control · Jan 23, 2017 · d1d2507 · d1d2507
1 parent e7f06e9
commit d1d2507
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/src/kdl_parser.cpp b/src/kdl_parser.cpp
@@ -161,7 +161,10 @@ bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree)
 
     // warn if root link has inertia. KDL does not support this
     if (robot_model.getRoot()->inertial)
-        LOG_DEBUG("WARN: The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.", robot_model.getRoot()->name.c_str());
+    {
+        KDL::RigidBodyInertia *i = const_cast<KDL::RigidBodyInertia *>(&(tree.getRootSegment()->second.segment.getInertia()));
+        *i = toKdl(robot_model.getRoot()->inertial);
+    }
 
     //  add all children
     for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)