11 #include <yarp/os/Bottle.h>
12 #include <yarp/os/Time.h>
18 #include <FeatureInterface.h>
19 #include <yarp/conf/environment.h>
21 #include <yarp/os/LogStream.h>
25 #include "EoProtocol.h"
26 #include "EoProtocolMN.h"
27 #include "EoProtocolMC.h"
28 #include "EoProtocolAS.h"
31 #include <yarp/os/NetType.h>
32 #include <yarp/dev/ControlBoardHelper.h>
38 using namespace yarp::os;
39 using namespace yarp::os::impl;
47 #define ASK_REFERENCE_TO_FIRMWARE 1
49 #define PARSER_MOTION_CONTROL_VERSION 6
57 yError() << txt <<
" is not yet implemented for embObjMotionControl";
63 yError() << txt <<
" has been deprecated for embObjMotionControl";
67 #define NV_NOT_FOUND return nv_not_found();
71 yError () <<
" nv_not_found!! This may mean that this variable is not handled by this EMS\n";
83 std::string embObjMotionControl::getBoardInfo(
void)
87 return " BOARD name_unknown (IP unknown) ";
91 return (
"BOARD " + res->getProperties().boardnameString +
" (IP " + res->getProperties().ipv4addrString +
") ");
96 bool embObjMotionControl::alloc(
int nj)
98 _axisMap = allocAndCheck<int>(nj);
100 _encodersStamp = allocAndCheck<double>(nj);
101 _gearbox_M2J = allocAndCheck<double>(nj);
102 _gearbox_E2J = allocAndCheck<double>(nj);
103 _deadzone = allocAndCheck<double>(nj);
104 _foc_based_info=allocAndCheck<eomc::focBasedSpecificInfo_t>(nj);
110 _impedance_limits=allocAndCheck<eomc::impedanceLimits_t>(nj);
111 checking_motiondone=allocAndCheck<bool>(nj);
112 _last_position_move_time=allocAndCheck<double>(nj);
115 _ref_command_positions = allocAndCheck<double>(nj);
116 _ref_positions = allocAndCheck<double>(nj);
117 _ref_command_speeds = allocAndCheck<double>(nj);
118 _ref_speeds = allocAndCheck<double>(nj);
119 _ref_accs = allocAndCheck<double>(nj);
121 _enabledAmp = allocAndCheck<bool>(nj);
122 _enabledPid = allocAndCheck<bool>(nj);
123 _calibrated = allocAndCheck<bool>(nj);
124 _cacheImpedance = allocAndCheck<eOmc_impedance_t>(nj);
127 _rotorsLimits.resize(nj);
128 _jointsLimits.resize(nj);
129 _currentLimits.resize(nj);
131 _joint2set.resize(nj);
132 _timeouts.resize(nj);
133 _impedance_params.resize(nj);
134 _axesInfo.resize(nj);
135 _jointEncs.resize(nj);
136 _motorEncs.resize(nj);
137 _kalman_params.resize(nj);
144 bool embObjMotionControl::dealloc()
146 checkAndDestroy(_axisMap);
147 checkAndDestroy(_encodersStamp);
148 checkAndDestroy(_gearbox_M2J);
149 checkAndDestroy(_gearbox_E2J);
150 checkAndDestroy(_deadzone);
151 checkAndDestroy(_impedance_limits);
152 checkAndDestroy(checking_motiondone);
153 checkAndDestroy(_ref_command_positions);
154 checkAndDestroy(_ref_positions);
155 checkAndDestroy(_ref_command_speeds);
156 checkAndDestroy(_ref_speeds);
157 checkAndDestroy(_ref_accs);
159 checkAndDestroy(_enabledAmp);
160 checkAndDestroy(_enabledPid);
161 checkAndDestroy(_calibrated);
162 checkAndDestroy(_foc_based_info);
185 ImplementControlCalibration(this),
186 ImplementAmplifierControl(this),
187 ImplementPidControl(this),
188 ImplementEncodersTimed(this),
189 ImplementPositionControl(this),
190 ImplementVelocityControl(this),
191 ImplementControlMode(this),
192 ImplementImpedanceControl(this),
193 ImplementMotorEncoders(this),
194 #ifdef IMPLEMENT_DEBUG_INTERFACE
197 ImplementTorqueControl(this),
198 ImplementControlLimits(this),
199 ImplementPositionDirect(this),
200 ImplementInteractionMode(this),
201 ImplementMotor(this),
202 ImplementRemoteVariables(this),
203 ImplementAxisInfo(this),
204 ImplementPWMControl(this),
205 ImplementCurrentControl(this),
206 ImplementJointFault(this),
207 SAFETY_THRESHOLD(2.0),
214 _impedance_params(0),
232 _encodersStamp = NULL;
233 _foc_based_info = NULL;
234 _cacheImpedance = NULL;
235 _impedance_limits = NULL;
237 _ref_command_speeds = NULL;
238 _ref_command_positions= NULL;
239 _ref_positions = NULL;
241 _measureConverter = NULL;
243 checking_motiondone = NULL;
254 _last_position_move_time = NULL;
259 std::string
tmp = yarp::conf::environment::get_string(
"ETH_VERBOSEWHENOK");
271 #ifdef NETWORK_PERFORMANCE_BENCHMARK
275 m_responseTimingVerifier.init(0.003, 0.00025, 0.001, 0.01, 0.0005, 30);
283 yTrace() <<
"embObjMotionControl::~embObjMotionControl()";
291 if(NULL != _mcparser)
309 ImplementControlCalibration::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
310 ImplementAmplifierControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.ampsToSensor);
311 ImplementEncodersTimed::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
312 ImplementMotorEncoders::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
313 ImplementPositionControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
314 ImplementPidControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.newtonsToSensor,
f.ampsToSensor,
f.dutycycleToPWM);
315 ImplementControlMode::initialize(_njoints, _axisMap);
316 ImplementVelocityControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
317 ImplementControlLimits::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
318 ImplementImpedanceControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.newtonsToSensor);
319 ImplementTorqueControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.newtonsToSensor,
f.ampsToSensor,
f.dutycycleToPWM,
f.bemf2raw,
f.ktau2raw);
320 ImplementPositionDirect::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
321 ImplementInteractionMode::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
322 ImplementMotor::initialize(_njoints, _axisMap);
323 ImplementRemoteVariables::initialize(_njoints, _axisMap);
324 ImplementAxisInfo::initialize(_njoints, _axisMap);
325 ImplementCurrentControl::initialize(_njoints, _axisMap,
f.ampsToSensor);
326 ImplementPWMControl::initialize(_njoints, _axisMap,
f.dutycycleToPWM);
327 ImplementJointFault::initialize(_njoints, _axisMap);
338 if(NULL == ethManager)
340 yFatal() <<
"embObjMotionControl::open() fails to instantiate ethManager";
344 eOipv4addr_t ipv4addr;
345 string boardIPstring;
347 if(
false == ethManager->
verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
349 yError() <<
"embObjMotionControl::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
368 yError() <<
"embObjMotionControl::open() fails because could not instantiate the ethResource for " << getBoardInfo() <<
" ... unable to continue";
372 if(!fromConfig(config))
374 yError() << getBoardInfo() <<
"Missing motion control parameters in config file";
380 yError() <<
"embObjMotionControl: failed verifyEPprotocol. Cannot continue!";
386 const eOmn_serv_parameter_t* servparam = &serviceConfig.
ethservice;
387 if(eomn_serv_MC_generic == serviceConfig.
ethservice.configuration.type)
393 mcdiagnostics.
config.mode = serviceConfig.
ethservice.configuration.diagnosticsmode;
394 mcdiagnostics.
config.par16 = serviceConfig.
ethservice.configuration.diagnosticsparam;
395 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
398 mcdiagnostics.
ports.resize(2);
399 for(
size_t i=0; i<mcdiagnostics.
ports.size(); i++)
401 mcdiagnostics.
ports[i] =
new BufferedPort<Bottle>;
412 event_downsampler->
config.
info = getBoardInfo();
413 event_downsampler->
start();
417 yError() <<
"embObjMotionControl::open() has an error in call of ethResources::serviceVerifyActivate() for" << getBoardInfo();
422 yDebug() <<
"embObjMotionControl:serviceVerifyActivate OK!";
427 yError() <<
"embObjMotionControl::open() has an error in call of embObjMotionControl::init() for" << getBoardInfo();
434 yDebug() <<
"embObjMotionControl::init() has succesfully initted" << getBoardInfo();
441 yError() <<
"embObjMotionControl::open() fails to start mc service for" << getBoardInfo() <<
": cannot continue";
449 yDebug() <<
"embObjMotionControl::open() correctly starts mc service of" << getBoardInfo();
457 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
488 SystemClock::delaySystem(0.001*mcdiagnostics.
config.par16);
495 int embObjMotionControl::fromConfig_NumOfJoints(yarp::os::Searchable &config)
500 int jn = config.findGroup(
"GENERAL").check(
"Joints", Value(1),
"Number of degrees of freedom").asInt32();
507 void embObjMotionControl::debugUtil_printJointsetInfo(
void)
510 yError() <<
"****** DEBUG PRINTS **********";
511 yError() <<
"joint to set:";
512 for(
int x=0;
x< _njoints;
x++)
513 yError() <<
" /t j " <<
x <<
": set " <<_joint2set[
x];
514 yError() <<
"jointmap:";
516 yError() <<
" number of sets" << _jsets.size();
517 for(
size_t x=0;
x< _jsets.size();
x++)
519 yError() <<
"set " <<
x<<
"has size " <<_jsets[
x].getNumberofJoints();
520 for(
int y=0;
y<_jsets[
x].getNumberofJoints();
y++)
521 yError() <<
"set " <<
x <<
": " << _jsets[
x].joints[
y];
523 yError() <<
"********* END ****************";
530 bool embObjMotionControl::verifyUserControlLawConsistencyInJointSet(
eomc::PidInfo *pidInfo)
533 for(
size_t s=0; s<_jsets.size(); s++)
535 int numofjoints = _jsets[s].getNumberofJoints();
539 yError() <<
"embObjMC" << getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
542 int firstjoint = _jsets[s].joints[0];
544 for(
int k=1; k<numofjoints; k++)
546 int otherjoint = _jsets[s].joints[k];
548 if(pidInfo[firstjoint].usernamePidSelected != pidInfo[otherjoint].usernamePidSelected)
550 yError() <<
"embObjMC "<< getBoardInfo() <<
"Joints beloning to same set must be have same control law. Joint " << otherjoint <<
" differs from " << firstjoint <<
"Set num " << s ;
563 bool embObjMotionControl::verifyUserControlLawConsistencyInJointSet(
eomc::TrqPidInfo *pidInfo)
565 for(
size_t s=0; s<_jsets.size(); s++)
567 int numofjoints = _jsets[s].getNumberofJoints();
571 yError() <<
"embObjMC "<< getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
574 int firstjoint = _jsets[s].joints[0];
576 for(
int k=1; k<numofjoints; k++)
578 int otherjoint = _jsets[s].joints[k];
580 if(pidInfo[firstjoint].usernamePidSelected != pidInfo[otherjoint].usernamePidSelected)
582 yError() <<
"embObjMC"<< getBoardInfo() <<
"Joints beloning to same set must be have same control law. Joint " << otherjoint <<
" differs from " << firstjoint <<
"Set num " << s ;
592 bool embObjMotionControl::updatedJointsetsCfgWithControlInfo()
596 for(
size_t s=0; s<_jsets.size(); s++)
598 if(_jsets[s].getNumberofJoints() == 0)
600 yError() <<
"embObjMC"<< getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
604 int joint = _jsets[s].joints[0];
614 _jsets[s].cfg.pid_output_types.postrj_ctrl_out_type = _trj_pids[joint].
out_type;
615 _jsets[s].cfg.pid_output_types.veltrj_ctrl_out_type = _trj_pids[joint].
out_type;
616 _jsets[s].cfg.pid_output_types.mixtrj_ctrl_out_type = _trj_pids[joint].
out_type;
620 _jsets[s].cfg.pid_output_types.posdir_ctrl_out_type = _trj_pids[joint].
out_type;
621 _jsets[s].cfg.pid_output_types.veldir_ctrl_out_type = _trj_pids[joint].
out_type;
623 _jsets[s].cfg.pid_output_types.torque_ctrl_out_type = _trq_pids[joint].
out_type;
625 _jsets[s].cfg.pid_output_types.pwm_ctrl_out_type = eomc_ctrl_out_type_pwm;
627 if (_cur_pids[joint].enabled)
629 _jsets[s].cfg.pid_output_types.cur_ctrl_out_type = eomc_ctrl_out_type_cur;
633 _jsets[s].cfg.pid_output_types.cur_ctrl_out_type = eomc_ctrl_out_type_n_a;
642 bool embObjMotionControl::saveCouplingsData(
void)
644 eOmc_4jomo_coupling_t *jc_dest;
646 static eOmc_4jomo_coupling_t dummyjomocoupling = {};
648 switch(serviceConfig.
ethservice.configuration.type)
650 case eomn_serv_MC_foc:
652 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.foc_based.jomocoupling);
655 case eomn_serv_MC_mc4plus:
657 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc4plus_based.jomocoupling);
660 case eomn_serv_MC_mc4plusmais:
662 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc4plusmais_based.jomocoupling);
665 case eomn_serv_MC_mc2pluspsc:
667 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc2pluspsc.jomocoupling);
670 case eomn_serv_MC_mc4plusfaps:
672 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc4plusfaps.jomocoupling);
675 case eomn_serv_MC_mc4pluspmc:
677 jc_dest = &dummyjomocoupling;
680 case eomn_serv_MC_mc4:
685 case eomn_serv_MC_generic:
697 if(eomn_serv_MC_mc4pluspmc == serviceConfig.
ethservice.configuration.type)
699 eOmc_arrayof_7jointsetconfig_t *arrayof7jset = &(serviceConfig.
ethservice.configuration.data.mc.mc4pluspmc.arrayof7jointsets);
700 EOarray *array = eo_array_New(7,
sizeof(eOmc_jointset_configuration_t), arrayof7jset);
704 for(
size_t s=0; s<4; s++)
706 eOmc_jointset_configuration_t* cfg_ptr = _jsets[s].getConfiguration();
707 eo_array_PushBack(array, cfg_ptr);
709 for(
size_t e=0;
e<3;
e++)
711 eOmc_jointset_configuration_t cfg = {0};
712 eo_array_PushBack(array, &cfg);
719 memset(jc_dest, 0,
sizeof(eOmc_4jomo_coupling_t));
723 for(
int i=0; i<4; i++)
725 jc_dest->joint2set[i] = eomc_jointSetNum_none;
728 if(_joint2set.size() > 4 )
730 yError() <<
"embObjMC "<< getBoardInfo() <<
"Jointsset size is bigger than 4. I can't send jointset information to fw.";
734 for(
size_t i=0; i<_joint2set.size(); i++)
736 jc_dest->joint2set[i] = _joint2set[i];
739 for(
int i=0; i<4; i++)
741 for(
int j=0; j<4; j++)
743 jc_dest->joint2motor[i][j] = eo_common_float_to_Q17_14((
float)_couplingInfo.
matrixJ2M[4*i+j]);
744 jc_dest->motor2joint[i][j] = eo_common_float_to_Q17_14((
float)_couplingInfo.
matrixM2J[4*i+j]);
749 for(
int r=0; r<4; r++)
751 for(
int c=0; c<6; c++)
753 jc_dest->encoder2joint[r][c] = eo_common_float_to_Q17_14((
float)_couplingInfo.
matrixE2J[6*r+c]);
757 for(
size_t s=0; s< _jsets.size(); s++)
759 eOmc_jointset_configuration_t* cfg_ptr = _jsets[s].getConfiguration();
760 memcpy(&(jc_dest->jsetcfg[s]), cfg_ptr,
sizeof(eOmc_jointset_configuration_t));
768 bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config)
775 if(iNeedCouplingsInfo())
801 for (i = 0; i < _njoints; i++)
803 measConvFactors.angleToEncoder[i] = 1;
824 if(eomn_serv_MC_mc4 != (eOmn_serv_type_t)serviceConfig.
ethservice.configuration.type)
826 int* useMotorSpeedFbk = 0;
827 useMotorSpeedFbk =
new int[_njoints];
830 delete[] useMotorSpeedFbk;
835 if (!verifyUseMotorSpeedFbkInJointSet(useMotorSpeedFbk))
837 delete[] useMotorSpeedFbk;
840 delete[] useMotorSpeedFbk;
842 bool deadzoneIsAvailable;
845 if(!deadzoneIsAvailable)
847 updateDeadZoneWithDefaultValues();
859 bool lowLevPidisMandatory =
false;
861 if(serviceConfig.
ethservice.configuration.type == eomn_serv_MC_foc)
863 lowLevPidisMandatory =
true;
866 if(!_mcparser->
parsePids(config, _trj_pids, _trq_pids, _cur_pids, _spd_pids, lowLevPidisMandatory))
883 updatedJointsetsCfgWithControlInfo();
886 for (i = 0; i < _njoints; i++)
888 measConvFactors.newtonsToSensor[i] = 1000000.0f;
890 measConvFactors.bemf2raw[i] = measConvFactors.newtonsToSensor[i] / measConvFactors.angleToEncoder[i];
891 if (_trq_pids->
out_PidUnits == yarp::dev::PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT)
893 measConvFactors.ktau2raw[i] = measConvFactors.dutycycleToPWM[i] / measConvFactors.newtonsToSensor[i];
895 else if (_trq_pids->
out_PidUnits == yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS)
897 measConvFactors.ktau2raw[i] = 1.0 / measConvFactors.newtonsToSensor[i];
901 yError() <<
"Invalid ktau units";
return false;
906 _measureConverter =
new ControlBoardHelper(_njoints, _axisMap, measConvFactors.angleToEncoder, NULL, measConvFactors.newtonsToSensor, measConvFactors.ampsToSensor,
nullptr, measConvFactors.dutycycleToPWM , measConvFactors.bemf2raw, measConvFactors.ktau2raw);
907 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, _trj_pids->
fbk_PidUnits, _trj_pids->
out_PidUnits);
909 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, _trq_pids->
fbk_PidUnits, _trq_pids->
out_PidUnits);
910 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, _cur_pids->
fbk_PidUnits, _cur_pids->
out_PidUnits);
911 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, _spd_pids->
fbk_PidUnits, _spd_pids->
out_PidUnits);
924 initializeInterfaces(measConvFactors);
925 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, _trj_pids->
fbk_PidUnits, _trj_pids->
out_PidUnits);
927 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, _trq_pids->
fbk_PidUnits, _trq_pids->
out_PidUnits);
928 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, _cur_pids->
fbk_PidUnits, _cur_pids->
out_PidUnits);
929 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, _spd_pids->
fbk_PidUnits, _spd_pids->
out_PidUnits);
933 if(!saveCouplingsData())
940 yError() <<
"embObjMC " << getBoardInfo() <<
"IMPEDANCE section: error detected in parameters syntax";
946 for(j=0; j<_njoints; j++)
949 _impedance_limits[j].
min_damp= 0.001;
950 _impedance_limits[j].
max_damp= 9.888;
953 _impedance_limits[j].
param_a= 0.011;
954 _impedance_limits[j].
param_b= 0.012;
955 _impedance_limits[j].
param_c= 0.013;
973 if(serviceConfig.
ethservice.configuration.type == eomn_serv_MC_foc)
975 std::string groupName = (
static_cast<eObrd_type_t
>(serviceConfig.
ethservice.configuration.data.mc.foc_based.type) == eobrd_foc) ?
"2FOC" :
"AMCBLDC";
976 if(!_mcparser->
parseFocGroup(config, _foc_based_info, groupName))
991 bool embObjMotionControl::verifyUseMotorSpeedFbkInJointSet(
int useMotorSpeedFbk [])
993 for(
size_t s=0; s< _jsets.size(); s++)
995 int numofjointsinset = _jsets[s].getNumberofJoints();
996 if(numofjointsinset == 0 )
998 yError() <<
"embObjMC " << getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
1002 int firstjointofset = _jsets[s].joints[0];
1003 for(
int j=1; j<numofjointsinset; j++)
1005 int joint = _jsets[s].joints[j];
1006 if(useMotorSpeedFbk[firstjointofset] != useMotorSpeedFbk[joint])
1008 yError() <<
"embObjMC " << getBoardInfo() <<
". Param useMotorSpeedFbk should have same value for joints belong same set. See joint " << firstjointofset <<
" and " << joint;
1013 _jsets[s].setUseSpeedFeedbackFromMotors(useMotorSpeedFbk[firstjointofset]);
1020 bool embObjMotionControl::verifyTorquePidshasSameUnitTypes(yarp::dev::PidFeedbackUnitsEnum &fbk_pidunits, yarp::dev::PidOutputUnitsEnum& out_pidunits)
1022 fbk_pidunits = yarp::dev::PidFeedbackUnitsEnum::RAW_MACHINE_UNITS;
1023 out_pidunits = yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS;
1025 int firstjoint = -1;
1026 for(
int i=0; i<_njoints; i++)
1028 if(_trq_pids[i].enabled)
1038 for(
int i=firstjoint+1; i<_njoints; i++)
1040 if(_trq_pids[i].enabled)
1042 if(_trq_pids[firstjoint].fbk_PidUnits != _trq_pids[i].fbk_PidUnits ||
1043 _trq_pids[firstjoint].out_PidUnits != _trq_pids[i].out_PidUnits)
1045 yError() <<
"embObjMC " << getBoardInfo() <<
"all joints with torque enabled should have same controlunits type. Joint " << firstjoint <<
" differs from joint " << i;
1056 bool embObjMotionControl::isTorqueControlEnabled(
int joint)
1058 return (_trq_pids[joint].enabled);
1061 bool embObjMotionControl::isVelocityControlEnabled(
int joint)
1064 return (_trj_pids[joint].enabled);
1068 void embObjMotionControl::updateDeadZoneWithDefaultValues(
void)
1070 for(
int i=0; i<_njoints; i++)
1072 switch(_jointEncs[i].
type)
1080 case eomc_enc_aksim2:
1086 case eomc_enc_absanalog:
1089 case eomc_enc_hallmotor:
1090 case eomc_enc_spichainof2:
1091 case eomc_enc_spichainof3:
1100 bool embObjMotionControl::fromConfig_readServiceCfg(yarp::os::Searchable &config)
1103 if(
false == parser->
parseService(config, serviceConfig))
1105 yError() <<
"embObjMC " << getBoardInfo() <<
"cannot parse service" ;
1109 if(eomn_serv_MC_generic == serviceConfig.
ethservice.configuration.type)
1111 yError() <<
"embObjMC " << getBoardInfo() <<
"it is no longer possible use eomn_serv_MC_generic, because firmware cannot configure itself!" ;
1119 for(
int i=0; i<_njoints; i++)
1124 if(NULL == jointEncoder_ptr)
1126 _jointEncs[i].resolution = 1;
1127 _jointEncs[i].type = eomc_enc_none;
1128 _jointEncs[i].tolerance = 0;
1132 _jointEncs[i].resolution = jointEncoder_ptr->
resolution;
1133 _jointEncs[i].type = (eOmc_encoder_t)jointEncoder_ptr->
desc.type;
1134 _jointEncs[i].tolerance = jointEncoder_ptr->
tolerance;
1138 if(NULL == motorEncoder_ptr)
1140 _motorEncs[i].resolution = 1;
1141 _motorEncs[i].type = eomc_enc_none;
1142 _motorEncs[i].tolerance = 0;
1146 _motorEncs[i].resolution = motorEncoder_ptr->
resolution;
1147 _motorEncs[i].type = (eOmc_encoder_t)motorEncoder_ptr->
desc.type;
1148 _motorEncs[i].tolerance = motorEncoder_ptr->
tolerance;
1160 bool embObjMotionControl::fromConfig(yarp::os::Searchable &config)
1163 _njoints = fromConfig_NumOfJoints(config);
1167 yError() <<
"embObjMC"<< getBoardInfo() <<
"fromConfig(): detected _njoints = " << _njoints;
1172 if(!alloc(_njoints))
1174 yError() <<
"embObjMC"<< getBoardInfo() <<
"fromConfig(): alloc() failed for _njoints = " << _njoints;
1182 int currentMCversion =0;
1188 yError() <<
"embObjMC" << getBoardInfo() <<
"------ ATTENTION!!!! Wrong value of <MotioncontrolVersion> parameter !!!! ---------------------------------------------------------------------------------------";
1189 yError() <<
"embObjMC" << getBoardInfo() <<
"------ This means that the configuration files of this device are not compatible with my parser, so I cannot start. ";
1190 yError() <<
"embObjMC" << getBoardInfo() <<
"------ I need version " <<
PARSER_MOTION_CONTROL_VERSION <<
", but in configuration files have version " << currentMCversion <<
".";
1191 yError() <<
"embObjMC" << getBoardInfo() <<
"------ Please update configuration files in robots-configuration repository. (see http://wiki.icub.org/wiki/Robot_configuration for more information). ";
1192 yError() <<
"embObjMC" << getBoardInfo() <<
"------ If the problem persists contact icub-support@iit.it DO NOT DO IT BY YOURSELF.";
1193 yError() <<
"embObjMC" << getBoardInfo() <<
"----------------------------------------------------------------------------------------------------------------------------------------------------------------";
1199 yTrace() << config.toString().c_str();
1204 if(
false == fromConfig_readServiceCfg(config))
1216 if(
false == fromConfig_Step2(config))
1228 bool embObjMotionControl::init()
1230 eOprotID32_t protid = 0;
1236 for(
int logico=0; logico< _njoints; logico++)
1238 int fisico = _axisMap[logico];
1239 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, fisico, eoprot_tag_mc_joint_cmmnds_controlmode);
1240 eOenum08_t controlMode = eomc_controlmode_cmd_idle;
1244 yError() <<
"embObjMotionControl::init() had an error while setting eomc_controlmode_cmd_idle in "<< getBoardInfo();
1250 SystemClock::delaySystem(0.010);
1257 vector<eOprotID32_t> id32v(0);
1258 for(
int n=0;
n<_njoints;
n++)
1260 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint,
n, eoprot_tag_mc_joint_status_core);
1261 id32v.push_back(protid);
1262 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor,
n, eoprot_tag_mc_motor_status);
1263 id32v.push_back(protid);
1266 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
1268 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, 0, eoprot_tag_mc_joint_status_debug);
1269 id32v.push_back(protid);
1270 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, 1, eoprot_tag_mc_joint_status_debug);
1271 id32v.push_back(protid);
1277 yError() <<
"embObjMotionControl::init() fails to add its variables to regulars in "<< getBoardInfo() <<
": cannot proceed any further";
1284 yDebug() <<
"embObjMotionControl::init() added" << id32v.size() <<
"regular rops to "<< getBoardInfo();
1286 for(
unsigned int r=0; r<id32v.size(); r++)
1288 uint32_t id32 = id32v.at(r);
1289 eoprot_ID2information(id32, nvinfo,
sizeof(nvinfo));
1290 yDebug() <<
"\t it added regular rop for" << nvinfo;
1295 SystemClock::delaySystem(0.005);
1302 for(
int logico=0; logico< _njoints; logico++)
1304 int fisico = _axisMap[logico];
1305 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, fisico, eoprot_tag_mc_joint_config);
1307 eOmc_joint_config_t jconfig = {0};
1308 memset(&jconfig, 0,
sizeof(eOmc_joint_config_t));
1310 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_POSITION,_trj_pids[logico].pid, fisico);
1314 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_TORQUE, _trq_pids[logico].pid, fisico);
1318 jconfig.impedance.damping = (eOmeas_damping_t) _measureConverter->impN2S(_impedance_params[logico].damping, fisico);
1319 jconfig.impedance.stiffness = (eOmeas_stiffness_t) _measureConverter->impN2S(_impedance_params[logico].stiffness, fisico);
1320 jconfig.impedance.offset = 0;
1322 _cacheImpedance[logico].stiffness = jconfig.impedance.stiffness;
1323 _cacheImpedance[logico].damping = jconfig.impedance.damping;
1324 _cacheImpedance[logico].offset = jconfig.impedance.offset;
1326 jconfig.userlimits.max = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posMax, fisico));
1327 jconfig.userlimits.min = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posMin, fisico));
1329 jconfig.hardwarelimits.max = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posHwMax, fisico));
1330 jconfig.hardwarelimits.min = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posHwMin, fisico));
1333 jconfig.maxvelocityofjoint =
S_32(_measureConverter->posA2E(_jointsLimits[logico].velMax, fisico));
1334 jconfig.velocitysetpointtimeout = (eOmeas_time_t)
U_16(_timeouts[logico].velocity);
1336 jconfig.jntEncoderResolution = _jointEncs[logico].resolution;
1337 jconfig.jntEncoderType = _jointEncs[logico].type;
1338 jconfig.jntEncTolerance = _jointEncs[logico].tolerance;
1340 jconfig.motor_params.bemf_value = _measureConverter->bemf_user2raw(_trq_pids[logico].kbemf, fisico);
1341 jconfig.motor_params.bemf_scale = 0;
1342 jconfig.motor_params.ktau_value = _measureConverter->ktau_user2raw(_trq_pids[logico].ktau, fisico);
1343 jconfig.motor_params.ktau_scale = 0;
1344 jconfig.motor_params.friction.viscous_pos_val = _measureConverter->viscousPos_user2raw(_trq_pids[logico].viscousPos, fisico);
1345 jconfig.motor_params.friction.viscous_neg_val = _measureConverter->viscousNeg_user2raw(_trq_pids[logico].viscousNeg, fisico);
1346 jconfig.motor_params.friction.coulomb_pos_val = _measureConverter->coulombPos_user2raw(_trq_pids[logico].coulombPos, fisico);
1347 jconfig.motor_params.friction.coulomb_neg_val = _measureConverter->coulombNeg_user2raw(_trq_pids[logico].coulombNeg, fisico);
1349 jconfig.gearbox_E2J = _gearbox_E2J[logico];
1351 jconfig.deadzone = _measureConverter->posA2E(_deadzone[logico], fisico);
1353 jconfig.tcfiltertype=_trq_pids[logico].
filterType;
1355 jconfig.kalman_params.enabled = _kalman_params[logico].enabled;
1356 for(
int i=0; i<_kalman_params[logico].x0.size(); i++) jconfig.kalman_params.x0[i] = _kalman_params[logico].x0.at(i);
1357 for(
int i=0; i<_kalman_params[logico].Q.size(); i++) jconfig.kalman_params.Q[i] = _kalman_params[logico].Q.at(i);
1358 jconfig.kalman_params.R = _kalman_params[logico].R;
1359 jconfig.kalman_params.P0 = _kalman_params[logico].P0;
1363 yError() <<
"FATAL: embObjMotionControl::init() had an error while calling setcheckRemoteValue() for joint config fisico #" << fisico <<
"in "<< getBoardInfo();
1370 yDebug() <<
"embObjMotionControl::init() correctly configured joint config fisico #" << fisico <<
"in "<< getBoardInfo();
1381 for(
int logico=0; logico<_njoints; logico++)
1383 int fisico = _axisMap[logico];
1385 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, fisico, eoprot_tag_mc_motor_config);
1386 eOmc_motor_config_t motor_cfg = {0};
1387 motor_cfg.maxvelocityofmotor = 0;
1388 motor_cfg.currentLimits.nominalCurrent = _currentLimits[logico].nominalCurrent;
1389 motor_cfg.currentLimits.overloadCurrent = _currentLimits[logico].overloadCurrent;
1390 motor_cfg.currentLimits.peakCurrent = _currentLimits[logico].peakCurrent;
1391 motor_cfg.gearbox_M2J = _gearbox_M2J[logico];
1392 motor_cfg.rotorEncoderResolution = _motorEncs[logico].resolution;
1393 motor_cfg.rotEncTolerance = _motorEncs[logico].tolerance;
1394 motor_cfg.hasHallSensor = _foc_based_info[logico].
hasHallSensor;
1396 motor_cfg.hasTempSensor = _foc_based_info[logico].
hasTempSensor;
1399 motor_cfg.verbose = _foc_based_info[logico].
verbose;
1400 motor_cfg.motorPoles = _foc_based_info[logico].
motorPoles;
1402 motor_cfg.rotorEncoderType = _motorEncs[logico].type;
1403 motor_cfg.pwmLimit =_rotorsLimits[logico].pwmMax;
1404 motor_cfg.limitsofrotor.max = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMax, fisico ));
1405 motor_cfg.limitsofrotor.min = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMin, fisico ));
1408 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_CURRENT, _cur_pids[logico].pid, fisico);
1411 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_VELOCITY, _spd_pids[logico].pid, fisico);
1416 yError() <<
"FATAL: embObjMotionControl::init() had an error while calling setcheckRemoteValue() for motor config fisico #" << fisico <<
"in "<< getBoardInfo();
1423 yDebug() <<
"embObjMotionControl::init() correctly configured motor config fisico #" << fisico <<
"in "<< getBoardInfo();
1434 yTrace() <<
"embObjMotionControl::init(): correctly instantiated for " << getBoardInfo();
1442 yTrace() <<
" embObjMotionControl::close()";
1444 ImplementControlMode::uninitialize();
1445 ImplementEncodersTimed::uninitialize();
1446 ImplementMotorEncoders::uninitialize();
1447 ImplementPositionControl::uninitialize();
1448 ImplementVelocityControl::uninitialize();
1449 ImplementPidControl::uninitialize();
1450 ImplementControlCalibration::uninitialize();
1451 ImplementAmplifierControl::uninitialize();
1452 ImplementImpedanceControl::uninitialize();
1453 ImplementControlLimits::uninitialize();
1454 ImplementTorqueControl::uninitialize();
1455 ImplementPositionDirect::uninitialize();
1456 ImplementInteractionMode::uninitialize();
1457 ImplementRemoteVariables::uninitialize();
1458 ImplementAxisInfo::uninitialize();
1459 ImplementCurrentControl::uninitialize();
1460 ImplementPWMControl::uninitialize();
1461 ImplementJointFault::uninitialize();
1463 if (_measureConverter) {
delete _measureConverter; _measureConverter=0;}
1466 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
1469 for(
size_t i=0; i<mcdiagnostics.
ports.size(); i++)
1471 mcdiagnostics.
ports[i]->close();
1472 delete mcdiagnostics.
ports[i];
1474 mcdiagnostics.
ports.clear();
1476 mcdiagnostics.
config.mode = eomn_serv_diagn_mode_NONE;
1477 mcdiagnostics.
config.par16 = 0;
1480 delete event_downsampler;
1494 void embObjMotionControl::cleanup(
void)
1496 if(ethManager == NULL)
return;
1515 size_t joint = eoprot_ID2index(id32);
1529 std::lock_guard<std::mutex> lck(_mutex);
1530 _encodersStamp[joint] = timestamp;
1534 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
1536 eOprotEntity_t ent = eoprot_ID2entity(id32);
1537 eOprotTag_t tag = eoprot_ID2tag(id32);
1539 char str[128] =
"boh";
1541 eoprot_ID2information(id32, str,
sizeof(str));
1543 if((eoprot_entity_mc_joint == ent) && (eoprot_tag_mc_joint_status_debug == tag) && (joint < mcdiagnostics.
ports.size()))
1546 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core);
1547 eOmc_joint_status_core_t jcore = {};
1551 int32_t *debug32 =
reinterpret_cast<int32_t*
>(rxdata);
1554 Bottle& output = mcdiagnostics.
ports[joint]->prepare();
1557 output.addString(
"[yt, amo, reg, pos]");
1558 output.addFloat64(timestamp);
1559 output.addInt32(debug32[0]);
1560 output.addInt32(debug32[1]);
1561 output.addInt32(jcore.measures.meas_position);
1562 mcdiagnostics.
ports[joint]->write();
1575 case VOCAB_PIDTYPE_POSITION:
1576 helper_setPosPidRaw(j,pid);
1578 case VOCAB_PIDTYPE_VELOCITY:
1580 helper_setSpdPidRaw(j, pid);
1582 case VOCAB_PIDTYPE_TORQUE:
1583 helper_setTrqPidRaw(j, pid);
1585 case VOCAB_PIDTYPE_CURRENT:
1586 helper_setCurPidRaw(j,pid);
1589 yError()<<
"Invalid pidtype:"<<pidtype;
1599 case VOCAB_PIDTYPE_POSITION:
1600 helper_getPosPidRaw(axis,pid);
1602 case VOCAB_PIDTYPE_VELOCITY:
1604 helper_getSpdPidRaw(axis, pid);
1606 case VOCAB_PIDTYPE_TORQUE:
1607 helper_getTrqPidRaw(axis, pid);
1609 case VOCAB_PIDTYPE_CURRENT:
1610 helper_getCurPidRaw(axis,pid);
1613 yError()<<
"Invalid pidtype:"<<pidtype;
1619 bool embObjMotionControl::helper_setPosPidRaw(
int j,
const Pid &pid)
1621 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtrajectory);
1630 yError() <<
"while setting position PIDs for " << getBoardInfo() <<
" joint " << j;
1640 for(
int j=0; j< _njoints; j++)
1655 for(
int j=0, index=0; j< _njoints; j++, index++)
1677 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
1678 eOmc_joint_status_core_t jcore = {0};
1685 case VOCAB_PIDTYPE_POSITION:
1687 if((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
1688 (eomc_controlmode_openloop == jcore.modes.controlmodestatus) ||
1689 (eomc_controlmode_current == jcore.modes.controlmodestatus))
1692 *err = (double) jcore.ofpid.generic.error1;
1703 case VOCAB_PIDTYPE_TORQUE:
1705 if ((eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus) &&
1706 (eomc_controlmode_position == jcore.modes.controlmodestatus))
1708 *err = (double) jcore.ofpid.complpos.errtrq;
1711 if(eomc_controlmode_torque == jcore.modes.controlmodestatus)
1713 *err = (double) jcore.ofpid.torque.errtrq;
1717 case VOCAB_PIDTYPE_VELOCITY:
1723 case VOCAB_PIDTYPE_CURRENT:
1731 yError()<<
"Invalid pidtype:"<<pidtype;
1741 for(
int j=0; j< _njoints; j++)
1748 bool embObjMotionControl::helper_getPosPidRaw(
int j, Pid *pid)
1750 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtrajectory);
1753 eOmc_PID_t eoPID = {0};
1756 #ifdef NETWORK_PERFORMANCE_BENCHMARK
1757 double start = yarp::os::Time::now();
1760 bool ret = askRemoteValue(protid, &eoPID, size);
1762 #ifdef NETWORK_PERFORMANCE_BENCHMARK
1763 double end = yarp::os::Time::now();
1764 m_responseTimingVerifier.tick(end-start, start);
1777 bool embObjMotionControl::helper_getPosPidsRaw(Pid *pid)
1779 std::vector<eOmc_PID_t> eoPIDList(_njoints);
1780 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_pidtrajectory, eoPIDList);
1783 yError() <<
"failed helper_getPosPidsRaw for" << getBoardInfo();
1787 for(
int j=0; j<_njoints; j++)
1801 case VOCAB_PIDTYPE_POSITION:
1802 helper_getPosPidsRaw(pids);
1807 case VOCAB_PIDTYPE_TORQUE:
1808 helper_getTrqPidsRaw(pids);
1810 case VOCAB_PIDTYPE_CURRENT:
1811 helper_getCurPidsRaw(pids);
1813 case VOCAB_PIDTYPE_VELOCITY:
1814 helper_getSpdPidsRaw(pids);
1817 yError()<<
"Invalid pidtype:"<<pidtype;
1825 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
1826 eOmc_joint_status_core_t jcore = {0};
1833 case VOCAB_PIDTYPE_POSITION:
1835 if((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
1836 (eomc_controlmode_openloop == jcore.modes.controlmodestatus) ||
1837 (eomc_controlmode_current == jcore.modes.controlmodestatus))
1838 { *ref = 0; yError() <<
"Invalid getPidReferenceRaw() request for current control mode";
return true; }
1839 *ref = (double) jcore.ofpid.generic.reference1;
1848 case VOCAB_PIDTYPE_TORQUE:
1854 case VOCAB_PIDTYPE_CURRENT:
1860 case VOCAB_PIDTYPE_VELOCITY:
1869 yError()<<
"Invalid pidtype:"<<pidtype;
1882 for(
int j=0; j< _njoints; j++)
1927 if( (mode != VOCAB_CM_VELOCITY) &&
1928 (mode != VOCAB_CM_MIXED) &&
1929 (mode != VOCAB_CM_IMPEDANCE_VEL) &&
1930 (mode != VOCAB_CM_IDLE))
1934 yError() <<
"velocityMoveRaw: skipping command because " << getBoardInfo() <<
" joint " << j <<
" is not in VOCAB_CM_VELOCITY mode";
1939 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
1941 _ref_command_speeds[j] = sp ;
1943 eOmc_setpoint_t setpoint;
1944 setpoint.type = eomc_setpoint_velocity;
1945 setpoint.to.velocity.value = (eOmeas_velocity_t)
S_32(_ref_command_speeds[j]);
1946 setpoint.to.velocity.withacceleration = (eOmeas_acceleration_t)
S_32(_ref_accs[j]);
1951 yError() <<
"while setting velocity mode";
1960 eOmc_setpoint_t setpoint;
1962 setpoint.type = eomc_setpoint_velocity;
1964 for(
int j=0; j<_njoints; j++)
1979 yTrace() <<
"setCalibrationParametersRaw for " << getBoardInfo() <<
"joint" << j;
1981 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_calibration);
1982 eOmc_calibrator_t calib;
1983 memset(&calib, 0x00,
sizeof(calib));
1984 calib.type = params.type;
1989 case eomc_calibration_type0_hard_stops:
1990 calib.params.type0.pwmlimit = (int16_t)
S_16(params.param1);
1991 calib.params.type0.velocity = (eOmeas_velocity_t)
S_32(params.param2);
1992 calib.params.type0.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
1996 case eomc_calibration_type1_abs_sens_analog:
1997 calib.params.type1.position = (int16_t)
S_16(params.param1);
1998 calib.params.type1.velocity = (eOmeas_velocity_t)
S_32(params.param2);
1999 calib.params.type1.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2003 case eomc_calibration_type2_hard_stops_diff:
2004 calib.params.type2.pwmlimit = (int16_t)
S_16(params.param1);
2005 calib.params.type2.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2006 calib.params.type2.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2010 case eomc_calibration_type3_abs_sens_digital:
2011 calib.params.type3.position = (int16_t)
S_16(params.param1);
2012 calib.params.type3.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2013 calib.params.type3.offset = (int32_t)
S_32(params.param3);
2014 calib.params.type3.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2018 case eomc_calibration_type4_abs_and_incremental:
2019 calib.params.type4.position = (int16_t)
S_16(params.param1);
2020 calib.params.type4.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2021 calib.params.type4.maxencoder = (int32_t)
S_32(params.param3);
2022 calib.params.type4.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2026 case eomc_calibration_type5_hard_stops:
2027 calib.params.type5.pwmlimit = (int32_t)
S_32(params.param1);
2028 calib.params.type5.final_pos = (int32_t)
S_32(params.param2);
2029 calib.params.type5.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2033 case eomc_calibration_type6_mais:
2034 calib.params.type6.position = (int32_t)
S_32(params.param1);
2035 calib.params.type6.velocity = (int32_t)
S_32(params.param2);
2036 calib.params.type6.current = (int32_t)
S_32(params.param3);
2037 calib.params.type6.vmin = (int32_t)
S_32(params.param4);
2038 calib.params.type6.vmax = (int32_t)
S_32(params.param5);
2039 calib.params.type6.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2043 case eomc_calibration_type7_hall_sensor:
2044 calib.params.type7.position = (int32_t)
S_32(params.param1);
2045 calib.params.type7.velocity = (int32_t)
S_32(params.param2);
2047 calib.params.type7.vmin = (int32_t)
S_32(params.param4);
2048 calib.params.type7.vmax = (int32_t)
S_32(params.param5);
2049 calib.params.type7.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2053 case eomc_calibration_type8_tripod_internal_hard_stop:
2054 calib.params.type8.pwmlimit = (int32_t)
S_32(params.param1);
2055 calib.params.type8.max_delta = (int32_t)
S_32(params.param2);
2056 calib.params.type8.calibrationZero = (int32_t)
S_32(params.paramZero );
2059 case eomc_calibration_type9_tripod_external_hard_stop:
2060 calib.params.type9.pwmlimit = (int32_t)
S_32(params.param1);
2061 calib.params.type9.max_delta = (int32_t)
S_32(params.param2);
2062 calib.params.type9.calibrationZero = (int32_t)
S_32(params.paramZero );
2065 case eomc_calibration_type10_abs_hard_stop:
2066 calib.params.type10.pwmlimit = (int32_t)
S_32(params.param1);
2067 calib.params.type10.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2070 case eomc_calibration_type11_cer_hands:
2071 calib.params.type11.offset0 = (int32_t)
S_32(params.param1);
2072 calib.params.type11.offset1 = (int32_t)
S_32(params.param2);
2073 calib.params.type11.offset2 = (int32_t)
S_32(params.param3);
2074 calib.params.type11.cable_range = (int32_t)
S_32(params.param4);
2075 calib.params.type11.pwm = (int32_t)
S_32(params.param5);
2079 case eomc_calibration_type12_absolute_sensor:
2080 calib.params.type12.rawValueAtZeroPos = (int32_t)
S_32(params.param1);
2081 calib.params.type12.calibrationDelta = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2084 case eomc_calibration_type13_cer_hands_2:
2085 calib.params.type13.rawValueAtZeroPos0 = (int32_t)
S_32(params.param1);
2086 calib.params.type13.rawValueAtZeroPos1 = (int32_t)
S_32(params.param2);
2087 calib.params.type13.rawValueAtZeroPos2 = (int32_t)
S_32(params.param3);
2088 calib.params.type13.rawValueAtZeroPos3 = (int32_t)
S_32(params.param4);
2092 yError() <<
"Calibration type unknown!! (embObjMotionControl)\n";
2099 yError() <<
"while setting velocity mode";
2103 _calibrated[j] =
true;
2110 yTrace() <<
"calibrateRaw for" << getBoardInfo() <<
"joint" << j;
2128 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_calibration);
2129 eOmc_calibrator_t calib;
2130 memset(&calib, 0x00,
sizeof(calib));
2136 case eomc_calibration_type0_hard_stops:
2137 calib.params.type0.pwmlimit = (int16_t)
S_16(p1);
2138 calib.params.type0.velocity = (eOmeas_velocity_t)
S_32(p2);
2142 case eomc_calibration_type1_abs_sens_analog:
2143 calib.params.type1.position = (int16_t)
S_16(p1);
2144 calib.params.type1.velocity = (eOmeas_velocity_t)
S_32(p2);
2148 case eomc_calibration_type2_hard_stops_diff:
2149 calib.params.type2.pwmlimit = (int16_t)
S_16(p1);
2150 calib.params.type2.velocity = (eOmeas_velocity_t)
S_32(p2);
2154 case eomc_calibration_type3_abs_sens_digital:
2155 calib.params.type3.position = (int16_t)
S_16(p1);
2156 calib.params.type3.velocity = (eOmeas_velocity_t)
S_32(p2);
2157 calib.params.type3.offset = (int32_t)
S_32(p3);
2161 case eomc_calibration_type4_abs_and_incremental:
2162 calib.params.type4.position = (int16_t)
S_16(p1);
2163 calib.params.type4.velocity = (eOmeas_velocity_t)
S_32(p2);
2164 calib.params.type4.maxencoder = (int32_t)
S_32(p3);
2168 yError () <<
"Calibration type unknown!! (embObjMotionControl)\n";
2175 yError() <<
"while setting velocity mode";
2179 _calibrated[j ] =
true;
2187 bool result =
false;
2188 eOenum08_t temp = 0;
2190 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_core_modes_controlmodestatus);
2191 if(
false == askRemoteValue(id32, &temp, size))
2193 yError () <<
"Failure of askRemoteValue() inside embObjMotionControl::doneRaw(axis=" << axis <<
") for " << getBoardInfo();
2197 eOmc_controlmode_t
type = (eOmc_controlmode_t) temp;
2201 if (eomc_controlmode_idle ==
type)
2205 else if (eomc_controlmode_calib ==
type)
2209 else if (eomc_controlmode_hwFault ==
type)
2211 yError(
"unable to complete calibration: joint %d in 'hw_fault status' inside doneRaw() function", axis);
2214 else if (eomc_controlmode_notConfigured ==
type)
2216 yError(
"unable to complete calibration: joint %d in 'not_configured' status inside doneRaw() function", axis);
2219 else if (eomc_controlmode_unknownError ==
type)
2221 yError(
"unable to complete calibration: joint %d in 'unknownError' status inside doneRaw() function", axis);
2224 else if (eomc_controlmode_configured ==
type)
2226 yError(
"unable to complete calibration: joint %d in 'configured' status inside doneRaw() function", axis);
2251 yWarning() <<
"Performance warning: You are using positionMove commands at high rate (<"<<
MAX_POSITION_MOVE_INTERVAL*1000.0 <<
" ms). Probably position control mode is not the right control mode to use.";
2253 _last_position_move_time[j] = yarp::os::Time::now();
2257 if( (mode != VOCAB_CM_POSITION) &&
2258 (mode != VOCAB_CM_MIXED) &&
2259 (mode != VOCAB_CM_IMPEDANCE_POS) &&
2260 (mode != VOCAB_CM_IDLE))
2264 yError() <<
"positionMoveRaw: skipping command because " << getBoardInfo() <<
" joint " << j <<
" is not in VOCAB_CM_POSITION mode";
2269 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
2270 _ref_command_positions[j] = ref;
2272 eOmc_setpoint_t setpoint;
2274 setpoint.type = (eOenum08_t) eomc_setpoint_position;
2275 setpoint.to.position.value = (eOmeas_position_t)
S_32(_ref_command_positions[j]);
2276 setpoint.to.position.withvelocity = (eOmeas_velocity_t)
S_32(_ref_speeds[j]);
2285 for(
int j=0, index=0; j< _njoints; j++, index++)
2305 eObool_t ismotiondone = eobool_false;
2308 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core_modes_ismotiondone);
2309 if(
false == askRemoteValue(id32, &ismotiondone, size))
2311 yError () <<
"Failure of askRemoteValue() inside embObjMotionControl::checkMotionDoneRaw(j=" << j <<
") for " << getBoardInfo();
2316 *flag = ismotiondone;
2323 std::vector <eObool_t> ismotiondoneList(_njoints);
2324 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_core_modes_ismotiondone, ismotiondoneList);
2327 yError () <<
"Failure of askRemoteValues() inside embObjMotionControl::checkMotionDoneRaw for all joints of" << getBoardInfo();
2331 for(
int j=0; j<_njoints; j++)
2333 *flag &= ismotiondoneList[j];
2343 _ref_speeds[index] = sp;
2351 for(
int j=0, index=0; j< _njoints; j++, index++)
2353 _ref_speeds[index] = spds[index];
2365 _ref_accs[j ] = 1e6;
2367 else if (
acc < -1e6)
2369 _ref_accs[j ] = -1e6;
2373 _ref_accs[j ] =
acc;
2383 for(
int j=0, index=0; j< _njoints; j++, index++)
2387 _ref_accs[index] = 1e6;
2389 else if (accs[j] < -1e6)
2391 _ref_accs[index] = -1e6;
2395 _ref_accs[index] = accs[j];
2403 if (j<0 || j>_njoints)
return false;
2404 #if ASK_REFERENCE_TO_FIRMWARE
2405 *spd = _ref_speeds[j];
2408 *spd = _ref_speeds[j];
2415 memcpy(spds, _ref_speeds,
sizeof(
double) * _njoints);
2421 *
acc = _ref_accs[j];
2427 memcpy(accs, _ref_accs,
sizeof(
double) * _njoints);
2433 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_stoptrajectory);
2435 eObool_t stop = eobool_true;
2443 for(
int j=0; j< _njoints; j++)
2458 for(
int j=0; j<n_joint; j++)
2468 for(
int j=0; j<n_joint; j++)
2479 for(
int j=0; j<n_joint; j++)
2481 if(joints[j] >= _njoints)
2483 yError() << getBoardInfo() <<
":checkMotionDoneRaw required for not existing joint ( " << joints[j] <<
")";
2489 std::vector <eObool_t> ismotiondoneList(_njoints);
2490 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_core_modes_ismotiondone, ismotiondoneList);
2493 yError () << getBoardInfo() <<
"Failure of askRemoteValues() inside embObjMotionControl::checkMotionDoneRaw for a group of joint"; getBoardInfo();
2498 bool tot_val =
true;
2499 for(
int j=0; j<n_joint; j++)
2501 tot_val &= ismotiondoneList[joints[j]];
2512 for(
int j=0; j<n_joint; j++)
2522 for(
int j=0; j<n_joint; j++)
2532 for(
int j=0; j<n_joint; j++)
2542 for(
int j=0; j<n_joint; j++)
2552 for(
int j=0; j<n_joint; j++)
2554 ret = ret &&
stopRaw(joints[j]);
2565 eOmc_joint_status_core_t jcore = {0};
2566 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2570 eOmc_controlmode_t
type = (eOmc_controlmode_t) jcore.modes.controlmodestatus;
2580 for(
int j=0; j< _njoints; j++)
2590 for(
int j=0; j< n_joint; j++)
2605 eOenum08_t controlmodecommand = 0;
2607 if((_mode == VOCAB_CM_TORQUE) && (_trq_pids[j].enabled ==
false))
2609 yError()<<
"Torque control is disabled. Check your configuration parameters";
2615 yError() <<
"SetControlMode: received unknown control mode for " << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
2619 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_controlmode);
2622 yError() <<
"setControlModeRaw failed for " << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
2627 ret = checkRemoteControlModeStatus(j, _mode);
2631 yError() <<
"In embObjMotionControl::setControlModeRaw(j=" << j <<
", mode=" << yarp::os::Vocab32::decode(_mode).c_str() <<
") for " << getBoardInfo() <<
" has failed checkRemoteControlModeStatus()";
2641 eOenum08_t controlmodecommand = 0;
2644 for(
int i=0; i<n_joint; i++)
2646 if ((modes[i] == VOCAB_CM_TORQUE) && (_trq_pids[i].enabled ==
false)) {yError()<<
"Torque control is disabled. Check your configuration parameters";
continue;}
2650 yError() <<
"SetControlModesRaw(): received unknown control mode for " << getBoardInfo() <<
" joint " << joints[i] <<
" mode " << Vocab32::decode(modes[i]);
2655 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joints[i], eoprot_tag_mc_joint_cmmnds_controlmode);
2658 yError() <<
"setControlModesRaw() could not send set<cmmnds_controlmode> for " << getBoardInfo() <<
" joint " << joints[i] <<
" mode " << Vocab32::decode(modes[i]);
2663 bool tmpresult = checkRemoteControlModeStatus(joints[i], modes[i]);
2664 if(
false == tmpresult)
2666 yError() <<
"setControlModesRaw(const int n_joint, const int *joints, int *modes) could not check with checkRemoteControlModeStatus() for " << getBoardInfo() <<
" joint " << joints[i] <<
" mode " << Vocab32::decode(modes[i]);
2669 ret = ret && tmpresult;
2679 eOenum08_t controlmodecommand = 0;
2681 for(
int i=0; i<_njoints; i++)
2684 if ((modes[i] == VOCAB_CM_TORQUE) && (_trq_pids[i].enabled ==
false))
2686 yError()<<
"Torque control is disabled. Check your configuration parameters";
2692 yError() <<
"SetControlMode: received unknown control mode for" << getBoardInfo() <<
" joint " << i <<
" mode " << Vocab32::decode(modes[i]);
2696 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, i, eoprot_tag_mc_joint_cmmnds_controlmode);
2699 yError() <<
"setControlModesRaw failed for " << getBoardInfo() <<
" joint " << i <<
" mode " << Vocab32::decode(modes[i]);
2703 bool tmpresult = checkRemoteControlModeStatus(i, modes[i]);
2704 if(
false == tmpresult)
2706 yError() <<
"setControlModesRaw(int *modes) could not check with checkRemoteControlModeStatus() for" << getBoardInfo() <<
" joint " << i <<
" mode " << Vocab32::decode(modes[i]);
2709 ret = ret && tmpresult;
2742 eOmc_joint_status_core_t core;
2743 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2749 *value = (double) core.measures.meas_position;
2753 yError() <<
"embObjMotionControl while reading encoder";
2763 for(
int j=0; j< _njoints; j++)
2773 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2774 eOmc_joint_status_core_t core;
2781 *sp = (double) core.measures.meas_velocity;
2788 for(
int j=0; j< _njoints; j++)
2797 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2798 eOmc_joint_status_core_t core;
2804 *
acc = (double) core.measures.meas_acceleration;
2811 for(
int j=0; j< _njoints; j++)
2823 std::lock_guard<std::mutex> lck(_mutex);
2824 for(
int i=0; i<_njoints; i++)
2825 stamps[i] = _encodersStamp[i];
2832 std::lock_guard<std::mutex> lck(_mutex);
2833 *stamp = _encodersStamp[j];
2877 eOmc_motor_status_basic_t status;
2878 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
2883 *value = (double) status.mot_position;
2887 yError() <<
"embObjMotionControl while reading motor encoder position";
2897 for(
int j=0; j< _njoints; j++)
2907 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
2908 eOmc_motor_status_basic_t tmpMotorStatus;
2912 *sp = (double) tmpMotorStatus.mot_velocity;
2916 yError() <<
"embObjMotionControl while reading motor encoder speed";
2925 for(
int j=0; j< _njoints; j++)
2934 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
2935 eOmc_motor_status_basic_t tmpMotorStatus;
2939 *
acc = (double) tmpMotorStatus.mot_acceleration;
2943 yError() <<
"embObjMotionControl while reading motor encoder acceleration";
2952 for(
int j=0; j< _njoints; j++)
2962 std::lock_guard<std::mutex> lck(_mutex);
2963 for(
int i=0; i<_njoints; i++)
2964 stamps[i] = _encodersStamp[i];
2971 std::lock_guard<std::mutex> lck(_mutex);
2972 *stamp = _encodersStamp[m];
2991 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
2992 eOmc_motor_status_basic_t tmpMotorStatus;
2995 *value = (double) tmpMotorStatus.mot_current;
3002 for(
int j=0; j< _njoints; j++)
3011 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_currentlimits);
3013 eOmc_current_limits_params_t currentlimits = {0};
3015 if(!askRemoteValue(protid, ¤tlimits, size))
3017 yError() <<
"embObjMotionControl::setMaxCurrentRaw() could not read max current for " << getBoardInfo() <<
"joint " << j;
3022 currentlimits.overloadCurrent = (eOmeas_current_t)
S_16(val);
3030 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_currentlimits);
3032 eOmc_current_limits_params_t currentlimits = {0};
3035 if(!askRemoteValue(protid, ¤tlimits, size))
3037 yError() <<
"embObjMotionControl::getMaxCurrentRaw() could not read max current for " << getBoardInfo() <<
"joint " << j;
3041 *val = (double) currentlimits.overloadCurrent;
3049 (_enabledAmp[j ]) ? *st = 1 : *st = 0;
3056 for(
int j=0; j<_njoints; j++)
3058 sts[j] = _enabledAmp[j];
3064 #ifdef IMPLEMENT_DEBUG_INTERFACE
3069 bool embObjMotionControl::setParameterRaw(
int j,
unsigned int type,
double value) {
return NOT_YET_IMPLEMENTED(
"setParameterRaw"); }
3070 bool embObjMotionControl::getParameterRaw(
int j,
unsigned int type,
double* value) {
return NOT_YET_IMPLEMENTED(
"getParameterRaw"); }
3071 bool embObjMotionControl::getDebugParameterRaw(
int j,
unsigned int index,
double* value) {
return NOT_YET_IMPLEMENTED(
"getDebugParameterRaw"); }
3072 bool embObjMotionControl::setDebugParameterRaw(
int j,
unsigned int index,
double value) {
return NOT_YET_IMPLEMENTED(
"setDebugParameterRaw"); }
3073 bool embObjMotionControl::setDebugReferencePositionRaw(
int j,
double value) {
return NOT_YET_IMPLEMENTED(
"setDebugReferencePositionRaw"); }
3074 bool embObjMotionControl::getDebugReferencePositionRaw(
int j,
double* value) {
return NOT_YET_IMPLEMENTED(
"getDebugReferencePositionRaw");}
3082 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_userlimits);
3084 eOmeas_position_limits_t limits;
3085 limits.max = (eOmeas_position_t)
S_32(
max);
3086 limits.min = (eOmeas_position_t)
S_32(
min);
3093 yError() <<
"while setting position limits for joint" << j <<
" \n";
3100 eOmeas_position_limits_t limits;
3101 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_userlimits);
3104 if(! askRemoteValue(protoid, &limits, size))
3107 *
min = (double)limits.min + SAFETY_THRESHOLD;
3108 *
max = (
double)limits.max - SAFETY_THRESHOLD;
3114 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3116 eOmc_motor_config_t motor_cfg;
3117 if(! askRemoteValue(protoid, &motor_cfg, size))
3121 *gearbox = (double)motor_cfg.gearbox_M2J;
3128 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3130 eOmc_motor_config_t motor_cfg;
3131 if(! askRemoteValue(protoid, &motor_cfg, size))
3133 *rotorMax = (double)( motor_cfg.limitsofrotor.max);
3134 *rotorMin = (double)( motor_cfg.limitsofrotor.min);
3140 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3142 eOmc_joint_config_t joint_cfg;
3143 if(! askRemoteValue(protoid, &joint_cfg, size))
3147 type = (int)joint_cfg.tcfiltertype;
3153 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3155 eOmc_motor_config_t motor_cfg;
3156 if(! askRemoteValue(protoid, &motor_cfg, size))
3160 rotres = (double)motor_cfg.rotorEncoderResolution;
3167 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3169 eOmc_joint_config_t joint_cfg;
3170 if(! askRemoteValue(protoid, &joint_cfg, size))
3174 jntres = (double)joint_cfg.jntEncoderResolution;
3181 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3183 eOmc_joint_config_t joint_cfg;
3184 if(! askRemoteValue(protoid, &joint_cfg, size))
3188 type = (int)joint_cfg.jntEncoderType;
3195 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3197 eOmc_motor_config_t motor_cfg;
3198 if(! askRemoteValue(protoid, &motor_cfg, size))
3202 type = (int)motor_cfg.rotorEncoderType;
3209 yError(
"getKinematicMJRaw not yet implemented");
3215 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3217 eOmc_motor_config_t motor_cfg;
3218 if(! askRemoteValue(protoid, &motor_cfg, size))
3222 ret = (int)motor_cfg.hasTempSensor;
3229 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3231 eOmc_motor_config_t motor_cfg;
3232 if(! askRemoteValue(protoid, &motor_cfg, size))
3236 ret = (int)motor_cfg.hasHallSensor;
3243 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3245 eOmc_motor_config_t motor_cfg;
3246 if(! askRemoteValue(protoid, &motor_cfg, size))
3250 ret = (int)motor_cfg.hasRotorEncoder;
3257 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3259 eOmc_motor_config_t motor_cfg;
3260 if(! askRemoteValue(protoid, &motor_cfg, size))
3264 ret = (int)motor_cfg.hasRotorEncoderIndex;
3271 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3274 eOmc_motor_config_t motor_cfg;
3275 if(! askRemoteValue(protoid, &motor_cfg, size))
3279 poles = (int)motor_cfg.motorPoles;
3286 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3288 eOmc_motor_config_t motor_cfg;
3289 if(! askRemoteValue(protoid, &motor_cfg, size))
3293 rotorOffset = (double)motor_cfg.rotorIndexOffset;
3300 if (axis >= 0 && axis < _njoints)
3302 name = _axesInfo[axis].name;
3314 if (axis >= 0 && axis < _njoints)
3316 type = _axesInfo[axis].type;
3325 bool embObjMotionControl::getJointDeadZoneRaw(
int j,
double &jntDeadZone)
3327 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3329 eOmc_joint_config_t joint_cfg;
3330 if(! askRemoteValue(protoid, &joint_cfg, size))
3334 jntDeadZone = _measureConverter->posE2A((
double)joint_cfg.deadzone, _axisMap[j]);
3343 if (key ==
"kinematic_mj")
3346 Bottle& ret = val.addList();
3348 eOmn_serv_type_t mc_serv_type = (eOmn_serv_type_t)serviceConfig.
ethservice.configuration.type;
3349 if(iNeedCouplingsInfo())
3351 for (
int r=0; r<_njoints; r++)
3353 for (
int c = 0; c < _njoints; c++)
3357 ret.addFloat64(_couplingInfo.
matrixJ2M[4 * r + c]);
3363 ret.addFloat64(0.0);
3367 else if (key ==
"encoders")
3369 Bottle& r = val.addList();
for (
int i = 0; i < _njoints; i++) { r.addFloat64(_measureConverter->posA2E(1.0, i)); }
3372 else if (key ==
"rotorEncoderResolution")
3377 else if (key ==
"jointEncoderResolution")
3382 else if (key ==
"gearbox_M2J")
3387 else if (key ==
"gearbox_E2J")
3389 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp=0; getGerabox_E2J(i, &
tmp); r.addFloat64(
tmp); }
3392 else if (key ==
"hasHallSensor")
3397 else if (key ==
"hasTempSensor")
3402 else if (key ==
"hasRotorEncoder")
3407 else if (key ==
"hasRotorEncoderIndex")
3412 else if (key ==
"rotorIndexOffset")
3417 else if (key ==
"motorPoles")
3422 else if (key ==
"pidCurrentKp")
3424 Bottle& r = val.addList();
for (
int i = 0; i < _njoints; i++) { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p); r.addFloat64(
p.kp); }
3427 else if (key ==
"pidCurrentKi")
3429 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p); r.addFloat64(
p.ki); }
3432 else if (key ==
"pidCurrentShift")
3434 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p); r.addFloat64(
p.scale); }
3437 else if (key ==
"pidCurrentOutput")
3439 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p); r.addFloat64(
p.max_output); }
3442 else if (key ==
"jointEncoderType")
3444 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++)
3450 yError(
"Invalid jointEncoderType");
3456 else if (key ==
"rotorEncoderType")
3458 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++)
3464 yError(
"Invalid motorEncoderType");
3470 else if (key ==
"coulombThreshold")
3472 val.addString(
"not implemented yet");
3475 else if (key ==
"torqueControlFilterType")
3480 else if (key ==
"torqueControlEnabled")
3483 Bottle& r = val.addList();
3484 for(
int i = 0; i<_njoints; i++)
3486 r.addInt32((
int)_trq_pids[i].enabled );
3490 else if (key ==
"PWMLimit")
3492 Bottle& r = val.addList();
for (
int i = 0; i< _njoints; i++) {
double tmp = 0;
getPWMLimitRaw(i, &
tmp); r.addFloat64(
tmp); }
3495 else if (key ==
"motOverloadCurr")
3497 Bottle& r = val.addList();
for (
int i = 0; i< _njoints; i++) {
double tmp = 0;
getMaxCurrentRaw(i, &
tmp); r.addFloat64(
tmp); }
3500 else if (key ==
"motNominalCurr")
3505 else if (key ==
"motPeakCurr")
3507 Bottle& r = val.addList();
for (
int i = 0; i< _njoints; i++) {
double tmp = 0;
getPeakCurrentRaw(i, &
tmp); r.addFloat64(
tmp); }
3510 else if (key ==
"PowerSuppVoltage")
3515 else if (key ==
"rotorMax")
3518 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getRotorLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp2); }
3521 else if (key ==
"rotorMin")
3524 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getRotorLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp1); }
3527 else if (key ==
"jointMax")
3530 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp2); }
3533 else if (key ==
"jointMin")
3536 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp1); }
3539 else if (key ==
"jointEncTolerance")
3542 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0; getJointEncTolerance(i, &tmp1); r.addFloat64(tmp1); }
3545 else if (key ==
"motorEncTolerance")
3548 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0; getMotorEncTolerance(i, &tmp1); r.addFloat64(tmp1); }
3551 else if (key ==
"jointDeadZone")
3554 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0; getJointDeadZoneRaw(i, tmp1); r.addFloat64(tmp1); }
3557 else if (key ==
"readonly_position_PIDraw")
3559 Bottle& r = val.addList();
3560 for (
int i = 0; i < _njoints; i++)
3562 getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, i, &
p);
3564 snprintf(buff, 1000,
"J %d : kp %+3.3f ki %+3.3f kd %+3.3f maxint %+3.3f maxout %+3.3f off %+3.3f scale %+3.3f up %+3.3f dwn %+3.3f kff %+3.3f", i,
p.kp,
p.ki,
p.kd,
p.max_int,
p.max_output,
p.offset,
p.scale,
p.stiction_up_val,
p.stiction_down_val,
p.kff);
3569 else if (key ==
"readonly_velocity_PIDraw")
3571 Bottle& r = val.addList();
3572 for (
int i = 0; i < _njoints; i++)
3573 { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, i, &
p);
3575 snprintf(buff, 1000,
"J %d : kp %+3.3f ki %+3.3f kd %+3.3f maxint %+3.3f maxout %+3.3f off %+3.3f scale %+3.3f up %+3.3f dwn %+3.3f kff %+3.3f", i,
p.kp,
p.ki,
p.kd,
p.max_int,
p.max_output,
p.offset,
p.scale,
p.stiction_up_val,
p.stiction_down_val,
p.kff);
3580 else if (key ==
"readonly_torque_PIDraw")
3582 Bottle& r = val.addList();
3583 for (
int i = 0; i < _njoints; i++)
3584 { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, i, &
p);
3586 snprintf(buff, 1000,
"J %d : kp %+3.3f ki %+3.3f kd %+3.3f maxint %+3.3f maxout %+3.3f off %+3.3f scale %+3.3f up %+3.3f dwn %+3.3f kff %+3.3f", i,
p.kp,
p.ki,
p.kd,
p.max_int,
p.max_output,
p.offset,
p.scale,
p.stiction_up_val,
p.stiction_down_val,
p.kff);
3591 else if (key ==
"readonly_current_PIDraw")
3593 Bottle& r = val.addList();
3594 for (
int i = 0; i < _njoints; i++)
3595 { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p);
3597 snprintf(buff, 1000,
"J %d : kp %+3.3f ki %+3.3f kd %+3.3f maxint %+3.3f maxout %+3.3f off %+3.3f scale %+3.3f up %+3.3f dwn %+3.3f kff %+3.3f", i,
p.kp,
p.ki,
p.kd,
p.max_int,
p.max_output,
p.offset,
p.scale,
p.stiction_up_val,
p.stiction_down_val,
p.kff);
3602 else if (key ==
"readonly_llspeed_PIDraw")
3604 Bottle& r = val.addList();
3605 for (
int i = 0; i < _njoints; i++)
3607 Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, i, &
p);
3609 snprintf(buff, 1000,
"J %d : kp %+3.3f ki %+3.3f kd %+3.3f maxint %+3.3f maxout %+3.3f off %+3.3f scale %+3.3f up %+3.3f dwn %+3.3f kff %+3.3f", i,
p.kp,
p.ki,
p.kd,
p.max_int,
p.max_output,
p.offset,
p.scale,
p.stiction_up_val,
p.stiction_down_val,
p.kff);
3614 else if (key ==
"readonly_motor_torque_params_raw")
3616 Bottle& r = val.addList();
3617 for (
int i = 0; i < _njoints; i++)
3619 MotorTorqueParameters params;
3622 snprintf(buff, 1000,
"J %d : bemf %+3.3f bemf_scale %+3.3f ktau %+3.3f ktau_scale %+3.3f viscousPos %+3.3f viscousNeg %+3.3f coulombPos %+3.3f coulombNeg %+3.3f", i, params.bemf, params.bemf_scale, params.ktau, params.ktau_scale, params.viscousPos, params.viscousNeg, params.coulombPos, params.coulombNeg);
3627 yWarning(
"getRemoteVariable(): Unknown variable %s", key.c_str());
3633 string s1 = val.toString();
3634 if (val.size() != _njoints)
3636 yWarning(
"setRemoteVariable(): Protocol error %s", s1.c_str());
3640 if (key ==
"kinematic_mj")
3642 yWarning(
"setRemoteVariable(): Impossible to set kinematic_mj parameter at runtime.");
3655 else if (key ==
"PWMLimit")
3657 for (
int i = 0; i < _njoints; i++)
setPWMLimitRaw(i, val.get(i).asFloat64());
3662 else if (key ==
"jointMax")
3665 for (
int i = 0; i < _njoints; i++)
3672 else if (key ==
"jointMin")
3675 for (
int i = 0; i < _njoints; i++)
3682 yWarning(
"setRemoteVariable(): Unknown variable %s", key.c_str());
3688 listOfKeys->clear();
3689 listOfKeys->addString(
"kinematic_mj");
3690 listOfKeys->addString(
"encoders");
3691 listOfKeys->addString(
"gearbox_M2J");
3692 listOfKeys->addString(
"gearbox_E2J");
3693 listOfKeys->addString(
"hasHallSensor");
3694 listOfKeys->addString(
"hasTempSensor");
3695 listOfKeys->addString(
"hasRotorEncoder");
3696 listOfKeys->addString(
"hasRotorEncoderIndex");
3697 listOfKeys->addString(
"rotorIndexOffset");
3698 listOfKeys->addString(
"rotorEncoderResolution");
3699 listOfKeys->addString(
"jointEncoderResolution");
3700 listOfKeys->addString(
"motorPoles");
3701 listOfKeys->addString(
"pidCurrentKp");
3702 listOfKeys->addString(
"pidCurrentKi");
3703 listOfKeys->addString(
"pidCurrentShift");
3704 listOfKeys->addString(
"pidCurrentOutput");
3705 listOfKeys->addString(
"coulombThreshold");
3706 listOfKeys->addString(
"torqueControlFilterType");
3707 listOfKeys->addString(
"jointEncoderType");
3708 listOfKeys->addString(
"rotorEncoderType");
3709 listOfKeys->addString(
"PWMLimit");
3710 listOfKeys->addString(
"motOverloadCurr");
3711 listOfKeys->addString(
"motNominalCurr");
3712 listOfKeys->addString(
"motPeakCurr");
3713 listOfKeys->addString(
"PowerSuppVoltage");
3714 listOfKeys->addString(
"rotorMax");
3715 listOfKeys->addString(
"rotorMin");
3716 listOfKeys->addString(
"jointMax");
3717 listOfKeys->addString(
"jointMin");
3718 listOfKeys->addString(
"jointEncTolerance");
3719 listOfKeys->addString(
"motorEncTolerance");
3720 listOfKeys->addString(
"jointDeadZone");
3721 listOfKeys->addString(
"readonly_position_PIDraw");
3722 listOfKeys->addString(
"readonly_velocity_PIDraw");
3723 listOfKeys->addString(
"readonly_current_PIDraw");
3724 listOfKeys->addString(
"readonly_torque_PIDraw");
3725 listOfKeys->addString(
"readonly_motor_torque_params_raw");
3737 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_config);
3739 eOmc_joint_config_t joint_cfg;
3740 if(! askRemoteValue(protoid, &joint_cfg, size))
3743 *
max = joint_cfg.maxvelocityofjoint;
3759 return VAS_status::VAS_OK;
3771 for(
int j=0; j< _njoints; j++)
3780 int j = _axisMap[userLevel_jointNumber];
3782 eOmeas_torque_t meas_torque = 0;
3783 static double curr_time = Time::now();
3784 static int count_saturation=0;
3786 meas_torque = (eOmeas_torque_t)
S_32(_measureConverter->trqN2S(fTorque, j));
3788 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_inputs_externallymeasuredtorque);
3803 eOmc_joint_status_core_t jstatus;
3804 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
3806 *t = (double) _measureConverter->trqS2N(jstatus.measures.meas_torque, j);
3813 for(
int j=0; j<_njoints; j++)
3831 for(
int j=0; j<_njoints && ret; j++)
3838 eOmc_setpoint_t setpoint;
3839 setpoint.type = (eOenum08_t) eomc_setpoint_torque;
3840 setpoint.to.torque.value = (eOmeas_torque_t)
S_32(t);
3842 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
3849 for(
int j=0; j< n_joint; j++)
3859 for(
int j=0; j<_njoints && ret; j++)
3866 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
3867 eOmc_joint_status_core_t jcore = {0};
3873 yError() <<
"embObjMotionControl::getRefTorqueRaw() could not read pid torque reference pos for " << getBoardInfo() <<
"joint " << j;
3877 if ((eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus) &&
3878 (eomc_controlmode_position == jcore.modes.controlmodestatus))
3880 *t = (double) jcore.ofpid.complpos.reftrq;
3883 if(eomc_controlmode_torque == jcore.modes.controlmodestatus)
3885 *t = (double) jcore.ofpid.torque.reftrq;
3891 bool embObjMotionControl::helper_setTrqPidRaw(
int j,
const Pid &pid)
3899 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtorque);
3903 bool embObjMotionControl::helper_getTrqPidRaw(
int j, Pid *pid)
3905 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtorque);
3909 if(! askRemoteValue(protoid, &eoPID, size))
3918 bool embObjMotionControl::helper_getTrqPidsRaw(Pid *pid)
3920 std::vector<eOmc_PID_t> eoPIDList (_njoints);
3921 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_pidtorque, eoPIDList);
3924 for(
int j=0; j< _njoints; j++)
3936 eOmc_impedance_t val;
3941 *stiffness = (double) (val.stiffness);
3942 *damping = (double) (val.damping);
3950 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
3952 if(! askRemoteValue(protoid, &imped, size))
3956 _cacheImpedance->damping = imped.damping;
3957 _cacheImpedance->stiffness = imped.stiffness;
3958 _cacheImpedance->offset = imped.offset;
3965 eOmc_impedance_t val;
3973 _cacheImpedance[j].stiffness = (eOmeas_stiffness_t) stiffness;
3974 _cacheImpedance[j].damping = (eOmeas_damping_t) damping;
3976 val.stiffness = _cacheImpedance[j].stiffness;
3977 val.damping = _cacheImpedance[j].damping;
3978 val.offset = _cacheImpedance[j].offset;
3980 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
3990 eOmc_impedance_t val;
3996 _cacheImpedance[j].offset = (eOmeas_torque_t)
S_32(
offset);
3997 val.stiffness = _cacheImpedance[j].stiffness;
3998 val.damping = _cacheImpedance[j].damping;
3999 val.offset = _cacheImpedance[j].offset;
4001 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
4011 eOmc_impedance_t val;
4022 *min_stiff = _impedance_limits[j].
min_stiff;
4023 *max_stiff = _impedance_limits[j].
max_stiff;
4024 *min_damp = _impedance_limits[j].
min_damp;
4025 *max_damp = _impedance_limits[j].
max_damp;
4031 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_motor_params);
4034 eOmc_motor_params_t eo_params = {0};
4035 if(! askRemoteValue(protoid, &eo_params, size))
4038 params->bemf = eo_params.bemf_value;
4039 params->bemf_scale = eo_params.bemf_scale;
4040 params->ktau = eo_params.ktau_value;
4041 params->ktau_scale = eo_params.ktau_scale;
4042 params->viscousPos = eo_params.friction.viscous_pos_val;
4043 params->viscousNeg = eo_params.friction.viscous_neg_val ;
4044 params->coulombPos = eo_params.friction.coulomb_pos_val;
4045 params->coulombNeg = eo_params.friction.coulomb_neg_val;
4054 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_motor_params);
4055 eOmc_motor_params_t eo_params = {0};
4059 eo_params.bemf_value = (float) params.bemf;
4060 eo_params.bemf_scale = (uint8_t) params.bemf_scale;
4061 eo_params.ktau_value = (float) params.ktau;
4062 eo_params.ktau_scale = (uint8_t) params.ktau_scale;
4063 eo_params.friction.viscous_pos_val =
static_cast<float32_t
>(params.viscousPos);
4064 eo_params.friction.viscous_neg_val =
static_cast<float32_t
>(params.viscousNeg);
4065 eo_params.friction.coulomb_pos_val =
static_cast<float32_t
>(params.coulombPos);
4066 eo_params.friction.coulomb_neg_val =
static_cast<float32_t
>(params.coulombNeg);
4071 yError() <<
"embObjMotionControl::setMotorTorqueParamsRaw() could not send set message for" << getBoardInfo() <<
"joint " << j;
4083 for(
int j=0; j< n_joint; j++)
4117 bool embObjMotionControl::helper_getVelPidRaw(
int j, Pid *pid)
4119 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_piddirect);
4122 if(! askRemoteValue(protoid, &eoPID, size))
4132 bool embObjMotionControl::helper_getVelPidsRaw(Pid *pid)
4134 std::vector <eOmc_PID_t> eoPIDList (_njoints);
4135 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_piddirect, eoPIDList);
4139 for(
int j=0; j<_njoints; j++)
4154 if (mode != VOCAB_CM_POSITION_DIRECT &&
4155 mode != VOCAB_CM_IDLE)
4159 yError() <<
"setReferenceRaw: skipping command because" << getBoardInfo() <<
" joint " << j <<
" is not in VOCAB_CM_POSITION_DIRECT mode";
4164 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
4165 eOmc_setpoint_t setpoint = {0};
4167 _ref_positions[j] = ref;
4168 setpoint.type = (eOenum08_t) eomc_setpoint_positionraw;
4169 setpoint.to.position.value = (eOmeas_position_t)
S_32(ref);
4170 setpoint.to.position.withvelocity = 0;
4178 for(
int i=0; i<n_joint; i++)
4188 for (
int i = 0; i<_njoints; i++)
4198 if (axis<0 || axis>_njoints)
return false;
4199 #if ASK_REFERENCE_TO_FIRMWARE
4200 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4205 eOmc_joint_status_target_t target = {0};
4206 if(!askRemoteValue(id32, &target, size))
4208 yError() <<
"embObjMotionControl::getTargetPositionRaw() could not read reference pos for " << getBoardInfo() <<
"joint " << axis;
4212 *ref = (double) target.trgt_position;
4216 *ref = _ref_command_positions[axis];
4224 for (
int i = 0; i<_njoints; i++)
4234 for (
int i = 0; i<nj; i++)
4243 if (axis<0 || axis>_njoints)
return false;
4244 #if ASK_REFERENCE_TO_FIRMWARE
4245 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4250 eOmc_joint_status_target_t target = {0};
4251 if(!askRemoteValue(id32, &target, size))
4253 yError() <<
"embObjMotionControl::getRefVelocityRaw() could not read reference vel for " << getBoardInfo() <<
"joint " << axis;
4256 *ref = (double) target.trgt_velocity;
4259 *ref = _ref_command_speeds[axis];
4266 #if ASK_REFERENCE_TO_FIRMWARE
4267 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
4268 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
4271 yError() <<
"embObjMotionControl::getRefVelocitiesRaw() could not read reference vel for " << getBoardInfo() ;
4275 for(
int j=0; j<_njoints; j++)
4277 refs[j] = (double) targetList[j].trgt_velocity;
4281 for(
int j=0; j<_njoints; j++)
4283 refs[j] = _ref_command_speeds[j];
4291 std::vector <double> refsList(_njoints);
4295 for (
int i = 0; i<nj; i++)
4297 if(
jnts[i]>= _njoints)
4299 yError() << getBoardInfo() <<
"getRefVelocitiesRaw: joint " <<
jnts[i] <<
"doesn't exist";
4302 refs[i] = refsList[
jnts[i]];
4309 if (axis<0 || axis>_njoints)
return false;
4310 #if ASK_REFERENCE_TO_FIRMWARE
4311 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4315 eOmc_joint_status_target_t target = {0};
4316 if(!askRemoteValue(id32, &target, size))
4318 yError() <<
"embObjMotionControl::getRefPositionRaw() could not read reference pos for " << getBoardInfo() <<
"joint " << axis;
4322 *ref = (double) target.trgt_positionraw;
4325 *ref = _ref_positions[axis];
4332 #if ASK_REFERENCE_TO_FIRMWARE
4333 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
4334 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
4337 yError() <<
"embObjMotionControl::getRefPositionRaw() could not read reference pos for " << getBoardInfo();
4341 for(
int j=0; j< _njoints; j++)
4342 refs[j] = (
double) targetList[j].trgt_positionraw;
4345 for(
int j=0; j< _njoints; j++)
4346 refs[j] = _ref_positions[j];
4354 for (
int i = 0; i<nj; i++)
4367 eOenum08_t interactionmodestatus;
4370 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core_modes_interactionmodestatus);
4374 int tmp = (int) *_mode;
4378 *_mode = (yarp::dev::InteractionModeEnum)
tmp;
4397 for(
int j=0; j<_njoints; j++)
4407 eOenum08_t interactionmodecommand = 0;
4412 if (_mode == VOCAB_IM_COMPLIANT && _trq_pids[j].enabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
return false;}
4416 yError() <<
"setInteractionModeRaw: received unknown mode for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
4419 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4421 if(
false == res->
setRemoteValue(protid, &interactionmodecommand) )
4423 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
4429 eOenum08_t interactionmodestatus = 0;
4431 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4432 bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4434 if((
false == ret) || (interactionmodecommand != interactionmodestatus))
4436 yError() <<
"check of embObjMotionControl::setInteractionModeRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
4449 eOenum08_t interactionmodecommand = 0;
4451 for(
int j=0; j<n_joints; j++)
4453 if (modes[j] == VOCAB_IM_COMPLIANT && _trq_pids[j].enabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
continue;}
4457 yError() <<
"embObjMotionControl::setInteractionModesRaw(): received unknown interactionMode for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]) <<
" " << modes[j];
4461 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4462 if(
false == res->
setRemoteValue(protid, &interactionmodecommand) )
4464 yError() <<
"embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4470 eOenum08_t interactionmodestatus = 0;
4472 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4473 bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4475 if((
false == ret) || (interactionmodecommand != interactionmodestatus))
4479 yError() <<
"check of embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4485 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4486 << Vocab32::decode(modes[j]) <<
" Got " << Vocab32::decode(
tmp);
4488 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4489 << Vocab32::decode(modes[j]) <<
" Got an unknown value!";
4502 eOenum08_t interactionmodecommand = 0;
4504 for(
int j=0; j<_njoints; j++)
4506 if ((modes[j] == VOCAB_IM_COMPLIANT) && (_trq_pids[j].enabled ==
false))
4508 yError()<<
"Torque control is disabled. Check your configuration parameters";
4514 yError() <<
"setInteractionModeRaw: received unknown interactionMode for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4518 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4519 if(
false == res->
setRemoteValue(protid, &interactionmodecommand) )
4521 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4527 eOenum08_t interactionmodestatus = 0;
4529 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4530 bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4532 if((
false == ret) || (interactionmodecommand != interactionmodestatus))
4536 yError() <<
"check of embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4542 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4543 << Vocab32::decode(modes[j]) <<
" Got " << Vocab32::decode(
tmp);
4545 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4546 << Vocab32::decode(modes[j]) <<
" Got an unknown value!";
4559 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
4560 eOmc_joint_status_core_t jcore = {0};
4567 case VOCAB_PIDTYPE_POSITION:
4568 if((eomc_controlmode_torque == jcore.modes.controlmodestatus) || (eomc_controlmode_current == jcore.modes.controlmodestatus))
4571 *
out = (double) jcore.ofpid.generic.output;
4576 case VOCAB_PIDTYPE_TORQUE:
4577 if ((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
4578 ((eomc_controlmode_position == jcore.modes.controlmodestatus) && (eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus)))
4579 *
out = jcore.ofpid.generic.output;
4583 case VOCAB_PIDTYPE_CURRENT:
4586 case VOCAB_PIDTYPE_VELOCITY:
4590 yError()<<
"Invalid pidtype:"<<pidtype;
4599 for(
int j=0; j< _njoints; j++)
4619 eOmc_motor_status_basic_t status;
4620 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
4625 *val = (double) status.mot_temperature;
4629 yError() <<
"embObjMotionControl::getTemperatureRaw failed for" << getBoardInfo() <<
" motor " << m ;
4639 for(
int j=0; j< _njoints; j++)
4648 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_temperaturelimit);
4650 eOmeas_temperature_t temperaturelimit = {0};
4652 if(!askRemoteValue(protid, &temperaturelimit, size))
4654 yError() <<
"embObjMotionControl::getTemperatureLimitRaw() can't read temperature limits for" << getBoardInfo() <<
" motor " << m;
4658 *temp = (double) temperaturelimit;
4665 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_temperaturelimit);
4666 eOmeas_temperature_t temperatureLimit = (eOmeas_pwm_t)
S_16(temp);
4673 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4675 eOmc_current_limits_params_t currentlimits = {0};
4677 if(!askRemoteValue(protid, ¤tlimits, size))
4679 yError() <<
"embObjMotionControl::getPeakCurrentRaw() can't read current limits for" << getBoardInfo() <<
" motor " << m;
4683 *val = (double) currentlimits.peakCurrent ;
4689 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4692 eOmc_current_limits_params_t currentlimits = {0};
4693 if(!askRemoteValue(protid, ¤tlimits, size))
4695 yError() <<
"embObjMotionControl::setPeakCurrentRaw can't read current limits for" << getBoardInfo() <<
" motor " << m ;
4700 currentlimits.peakCurrent = (eOmeas_current_t)
S_16(val);
4706 yError() <<
"embObjMotionControl::setPeakCurrentRaw failed sending new value for" << getBoardInfo() <<
" motor " << m ;
4713 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4715 eOmc_current_limits_params_t currentlimits = {0};
4717 if(!askRemoteValue(protid, ¤tlimits, size))
4719 yError() <<
"embObjMotionControl::getNominalCurrentRaw() can't read current limits for" << getBoardInfo() <<
" motor " << m;
4723 *val = (double) currentlimits.nominalCurrent ;
4729 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4733 eOmc_current_limits_params_t currentlimits = {0};
4734 if(!askRemoteValue(protid, ¤tlimits, size))
4736 yError() <<
"embObjMotionControl::setNominalCurrentRaw can't read current limits for" << getBoardInfo() <<
" motor " << m ;
4741 currentlimits.nominalCurrent = (eOmeas_current_t)
S_16(val);
4747 yError() <<
"embObjMotionControl::setNominalCurrentRaw failed sending new value for" << getBoardInfo() <<
" motor " << m ;
4755 eOmc_motor_status_basic_t status;
4756 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
4761 *val = (double) status.mot_pwm;
4765 yError() <<
"embObjMotionControl::getPWMRaw failed for" << getBoardInfo() <<
" motor " << j ;
4774 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pwmlimit);
4776 eOmeas_pwm_t motorPwmLimit;
4778 bool ret = askRemoteValue(protid, &motorPwmLimit, size);
4781 *val = (double) motorPwmLimit;
4785 yError() <<
"embObjMotionControl::getPWMLimitRaw failed for" << getBoardInfo() <<
" motor " << j ;
4796 yError() <<
"embObjMotionControl::setPWMLimitRaw failed because pwmLimit is negative for" << getBoardInfo() <<
" motor " << j ;
4799 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pwmlimit);
4800 eOmeas_pwm_t motorPwmLimit = (eOmeas_pwm_t)
S_16(val);
4806 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_controller, 0, eoprot_tag_mc_controller_status);
4808 eOmc_controller_status_t controllerStatus;
4810 bool ret = askRemoteValue(protid, &controllerStatus, size);
4813 *val = (double) controllerStatus.supplyVoltage;
4817 yError() <<
"embObjMotionControl::getPowerSupplyVoltageRaw failed for" << getBoardInfo() <<
" motor " << j ;
4824 bool embObjMotionControl::askRemoteValue(eOprotID32_t id32,
void* value, uint16_t& size)
4831 bool embObjMotionControl::askRemoteValues(eOprotEndpoint_t ep, eOprotEntity_t entity, eOprotTag_t tag, std::vector<T>& values)
4833 std::vector<eOprotID32_t> idList;
4834 std::vector<void*> valueList;
4837 for(
int j=0; j<_njoints; j++)
4839 eOprotID32_t protoId = eoprot_ID_get(ep, entity, j, tag);
4840 idList.push_back(protoId);
4841 valueList.push_back((
void*)&values[j]);
4847 yError() <<
"embObjMotionControl::askRemoteValues failed for all joints of" << getBoardInfo();
4856 bool embObjMotionControl::checkRemoteControlModeStatus(
int joint,
int target_mode)
4859 eOenum08_t temp = 0;
4862 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core_modes_controlmodestatus);
4863 const double timeout = 0.250;
4864 const int maxretries = 25;
4865 const double delaybetweenqueries = 0.010;
4869 double timeofstart = yarp::os::Time::now();
4872 for( attempt = 0; attempt < maxretries; attempt++)
4874 ret = askRemoteValue(id32, &temp, size);
4877 yError (
"An error occurred inside embObjMotionControl::checkRemoteControlModeStatus(j=%d, targetmode=%s) for BOARD %s IP %s", joint, yarp::os::Vocab32::decode(target_mode).c_str(), res->
getProperties().
boardnameString.c_str(), res->
getProperties().
ipv4addrString.c_str());
4881 if(current_mode == target_mode)
4886 if((current_mode == VOCAB_CM_IDLE) && (target_mode == VOCAB_CM_FORCE_IDLE))
4891 if(current_mode == VOCAB_CM_HW_FAULT)
4893 if(target_mode != VOCAB_CM_FORCE_IDLE) { yError (
"embObjMotionControl::checkRemoteControlModeStatus(%d, %d) is unable to check the control mode of BOARD %s IP %s because it is now in HW_FAULT", joint, target_mode, res->
getProperties().
boardnameString.c_str(), res->
getProperties().
ipv4addrString.c_str()); }
4898 if((yarp::os::Time::now()-timeofstart) > timeout)
4901 yError (
"A %f sec timeout occured in embObjMotionControl::checkRemoteControlModeStatus(), BOARD %s IP %s, joint %d, current mode: %s, requested: %s", timeout, res->
getProperties().
boardnameString.c_str(), res->
getProperties().
ipv4addrString.c_str(), joint, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(target_mode).c_str());
4906 yWarning (
"embObjMotionControl::checkRemoteControlModeStatus() has done %d attempts and will retry again after a %f sec delay. (BOARD %s IP %s, joint %d) -> current mode = %s, requested = %s", attempt+1, delaybetweenqueries, res->
getProperties().
boardnameString.c_str() , res->
getProperties().
ipv4addrString.c_str(), joint, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(target_mode).c_str());
4908 SystemClock::delaySystem(delaybetweenqueries);
4913 yError(
"failure of embObjMotionControl::checkRemoteControlModeStatus(j=%d, targetmode=%s) for BOARD %s IP %s after %d attempts and %f seconds", joint, yarp::os::Vocab32::decode(target_mode).c_str(), res->
getProperties().
boardnameString.c_str(), res->
getProperties().
ipv4addrString.c_str(), attempt, yarp::os::Time::now()-timeofstart);
4921 bool embObjMotionControl::iNeedCouplingsInfo(
void)
4923 eOmn_serv_type_t mc_serv_type = (eOmn_serv_type_t)serviceConfig.
ethservice.configuration.type;
4924 if( (mc_serv_type == eomn_serv_MC_foc) ||
4925 (mc_serv_type == eomn_serv_MC_mc4plus) ||
4926 (mc_serv_type == eomn_serv_MC_mc4plusmais) ||
4927 (mc_serv_type == eomn_serv_MC_mc2pluspsc) ||
4928 (mc_serv_type == eomn_serv_MC_mc4plusfaps)
4929 || (mc_serv_type == eomn_serv_MC_mc4pluspmc)
4939 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
4941 eOmc_setpoint_t setpoint;
4943 setpoint.type = (eOenum08_t)eomc_setpoint_openloop;
4944 setpoint.to.openloop.value = (eOmeas_pwm_t)
S_16(v);
4952 for (
int j = 0; j<_njoints; j++)
4961 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_target);
4964 eOmc_joint_status_target_t target = { 0 };
4967 if (!askRemoteValue(protoId, &target, size))
4969 yError() <<
"embObjMotionControl::getRefDutyCycleRaw() could not read openloop reference for " << getBoardInfo() <<
"joint " << j;
4973 *v = (double)target.trgt_pwm;
4980 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
4981 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
4984 yError() <<
"embObjMotionControl::getDutyCyclesRaw failed for all joints of" << getBoardInfo();
4987 for (
int j = 0; j<_njoints; j++)
4989 v[j]= targetList[j].trgt_pwm;
4996 eOmc_motor_status_basic_t status;
4997 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
5002 *v = (double)status.mot_pwm;
5006 yError() <<
"embObjMotionControl::getDutyCycleRaw failed for" << getBoardInfo() <<
" motor " << j;
5016 for (
int j = 0; j< _njoints; j++)
5038 for (
int j = 0; j< _njoints; j++)
5048 for (
int j = 0; j<_njoints; j++)
5057 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
5059 eOmc_setpoint_t setpoint;
5061 setpoint.type = (eOenum08_t)eomc_setpoint_current;
5062 setpoint.to.current.value = (eOmeas_pwm_t)
S_16(t);
5070 for (
int j = 0; j<n_joint; j++)
5079 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
5080 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
5083 yError() <<
"embObjMotionControl::getDutyCyclesRaw failed for all joints of" << getBoardInfo();
5086 for (
int j = 0; j<_njoints; j++)
5088 t[j] = targetList[j].trgt_current;
5095 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_target);
5098 eOmc_joint_status_target_t target = { 0 };
5101 if (!askRemoteValue(protoId, &target, size))
5103 yError() <<
"embObjMotionControl::getRefDutyCycleRaw() could not read openloop reference for " << getBoardInfo() <<
"joint " << j;
5107 *t = (double)target.trgt_current;
5112 bool embObjMotionControl::helper_setCurPidRaw(
int j,
const Pid &pid)
5114 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pidcurrent);
5118 if (!_cur_pids[j].enabled)
5120 yError() <<
"eoMc " << getBoardInfo() <<
": it is not possible set current pid for motor " << j <<
", because current pid is not enabled in xml files";
5128 yError() <<
"while setting velocity PIDs for" << getBoardInfo() <<
" joint " << j;
5137 bool embObjMotionControl::helper_setSpdPidRaw(
int j,
const Pid &pid)
5139 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pidspeed);
5143 if (!_cur_pids[j].enabled)
5145 yError() <<
"eoMc " << getBoardInfo() <<
": it is not possible set speed pid for motor " << j <<
", because speed pid is not enabled in xml files";
5153 yError() <<
"while setting velocity PIDs for" << getBoardInfo() <<
" joint " << j;
5162 bool embObjMotionControl::helper_getCurPidRaw(
int j, Pid *pid)
5164 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
5166 eOmc_motor_config_t motor_cfg;
5167 if(! askRemoteValue(protoid, &motor_cfg, size))
5171 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg.pidcurrent;
5177 bool embObjMotionControl::helper_getSpdPidRaw(
int j, Pid *pid)
5179 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
5181 eOmc_motor_config_t motor_cfg;
5182 if (!askRemoteValue(protoid, &motor_cfg, size))
5186 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg.pidspeed;
5192 bool embObjMotionControl::helper_getCurPidsRaw(Pid *pid)
5194 std::vector <eOmc_motor_config_t> motor_cfg_list(_njoints);
5195 bool ret = askRemoteValues<eOmc_motor_config_t>(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, eoprot_tag_mc_motor_config, motor_cfg_list);
5199 for(
int j=0; j<_njoints; j++)
5201 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg_list[j].pidcurrent;
5207 bool embObjMotionControl::helper_getSpdPidsRaw(Pid *pid)
5209 std::vector <eOmc_motor_config_t> motor_cfg_list(_njoints);
5210 bool ret = askRemoteValues<eOmc_motor_config_t>(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, eoprot_tag_mc_motor_config, motor_cfg_list);
5214 for (
int j = 0; j<_njoints; j++)
5216 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg_list[j].pidspeed;
5222 bool embObjMotionControl::getJointConfiguration(
int joint, eOmc_joint_config_t *jntCfg_ptr)
5224 uint32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_config);
5226 if(!askRemoteValue(protoid, jntCfg_ptr, size))
5234 bool embObjMotionControl::getMotorConfiguration(
int axis, eOmc_motor_config_t *motCfg_ptr)
5236 uint32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, axis, eoprot_tag_mc_motor_config);
5238 if(!askRemoteValue(protoid, motCfg_ptr, size))
5247 bool embObjMotionControl::getGerabox_E2J(
int joint,
double *gearbox_E2J_ptr)
5249 eOmc_joint_config_t jntCfg;
5251 if(!getJointConfiguration(joint, &jntCfg))
5256 *gearbox_E2J_ptr = jntCfg.gearbox_E2J;
5260 bool embObjMotionControl::getJointEncTolerance(
int joint,
double *jEncTolerance_ptr)
5262 eOmc_joint_config_t jntCfg;
5264 if(!getJointConfiguration(joint, &jntCfg))
5269 *jEncTolerance_ptr = jntCfg.jntEncTolerance;
5273 bool embObjMotionControl::getMotorEncTolerance(
int axis,
double *mEncTolerance_ptr)
5275 eOmc_motor_config_t motorCfg;
5276 if(!getMotorConfiguration(axis, &motorCfg))
5281 *mEncTolerance_ptr = motorCfg.rotEncTolerance;
5287 eOmc_motor_status_t status;
5289 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol,
5290 eoprot_entity_mc_motor, j,
5291 eoprot_tag_mc_motor_status);
5300 message =
"Could not retrieve the fault state.";
5304 if (status.mc_fault_state == EOERROR_CODE_DUMMY)
5306 fault = EOERROR_CODE_DUMMY;
5307 message =
"No fault detected.";
5312 fault = eoerror_code2value(status.mc_fault_state);
5313 message = eoerror_code2string(status.mc_fault_state);
#define MAX_POSITION_MOVE_INTERVAL
bool parseService(yarp::os::Searchable &config, servConfigMais_t &maisconfig)
servMC_encoder_t * getEncoderAtMotor(int index)
servMC_encoder_t * getEncoderAtJoint(int index)
virtual const Properties & getProperties()=0
virtual bool setcheckRemoteValue(const eOprotID32_t id32, void *value, const unsigned int retries=10, const double waitbeforecheck=0.001, const double timeout=0.050)=0
virtual bool serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t *param, double timeout=0.500)=0
virtual bool setRemoteValue(const eOprotID32_t id32, void *value)=0
virtual bool verifyEPprotocol(eOprot_endpoint_t ep)=0
virtual bool getLocalValue(const eOprotID32_t id32, void *value)=0
virtual bool serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)=0
virtual bool serviceStart(eOmn_serv_category_t category, double timeout=0.500)=0
virtual bool getRemoteValue(const eOprotID32_t id32, void *value, const double timeout=0.100, const unsigned int retries=0)=0
virtual bool getRemoteValues(const std::vector< eOprotID32_t > &id32s, const std::vector< void * > &values, const double timeout=0.500)=0
int releaseResource2(eth::AbstractEthResource *ethresource, IethResource *interface)
bool verifyEthBoardInfo(yarp::os::Searchable &cfgtotal, eOipv4addr_t &boardipv4, string boardipv4string, string boardname)
static bool killYourself()
static TheEthManager * instance()
eth::AbstractEthResource * requestResource2(IethResource *interface, yarp::os::Searchable &cfgtotal)
bool start()
Instantiates the yarp::os::Timer object and starts it.
bool canprint()
Called by the object that implements the downsampler.
virtual bool getPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double *ref) override
virtual bool setVelLimitsRaw(int axis, double min, double max) override
virtual bool setMaxCurrentRaw(int j, double val) override
virtual bool getMotorEncoderRaw(int m, double *v) override
virtual bool enablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool setPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, const double *limits) override
virtual bool setRefSpeedsRaw(const double *spds) override
virtual bool getTorqueRangesRaw(double *min, double *max) override
virtual bool getControlModesRaw(int *v) override
virtual bool getRefCurrentRaw(int j, double *t) override
virtual bool setInteractionModeRaw(int j, yarp::dev::InteractionModeEnum _mode) override
virtual bool getPidOutputRaw(const PidControlTypeEnum &pidtype, int j, double *out) override
virtual bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
bool getRotorEncoderTypeRaw(int j, int &type)
virtual bool setRefAccelerationsRaw(const double *accs) override
virtual bool disablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool getImpedanceRaw(int j, double *stiffness, double *damping) override
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
virtual bool setPidOffsetRaw(const PidControlTypeEnum &pidtype, int j, double v) override
virtual bool getPowerSupplyVoltageRaw(int j, double *val) override
virtual bool setPidsRaw(const PidControlTypeEnum &pidtype, const Pid *pids) override
virtual bool getPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double *limit) override
virtual bool setPeakCurrentRaw(int m, const double val) override
virtual bool enableAmpRaw(int j) override
bool getHasRotorEncoderRaw(int j, int &ret)
virtual bool setRefTorqueRaw(int j, double t) override
virtual bool relativeMoveRaw(int j, double delta) override
virtual bool disableAmpRaw(int j) override
virtual bool getRefVelocityRaw(const int joint, double *ref) override
virtual bool getEncodersRaw(double *encs) override
virtual bool getLastJointFaultRaw(int j, int &fault, std::string &message) override
virtual bool getDutyCyclesRaw(double *v) override
virtual bool getPidReferencesRaw(const PidControlTypeEnum &pidtype, double *refs) override
virtual bool getMotorEncoderAccelerationsRaw(double *accs) override
virtual bool resetMotorEncoderRaw(int m) override
virtual bool checkMotionDoneRaw(bool *flag) override
virtual bool getPWMLimitRaw(int j, double *val) override
virtual bool getControlModeRaw(int j, int *v) override
virtual bool getPWMRaw(int j, double *val) override
bool getRotorLimitsRaw(int j, double *rotorMin, double *rotorMax)
virtual bool getTorqueRaw(int j, double *t) override
virtual bool getMotorEncodersRaw(double *encs) override
virtual bool open(yarp::os::Searchable &par)
virtual bool getEncodersTimedRaw(double *encs, double *stamps) override
bool getHasHallSensorRaw(int j, int &ret)
bool getJointEncoderResolutionRaw(int m, double &jntres)
virtual bool getRefSpeedsRaw(double *spds) override
virtual bool getInteractionModeRaw(int j, yarp::dev::InteractionModeEnum *_mode) override
virtual eth::iethresType_t type()
virtual bool getCurrentRangesRaw(double *min, double *max) override
virtual bool getLimitsRaw(int axis, double *min, double *max) override
virtual bool setPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double ref) override
virtual bool updateVirtualAnalogSensorMeasure(yarp::sig::Vector &fTorques) override
virtual bool setImpedanceRaw(int j, double stiffness, double damping) override
virtual bool setRemoteVariableRaw(std::string key, const yarp::os::Bottle &val) override
virtual bool getCurrentsRaw(double *vals) override
virtual bool getTorqueRangeRaw(int j, double *min, double *max) override
virtual bool getTorquesRaw(double *t) override
virtual bool getAmpStatusRaw(int *st) override
virtual yarp::dev::VAS_status getVirtualAnalogSensorStatus(int ch) override
bool getHasTempSensorsRaw(int j, int &ret)
virtual bool setRefCurrentsRaw(const double *t) override
virtual bool getTargetPositionRaw(const int joint, double *ref) override
virtual bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
virtual bool getGearboxRatioRaw(int m, double *gearbox) override
virtual bool resetEncodersRaw() override
virtual bool getTemperatureLimitRaw(int m, double *temp) override
virtual bool setCalibrationParametersRaw(int axis, const CalibrationParameters ¶ms) override
virtual bool setNominalCurrentRaw(int m, const double val) override
virtual bool setMotorTorqueParamsRaw(int j, const MotorTorqueParameters params) override
virtual bool setRefCurrentRaw(int j, double t) override
virtual bool setTemperatureLimitRaw(int m, const double temp) override
virtual bool getRemoteVariableRaw(std::string key, yarp::os::Bottle &val) override
virtual bool calibrationDoneRaw(int j) override
virtual bool getEncoderTimedRaw(int j, double *encs, double *stamp) override
virtual bool getAxisNameRaw(int axis, std::string &name) override
virtual bool setPWMLimitRaw(int j, const double val) override
virtual bool getRefAccelerationRaw(int j, double *acc) override
virtual bool resetPidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool getPidRaw(const PidControlTypeEnum &pidtype, int j, Pid *pid) override
virtual bool setPositionRaw(int j, double ref) override
virtual bool getRefSpeedRaw(int j, double *ref) override
virtual bool setPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double limit) override
virtual bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
bool getRotorEncoderResolutionRaw(int m, double &rotres)
virtual bool setRefDutyCycleRaw(int j, double v) override
virtual bool getImpedanceOffsetRaw(int j, double *offset) override
bool getWholeImpedanceRaw(int j, eOmc_impedance_t &imped)
virtual bool getEncoderSpeedsRaw(double *spds) override
virtual bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
virtual bool resetMotorEncodersRaw() override
bool getJointEncoderTypeRaw(int j, int &type)
virtual bool getNumberOfMotorEncodersRaw(int *num) override
virtual bool getPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, double *limits) override
virtual bool setMotorEncoderRaw(int m, const double val) override
virtual bool stopRaw() override
virtual bool getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
virtual bool setEncoderRaw(int j, double val) override
virtual bool resetEncoderRaw(int j) override
virtual bool setPidRaw(const PidControlTypeEnum &pidtype, int j, const Pid &pid) override
virtual bool getRefAccelerationsRaw(double *accs) override
virtual bool getPidOutputsRaw(const PidControlTypeEnum &pidtype, double *outs) override
virtual bool getRefPositionsRaw(double *refs) override
virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
virtual bool getNominalCurrentRaw(int m, double *val) override
virtual bool initialised()
virtual bool setRefTorquesRaw(const double *t) override
virtual bool getRefDutyCyclesRaw(double *v) override
virtual bool getCurrentRaw(int j, double *val) override
bool getMotorPolesRaw(int j, int &poles)
virtual bool getCurrentRangeRaw(int j, double *min, double *max) override
virtual bool getRefTorquesRaw(double *t) override
virtual bool setEncodersRaw(const double *vals) override
virtual bool getEncoderRaw(int j, double *v) override
virtual bool setControlModeRaw(const int j, const int mode) override
virtual bool positionMoveRaw(int j, double ref) override
bool getKinematicMJRaw(int j, double &rotres)
virtual bool setLimitsRaw(int axis, double min, double max) override
virtual bool getEncoderAccelerationRaw(int j, double *spds) override
virtual bool getMotorTorqueParamsRaw(int j, MotorTorqueParameters *params) override
virtual bool getTemperatureRaw(int m, double *val) override
virtual bool setPidReferencesRaw(const PidControlTypeEnum &pidtype, const double *refs) override
virtual bool getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
virtual bool getVelLimitsRaw(int axis, double *min, double *max) override
virtual bool getRefTorqueRaw(int j, double *t) override
virtual bool getNumberOfMotorsRaw(int *num) override
virtual bool setRefSpeedRaw(int j, double sp) override
virtual bool getMotorEncoderSpeedsRaw(double *spds) override
virtual bool getMaxCurrentRaw(int j, double *val) override
virtual bool getTemperaturesRaw(double *vals) override
virtual bool getMotorEncoderCountsPerRevolutionRaw(int m, double *v) override
virtual bool getEncoderAccelerationsRaw(double *accs) override
virtual bool getPidsRaw(const PidControlTypeEnum &pidtype, Pid *pids) override
virtual bool getRemoteVariablesListRaw(yarp::os::Bottle *listOfKeys) override
bool getTorqueControlFilterType(int j, int &type)
virtual bool getRefCurrentsRaw(double *t) override
virtual bool getPidErrorRaw(const PidControlTypeEnum &pidtype, int j, double *err) override
virtual bool setImpedanceOffsetRaw(int j, double offset) override
virtual bool getRefDutyCycleRaw(int j, double *v) override
virtual bool getMotorEncodersTimedRaw(double *encs, double *stamps) override
virtual int getVirtualAnalogSensorChannels() override
bool getRotorIndexOffsetRaw(int j, double &rotorOffset)
virtual bool setMotorEncodersRaw(const double *vals) override
virtual bool isPidEnabledRaw(const PidControlTypeEnum &pidtype, int j, bool *enabled) override
virtual bool setRefAccelerationRaw(int j, double acc) override
virtual bool getEncoderSpeedRaw(int j, double *sp) override
virtual bool getMotorEncoderSpeedRaw(int m, double *sp) override
virtual bool getMotorEncoderAccelerationRaw(int m, double *spds) override
virtual bool getAxes(int *ax) override
virtual bool getRefPositionRaw(const int joint, double *ref) override
virtual bool setRefDutyCyclesRaw(const double *v) override
virtual bool velocityMoveRaw(int j, double sp) override
virtual bool getPeakCurrentRaw(int m, double *val) override
virtual bool getPidErrorsRaw(const PidControlTypeEnum &pidtype, double *errs) override
virtual bool getRefVelocitiesRaw(double *refs) override
virtual bool getTargetPositionsRaw(double *refs) override
bool getHasRotorEncoderIndexRaw(int j, int &ret)
virtual bool getDutyCycleRaw(int j, double *v) override
bool parseJointsetCfgGroup(yarp::os::Searchable &config, std::vector< JointsSet > &jsets, std::vector< int > &jointtoset)
bool parseJointsLimits(yarp::os::Searchable &config, std::vector< jointLimits_t > &jointsLimits)
bool parseAmpsToSensor(yarp::os::Searchable &config, double ampsToSensor[])
bool parseMechanicalsFlags(yarp::os::Searchable &config, int useMotorSpeedFbk[])
bool parseMotioncontrolVersion(yarp::os::Searchable &config, int &version)
bool parseAxisInfo(yarp::os::Searchable &config, int axisMap[], std::vector< axisInfo_t > &axisInfo)
bool parseCouplingInfo(yarp::os::Searchable &config, couplingInfo_t &couplingInfo)
bool parsePids(yarp::os::Searchable &config, PidInfo *ppids, TrqPidInfo *tpids, PidInfo *cpids, PidInfo *spids, bool lowLevPidisMandatory)
bool parseFocGroup(yarp::os::Searchable &config, focBasedSpecificInfo_t *foc_based_info, std::string groupName)
bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found)
bool isVerboseEnabled(yarp::os::Searchable &config)
bool parseTimeoutsGroup(yarp::os::Searchable &config, std::vector< timeouts_t > &timeouts, int defaultVelocityTimeout)
bool parseCurrentLimits(yarp::os::Searchable &config, std::vector< motorCurrentLimits_t > &currLimits)
bool parseEncoderFactor(yarp::os::Searchable &config, double encoderFactor[])
bool parsefullscalePWM(yarp::os::Searchable &config, double dutycycleToPWM[])
bool parseRotorsLimits(yarp::os::Searchable &config, std::vector< rotorLimits_t > &rotorsLimits)
bool parseImpedanceGroup(yarp::os::Searchable &config, std::vector< impedanceParameters_t > &impedance)
bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector< kalmanFilterParams_t > &kalmanFilterParams)
bool parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J[], double gearbox_E2J[])
bool parseBehaviourFalgs(yarp::os::Searchable &config, bool &useRawEncoderData, bool &pwmIsLimited)
std::string usernamePidSelected
yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits
eOmc_ctrl_out_type_t out_type
yarp::dev::PidOutputUnitsEnum out_PidUnits
static uint32_t idx[BOARD_NUM]
static bool nv_not_found(void)
static bool NOT_YET_IMPLEMENTED(const char *txt)
#define PARSER_MOTION_CONTROL_VERSION
static bool DEPRECATED(const char *txt)
const double jointWithAKSIM2
const double jointWithAEA3
const double jointWithAMO
const double jointWithAEA
bool EncoderType_eo2iCub(const uint8_t *in, string *out)
int controlModeStatusConvert_embObj2yarp(eOenum08_t embObjMode)
bool interactionModeStatusConvert_embObj2yarp(eOenum08_t embObjMode, int &vocabOut)
bool controlModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
void copyPid_eo2iCub(eOmc_PID_t *in, Pid *out)
void copyPid_iCub2eo(const Pid *in, eOmc_PID_t *out)
bool interactionModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
std::vector< BufferedPort< Bottle > * > ports
eOmn_serv_diagn_cfg_t config
eOmn_serv_parameter_t ethservice
eOmc_encoder_descriptor_t desc
bool useRawEncoderData
its value depends on environment variable "ETH_VERBOSEWHENOK"
bool pwmIsLimited
if true than do not use calibration data
std::vector< double > matrixM2J
std::vector< double > matrixE2J
std::vector< double > matrixJ2M
bool hasRotorEncoderIndex