Merge pull request #117 from ros/fix-com-import

Fix com import

kdl_parser/CMakeLists.txt

@@ -38,6 +38,9 @@ if(CATKIN_ENABLE_TESTING)
   find_package(catkin REQUIRED COMPONENTS rostest)
   add_rostest_gtest(test_kdl_parser test/test_kdl_parser.launch test/test_kdl_parser.cpp )
   target_link_libraries(test_kdl_parser ${PROJECT_NAME})
+
+  add_rostest_gtest(test_inertia_rpy test/test_inertia_rpy.launch test/test_inertia_rpy.cpp )
+  target_link_libraries(test_inertia_rpy ${PROJECT_NAME})
 endif()
 
 # How does CATKIN do this?

kdl_parser/src/kdl_parser.cpp

@@ -96,8 +96,30 @@ Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
 RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
 {
   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);  
 }
 
 

kdl_parser/test/testInertiaRPYmodel1.urdf

@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+<robot name="testInertialRPYmodel1">
+   <link name="base_link" />
+   <joint name="base_fixed_joint" type="fixed">
+        <origin xyz="0 0 0" rpy="0 -0 0" />
+        <axis xyz="0 0 0" />
+        <parent link="base_link" />
+        <child link="link1" />
+    </joint>
+    <link name="link1">
+        <inertial>
+            <mass value="1" />
+            <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
+        </inertial>
+    </link>
+    <joint name="joint_1_2" type="continuous">
+        <origin xyz="0 0 0" rpy="0 -0 0" />
+        <axis xyz="0 0 1" />
+        <parent link="link1" />
+        <child link="link2" />
+    </joint>
+    <link name="link2">
+        <inertial>
+            <mass value="100" />
+            <origin xyz="1 2 3" rpy="0 -0 0" />
+            <inertia ixx="5" ixy="0.0" ixz="0.0" iyy="15" iyz="0.0" izz="25" />
+        </inertial>
+    </link>
+</robot>

kdl_parser/test/testInertiaRPYmodel2.urdf

@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+<robot name="testInertialRPYmodel2">
+   <link name="base_link" />
+   <joint name="base_fixed_joint" type="fixed">
+        <origin xyz="0 0 0" rpy="0 -0 0" />
+        <axis xyz="0 0 0" />
+        <parent link="base_link" />
+        <child link="link1" />
+    </joint>
+    <link name="link1">
+        <inertial>
+            <mass value="1" />
+            <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
+        </inertial>
+    </link>
+    <joint name="joint_1_2" type="continuous">
+        <origin xyz="0 0 0" rpy="0 -0 0" />
+        <axis xyz="0 0 1" />
+        <parent link="link1" />
+        <child link="link2" />
+    </joint>
+    <link name="link2">
+        <inertial>
+            <mass value="100" />
+            <origin xyz="1 2 3" rpy="3.141592653589793 0 0" />
+            <inertia ixx="5" ixy="0.0" ixz="0.0" iyy="15" iyz="0.0" izz="25" />
+        </inertial>
+    </link>
+</robot>

kdl_parser/test/test_inertia_rpy.cpp

@@ -0,0 +1,101 @@
+
+#include <string>
+#include <gtest/gtest.h>
+#include <kdl/jntarray.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+#include "kdl_parser/kdl_parser.hpp"
+
+using namespace kdl_parser;
+
+int g_argc;
+char** g_argv;
+
+class TestInertiaRPY : public testing::Test
+{
+public:
+
+protected:
+  /// constructor
+  TestInertiaRPY()
+  {
+  }
+
+
+  /// Destructor
+  ~TestInertiaRPY()
+  {
+  }
+};
+
+
+TEST_F(TestInertiaRPY, test_torques)
+{
+  //ASSERT_EQ(g_argc, 3);
+
+  // workaround for segfault issue with parsing 2 trees instantiated on the stack
+  KDL::Tree * tree_1 = new KDL::Tree;
+  KDL::Tree * tree_2 = new KDL::Tree;
+  KDL::JntArray torques_1;
+  KDL::JntArray torques_2;
+
+  {
+    ASSERT_TRUE(treeFromFile(g_argv[1], *tree_1));
+    KDL::Vector gravity(0, 0, -9.81);
+    KDL::Chain chain;
+    std::cout << "number of joints: " << tree_1->getNrOfJoints() << std::endl;
+    std::cout << "number of segments: " << tree_1->getNrOfSegments() << std::endl;
+
+    ASSERT_TRUE(tree_1->getChain("base_link", "link2", chain));
+    KDL::ChainIdSolver_RNE solver(chain, gravity);
+    //JntArrays get initialized with all 0 values
+    KDL::JntArray q(chain.getNrOfJoints());
+    KDL::JntArray qdot(chain.getNrOfJoints());
+    KDL::JntArray qdotdot(chain.getNrOfJoints());
+    //KDL::JntArray torques(chain.getNrOfJoints());
+    std::vector<KDL::Wrench> wrenches(chain.getNrOfJoints());
+    solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_1);
+
+    delete tree_1;
+    tree_1 = NULL;
+  }
+
+  {
+    ASSERT_TRUE(treeFromFile(g_argv[2], *tree_2));
+    KDL::Vector gravity(0, 0, -9.81);
+    KDL::Chain chain;
+
+    ASSERT_TRUE(tree_2->getChain("base_link", "link2", chain));
+    KDL::ChainIdSolver_RNE solver(chain, gravity);
+    //JntArrays get initialized with all 0 values
+    KDL::JntArray q(chain.getNrOfJoints());
+    KDL::JntArray qdot(chain.getNrOfJoints());
+    KDL::JntArray qdotdot(chain.getNrOfJoints());
+    //KDL::JntArray torques(chain.getNrOfJoints());
+    std::vector<KDL::Wrench> wrenches(chain.getNrOfJoints());
+    solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_2);
+
+    delete tree_2;
+    tree_2 = NULL;
+  }
+
+  ASSERT_TRUE(torques_1 == torques_2);
+
+  SUCCEED();
+}
+
+
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "test_kdl_parser");
+  for (size_t i = 0; i < argc; ++i) {
+    std::cout << argv[i] << std::endl;
+  }
+  g_argc = argc;
+  g_argv = argv;
+  return RUN_ALL_TESTS();
+}

kdl_parser/test/test_inertia_rpy.launch

@@ -0,0 +1,6 @@
+<launch>
+  <test test-name="test_inertia_rpy" pkg="kdl_parser" type="test_inertia_rpy"
+        args="$(find kdl_parser)/test/testInertiaRPYmodel1.urdf 
+              $(find kdl_parser)/test/testInertiaRPYmodel2.urdf"
+ />
+</launch>