icub-test
Loading...
Searching...
No Matches
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
40using namespace std;
41using namespace robottestingframework;
42using namespace yarp::os;
43using namespace yarp::sig;
44using namespace yarp::math;
45using namespace iCub::iKin;
46using namespace iCub::iDyn;
47
48
49// some utils
50void 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
59ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(iKiniDynConsistencyTest)
60
61iKiniDynConsistencyTest::iKiniDynConsistencyTest() : TestCase("iKiniDynConsistencyTest") {
62}
63
64void 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
84iKiniDynConsistencyTest::~iKiniDynConsistencyTest() { }
85
86bool iKiniDynConsistencyTest::setup(int argc, char** argv) {
87
88 return true;
89}
90
91void iKiniDynConsistencyTest::tearDown() {
92 // finalization goes her ...
93}
94
95
96Matrix 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
117Matrix 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
140void 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.8