test_inertia_rpy.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015 Open Source Robotics Foundation, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the copyright holder nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Jackie Kay */
36 
37 #include <iostream>
38 #include <vector>
39 
40 #include <gtest/gtest.h>
41 #include <kdl/jntarray.hpp>
42 #include <kdl/chainidsolver_recursive_newton_euler.hpp>
43 
44 #include <ros/ros.h>
45 #include <ros/console.h>
47 
48 int g_argc;
49 char ** g_argv;
50 
51 class TestInertiaRPY : public testing::Test
52 {
53 public:
54 protected:
57  {
58  }
59 
62  {
63  }
64 };
65 
66 
67 TEST_F(TestInertiaRPY, test_torques) {
68  // workaround for segfault issue with parsing 2 trees instantiated on the stack
69  KDL::Tree * tree_1 = new KDL::Tree;
70  KDL::Tree * tree_2 = new KDL::Tree;
71  KDL::JntArray torques_1;
72  KDL::JntArray torques_2;
73 
74  {
75  ASSERT_TRUE(kdl_parser::treeFromFile(g_argv[1], *tree_1));
76  KDL::Vector gravity(0, 0, -9.81);
77  KDL::Chain chain;
78  std::cout << "number of joints: " << tree_1->getNrOfJoints() << std::endl;
79  std::cout << "number of segments: " << tree_1->getNrOfSegments() << std::endl;
80 
81  ASSERT_TRUE(tree_1->getChain("base_link", "link2", chain));
82  KDL::ChainIdSolver_RNE solver(chain, gravity);
83  KDL::JntArray q(chain.getNrOfJoints());
84  KDL::JntArray qdot(chain.getNrOfJoints());
85  KDL::JntArray qdotdot(chain.getNrOfJoints());
86  std::vector<KDL::Wrench> wrenches(chain.getNrOfJoints());
87  solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_1);
88 
89  delete tree_1;
90  tree_1 = NULL;
91  }
92 
93  {
94  ASSERT_TRUE(kdl_parser::treeFromFile(g_argv[2], *tree_2));
95  KDL::Vector gravity(0, 0, -9.81);
96  KDL::Chain chain;
97 
98  ASSERT_TRUE(tree_2->getChain("base_link", "link2", chain));
99  KDL::ChainIdSolver_RNE solver(chain, gravity);
100  KDL::JntArray q(chain.getNrOfJoints());
101  KDL::JntArray qdot(chain.getNrOfJoints());
102  KDL::JntArray qdotdot(chain.getNrOfJoints());
103  std::vector<KDL::Wrench> wrenches(chain.getNrOfJoints());
104  solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_2);
105 
106  delete tree_2;
107  tree_2 = NULL;
108  }
109 
110  ASSERT_TRUE(torques_1 == torques_2);
111 
112  SUCCEED();
113 }
114 
115 int main(int argc, char ** argv)
116 {
117  testing::InitGoogleTest(&argc, argv);
118  ros::init(argc, argv, "test_kdl_parser");
119  for (size_t i = 0; i < argc; ++i) {
120  std::cout << argv[i] << std::endl;
121  }
122  g_argc = argc;
123  g_argv = argv;
124  return RUN_ALL_TESTS();
125 }
unsigned int getNrOfJoints() const
TEST_F(TestInertiaRPY, test_torques)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
unsigned int getNrOfSegments() const
unsigned int getNrOfJoints() const
int g_argc
TestInertiaRPY()
constructor
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
KDL_PARSER_PUBLIC bool treeFromFile(const std::string &file, KDL::Tree &tree)
Definition: kdl_parser.cpp:172
int main(int argc, char **argv)
~TestInertiaRPY()
Destructor.
char ** g_argv


kdl_parser
Author(s): Wim Meeussen , Ioan Sucan , Jackie Kay
autogenerated on Fri Apr 3 2020 04:02:36