11 #include <yarp/os/Bottle.h>
19 #include <yarp/os/LogStream.h>
23 #include "EoProtocol.h"
24 #include "EoProtocolMN.h"
25 #include "EoProtocolMC.h"
26 #include "EoProtocolAS.h"
32 using namespace yarp::os;
40 _njoints = numofjoints;
41 _boardname = boardname;
43 _positionControlLaw.resize(0);
44 _velocityControlLaw.resize(0);
45 _mixedControlLaw.resize(0);
48 _torqueControlLaw.resize(0);
49 _currentControlLaw.resize(0);
50 _speedControlLaw.resize(0);
52 _kbemf=allocAndCheck<double>(_njoints);
53 _ktau=allocAndCheck<double>(_njoints);
54 _filterType=allocAndCheck<int>(_njoints);
55 _viscousPos=allocAndCheck<double>(_njoints);
56 _viscousNeg=allocAndCheck<double>(_njoints);
57 _coulombPos=allocAndCheck<double>(_njoints);
58 _coulombNeg=allocAndCheck<double>(_njoints);
60 minjerkAlgoMap.clear();
62 torqueAlgoMap.clear();
67 checkAndDestroy(_kbemf);
68 checkAndDestroy(_ktau);
69 checkAndDestroy(_filterType);
70 checkAndDestroy(_viscousPos);
71 checkAndDestroy(_viscousNeg);
72 checkAndDestroy(_coulombPos);
73 checkAndDestroy(_coulombNeg);
89 if(!parseControlsGroup(config))
94 if(!parseSelectedCurrentPid(config, lowLevPidisMandatory, cpids))
99 if(!parseSelectedSpeedPid(config, lowLevPidisMandatory, spids))
104 if(!parseSelectedPositionControl(config))
109 if(!parseSelectedVelocityControl(config))
114 if(!parseSelectedMixedControl(config))
129 if(!parseSelectedTorqueControl(config))
134 if(!getCorrectPidForEachJoint(ppids, tpids))
141 #define LOAD_STRINGS(dest, source) for (unsigned int i=1; i<source.size(); ++i) dest.push_back(source.get(i).asString())
143 bool Parser::parseControlsGroup(yarp::os::Searchable &config)
147 Bottle controlsGroup = config.findGroup(
"CONTROLS",
"Configuration of used control laws ");
148 if(controlsGroup.isNull())
150 yError() <<
"embObjMC BOARD " << _boardname <<
" no CONTROLS group found in config file, returning";
154 if (!
extractGroup(controlsGroup, xtmp,
"positionControl",
"Position Control ", _njoints))
158 if (!
extractGroup(controlsGroup, xtmp,
"velocityControl",
"Velocity Control ", _njoints))
162 if (!
extractGroup(controlsGroup, xtmp,
"mixedControl",
"Mixed Control ", _njoints))
174 if (!
extractGroup(controlsGroup, xtmp,
"torqueControl",
"Torque Control ", _njoints))
178 if (!
extractGroup(controlsGroup, xtmp,
"currentPid",
"Current Pid ", _njoints))
182 if (!
extractGroup(controlsGroup, xtmp,
"speedPid",
"Speed Pid ", _njoints))
191 bool Parser::parseSelectedCurrentPid(yarp::os::Searchable &config,
bool pidisMandatory,
PidInfo *pids)
194 for (
int i = 0; i<_njoints; i++)
196 if (_currentControlLaw[i] ==
"none")
200 yError() <<
"embObjMC BOARD " << _boardname <<
"CurrentPid is mandatory. It should be different from none ";
207 Bottle botControlLaw = config.findGroup(_currentControlLaw[i]);
208 if (botControlLaw.isNull())
210 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing " << i <<
" current control law " << _currentControlLaw[i].c_str();
215 Value &valControlLaw = botControlLaw.find(
"controlLaw");
216 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
218 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read " << i <<
" control law parameter for " << _currentControlLaw[i].c_str() <<
". Quitting.";
222 string strControlLaw = valControlLaw.toString();
223 if (strControlLaw !=
string(
"low_lev_current"))
225 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable to use " << i <<
" control law " << strControlLaw <<
" for current pid. Quitting.";
229 yarp::dev::PidFeedbackUnitsEnum fbk_unitstype;
230 yarp::dev::PidOutputUnitsEnum out_unitstype;
231 if (!parsePidUnitsType(botControlLaw, fbk_unitstype, out_unitstype))
234 yarp::dev::Pid *mycpids =
new yarp::dev::Pid[_njoints];
236 if (!parsePidsGroup2FOC(botControlLaw, mycpids))
244 pids[i].
out_type = eomc_ctrl_out_type_cur;
248 pids[i].
pid = mycpids[i];
254 return checkJointTypes(pids,
"CURRENT");
257 bool Parser::parseSelectedSpeedPid(yarp::os::Searchable &config,
bool pidisMandatory,
PidInfo *pids)
260 for (
int i = 0; i<_njoints; i++)
262 if (_speedControlLaw[i] ==
"none")
266 yError() <<
"embObjMC BOARD " << _boardname <<
"SpeedPid is mandatory. It should be different from none ";
273 Bottle botControlLaw = config.findGroup(_speedControlLaw[i]);
274 if (botControlLaw.isNull())
276 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing " << i <<
" control law " << _speedControlLaw[i].c_str();
281 Value &valControlLaw = botControlLaw.find(
"controlLaw");
282 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
284 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read " << i <<
" control law parameter for " << _speedControlLaw[i].c_str() <<
". Quitting.";
288 string strControlLaw= valControlLaw.toString();
289 if (strControlLaw !=
string(
"low_lev_speed"))
291 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable to use " << i <<
" control law " << strControlLaw <<
" for speed pid. Quitting.";
295 yarp::dev::PidFeedbackUnitsEnum fbk_unitstype;
296 yarp::dev::PidOutputUnitsEnum out_unitstype;
297 if (!parsePidUnitsType(botControlLaw, fbk_unitstype, out_unitstype))
300 yarp::dev::Pid *mycpids =
new yarp::dev::Pid[_njoints];
302 if (!parsePidsGroup2FOC(botControlLaw, mycpids))
313 pids[i].
pid = mycpids[i];
319 return checkJointTypes(pids,
"SPEED");
322 bool Parser::parseSelectedPositionControl(yarp::os::Searchable &config)
324 for(
int i=0; i<_njoints; i++)
327 Bottle botControlLaw = config.findGroup(_positionControlLaw[i]);
328 if (botControlLaw.isNull())
330 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing " << _positionControlLaw[i].c_str();
335 Value &valControlLaw= botControlLaw.find(
"controlLaw");
337 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
339 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read controlLaw parameter for " << _positionControlLaw[i].c_str() <<
". Quitting.";
344 string strControlLaw = valControlLaw.toString();
345 if (strControlLaw !=
"minjerk")
347 yError() <<
"embObjMC BOARD " << _boardname <<
"Unknown control law for " << _positionControlLaw[i].c_str() <<
". Quitting.";
351 Value &valOutputType= botControlLaw.find(
"outputType");
352 if( (valOutputType.isNull()) || (!valOutputType.isString()) )
354 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read outputType parameter for " << _positionControlLaw[i].c_str() <<
". Quitting.";
358 string strOutputType= valOutputType.toString();
359 if (strOutputType ==
string(
"pwm"))
361 if (!parsePid_minJerk_outPwm(botControlLaw, _positionControlLaw[i]))
363 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _positionControlLaw[i];
367 else if (strOutputType ==
string(
"velocity"))
369 if (!parsePid_minJerk_outVel(botControlLaw, _positionControlLaw[i]))
371 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _positionControlLaw[i];
375 else if (strOutputType ==
string(
"current"))
377 if (!parsePid_minJerk_outCur(botControlLaw, _positionControlLaw[i]))
379 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _positionControlLaw[i];
385 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable to use output type " << strOutputType <<
" for position control. Quitting.";
394 bool Parser::parseSelectedVelocityControl(yarp::os::Searchable &config)
396 for(
int i=0; i<_njoints; i++)
398 if(_velocityControlLaw[i] ==
"none")
404 Bottle botControlLaw = config.findGroup(_velocityControlLaw[i]);
405 if (botControlLaw.isNull())
407 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing " << _velocityControlLaw[i].c_str();
412 Value &valControlLaw=botControlLaw.find(
"controlLaw");
414 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
416 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read controlLaw parameter for " << _velocityControlLaw[i].c_str() <<
". Quitting.";
421 string strControlLaw = valControlLaw.toString();
422 if (strControlLaw !=
"minjerk")
424 yError() <<
"embObjMC BOARD " << _boardname <<
"Unknown control law for " << _velocityControlLaw[i].c_str() <<
". Quitting.";
428 Value &valOutputType = botControlLaw.find(
"outputType");
429 if ((valOutputType.isNull()) || (!valOutputType.isString()))
431 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read control law parameter for " << _velocityControlLaw[i].c_str() <<
". Quitting.";
435 string strOutputType = valOutputType.toString();
436 if (strOutputType ==
string(
"pwm"))
438 if (!parsePid_minJerk_outPwm(botControlLaw, _velocityControlLaw[i]))
440 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in "<< _velocityControlLaw[i];
444 else if (strOutputType ==
string(
"velocity"))
446 if (!parsePid_minJerk_outVel(botControlLaw, _velocityControlLaw[i]))
448 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _velocityControlLaw[i];
452 else if (strOutputType ==
string(
"current"))
454 if (!parsePid_minJerk_outCur(botControlLaw, _velocityControlLaw[i]))
456 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _velocityControlLaw[i];
462 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable to use output type " << strOutputType <<
" for velocity control. Quitting.";
471 bool Parser::parseSelectedMixedControl(yarp::os::Searchable &config)
473 for (
int i = 0; i<_njoints; i++)
475 if(_mixedControlLaw[i] ==
"none")
481 Bottle botControlLaw = config.findGroup(_mixedControlLaw[i]);
482 if (botControlLaw.isNull())
484 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing " << _mixedControlLaw[i].c_str();
489 Value &valControlLaw = botControlLaw.find(
"controlLaw");
491 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
493 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read controlLaw parameter for " << _mixedControlLaw[i].c_str() <<
". Quitting.";
498 string strControlLaw = valControlLaw.toString();
499 if (strControlLaw !=
"minjerk")
501 yError() <<
"embObjMC BOARD " << _boardname <<
"Unknown control law for " << _mixedControlLaw[i].c_str() <<
". Quitting.";
506 Value &valOutputType = botControlLaw.find(
"outputType");
507 if ((valOutputType.isNull()) || (!valOutputType.isString()))
509 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read control law parameter for " << _mixedControlLaw[i].c_str() <<
". Quitting.";
513 string strOutputType = valOutputType.toString();
514 if (strOutputType ==
string(
"pwm"))
516 if (!parsePid_minJerk_outPwm(botControlLaw, _mixedControlLaw[i]))
518 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _velocityControlLaw[i];
522 else if (strOutputType ==
string(
"velocity"))
524 if (!parsePid_minJerk_outVel(botControlLaw, _mixedControlLaw[i]))
526 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _velocityControlLaw[i];
530 else if (strOutputType ==
string(
"current"))
532 if (!parsePid_minJerk_outCur(botControlLaw, _mixedControlLaw[i]))
534 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _velocityControlLaw[i];
540 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable to use output type " << strOutputType <<
" for mixed control. Quitting.";
549 bool Parser::parseSelectedPosDirectControl(yarp::os::Searchable &config)
551 for (
int i = 0; i<_njoints; i++)
554 Bottle botControlLaw = config.findGroup(_posDirectControlLaw[i]);
555 if (botControlLaw.isNull())
557 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing " << _posDirectControlLaw[i].c_str();
562 Value &valControlLaw= botControlLaw.find(
"controlLaw");
564 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
566 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read controlLaw parameter for " << _posDirectControlLaw[i].c_str() <<
". Quitting.";
571 string strControlLaw = valControlLaw.toString();
572 if (strControlLaw !=
"direct")
574 yError() <<
"embObjMC BOARD " << _boardname <<
"Unknown control law for " << _posDirectControlLaw[i].c_str() <<
". Quitting.";
578 Value &valOutputType = botControlLaw.find(
"outputType");
579 if ((valOutputType.isNull()) || (!valOutputType.isString()))
581 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read outputType parameter for " << _posDirectControlLaw[i].c_str() <<
". Quitting.";
585 string strOutputType = valOutputType.toString();
586 if (strOutputType ==
string(
"pwm"))
588 if (!parsePid_direct_outPwm(botControlLaw, _posDirectControlLaw[i]))
590 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _posDirectControlLaw[i];
594 else if (strOutputType ==
string(
"velocity"))
596 if (!parsePid_direct_outVel(botControlLaw, _posDirectControlLaw[i]))
598 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _posDirectControlLaw[i];
602 else if (strOutputType ==
string(
"current"))
604 if (!parsePid_direct_outCur(botControlLaw, _posDirectControlLaw[i]))
606 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _posDirectControlLaw[i];
612 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable to use output type " << strOutputType <<
" for direct position control. Quitting.";
621 bool Parser::parseSelectedVelDirectControl(yarp::os::Searchable &config)
623 for (
int i = 0; i<_njoints; i++)
626 Bottle botControlLaw = config.findGroup(_velDirectControlLaw[i]);
627 if (botControlLaw.isNull())
629 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing " << _velDirectControlLaw[i].c_str();
634 Value &valControlLaw = botControlLaw.find(
"controlLaw");
636 if ((valControlLaw.isNull()) || (!valControlLaw.isString()))
638 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read controlLaw parameter for " << _velDirectControlLaw[i].c_str() <<
". Quitting.";
643 string strControlLaw = valControlLaw.toString();
644 if (strControlLaw !=
"direct")
646 yError() <<
"embObjMC BOARD " << _boardname <<
"Unknown control law for " << _velDirectControlLaw[i].c_str() <<
". Quitting.";
650 Value &valOutputType = botControlLaw.find(
"outputType");
651 if ((valOutputType.isNull()) || (!valOutputType.isString()))
653 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read outputType parameter for " << _velDirectControlLaw[i].c_str() <<
". Quitting.";
657 string strOutputType = valOutputType.toString();
658 if (strOutputType ==
string(
"pwm"))
660 if (!parsePid_direct_outPwm(botControlLaw, _velDirectControlLaw[i]))
662 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _velDirectControlLaw[i];
666 else if (strOutputType ==
string(
"velocity"))
668 if (!parsePid_direct_outVel(botControlLaw, _velDirectControlLaw[i]))
670 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _velDirectControlLaw[i];
674 else if (strOutputType ==
string(
"current"))
676 if (!parsePid_direct_outCur(botControlLaw, _velDirectControlLaw[i]))
678 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _velDirectControlLaw[i];
684 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable to use output type " << strOutputType <<
" for direct velocity control. Quitting.";
694 bool Parser::parseSelectedTorqueControl(yarp::os::Searchable &config)
696 for(
int i=0; i<_njoints; i++)
698 if(_torqueControlLaw[i] ==
"none")
703 Bottle botControlLaw = config.findGroup(_torqueControlLaw[i]);
704 if (botControlLaw.isNull())
706 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing " << _torqueControlLaw[i].c_str();
711 Value &valControlLaw= botControlLaw.find(
"controlLaw");
712 if( (valControlLaw.isNull()) || (!valControlLaw.isString()) )
714 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read control law parameter for " << _torqueControlLaw[i].c_str() <<
". Quitting.";
718 string strControlLaw = valControlLaw.toString();
719 if (strControlLaw !=
"torque")
721 yError() <<
"embObjMC BOARD " << _boardname <<
"Unknown control law for " << _torqueControlLaw[i].c_str() <<
". Quitting.";
725 Value &valOutputType = botControlLaw.find(
"outputType");
726 if ((valOutputType.isNull()) || (!valOutputType.isString()))
728 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable read outputType parameter for " << _torqueControlLaw[i].c_str() <<
". Quitting.";
732 string strOutputType = valOutputType.toString();
733 if (strOutputType ==
string(
"pwm"))
735 if (!parsePid_torque_outPwm(botControlLaw, _torqueControlLaw[i]))
737 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _torqueControlLaw[i];
741 else if (strOutputType ==
string(
"velocity"))
743 if (!parsePid_torque_outVel(botControlLaw, _torqueControlLaw[i]))
745 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _torqueControlLaw[i];
749 else if (strOutputType ==
string(
"current"))
751 if (!parsePid_torque_outCur(botControlLaw, _torqueControlLaw[i]))
753 yError() <<
"embObjMC BOARD " << _boardname <<
"format error in " << _torqueControlLaw[i];
759 yError() <<
"embObjMC BOARD " << _boardname <<
"Unable to use output type " << strOutputType <<
" for torque control. Quitting.";
796 bool Parser::parsePidsGroup2FOC(Bottle& pidsGroup, Pid myPid[])
801 if (!
extractGroup(pidsGroup, xtmp,
"kff",
"kff parameter", _njoints))
return false;
802 for (
int j = 0; j<_njoints; j++) myPid[j].kff = xtmp.get(j + 1).asFloat64();
804 if (!
extractGroup(pidsGroup, xtmp,
"kp",
"kp parameter", _njoints))
return false;
805 for (
int j = 0; j<_njoints; j++) myPid[j].kp = xtmp.get(j + 1).asFloat64();
807 if (!
extractGroup(pidsGroup, xtmp,
"kd",
"kd parameter", _njoints))
return false;
808 for (
int j = 0; j<_njoints; j++) myPid[j].kd = xtmp.get(j + 1).asFloat64();
810 if (!
extractGroup(pidsGroup, xtmp,
"maxOutput",
"maxOutput parameter", _njoints))
return false;
811 for (
int j = 0; j<_njoints; j++) myPid[j].max_output = xtmp.get(j + 1).asFloat64();
813 if (!
extractGroup(pidsGroup, xtmp,
"ki",
"ki parameter", _njoints))
return false;
814 for (
int j = 0; j<_njoints; j++) myPid[j].ki = xtmp.get(j + 1).asFloat64();
816 if (!
extractGroup(pidsGroup, xtmp,
"maxInt",
"maxInt parameter", _njoints))
return false;
817 for (
int j = 0; j<_njoints; j++) myPid[j].max_int = xtmp.get(j + 1).asFloat64();
819 if (!
extractGroup(pidsGroup, xtmp,
"shift",
"shift parameter", _njoints))
return false;
820 for (
int j = 0; j<_njoints; j++) myPid[j].scale = xtmp.get(j + 1).asFloat64();
826 bool Parser::parsePidsGroupSimple(Bottle& pidsGroup, Pid myPid[])
837 if (!
extractGroup(pidsGroup, xtmp,
"kff",
"kff parameter", _njoints))
return false;
838 for (
int j = 0; j<_njoints; j++) myPid[j].kff = xtmp.get(j + 1).asFloat64();
840 if (!
extractGroup(pidsGroup, xtmp,
"kp",
"kp parameter", _njoints))
return false;
841 for (
int j = 0; j<_njoints; j++) myPid[j].kp = xtmp.get(j + 1).asFloat64();
843 if (!
extractGroup(pidsGroup, xtmp,
"kd",
"kd parameter", _njoints))
return false;
844 for (
int j = 0; j<_njoints; j++) myPid[j].kd = xtmp.get(j + 1).asFloat64();
846 if (!
extractGroup(pidsGroup, xtmp,
"maxOutput",
"maxOutput parameter", _njoints))
return false;
847 for (
int j = 0; j<_njoints; j++) myPid[j].max_output = xtmp.get(j + 1).asFloat64();
852 bool Parser::parsePidsGroupExtended(Bottle& pidsGroup, Pid myPid[])
861 if (!parsePidsGroupSimple(pidsGroup, myPid))
return false;
872 if (!
extractGroup(pidsGroup, xtmp,
"ki",
"ki parameter", _njoints))
return false;
873 for (
int j = 0; j<_njoints; j++) myPid[j].ki = xtmp.get(j + 1).asFloat64();
875 if (!
extractGroup(pidsGroup, xtmp,
"maxInt",
"maxInt parameter", _njoints))
return false;
876 for (
int j = 0; j<_njoints; j++) myPid[j].max_int = xtmp.get(j + 1).asFloat64();
878 if (!
extractGroup(pidsGroup, xtmp,
"stictionUp",
"stictionUp parameter", _njoints))
return false;
879 for (
int j = 0; j<_njoints; j++) myPid[j].stiction_up_val = xtmp.get(j + 1).asFloat64();
881 if (!
extractGroup(pidsGroup, xtmp,
"stictionDown",
"stictionDown parameter", _njoints))
return false;
882 for (
int j = 0; j<_njoints; j++) myPid[j].stiction_down_val = xtmp.get(j + 1).asFloat64();
887 bool Parser::parsePidsGroupDeluxe(Bottle& pidsGroup, Pid myPid[])
900 if (!parsePidsGroupExtended(pidsGroup, myPid))
return false;
904 if (!
extractGroup(pidsGroup, xtmp,
"kbemf",
"kbemf parameter", _njoints))
return false;
905 for (
int j = 0; j<_njoints; j++) _kbemf[j] = xtmp.get(j + 1).asFloat64();
907 if (!
extractGroup(pidsGroup, xtmp,
"ktau",
"ktau parameter", _njoints))
return false;
908 for (
int j = 0; j<_njoints; j++) _ktau[j] = xtmp.get(j + 1).asFloat64();
910 if (!
extractGroup(pidsGroup, xtmp,
"filterType",
"filterType param", _njoints))
return false;
911 for (
int j = 0; j<_njoints; j++) _filterType[j] = xtmp.get(j + 1).asInt32();
914 if (!
extractGroup(pidsGroup, xtmp,
"viscousPos",
"viscousPos parameter", _njoints,
false))
916 for (
int j = 0; j<_njoints; j++) _viscousPos[j] = 0;
920 for (
int j = 0; j<_njoints; j++) _viscousPos[j] = xtmp.get(j + 1).asFloat64();
923 if (!
extractGroup(pidsGroup, xtmp,
"viscousNeg",
"viscousNeg parameter", _njoints,
false))
925 for (
int j = 0; j<_njoints; j++) _viscousNeg[j] = 0;
929 for (
int j = 0; j<_njoints; j++) _viscousNeg[j] = xtmp.get(j + 1).asFloat64();
932 if (!
extractGroup(pidsGroup, xtmp,
"coulombPos",
"coulombPos parameter", _njoints,
false))
934 for (
int j = 0; j<_njoints; j++) _coulombPos[j] = 0;
938 for (
int j = 0; j<_njoints; j++) _coulombPos[j] = xtmp.get(j + 1).asFloat64();
941 if (!
extractGroup(pidsGroup, xtmp,
"coulombNeg",
"coulombNeg parameter", _njoints,
false))
943 for (
int j = 0; j<_njoints; j++) _coulombNeg[j] = 0;
947 for (
int j = 0; j<_njoints; j++) _coulombNeg[j] = xtmp.get(j + 1).asFloat64();
994 bool Parser::extractGroup(Bottle &input, Bottle &
out,
const std::string &key1,
const std::string &txt,
int size,
bool mandatory)
997 Bottle &
tmp=input.findGroup(key1.c_str(), txt.c_str());
1000 std::string message = key1 +
" parameter not found for board " + _boardname +
" in bottle " + input.toString();
1002 yError () << message.c_str();
1004 yWarning() << message.c_str();
1008 if(
tmp.size()!=size)
1010 yError () << key1.c_str() <<
" incorrect number of entries in BOARD " << _boardname;
1018 bool Parser::parsePid_minJerk_outPwm(Bottle &b_pid,
string controlLaw)
1020 if (minjerkAlgoMap.find(controlLaw) != minjerkAlgoMap.end())
return true;
1024 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1025 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1026 if (!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits))
return false;
1027 pidAlgo_ptr->
setUnits(fbk_PidUnits, out_PidUnits);
1029 parsePidsGroupExtended(b_pid, pidAlgo_ptr->
pid);
1031 minjerkAlgoMap.insert(std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1036 bool Parser::parsePid_minJerk_outCur(Bottle &b_pid,
string controlLaw)
1038 if (minjerkAlgoMap.find(controlLaw) != minjerkAlgoMap.end())
return true;
1042 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1043 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1044 if (!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits))
return false;
1045 pidAlgo_ptr->
setUnits(fbk_PidUnits, out_PidUnits);
1047 parsePidsGroupExtended(b_pid, pidAlgo_ptr->
pid);
1049 minjerkAlgoMap.insert(std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1054 bool Parser::parsePid_minJerk_outVel(Bottle &b_pid,
string controlLaw)
1056 if (minjerkAlgoMap.find(controlLaw) != minjerkAlgoMap.end())
return true;
1060 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1061 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1062 if (!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits))
return false;
1063 pidAlgo_ptr->
setUnits(fbk_PidUnits, out_PidUnits);
1065 parsePidsGroupSimple(b_pid, pidAlgo_ptr->
pid);
1067 minjerkAlgoMap.insert(std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1128 bool Parser::parsePid_torque_outPwm(Bottle &b_pid,
string controlLaw)
1130 if (torqueAlgoMap.find(controlLaw) != torqueAlgoMap.end())
return true;
1134 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1135 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1136 if(!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits))
return false;
1137 pidAlgo_ptr->
setUnits(fbk_PidUnits, out_PidUnits);
1139 parsePidsGroupDeluxe(b_pid, pidAlgo_ptr->
pid);
1141 torqueAlgoMap.insert( std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1146 bool Parser::parsePid_torque_outCur(Bottle &b_pid,
string controlLaw)
1148 if (torqueAlgoMap.find(controlLaw) != torqueAlgoMap.end())
return true;
1152 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1153 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1154 if (!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits))
return false;
1155 pidAlgo_ptr->
setUnits(fbk_PidUnits, out_PidUnits);
1157 parsePidsGroupDeluxe(b_pid, pidAlgo_ptr->
pid);
1159 torqueAlgoMap.insert(std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr));
1164 bool Parser::parsePid_torque_outVel(Bottle &b_pid,
string controlLaw)
1166 if (torqueAlgoMap.find(controlLaw) != torqueAlgoMap.end())
return true;
1170 yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits;
1171 yarp::dev::PidOutputUnitsEnum out_PidUnits;
1172 if(!parsePidUnitsType(b_pid, fbk_PidUnits, out_PidUnits))
return false;
1173 pidAlgo_ptr->
setUnits(fbk_PidUnits, out_PidUnits);
1175 parsePidsGroupExtended(b_pid, pidAlgo_ptr->
pid);
1177 torqueAlgoMap.insert ( std::pair<std::string, Pid_Algorithm*>(controlLaw, pidAlgo_ptr) );
1189 memset(ppids, 0,
sizeof(
PidInfo)*_njoints);
1191 memset(tpids, 0,
sizeof(
TrqPidInfo)*_njoints);
1193 map<string, Pid_Algorithm*>::iterator it;
1195 for (
int i = 0; i < _njoints; i++)
1198 it = minjerkAlgoMap.find(_positionControlLaw[i]);
1199 if (it == minjerkAlgoMap.end())
1201 yError() <<
"embObjMC BOARD " << _boardname <<
"Cannot find " << _positionControlLaw[i].c_str() <<
"in parsed pos pid";
1205 minjerkAlgo_ptr = minjerkAlgoMap[_positionControlLaw[i]];
1207 ppids[i].
pid = minjerkAlgo_ptr->
getPID(i);
1251 if (_torqueControlLaw[i] ==
"none")
1253 torqueAlgo_ptr = NULL;
1257 it = torqueAlgoMap.find(_torqueControlLaw[i]);
1258 if (it == torqueAlgoMap.end())
1260 yError() <<
"embObjMC BOARD " << _boardname <<
"Cannot find " << _torqueControlLaw[i].c_str() <<
"in parsed trq pid";
1264 torqueAlgo_ptr = torqueAlgoMap[_torqueControlLaw[i]];
1269 tpids[i].
pid = torqueAlgo_ptr->
getPID(i);
1276 tpids[i].
kbemf = _kbemf[i];
1277 tpids[i].
ktau = _ktau[i];
1296 return checkJointTypes(tpids,
"TORQUE") && checkJointTypes(ppids,
"POSITION");
1299 bool Parser::parsePidUnitsType(Bottle &bPid, yarp::dev::PidFeedbackUnitsEnum &fbk_pidunits, yarp::dev::PidOutputUnitsEnum& out_pidunits)
1302 Value &fbkControlUnits=bPid.find(
"fbkControlUnits");
1303 Value &outControlUnits = bPid.find(
"outputControlUnits");
1304 if(fbkControlUnits.isNull())
1306 yError() <<
"embObjMC BOARD " << _boardname <<
" missing fbkControlUnits parameter";
1309 if(!fbkControlUnits.isString())
1311 yError() <<
"embObjMC BOARD " << _boardname <<
" fbkControlUnits parameter is not a string";
1314 if (outControlUnits.isNull())
1316 yError() <<
"embObjMC BOARD " << _boardname <<
" missing outputControlUnits parameter";
1319 if (!outControlUnits.isString())
1321 yError() <<
"embObjMC BOARD " << _boardname <<
" outputControlUnits parameter is not a string";
1325 if(fbkControlUnits.toString()==
string(
"metric_units"))
1327 fbk_pidunits = yarp::dev::PidFeedbackUnitsEnum::METRIC;
1329 else if(fbkControlUnits.toString()==
string(
"machine_units"))
1331 fbk_pidunits = yarp::dev::PidFeedbackUnitsEnum::RAW_MACHINE_UNITS;
1335 yError() <<
"embObjMC BOARD " << _boardname <<
"invalid fbkControlUnits value: " << fbkControlUnits.toString().c_str();
1339 if (outControlUnits.toString() ==
string(
"dutycycle_percent"))
1341 out_pidunits = yarp::dev::PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT;
1343 else if (outControlUnits.toString() ==
string(
"machine_units"))
1345 out_pidunits = yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS;
1349 yError() <<
"embObjMC BOARD " << _boardname <<
"invalid outputControlUnits value: " << outControlUnits.toString().c_str();
1357 Bottle &focGroup=config.findGroup(groupName);
1358 if (focGroup.isNull() )
1360 yError() <<
"embObjMC BOARD " << _boardname <<
" detected that Group " << groupName <<
" is not found in configuration file";
1367 if (!
extractGroup(focGroup, xtmp,
"HasHallSensor",
"HasHallSensor 0/1 ", _njoints))
1373 for (i = 1; i < xtmp.size(); i++)
1374 foc_based_info[i - 1].hasHallSensor = xtmp.get(i).asInt32() != 0;
1376 if (!
extractGroup(focGroup, xtmp,
"HasTempSensor",
"HasTempSensor 0/1 ", _njoints))
1382 for (i = 1; i < xtmp.size(); i++)
1383 foc_based_info[i - 1].hasTempSensor = xtmp.get(i).asInt32() != 0;
1385 if (!
extractGroup(focGroup, xtmp,
"HasRotorEncoder",
"HasRotorEncoder 0/1 ", _njoints))
1392 for (i = 1; i < xtmp.size(); i++)
1393 foc_based_info[i - 1].hasRotorEncoder = xtmp.get(i).asInt32() != 0;
1395 if (!
extractGroup(focGroup, xtmp,
"HasRotorEncoderIndex",
"HasRotorEncoderIndex 0/1 ", _njoints))
1401 for (i = 1; i < xtmp.size(); i++)
1402 foc_based_info[i - 1].hasRotorEncoderIndex = xtmp.get(i).asInt32() != 0;
1405 if (!
extractGroup(focGroup, xtmp,
"Verbose",
"Verbose 0/1 ", _njoints,
false))
1408 yWarning() <<
"In " << _boardname <<
" there isn't " << groupName <<
". Verbose filed. For default it is enabled" ;
1409 for (i = 0; i < (unsigned)_njoints; i++)
1410 foc_based_info[i].verbose = 1;
1414 for (i = 1; i < xtmp.size(); i++)
1415 foc_based_info[i - 1].verbose = xtmp.get(i).asInt32() != 0;
1418 std::vector<int> AutoCalibration (_njoints);
1419 if (!
extractGroup(focGroup, xtmp,
"AutoCalibration",
"AutoCalibration 0/1 ", _njoints,
false))
1422 yWarning() <<
"In " << _boardname <<
" there isn't " << groupName <<
". AutoCalibration filed. For default it is disabled" ;
1423 for (i = 0; i < (unsigned)_njoints; i++)
1424 AutoCalibration[i] = 0;
1428 for (i = 1; i < xtmp.size(); i++)
1429 AutoCalibration[i - 1] = xtmp.get(i).asInt32();
1433 if (!
extractGroup(focGroup, xtmp,
"RotorIndexOffset",
"RotorIndexOffset", _njoints))
1439 for (i = 1; i < xtmp.size(); i++)
1441 if(AutoCalibration[i-1] == 0)
1444 if (foc_based_info[i - 1].rotorIndexOffset <0 || foc_based_info[i - 1].rotorIndexOffset >359)
1446 yError() <<
"In " << _boardname <<
"joint " << i-1 <<
": rotorIndexOffset should be in [0,359] range." ;
1452 yWarning() <<
"In " << _boardname <<
"joint " << i-1 <<
": motor autocalibration is enabled!!! ATTENTION!!!" ;
1460 for (i = 0; i < (unsigned)_njoints; i++)
1462 if((0 == foc_based_info[i].hasRotorEncoderIndex) && (0 != foc_based_info[i].
rotorIndexOffset))
1464 yError() <<
"In " << _boardname <<
"joint " << i <<
": inconsistent configuration: if rotor encoder hasn't index then its offset should be 0." ;
1470 if (!
extractGroup(focGroup, xtmp,
"MotorPoles",
"MotorPoles", _njoints))
1476 for (i = 1; i < xtmp.size(); i++)
1477 foc_based_info[i - 1].motorPoles = xtmp.get(i).asInt32();
1480 if (!
extractGroup(focGroup, xtmp,
"HasSpeedEncoder",
"HasSpeedEncoder 0/1 ", _njoints))
1482 yWarning () <<
"missing param HasSpeedEncoder";
1488 for (i = 1; i < xtmp.size(); i++)
1489 foc_based_info[i - 1].hasSpeedEncoder = xtmp.get(i).asInt32() != 0;
1496 bool Parser::parseJointsetCfgGroup(yarp::os::Searchable &config, std::vector<JointsSet> &jsets, std::vector<int> &joint2set)
1498 Bottle jointsetcfg = config.findGroup(
"JOINTSET_CFG");
1499 if (jointsetcfg.isNull())
1501 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing JOINTSET_CFG group";
1509 if(!
extractGroup(jointsetcfg, xtmp,
"numberofsets",
"number of sets ", 1))
1514 numofsets = xtmp.get(1).asInt32();
1516 if((0 == numofsets) || (numofsets > _njoints))
1518 yError() <<
"embObjMC BOARD " << _boardname <<
"Number of jointsets is not correct. it should belong to (1, " << _njoints <<
")";
1524 if(!checkAndSetVectorSize(jsets, numofsets,
"parseJointsetCfgGroup"))
1527 if(!checkAndSetVectorSize(joint2set, _njoints,
"parseJointsetCfgGroup"))
1530 for(
unsigned int s=0;s<(unsigned)numofsets;s++)
1532 char jointset_string[80];
1533 sprintf(jointset_string,
"JOINTSET_%d", s);
1534 bool formaterror =
false;
1537 Bottle &js_cfg = jointsetcfg.findGroup(jointset_string);
1540 yError() <<
"embObjMC BOARD " << _boardname <<
"cannot find " << jointset_string;
1549 Bottle &b_listofjoints=js_cfg.findGroup(
"listofjoints",
"list of joints");
1550 if (b_listofjoints.isNull())
1552 yError() <<
"embObjMC BOARD " << _boardname <<
"listofjoints parameter not found";
1556 int numOfJointsInSet = b_listofjoints.size()-1;
1557 if((numOfJointsInSet < 1) || (numOfJointsInSet>_njoints))
1559 yError() <<
"embObjMC BOARD " << _boardname <<
"numof joints of set " << s <<
" is not correct";
1564 for (
int j = 0; j <numOfJointsInSet; j++)
1566 int jointofthisset = b_listofjoints.get(j+1).asInt32();
1568 if((jointofthisset< 0) || (jointofthisset>_njoints))
1570 yError() <<
"embObjMC BOARD " << _boardname <<
"invalid joint number for set " << s;
1574 jsets.at(s).joints.push_back(jointofthisset);
1577 joint2set.at(jointofthisset) = s;
1581 if(!
extractGroup(js_cfg, xtmp,
"constraint",
"type of jointset constraint ", 1))
1586 eOmc_jsetconstraint_t constraint;
1587 if(!
convert(xtmp.get(1).asString(), constraint, formaterror))
1591 jsets.at(s).cfg.constraints.type = constraint;
1594 if(!
extractGroup(js_cfg, xtmp,
"param1",
"param1 of jointset constraint ", 1))
1598 jsets.at(s).cfg.constraints.param1 = (float)xtmp.get(1).asFloat64();
1601 if(!
extractGroup(js_cfg, xtmp,
"param2",
"param2 of jointset constraint ", 1))
1605 jsets.at(s).cfg.constraints.param2 = (float)xtmp.get(1).asFloat64();
1612 bool Parser::parseTimeoutsGroup(yarp::os::Searchable &config, std::vector<timeouts_t> &timeouts,
int defaultVelocityTimeout)
1614 if(!checkAndSetVectorSize(timeouts, _njoints,
"parseTimeoutsGroup"))
1619 Bottle timeoutsGroup =config.findGroup(
"TIMEOUTS");
1620 if(timeoutsGroup.isNull())
1622 yError() <<
"embObjMC BOARD " << _boardname <<
" no TIMEOUTS group found in config file.";
1628 if (!
extractGroup(timeoutsGroup, xtmp,
"velocity",
"a list of timeout to be used in the vmo control", _njoints))
1630 yError() <<
"embObjMC BOARD " << _boardname <<
" no velocity parameter found in TIMEOUTS group in motion control config file.";
1635 for(i=1; i<xtmp.size(); i++)
1636 timeouts[i-1].velocity = xtmp.get(i).asInt32();
1644 bool Parser::parseCurrentLimits(yarp::os::Searchable &config, std::vector<motorCurrentLimits_t> &currLimits)
1646 Bottle &limits=config.findGroup(
"LIMITS");
1647 if (limits.isNull())
1649 yError() <<
"embObjMC BOARD " << _boardname <<
" detected that Group LIMITS is not found in configuration file";
1653 currLimits.resize(_njoints);
1658 if (!
extractGroup(limits, xtmp,
"motorOverloadCurrents",
"a list of current limits", _njoints))
1661 for(i=1; i<xtmp.size(); i++) currLimits[i-1].overloadCurrent=xtmp.get(i).asFloat64();
1664 if (!
extractGroup(limits, xtmp,
"motorNominalCurrents",
"a list of nominal current limits", _njoints))
1667 for(i=1; i<xtmp.size(); i++) currLimits[i-1].nominalCurrent =xtmp.get(i).asFloat64();
1670 if (!
extractGroup(limits, xtmp,
"motorPeakCurrents",
"a list of peak current limits", _njoints))
1673 for(i=1; i<xtmp.size(); i++) currLimits[i-1].peakCurrent=xtmp.get(i).asFloat64();
1679 bool Parser::parseJointsLimits(yarp::os::Searchable &config, std::vector<jointLimits_t> &jointsLimits)
1681 Bottle &limits=config.findGroup(
"LIMITS");
1682 if (limits.isNull())
1684 yError() <<
"embObjMC BOARD " << _boardname <<
" detected that Group LIMITS is not found in configuration file";
1688 jointsLimits.resize(_njoints);
1693 if (!
extractGroup(limits, xtmp,
"jntPosMax",
"a list of user maximum angles (in degrees)", _njoints))
1696 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posMax = xtmp.get(i).asFloat64();
1699 if (!
extractGroup(limits, xtmp,
"jntPosMin",
"a list of user minimum angles (in degrees)", _njoints))
1702 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posMin = xtmp.get(i).asFloat64();
1705 if (!
extractGroup(limits, xtmp,
"hardwareJntPosMax",
"a list of hardware maximum angles (in degrees)", _njoints))
1709 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posHwMax = xtmp.get(i).asFloat64();
1712 for(i=0; i<(unsigned)_njoints; i++)
1714 if(jointsLimits[i].posMax > jointsLimits[i].posHwMax)
1716 yError() <<
"embObjMotionControl: user has set a limit bigger then hardware limit!. Please check jntPosMax.";
1723 if (!
extractGroup(limits, xtmp,
"hardwareJntPosMin",
"a list of hardware minimum angles (in degrees)", _njoints))
1729 for(i=1; i<xtmp.size(); i++) jointsLimits[i-1].posHwMin = xtmp.get(i).asFloat64();
1732 for(i=0; i<(unsigned)_njoints; i++)
1734 if(jointsLimits[i].posMin < jointsLimits[i].posHwMin)
1736 yError() <<
"embObjMotionControl: user has set a limit bigger then hardware limit!. Please check jntPosMin.";
1744 if (!
extractGroup(limits, xtmp,
"jntVelMax",
"a list of maximum velocities for the joints (in degrees/s)", _njoints))
1747 for (i = 1; i<xtmp.size(); i++) jointsLimits[i - 1].velMax = xtmp.get(i).asFloat64();
1752 bool Parser::parseRotorsLimits(yarp::os::Searchable &config, std::vector<rotorLimits_t> &rotorsLimits)
1754 Bottle &limits=config.findGroup(
"LIMITS");
1755 if (limits.isNull())
1757 yError() <<
"embObjMC BOARD " << _boardname <<
" detected that Group LIMITS is not found in configuration file";
1761 if(!checkAndSetVectorSize(rotorsLimits, _njoints,
"parseRotorsLimits"))
1768 if (!
extractGroup(limits, xtmp,
"rotorPosMax",
"a list of maximum rotor angles (in degrees)", _njoints))
1771 for(i=1; i<xtmp.size(); i++) rotorsLimits[i-1].posMax = xtmp.get(i).asFloat64();
1776 if (!
extractGroup(limits, xtmp,
"rotorPosMin",
"a list of minimum roto angles (in degrees)", _njoints))
1779 for(i=1; i<xtmp.size(); i++) rotorsLimits[i-1].posMin = xtmp.get(i).asFloat64();
1782 if (!
extractGroup(limits, xtmp,
"motorPwmLimit",
"a list of motor PWM limits", _njoints))
1785 for(i=1; i<xtmp.size(); i++)
1787 rotorsLimits[i-1].pwmMax = xtmp.get(i).asFloat64();
1788 if(rotorsLimits[i-1].pwmMax<0)
1790 yError() <<
"motorPwmLimit should be a positive value";
1799 bool Parser::parseCouplingInfo(yarp::os::Searchable &config,
couplingInfo_t &couplingInfo)
1801 Bottle coupling_bottle = config.findGroup(
"COUPLINGS");
1802 if (coupling_bottle.isNull())
1804 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing Coupling group";
1808 int fixedMatrix4X4Size = 16;
1809 int fixedMatrix4X6Size = 24;
1810 bool formaterror =
false;
1813 if (!
extractGroup(coupling_bottle, xtmp,
"matrixJ2M",
"matrixJ2M ", fixedMatrix4X4Size))
1818 if(
false ==
convert(xtmp, couplingInfo.
matrixJ2M, formaterror, fixedMatrix4X4Size))
1820 yError() <<
"embObjMC BOARD " << _boardname <<
" has detected an illegal format for some of the values of CONTROLLER.matrixJ2M";
1826 if (!
extractGroup(coupling_bottle, xtmp,
"matrixE2J",
"matrixE2J ", fixedMatrix4X6Size))
1831 formaterror =
false;
1832 if(
false ==
convert(xtmp, couplingInfo.
matrixE2J, formaterror, fixedMatrix4X6Size))
1834 yError() <<
"embObjMC BOARD " << _boardname <<
" has detected an illegal format for some of the values of CONTROLLER.matrixE2J";
1840 if (!
extractGroup(coupling_bottle, xtmp,
"matrixM2J",
"matrixM2J ", fixedMatrix4X4Size))
1845 formaterror =
false;
1846 if(
false ==
convert(xtmp, couplingInfo.
matrixM2J, formaterror, fixedMatrix4X4Size))
1848 yError() <<
"embObjMC BOARD " << _boardname <<
" has detected an illegal format for some of the values of CONTROLLER.matrixM2J";
1855 bool Parser::parseMotioncontrolVersion(yarp::os::Searchable &config,
int &version)
1857 if (!config.findGroup(
"GENERAL").find(
"MotioncontrolVersion").isInt32())
1859 yError() <<
"Missing MotioncontrolVersion parameter. RobotInterface cannot start. Please contact icub-support@iit.it";
1863 version = config.findGroup(
"GENERAL").find(
"MotioncontrolVersion").asInt32();
1868 bool Parser::isVerboseEnabled(yarp::os::Searchable &config)
1871 if(!config.findGroup(
"GENERAL").find(
"verbose").isBool())
1873 yError() <<
"embObjMotionControl::open() detects that general->verbose bool param is different from accepted values (true / false). Assuming false";
1878 ret = config.findGroup(
"GENERAL").find(
"verbose").asBool();
1880 _verbosewhenok = ret;
1884 bool Parser::parseBehaviourFalgs(yarp::os::Searchable &config,
bool &useRawEncoderData,
bool &pwmIsLimited )
1888 Value use_raw = config.findGroup(
"GENERAL").find(
"useRawEncoderData");
1890 if(use_raw.isNull())
1892 useRawEncoderData =
false;
1896 if(!use_raw.isBool())
1898 yWarning() <<
"embObjMotionControl::open() detected that useRawEncoderData bool param is different from accepted values (true / false). Assuming false";
1899 useRawEncoderData =
false;
1903 useRawEncoderData = use_raw.asBool();
1904 if(useRawEncoderData)
1906 yWarning() <<
"embObjMotionControl::open() detected that it is using raw data from encoders! Be careful See 'useRawEncoderData' param in config file";
1907 yWarning() <<
"DO NOT USE OR CALIBRATE THE ROBOT IN THIS CONFIGURATION!";
1908 yWarning() <<
"CHECK IF THE FAULT BUTTON IS PRESSED and press ENTER to continue";
1915 Value use_limitedPWM = config.findGroup(
"GENERAL").find(
"useLimitedPWM");
1916 if(use_limitedPWM.isNull())
1918 pwmIsLimited =
false;
1922 if(!use_limitedPWM.isBool())
1924 pwmIsLimited =
false;
1928 pwmIsLimited = use_limitedPWM.asBool();
1935 bool Parser::parseAxisInfo(yarp::os::Searchable &config,
int axisMap[], std::vector<axisInfo_t> &axisInfo)
1940 axisInfo.resize(_njoints);
1942 Bottle general = config.findGroup(
"GENERAL");
1943 if (general.isNull())
1945 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing General group" ;
1950 if (!
extractGroup(general, xtmp,
"AxisMap",
"a list of reordered indices for the axes", _njoints))
1953 for (i = 1; i < xtmp.size(); i++)
1955 int user_joint = xtmp.get(i).asInt32();
1956 if(user_joint>= _njoints)
1958 yError() <<
"embObjMC BOARD " << _boardname <<
"In AxisMap param: joint " << i-1 <<
"has been mapped to not-existing joint ("<< user_joint <<
"). Here there are only "<< _njoints <<
"joints";
1961 axisMap[i-1] = user_joint;
1965 if (!
extractGroup(general, xtmp,
"AxisName",
"a list of strings representing the axes names", _njoints))
1969 for (i = 1; i < xtmp.size(); i++)
1971 int mappedto = axisInfo[i-1].mappedto;
1972 axisInfo[axisMap[i - 1]].name = xtmp.get(i).asString();
1975 if (!
extractGroup(general, xtmp,
"AxisType",
"a list of strings representing the axes type (revolute/prismatic)", _njoints))
1979 for (i = 1; i < xtmp.size(); i++)
1981 string s = xtmp.get(i).asString();
1982 int mappedto = axisInfo[i-1].mappedto;
1983 if (s ==
"revolute") axisInfo[axisMap[i - 1]].type = VOCAB_JOINTTYPE_REVOLUTE;
1984 else if (s ==
"prismatic") axisInfo[axisMap[i - 1]].type = VOCAB_JOINTTYPE_PRISMATIC;
1987 yError(
"Unknown AxisType value %s!", s.c_str());
1988 axisInfo[axisMap[i - 1]].type = VOCAB_JOINTTYPE_UNKNOWN;
1996 bool Parser::parseEncoderFactor(yarp::os::Searchable &config,
double encoderFactor[])
1998 Bottle general = config.findGroup(
"GENERAL");
1999 if (general.isNull())
2001 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing General group" ;
2009 if (!
extractGroup(general, xtmp,
"Encoder",
"a list of scales for the encoders", _njoints))
2014 for (i = 1; i < xtmp.size(); i++)
2016 tmp_A2E = xtmp.get(i).asFloat64();
2019 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Encoder parameter should be positive!";
2021 encoderFactor[i - 1] = tmp_A2E;
2027 bool Parser::parsefullscalePWM(yarp::os::Searchable &config,
double dutycycleToPWM[])
2029 Bottle general = config.findGroup(
"GENERAL");
2030 if (general.isNull())
2032 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing General group";
2040 if (!
extractGroup(general, xtmp,
"fullscalePWM",
"a list of scales for the fullscalePWM conversion factor", _njoints))
2042 yError(
"fullscalePWM param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
2046 for (i = 1; i < xtmp.size(); i++)
2048 tmpval = xtmp.get(i).asFloat64();
2051 yError() <<
"embObjMC BOARD " << _boardname <<
"fullscalePWM parameter should be positive!";
2054 dutycycleToPWM[i - 1] = tmpval / 100.0;
2060 bool Parser::parseAmpsToSensor(yarp::os::Searchable &config,
double ampsToSensor[])
2062 Bottle general = config.findGroup(
"GENERAL");
2063 if (general.isNull())
2065 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing General group";
2073 if (!
extractGroup(general, xtmp,
"ampsToSensor",
"a list of scales for the ampsToSensor conversion factor", _njoints))
2075 yError(
"ampsToSensor param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
2079 for (i = 1; i < xtmp.size(); i++)
2081 tmpval = xtmp.get(i).asFloat64();
2084 yError() <<
"embObjMC BOARD " << _boardname <<
"ampsToSensor parameter should be positive!";
2087 ampsToSensor[i - 1] = tmpval;
2093 bool Parser::parseGearboxValues(yarp::os::Searchable &config,
double gearbox_M2J[],
double gearbox_E2J[])
2095 Bottle general = config.findGroup(
"GENERAL");
2096 if (general.isNull())
2098 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing General group" ;
2106 if (!
extractGroup(general, xtmp,
"Gearbox_M2J",
"The gearbox reduction ratio", _njoints))
2111 for (i = 1; i < xtmp.size(); i++)
2113 gearbox_M2J[i-1] = xtmp.get(i).asFloat64();
2114 if (gearbox_M2J[i-1]==0)
2116 yError() <<
"embObjMC BOARD " << _boardname <<
"Using a gearbox value = 0 may cause problems! Check your configuration files";
2123 if (!
extractGroup(general, xtmp,
"Gearbox_E2J",
"The gearbox reduction ratio between encoder and joint", _njoints))
2128 int test = xtmp.size();
2129 for (i = 1; i < xtmp.size(); i++)
2131 gearbox_E2J[i-1] = xtmp.get(i).asFloat64();
2132 if (gearbox_E2J[i-1]==0)
2134 yError() <<
"embObjMC BOARD " << _boardname <<
"Using a gearbox value = 0 may cause problems! Check your configuration files";
2143 bool Parser::parseDeadzoneValue(yarp::os::Searchable &config,
double deadzone[],
bool *found)
2152 Bottle general = config.findGroup(
"OTHER_CONTROL_PARAMETERS");
2153 if (general.isNull())
2155 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing OTHER_CONTROL_PARAMETERS.DeadZone parameter. I'll use default value. (see documentation for more datails)";
2163 if (!
extractGroup(general, xtmp,
"deadZone",
"The deadzone of joint", _njoints,
false))
2165 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing OTHER_CONTROL_PARAMETERS group.DeadZone parameter. I'll use default value. (see documentation for more datails)";
2171 for (i = 1; i < xtmp.size(); i++)
2173 deadzone[i-1] = xtmp.get(i).asFloat64();
2179 bool Parser::parseKalmanFilterParams(yarp::os::Searchable &config, std::vector<kalmanFilterParams_t> &kalmanFilterParams)
2181 Bottle general = config.findGroup(
"KALMAN_FILTER");
2182 if (general.isNull())
2184 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing KALMAN_FILTER group. Kalman Filter will be disabled by default.";
2187 for(
int j=0; j<_njoints; j++)
2189 kalmanFilterParams[j].enabled =
false;
2190 kalmanFilterParams[j].x0.fill(0.0);
2191 kalmanFilterParams[j].Q.fill(0.0);
2192 kalmanFilterParams[j].R = 0.0;
2193 kalmanFilterParams[j].P0 = 0.0;
2202 if (!
extractGroup(general, xtmp,
"kalmanFilterEnabled",
"kalman filter of joint: ", _njoints,
true))
2204 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing kalmanFilterEnabled parameter. It will be disabled by default.";
2207 for (
int j = 0; j<_njoints; j++) kalmanFilterParams[j].enabled = xtmp.get(j + 1).asBool();
2210 if (!
extractGroup(general, xtmp,
"x0",
"Initial state x0 of kalman filter of joint: ", _njoints,
true))
2212 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing initial state x0 parameter. Kalman Filter will be disabled by default.";
2215 for (
int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(0) = xtmp.get(j + 1).asFloat32();
2218 if (!
extractGroup(general, xtmp,
"x1",
"Initial state x1 of kalman filter of joint: ", _njoints,
true))
2220 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing initial state x1 parameter. Kalman Filter will be disabled by default.";
2223 for (
int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(1) = xtmp.get(j + 1).asFloat32();
2226 if (!
extractGroup(general, xtmp,
"x2",
"Initial state x2 of kalman filter of joint: ", _njoints,
true))
2228 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing initial state x2 parameter. Kalman Filter will be disabled by default.";
2231 for (
int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(2) = xtmp.get(j + 1).asFloat32();
2234 if (!
extractGroup(general, xtmp,
"Q0",
"Process Q0 noise covariance matrix of kalman filter of joint: ", _njoints,
true))
2236 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing Q0 parameter. Kalman Filter will be disabled by default.";
2239 for (
int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(0) = xtmp.get(j + 1).asFloat32();
2242 if (!
extractGroup(general, xtmp,
"Q1",
"Process Q1 noise covariance matrix of kalman filter of joint: ", _njoints,
true))
2244 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing Q1 parameter. Kalman Filter will be disabled by default.";
2247 for (
int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(1) = xtmp.get(j + 1).asFloat32();
2250 if (!
extractGroup(general, xtmp,
"Q2",
"Process Q2 noise covariance matrix of kalman filter of joint: ", _njoints,
true))
2252 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing Q2 parameter. Kalman Filter will be disabled by default.";
2255 for (
int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(2) = xtmp.get(j + 1).asFloat32();
2258 if (!
extractGroup(general, xtmp,
"R",
"Measurement noise covariance matrix of kalman filter for joint: ", _njoints,
true))
2260 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing R scalar parameter. Kalman Filter will be disabled by default.";
2263 for (
int j = 0; j<_njoints; j++) kalmanFilterParams[j].R = xtmp.get(j + 1).asFloat32();
2266 if (!
extractGroup(general, xtmp,
"P0",
"Initial state estimation error covariance matrix of kalman filter of joint: ", _njoints,
true))
2268 yWarning() <<
"embObjMC BOARD " << _boardname <<
"Missing P0 scalar parameter. Kalman Filter will be disabled by default.";
2271 for (
int j = 0; j<_njoints; j++) kalmanFilterParams[j].P0 = xtmp.get(j + 1).asFloat32();
2278 bool Parser::parseMechanicalsFlags(yarp::os::Searchable &config,
int useMotorSpeedFbk[])
2280 Bottle general = config.findGroup(
"GENERAL");
2281 if (general.isNull())
2283 yError() <<
"embObjMC BOARD " << _boardname <<
"Missing General group" ;
2289 if(!
extractGroup(general, xtmp,
"useMotorSpeedFbk",
"Use motor speed feedback", _njoints))
2294 for (i = 1; i < xtmp.size(); i++)
2296 useMotorSpeedFbk[i-1] = xtmp.get(i).asInt32();
2303 bool Parser::parseImpedanceGroup(yarp::os::Searchable &config,std::vector<impedanceParameters_t> &impedance)
2305 Bottle impedanceGroup;
2306 impedanceGroup=config.findGroup(
"IMPEDANCE",
"IMPEDANCE parameters");
2308 if(impedanceGroup.isNull())
2310 yError() <<
"embObjMC BOARD " << _boardname <<
"fromConfig(): Error: no IMPEDANCE group found in config file, returning";
2318 yDebug() <<
"embObjMC BOARD " << _boardname <<
":fromConfig() detected that IMPEDANCE parameters section is found";
2321 if(!checkAndSetVectorSize(impedance, _njoints,
"parseImpedanceGroup"))
2327 if (!
extractGroup(impedanceGroup, xtmp,
"stiffness",
"stiffness parameter", _njoints))
2330 for (j=0; j<_njoints; j++)
2331 impedance[j].stiffness = xtmp.get(j+1).asFloat64();
2333 if (!
extractGroup(impedanceGroup, xtmp,
"damping",
"damping parameter", _njoints))
2336 for (j=0; j<_njoints; j++)
2337 impedance[j].damping = xtmp.get(j+1).asFloat64();
2341 yInfo() <<
"embObjMC BOARD " << _boardname <<
"IMPEDANCE section: parameters successfully loaded";
2347 bool Parser::convert(std::string
const &fromstring, eOmc_jsetconstraint_t &jsetconstraint,
bool& formaterror)
2349 const char *t = fromstring.c_str();
2351 eObool_t usecompactstring = eobool_false;
2352 jsetconstraint = eomc_string2jsetconstraint(t, usecompactstring);
2354 if(eomc_jsetconstraint_unknown == jsetconstraint)
2356 usecompactstring = eobool_true;
2357 jsetconstraint = eomc_string2jsetconstraint(t, usecompactstring);
2360 if(eomc_jsetconstraint_unknown == jsetconstraint)
2362 yError() <<
"embObjMC BOARD " << _boardname <<
"String" << t <<
"cannot be converted into a proper eOmc_jsetconstraint_t";
2370 bool Parser::convert(Bottle &bottle, vector<double> &matrix,
bool &formaterror,
int targetsize)
2374 int tmp = bottle.size();
2375 int sizeofmatrix =
tmp - 1;
2378 if(targetsize != sizeofmatrix)
2380 yError() <<
"embObjMC BOARD " << _boardname <<
" in converting string do matrix.In the matrix there are not" << targetsize <<
"elements";
2384 formaterror =
false;
2385 for(
int i=0; i<sizeofmatrix; i++)
2390 item = bottle.get(i+1).asFloat64();
2391 matrix.push_back(item);
2402 void Parser::debugUtil_printControlLaws(
void)
2405 yError() <<
"position control law: ";
2406 for(
int x=0;
x<_njoints;
x++)
2408 yError() <<
" - j " <<
x << _positionControlLaw[
x].c_str();
2411 yError() <<
"velocity control law: ";
2412 for(
int x=0;
x<_njoints;
x++)
2414 yError() <<
"- j " <<
x << _velocityControlLaw[
x].c_str();
2418 yError() <<
"torque control law: ";
2419 for(
int x=0;
x<_njoints;
x++)
2421 yError() <<
" - j " <<
x << _torqueControlLaw[
x].c_str();
2458 void JointsSet::dumpdata(
void)
2460 switch(cfg.constraints.type)
2462 case eomc_jsetconstraint_none:
2463 cout <<
"constraint is " <<
"eomc_jsetconstraint_none";
2465 case eomc_jsetconstraint_cerhand:
2466 cout <<
"constraint is " <<
"eomc_jsetconstraint_cerhand";
2468 case eomc_jsetconstraint_trifid:
2469 cout <<
"constraint is " <<
"eomc_jsetconstraint_trifid";
2472 cout <<
". constraint is " <<
"unknown";
2475 cout <<
" param1="<< cfg.constraints.param1 <<
" param2=" << cfg.constraints.param2 << endl;
2479 bool Parser::checkJointTypes(
PidInfo *pids,
const std::string &pid_type)
2483 int firstjoint = -1;
2486 if(pid_type ==
"TORQUE")
2492 for(
int i=0; i<_njoints; i++)
2495 if(firstjoint != -1 && !checkSinglePid(trq_pids[firstjoint], trq_pids[i], firstjoint, i, pid_type))
2500 if(firstjoint == -1 && trq_pids[i].enabled)
2508 for(
int i=0; i<_njoints; i++)
2511 if(firstjoint != -1 && !checkSinglePid(pids[firstjoint], pids[i], firstjoint, i, pid_type))
2516 if(firstjoint == -1 && pids[i].enabled)
2526 bool Parser::checkSinglePid(
PidInfo &firstPid,
PidInfo ¤tPid,
const int &firstjoint,
const int ¤tjoint,
const std::string &pid_type)
2535 yError() <<
"embObjMC BOARD " << _boardname <<
"all joints with " << pid_type <<
" enabled should have same controlunits type. Joint " << firstjoint <<
" differs from joint " << currentjoint;
Parser(int numofjoints, std::string boardname)
std::string usernamePidSelected
yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits
eOmc_ctrl_out_type_t out_type
yarp::dev::PidOutputUnitsEnum out_PidUnits
yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits
void setUnits(yarp::dev::PidFeedbackUnitsEnum fu, yarp::dev::PidOutputUnitsEnum ou)
eOmc_ctrl_out_type_t out_type
yarp::dev::PidOutputUnitsEnum out_PidUnits
virtual yarp::dev::Pid getPID(int joint)=0
double viscousPos
motor torque constant
double ktau
back-emf compensation parameter
#define LOAD_STRINGS(dest, source)
Gain convert(const DiscreteGain dg)
static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
std::vector< double > matrixM2J
std::vector< double > matrixE2J
std::vector< double > matrixJ2M