diff --git a/kdl_parser/CMakeLists.txt b/kdl_parser/CMakeLists.txt
index eedaf8a..43b96b7 100644
--- a/kdl_parser/CMakeLists.txt
+++ b/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?
diff --git a/kdl_parser/test/testInertiaRPYmodel1.urdf b/kdl_parser/test/testInertiaRPYmodel1.urdf
new file mode 100644
index 0000000..7e390a6
--- /dev/null
+++ b/kdl_parser/test/testInertiaRPYmodel1.urdf
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/kdl_parser/test/testInertiaRPYmodel2.urdf b/kdl_parser/test/testInertiaRPYmodel2.urdf
new file mode 100644
index 0000000..33dc229
--- /dev/null
+++ b/kdl_parser/test/testInertiaRPYmodel2.urdf
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/kdl_parser/test/test_inertia_rpy.cpp b/kdl_parser/test/test_inertia_rpy.cpp
new file mode 100644
index 0000000..05b12d2
--- /dev/null
+++ b/kdl_parser/test/test_inertia_rpy.cpp
@@ -0,0 +1,101 @@
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#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 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 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();
+}
diff --git a/kdl_parser/test/test_inertia_rpy.launch b/kdl_parser/test/test_inertia_rpy.launch
new file mode 100644
index 0000000..54fc441
--- /dev/null
+++ b/kdl_parser/test/test_inertia_rpy.launch
@@ -0,0 +1,6 @@
+
+
+