From f3639205a06f43f0a35c5969c71ec495f3a667cc Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 26 Mar 2014 10:06:20 +0100 Subject: [PATCH 1/3] [kdl_parser] Fix bug in importing com if inertia and link frames are different --- kdl_parser/src/kdl_parser.cpp | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp index 4b8c3ab..e1efd51 100644 --- a/kdl_parser/src/kdl_parser.cpp +++ b/kdl_parser/src/kdl_parser.cpp @@ -96,11 +96,34 @@ Joint toKdl(boost::shared_ptr jnt) RigidBodyInertia toKdl(boost::shared_ptr 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); } + // recursive function to walk through tree bool addChildrenToTree(boost::shared_ptr root, Tree& tree) { From add47a792b96ac8363ae71e411faeef3e2cfdd89 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 26 Mar 2014 10:08:14 +0100 Subject: [PATCH 2/3] [kdl_parser] remove spurious newline --- kdl_parser/src/kdl_parser.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp index e1efd51..7031247 100644 --- a/kdl_parser/src/kdl_parser.cpp +++ b/kdl_parser/src/kdl_parser.cpp @@ -123,7 +123,6 @@ RigidBodyInertia toKdl(boost::shared_ptr i) } - // recursive function to walk through tree bool addChildrenToTree(boost::shared_ptr root, Tree& tree) { From ed6237cf9245d236df6a23278f3051d217eca1c2 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Tue, 13 Oct 2015 13:24:02 -0700 Subject: [PATCH 3/3] Add COM import test --- kdl_parser/CMakeLists.txt | 3 + kdl_parser/test/testInertiaRPYmodel1.urdf | 29 +++++++ kdl_parser/test/testInertiaRPYmodel2.urdf | 29 +++++++ kdl_parser/test/test_inertia_rpy.cpp | 101 ++++++++++++++++++++++ kdl_parser/test/test_inertia_rpy.launch | 6 ++ 5 files changed, 168 insertions(+) create mode 100644 kdl_parser/test/testInertiaRPYmodel1.urdf create mode 100644 kdl_parser/test/testInertiaRPYmodel2.urdf create mode 100644 kdl_parser/test/test_inertia_rpy.cpp create mode 100644 kdl_parser/test/test_inertia_rpy.launch 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 @@ + + +