icub-test
jointLimits.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 <math.h>
22 #include <robottestingframework/TestAssert.h>
23 #include <robottestingframework/dll/Plugin.h>
24 #include <yarp/os/Time.h>
25 #include <yarp/math/Math.h>
26 #include <yarp/os/Property.h>
27 #include <fstream>
28 #include <algorithm>
29 #include <cstdlib>
30 #include "jointLimits.h"
31 
32 #include <stdio.h>
33 
34 using namespace robottestingframework;
35 using namespace yarp::os;
36 using namespace yarp::dev;
37 using namespace yarp::math;
38 
39 // prepare the plugin
40 ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(JointLimits)
41 
42 JointLimits::JointLimits() : yarp::robottestingframework::TestCase("JointLimits") {
43  jointsList=0;
44  dd=0;
45  ipos=0;
46  icmd=0;
47  iimd=0;
48  ienc=0;
49  ilim=0;
50  enc_jnt=0;
51  original_pids=0;
52  pids_saved=false;
53 }
54 
55 JointLimits::~JointLimits() { }
56 
57 bool JointLimits::setup(yarp::os::Property& property) {
58  if(property.check("name"))
59  setName(property.find("name").asString());
60 
61  // updating parameters
62  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"), "The robot name must be given as the test parameter!");
63  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"), "The part name must be given as the test parameter!");
64  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("joints"), "The joints list must be given as the test parameter!");
65  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("home"), "The home position must be given as the test parameter!");
66  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("speed"), "The positionMove reference speed must be given as the test parameter!");
67  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("outOfBoundPosition"), "The outOfBoundPosition parameter must be given as the test parameter!");
68  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("outputLimitPercent"), "The outputLimitPercent must be given as the test parameter!");
69  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("tolerance"), " The max error tolerance must be given as the test parameter!");
70 
71  robotName = property.find("robot").asString();
72  partName = property.find("part").asString();
73 
74  char buff[500];
75  sprintf(buff, "setting up test for %s", partName.c_str());
76  ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
77 
78  Bottle* jointsBottle = property.find("joints").asList();
79  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(jointsBottle!=0,"unable to parse joints parameter");
80 
81  Bottle* homeBottle = property.find("home").asList();
82  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(homeBottle!=0,"unable to parse zero parameter");
83 
84  Bottle* speedBottle = property.find("speed").asList();
85  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(speedBottle!=0,"unable to parse speed parameter");
86 
87  Bottle* outOfBoundPosition = property.find("outOfBoundPosition").asList();
88  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(outOfBoundPosition != 0, "unable to parse outOfBoundPosition parameter");
89 
90  Bottle* outputLimitBottle = property.find("outputLimitPercent").asList();
91  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(outputLimitBottle!=0,"unable to parse speed parameter");
92 
93  tolerance = property.find("tolerance").asFloat64();
94  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tolerance>=0,"invalid tolerance");
95 
96  Bottle* toleranceListBottle = property.find("toleranceList").asList(); //optional param
97 
98  Property options;
99  options.put("device", "remote_controlboard");
100  options.put("remote", "/"+robotName+"/"+partName);
101  options.put("local", "/jointsLimitsTest/"+robotName+"/"+partName);
102 
103  dd = new PolyDriver(options);
104  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->isValid(),"Unable to open device driver");
105  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ienc),"Unable to open encoders interface");
106  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipos),"Unable to open position interface");
107  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(icmd),"Unable to open control mode interface");
108  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iimd),"Unable to open interaction mode interface");
109  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ilim),"Unable to open limits interface");
110  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipid),"Unable to open pid interface");
111 
112  if (!ienc->getAxes(&n_part_joints))
113  {
114  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("unable to get the number of joints of the part");
115  }
116 
117  int n_cmd_joints = jointsBottle->size();
118  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(n_cmd_joints>0 && n_cmd_joints<=n_part_joints,"invalid number of joints, it must be >0 & <= number of part joints");
119  for (int i=0; i <n_cmd_joints; i++) jointsList.push_back(jointsBottle->get(i).asInt32());
120 
121  enc_jnt.resize(n_part_joints);
122  max_lims.resize(n_cmd_joints);
123  min_lims.resize(n_cmd_joints);
124 
125  home.resize (n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) home[i]=homeBottle->get(i).asFloat64();
126  speed.resize(n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) speed[i]=speedBottle->get(i).asFloat64();
127  outputLimit.resize(n_cmd_joints);for (int i=0; i< n_cmd_joints; i++) outputLimit[i]=outputLimitBottle->get(i).asFloat64();
128  outOfBoundPos.resize(n_cmd_joints); for (int i = 0; i < n_cmd_joints; i++) { outOfBoundPos[i] = outOfBoundPosition->get(i).asFloat64(); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(outOfBoundPos[i] > 0 , "outOfBoundPosition must be > 0"); }
129  toleranceList.resize(n_cmd_joints);
130  for (int i = 0; i < n_cmd_joints; i++)
131  {
132  if(toleranceListBottle)
133  {
134  toleranceList[i] = toleranceListBottle->get(i).asFloat64();
135  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(toleranceList[i] >= 0 , "toleranceList must be > 0");
136  }
137  else
138  toleranceList[i] = tolerance;
139  }
140 
141  original_pids = new yarp::dev::Pid[n_cmd_joints];
142  for (unsigned int i=0; i<jointsList.size(); i++)
143  {
144  ipid->getPid(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, (int)jointsList[i],&original_pids[i]);
145  }
146  pids_saved=true;
147 
148  for (unsigned int i=0; i<jointsList.size(); i++)
149  {
150  yarp::dev::Pid p = original_pids[i];
151  p.max_output = p.max_output/100*outputLimit[i];
152  p.max_int = p.max_int/100*outputLimit[i];
153  ipid->setPid(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, (int)jointsList[i],p);
154  yarp::os::Time::delay(0.010);
155  yarp::dev::Pid t;
156  ipid->getPid(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, (int)jointsList[i],&t);
157 
158  //since pid values are double, the returned values may differ from those sent due to conversion.
159  if (fabs(t.max_output-p.max_output) > 1.0 ||
160  fabs(t.max_int-p.max_int) > 1.0 ||
161  fabs(t.kp-p.kp) > 1.0 ||
162  fabs(t.kd-p.kd) > 1.0 ||
163  fabs(t.ki-p.ki) > 1.0)
164  {
165  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Unable to set output limits");
166  }
167 
168  }
169  return true;
170 }
171 
172 void JointLimits::tearDown()
173 {
174  if (original_pids)
175  {
176  if (pids_saved)
177  {
178  for (unsigned int i=0; i<jointsList.size(); i++)
179  {
180  ipid->setPid(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, (int)jointsList[i],original_pids[i]);
181  }
182  }
183  delete[] original_pids; original_pids=0;
184  }
185  if (dd) {delete dd; dd =0;}
186 }
187 
188 void JointLimits::setMode(int desired_mode)
189 {
190  for (unsigned int i=0; i<jointsList.size(); i++)
191  {
192  icmd->setControlMode((int)jointsList[i],desired_mode);
193  iimd->setInteractionMode((int)jointsList[i],VOCAB_IM_STIFF);
194  yarp::os::Time::delay(0.010);
195  }
196 
197  int cmode;
198  yarp::dev::InteractionModeEnum imode;
199  int timeout = 0;
200 
201  while (1)
202  {
203  int ok=0;
204  for (unsigned int i=0; i<jointsList.size(); i++)
205  {
206  icmd->getControlMode ((int)jointsList[i],&cmode);
207  iimd->getInteractionMode((int)jointsList[i],&imode);
208  if (cmode==desired_mode && imode==VOCAB_IM_STIFF) ok++;
209  }
210  if (ok==jointsList.size()) break;
211  if (timeout>100)
212  {
213  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Unable to set control mode/interaction mode");
214  }
215  yarp::os::Time::delay(0.2);
216  timeout++;
217  }
218 }
219 
220 void JointLimits::goTo(yarp::sig::Vector position)
221 {
222  for (unsigned int i=0; i<jointsList.size(); i++)
223  {
224  ipos->setRefSpeed((int)jointsList[i],speed[i]);
225  ipos->positionMove((int)jointsList[i],position[i]);
226  }
227 
228  int timeout = 0;
229  while (1)
230  {
231  int in_position=0;
232  for (unsigned int i=0; i<jointsList.size(); i++)
233  {
234  double tmp=0;
235  ienc->getEncoder((int)jointsList[i],&tmp);
236  if (fabs(tmp-position[i])<toleranceList[i]) in_position++;
237  }
238  if (in_position==jointsList.size()) break;
239  if (timeout>100)
240  {
241  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Timeout while reaching desired position");
242  }
243  yarp::os::Time::delay(0.2);
244  timeout++;
245  }
246 }
247 
248 bool JointLimits::goToSingle(int i, double pos, double *reached_pos)
249 {
250  ipos->setRefSpeed((int)jointsList[i],speed[i]);
251  ipos->positionMove((int)jointsList[i],pos);
252  double tmp=0;
253 
254  int timeout = 0;
255  while (1)
256  {
257  ienc->getEncoder((int)jointsList[i],&tmp);
258  if (fabs(tmp-pos)<toleranceList[i]) break;
259 
260  if (timeout>100)
261  {
262  if(reached_pos != NULL)
263  {
264  *reached_pos = tmp;
265  }
266  return(false);
267  }
268 
269  yarp::os::Time::delay(0.2);
270  timeout++;
271  }
272  if(reached_pos != NULL)
273  {
274  *reached_pos = tmp;
275  }
276  return(true);
277 }
278 
279 bool JointLimits::goToSingleExceed(int i, double position_to_reach, double limit, double reachedLimit, double *reached_pos)
280 {
281  ipos->setRefSpeed((int)jointsList[i], speed[i]);
282  ipos->positionMove((int)jointsList[i], position_to_reach);
283  double tmp = 0;
284  bool excededLimit = false;
285  int timeout = 0;
286  double limitToCheck;
287  if(fabs(reachedLimit-limit)>toleranceList[i])
288  {
289  //if i'm here joint did NOT reached the limit and I would check joint doesn't exced reached limit
290  limitToCheck = reachedLimit;
291  }
292  else
293  {
294  limitToCheck = limit;
295  }
296  while (1)
297  {
298  ienc->getEncoder((int)jointsList[i], &tmp);
299  if(fabs(tmp - limitToCheck)>toleranceList[i])
300  {
301  excededLimit = true;
302  //ROBOTTESTINGFRAMEWORK_TEST_REPORT (Asserter::format("j %d exced limitTocheck %f: reached %f",(int)jointsList[i], limitToCheck, tmp));
303  break;
304  }
305  if (fabs(tmp - position_to_reach)<toleranceList[i])
306  {
307  excededLimit = true;
308  //ROBOTTESTINGFRAMEWORK_TEST_REPORT (Asserter::format("j %d is near position_to_reach %f: reached %f",(int)jointsList[i], position_to_reach, tmp));
309  break;
310  }
311 
312  if (timeout>50) break;
313  yarp::os::Time::delay(0.2);
314  timeout++;
315  }
316 
317  *reached_pos = tmp;
318 
319 /* if (fabs(tmp - position_to_reach)<toleranceList[i])
320  {
321  //I reached the out of bound target. That's bad!
322  return (true);
323  }
324  if (fabs(tmp - limit)<toleranceList[i])
325  {
326  //I'm still near the joint limit. That's fine.
327  return (false);
328  }
329  //I dont know where I am, that's bad!
330  return(true);
331 */
332 
333  if( excededLimit)
334  return (true);
335  else
336  return(false);
337 }
338 
339 
340 void JointLimits::run()
341 {
342  char buff[500];
343  setMode(VOCAB_CM_POSITION);
344  ROBOTTESTINGFRAMEWORK_TEST_REPORT("all joints are going to home....");
345  goTo(home);
346 
347  for (unsigned int i=0; i<jointsList.size(); i++)
348  {
349  ilim->getLimits((int)jointsList[i],&min_lims[i],&max_lims[i]);
350  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(max_lims[i]!=min_lims[i],"Invalid limit: max==min?");
351  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(max_lims[i]>min_lims[i],"Invalid limit: max<min?");
352  if (max_lims[i] == 0 && min_lims[i] == 0) ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid limit: max==min==0");
353  }
354 
355  for (unsigned int i=0; i<jointsList.size(); i++)
356  {
357  bool res;
358  double reached_pos=0;
359  double excededpos_reached = 0;
360 
361  //1) Check max limit
362  sprintf(buff,"Testing if max limit is reachable, joint %d, max limit: %f",(int)jointsList[i],max_lims[i]);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
363  //check that max_limit is reachable
364  res = goToSingle(i, max_lims[i], &reached_pos);
365  ROBOTTESTINGFRAMEWORK_TEST_CHECK (res, Asserter::format("joint %d moved to max limit: %f reached: %f", (int)jointsList[i], max_lims[i], reached_pos));
366  //if(!res)
367  //{
368  // goToSingle(i,home[i], NULL); //I need to go to home in order to leave joint in safe position for further tests (other body parts)
369  // sprintf(buff, "Timeout while reaching desired position(%.2f). Reached pos=%.2f", max_lims[i], reached_pos);ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
370  //}
371  //else { sprintf(buff, "Test successfull, joint %d, max limit: %f reached: %f", (int)jointsList[i], max_lims[i], reached_pos); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff); }
372 
373  //2) check that max_limit + outOfBoundPos is NOT reachable
374  sprintf(buff, "Testing that max limit cannot be exceeded, joint %d, max limit: %f", (int)jointsList[i], max_lims[i]); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
375  res = goToSingleExceed(i, max_lims[i] + outOfBoundPos[i], max_lims[i], reached_pos, &excededpos_reached);
376  ROBOTTESTINGFRAMEWORK_TEST_CHECK (!res, Asserter::format("check if joint %d desn't exced max limit. target was: %f reached: %f, limit %f ", (int)jointsList[i], max_lims[i] + outOfBoundPos[i], excededpos_reached, max_lims[i]));
377  //if (res)
378  //{
379  // goToSingle(i, home[i], NULL); //I need to go to home in order to leave joint in safe position for further tests (other body parts)
380  // sprintf(buff, "Limit execeeded! Limit was (%.2f). Reached pos=%.2f", max_lims[i], reached_pos); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
381  //}
382  //else { sprintf(buff, "Test successfull, joint %d, target was: %f reached: %f, limit %f ", (int)jointsList[i], max_lims[i] + outOfBoundPos[i], reached_pos, max_lims[i]); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff); }
383 
384  //3) Check min limit
385  //check that min_limit is reachable
386  sprintf(buff,"Testing if min limit is reachable, joint %d, min limit: %f",(int)jointsList[i],min_lims[i]);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
387  res = goToSingle(i, min_lims[i], &reached_pos);
388  ROBOTTESTINGFRAMEWORK_TEST_CHECK (res, Asserter::format("joint %d moved to min limit: %f reached: %f", (int)jointsList[i], min_lims[i], reached_pos));
389 
390  /*if(!res)
391  {
392  goToSingle(i,home[i], NULL);
393  sprintf(buff, "Timeout while reaching desired position(%.2f). Reached pos=%.2f", min_lims[i], reached_pos);ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
394  }
395  else { sprintf(buff, "Test successfull, joint %d, min limit: %f reached: %f", (int)jointsList[i], min_lims[i], reached_pos); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff); }*/
396 
397  //4) check that min_limit - outOfBoundPos is NOT reachable
398  sprintf(buff, "Testing that min limit cannot be exceeded, joint %d, min limit: %f", (int)jointsList[i], min_lims[i]); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
399  res = goToSingleExceed(i, min_lims[i] - outOfBoundPos[i], min_lims[i], reached_pos, & excededpos_reached);
400  ROBOTTESTINGFRAMEWORK_TEST_CHECK (!res, Asserter::format("check if joint %d desn't exced min limit. target was: %f reached: %f, limit %f ", (int)jointsList[i], min_lims[i] - outOfBoundPos[i], excededpos_reached, min_lims[i]));
401 
402  //if (res)
403  //{
404  // goToSingle(i, home[i], NULL); //I need to go to home in order to leave joint in safe position for further tests (other body parts)
405  // sprintf(buff, "Limit execeeded! Limit was (%.2f). Reached pos=%.2f", min_lims[i], reached_pos); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
406  //}
407  //else { sprintf(buff, "Test successfull, joint %d, target was: %f reached: %f, limit %f ", (int)jointsList[i], min_lims[i] - outOfBoundPos[i], reached_pos, min_lims[i]); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff); }
408 
409  //5) Check home position
410  sprintf(buff,"Testing joint %d, homing to: %f",(int)jointsList[i],home[i]);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
411  res = goToSingle(i, home[i], &reached_pos);
412  if(!res)
413  {
414  sprintf(buff, "Timeout while reaching desired position(%.2f). Reached pos=%.2f", home[i], reached_pos);ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
415  }
416  else{ sprintf(buff, "Homing joint %d complete", (int)jointsList[i]); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff); }
417 
418  }
419  ienc->getEncoders(enc_jnt.data());
420  ROBOTTESTINGFRAMEWORK_TEST_REPORT("Test ends. All joints are going to home....");
421  goTo(home);
422  yarp::os::Time::delay(2.0);
423 }
JointLimits
Check if the software joint limits are properly set.
Definition: jointLimits.h:74
Generated by doxygen 1.9.1