iCub-main
parametricCalibratorEth.cpp
Go to the documentation of this file.
1 
2 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
3 
4 /*
5  * Copyright (C) 2014 iCub Facility, Istituto Italiano di Tecnologia
6  * Authors: Alberto Cardellino, Marco Randazzo, Valentina Gaggero
7  * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
8  *
9  */
10 
11 #include <yarp/os/Time.h>
12 #include <yarp/dev/PolyDriver.h>
13 
15 #include <math.h>
16 #include <algorithm>
17 
18 #include <yarp/os/LogStream.h>
19 
20 using namespace yarp::os;
21 using namespace yarp::dev;
22 
23 const int PARK_TIMEOUT = 30;
24 const double GO_TO_ZERO_TIMEOUT = 10;
25 const int CALIBRATE_JOINT_TIMEOUT = 20;
26 
27 //#warning "Use extractGroup to verify size of parameters matches with number of joints, this will avoid crashes"
28 static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
29 {
30  size++; // size includes also the name of the parameter
31  Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
32  if (tmp.isNull())
33  {
34  yError () << key1.c_str() << " not found\n";
35  return false;
36  }
37 
38  if(tmp.size()!=size)
39  {
40  yError () << key1.c_str() << " incorrect number of entries in board.";
41  return false;
42  }
43 
44  out=tmp;
45  return true;
46 }
47 
48 
49 // helper for parsing config file
50 bool parametricCalibratorEth::parseSequenceGroup(yarp::os::Searchable &config, std::string sequence, std::vector<PositionSequence> &seqList)
51 {
52  Bottle parkSeq_group = config.findGroup(sequence);
53  if (parkSeq_group.isNull())
54  {
55  // yWarning() << "parametricCalibrator " << deviceName << "Missing <" << sequence << "> group";
56  return false;
57  }
58 
59  Bottle xtmp;
60  int numOfSeq = 0;
61 
62  if(!extractGroup(parkSeq_group, xtmp, "numberOfSequences", "number of sequences listed ", 1))
63  {
64  return false;
65  }
66 
67  numOfSeq = xtmp.get(1).asInt32();
68  // create space in vector of sequences
69  seqList.resize(numOfSeq);
70 
71  if(numOfSeq < 0)
72  {
73  yError() << "ParametricCalibratorEth " << deviceName << "<numberOfSequences> must be a positive integer";
74  return false;
75  }
76 
77  // read all sequences
78  for(int seq_idx=0; seq_idx<numOfSeq; seq_idx++)
79  {
80  char sequence_name[80];
81  snprintf(sequence_name, 80, "SEQUENCE_%d", seq_idx);
82 
83  Bottle &seq_i = parkSeq_group.findGroup(sequence_name);
84  if(seq_i.isNull())
85  {
86  yError() << "ParametricCalibratorEth " << deviceName << "cannot find " << sequence_name;
87  return false;
88  }
89 
90  // 1) Seq number
91  seqList.at(seq_idx).seq_num = seq_idx;
92 
93  // 2) Read positions fromn config file
94  Bottle & poss = seq_i.findGroup("position", "desired parking position");
95  if (poss.isNull())
96  {
97  yError() << "ParametricCalibratorEth " << deviceName << ": <position> parameter not found for sequence " << sequence_name;
98  return false;
99  }
100 
101  // 3) Read velocities fromn config file
102  Bottle &vels = seq_i.findGroup("velocity", "desired parking velocities");
103  if (vels.isNull())
104  {
105  yError() << "ParametricCalibratorEth " << deviceName << ": <velocity> parameter not found for sequence " << sequence_name;
106  return false;
107  }
108 
109  if(( poss.size() -1 != n_joints) || (vels.size() -1 != n_joints))
110  {
111  yError() << "ParametricCalibratorEth " << deviceName << ": <position> or <velocity> parameter size for sequence " << sequence_name << " doesn not match the number of joint being calibrated.\n" << \
112  "Part joint number is " << n_joints << " while <position> size is " << poss.size()-1 << " and <velocity> size is " << vels.size()-1 << "; joint number is " << n_joints;
113  return false;
114  }
115 
116  // Store data in memory
117  seqList[seq_idx].seq_num = seq_idx;
118  seqList[seq_idx].positions.reserve(n_joints);
119  seqList[seq_idx].velocities.reserve(n_joints);
120 
121  for (int j = 1; j <n_joints+1; j++)
122  {
123  seqList[seq_idx].positions .push_back(poss.get(j).asFloat64());
124  seqList[seq_idx].velocities.push_back(vels.get(j).asFloat64());
125  }
126  }
127  return true;
128 }
129 
130 parametricCalibratorEth::parametricCalibratorEth() :
131  calibParams(nullptr),
132  original_max_pwm(nullptr),
133  limited_max_pwm(nullptr),
134  startupMaxPWM(nullptr),
135  currPos(nullptr),
136  currVel(nullptr),
137 // legacyParkingPosition(0),
138  startupPosThreshold(0),
139  abortCalib(false),
140  isCalibrated(false),
141  skipCalibration(false),
142  clearHwFault(false),
143  n_joints(0),
144  timeout_goToZero(nullptr),
145  timeout_calibration(nullptr),
146  disableHomeAndPark(nullptr),
147  disableStartupPosCheck(nullptr),
148  totJointsToCalibrate(0),
149  useLegacyParking(true),
150  currentParkingSeq_step(0)
151 {
152 }
153 
155 {
156  yTrace();
157  close();
158 }
159 
160 bool parametricCalibratorEth::open(yarp::os::Searchable& config)
161 {
162  yTrace();
163  Property p;
164  p.fromString(config.toString());
165 
166  if (p.check("GENERAL")==false)
167  {
168  yError() << "Parametric calibrator: missing [GENERAL] section";
169  return false;
170  }
171 
172  if(p.findGroup("GENERAL").check("deviceName"))
173  {
174  deviceName = p.findGroup("GENERAL").find("deviceName").asString();
175  }
176  else
177  {
178  yError() << "Parametric calibrator: missing deviceName parameter";
179  return false;
180  }
181 
182  std::string str;
183  if(config.findGroup("GENERAL").find("verbose").asInt32())
184  {
185  str=config.toString().c_str();
186  yTrace() << deviceName.c_str() << str;
187  }
188 
189  // Check clearHwFaultBeforeCalibration
190  Value val_clearHwFault = config.findGroup("GENERAL").find("clearHwFaultBeforeCalibration");
191  if(val_clearHwFault.isNull())
192  {
193  clearHwFault = false;
194  }
195  else
196  {
197  if(!val_clearHwFault.isBool())
198  {
199  yError() << deviceName.c_str() << ": clearHwFaultBeforeCalibration bool param is different from accepted values (true / false). Assuming false";
200  clearHwFault = false;
201  }
202  else
203  {
204  clearHwFault = val_clearHwFault.asBool();
205  if(clearHwFault)
206  yInfo() << deviceName.c_str() << ": clearHwFaultBeforeCalibration option enabled\n";
207  }
208  }
209 
210  // Check useRawEncoderData, it robot is using raw data, force to skip the calibration because it will be dangerous!
211  Value use_raw = config.findGroup("GENERAL").find("useRawEncoderData");
212  bool useRawEncoderData;
213 
214  if(use_raw.isNull())
215  {
216  useRawEncoderData = false;
217  }
218  else
219  {
220  if(!use_raw.isBool())
221  {
222  yError() << deviceName.c_str() << ": useRawEncoderData bool param is different from accepted values (true / false). Assuming false";
223  useRawEncoderData = false;
224  }
225  else
226  {
227  useRawEncoderData = use_raw.asBool();
228  if(useRawEncoderData)
229  yWarning() << deviceName.c_str() << ": MotionControl is using raw data from encoders! Be careful. \n" <<
230  "\t forcing to skip the calibration";
231  }
232  }
233 
234 // yWarning() << deviceName.c_str() << ": useRawEncoderData is " << useRawEncoderData;
235 
236  if(useRawEncoderData)
237  {
238  skipCalibration = true;
239  }
240  else
241  {
242  // Check useRawEncoderData = skip root calibration -- use with care
243  Value checkSkipCalib = config.findGroup("GENERAL").find("skipCalibration");
244  if(checkSkipCalib.isNull())
245  {
246  skipCalibration = false;
247  }
248  else
249  {
250  if(!checkSkipCalib.isBool())
251  {
252  yWarning() << deviceName << ": skipCalibration bool param is different from accepted values (true / false). Assuming false";
253  skipCalibration = false;
254  }
255  else
256  {
257  skipCalibration = checkSkipCalib.asBool();
258  if(skipCalibration)
259  {
260  yWarning() << deviceName << ": skipping calibration!! This option was set in general.xml file.";
261  yWarning() << deviceName << ": BE CAREFUL USING THE ROBOT IN THIS CONFIGURATION! See 'skipCalibration' param in config file";
262  }
263  }
264  }
265  }
266 
267  if(p.findGroup("GENERAL").check("joints"))
268  {
269  n_joints = p.findGroup("GENERAL").find("joints").asInt32();
270  }
271  else if(p.findGroup("GENERAL").check("Joints"))
272  {
273  // This is needed to be backward compatibile with old iCubInterface
274  n_joints = p.findGroup("GENERAL").find("Joints").asInt32();
275  }
276  else
277  {
278  yError() << deviceName.c_str() << ": missing joints parameter" ;
279  return false;
280  }
281 
282  calibParams = new CalibrationParameters[n_joints];
283  startupMaxPWM = new int[n_joints];
284 
285  legacyStartupPosition.seq_num = 0;
286  legacyStartupPosition.positions.resize(n_joints);
287  legacyStartupPosition.velocities.resize(n_joints);
288  currPos = new double[n_joints];
289  currVel = new double[n_joints];
290  legacyParkingPosition.seq_num = 0;
291  legacyParkingPosition.positions.resize(n_joints);
292  legacyParkingPosition.velocities.resize(n_joints);
293 
294 
295  timeout_goToZero = new int[n_joints];
296  timeout_calibration = new int[n_joints];
297  startupPosThreshold = new double[n_joints];
298  disableHomeAndPark = new int[n_joints];
299  disableStartupPosCheck = new int[n_joints];
300  original_max_pwm = new double[n_joints];
301  limited_max_pwm = new double[n_joints];
302 
303  for (int i = 0; i < n_joints; i++) timeout_goToZero[i] = 10;
304  for (int i = 0; i < n_joints; i++) timeout_calibration[i] = 20;
305  for (int i = 0; i < n_joints; i++) disableHomeAndPark[i] = false;
306  for (int i = 0; i < n_joints; i++) disableStartupPosCheck[i] = false;
307 
308  int i=0;
309 
310  Bottle& xtmp = p.findGroup("CALIBRATION").findGroup("calibration1");
311  if (xtmp.size()-1!=n_joints) {yError() << deviceName << ": invalid number of Calibration1 params " << xtmp.size()<< " " << n_joints; return false;}
312  for (i = 1; i < xtmp.size(); i++) calibParams[i-1].param1 = xtmp.get(i).asFloat64();
313 
314  xtmp = p.findGroup("CALIBRATION").findGroup("calibration2");
315  if (xtmp.size()-1!=n_joints) {yError() << deviceName << ": invalid number of Calibration2 params"; return false;}
316  for (i = 1; i < xtmp.size(); i++) calibParams[i - 1].param2 = xtmp.get(i).asFloat64();
317 
318  xtmp = p.findGroup("CALIBRATION").findGroup("calibration3");
319  if (xtmp.size()-1!=n_joints) {yError() << deviceName << ": invalid number of Calibration3 params"; return false;}
320  for (i = 1; i < xtmp.size(); i++) calibParams[i - 1].param3 = xtmp.get(i).asFloat64();
321 
322  xtmp = p.findGroup("CALIBRATION").findGroup("calibration4");
323  if (xtmp.size() - 1 != n_joints) { yError() << deviceName << ": invalid number of Calibration4 params"; return false; }
324  for (i = 1; i < xtmp.size(); i++) calibParams[i - 1].param4 = xtmp.get(i).asFloat64();
325 
326  xtmp = p.findGroup("CALIBRATION").findGroup("calibration5");
327  if (xtmp.size() - 1 != n_joints) { yError() << deviceName << ": invalid number of Calibration5 params"; return false; }
328  for (i = 1; i < xtmp.size(); i++) calibParams[i - 1].param5 = xtmp.get(i).asFloat64();
329 
330  xtmp = p.findGroup("CALIBRATION").findGroup("calibrationType");
331  if (xtmp.size()-1!=n_joints) {yError() << deviceName << ": invalid number of Calibration3 params"; return false;}
332  for (i = 1; i < xtmp.size(); i++) calibParams[i - 1].type = (unsigned char)xtmp.get(i).asFloat64();
333 
334  xtmp = p.findGroup("CALIBRATION").findGroup("calibrationZero");
335  if (xtmp.size() - 1 != n_joints) { yError() << deviceName << ": invalid number of calibrationZero params"; return false; }
336  for (i = 1; i < xtmp.size(); i++) calibParams[i - 1].paramZero = xtmp.get(i).asFloat64();
337 
338  xtmp = p.findGroup("CALIBRATION").findGroup("calibrationDelta");
339  if (xtmp.size() - 1 != n_joints) { yError() << deviceName << ": invalid number of calibrationDelta params"; return false; }
340  for (i = 1; i < xtmp.size(); i++) calibParams[i - 1].paramZero += xtmp.get(i).asFloat64();
341 
342  xtmp = p.findGroup("CALIBRATION").findGroup("calibrationTimeout");
343  if (xtmp.size() - 1 != n_joints) { } //this parameter is optional
344  else { for (i = 1; i < xtmp.size(); i++) timeout_calibration[i - 1] = (int)xtmp.get(i).asFloat64(); }
345 
346  xtmp = p.findGroup("CALIBRATION").findGroup("startupPosition");
347  if (xtmp.size()-1!=n_joints) {yError() << deviceName << ": invalid number of startupPosition params"; return false;}
348  for (i = 1; i < xtmp.size(); i++) legacyStartupPosition.positions[i-1] = xtmp.get(i).asFloat64();
349 
350  xtmp = p.findGroup("CALIBRATION").findGroup("startupVelocity");
351  if (xtmp.size()-1!=n_joints) {yError() << deviceName << ": invalid number of startupVelocity params"; return false;}
352  for (i = 1; i < xtmp.size(); i++) legacyStartupPosition.velocities[i - 1] = xtmp.get(i).asFloat64();
353 
354  // First find new version of parking sequence. Optional right now for back compatibility
355  useLegacyParking = ! parseSequenceGroup(p, "PARKING_SEQUENCE", parkingSequence);
356 
357  Bottle homeGroup = p.findGroup("HOME");
358  if(useLegacyParking)
359  {
360  if( homeGroup.isNull())
361  {
362  yError() << "Parking position not found. Either <HOME> or <PARKING_SEQUENCE> must be specified in config file";
363  return false;
364  }
365  /*
366  else
367  {
368  yWarning() << "<HOME> group is deprecated in favour of <PARKING_SEQUENCE>";
369  }
370  */
371  xtmp = homeGroup.findGroup("positionHome");
372  if (xtmp.size()-1!=n_joints) {yError() << deviceName << ": invalid number of PositionHome params"; return false;}
373  legacyParkingPosition.positions.resize(n_joints);
374  for (i = 1; i < xtmp.size(); i++)
375  legacyParkingPosition.positions[i-1] = xtmp.get(i).asFloat64();
376 
377  xtmp = homeGroup.findGroup("velocityHome");
378  if (xtmp.size()-1!=n_joints) {yError() << deviceName << ": invalid number of VelocityHome params"; return false;}
379  legacyParkingPosition.velocities.resize(n_joints);
380  for (i = 1; i < xtmp.size(); i++)
381  legacyParkingPosition.velocities[i-1] = xtmp.get(i).asFloat64();
382  }
383 
384  // this parameter may be superseded by new park sequence mechanism, probably also for startup.
385  xtmp = homeGroup.findGroup("disableHomeAndPark");
386  if (xtmp.size() - 1 != n_joints) { } //this parameter is optional
387  else { for (i = 1; i < xtmp.size(); i++) disableHomeAndPark[i - 1] = xtmp.get(i).asInt32(); }
388 
389  xtmp = p.findGroup("CALIBRATION").findGroup("startupMaxPwm");
390  if (xtmp.size()-1!=n_joints) {yError() << deviceName << ": invalid number of startupMaxPwm params"; return false;}
391  for (i = 1; i < xtmp.size(); i++) startupMaxPWM[i-1] = xtmp.get(i).asInt32();
392 
393  xtmp = p.findGroup("CALIBRATION").findGroup("startupPosThreshold");
394  if (xtmp.size()-1!=n_joints) {yError() << deviceName << ": invalid number of startupPosThreshold params"; return false;}
395  for (i = 1; i < xtmp.size(); i++) startupPosThreshold[i-1] = xtmp.get(i).asFloat64();
396 
397  xtmp = p.findGroup("CALIBRATION").findGroup("startupDisablePosCheck");
398  if (xtmp.size() - 1 != n_joints) { } //this parameter is optional
399  else { for (i = 1; i < xtmp.size(); i++) disableStartupPosCheck[i - 1] = xtmp.get(i).asInt32(); }
400 
401  xtmp = p.findGroup("CALIBRATION").findGroup("startupTimeout");
402  if (xtmp.size() - 1 != n_joints) {} //this parameter is optional
403  else { for (i = 1; i < xtmp.size(); i++) timeout_goToZero[i - 1] = xtmp.get(i).asFloat64(); }
404 
405  xtmp = p.findGroup("CALIB_ORDER");
406  int calib_order_size = xtmp.size();
407  if (calib_order_size <= 1) {yError() << deviceName << ": invalid number CALIB_ORDER params"; return false;}
408  //yDebug() << "CALIB_ORDER: group size: " << xtmp.size() << " values: " << xtmp.toString().c_str();
409 
410  std::list<int> tmp;
411  for(int i=1; i<xtmp.size(); i++)
412  {
413  tmp.clear();
414  Bottle *set;
415  set= xtmp.get(i).asList();
416 
417  for(int j=0; j<set->size(); j++)
418  {
419  tmp.push_back(set->get(j).asInt32() );
420  }
421  joints.push_back(tmp);
422  }
423  return true;
424 }
425 
427 {
428  yTrace();
429  if (calibParams != NULL) {
430  delete[] calibParams;
431  calibParams = NULL;
432  }
433 
434  if (startupMaxPWM != NULL) {
435  delete[] startupMaxPWM;
436  startupMaxPWM = NULL;
437  }
438  if (original_max_pwm != NULL) {
439  delete[] original_max_pwm;
440  original_max_pwm = NULL;
441  }
442  if (limited_max_pwm != NULL) {
443  delete[] limited_max_pwm;
444  limited_max_pwm = NULL;
445  }
446 
447  if (timeout_goToZero != NULL) {
448  delete[] timeout_goToZero;
449  timeout_goToZero = NULL;
450  }
451 
452  if (timeout_calibration != NULL) {
453  delete[] timeout_calibration;
454  timeout_calibration = NULL;
455  }
456 
457  if (currPos != NULL) {
458  delete[] currPos;
459  currPos = NULL;
460  }
461  if (currVel != NULL) {
462  delete[] currVel;
463  currVel = NULL;
464  }
465 
466  if (disableHomeAndPark != NULL) {
467  delete[] disableHomeAndPark;
468  disableHomeAndPark = NULL;
469  }
470 
471  if (disableStartupPosCheck != NULL) {
472  delete[] disableStartupPosCheck;
473  disableStartupPosCheck = NULL;
474  }
475 
476  return true;
477 }
478 
479 bool parametricCalibratorEth::calibrate(DeviceDriver *device)
480 {
481  yInfo() << deviceName << ": starting calibration";
482  yTrace();
483  abortCalib = false; //set true in quitCalibrate function (called on ctrl +c signal )
484 
485  if (device==0)
486  {
487  yError() << deviceName << ": invalid device driver";
488  return false;
489  }
490 
491  yarp::dev::PolyDriver *p = dynamic_cast<yarp::dev::PolyDriver *>(device);
492  dev2calibrate = p;
493  if (p!=0)
494  {
495  p->view(iCalibrate);
496  p->view(iEncoders);
497  p->view(iPosition);
498  p->view(iPids);
499  p->view(iControlMode);
500  p->view(iAmp);
501  }
502  else
503  {
504  //yError() << deviceName << ": invalid dynamic cast to yarp::dev::PolyDriver";
505  //return false;
506 
507  //This is to ensure backward-compatibility with iCubInterface
508  yWarning() << deviceName << ": using parametricCalibrator on an old iCubInterface system. Upgrade to robotInterface is recommended.";
509  device->view(iCalibrate);
510  device->view(iEncoders);
511  device->view(iPosition);
512  device->view(iPids);
513  device->view(iControlMode);
514  device->view(iAmp);
515  }
516 
517  if (!(iCalibrate && iEncoders && iPosition && iPids && iControlMode)) {
518  yError() << deviceName << ": interface not found" << iCalibrate << iPosition << iPids << iControlMode;
519  return false;
520  }
521 
522  return calibrate();
523 }
524 
525 bool parametricCalibratorEth::calibrate()
526 {
527  int setOfJoint_idx = 0;
528  totJointsToCalibrate = 0;
529  calibJoints.clear();
530 
531  if (dev2calibrate==0)
532  {
533  yError() << deviceName << ": not able to find a valid device to calibrate";
534  return false;
535  }
536 
537  int n_joints_board{0};
538  if ( !iEncoders->getAxes(&n_joints_board))
539  {
540  yError() << deviceName << ": error getting number of axes" ;
541  return false;
542  }
543  if(n_joints_board != n_joints)
544  {
545  yError() << "ParametricCalibratorEth: " << deviceName << ": number of joints of device to calibrate (" << n_joints_board << \
546  ") does not match the number of joints in calibrator config file ("<< n_joints << ")";
547  return false;
548  }
549 
550  std::list<int> currentSetList;
551  std::list<std::list<int> >::iterator Bit=joints.begin();
552  std::list<std::list<int> >::iterator Bend=joints.end();
553 
554  // count how many joints there are in the list of things to be calibrated
555  while(Bit != Bend)
556  {
557  currentSetList.clear();
558  currentSetList = (*Bit);
559  std::list<int>::iterator lit = currentSetList.begin();
560  std::list<int>::iterator lend = currentSetList.end();
561  totJointsToCalibrate += currentSetList.size();
562 
563  while(lit != lend)
564  {
565  calibJoints.push_back(*lit);
566  lit++;
567  }
568  Bit++;
569  }
570 
571  //before starting the calibration, checks for joints in hardware fault, and clears them if the user set the clearHwFaultBeforeCalibration option
572  for (int i=0; i<totJointsToCalibrate; i++)
573  {
574  checkHwFault(i);
575  }
576 
577  yDebug() << deviceName << ": Joints calibration order:" << calibJointsString.toString();
578 
579  if (totJointsToCalibrate > n_joints)
580  {
581  yError() << deviceName << ": too much axis to calibrate for this part..." << totJointsToCalibrate << " bigger than " << n_joints;
582  return false;
583  }
584 
585  if (totJointsToCalibrate < n_joints)
586  {
587  yWarning() << deviceName << " is calibrating only a subset of the robot part. Calibrating " << totJointsToCalibrate << " over a total of " << n_joints;
588  }
589 
590 
591  if(skipCalibration)
592  yWarning() << deviceName << ": skipCalibration flag is on! Setting safe pid but skipping calibration.";
593 
594  Bit=joints.begin();
595  std::list<int>::iterator lit; //iterator for joint in a set
596  while( (Bit != Bend) && (!abortCalib) ) // for each set of joints
597  {
598 
599  setOfJoint_idx++;
600  currentSetList.clear();
601  currentSetList = (*Bit);
602 
603  // 1) set safe pid
604  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
605  {
606  if ( ((*lit) <0) || ((*lit) >= n_joints) ) // check the axes actually exists
607  {
608  yError() << deviceName << ": asked to calibrate joint" << (*lit) << ", which is negative OR bigger than the number of axes for this part ("<< n_joints << ")";
609  abortCalib = true;
610  break;
611  }
612 
613  if(!iAmp->getPWMLimit((*lit), &original_max_pwm[(*lit)]) )
614  {
615  yError() << deviceName << ": getPid joint " << (*lit) << "failed... aborting calibration";
616  abortCalib = true;
617  break;
618  }
619 
620  limited_max_pwm[(*lit)] = original_max_pwm[(*lit)];
621 
622  if (startupMaxPWM[(*lit)]==0)
623  {
624  yDebug() << deviceName << ": skipping startupMaxPWM=0 of joint " << (*lit);
625  iAmp->setPWMLimit((*lit),original_max_pwm[(*lit)]);
626  }
627  else
628  {
629  if (startupMaxPWM[(*lit)]<limited_max_pwm[(*lit)])
630  {
631  limited_max_pwm[(*lit)]=startupMaxPWM[(*lit)];
632  iAmp->setPWMLimit((*lit), limited_max_pwm[(*lit)]);
633  }
634  else
635  {
636  yDebug() << deviceName << ": joint " << (*lit) << " has max_output already limited to a safe value: " << limited_max_pwm[(*lit)];
637  }
638  }
639  }
640 
641  if(skipCalibration) // if this flag is on, fake calibration
642  {
643  Bit++;
644  continue;
645  }
646 
647  //2) if calibration needs to go to hardware limits, enable joint
648  //VALE: i can add this cycle for calib on eth because it does nothing,
649  // because enablePid doesn't send command because joints are not calibrated
650 
651  /*for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
652  {
653  if (type[*lit]==0 ||
654  type[*lit]==2 ||
655  type[*lit]==4 )
656  {
657  yDebug() << "In calibration " << deviceName << ": enabling joint " << *lit << " to test hardware limit";
658  iControlMode->setControlMode((*lit), VOCAB_CM_POSITION);
659  }
660  }*/
661 
662  Time::delay(0.1f);
663  if(abortCalib)
664  {
665  Bit++;
666  continue;
667  }
668 
669  //3) send calibration command
670  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
671  {
672  // Enable amp moved into EMS class;
673  // Here we just call the calibration procedure
674  calibrateJoint((*lit));
675  }
676 
677  Time::delay(0.1f);
678 
679  for(lit = currentSetList.begin(); lit != currentSetList.end(); lit++) //for each joint of set
680  {
681  iEncoders->getEncoder((*lit), &currPos[(*lit)]);
682  yDebug() << deviceName << ": set" << setOfJoint_idx << "j" << (*lit) << ": Calibrating... enc values AFTER calib: " << currPos[(*lit)];
683  }
684 
685  if(abortCalib)
686  {
687  Bit++;
688  continue;
689  }
690 
691  //4) check calibration result
692  if(checkCalibrateJointEnded((*Bit)) ) //check calibration on entire set
693  {
694  yDebug() << deviceName << ": set" << setOfJoint_idx << ": Calibration ended, going to zero!\n";
695  }
696  else // keep pid safe and go on
697  {
698  yError() << deviceName << ": set" << setOfJoint_idx << ": Calibration went wrong! Disabling axes and keeping safe pid limit\n";
699 
700  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
701  {
702  iControlMode->setControlMode((*lit),VOCAB_CM_IDLE);
703  }
704  Bit++;
705  continue; //go to next set
706  }
707 
708  // 5) if calibration finish with success enable disabled joints in order to move them to zero
709  /*for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
710  {
711  // if the joint han not been enabled at point 1, now i enable it
712  //iAmps->enableAmp((*lit));
713  if (type[*lit]!=0 &&
714  type[*lit]!=2 &&
715  type[*lit]!=4 )
716  {
717  iControlMode->setControlMode((*lit), VOCAB_CM_POSITION);
718  }
719  }*/
720 
721  if(abortCalib)
722  {
723  Bit++;
724  continue;
725  }
726  Time::delay(0.5f); // needed?
727 
728  //6) go to zero
729  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
730  {
731  // Manda in Zero
732  goToStartupPosition((*lit));
733  }
734 
735  if(abortCalib)
736  {
737  Bit++;
738  continue;
739  }
740  Time::delay(1.0); // needed?
741 
742  //7) check joints are in position
743  bool goneToZero = true;
744  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
745  {
746  goneToZero &= checkGoneToZeroThreshold(*lit);
747  }
748 
749  if(abortCalib)
750  {
751  Bit++;
752  continue;
753  }
754 
755  if(goneToZero)
756  {
757  yDebug() << deviceName << ": set" << setOfJoint_idx << ": Reached zero position!\n";
758  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
759  {
760  iAmp->setPWMLimit((*lit),original_max_pwm[(*lit)]);
761  }
762  }
763  else // keep pid safe and go on
764  {
765  yError() << deviceName << ": set" << setOfJoint_idx << ": some axis got timeout while reaching zero position... disabling this set of axes\n";
766  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
767  {
768  iControlMode->setControlMode((*lit),VOCAB_CM_IDLE);
769  }
770  }
771 
772  // Go to the next set of joints to calibrate... if any
773  Bit++;
774  }
775 
776  if(abortCalib)
777  {
778  yError() << deviceName << ": calibration has been aborted!I'm going to disable all joints..." ;
779  for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
780  {
781  iControlMode->setControlMode(*lit, VOCAB_CM_IDLE);
782  }
783  return false;
784  }
785  isCalibrated = true;
786  return isCalibrated;
787 }
788 
789 bool parametricCalibratorEth::calibrateJoint(int j)
790 {
791  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
792  {
793  yError("%s cannot perform 'calibration' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
794  return false;
795  }
796  yDebug() << deviceName << ": Calling calibrateJoint on joint " << j << ": type "<< calibParams[j].type << " with params: " << calibParams[j].param1 << calibParams[j].param2 << calibParams[j].param3 << calibParams[j].param4 << calibParams[j].param5;
797  bool b = iCalibrate->setCalibrationParameters(j, calibParams[j]);
798  return b;
799 }
800 
801 bool parametricCalibratorEth::checkCalibrateJointEnded(std::list<int> set)
802 {
803  bool calibration_ok = true;
804  int timeout = 0;
805 
806  std::list<int>::iterator lit;
807  std::list<int>::iterator lend;
808 
809  lit = set.begin();
810  lend = set.end();
811 
812  while (lit != lend)
813  {
814  if (abortCalib)
815  {
816  yWarning() << deviceName << ": calibration aborted\n";
817  }
818 
819  if (iCalibrate->calibrationDone(*lit))
820  {
821  yDebug() << deviceName << ": calib joint " << (*lit) << "ended";
822  lit++;
823  timeout = 0;
824  }
825  else
826  {
827  if (timeout > timeout_calibration[*lit])
828  {
829  yError() << deviceName << ": Timeout while calibrating " << (*lit);
830  calibration_ok = false;
831  lit++;
832  timeout = 0;
833  }
834 
835  yarp::os::Time::delay(1.0);
836  timeout++;
837  }
838  }
839 
840  return calibration_ok;
841 }
842 
843 bool parametricCalibratorEth::checkHwFault(int j)
844 {
845  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
846  {
847  yError("%s cannot perform 'check hardware fault' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
848  return false;
849  }
850 
851  int mode=0;
852  iControlMode->getControlMode(j,&mode);
853  if (mode == VOCAB_CM_HW_FAULT)
854  {
855  if (clearHwFault)
856  {
857  iControlMode->setControlMode(j,VOCAB_CM_FORCE_IDLE);
858  yWarning() << deviceName <<": detected an hardware fault on joint " << j << ". An attempt will be made to clear it.";
859  Time::delay(0.02f);
860  iControlMode->getControlMode(j,&mode);
861  if (mode == VOCAB_CM_HW_FAULT)
862  {
863  yError() << deviceName <<": unable to clear the hardware fault detected on joint " << j << " before starting the calibration procedure!";
864  return false;
865  }
866  else if (mode == VOCAB_CM_IDLE)
867  {
868  yWarning() << deviceName <<": hardware fault on joint " << j << " successfully cleared.";
869  return true;
870  }
871  else
872  {
873  yError() << deviceName <<": an unknown error occured while trying the hardware fault on joint " << j ;
874  return false;
875  }
876  }
877  else
878  {
879  yError() << deviceName <<": detected an hardware fault on joint " << j << " before starting the calibration procedure!";
880  return false;
881  }
882  }
883  return true;
884 }
885 
886 // this function may be updated in the future to use a startup sequence, like parking
887 bool parametricCalibratorEth::goToStartupPosition(int j)
888 {
889  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
890  {
891  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());
892  return false;
893  }
894 
895  bool ret = true;
896  if (disableStartupPosCheck[j])
897  {
898  yWarning() << deviceName << ": goToZero, joint " << j << " is disabled on user request";
899  return true;
900  }
901 
902  if (abortCalib) return true;
903  yDebug() << deviceName << ": Sending positionMove to joint" << j << " (desired pos: " << legacyStartupPosition.positions[j] << \
904  "desired speed: " << legacyStartupPosition.velocities[j] <<" )";
905  ret = iPosition->setRefSpeed(j, legacyStartupPosition.velocities[j]);
906  ret &= iPosition->positionMove(j, legacyStartupPosition.positions[j]);
907  return ret;
908 }
909 
910 bool parametricCalibratorEth::checkGoneToZeroThreshold(int j)
911 {
912  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
913  {
914  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());
915  return false;
916  }
917 
918  if (disableStartupPosCheck[j])
919  {
920  yWarning() << deviceName << ": checkGoneToZeroThreshold, joint " << j << " is disabled on user request";
921  return true;
922  }
923  if (skipCalibration) return false;
924 
925  // wait.
926  bool finished = false;
927 // double ang[4];
928  double angj = 0;
929  double output = 0;
930  double delta=0;
931  int mode=0;
932  bool done = false;
933 
934  double start_time = yarp::os::Time::now();
935  while ( (!finished) && (!abortCalib))
936  {
937  iEncoders->getEncoder(j, &angj);
938  iPosition->checkMotionDone(j, &done);
939  iControlMode->getControlMode(j, &mode);
940  iPids->getPidOutput(VOCAB_PIDTYPE_POSITION,j, &output);
941 
942  delta = fabs(angj-legacyStartupPosition.positions[j]);
943  yDebug("%s: checkGoneToZeroThreshold: joint: %d curr: %.3f des: %.3f -> delta: %.3f threshold: %.3f output: %.3f mode: %s" , \
944  deviceName.c_str(), j, angj, legacyStartupPosition.positions[j], delta, startupPosThreshold[j], output, yarp::os::Vocab32::decode(mode).c_str());
945 
946  if (delta < startupPosThreshold[j] && done)
947  {
948  yDebug("%s: checkGoneToZeroThreshold: joint: %d completed with delta: %.3f over: %.3f" ,deviceName.c_str(),j,delta, startupPosThreshold[j]);
949  finished=true;
950  break;
951  }
952  if (yarp::os::Time::now() - start_time > timeout_goToZero[j])
953  {
954  yError() << deviceName << ": checkGoneToZeroThreshold: joint " << j << " Timeout while going to zero!";
955  break;
956  }
957  if (mode == VOCAB_CM_IDLE)
958  {
959  yError() << deviceName << ": checkGoneToZeroThreshold: joint " << j << " is idle, skipping!";
960  break;
961  }
962  if (mode == VOCAB_CM_HW_FAULT)
963  {
964  yError() << deviceName <<": checkGoneToZeroThreshold: hardware fault on joint " << j << ", skipping!";
965  break;
966  }
967  if (abortCalib)
968  {
969  yWarning() << deviceName <<": checkGoneToZeroThreshold: joint " << j << " Aborting wait while going to zero!\n";
970  break;
971  }
972  Time::delay(0.5);
973  }
974  return finished;
975 }
976 
977 // called by robotinterface (during interrupt action??) // done
978 bool parametricCalibratorEth::park(DeviceDriver *dd, bool wait)
979 {
980  // parameter device driver is not used, because we already stored and got interfaces view
981  // when function 'calibration' was called.
982  yTrace();
983  std::list<int>::iterator joint;
984 
985  // legacy version: can be removed when legacy will not be supported anymore
986  if (useLegacyParking)
987  {
988  bool allJointsCanParkSimultaneously = true;
989  for (int i = 0; i < n_joints; i++)
990  {
991  if (disableHomeAndPark[i]) allJointsCanParkSimultaneously = false;
992  }
993 
994  if(allJointsCanParkSimultaneously == false)
995  {
996  yWarning() << deviceName << "Joints will be parked separately, since some of them have the disableHomeAndPark flag set";
997  bool ret = true;
998  for(joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++) //for each joint of set
999  {
1000  ret &= this->parkSingleJoint(*joint);
1001  }
1002  return ret;
1003  }
1004  }
1005  abortParking=false;
1006 
1007  if(!isCalibrated)
1008  {
1009  yWarning() << deviceName << ": Calling park without calibration... skipping";
1010  return true;
1011  }
1012 
1013  if(skipCalibration)
1014  {
1015  yWarning() << deviceName << ": skipCalibration flag is on!! Faking park!!";
1016  return true;
1017  }
1018 
1019  int * currentControlModes = new int[n_joints];
1020  std::vector<bool> cannotPark(n_joints);
1021  bool res = iControlMode->getControlModes(currentControlModes);
1022  if(!res)
1023  {
1024  yError() << deviceName << ": error getting control mode during parking";
1025  }
1026 
1027  for(joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++) //for each joint of set
1028  {
1029  switch(currentControlModes[(*joint)])
1030  {
1031  case VOCAB_CM_IDLE:
1032  {
1033  yError() << deviceName << ": joint " << (*joint) << " is idle, skipping park";
1034  cannotPark[(*joint)] = true;
1035  }
1036  break;
1037 
1038  case VOCAB_CM_HW_FAULT:
1039  {
1040  yError() << deviceName << ": joint " << (*joint) << " has an hardware fault, skipping park";
1041  cannotPark[(*joint)] = true;
1042  }
1043  break;
1044 
1045  case VOCAB_CM_NOT_CONFIGURED:
1046  {
1047  yError() << deviceName << ": joint " << (*joint) << " is not configured, skipping park";
1048  cannotPark[(*joint)] = true;
1049  }
1050  break;
1051 
1052  case VOCAB_CM_UNKNOWN:
1053  {
1054  yError() << deviceName << ": joint " << (*joint) << " is in unknown state, skipping park";
1055  cannotPark[(*joint)] = true;
1056  }
1057 
1058  default:
1059  {
1060  iControlMode->setControlMode((*joint), VOCAB_CM_POSITION);
1061  cannotPark[(*joint)] = false;
1062  }
1063  }
1064  }
1065 
1066  bool parkSequenceDone{false};
1067  if(useLegacyParking)
1068  {
1069  moveAndCheck_legacy(legacyParkingPosition, cannotPark, wait);
1070  // for legacy version, parkSequenceDone is always true, because there is no sequence
1071  parkSequenceDone = true;
1072  }
1073  else
1074  {
1075  // call one step of parking sequence for each 'park' call
1076 
1077  bool stepDone = true;
1078  if(parkingSequence.size() > 0)
1079  {
1080  stepDone = moveAndCheck(parkingSequence.at(currentParkingSeq_step));
1081  }
1082  if(stepDone)
1083  {
1084  yDebug() << "ParametricCalibratorEth: park sequence step num " << currentParkingSeq_step << " ended with " << \
1085  (abortParking ? "failure" : "success");
1086  }
1087  else
1088  abortParking = true;
1089 
1090  currentParkingSeq_step++;
1091  // parking sequence is completed if all steps are completed, or aborted if any one goes bad.
1092  // It is not safe to continue, if one parking step fails
1093  if( (currentParkingSeq_step >= parkingSequence.size()) || stepDone == false)
1094  parkSequenceDone = true;
1095  }
1096 
1097  // when parking is done (all steps of sequence where is the case), set all joints in idle
1098  if(parkSequenceDone)
1099  {
1100  yDebug() << deviceName.c_str() << ": Park " << (abortParking ? "aborted" : "completed");
1101  for(joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++) //for each joint of set
1102  {
1103  switch(currentControlModes[(*joint)])
1104  {
1105  case VOCAB_CM_IDLE:
1106  case VOCAB_CM_HW_FAULT:
1107  case VOCAB_CM_NOT_CONFIGURED:
1108  case VOCAB_CM_UNKNOWN:
1109  // Do nothing.
1110  break;
1111  default:
1112  {
1113  iControlMode->setControlMode((*joint), VOCAB_CM_IDLE);
1114  }
1115  }
1116  }
1117  }
1118  return true;
1119 }
1120 
1121 bool parametricCalibratorEth::moveAndCheck(PositionSequence &data)
1122 {
1123  iPosition->setRefSpeeds(data.velocities.data());
1124  iPosition->positionMove(data.positions.data());
1125 
1126  bool done = false;
1127  int timeout = 0;
1128  do
1129  {
1130  Time::delay(1);
1131  timeout++;
1132  iPosition->checkMotionDone(&done);
1133  }
1134  while((!done) && (timeout < PARK_TIMEOUT) && (!abortParking));
1135 
1136  if(!done)
1137  {
1138  yError() << "ParametricCalibratorEth: parking " << deviceName << " not in position after a timeout of" << PARK_TIMEOUT << " seconds";
1139  }
1140 
1141  return done;
1142 }
1143 
1144 bool parametricCalibratorEth::moveAndCheck_legacy(PositionSequence &data, std::vector<bool> &cannotPark, bool wait)
1145 {
1146  bool done=false;
1147  // send references to joints that need to be parked
1148  for(auto joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++) //for each joint of set
1149  {
1150  if (cannotPark[(*joint)] ==false)
1151  {
1152  iPosition->setRefSpeed((*joint), data.velocities[(*joint)]);
1153  iPosition->positionMove((*joint), data.positions[(*joint)]);
1154  }
1155  }
1156 
1157  // wait for the parking to be completed
1158  if (wait)
1159  {
1160  for(auto joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++) //for each joint of set
1161  {
1162  int timeout = 0;
1163  if (cannotPark[(*joint)] ==false)
1164  {
1165  yDebug() << deviceName.c_str() << ": Moving to park position, joint:" << (*joint);
1166  done=false;
1167  iPosition->checkMotionDone((*joint), &done);
1168  while ((!done) && (timeout < PARK_TIMEOUT) && (!abortParking))
1169  {
1170  Time::delay(1);
1171  timeout++;
1172  iPosition->checkMotionDone((*joint), &done);
1173  }
1174  if(!done)
1175  {
1176  yError() << deviceName << ": joint " << (*joint) << " not in position after a timeout of" << PARK_TIMEOUT << " seconds";
1177  }
1178  }
1179  }
1180  }
1181  return done;
1182 }
1183 
1185 {
1186  yDebug() << deviceName.c_str() << ": Quitting calibrate\n";
1187  abortCalib = true;
1188  return true;
1189 }
1190 
1192 {
1193  yDebug() << deviceName.c_str() << ": Quitting parking\n";
1194  abortParking=true;
1195  return true;
1196 }
1197 
1198 yarp::dev::IRemoteCalibrator *parametricCalibratorEth::getCalibratorDevice()
1199 {
1200  return this;
1201 }
1202 
1204 {
1205  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
1206  {
1207  yError("%s cannot perform 'calibration' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
1208  return false;
1209  }
1210 
1211  return calibrateJoint(j);
1212 }
1213 
1215 {
1216  yTrace();
1217  return calibrate();
1218 }
1219 
1221 {
1222  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
1223  {
1224  yError("%s cannot perform 'homing' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
1225  return false;
1226  }
1227 
1228  if (disableHomeAndPark[j])
1229  {
1230  yWarning() << deviceName << ": homingSingleJoint, joint " << j << " is disabled on user request";
1231  return true;
1232  }
1233  return goToStartupPosition(j);
1234 }
1235 
1237 {
1238  yTrace();
1239  bool ret = true;
1240  std::list<int>::iterator lit;
1241  for(lit = calibJoints.begin(); lit != calibJoints.end() && !abortCalib; lit++) //for each joint of set
1242  {
1243  ret = homingSingleJoint(*lit) && ret;
1244  }
1245  return ret;
1246 }
1247 
1249 {
1250  // check input joint number is valid
1251  if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
1252  {
1253  yError("%s cannot perform 'park' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
1254  return false;
1255  }
1256 
1257  if(useLegacyParking) // legacy version: can be removed when legacy will not be supported anymore
1258  {
1259  if (disableHomeAndPark[j])
1260  {
1261  yWarning() << deviceName << ": parkSingleJoint, joint " << j << " is disabled on user request";
1262  return true;
1263  }
1264  }
1265 
1266  abortParking=false;
1267 
1268  if(!isCalibrated)
1269  {
1270  yWarning() << deviceName << ": Calling park without calibration... skipping";
1271  return true;
1272  }
1273 
1274  if(skipCalibration)
1275  {
1276  yWarning() << deviceName << ": skipCalibration flag is on!! Faking park!!";
1277  return true;
1278  }
1279 
1280  int currentControlMode;
1281  bool cannotPark;
1282  bool res = iControlMode->getControlMode(j, &currentControlMode);
1283  if(!res)
1284  {
1285  yError() << deviceName << ": error getting control mode during parking";
1286  }
1287 
1288  if(currentControlMode != VOCAB_CM_IDLE &&
1289  currentControlMode != VOCAB_CM_HW_FAULT)
1290  {
1291  iControlMode->setControlMode(j, VOCAB_CM_POSITION);
1292  cannotPark = false;
1293  }
1294  else if (currentControlMode == VOCAB_CM_IDLE)
1295  {
1296  yError() << deviceName << ": joint " << j << " is idle, skipping park";
1297  cannotPark = true;
1298  }
1299  else if (currentControlMode == VOCAB_CM_HW_FAULT)
1300  {
1301  yError() << deviceName << ": joint " << j << " has an hardware fault, skipping park";
1302  cannotPark = true;
1303  }
1304 
1305  if(useLegacyParking) // legacy version: can be removed when legacy will not be supported anymore
1306  {
1307  iPosition->setRefSpeed (j, legacyStartupPosition.velocities[j]);
1308  iPosition->positionMove(j, legacyStartupPosition.positions[j]);
1309  }
1310  else
1311  {
1312  // Send the position and velocities of the last sequence step
1313  iPosition->setRefSpeed (j, parkingSequence.rbegin()->velocities[j]);
1314  iPosition->positionMove(j, parkingSequence.rbegin()->positions[j]);
1315  }
1316 
1317  if (_wait)
1318  {
1319  if (cannotPark == false)
1320  {
1321  int timeout = 0;
1322  yDebug() << deviceName.c_str() << ": Moving to park position, joint:" << j;
1323  bool done=false;
1324  iPosition->checkMotionDone(j, &done);
1325  while ((!done) && (timeout < PARK_TIMEOUT) && (!abortParking))
1326  {
1327  Time::delay(1);
1328  timeout++;
1329  iPosition->checkMotionDone(j, &done);
1330  }
1331  if(!done)
1332  {
1333  yError() << deviceName << ": joint " << j << " not in position after a timeout of" << PARK_TIMEOUT << " seconds";
1334  }
1335  }
1336  }
1337 
1338  yDebug() << deviceName.c_str() << ": Park " << (abortParking ? "aborted" : "completed");
1339  iControlMode->setControlMode(j,VOCAB_CM_IDLE);
1340  return true;
1341 }
1342 
1343 // called from motorgui or remote devices
1345 {
1346  yTrace();
1347 
1348  if(!isCalibrated)
1349  {
1350  yError() << "Device is not calibrated therefore cannot be parked";
1351  return false;
1352  }
1353 
1354  return park(dev2calibrate);
1355 }
1356 
1357 
1358 // eof
1359 
@ data
virtual bool park(DeviceDriver *dd, bool wait=true) override
virtual bool open(yarp::os::Searchable &config) override
Open the device driver.
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 yarp::dev::IRemoteCalibrator * getCalibratorDevice() override
bool calibrate(DeviceDriver *device) override
Calibrate method.
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
GLenum mode
Definition: rendering.cpp:48
out
Definition: sine.m:8