iCub-main
parametricCalibrator.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2014 iCub Facility, Istituto Italiano di Tecnologia
5  * Authors: Alberto Cardellino, Marco Randazzo, Valentina Gaggero
6  * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
7  *
8  */
9 
10 #include <yarp/os/Time.h>
11 #include <yarp/dev/PolyDriver.h>
12 
13 #include "parametricCalibrator.h"
14 #include <math.h>
15 #include <algorithm>
16 #include <yarp/os/LogStream.h>
17 
18 using namespace yarp::os;
19 using namespace yarp::dev;
20 
21 // calibrator for the arm of the Arm iCub
22 
23 const int PARK_TIMEOUT = 30;
24 const double GO_TO_ZERO_TIMEOUT = 10; //seconds how many? // was 10
25 const int CALIBRATE_JOINT_TIMEOUT = 20;
26 const double POSITION_THRESHOLD = 2.0;
27 
28 // TODO use it!!
29 //#warning "Use extractGroup to verify size of parameters matches with number of joints, this will avoid crashes"
30 static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
31 {
32  size++; // size includes also the name of the parameter
33  Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
34  if (tmp.isNull())
35  {
36  yError () << key1.c_str() << " not found\n";
37  return false;
38  }
39 
40  if(tmp.size()!=size)
41  {
42  yError () << key1.c_str() << " incorrect number of entries in board.";
43  return false;
44  }
45 
46  out=tmp;
47  return true;
48 }
49 
50 parametricCalibrator::parametricCalibrator() :
51  type(NULL),
52  param1(NULL),
53  param2(NULL),
54  param3(NULL),
55  original_max_pwm(NULL),
56  limited_max_pwm(NULL),
57  maxPWM(NULL),
58  currPos(NULL),
59  currVel(NULL),
60  zeroPos(NULL),
61  zeroVel(NULL),
62  homeVel(0),
63  homePos(0),
64  zeroPosThreshold(0),
65  abortCalib(false),
66  isCalibrated(false),
67  skipCalibration(false),
68  clearHwFault(false),
69  n_joints(0),
70  totJointsToCalibrate(0)
71 {
72 }
73 
75 {
76  yTrace();
77  close();
78 }
79 
80 bool parametricCalibrator::open(yarp::os::Searchable& config)
81 {
82  yTrace();
83  Property p;
84  p.fromString(config.toString());
85 
86  if (p.check("GENERAL") == false)
87  {
88  yError() << "Parametric calibrator: missing [GENERAL] section";
89  return false;
90  }
91 
92  if (p.findGroup("GENERAL").check("deviceName"))
93  {
94  deviceName = p.findGroup("GENERAL").find("deviceName").asString();
95  }
96  else
97  {
98  yError() << "Parametric calibrator: missing deviceName parameter";
99  return false;
100  }
101 
102  std::string str;
103  if (config.findGroup("GENERAL").find("verbose").asInt32())
104  {
105  str = config.toString().c_str();
106  yTrace() << deviceName.c_str() << str;
107  }
108 
109  // Check clearHwFaultBeforeCalibration
110  Value val_clearHwFault = config.findGroup("GENERAL").find("clearHwFaultBeforeCalibration");
111  if (val_clearHwFault.isNull())
112  {
113  clearHwFault = false;
114  }
115  else
116  {
117  if (!val_clearHwFault.isBool())
118  {
119  yError() << deviceName.c_str() << ": clearHwFaultBeforeCalibration bool param is different from accepted values (true / false). Assuming false";
120  clearHwFault = false;
121  }
122  else
123  {
124  clearHwFault = val_clearHwFault.asBool();
125  if (clearHwFault)
126  yInfo() << deviceName.c_str() << ": clearHwFaultBeforeCalibration option enabled\n";
127  }
128  }
129 
130  // Check Vanilla = do not use calibration!
131  skipCalibration = config.findGroup("GENERAL").find("skipCalibration").asBool();// .check("Vanilla",Value(1), "Vanilla config");
132  skipCalibration = !!skipCalibration;
133  if (skipCalibration)
134  {
135  yWarning() << deviceName << ": skipping calibration!! This option was set in general.xml file.";
136  yWarning() << deviceName << ": BE CAREFUL USING THE ROBOT IN THIS CONFIGURATION! See 'skipCalibration' param in config file";
137  }
138 
139  int nj = 0;
140  if (p.findGroup("GENERAL").check("joints"))
141  {
142  nj = p.findGroup("GENERAL").find("joints").asInt32();
143  }
144  else if (p.findGroup("GENERAL").check("Joints"))
145  {
146  // This is needed to be backward compatibile with old iCubInterface
147  nj = p.findGroup("GENERAL").find("Joints").asInt32();
148  }
149  else
150  {
151  yError() << deviceName.c_str() << ": missing joints parameter";
152  return false;
153  }
154 
155  type = new unsigned char[nj];
156  param1 = new double[nj];
157  param2 = new double[nj];
158  param3 = new double[nj];
159  maxPWM = new int[nj];
160 
161  zeroPos = new double[nj];
162  zeroVel = new double[nj];
163  currPos = new double[nj];
164  currVel = new double[nj];
165  homePos = new double[nj];
166  homeVel = new double[nj];
167  zeroPosThreshold = new double[nj];
168 
169  int i = 0;
170 
171  Bottle& xtmp = p.findGroup("CALIBRATION").findGroup("calibration1");
172  if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of Calibration1 params " << xtmp.size() << " " << nj; return false; }
173  for (i = 1; i < xtmp.size(); i++) param1[i - 1] = xtmp.get(i).asFloat64();
174 
175  xtmp = p.findGroup("CALIBRATION").findGroup("calibration2");
176  if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of Calibration2 params"; return false; }
177  for (i = 1; i < xtmp.size(); i++) param2[i - 1] = xtmp.get(i).asFloat64();
178 
179  xtmp = p.findGroup("CALIBRATION").findGroup("calibration3");
180  if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of Calibration3 params"; return false; }
181  for (i = 1; i < xtmp.size(); i++) param3[i - 1] = xtmp.get(i).asFloat64();
182 
183  xtmp = p.findGroup("CALIBRATION").findGroup("calibrationType");
184  if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of Calibration3 params"; return false; }
185  for (i = 1; i < xtmp.size(); i++) type[i - 1] = (unsigned char)xtmp.get(i).asFloat64();
186 
187  xtmp = p.findGroup("CALIBRATION").findGroup("startupPosition");
188  if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of startupPosition params"; return false; }
189  for (i = 1; i < xtmp.size(); i++) zeroPos[i - 1] = xtmp.get(i).asFloat64();
190 
191  xtmp = p.findGroup("CALIBRATION").findGroup("startupVelocity");
192  if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of startupVelocity params"; return false; }
193  for (i = 1; i < xtmp.size(); i++) zeroVel[i - 1] = xtmp.get(i).asFloat64();
194 
195  xtmp = p.findGroup("HOME").findGroup("positionHome");
196  if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of PositionHome params"; return false; }
197  for (i = 1; i < xtmp.size(); i++) homePos[i - 1] = xtmp.get(i).asFloat64();
198 
199  xtmp = p.findGroup("HOME").findGroup("velocityHome");
200  if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of VelocityHome params"; return false; }
201  for (i = 1; i < xtmp.size(); i++) homeVel[i - 1] = xtmp.get(i).asFloat64();
202 
203  xtmp = p.findGroup("CALIBRATION").findGroup("startupMaxPwm");
204  if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of startupMaxPwm params"; return false; }
205  for (i = 1; i < xtmp.size(); i++) maxPWM[i - 1] = xtmp.get(i).asInt32();
206 
207  xtmp = p.findGroup("CALIBRATION").findGroup("startupPosThreshold");
208  if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of startupPosThreshold params"; return false;}
209  for (i = 1; i < xtmp.size(); i++) zeroPosThreshold[i-1] = xtmp.get(i).asFloat64();
210 
211  calibJointsString = p.findGroup("CALIB_ORDER");
212  int calib_order_size = calibJointsString.size();
213  if (calib_order_size <= 1) {yError() << deviceName << ": invalid number CALIB_ORDER params"; return false;}
214  //yDebug() << "CALIB_ORDER: group size: " << calibJointsString.size() << " values: " << calibJointsString.toString().c_str();
215 
216  std::list<int> tmp;
217  for(int i=1; i<calibJointsString.size(); i++)
218  {
219  tmp.clear();
220  Bottle *set;
221  set= calibJointsString.get(i).asList();
222 
223  for(int j=0; j<set->size(); j++)
224  {
225  tmp.push_back(set->get(j).asInt32() );
226  }
227  joints.push_back(tmp);
228  }
229  return true;
230 }
231 
233 {
234  yTrace();
235  if (type != NULL) {
236  delete[] type;
237  type = NULL;
238  }
239  if (param1 != NULL) {
240  delete[] param1;
241  param1 = NULL;
242  }
243  if (param2 != NULL) {
244  delete[] param2;
245  param2 = NULL;
246  }
247  if (param3 != NULL) {
248  delete[] param3;
249  param3 = NULL;
250  }
251 
252  if (maxPWM != NULL) {
253  delete[] maxPWM;
254  maxPWM = NULL;
255  }
256  if (original_max_pwm != NULL) {
257  delete[] original_max_pwm;
258  original_max_pwm = NULL;
259  }
260  if (limited_max_pwm != NULL) {
261  delete[] limited_max_pwm;
262  limited_max_pwm = NULL;
263  }
264 
265  if (currPos != NULL) {
266  delete[] currPos;
267  currPos = NULL;
268  }
269  if (currVel != NULL) {
270  delete[] currVel;
271  currVel = NULL;
272  }
273 
274  if (zeroPos != NULL) {
275  delete[] zeroPos;
276  zeroPos = NULL;
277  }
278  if (zeroVel != NULL) {
279  delete[] zeroVel;
280  zeroVel = NULL;
281  }
282 
283  if (homePos != NULL) {
284  delete[] homePos;
285  homePos = NULL;
286  }
287  if (homeVel != NULL) {
288  delete[] homeVel;
289  homeVel = NULL;
290  }
291 
292  return true;
293 }
294 
295 bool parametricCalibrator::calibrate(DeviceDriver *device)
296 {
297  yInfo() << deviceName << ": starting calibration";
298  yTrace();
299  abortCalib = false; //set true in quitCalibrate function (called on ctrl +c signal )
300 
301 
302  if (device==0)
303  {
304  yError() << deviceName << ": invalid device driver";
305  return false;
306  }
307 
308  yarp::dev::PolyDriver *p = dynamic_cast<yarp::dev::PolyDriver *>(device);
309  dev2calibrate = p;
310  if (p!=0)
311  {
312  p->view(iCalibrate);
313  p->view(iEncoders);
314  p->view(iPosition);
315  p->view(iPids);
316  p->view(iControlMode);
317  p->view(iAmp);
318  }
319  else
320  {
321  //yError() << deviceName << ": invalid dynamic cast to yarp::dev::PolyDriver";
322  //return false;
323 
324  //This is to ensure backward-compatibility with iCubInterface
325  yWarning() << deviceName << ": using parametricCalibrator on an old iCubInterface system. Upgrade to robotInterface is recommended.";
326  device->view(iCalibrate);
327  device->view(iEncoders);
328  device->view(iPosition);
329  device->view(iPids);
330  device->view(iControlMode);
331  device->view(iAmp);
332  }
333 
334  if (!(iCalibrate && iEncoders && iPosition && iPids && iControlMode)) {
335  yError() << deviceName << ": interface not found" << iCalibrate << iPosition << iPids << iControlMode;
336  return false;
337  }
338 
339  return calibrate();
340 }
341 
342 bool parametricCalibrator::calibrate()
343 {
344  int setOfJoint_idx = 0;
345  totJointsToCalibrate = 0;
346  calibJoints.clear();
347 
348  if (dev2calibrate==0)
349  {
350  yError() << deviceName << ": not able to find a valid device to calibrate";
351  return false;
352  }
353 
354  if ( !iEncoders->getAxes(&n_joints))
355  {
356  yError() << deviceName << ": error getting number of axes" ;
357  return false;
358  }
359 
360  std::list<int> currentSetList;
361  std::list<std::list<int> >::iterator Bit=joints.begin();
362  std::list<std::list<int> >::iterator Bend=joints.end();
363 
364  // count how many joints are there in the list of things to be calibrated
365  while(Bit != Bend)
366  {
367  currentSetList.clear();
368  currentSetList = (*Bit);
369  std::list<int>::iterator lit = currentSetList.begin();
370  std::list<int>::iterator lend = currentSetList.end();
371  totJointsToCalibrate += currentSetList.size();
372 
373  while(lit != lend)
374  {
375  calibJoints.push_back(*lit);
376  lit++;
377  }
378  Bit++;
379  }
380 
381  //before starting the calibration, checks for joints in hardware fault, and clears them if the user set the clearHwFaultBeforeCalibration option
382  for (int i=0; i<totJointsToCalibrate; i++)
383  {
384  checkHwFault(i);
385  }
386 
387  yDebug() << deviceName << ": Joints calibration order:" << calibJointsString.toString();
388 
389  if (totJointsToCalibrate > n_joints)
390  {
391  yError() << deviceName << ": too much axis to calibrate for this part..." << totJointsToCalibrate << " bigger than "<< n_joints;
392  return false;
393  }
394 
395  if (totJointsToCalibrate < n_joints)
396  {
397  yWarning() << deviceName << " is calibrating only a subset of the robot part. Calibrating " << totJointsToCalibrate << " over a total of " << n_joints;
398  }
399 
400  original_max_pwm = new double[n_joints];
401  limited_max_pwm = new double[n_joints];
402 
403  if(skipCalibration)
404  yWarning() << deviceName << ": skipCalibration flag is on! Setting safe pid but skipping calibration.";
405 
406  Bit=joints.begin();
407  std::list<int>::iterator lit; //iterator for joint in a set
408  while( (Bit != Bend) && (!abortCalib) ) // for each set of joints
409  {
410  setOfJoint_idx++;
411  currentSetList.clear();
412  currentSetList = (*Bit);
413 
414  // 1) set safe pid
415  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
416  {
417  yWarning() << deviceName << "lit is " << *lit;
418  if ( ((*lit) <0) || ((*lit) >= n_joints) ) // check the axes is within range
419  {
420  yError() << deviceName << ": asked to calibrate joint" << (*lit) << ", which is negative OR bigger than the number of axes for this part ("<< n_joints << ")";
421  abortCalib = true;
422  break;
423  }
424 
425  if(!iAmp->getPWMLimit((*lit), &original_max_pwm[(*lit)]) )
426  {
427  yError() << deviceName << ": getPWMLimit joint " << (*lit) << "failed... aborting calibration";
428  abortCalib = true;
429  break;
430  }
431 
432  limited_max_pwm[(*lit)] = original_max_pwm[(*lit)];
433 
434  if (maxPWM[(*lit)]==0)
435  {
436  yDebug() << deviceName << ": skipping maxPwm=0 of joint " << (*lit);
437  iAmp->setPWMLimit((*lit), original_max_pwm[(*lit)]);
438  }
439  else
440  {
441  if (maxPWM[(*lit)]<limited_max_pwm[(*lit)])
442  {
443  limited_max_pwm[(*lit)] = maxPWM[(*lit)];
444  iAmp->setPWMLimit((*lit), limited_max_pwm[(*lit)]);
445  }
446  else
447  {
448  yDebug() << deviceName << ": joint " << (*lit) << " has max_output already limited to a safe value: " << limited_max_pwm[(*lit)];
449  }
450  }
451  }
452 
453  if(skipCalibration) // if this flag is on, fake calibration
454  {
455  Bit++;
456  continue;
457  }
458 
459  //2) if calibration needs to go to hardware limits, enable joint
460  //VALE: i can add this cycle for calib on eth because it does nothing,
461  // because enablePid doesn't send command because joints are not calibrated
462 
463  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
464  {
465  if (type[*lit]==0 ||
466  type[*lit]==2 ||
467  type[*lit]==4 )
468  {
469  yDebug() << "In calibration " << deviceName << ": enabling joint " << *lit << " to test hardware limit";
470  iControlMode->setControlMode((*lit), VOCAB_CM_POSITION);
471  }
472  }
473 
474  Time::delay(0.1f);
475  if(abortCalib)
476  {
477  Bit++;
478  continue;
479  }
480 
481  //3) send calibration command
482  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
483  {
484  // Enable amp moved into EMS class;
485  // Here we just call the calibration procedure
486  calibrateJoint((*lit));
487  }
488 
489  Time::delay(0.1f);
490 
491  for(lit = currentSetList.begin(); lit != currentSetList.end(); lit++) //for each joint of set
492  {
493  iEncoders->getEncoder((*lit), &currPos[(*lit)]);
494  yDebug() << deviceName << ": set" << setOfJoint_idx << "j" << (*lit) << ": Calibrating... enc values AFTER calib: " << currPos[(*lit)];
495  }
496 
497  if(abortCalib)
498  {
499  Bit++;
500  continue;
501  }
502 
503  //4) check calibration result
504  if(checkCalibrateJointEnded((*Bit)) ) //check calibration on entire set
505  {
506  yDebug() << deviceName << ": set" << setOfJoint_idx << ": Calibration ended, going to zero!\n";
507  }
508  else // keep pid safe and go on
509  {
510  yError() << deviceName << ": set" << setOfJoint_idx << ": Calibration went wrong! Disabling axes and keeping safe pid limit\n";
511 
512  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
513  {
514  iControlMode->setControlMode((*lit),VOCAB_CM_IDLE);
515  }
516  Bit++;
517  continue; //go to next set
518  }
519 
520  // 5) if calibration finish with success enable disabled joints in order to move them to zero
521  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
522  {
523  // if the joint han not been enabled at point 1, now i enable it
524  //iAmps->enableAmp((*lit));
525  if (type[*lit]!=0 &&
526  type[*lit]!=2 &&
527  type[*lit]!=4 )
528  {
529  iControlMode->setControlMode((*lit), VOCAB_CM_POSITION);
530  }
531  }
532 
533  if(abortCalib)
534  {
535  Bit++;
536  continue;
537  }
538  Time::delay(0.5f); // needed?
539 
540  //6) go to zero
541  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
542  {
543  // Manda in Zero
544  goToZero((*lit));
545  }
546 
547  if(abortCalib)
548  {
549  Bit++;
550  continue;
551  }
552  Time::delay(1.0); // needed?
553 
554  //7) check joints are in position
555  bool goneToZero = true;
556  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
557  {
558  goneToZero &= checkGoneToZeroThreshold(*lit);
559  }
560 
561  if(abortCalib)
562  {
563  Bit++;
564  continue;
565  }
566 
567  if(goneToZero)
568  {
569  yDebug() << deviceName << ": set" << setOfJoint_idx << ": Reached zero position!\n";
570  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
571  {
572  iAmp->setPWMLimit((*lit),original_max_pwm[(*lit)]);
573  }
574  }
575  else // keep pid safe and go on
576  {
577  yError() << deviceName << ": set" << setOfJoint_idx << ": some axis got timeout while reaching zero position... disabling this set of axes\n";
578  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
579  {
580  iControlMode->setControlMode((*lit),VOCAB_CM_IDLE);
581  }
582  }
583 
584  // Go to the next set of joints to calibrate... if any
585  Bit++;
586  }
587 
588  if(abortCalib)
589  {
590  yError() << deviceName << ": calibration has been aborted!I'm going to disable all joints..." ;
591  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
592  {
593  iControlMode->setControlMode(*lit, VOCAB_CM_IDLE);
594  }
595  return false;
596  }
597  std::lock_guard<std::mutex> lck(calibMutex);
598  isCalibrated = true;
599  return isCalibrated;
600 }
601 
602 bool parametricCalibrator::calibrateJoint(int j)
603 {
604  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
605  {
606  yError("%s cannot perform 'calibration' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
607  return false;
608  }
609  yInfo() << deviceName << ": Calling calibrateJoint on joint "<< j << " with params: " << type[j] << param1[j] << param2[j] << param3[j];
610  return iCalibrate->calibrateAxisWithParams(j, type[j], param1[j], param2[j], param3[j]);
611 }
612 
613 bool parametricCalibrator::checkCalibrateJointEnded(std::list<int> set)
614 {
615  int timeout = 0;
616  bool calibration_ok = false;
617 
618  std::list<int>::iterator lit;
619  std::list<int>::iterator lend;
620 
621  lend = set.end();
622  while(!calibration_ok && (timeout <= CALIBRATE_JOINT_TIMEOUT))
623  {
624  calibration_ok = true;
625  Time::delay(1.0);
626  lit = set.begin();
627  while(lit != lend) // per ogni giunto del set
628  {
629  if (abortCalib)
630  {
631  yWarning() << deviceName << ": calibration aborted\n";
632  }
633 
634  // Joint with absolute sensor doesn't need to move, so they are ok with just the calibration message,
635  // but I'll check anyway, in order to have everything the same
636  if( !(calibration_ok &= iCalibrate->calibrationDone((*lit))) ) // the assignement inside the if is INTENTIONAL
637  break;
638  lit++;
639  }
640 
641  timeout++;
642  }
643 
644  if(timeout > CALIBRATE_JOINT_TIMEOUT)
645  yError() << deviceName << ":Timeout while calibrating " << (*lit) << "\n";
646  else
647  yDebug() << deviceName << ": calib joint ended";
648 
649  return calibration_ok;
650 }
651 
652 bool parametricCalibrator::checkHwFault(int j)
653 {
654  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
655  {
656  yError("%s cannot perform 'check hw fault' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
657  return false;
658  }
659 
660  int mode=0;
661  iControlMode->getControlMode(j,&mode);
662  if (mode == VOCAB_CM_HW_FAULT)
663  {
664  if (clearHwFault)
665  {
666  iControlMode->setControlMode(j,VOCAB_CM_FORCE_IDLE);
667  yWarning() << deviceName <<": detected an hardware fault on joint " << j << ". An attempt will be made to clear it.";
668  Time::delay(0.02f);
669  iControlMode->getControlMode(j,&mode);
670  if (mode == VOCAB_CM_HW_FAULT)
671  {
672  yError() << deviceName <<": unable to clear the hardware fault detected on joint " << j << " before starting the calibration procedure!";
673  return false;
674  }
675  else if (mode == VOCAB_CM_IDLE)
676  {
677  yWarning() << deviceName <<": hardware fault on joint " << j << " successfully cleared.";
678  return true;
679  }
680  else
681  {
682  yError() << deviceName <<": an unknown error occured while trying the hardware fault on joint " << j ;
683  return false;
684  }
685  }
686  else
687  {
688  yError() << deviceName <<": detected an hardware fault on joint " << j << " before starting the calibration procedure!";
689  return false;
690  }
691  }
692  return true;
693 }
694 
695 bool parametricCalibrator::goToZero(int j)
696 {
697  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
698  {
699  yError("%s cannot perform 'go to zero' operation because joint number %d is out of range [%s]!!", deviceName.c_str(), j, calibJointsString.toString().c_str());
700  return false;
701  }
702 
703  bool ret = true;
704  if (abortCalib) return true;
705  yDebug() << deviceName << ": Sending positionMove to joint" << j << " (desired pos: " << zeroPos[j] << "desired speed: " << zeroVel[j] <<" )";
706  ret = iPosition->setRefSpeed(j, zeroVel[j]);
707  ret &= iPosition->positionMove(j, zeroPos[j]);
708  return ret;
709 }
710 
711 bool parametricCalibrator::checkGoneToZeroThreshold(int j)
712 {
713  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
714  {
715  yError("%s cannot perform 'check gone to zero' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
716  return false;
717  }
718 
719  if (skipCalibration) return false;
720 
721  // wait.
722  bool finished = false;
723 // double ang[4];
724  double angj = 0;
725  double output = 0;
726  double delta=0;
727  int mode=0;
728  bool done = false;
729 
730  double start_time = yarp::os::Time::now();
731  while ( (!finished) && (!abortCalib))
732  {
733  iEncoders->getEncoder(j, &angj);
734  iPosition->checkMotionDone(j, &done);
735  iControlMode->getControlMode(j, &mode);
736  iPids->getPidOutput(VOCAB_PIDTYPE_POSITION, j, &output);
737 
738  delta = fabs(angj-zeroPos[j]);
739  yDebug("%s: checkGoneToZeroThreshold: joint: %d curr: %.3f des: %.3f -> delta: %.3f threshold: %.3f output: %.3f mode: %s" ,deviceName.c_str(),j,angj, zeroPos[j],delta, zeroPosThreshold[j], output, yarp::os::Vocab32::decode(mode).c_str());
740 
741  if (delta < zeroPosThreshold[j] && done)
742  {
743  yDebug("%s: checkGoneToZeroThreshold: joint: %d completed with delta: %.3f over: %.3f" ,deviceName.c_str(),j,delta, zeroPosThreshold[j]);
744  finished=true;
745  break;
746  }
747  if (yarp::os::Time::now() - start_time > GO_TO_ZERO_TIMEOUT)
748  {
749  yError() << deviceName << ": checkGoneToZeroThreshold: joint " << j << " Timeout while going to zero!";
750  break;
751  }
752  if (mode == VOCAB_CM_IDLE)
753  {
754  yError() << deviceName << ": checkGoneToZeroThreshold: joint " << j << " is idle, skipping!";
755  break;
756  }
757  if (mode == VOCAB_CM_HW_FAULT)
758  {
759  yError() << deviceName <<": checkGoneToZeroThreshold: hardware fault on joint " << j << ", skipping!";
760  break;
761  }
762  if (abortCalib)
763  {
764  yWarning() << deviceName <<": checkGoneToZeroThreshold: joint " << j << " Aborting wait while going to zero!\n";
765  break;
766  }
767  Time::delay(0.5);
768  }
769  return finished;
770 }
771 
772 bool parametricCalibrator::park(DeviceDriver *dd, bool wait)
773 {
774  yTrace();
775  bool ret=false;
776  abortParking=false;
777 
778  calibMutex.lock();
779  if(!isCalibrated)
780  {
781  yWarning() << deviceName << ": Calling park without calibration... skipping";
782  calibMutex.unlock();
783  return true;
784  }
785  calibMutex.unlock();
786 
787  if(skipCalibration)
788  {
789  yWarning() << deviceName << ": skipCalibration flag is on!! Faking park!!";
790  return true;
791  }
792 
793  int* currentControlModes = new int [n_joints];
794  bool* cannotPark = new bool [n_joints];
795  bool res = iControlMode->getControlModes(currentControlModes);
796  if(!res)
797  {
798  yError() << deviceName << ": error getting control mode during parking";
799  }
800 
801  std::list<int>::iterator lit;
802  for(lit = calibJoints.begin(); lit != calibJoints.end() && !abortCalib; lit++) //for each joint of set
803  {
804  switch(currentControlModes[(*lit)])
805  {
806  case VOCAB_CM_IDLE:
807  {
808  yError() << deviceName << ": joint " << (*lit) << " is idle, skipping park";
809  cannotPark[(*lit)] = true;
810  }
811  break;
812 
813  case VOCAB_CM_HW_FAULT:
814  {
815  yError() << deviceName << ": joint " << (*lit) << " has an hardware fault, skipping park";
816  cannotPark[(*lit)] = true;
817  }
818  break;
819 
820  case VOCAB_CM_NOT_CONFIGURED:
821  {
822  yError() << deviceName << ": joint " << (*lit) << " is not configured, skipping park";
823  cannotPark[(*lit)] = true;
824  }
825  break;
826 
827  case VOCAB_CM_UNKNOWN:
828  {
829  yError() << deviceName << ": joint " << (*lit) << " is in unknown state, skipping park";
830  cannotPark[(*lit)] = true;
831  }
832 
833  default:
834  {
835  iControlMode->setControlMode((*lit), VOCAB_CM_POSITION);
836  cannotPark[(*lit)] = false;
837  }
838  }
839  }
840 
841  iPosition->setRefSpeeds(homeVel);
842  iPosition->positionMove(homePos);
843  Time::delay(0.01);
844 
845  if (wait)
846  {
847  int timeout = 0; //this variable is shared between all joints
848  for(lit = calibJoints.begin(); lit != calibJoints.end() && !abortCalib; lit++) //for each joint of set
849  {
850  if (cannotPark[(*lit)] ==false)
851  {
852  yDebug() << deviceName.c_str() << ": Moving to park position, joint:" << (*lit);
853  bool done=false;
854  iPosition->checkMotionDone((*lit), &done);
855  while ( (!done) && (timeout<PARK_TIMEOUT) && (!abortParking) )
856  {
857  Time::delay(1);
858  timeout++;
859  iPosition->checkMotionDone((*lit), &done);
860  }
861  if (!done)
862  {
863  yError() << deviceName << ": joint " << (*lit) << " not in position after a timeout of" << PARK_TIMEOUT <<" seconds";
864  }
865  }
866  }
867  }
868 
869  yDebug() << deviceName.c_str() << ": Park " << (abortParking ? "aborted" : "completed");
870  for(lit = calibJoints.begin(); lit != calibJoints.end() && !abortCalib; lit++) //for each joint of set
871 
872  {
873  switch(currentControlModes[(*lit)])
874  {
875  case VOCAB_CM_IDLE:
876  case VOCAB_CM_HW_FAULT:
877  case VOCAB_CM_NOT_CONFIGURED:
878  case VOCAB_CM_UNKNOWN:
879  // Do nothing.
880  break;
881  default:
882  {
883  iControlMode->setControlMode((*lit), VOCAB_CM_IDLE);
884  }
885  }
886  }
887  return true;
888 }
889 
891 {
892  yDebug() << deviceName.c_str() << ": Quitting calibrate\n";
893  abortCalib = true;
894  return true;
895 }
896 
898 {
899  yDebug() << deviceName.c_str() << ": Quitting parking\n";
900  abortParking=true;
901  return true;
902 }
903 
904 yarp::dev::IRemoteCalibrator *parametricCalibrator::getCalibratorDevice()
905 {
906  return this;
907 }
908 
910 {
911  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
912  {
913  yError("%s cannot perform 'calibrate' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
914  return false;
915  }
916  return calibrateJoint(j);
917 }
918 
920 {
921  yTrace();
922  return calibrate();
923 }
924 
926 {
927  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
928  {
929  yError("%s cannot perform 'homing' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
930  return false;
931  }
932  return goToZero(j);
933 }
934 
936 {
937  yTrace();
938  bool ret = true;
939  std::list<int>::iterator lit;
940  for(lit = calibJoints.begin(); lit != calibJoints.end() && !abortCalib; lit++) //for each joint of set
941  {
942  ret = homingSingleJoint(*lit) && ret;
943  }
944  return ret;
945 }
946 
948 {
949  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
950  {
951  yError("%s cannot perform 'park' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
952  return false;
953  }
954 
955  int nj=0;
956  abortParking=false;
957 
958  calibMutex.lock();
959  if(!isCalibrated)
960  {
961  yWarning() << deviceName << ": Calling park without calibration... skipping";
962  calibMutex.unlock();
963  return true;
964  }
965  calibMutex.unlock();
966 
967  if(skipCalibration)
968  {
969  yWarning() << deviceName << ": skipCalibration flag is on!! Faking park!!";
970  return true;
971  }
972 
973  int currentControlMode;
974  bool cannotPark;
975  bool res = iControlMode->getControlMode(j, &currentControlMode);
976  if(!res)
977  {
978  yError() << deviceName << ": error getting control mode during parking";
979  }
980 
981  if(currentControlMode != VOCAB_CM_IDLE &&
982  currentControlMode != VOCAB_CM_HW_FAULT)
983  {
984  iControlMode->setControlMode(j, VOCAB_CM_POSITION);
985  cannotPark = false;
986  }
987  else if (currentControlMode == VOCAB_CM_IDLE)
988  {
989  yError() << deviceName << ": joint " << j << " is idle, skipping park";
990  cannotPark = true;
991  }
992  else if (currentControlMode == VOCAB_CM_HW_FAULT)
993  {
994  yError() << deviceName << ": joint " << j << " has an hardware fault, skipping park";
995  cannotPark = true;
996  }
997 
998  iPosition->setRefSpeed(j, homeVel[j]);
999  iPosition->positionMove(j, homePos[j]);
1000  Time::delay(0.01);
1001 
1002  if (_wait)
1003  {
1004  int timeout = 0; //this variable is shared between all joints
1005  if (cannotPark == false)
1006  {
1007  yDebug() << deviceName.c_str() << ": Moving to park position, joint:" << j;
1008  bool done=false;
1009  iPosition->checkMotionDone(j, &done);
1010  while((!done) && (timeout<PARK_TIMEOUT) && (!abortParking))
1011  {
1012  Time::delay(1);
1013  timeout++;
1014  iPosition->checkMotionDone(j, &done);
1015  }
1016  if(!done)
1017  {
1018  yError() << deviceName << ": joint " << j << " not in position after a timeout of" << PARK_TIMEOUT <<" seconds";
1019  }
1020  }
1021  }
1022 
1023  yDebug() << deviceName.c_str() << ": Park " << (abortParking ? "aborted" : "completed");
1024  iControlMode->setControlMode(j,VOCAB_CM_IDLE);
1025  return true;
1026 }
1027 
1029 {
1030  yTrace();
1031  if(!isCalibrated)
1032  {
1033  yError() << "Device is not calibrated therefore cannot be parked";
1034  return false;
1035  }
1036 
1037  return park(dev2calibrate);
1038 }
1039 
1040 
1041 // eof
1042 
virtual bool homingSingleJoint(int j) override
virtual bool calibrateSingleJoint(int j) override
virtual bool parkSingleJoint(int j, bool _wait=true) override
virtual bool close() override
Close the device driver.
virtual bool park(DeviceDriver *dd, bool wait=true) override
virtual bool calibrateWholePart() override
virtual yarp::dev::IRemoteCalibrator * getCalibratorDevice()
bool calibrate(DeviceDriver *dd) override
Calibrate method.
virtual bool homingWholePart() override
virtual bool open(yarp::os::Searchable &config) override
Open the device driver.
bool done
Definition: main.cpp:42
const double GO_TO_ZERO_TIMEOUT
const int CALIBRATE_JOINT_TIMEOUT
static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
const int PARK_TIMEOUT
const double POSITION_THRESHOLD
out
Definition: sine.m:8