11 #include <yarp/os/Time.h>
12 #include <yarp/dev/PolyDriver.h>
18 #include <yarp/os/LogStream.h>
20 using namespace yarp::os;
28 static bool extractGroup(Bottle &input, Bottle &
out,
const std::string &key1,
const std::string &txt,
int size)
31 Bottle &
tmp=input.findGroup(key1.c_str(), txt.c_str());
34 yError () << key1.c_str() <<
" not found\n";
40 yError () << key1.c_str() <<
" incorrect number of entries in board.";
50 bool parametricCalibratorEth::parseSequenceGroup(yarp::os::Searchable &config, std::string sequence, std::vector<PositionSequence> &seqList)
52 Bottle parkSeq_group = config.findGroup(sequence);
53 if (parkSeq_group.isNull())
62 if(!
extractGroup(parkSeq_group, xtmp,
"numberOfSequences",
"number of sequences listed ", 1))
67 numOfSeq = xtmp.get(1).asInt32();
69 seqList.resize(numOfSeq);
73 yError() <<
"ParametricCalibratorEth " << deviceName <<
"<numberOfSequences> must be a positive integer";
78 for(
int seq_idx=0; seq_idx<numOfSeq; seq_idx++)
80 char sequence_name[80];
81 snprintf(sequence_name, 80,
"SEQUENCE_%d", seq_idx);
83 Bottle &seq_i = parkSeq_group.findGroup(sequence_name);
86 yError() <<
"ParametricCalibratorEth " << deviceName <<
"cannot find " << sequence_name;
91 seqList.at(seq_idx).seq_num = seq_idx;
94 Bottle & poss = seq_i.findGroup(
"position",
"desired parking position");
97 yError() <<
"ParametricCalibratorEth " << deviceName <<
": <position> parameter not found for sequence " << sequence_name;
102 Bottle &vels = seq_i.findGroup(
"velocity",
"desired parking velocities");
105 yError() <<
"ParametricCalibratorEth " << deviceName <<
": <velocity> parameter not found for sequence " << sequence_name;
109 if(( poss.size() -1 != n_joints) || (vels.size() -1 != n_joints))
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;
117 seqList[seq_idx].seq_num = seq_idx;
118 seqList[seq_idx].positions.reserve(n_joints);
119 seqList[seq_idx].velocities.reserve(n_joints);
121 for (
int j = 1; j <n_joints+1; j++)
123 seqList[seq_idx].positions .push_back(poss.get(j).asFloat64());
124 seqList[seq_idx].velocities.push_back(vels.get(j).asFloat64());
130 parametricCalibratorEth::parametricCalibratorEth() :
131 calibParams(nullptr),
132 original_max_pwm(nullptr),
133 limited_max_pwm(nullptr),
134 startupMaxPWM(nullptr),
138 startupPosThreshold(0),
141 skipCalibration(false),
144 timeout_goToZero(nullptr),
145 timeout_calibration(nullptr),
146 disableHomeAndPark(nullptr),
147 disableStartupPosCheck(nullptr),
148 totJointsToCalibrate(0),
149 useLegacyParking(true),
150 currentParkingSeq_step(0)
164 p.fromString(config.toString());
166 if (
p.check(
"GENERAL")==
false)
168 yError() <<
"Parametric calibrator: missing [GENERAL] section";
172 if(
p.findGroup(
"GENERAL").check(
"deviceName"))
174 deviceName =
p.findGroup(
"GENERAL").find(
"deviceName").asString();
178 yError() <<
"Parametric calibrator: missing deviceName parameter";
183 if(config.findGroup(
"GENERAL").find(
"verbose").asInt32())
185 str=config.toString().c_str();
186 yTrace() << deviceName.c_str() << str;
190 Value val_clearHwFault = config.findGroup(
"GENERAL").find(
"clearHwFaultBeforeCalibration");
191 if(val_clearHwFault.isNull())
193 clearHwFault =
false;
197 if(!val_clearHwFault.isBool())
199 yError() << deviceName.c_str() <<
": clearHwFaultBeforeCalibration bool param is different from accepted values (true / false). Assuming false";
200 clearHwFault =
false;
204 clearHwFault = val_clearHwFault.asBool();
206 yInfo() << deviceName.c_str() <<
": clearHwFaultBeforeCalibration option enabled\n";
211 Value use_raw = config.findGroup(
"GENERAL").find(
"useRawEncoderData");
212 bool useRawEncoderData;
216 useRawEncoderData =
false;
220 if(!use_raw.isBool())
222 yError() << deviceName.c_str() <<
": useRawEncoderData bool param is different from accepted values (true / false). Assuming false";
223 useRawEncoderData =
false;
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";
236 if(useRawEncoderData)
238 skipCalibration =
true;
243 Value checkSkipCalib = config.findGroup(
"GENERAL").find(
"skipCalibration");
244 if(checkSkipCalib.isNull())
246 skipCalibration =
false;
250 if(!checkSkipCalib.isBool())
252 yWarning() << deviceName <<
": skipCalibration bool param is different from accepted values (true / false). Assuming false";
253 skipCalibration =
false;
257 skipCalibration = checkSkipCalib.asBool();
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";
267 if(
p.findGroup(
"GENERAL").check(
"joints"))
269 n_joints =
p.findGroup(
"GENERAL").find(
"joints").asInt32();
271 else if(
p.findGroup(
"GENERAL").check(
"Joints"))
274 n_joints =
p.findGroup(
"GENERAL").find(
"Joints").asInt32();
278 yError() << deviceName.c_str() <<
": missing joints parameter" ;
282 calibParams =
new CalibrationParameters[n_joints];
283 startupMaxPWM =
new int[n_joints];
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);
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];
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;
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();
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();
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();
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();
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();
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();
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();
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();
342 xtmp =
p.findGroup(
"CALIBRATION").findGroup(
"calibrationTimeout");
343 if (xtmp.size() - 1 != n_joints) { }
344 else {
for (i = 1; i < xtmp.size(); i++) timeout_calibration[i - 1] = (
int)xtmp.get(i).asFloat64(); }
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();
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();
355 useLegacyParking = ! parseSequenceGroup(
p,
"PARKING_SEQUENCE", parkingSequence);
357 Bottle homeGroup =
p.findGroup(
"HOME");
360 if( homeGroup.isNull())
362 yError() <<
"Parking position not found. Either <HOME> or <PARKING_SEQUENCE> must be specified in config file";
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();
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();
385 xtmp = homeGroup.findGroup(
"disableHomeAndPark");
386 if (xtmp.size() - 1 != n_joints) { }
387 else {
for (i = 1; i < xtmp.size(); i++) disableHomeAndPark[i - 1] = xtmp.get(i).asInt32(); }
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();
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();
397 xtmp =
p.findGroup(
"CALIBRATION").findGroup(
"startupDisablePosCheck");
398 if (xtmp.size() - 1 != n_joints) { }
399 else {
for (i = 1; i < xtmp.size(); i++) disableStartupPosCheck[i - 1] = xtmp.get(i).asInt32(); }
401 xtmp =
p.findGroup(
"CALIBRATION").findGroup(
"startupTimeout");
402 if (xtmp.size() - 1 != n_joints) {}
403 else {
for (i = 1; i < xtmp.size(); i++) timeout_goToZero[i - 1] = xtmp.get(i).asFloat64(); }
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;}
411 for(
int i=1; i<xtmp.size(); i++)
415 set= xtmp.get(i).asList();
417 for(
int j=0; j<set->size(); j++)
419 tmp.push_back(set->get(j).asInt32() );
421 joints.push_back(
tmp);
429 if (calibParams != NULL) {
430 delete[] calibParams;
434 if (startupMaxPWM != NULL) {
435 delete[] startupMaxPWM;
436 startupMaxPWM = NULL;
438 if (original_max_pwm != NULL) {
439 delete[] original_max_pwm;
440 original_max_pwm = NULL;
442 if (limited_max_pwm != NULL) {
443 delete[] limited_max_pwm;
444 limited_max_pwm = NULL;
447 if (timeout_goToZero != NULL) {
448 delete[] timeout_goToZero;
449 timeout_goToZero = NULL;
452 if (timeout_calibration != NULL) {
453 delete[] timeout_calibration;
454 timeout_calibration = NULL;
457 if (currPos != NULL) {
461 if (currVel != NULL) {
466 if (disableHomeAndPark != NULL) {
467 delete[] disableHomeAndPark;
468 disableHomeAndPark = NULL;
471 if (disableStartupPosCheck != NULL) {
472 delete[] disableStartupPosCheck;
473 disableStartupPosCheck = NULL;
479 bool parametricCalibratorEth::calibrate(DeviceDriver *device)
481 yInfo() << deviceName <<
": starting calibration";
487 yError() << deviceName <<
": invalid device driver";
491 yarp::dev::PolyDriver *
p =
dynamic_cast<yarp::dev::PolyDriver *
>(device);
499 p->view(iControlMode);
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);
513 device->view(iControlMode);
517 if (!(iCalibrate && iEncoders && iPosition && iPids && iControlMode)) {
518 yError() << deviceName <<
": interface not found" << iCalibrate << iPosition << iPids << iControlMode;
525 bool parametricCalibratorEth::calibrate()
527 int setOfJoint_idx = 0;
528 totJointsToCalibrate = 0;
531 if (dev2calibrate==0)
533 yError() << deviceName <<
": not able to find a valid device to calibrate";
537 int n_joints_board{0};
538 if ( !iEncoders->getAxes(&n_joints_board))
540 yError() << deviceName <<
": error getting number of axes" ;
543 if(n_joints_board != n_joints)
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 <<
")";
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();
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();
565 calibJoints.push_back(*lit);
572 for (
int i=0; i<totJointsToCalibrate; i++)
577 yDebug() << deviceName <<
": Joints calibration order:" << calibJointsString.toString();
579 if (totJointsToCalibrate > n_joints)
581 yError() << deviceName <<
": too much axis to calibrate for this part..." << totJointsToCalibrate <<
" bigger than " << n_joints;
585 if (totJointsToCalibrate < n_joints)
587 yWarning() << deviceName <<
" is calibrating only a subset of the robot part. Calibrating " << totJointsToCalibrate <<
" over a total of " << n_joints;
592 yWarning() << deviceName <<
": skipCalibration flag is on! Setting safe pid but skipping calibration.";
595 std::list<int>::iterator lit;
596 while( (Bit != Bend) && (!abortCalib) )
600 currentSetList.clear();
601 currentSetList = (*Bit);
604 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++)
606 if ( ((*lit) <0) || ((*lit) >= n_joints) )
608 yError() << deviceName <<
": asked to calibrate joint" << (*lit) <<
", which is negative OR bigger than the number of axes for this part ("<< n_joints <<
")";
613 if(!iAmp->getPWMLimit((*lit), &original_max_pwm[(*lit)]) )
615 yError() << deviceName <<
": getPid joint " << (*lit) <<
"failed... aborting calibration";
620 limited_max_pwm[(*lit)] = original_max_pwm[(*lit)];
622 if (startupMaxPWM[(*lit)]==0)
624 yDebug() << deviceName <<
": skipping startupMaxPWM=0 of joint " << (*lit);
625 iAmp->setPWMLimit((*lit),original_max_pwm[(*lit)]);
629 if (startupMaxPWM[(*lit)]<limited_max_pwm[(*lit)])
631 limited_max_pwm[(*lit)]=startupMaxPWM[(*lit)];
632 iAmp->setPWMLimit((*lit), limited_max_pwm[(*lit)]);
636 yDebug() << deviceName <<
": joint " << (*lit) <<
" has max_output already limited to a safe value: " << limited_max_pwm[(*lit)];
670 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++)
674 calibrateJoint((*lit));
679 for(lit = currentSetList.begin(); lit != currentSetList.end(); lit++)
681 iEncoders->getEncoder((*lit), &currPos[(*lit)]);
682 yDebug() << deviceName <<
": set" << setOfJoint_idx <<
"j" << (*lit) <<
": Calibrating... enc values AFTER calib: " << currPos[(*lit)];
692 if(checkCalibrateJointEnded((*Bit)) )
694 yDebug() << deviceName <<
": set" << setOfJoint_idx <<
": Calibration ended, going to zero!\n";
698 yError() << deviceName <<
": set" << setOfJoint_idx <<
": Calibration went wrong! Disabling axes and keeping safe pid limit\n";
700 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++)
702 iControlMode->setControlMode((*lit),VOCAB_CM_IDLE);
729 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++)
732 goToStartupPosition((*lit));
743 bool goneToZero =
true;
744 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++)
746 goneToZero &= checkGoneToZeroThreshold(*lit);
757 yDebug() << deviceName <<
": set" << setOfJoint_idx <<
": Reached zero position!\n";
758 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++)
760 iAmp->setPWMLimit((*lit),original_max_pwm[(*lit)]);
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++)
768 iControlMode->setControlMode((*lit),VOCAB_CM_IDLE);
778 yError() << deviceName <<
": calibration has been aborted!I'm going to disable all joints..." ;
779 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++)
781 iControlMode->setControlMode(*lit, VOCAB_CM_IDLE);
789 bool parametricCalibratorEth::calibrateJoint(
int j)
791 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
793 yError(
"%s cannot perform 'calibration' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
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]);
801 bool parametricCalibratorEth::checkCalibrateJointEnded(std::list<int> set)
803 bool calibration_ok =
true;
806 std::list<int>::iterator lit;
807 std::list<int>::iterator lend;
816 yWarning() << deviceName <<
": calibration aborted\n";
819 if (iCalibrate->calibrationDone(*lit))
821 yDebug() << deviceName <<
": calib joint " << (*lit) <<
"ended";
827 if (timeout > timeout_calibration[*lit])
829 yError() << deviceName <<
": Timeout while calibrating " << (*lit);
830 calibration_ok =
false;
835 yarp::os::Time::delay(1.0);
840 return calibration_ok;
843 bool parametricCalibratorEth::checkHwFault(
int j)
845 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
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());
852 iControlMode->getControlMode(j,&
mode);
853 if (
mode == VOCAB_CM_HW_FAULT)
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.";
860 iControlMode->getControlMode(j,&
mode);
861 if (
mode == VOCAB_CM_HW_FAULT)
863 yError() << deviceName <<
": unable to clear the hardware fault detected on joint " << j <<
" before starting the calibration procedure!";
866 else if (
mode == VOCAB_CM_IDLE)
868 yWarning() << deviceName <<
": hardware fault on joint " << j <<
" successfully cleared.";
873 yError() << deviceName <<
": an unknown error occured while trying the hardware fault on joint " << j ;
879 yError() << deviceName <<
": detected an hardware fault on joint " << j <<
" before starting the calibration procedure!";
887 bool parametricCalibratorEth::goToStartupPosition(
int j)
889 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
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());
896 if (disableStartupPosCheck[j])
898 yWarning() << deviceName <<
": goToZero, joint " << j <<
" is disabled on user request";
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]);
910 bool parametricCalibratorEth::checkGoneToZeroThreshold(
int j)
912 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
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());
918 if (disableStartupPosCheck[j])
920 yWarning() << deviceName <<
": checkGoneToZeroThreshold, joint " << j <<
" is disabled on user request";
923 if (skipCalibration)
return false;
926 bool finished =
false;
934 double start_time = yarp::os::Time::now();
935 while ( (!finished) && (!abortCalib))
937 iEncoders->getEncoder(j, &angj);
938 iPosition->checkMotionDone(j, &
done);
939 iControlMode->getControlMode(j, &
mode);
940 iPids->getPidOutput(VOCAB_PIDTYPE_POSITION,j, &output);
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());
946 if (delta < startupPosThreshold[j] &&
done)
948 yDebug(
"%s: checkGoneToZeroThreshold: joint: %d completed with delta: %.3f over: %.3f" ,deviceName.c_str(),j,delta, startupPosThreshold[j]);
952 if (yarp::os::Time::now() - start_time > timeout_goToZero[j])
954 yError() << deviceName <<
": checkGoneToZeroThreshold: joint " << j <<
" Timeout while going to zero!";
957 if (
mode == VOCAB_CM_IDLE)
959 yError() << deviceName <<
": checkGoneToZeroThreshold: joint " << j <<
" is idle, skipping!";
962 if (
mode == VOCAB_CM_HW_FAULT)
964 yError() << deviceName <<
": checkGoneToZeroThreshold: hardware fault on joint " << j <<
", skipping!";
969 yWarning() << deviceName <<
": checkGoneToZeroThreshold: joint " << j <<
" Aborting wait while going to zero!\n";
983 std::list<int>::iterator joint;
986 if (useLegacyParking)
988 bool allJointsCanParkSimultaneously =
true;
989 for (
int i = 0; i < n_joints; i++)
991 if (disableHomeAndPark[i]) allJointsCanParkSimultaneously =
false;
994 if(allJointsCanParkSimultaneously ==
false)
996 yWarning() << deviceName <<
"Joints will be parked separately, since some of them have the disableHomeAndPark flag set";
998 for(joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++)
1009 yWarning() << deviceName <<
": Calling park without calibration... skipping";
1015 yWarning() << deviceName <<
": skipCalibration flag is on!! Faking park!!";
1019 int * currentControlModes =
new int[n_joints];
1020 std::vector<bool> cannotPark(n_joints);
1021 bool res = iControlMode->getControlModes(currentControlModes);
1024 yError() << deviceName <<
": error getting control mode during parking";
1027 for(joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++)
1029 switch(currentControlModes[(*joint)])
1033 yError() << deviceName <<
": joint " << (*joint) <<
" is idle, skipping park";
1034 cannotPark[(*joint)] =
true;
1038 case VOCAB_CM_HW_FAULT:
1040 yError() << deviceName <<
": joint " << (*joint) <<
" has an hardware fault, skipping park";
1041 cannotPark[(*joint)] =
true;
1045 case VOCAB_CM_NOT_CONFIGURED:
1047 yError() << deviceName <<
": joint " << (*joint) <<
" is not configured, skipping park";
1048 cannotPark[(*joint)] =
true;
1052 case VOCAB_CM_UNKNOWN:
1054 yError() << deviceName <<
": joint " << (*joint) <<
" is in unknown state, skipping park";
1055 cannotPark[(*joint)] =
true;
1060 iControlMode->setControlMode((*joint), VOCAB_CM_POSITION);
1061 cannotPark[(*joint)] =
false;
1066 bool parkSequenceDone{
false};
1067 if(useLegacyParking)
1069 moveAndCheck_legacy(legacyParkingPosition, cannotPark, wait);
1071 parkSequenceDone =
true;
1077 bool stepDone =
true;
1078 if(parkingSequence.size() > 0)
1080 stepDone = moveAndCheck(parkingSequence.at(currentParkingSeq_step));
1084 yDebug() <<
"ParametricCalibratorEth: park sequence step num " << currentParkingSeq_step <<
" ended with " << \
1085 (abortParking ?
"failure" :
"success");
1088 abortParking =
true;
1090 currentParkingSeq_step++;
1093 if( (currentParkingSeq_step >= parkingSequence.size()) || stepDone ==
false)
1094 parkSequenceDone =
true;
1098 if(parkSequenceDone)
1100 yDebug() << deviceName.c_str() <<
": Park " << (abortParking ?
"aborted" :
"completed");
1101 for(joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++)
1103 switch(currentControlModes[(*joint)])
1106 case VOCAB_CM_HW_FAULT:
1107 case VOCAB_CM_NOT_CONFIGURED:
1108 case VOCAB_CM_UNKNOWN:
1113 iControlMode->setControlMode((*joint), VOCAB_CM_IDLE);
1121 bool parametricCalibratorEth::moveAndCheck(PositionSequence &
data)
1123 iPosition->setRefSpeeds(
data.velocities.data());
1124 iPosition->positionMove(
data.positions.data());
1132 iPosition->checkMotionDone(&
done);
1138 yError() <<
"ParametricCalibratorEth: parking " << deviceName <<
" not in position after a timeout of" <<
PARK_TIMEOUT <<
" seconds";
1144 bool parametricCalibratorEth::moveAndCheck_legacy(PositionSequence &
data, std::vector<bool> &cannotPark,
bool wait)
1148 for(
auto joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++)
1150 if (cannotPark[(*joint)] ==
false)
1152 iPosition->setRefSpeed((*joint),
data.velocities[(*joint)]);
1153 iPosition->positionMove((*joint),
data.positions[(*joint)]);
1160 for(
auto joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++)
1163 if (cannotPark[(*joint)] ==
false)
1165 yDebug() << deviceName.c_str() <<
": Moving to park position, joint:" << (*joint);
1167 iPosition->checkMotionDone((*joint), &
done);
1172 iPosition->checkMotionDone((*joint), &
done);
1176 yError() << deviceName <<
": joint " << (*joint) <<
" not in position after a timeout of" <<
PARK_TIMEOUT <<
" seconds";
1186 yDebug() << deviceName.c_str() <<
": Quitting calibrate\n";
1193 yDebug() << deviceName.c_str() <<
": Quitting parking\n";
1205 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
1207 yError(
"%s cannot perform 'calibration' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
1211 return calibrateJoint(j);
1222 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
1224 yError(
"%s cannot perform 'homing' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
1228 if (disableHomeAndPark[j])
1230 yWarning() << deviceName <<
": homingSingleJoint, joint " << j <<
" is disabled on user request";
1233 return goToStartupPosition(j);
1240 std::list<int>::iterator lit;
1241 for(lit = calibJoints.begin(); lit != calibJoints.end() && !abortCalib; lit++)
1251 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
1253 yError(
"%s cannot perform 'park' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
1257 if(useLegacyParking)
1259 if (disableHomeAndPark[j])
1261 yWarning() << deviceName <<
": parkSingleJoint, joint " << j <<
" is disabled on user request";
1270 yWarning() << deviceName <<
": Calling park without calibration... skipping";
1276 yWarning() << deviceName <<
": skipCalibration flag is on!! Faking park!!";
1280 int currentControlMode;
1282 bool res = iControlMode->getControlMode(j, ¤tControlMode);
1285 yError() << deviceName <<
": error getting control mode during parking";
1288 if(currentControlMode != VOCAB_CM_IDLE &&
1289 currentControlMode != VOCAB_CM_HW_FAULT)
1291 iControlMode->setControlMode(j, VOCAB_CM_POSITION);
1294 else if (currentControlMode == VOCAB_CM_IDLE)
1296 yError() << deviceName <<
": joint " << j <<
" is idle, skipping park";
1299 else if (currentControlMode == VOCAB_CM_HW_FAULT)
1301 yError() << deviceName <<
": joint " << j <<
" has an hardware fault, skipping park";
1305 if(useLegacyParking)
1307 iPosition->setRefSpeed (j, legacyStartupPosition.velocities[j]);
1308 iPosition->positionMove(j, legacyStartupPosition.positions[j]);
1313 iPosition->setRefSpeed (j, parkingSequence.rbegin()->velocities[j]);
1314 iPosition->positionMove(j, parkingSequence.rbegin()->positions[j]);
1319 if (cannotPark ==
false)
1322 yDebug() << deviceName.c_str() <<
": Moving to park position, joint:" << j;
1324 iPosition->checkMotionDone(j, &
done);
1329 iPosition->checkMotionDone(j, &
done);
1333 yError() << deviceName <<
": joint " << j <<
" not in position after a timeout of" <<
PARK_TIMEOUT <<
" seconds";
1338 yDebug() << deviceName.c_str() <<
": Park " << (abortParking ?
"aborted" :
"completed");
1339 iControlMode->setControlMode(j,VOCAB_CM_IDLE);
1350 yError() <<
"Device is not calibrated therefore cannot be parked";
1354 return park(dev2calibrate);
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 parkWholePart() override
virtual bool calibrateSingleJoint(int j) override
virtual bool parkSingleJoint(int j, bool _wait=true) override
virtual bool quitPark() override
virtual bool close() override
Close the device driver.
~parametricCalibratorEth()
Destructor.
virtual bool calibrateWholePart() override
virtual yarp::dev::IRemoteCalibrator * getCalibratorDevice() override
virtual bool quitCalibrate() override
bool calibrate(DeviceDriver *device) override
Calibrate method.
virtual bool homingWholePart() override
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)