test_inertia_rpy.cpp
Go to the documentation of this file.
00001 
00002 #include <string>
00003 #include <gtest/gtest.h>
00004 #include <kdl/jntarray.hpp>
00005 #include <kdl/chainidsolver_recursive_newton_euler.hpp>
00006 
00007 #include <ros/ros.h>
00008 #include <ros/console.h>
00009 #include "kdl_parser/kdl_parser.hpp"
00010 
00011 using namespace kdl_parser;
00012 
00013 int g_argc;
00014 char** g_argv;
00015 
00016 class TestInertiaRPY : public testing::Test
00017 {
00018 public:
00019 
00020 protected:
00022   TestInertiaRPY()
00023   {
00024   }
00025 
00026 
00028   ~TestInertiaRPY()
00029   {
00030   }
00031 };
00032 
00033 
00034 TEST_F(TestInertiaRPY, test_torques)
00035 {
00036   //ASSERT_EQ(g_argc, 3);
00037 
00038   // workaround for segfault issue with parsing 2 trees instantiated on the stack
00039   KDL::Tree * tree_1 = new KDL::Tree;
00040   KDL::Tree * tree_2 = new KDL::Tree;
00041   KDL::JntArray torques_1;
00042   KDL::JntArray torques_2;
00043 
00044   {
00045     ASSERT_TRUE(treeFromFile(g_argv[1], *tree_1));
00046     KDL::Vector gravity(0, 0, -9.81);
00047     KDL::Chain chain;
00048     std::cout << "number of joints: " << tree_1->getNrOfJoints() << std::endl;
00049     std::cout << "number of segments: " << tree_1->getNrOfSegments() << std::endl;
00050 
00051     ASSERT_TRUE(tree_1->getChain("base_link", "link2", chain));
00052     KDL::ChainIdSolver_RNE solver(chain, gravity);
00053     //JntArrays get initialized with all 0 values
00054     KDL::JntArray q(chain.getNrOfJoints());
00055     KDL::JntArray qdot(chain.getNrOfJoints());
00056     KDL::JntArray qdotdot(chain.getNrOfJoints());
00057     //KDL::JntArray torques(chain.getNrOfJoints());
00058     std::vector<KDL::Wrench> wrenches(chain.getNrOfJoints());
00059     solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_1);
00060 
00061     delete tree_1;
00062     tree_1 = NULL;
00063   }
00064 
00065   {
00066     ASSERT_TRUE(treeFromFile(g_argv[2], *tree_2));
00067     KDL::Vector gravity(0, 0, -9.81);
00068     KDL::Chain chain;
00069 
00070     ASSERT_TRUE(tree_2->getChain("base_link", "link2", chain));
00071     KDL::ChainIdSolver_RNE solver(chain, gravity);
00072     //JntArrays get initialized with all 0 values
00073     KDL::JntArray q(chain.getNrOfJoints());
00074     KDL::JntArray qdot(chain.getNrOfJoints());
00075     KDL::JntArray qdotdot(chain.getNrOfJoints());
00076     //KDL::JntArray torques(chain.getNrOfJoints());
00077     std::vector<KDL::Wrench> wrenches(chain.getNrOfJoints());
00078     solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_2);
00079 
00080     delete tree_2;
00081     tree_2 = NULL;
00082   }
00083 
00084   ASSERT_TRUE(torques_1 == torques_2);
00085 
00086   SUCCEED();
00087 }
00088 
00089 
00090 
00091 int main(int argc, char** argv)
00092 {
00093   testing::InitGoogleTest(&argc, argv);
00094   ros::init(argc, argv, "test_kdl_parser");
00095   for (size_t i = 0; i < argc; ++i) {
00096     std::cout << argv[i] << std::endl;
00097   }
00098   g_argc = argc;
00099   g_argv = argv;
00100   return RUN_ALL_TESTS();
00101 }


kdl_parser
Author(s): Wim Meeussen , Ioan Sucan , Jackie Kay
autogenerated on Thu Jun 6 2019 21:11:45