iCub-main
Loading...
Searching...
No Matches
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
20using namespace yarp::os;
21using namespace yarp::dev;
22
23const int PARK_TIMEOUT = 30;
24const double GO_TO_ZERO_TIMEOUT = 10;
26
27//#warning "Use extractGroup to verify size of parameters matches with number of joints, this will avoid crashes"
28static 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
50bool 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
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
159
160bool 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
479bool 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
525bool 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 calibJointsString.addInt32(*lit);
567 lit++;
568 }
569 Bit++;
570 }
571
572 yDebug() << deviceName << ": Joints calibration order:" << calibJointsString.toString();
573
574 if (totJointsToCalibrate > n_joints)
575 {
576 yError() << deviceName << ": too much axis to calibrate for this part..." << totJointsToCalibrate << " bigger than " << n_joints;
577 return false;
578 }
579
580 //before starting the calibration, checks for joints in hardware fault, and clears them if the user set the clearHwFaultBeforeCalibration option
581 if(!checkHwFault())
582 return false;
583
584 if (totJointsToCalibrate < n_joints)
585 {
586 yWarning() << deviceName << " is calibrating only a subset of the robot part. Calibrating " << totJointsToCalibrate << " over a total of " << n_joints;
587 }
588
589
590 if(skipCalibration)
591 yWarning() << deviceName << ": skipCalibration flag is on! Setting safe pid but skipping calibration.";
592
593 Bit=joints.begin();
594 std::list<int>::iterator lit; //iterator for joint in a set
595 while( (Bit != Bend) && (!abortCalib) ) // for each set of joints
596 {
597
598 setOfJoint_idx++;
599 currentSetList.clear();
600 currentSetList = (*Bit);
601
602 // 1) set safe pid
603 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
604 {
605 if ( ((*lit) <0) || ((*lit) >= n_joints) ) // check the axes actually exists
606 {
607 yError() << deviceName << ": asked to calibrate joint" << (*lit) << ", which is negative OR bigger than the number of axes for this part ("<< n_joints << ")";
608 abortCalib = true;
609 break;
610 }
611
612 if(!iAmp->getPWMLimit((*lit), &original_max_pwm[(*lit)]) )
613 {
614 yError() << deviceName << ": getPid joint " << (*lit) << "failed... aborting calibration";
615 abortCalib = true;
616 break;
617 }
618
619 limited_max_pwm[(*lit)] = original_max_pwm[(*lit)];
620
621 if (startupMaxPWM[(*lit)]==0)
622 {
623 yDebug() << deviceName << ": skipping startupMaxPWM=0 of joint " << (*lit);
624 iAmp->setPWMLimit((*lit),original_max_pwm[(*lit)]);
625 }
626 else
627 {
628 if (startupMaxPWM[(*lit)]<limited_max_pwm[(*lit)])
629 {
630 limited_max_pwm[(*lit)]=startupMaxPWM[(*lit)];
631 iAmp->setPWMLimit((*lit), limited_max_pwm[(*lit)]);
632 }
633 else
634 {
635 yDebug() << deviceName << ": joint " << (*lit) << " has max_output already limited to a safe value: " << limited_max_pwm[(*lit)];
636 }
637 }
638 }
639
640 if(skipCalibration) // if this flag is on, fake calibration
641 {
642 Bit++;
643 continue;
644 }
645
646 //2) if calibration needs to go to hardware limits, enable joint
647 //VALE: i can add this cycle for calib on eth because it does nothing,
648 // because enablePid doesn't send command because joints are not calibrated
649
650 /*for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
651 {
652 if (type[*lit]==0 ||
653 type[*lit]==2 ||
654 type[*lit]==4 )
655 {
656 yDebug() << "In calibration " << deviceName << ": enabling joint " << *lit << " to test hardware limit";
657 iControlMode->setControlMode((*lit), VOCAB_CM_POSITION);
658 }
659 }*/
660
661 Time::delay(0.1f);
662 if(abortCalib)
663 {
664 Bit++;
665 continue;
666 }
667
668 //3) send calibration command
669 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
670 {
671 // Enable amp moved into EMS class;
672 // Here we just call the calibration procedure
673 calibrateJoint((*lit));
674 }
675
676 Time::delay(0.1f);
677
678 for(lit = currentSetList.begin(); lit != currentSetList.end(); lit++) //for each joint of set
679 {
680 iEncoders->getEncoder((*lit), &currPos[(*lit)]);
681 yDebug() << deviceName << ": set" << setOfJoint_idx << "j" << (*lit) << ": Calibrating... enc values AFTER calib: " << currPos[(*lit)];
682 }
683
684 if(abortCalib)
685 {
686 Bit++;
687 continue;
688 }
689
690 //4) check calibration result
691 if(checkCalibrateJointEnded((*Bit)) ) //check calibration on entire set
692 {
693 yDebug() << deviceName << ": set" << setOfJoint_idx << ": Calibration ended, going to zero!\n";
694 }
695 else // keep pid safe and go on
696 {
697 yError() << deviceName << ": set" << setOfJoint_idx << ": Calibration went wrong! Disabling axes and keeping safe pid limit\n";
698
699 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
700 {
701 iControlMode->setControlMode((*lit),VOCAB_CM_IDLE);
702 }
703 Bit++;
704 continue; //go to next set
705 }
706
707 // 5) if calibration finish with success enable disabled joints in order to move them to zero
708 /*for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
709 {
710 // if the joint han not been enabled at point 1, now i enable it
711 //iAmps->enableAmp((*lit));
712 if (type[*lit]!=0 &&
713 type[*lit]!=2 &&
714 type[*lit]!=4 )
715 {
716 iControlMode->setControlMode((*lit), VOCAB_CM_POSITION);
717 }
718 }*/
719
720 if(abortCalib)
721 {
722 Bit++;
723 continue;
724 }
725 Time::delay(0.5f); // needed?
726
727 //6) go to zero
728 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
729 {
730 // Manda in Zero
731 goToStartupPosition((*lit));
732 }
733
734 if(abortCalib)
735 {
736 Bit++;
737 continue;
738 }
739 Time::delay(1.0); // needed?
740
741 //7) check joints are in position
742 bool goneToZero = true;
743 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
744 {
745 goneToZero &= checkGoneToZeroThreshold(*lit);
746 }
747
748 if(abortCalib)
749 {
750 Bit++;
751 continue;
752 }
753
754 if(goneToZero)
755 {
756 yDebug() << deviceName << ": set" << setOfJoint_idx << ": Reached zero position!\n";
757 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
758 {
759 iAmp->setPWMLimit((*lit),original_max_pwm[(*lit)]);
760 }
761 }
762 else // keep pid safe and go on
763 {
764 yError() << deviceName << ": set" << setOfJoint_idx << ": some axis got timeout while reaching zero position... disabling this set of axes\n";
765 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
766 {
767 iControlMode->setControlMode((*lit),VOCAB_CM_IDLE);
768 }
769 }
770
771 // Go to the next set of joints to calibrate... if any
772 Bit++;
773 }
774
775 if(abortCalib)
776 {
777 yError() << deviceName << ": calibration has been aborted!I'm going to disable all joints..." ;
778 for(lit = currentSetList.begin(); lit != currentSetList.end() && !abortCalib; lit++) //for each joint of set
779 {
780 iControlMode->setControlMode(*lit, VOCAB_CM_IDLE);
781 }
782 return false;
783 }
784 isCalibrated = true;
785 return isCalibrated;
786}
787
788bool parametricCalibratorEth::calibrateJoint(int j)
789{
790 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
791 {
792 yError("%s cannot perform 'calibration' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
793 return false;
794 }
795 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;
796 bool b = iCalibrate->setCalibrationParameters(j, calibParams[j]);
797 return b;
798}
799
800bool parametricCalibratorEth::checkCalibrateJointEnded(std::list<int> set)
801{
802 bool calibration_ok = true;
803 int timeout = 0;
804
805 std::list<int>::iterator lit;
806 std::list<int>::iterator lend;
807
808 lit = set.begin();
809 lend = set.end();
810
811 while (lit != lend)
812 {
813 if (abortCalib)
814 {
815 yWarning() << deviceName << ": calibration aborted\n";
816 }
817
818 if (iCalibrate->calibrationDone(*lit))
819 {
820 yDebug() << deviceName << ": calib joint " << (*lit) << "ended";
821 lit++;
822 timeout = 0;
823 }
824 else
825 {
826 if (timeout > timeout_calibration[*lit])
827 {
828 yError() << deviceName << ": Timeout while calibrating joint" << (*lit);
829 calibration_ok = false;
830 lit++;
831 timeout = 0;
832 }
833
834 yarp::os::Time::delay(1.0);
835 timeout++;
836 }
837 }
838
839 return calibration_ok;
840}
841
842bool parametricCalibratorEth::checkHwFault()
843{
844 for(auto j : calibJoints)
845 {
846 int mode=0;
847 iControlMode->getControlMode(j,&mode);
848 if (mode == VOCAB_CM_HW_FAULT)
849 {
850 if (clearHwFault)
851 {
852 iControlMode->setControlMode(j,VOCAB_CM_FORCE_IDLE);
853 yWarning() << deviceName <<": detected an hardware fault on joint " << j << ". An attempt will be made to clear it.";
854 Time::delay(0.02f);
855 iControlMode->getControlMode(j,&mode);
856 if (mode == VOCAB_CM_HW_FAULT)
857 {
858 yError() << deviceName <<": unable to clear the hardware fault detected on joint " << j << " before starting the calibration procedure!";
859 return false;
860 }
861 else if (mode == VOCAB_CM_IDLE)
862 {
863 yWarning() << deviceName <<": hardware fault on joint " << j << " successfully cleared.";
864 return true;
865 }
866 else
867 {
868 yError() << deviceName <<": an unknown error occured while trying the hardware fault on joint " << j ;
869 return false;
870 }
871 }
872 else
873 {
874 yError() << deviceName <<": detected an hardware fault on joint " << j << " before starting the calibration procedure!";
875 return false;
876 }
877 }
878 }
879 return true;
880}
881
882// this function may be updated in the future to use a startup sequence, like parking
883bool parametricCalibratorEth::goToStartupPosition(int j)
884{
885 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
886 {
887 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());
888 return false;
889 }
890
891 bool ret = true;
892 if (disableStartupPosCheck[j])
893 {
894 yWarning() << deviceName << ": goToZero, joint " << j << " is disabled on user request";
895 return true;
896 }
897
898 if (abortCalib) return true;
899 yDebug() << deviceName << ": Sending positionMove to joint" << j << " (desired pos: " << legacyStartupPosition.positions[j] << \
900 "desired speed: " << legacyStartupPosition.velocities[j] <<" )";
901 ret = iPosition->setRefSpeed(j, legacyStartupPosition.velocities[j]);
902 ret &= iPosition->positionMove(j, legacyStartupPosition.positions[j]);
903 return ret;
904}
905
906bool parametricCalibratorEth::checkGoneToZeroThreshold(int j)
907{
908 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
909 {
910 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());
911 return false;
912 }
913
914 if (disableStartupPosCheck[j])
915 {
916 yWarning() << deviceName << ": checkGoneToZeroThreshold, joint " << j << " is disabled on user request";
917 return true;
918 }
919 if (skipCalibration) return false;
920
921 // wait.
922 bool finished = false;
923// double ang[4];
924 double angj = 0;
925 double output = 0;
926 double delta=0;
927 int mode=0;
928 bool done = false;
929
930 double start_time = yarp::os::Time::now();
931 while ( (!finished) && (!abortCalib))
932 {
933 iEncoders->getEncoder(j, &angj);
934 iPosition->checkMotionDone(j, &done);
935 iControlMode->getControlMode(j, &mode);
936 iPids->getPidOutput(VOCAB_PIDTYPE_POSITION,j, &output);
937
938 delta = fabs(angj-legacyStartupPosition.positions[j]);
939 yDebug("%s: checkGoneToZeroThreshold: joint: %d curr: %.3f des: %.3f -> delta: %.3f threshold: %.3f output: %.3f mode: %s" , \
940 deviceName.c_str(), j, angj, legacyStartupPosition.positions[j], delta, startupPosThreshold[j], output, yarp::os::Vocab32::decode(mode).c_str());
941
942 if (delta < startupPosThreshold[j] && done)
943 {
944 yDebug("%s: checkGoneToZeroThreshold: joint: %d completed with delta: %.3f over: %.3f" ,deviceName.c_str(),j,delta, startupPosThreshold[j]);
945 finished=true;
946 break;
947 }
948 if (yarp::os::Time::now() - start_time > timeout_goToZero[j])
949 {
950 yError() << deviceName << ": checkGoneToZeroThreshold: joint " << j << " Timeout while going to zero!";
951 break;
952 }
953 if (mode == VOCAB_CM_IDLE)
954 {
955 yError() << deviceName << ": checkGoneToZeroThreshold: joint " << j << " is idle, skipping!";
956 break;
957 }
958 if (mode == VOCAB_CM_HW_FAULT)
959 {
960 yError() << deviceName <<": checkGoneToZeroThreshold: hardware fault on joint " << j << ", skipping!";
961 break;
962 }
963 if (abortCalib)
964 {
965 yWarning() << deviceName <<": checkGoneToZeroThreshold: joint " << j << " Aborting wait while going to zero!\n";
966 break;
967 }
968 Time::delay(0.5);
969 }
970 return finished;
971}
972
973// called by robotinterface (during interrupt action??) // done
974bool parametricCalibratorEth::park(DeviceDriver *dd, bool wait)
975{
976 // parameter device driver is not used, because we already stored and got interfaces view
977 // when function 'calibration' was called.
978 yTrace();
979 std::list<int>::iterator joint;
980
981 // legacy version: can be removed when legacy will not be supported anymore
982 if (useLegacyParking)
983 {
984 bool allJointsCanParkSimultaneously = true;
985 for (int i = 0; i < n_joints; i++)
986 {
987 if (disableHomeAndPark[i]) allJointsCanParkSimultaneously = false;
988 }
989
990 if(allJointsCanParkSimultaneously == false)
991 {
992 yWarning() << deviceName << "Joints will be parked separately, since some of them have the disableHomeAndPark flag set";
993 bool ret = true;
994 for(joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++) //for each joint of set
995 {
996 ret &= this->parkSingleJoint(*joint);
997 }
998 return ret;
999 }
1000 }
1001 abortParking=false;
1002
1003 if(!isCalibrated)
1004 {
1005 yWarning() << deviceName << ": Calling park without calibration... skipping";
1006 return true;
1007 }
1008
1009 if(skipCalibration)
1010 {
1011 yWarning() << deviceName << ": skipCalibration flag is on!! Faking park!!";
1012 return true;
1013 }
1014
1015 int * currentControlModes = new int[n_joints];
1016 std::vector<bool> cannotPark(n_joints);
1017 bool res = iControlMode->getControlModes(currentControlModes);
1018 if(!res)
1019 {
1020 yError() << deviceName << ": error getting control mode during parking";
1021 }
1022
1023 for(joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++) //for each joint of set
1024 {
1025 switch(currentControlModes[(*joint)])
1026 {
1027 case VOCAB_CM_IDLE:
1028 {
1029 yError() << deviceName << ": joint " << (*joint) << " is idle, skipping park";
1030 cannotPark[(*joint)] = true;
1031 }
1032 break;
1033
1034 case VOCAB_CM_HW_FAULT:
1035 {
1036 yError() << deviceName << ": joint " << (*joint) << " has an hardware fault, skipping park";
1037 cannotPark[(*joint)] = true;
1038 }
1039 break;
1040
1041 case VOCAB_CM_NOT_CONFIGURED:
1042 {
1043 yError() << deviceName << ": joint " << (*joint) << " is not configured, skipping park";
1044 cannotPark[(*joint)] = true;
1045 }
1046 break;
1047
1048 case VOCAB_CM_UNKNOWN:
1049 {
1050 yError() << deviceName << ": joint " << (*joint) << " is in unknown state, skipping park";
1051 cannotPark[(*joint)] = true;
1052 }
1053
1054 default:
1055 {
1056 iControlMode->setControlMode((*joint), VOCAB_CM_POSITION);
1057 cannotPark[(*joint)] = false;
1058 }
1059 }
1060 }
1061
1062 bool parkSequenceDone{false};
1063 if(useLegacyParking)
1064 {
1065 moveAndCheck_legacy(legacyParkingPosition, cannotPark, wait);
1066 // for legacy version, parkSequenceDone is always true, because there is no sequence
1067 parkSequenceDone = true;
1068 }
1069 else
1070 {
1071 // call one step of parking sequence for each 'park' call
1072
1073 bool stepDone = true;
1074 if(parkingSequence.size() > 0)
1075 {
1076 stepDone = moveAndCheck(parkingSequence.at(currentParkingSeq_step));
1077 }
1078 if(stepDone)
1079 {
1080 yDebug() << "ParametricCalibratorEth: park sequence step num " << currentParkingSeq_step << " ended with " << \
1081 (abortParking ? "failure" : "success");
1082 }
1083 else
1084 abortParking = true;
1085
1086 currentParkingSeq_step++;
1087 // parking sequence is completed if all steps are completed, or aborted if any one goes bad.
1088 // It is not safe to continue, if one parking step fails
1089 if( (currentParkingSeq_step >= parkingSequence.size()) || stepDone == false)
1090 parkSequenceDone = true;
1091 }
1092
1093 // when parking is done (all steps of sequence where is the case), set all joints in idle
1094 if(parkSequenceDone)
1095 {
1096 yDebug() << deviceName.c_str() << ": Park " << (abortParking ? "aborted" : "completed");
1097 for(joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++) //for each joint of set
1098 {
1099 switch(currentControlModes[(*joint)])
1100 {
1101 case VOCAB_CM_IDLE:
1102 case VOCAB_CM_HW_FAULT:
1103 case VOCAB_CM_NOT_CONFIGURED:
1104 case VOCAB_CM_UNKNOWN:
1105 // Do nothing.
1106 break;
1107 default:
1108 {
1109 iControlMode->setControlMode((*joint), VOCAB_CM_IDLE);
1110 }
1111 }
1112 }
1113 }
1114 return true;
1115}
1116
1117bool parametricCalibratorEth::moveAndCheck(PositionSequence &data)
1118{
1119 iPosition->setRefSpeeds(data.velocities.data());
1120 iPosition->positionMove(data.positions.data());
1121
1122 bool done = false;
1123 int timeout = 0;
1124 do
1125 {
1126 Time::delay(1);
1127 timeout++;
1128 iPosition->checkMotionDone(&done);
1129 }
1130 while((!done) && (timeout < PARK_TIMEOUT) && (!abortParking));
1131
1132 if(!done)
1133 {
1134 yError() << "ParametricCalibratorEth: parking " << deviceName << " not in position after a timeout of" << PARK_TIMEOUT << " seconds";
1135 }
1136
1137 return done;
1138}
1139
1140bool parametricCalibratorEth::moveAndCheck_legacy(PositionSequence &data, std::vector<bool> &cannotPark, bool wait)
1141{
1142 bool done=false;
1143 // send references to joints that need to be parked
1144 for(auto joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++) //for each joint of set
1145 {
1146 if (cannotPark[(*joint)] ==false)
1147 {
1148 iPosition->setRefSpeed((*joint), data.velocities[(*joint)]);
1149 iPosition->positionMove((*joint), data.positions[(*joint)]);
1150 }
1151 }
1152
1153 // wait for the parking to be completed
1154 if (wait)
1155 {
1156 for(auto joint = calibJoints.begin(); joint != calibJoints.end() && !abortCalib; joint++) //for each joint of set
1157 {
1158 int timeout = 0;
1159 if (cannotPark[(*joint)] ==false)
1160 {
1161 yDebug() << deviceName.c_str() << ": Moving to park position, joint:" << (*joint);
1162 done=false;
1163 iPosition->checkMotionDone((*joint), &done);
1164 while ((!done) && (timeout < PARK_TIMEOUT) && (!abortParking))
1165 {
1166 Time::delay(1);
1167 timeout++;
1168 iPosition->checkMotionDone((*joint), &done);
1169 }
1170 if(!done)
1171 {
1172 yError() << deviceName << ": joint " << (*joint) << " not in position after a timeout of" << PARK_TIMEOUT << " seconds";
1173 }
1174 }
1175 }
1176 }
1177 return done;
1178}
1179
1181{
1182 yDebug() << deviceName.c_str() << ": Quitting calibrate\n";
1183 abortCalib = true;
1184 return true;
1185}
1186
1188{
1189 yDebug() << deviceName.c_str() << ": Quitting parking\n";
1190 abortParking=true;
1191 return true;
1192}
1193
1195{
1196 return this;
1197}
1198
1200{
1201 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
1202 {
1203 yError("%s cannot perform 'calibration' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
1204 return false;
1205 }
1206
1207 return calibrateJoint(j);
1208}
1209
1211{
1212 yTrace();
1213 return calibrate();
1214}
1215
1217{
1218 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
1219 {
1220 yError("%s cannot perform 'homing' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
1221 return false;
1222 }
1223
1224 if (disableHomeAndPark[j])
1225 {
1226 yWarning() << deviceName << ": homingSingleJoint, joint " << j << " is disabled on user request";
1227 return true;
1228 }
1229 return goToStartupPosition(j);
1230}
1231
1233{
1234 yTrace();
1235 bool ret = true;
1236 std::list<int>::iterator lit;
1237 for(lit = calibJoints.begin(); lit != calibJoints.end() && !abortCalib; lit++) //for each joint of set
1238 {
1239 ret = homingSingleJoint(*lit) && ret;
1240 }
1241 return ret;
1242}
1243
1245{
1246 // check input joint number is valid
1247 if(std::find(calibJoints.begin(), calibJoints.end(), j) == calibJoints.end())
1248 {
1249 yError("%s cannot perform 'park' operation because joint number %d is out of range [%s].", deviceName.c_str(), j, calibJointsString.toString().c_str());
1250 return false;
1251 }
1252
1253 if(useLegacyParking) // legacy version: can be removed when legacy will not be supported anymore
1254 {
1255 if (disableHomeAndPark[j])
1256 {
1257 yWarning() << deviceName << ": parkSingleJoint, joint " << j << " is disabled on user request";
1258 return true;
1259 }
1260 }
1261
1262 abortParking=false;
1263
1264 if(!isCalibrated)
1265 {
1266 yWarning() << deviceName << ": Calling park without calibration... skipping";
1267 return true;
1268 }
1269
1270 if(skipCalibration)
1271 {
1272 yWarning() << deviceName << ": skipCalibration flag is on!! Faking park!!";
1273 return true;
1274 }
1275
1276 int currentControlMode;
1277 bool cannotPark;
1278 bool res = iControlMode->getControlMode(j, &currentControlMode);
1279 if(!res)
1280 {
1281 yError() << deviceName << ": error getting control mode during parking";
1282 }
1283
1284 if(currentControlMode != VOCAB_CM_IDLE &&
1285 currentControlMode != VOCAB_CM_HW_FAULT)
1286 {
1287 iControlMode->setControlMode(j, VOCAB_CM_POSITION);
1288 cannotPark = false;
1289 }
1290 else if (currentControlMode == VOCAB_CM_IDLE)
1291 {
1292 yError() << deviceName << ": joint " << j << " is idle, skipping park";
1293 cannotPark = true;
1294 }
1295 else if (currentControlMode == VOCAB_CM_HW_FAULT)
1296 {
1297 yError() << deviceName << ": joint " << j << " has an hardware fault, skipping park";
1298 cannotPark = true;
1299 }
1300
1301 if(useLegacyParking) // legacy version: can be removed when legacy will not be supported anymore
1302 {
1303 iPosition->setRefSpeed (j, legacyStartupPosition.velocities[j]);
1304 iPosition->positionMove(j, legacyStartupPosition.positions[j]);
1305 }
1306 else
1307 {
1308 // Send the position and velocities of the last sequence step
1309 iPosition->setRefSpeed (j, parkingSequence.rbegin()->velocities[j]);
1310 iPosition->positionMove(j, parkingSequence.rbegin()->positions[j]);
1311 }
1312
1313 if (_wait)
1314 {
1315 if (cannotPark == false)
1316 {
1317 int timeout = 0;
1318 yDebug() << deviceName.c_str() << ": Moving to park position, joint:" << j;
1319 bool done=false;
1320 iPosition->checkMotionDone(j, &done);
1321 while ((!done) && (timeout < PARK_TIMEOUT) && (!abortParking))
1322 {
1323 Time::delay(1);
1324 timeout++;
1325 iPosition->checkMotionDone(j, &done);
1326 }
1327 if(!done)
1328 {
1329 yError() << deviceName << ": joint " << j << " not in position after a timeout of" << PARK_TIMEOUT << " seconds";
1330 }
1331 }
1332 }
1333
1334 yDebug() << deviceName.c_str() << ": Park " << (abortParking ? "aborted" : "completed");
1335 iControlMode->setControlMode(j,VOCAB_CM_IDLE);
1336 return true;
1337}
1338
1339// called from motorgui or remote devices
1341{
1342 yTrace();
1343
1344 if(!isCalibrated)
1345 {
1346 yError() << "Device is not calibrated therefore cannot be parked";
1347 return false;
1348 }
1349
1350 return park(dev2calibrate);
1351}
1352
1353
1354// eof
1355
@ 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
static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
const int PARK_TIMEOUT
out
Definition sine.m:8