icub-test
movementReferencesTest.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/dll/Plugin.h>
24 #include <robottestingframework/TestAssert.h>
25 #include <yarp/robottestingframework/TestAsserter.h>
26 
27 #include "movementReferencesTest.h"
28 
29 using namespace std;
30 using namespace robottestingframework;
31 
32 using namespace yarp::os;
33 
34 
35 // prepare the plugin
36 ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(MovementReferencesTest)
37 
38 MovementReferencesTest::MovementReferencesTest() : yarp::robottestingframework::TestCase("MovementReferencesTest")
39 {
40 
41 
42 }
43 
44 MovementReferencesTest::~MovementReferencesTest()
45 {
46 }
47 
48 bool MovementReferencesTest::setup(yarp::os::Property &config)
49 {
50 
51  // initialization goes here ...
52  numJointsInPart = 0;
53  jPosMotion=NULL;
54 
55  iEncoders=NULL;
56  iPosition=NULL;
57  iPosDirect=NULL;
58  iControlMode=NULL;
59  iVelocity=NULL;
60  initialized=false;
61 
62  if(config.check("name"))
63  setName(config.find("name").asString());
64  else
65  setName("MovementReferencesTest");
66 
67 
68  // updating parameters
69  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(config.check("robot"), "The robot name must be given as the test parameter!");
70  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(config.check("part"), "The part name must be given as the test parameter!");
71  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(config.check("joints"), "The joints list must be given as the test parameter!");
72  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(config.check("home"), "The home position must be given as the test parameter!");
73  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(config.check("target"), "Missing 'target' parameter, cannot open device");
74 
75  robotName = config.find("robot").asString();
76  partName = config.find("part").asString();
77 
78 
79  Property options;
80  options.put("device", "remote_controlboard");
81  options.put("remote", "/"+robotName+"/"+partName);
82  options.put("local", "/moveRefTest/"+robotName+"/"+partName);
83 
84  dd = new yarp::dev::PolyDriver(options);
85  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->isValid(),"Unable to open device driver");
86  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iPosition),"Unable to open position interface");
87  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iEncoders),"Unable to open encoders interface");
88  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iControlMode),"Unable to open control mode interface");
89  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iPWM), "Unable to open PWM interface");
90  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iPosDirect),"Unable to open OpnLoop interface");
91  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iVelocity),"Unable to open velocity2 interface");
92 
93 
94 
95  if (!iEncoders->getAxes(&numJointsInPart))
96  {
97  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("unable to get the number of joints of the part");
98  }
99 
100  Bottle* homeBottle = config.find("home").asList();
101  if(homeBottle == NULL)
102  {
103 
104  ROBOTTESTINGFRAMEWORK_TEST_REPORT("home bottle is null");
105  }
106  else
107  {
108 
109  ROBOTTESTINGFRAMEWORK_TEST_REPORT("home bottle is not null");
110  }
111 
112  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(homeBottle!=0,"unable to parse home parameter");
113 
114  Bottle* jointsBottle = config.find("joints").asList();
115  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(jointsBottle!=0,"unable to parse joints parameter");
116 
117  Bottle* targetBottle = config.find("target").asList();
118  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(targetBottle!=0,"unable to parse target parameter");
119 
120  Bottle* refAccBottle = config.find("refAcc").asList();
121  if(refAccBottle == NULL)
122  ROBOTTESTINGFRAMEWORK_TEST_REPORT("refAcc param not present. Test will use deafoult values.");
123 
124  Bottle* refVelBottle = config.find("refVel").asList();
125  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(refVelBottle!=0,"unable to parse refVel parameter");
126 
127 
128  numJoints = jointsBottle->size();
129  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("num joints: %d", numJoints));
130 
131 
132  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(numJoints>0 && numJoints<=numJointsInPart,"invalid number of joints, it must be >0 & <= number of part joints");
133 
134  //jointsList.clear();
135  targetPos.clear();
136 
137  jointsList.resize(numJoints);
138  for (int i=0; i< numJoints; i++)
139  {
140  jointsList[i]=jointsBottle->get(i).asInt32();
141  }
142 
143  jList = new int[numJoints];
144  for (int i=0; i< numJoints; i++)
145  {
146  jList[i]=jointsBottle->get(i).asInt32();
147  }
148 
149 
150 
151  //for (int i=0; i <numJoints; i++) jointsList.push_back(jointsBottle->get(i).asInt32());
152 
153  homePos.resize (numJoints); for (int i=0; i< numJoints; i++) homePos[i]=homeBottle->get(i).asFloat64();
154  targetPos.resize (numJoints); for (int i=0; i< numJoints; i++) targetPos[i]=targetBottle->get(i).asFloat64();
155  refVel.resize (numJoints); for (int i=0; i< numJoints; i++) refVel[i]=refVelBottle->get(i).asFloat64();
156  if(refAccBottle != NULL)
157  {
158  refAcc.resize (numJoints);
159  for (int i=0; i< numJoints; i++) refAcc[i]=refAccBottle->get(i).asFloat64();
160  }
161  else
162  {
163  refAcc.resize (0);
164  }
165 
166  jPosMotion = new yarp::robottestingframework::jointsPosMotion(dd, jointsList);
167  jPosMotion->setTolerance(2.0);
168  jPosMotion->setTimeout(5); //5 sec
169 
170 // ROBOTTESTINGFRAMEWORK_TEST_REPORT(jointsList.toString().c_str());
171 // ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("startup num joints: %d partnj=%d", numJoints, numJointsInPart));
172 // ROBOTTESTINGFRAMEWORK_TEST_REPORT(targetPos.toString().c_str());
173 // ROBOTTESTINGFRAMEWORK_TEST_REPORT(refVel.toString().c_str());
174 // ROBOTTESTINGFRAMEWORK_TEST_REPORT(refAcc.toString().c_str());
175 
176 // for(int k=0; k<numJoints; k++)
177 // {
178 // ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("targetpos [%d]=%.2f", k, targetPos[k]));
179 // }
180 
181  /* for(int k=0; k<numJoints; k++)
182  {
183  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("jointlist [%d]=%d", k, (int)jointsList[k]));
184  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("jlist [%d]=%d", k, jList[k]));
185  }*/
186  return true;
187 }
188 
189 void MovementReferencesTest::tearDown() {
190 
191  // finalization goes her ...
192 
193  for (int i=0; i<numJoints; ++i)
194  {
195 
196  // 1) check get reference position returns the target position set by positionMove(..)
197 
198  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("tear down: joint %d is going to home", jList[i]));
199 
200  setAndCheckControlMode(jList[i], VOCAB_CM_POSITION);
201 
202  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(jPosMotion->goToSingle(jList[i], homePos[i]),
203  Asserter::format(("go to target pos for j %d"),jList[i]));
204  }
205 
206 
207  if(jPosMotion)
208  delete jPosMotion;
209 }
210 
211 
212 void MovementReferencesTest::setAndCheckControlMode(int j, int mode)
213 {
214  int rec_mode=VOCAB_CM_IDLE;
215  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iControlMode->setControlMode(j, mode),
216  Asserter::format(("setting control mode for j %d"),j));
217 
218  yarp::os::Time::delay(0.1);
219 
220  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iControlMode->getControlMode(j, &rec_mode),
221  Asserter::format(("getting control mode for j %d"),j));
222 
223  ROBOTTESTINGFRAMEWORK_ASSERT_FAIL_IF_FALSE((rec_mode == mode),
224  Asserter::format(("joint %d: is not in position"),j));
225 
226 }
227 
228 void MovementReferencesTest::run() {
229 
230  int nJoints=0;
231  bool doneAll=false;
232  bool ret=false;
233 
234 // ROBOTTESTINGFRAMEWORK_TEST_REPORT(jointsList.toString().c_str());
235 // for(int k=0; k<numJoints; k++)
236 // {
237 // ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("targetpos [%d]=%.2f", k, targetPos[k]));
238 // }
239 
240 // for(int k=0; k<numJoints; k++)
241 // {
242 // ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("jointlist [%d]=%d", k, jointsList[k]));
243 // }
244 
245 
246  ROBOTTESTINGFRAMEWORK_TEST_REPORT("setting velocity refs for all joints...");
247 
248  if(refAcc.size() != 0)
249  {
250  ROBOTTESTINGFRAMEWORK_TEST_REPORT("setting acceleration refs for all joints...");
251  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iVelocity->setRefAccelerations(numJoints, jList, refAcc.data()),
252  Asserter::format("setting reference acceleration on joints"));
253  }
254 
255  ROBOTTESTINGFRAMEWORK_TEST_REPORT("setting velocity refs for all joints...");
256  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->setRefSpeeds(numJoints, jList, refVel.data()),
257  Asserter::format("setting reference speed on joints"));
258 
259 
260 
261 
262  ROBOTTESTINGFRAMEWORK_TEST_REPORT("all joints are going to home...");
263  for (int i=0; i<numJoints; ++i)
264  {
265 
266  // 1) check get reference position returns the target position set by positionMove(..)
267 
268  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format(" joint %d is going to home", jList[i]));
269 
270  setAndCheckControlMode(jList[i], VOCAB_CM_POSITION);
271 
272  ROBOTTESTINGFRAMEWORK_ASSERT_FAIL_IF_FALSE(jPosMotion->goToSingle(jList[i], homePos[i]),
273  Asserter::format(("go to target pos for j %d"),jList[i]));
274  }
275 
276  yarp::os::Time::delay(5);
277 
278  ROBOTTESTINGFRAMEWORK_TEST_REPORT("Checking individual joints...");
279 
280  const double res_th = 0.01; //resolution threshold
281  //numJoints=numJointsInPart;
282  for (int i=0; i<numJoints; ++i)
283  {
284  //double reached_pos;
285  double rec_targetPos=200.0;
286 
287  // 1) check get reference position returns the target position set by positionMove(..)
288 
289  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Checking PosReference joint %d with resolution threshold %.3f", jList[i], res_th));
290 
291  setAndCheckControlMode(jList[i], VOCAB_CM_POSITION);
292 
293  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(jPosMotion->goToSingle(jList[i], targetPos[i]),
294  Asserter::format(("go to target pos for j %d"),jList[i])); //Note: gotosingle use IPositioncontrol2::PositionMove
295 
296  yarp::os::Time::delay(0.5);
297 
298  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->getTargetPosition(jList[i], &rec_targetPos),
299  Asserter::format(("getting target pos for j %d"),jList[i]));
300 
301  bool res = yarp::robottestingframework::TestAsserter::isApproxEqual(targetPos[i], rec_targetPos, res_th, res_th);
302  ROBOTTESTINGFRAMEWORK_TEST_CHECK(res, Asserter::format(
303  ("IPositionControl: getting target pos for j %d: setval =%.2f received %.2f"),
304  jList[i], targetPos[i],rec_targetPos));
305 
306  //if(!res)
307  // std::cout <<"ERRORE: getTargetPosition: j " << jList[i] << "sent" << targetPos[i] << "rec" << rec_targetPos;
308  //else
309  // std::cout <<"OK: getTargetPosition: j " << jList[i] << "sent" << targetPos[i] << "rec" << rec_targetPos;
310 
311  yarp::os::Time::delay(3);
312 
313  //2) check get reference output (pwm mode) returns the ouput set by setRefOutput
314  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Checking pwm reference joint %d", jList[i]));
315 
316 
317  setAndCheckControlMode(jList[i], VOCAB_CM_PWM);
318 
319  double output = 2;
320  double rec_output = 0;
321  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPWM->setRefDutyCycle(jList[i], output),
322  Asserter::format(("set ref output for j %d"),jList[i]));
323 yarp::os::Time::delay(0.5);
324 
325  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPWM->getRefDutyCycle(jList[i], &rec_output),
326  Asserter::format(("get ref output for j %d"),jList[i]));
327 
328  ROBOTTESTINGFRAMEWORK_TEST_CHECK((output == rec_output),
329  Asserter::format(("getting target output for j %d: setval =%.2f received %.2f"),jList[i], output,rec_output));
330 
331  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->positionMove(jList[i], homePos[i]),
332  Asserter::format(("go to home for j %d"),jList[i]));
333 yarp::os::Time::delay(0.5);
334  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->getTargetPosition(jList[i], &rec_targetPos),
335  Asserter::format(("getting target pos for j %d"),jList[i]));
336 
337  //here I expect getTargetPosition returns targetPos[j] and not homePos[j] because joint is in pwm control mode and
338  //the positionMove(homepos) command should be discarded by firmware motor controller
339  res = yarp::robottestingframework::TestAsserter::isApproxEqual(homePos[i], rec_targetPos, res_th, res_th);
340  ROBOTTESTINGFRAMEWORK_TEST_CHECK(!res,
341  Asserter::format(("joint %d discards PosotinMove command while it is in opnLoop mode. Set=%.2f rec=%.2f"),jList[i], homePos[i], rec_targetPos));
342 
343  //3) check get reference pos (directPosition mode) returns the target position set by setPosition()
344  //set direct mode
345  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Checking directPos reference joint %d", jList[i]));
346 
347  setAndCheckControlMode(jList[i], VOCAB_CM_POSITION_DIRECT);
348 
349  double curr_pos=targetPos[i];
350  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iEncoders->getEncoder(jList[i], &curr_pos),
351  Asserter::format(("get encoders for j %d"),jList[i]));
352 
353  double delta = 0.1;
354  double new_directPos = curr_pos+delta;
355  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosDirect->setPosition(jList[i], new_directPos),
356  Asserter::format(("Direct:setPosition for j %d"),jList[i]));
357 yarp::os::Time::delay(0.5);
358 
359  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosDirect->getRefPosition(jList[i], &rec_targetPos),
360  Asserter::format(("getting target pos for j %d"),jList[i]));
361 
362  res = yarp::robottestingframework::TestAsserter::isApproxEqual(new_directPos, rec_targetPos, res_th, res_th);
363  ROBOTTESTINGFRAMEWORK_TEST_CHECK(res,
364  Asserter::format(("iDirect: getting target direct pos for j %d: setval =%.2f received %.2f"),jList[i], new_directPos,rec_targetPos));
365 
366  //here I'm going to check the position reference is not changed.
367  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->getTargetPosition(jList[i], &rec_targetPos),
368  Asserter::format(("getting target pos for j %d"),jList[i]));
369 
370  res = yarp::robottestingframework::TestAsserter::isApproxEqual(targetPos[i], rec_targetPos, res_th, res_th);
371  ROBOTTESTINGFRAMEWORK_TEST_CHECK(res, Asserter::format(
372  ("IPositionControl: getting target pos for j %d: setval =%.2f received %.2f"),
373  jList[i], targetPos[i],rec_targetPos));
374 
375  //4) check get reference velocity (velocity mode) returns the target velocity set by velocityMove()
376  //set velocity mode
377  ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Checking velocity reference joint %d", jList[i]));
378 
379  setAndCheckControlMode(jList[i], VOCAB_CM_VELOCITY);
380 
381  double vel= 0.5;
382  double rec_vel;
383  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iVelocity->velocityMove(jList[i], vel),
384  Asserter::format(("IVelocity:velocityMove for j %d"),jList[i]));
385 
386 yarp::os::Time::delay(0.5);
387  ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iVelocity->getRefVelocity(jList[i], &rec_vel),
388  Asserter::format(("IVelocity:getting target velocity for j %d"),jList[i]));
389  res = yarp::robottestingframework::TestAsserter::isApproxEqual(vel, rec_vel, res_th, res_th);
390  ROBOTTESTINGFRAMEWORK_TEST_CHECK(res,
391  Asserter::format(("iVelocity: getting target vel for j %d: setval =%.2f received %.2f"),jList[i], vel,rec_vel));
392  }
393 
394 }
MovementReferencesTest
Check IPositionControl, IVelocityControl, IPWMControl, IPositionDirect.
Definition: movementReferencesTest.h:56
Generated by doxygen 1.9.1