From ed6237cf9245d236df6a23278f3051d217eca1c2 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Tue, 13 Oct 2015 13:24:02 -0700 Subject: [PATCH] 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 @@ + + +