icub-test
•All Data Structures Modules Pages
iKiniDynConsistencyTest.cpp
1 /*
2  * iCub Robot Unit Tests (Robot Testing Framework)
3  *
4  * Copyright (C) 2015-2019 Istituto Italiano di Tecnologia (IIT)
5  *
6  * This library is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Lesser General Public
8  * License as published by the Free Software Foundation; either
9  * version 2.1 of the License, or (at your option) any later version.
10  *
11  * This library is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this library; if not, write to the Free Software
18  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19  */
20 
21 #include <cmath>
22 
23 #include <robottestingframework/TestAssert.h>
24 #include <robottestingframework/dll/Plugin.h>
25 #include "iKiniDynConsistencyTest.h"
26 
27 // Yarp includes
28 #include <yarp/math/api.h>
29 #include <yarp/math/Math.h>
30 #include <yarp/os/Random.h>
31 #include <yarp/os/Time.h>
32 
33 // iKin includes
34 #include <iCub/iKin/iKinFwd.h>
35 
36 // iDyn includes
37 #include <iCub/iDyn/iDyn.h>
38 #include <iCub/iDyn/iDynBody.h>
39 
40 using namespace std;
41 using namespace robottestingframework;
42 using namespace yarp::os;
43 using namespace yarp::sig;
44 using namespace yarp::math;
45 using namespace iCub::iKin;
46 using namespace iCub::iDyn;
47 
48 
49 // some utils
50 void set_random_vector(yarp::sig::Vector & vec, yarp::os::Random & rng, double coeff=0.0)
51 {
52  for( int i=0; i < (int)vec.size(); i++ ) {
53  vec[i] = coeff*M_PI*rng.uniform();
54  }
55 }
56 
57 
58 // prepare the plugin
59 ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(iKiniDynConsistencyTest)
60 
61 iKiniDynConsistencyTest::iKiniDynConsistencyTest() : TestCase("iKiniDynConsistencyTest") {
62 }
63 
64 void iKiniDynConsistencyTest::check_matrix_are_equal(const yarp::sig::Matrix & mat1,
65  const yarp::sig::Matrix & mat2,
66  double tol)
67 {
68  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Comparing mat1: \n %s \n",mat1.toString().c_str()));
69  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("with mat2: \n %s \n",mat2.toString().c_str()));
70  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(mat1.rows()==mat2.rows(),"matrix rows do not match");
71  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(mat1.cols()==mat2.cols(),"matrix cols do not match");
72  for(int row=0; row < mat1.rows(); row++ )
73  {
74  for(int col=0; col < mat1.cols(); col++ )
75  {
76  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(std::fabs(mat1(row,col)-mat2(row,col)) < tol,
77  Asserter::format("Element %d %d don't match",row,col));
78  }
79  }
80 
81 }
82 
83 
84 iKiniDynConsistencyTest::~iKiniDynConsistencyTest() { }
85 
86 bool iKiniDynConsistencyTest::setup(int argc, char** argv) {
87 
88  return true;
89 }
90 
91 void iKiniDynConsistencyTest::tearDown() {
92  // finalization goes her ...
93 }
94 
95 
96 Matrix iKiniDynConsistencyTest::getiKinTransform(const string part, int index)
97 {
98  if( part == "left_leg" )
99  {
100  return ikin_lleg.getH(index);
101  }
102  if( part == "right_leg" )
103  {
104  return ikin_rleg.getH(index);
105  }
106  if( part == "left_arm" )
107  {
108  return ikin_larm.getH(index);
109  }
110  if( part == "right_arm" )
111  {
112  return ikin_rarm.getH(index);
113  }
114  return Matrix();
115 }
116 
117 Matrix iKiniDynConsistencyTest::getiDynTransform(const string part, int index)
118 {
119  if( part == "left_leg" )
120  {
121  return icub->lowerTorso->HLeft*
122  icub->lowerTorso->left->getH();
123  }
124  if( part == "right_leg" )
125  {
126  return icub->lowerTorso->HRight*
127  icub->lowerTorso->right->getH();
128  }
129  if( part == "left_arm" )
130  {
131  //return ikin_larm.getH(index);
132  }
133  if( part == "right_arm" )
134  {
135  //return ikin_rarm.getH(index);
136  }
137  return Matrix();
138 }
139 
140 void iKiniDynConsistencyTest::run() {
141 
142  ROBOTTESTINGFRAMEWORK_TEST_REPORT("Creating iCubWholeBody object");
143  version_tag ver;
144  ver.head_version = 1;
145  ver.legs_version = 1;
146  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Creating iCubWholeBody object with head version %d and legs version %d", ver.head_version, ver.legs_version));
147  icub = new iCubWholeBody(ver);
148 
149  // now we set the joint angles for all the limbs
150  // if connected to the real robot, we can take this values from the encoders
151  q_head.resize(icub->upperTorso->getNLinks("head"));
152  q_larm.resize(icub->upperTorso->getNLinks("left_arm"));
153  q_rarm.resize(icub->upperTorso->getNLinks("right_arm"));
154  q_torso.resize(icub->lowerTorso->getNLinks("torso"));
155  q_lleg.resize(icub->lowerTorso->getNLinks("left_leg"));
156  q_rleg.resize(icub->lowerTorso->getNLinks("right_leg"));
157 
158  //We can initialize the joint position to random values
159 
160  yarp::os::Random rng;
161  rng.seed(147);
162  double coeff = 1.0;
163  set_random_vector(q_head,rng,coeff);
164  set_random_vector(q_larm,rng,coeff);
165  set_random_vector(q_rarm,rng,coeff);
166  set_random_vector(q_torso,rng,coeff);
167  set_random_vector(q_lleg,rng,coeff);
168  set_random_vector(q_rleg,rng,coeff);
169 
170 
171 
172  ROBOTTESTINGFRAMEWORK_TEST_REPORT("Setting positions in iCubWholeBody");
173  q_head = icub->upperTorso->setAng("head",q_head);
174  q_rarm = icub->upperTorso->setAng("right_arm",q_rarm);
175  q_larm = icub->upperTorso->setAng("left_arm",q_larm);
176  q_torso = icub->lowerTorso->setAng("torso",q_torso);
177  q_rleg = icub->lowerTorso->setAng("right_leg",q_rleg);
178  q_lleg = icub->lowerTorso->setAng("left_leg",q_lleg);
179 
180  yarp::sig::Matrix transform_ikin(4,4), transform_idyn(4,4);
181 
183  ROBOTTESTINGFRAMEWORK_TEST_REPORT("Checking left hand position");
184  new(&ikin_larm) iCubArm("left");
185 
186  Vector q_torso_larm = cat(q_torso,q_larm);
187  ikin_larm.setAllConstraints(false);
188  ikin_larm.releaseLink(0);
189  ikin_larm.releaseLink(1);
190  ikin_larm.releaseLink(2);
191  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("q_torso_larm : %d ikin_larm : %d",q_torso_larm.size(),ikin_larm.getDOF()));
192  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(q_torso_larm.size() == ikin_larm.getDOF(),"unexpected chain size");
193  ikin_larm.setAng(q_torso_larm);
194 
195  transform_ikin = ikin_larm.getH();
196  transform_idyn = icub->lowerTorso->HUp*
197  icub->lowerTorso->up->getH(2,true)*
198  icub->upperTorso->HLeft*
199  icub->upperTorso->left->getH();
200 
201  check_matrix_are_equal(transform_ikin,transform_idyn);
202 
203 
205  ROBOTTESTINGFRAMEWORK_TEST_REPORT("Checking right hand position");
206  new(&ikin_rarm) iCubArm("right");
207  Vector q_torso_rarm = cat(q_torso,q_rarm);
208  ikin_rarm.setAllConstraints(false);
209  ikin_rarm.releaseLink(0);
210  ikin_rarm.releaseLink(1);
211  ikin_rarm.releaseLink(2);
212  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(q_torso_rarm.size() == ikin_rarm.getDOF(),"unexpected chain size");
213  ikin_rarm.setAng(q_torso_rarm);
214 
215  transform_ikin = ikin_rarm.getH();
216  transform_idyn = icub->lowerTorso->HUp*
217  icub->lowerTorso->up->getH(2,true)*
218  icub->upperTorso->HRight*
219  icub->upperTorso->right->getH();
220 
221  check_matrix_are_equal(transform_ikin,transform_idyn);
222 
223 
225  ROBOTTESTINGFRAMEWORK_TEST_REPORT("Checking left leg end effector positions");
226  new(&ikin_lleg) iCubLeg("left");
227  ikin_lleg.setAllConstraints(false);
228  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("q_lleg : %d ikin_lleg : %d",q_lleg.size(),ikin_lleg.getDOF()));
229  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(q_lleg.size() == ikin_lleg.getDOF(),"unexpected chain size");
230  ikin_lleg.setAng(q_lleg);
231 
232  transform_ikin = ikin_lleg.getH();
233  transform_idyn = icub->lowerTorso->HLeft*
234  icub->lowerTorso->left->getH();
235 
236  check_matrix_are_equal(transform_ikin,transform_idyn);
237 
238  /*
239  for(int link=0; link < ikin_lleg.getDOF(); link++ )
240  {
241  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Checking %d DH frame of left_leg",link));
242  check_matrix_are_equal(this->getiKinTransform("left_leg",link),
243  this->getiDynTransform("left_leg",link));
244  }*/
245 
247  ROBOTTESTINGFRAMEWORK_TEST_REPORT("Checking right leg end effector positions");
248  new(&ikin_rleg) iCubLeg("right");
249  ikin_rleg.setAllConstraints(false);
250  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(q_rleg.size() == ikin_rleg.getDOF(),"unexpected chain size");
251  ikin_rleg.setAng(q_rleg);
252 
253  transform_ikin = ikin_rleg.getH();
254  transform_idyn = icub->lowerTorso->HRight*
255  icub->lowerTorso->right->getH();
256 
257  check_matrix_are_equal(transform_ikin,transform_idyn);
258 
259  /*
260  for(int link=0; link < ikin_rleg.getDOF(); link++ )
261  {
262  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Checking %d DH frame of right_leg",link));
263  check_matrix_are_equal(this->getiKinTransform("right_leg",link),
264  this->getiDynTransform("right_leg",link));
265  }*/
266 
267  delete icub;
268 }
269 
Generated by doxygen 1.9.1