11 #include <yarp/os/Bottle.h>
12 #include <yarp/os/Time.h>
19 #include <FeatureInterface.h>
20 #include <yarp/conf/environment.h>
22 #include <yarp/os/LogStream.h>
26 #include "EoProtocol.h"
27 #include "EoProtocolMN.h"
28 #include "EoProtocolMC.h"
29 #include "EoProtocolAS.h"
32 #include <yarp/os/NetType.h>
33 #include <yarp/dev/ControlBoardHelper.h>
39 using namespace yarp::os;
40 using namespace yarp::os::impl;
48 #define ASK_REFERENCE_TO_FIRMWARE 1
50 #define PARSER_MOTION_CONTROL_VERSION 6
58 yError() << txt <<
" is not yet implemented for embObjMotionControl";
64 yError() << txt <<
" has been deprecated for embObjMotionControl";
68 #define NV_NOT_FOUND return nv_not_found();
72 yError () <<
" nv_not_found!! This may mean that this variable is not handled by this EMS\n";
81 std::string embObjMotionControl::getBoardInfo(
void)
85 return " BOARD name_unknown (IP unknown) ";
89 return (
"BOARD " + res->getProperties().boardnameString +
" (IP " + res->getProperties().ipv4addrString +
") ");
94 bool embObjMotionControl::alloc(
int nj)
96 _axisMap = allocAndCheck<int>(nj);
98 _encodersStamp = allocAndCheck<double>(nj);
99 _gearbox_M2J = allocAndCheck<double>(nj);
100 _gearbox_E2J = allocAndCheck<double>(nj);
101 _deadzone = allocAndCheck<double>(nj);
102 _foc_based_info= allocAndCheck<eomc::focBasedSpecificInfo_t>(nj);
108 _impedance_limits=allocAndCheck<eomc::impedanceLimits_t>(nj);
109 checking_motiondone=allocAndCheck<bool>(nj);
110 _last_position_move_time=allocAndCheck<double>(nj);
113 _ref_command_positions = allocAndCheck<double>(nj);
114 _ref_positions = allocAndCheck<double>(nj);
115 _ref_command_speeds = allocAndCheck<double>(nj);
116 _ref_speeds = allocAndCheck<double>(nj);
117 _ref_accs = allocAndCheck<double>(nj);
119 _enabledAmp = allocAndCheck<bool>(nj);
120 _enabledPid = allocAndCheck<bool>(nj);
121 _calibrated = allocAndCheck<bool>(nj);
122 _cacheImpedance = allocAndCheck<eOmc_impedance_t>(nj);
124 _rotorsLimits.resize(nj);
125 _jointsLimits.resize(nj);
126 _currentLimits.resize(nj);
127 _temperatureLimits.resize(nj);
129 _joint2set.resize(nj);
130 _timeouts.resize(nj);
131 _impedance_params.resize(nj);
132 _axesInfo.resize(nj);
133 _jointEncs.resize(nj);
134 _motorEncs.resize(nj);
135 _kalman_params.resize(nj);
136 _temperatureSensorsVector.resize(nj);
137 _temperatureExceededLimitWatchdog.resize(nj);
138 _temperatureSensorErrorWatchdog.resize(nj);
143 bool embObjMotionControl::dealloc()
145 checkAndDestroy(_axisMap);
146 checkAndDestroy(_encodersStamp);
147 checkAndDestroy(_gearbox_M2J);
148 checkAndDestroy(_gearbox_E2J);
149 checkAndDestroy(_deadzone);
150 checkAndDestroy(_impedance_limits);
151 checkAndDestroy(checking_motiondone);
152 checkAndDestroy(_ref_command_positions);
153 checkAndDestroy(_ref_positions);
154 checkAndDestroy(_ref_command_speeds);
155 checkAndDestroy(_ref_speeds);
156 checkAndDestroy(_ref_accs);
158 checkAndDestroy(_enabledAmp);
159 checkAndDestroy(_enabledPid);
160 checkAndDestroy(_calibrated);
161 checkAndDestroy(_foc_based_info);
183 ImplementControlCalibration(this),
184 ImplementAmplifierControl(this),
185 ImplementPidControl(this),
186 ImplementEncodersTimed(this),
187 ImplementPositionControl(this),
188 ImplementVelocityControl(this),
189 ImplementControlMode(this),
190 ImplementImpedanceControl(this),
191 ImplementMotorEncoders(this),
192 #ifdef IMPLEMENT_DEBUG_INTERFACE
195 ImplementTorqueControl(this),
196 ImplementControlLimits(this),
197 ImplementPositionDirect(this),
198 ImplementInteractionMode(this),
199 ImplementMotor(this),
200 ImplementRemoteVariables(this),
201 ImplementAxisInfo(this),
202 ImplementPWMControl(this),
203 ImplementCurrentControl(this),
204 ImplementJointFault(this),
205 SAFETY_THRESHOLD(2.0),
209 _temperatureLimits(0),
213 _impedance_params(0),
218 _temperatureSensorsVector(0),
219 _temperatureExceededLimitWatchdog(0),
220 _temperatureSensorErrorWatchdog(0)
234 _encodersStamp = NULL;
235 _foc_based_info = NULL;
236 _cacheImpedance = NULL;
237 _impedance_limits = NULL;
239 _ref_command_speeds = NULL;
240 _ref_command_positions= NULL;
241 _ref_positions = NULL;
243 _measureConverter = NULL;
245 checking_motiondone = NULL;
256 _last_position_move_time = NULL;
261 std::string
tmp = yarp::conf::environment::get_string(
"ETH_VERBOSEWHENOK");
273 #ifdef NETWORK_PERFORMANCE_BENCHMARK
277 m_responseTimingVerifier.init(0.003, 0.00025, 0.001, 0.01, 0.0005, 30);
285 yTrace() <<
"embObjMotionControl::~embObjMotionControl()";
293 if(NULL != _mcparser)
311 ImplementControlCalibration::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
312 ImplementAmplifierControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.ampsToSensor);
313 ImplementEncodersTimed::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
314 ImplementMotorEncoders::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
315 ImplementPositionControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
316 ImplementPidControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.newtonsToSensor,
f.ampsToSensor,
f.dutycycleToPWM);
317 ImplementControlMode::initialize(_njoints, _axisMap);
318 ImplementVelocityControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
319 ImplementControlLimits::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
320 ImplementImpedanceControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.newtonsToSensor);
321 ImplementTorqueControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.newtonsToSensor,
f.ampsToSensor,
f.dutycycleToPWM,
f.bemf2raw,
f.ktau2raw);
322 ImplementPositionDirect::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
323 ImplementInteractionMode::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
324 ImplementMotor::initialize(_njoints, _axisMap);
325 ImplementRemoteVariables::initialize(_njoints, _axisMap);
326 ImplementAxisInfo::initialize(_njoints, _axisMap);
327 ImplementCurrentControl::initialize(_njoints, _axisMap,
f.ampsToSensor);
328 ImplementPWMControl::initialize(_njoints, _axisMap,
f.dutycycleToPWM);
329 ImplementJointFault::initialize(_njoints, _axisMap);
340 if(NULL == ethManager)
342 yFatal() <<
"embObjMotionControl::open() fails to instantiate ethManager";
346 eOipv4addr_t ipv4addr;
347 string boardIPstring;
349 if(
false == ethManager->
verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
351 yError() <<
"embObjMotionControl::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
370 yError() <<
"embObjMotionControl::open() fails because could not instantiate the ethResource for " << getBoardInfo() <<
" ... unable to continue";
374 if(!fromConfig(config))
376 yError() << getBoardInfo() <<
"Missing motion control parameters in config file";
382 yError() <<
"embObjMotionControl: failed verifyEPprotocol. Cannot continue!";
388 const eOmn_serv_parameter_t* servparam = &serviceConfig.
ethservice;
389 if(eomn_serv_MC_generic == serviceConfig.
ethservice.configuration.type)
395 mcdiagnostics.
config.mode = serviceConfig.
ethservice.configuration.diagnosticsmode;
396 mcdiagnostics.
config.par16 = serviceConfig.
ethservice.configuration.diagnosticsparam;
397 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
400 mcdiagnostics.
ports.resize(2);
401 for(
size_t i=0; i<mcdiagnostics.
ports.size(); i++)
403 mcdiagnostics.
ports[i] =
new BufferedPort<Bottle>;
414 event_downsampler->
config.
info = getBoardInfo();
415 event_downsampler->
start();
419 yError() <<
"embObjMotionControl::open() has an error in call of ethResources::serviceVerifyActivate() for" << getBoardInfo();
424 yDebug() <<
"embObjMotionControl:serviceVerifyActivate OK!";
429 yError() <<
"embObjMotionControl::open() has an error in call of embObjMotionControl::init() for" << getBoardInfo();
436 yDebug() <<
"embObjMotionControl::init() has succesfully initted" << getBoardInfo();
443 yError() <<
"embObjMotionControl::open() fails to start mc service for" << getBoardInfo() <<
": cannot continue";
451 yDebug() <<
"embObjMotionControl::open() correctly starts mc service of" << getBoardInfo();
459 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
490 SystemClock::delaySystem(0.001*mcdiagnostics.
config.par16);
497 int embObjMotionControl::fromConfig_NumOfJoints(yarp::os::Searchable &config)
502 int jn = config.findGroup(
"GENERAL").check(
"Joints", Value(1),
"Number of degrees of freedom").asInt32();
509 void embObjMotionControl::debugUtil_printJointsetInfo(
void)
512 yError() <<
"****** DEBUG PRINTS **********";
513 yError() <<
"joint to set:";
514 for(
int x=0;
x< _njoints;
x++)
515 yError() <<
" /t j " <<
x <<
": set " <<_joint2set[
x];
516 yError() <<
"jointmap:";
518 yError() <<
" number of sets" << _jsets.size();
519 for(
size_t x=0;
x< _jsets.size();
x++)
521 yError() <<
"set " <<
x<<
"has size " <<_jsets[
x].getNumberofJoints();
522 for(
int y=0;
y<_jsets[
x].getNumberofJoints();
y++)
523 yError() <<
"set " <<
x <<
": " << _jsets[
x].joints[
y];
525 yError() <<
"********* END ****************";
532 bool embObjMotionControl::verifyUserControlLawConsistencyInJointSet(
eomc::PidInfo *pidInfo)
535 for(
size_t s=0; s<_jsets.size(); s++)
537 int numofjoints = _jsets[s].getNumberofJoints();
541 yError() <<
"embObjMC" << getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
544 int firstjoint = _jsets[s].joints[0];
546 for(
int k=1; k<numofjoints; k++)
548 int otherjoint = _jsets[s].joints[k];
550 if(pidInfo[firstjoint].usernamePidSelected != pidInfo[otherjoint].usernamePidSelected)
552 yError() <<
"embObjMC "<< getBoardInfo() <<
"Joints beloning to same set must be have same control law. Joint " << otherjoint <<
" differs from " << firstjoint <<
"Set num " << s ;
565 bool embObjMotionControl::verifyUserControlLawConsistencyInJointSet(
eomc::TrqPidInfo *pidInfo)
567 for(
size_t s=0; s<_jsets.size(); s++)
569 int numofjoints = _jsets[s].getNumberofJoints();
573 yError() <<
"embObjMC "<< getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
576 int firstjoint = _jsets[s].joints[0];
578 for(
int k=1; k<numofjoints; k++)
580 int otherjoint = _jsets[s].joints[k];
582 if(pidInfo[firstjoint].usernamePidSelected != pidInfo[otherjoint].usernamePidSelected)
584 yError() <<
"embObjMC"<< getBoardInfo() <<
"Joints beloning to same set must be have same control law. Joint " << otherjoint <<
" differs from " << firstjoint <<
"Set num " << s ;
594 bool embObjMotionControl::updatedJointsetsCfgWithControlInfo()
598 for(
size_t s=0; s<_jsets.size(); s++)
600 if(_jsets[s].getNumberofJoints() == 0)
602 yError() <<
"embObjMC"<< getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
606 int joint = _jsets[s].joints[0];
616 _jsets[s].cfg.pid_output_types.postrj_ctrl_out_type = _trj_pids[joint].
out_type;
617 _jsets[s].cfg.pid_output_types.veltrj_ctrl_out_type = _trj_pids[joint].
out_type;
618 _jsets[s].cfg.pid_output_types.mixtrj_ctrl_out_type = _trj_pids[joint].
out_type;
622 _jsets[s].cfg.pid_output_types.posdir_ctrl_out_type = _trj_pids[joint].
out_type;
623 _jsets[s].cfg.pid_output_types.veldir_ctrl_out_type = _trj_pids[joint].
out_type;
625 _jsets[s].cfg.pid_output_types.torque_ctrl_out_type = _trq_pids[joint].
out_type;
627 _jsets[s].cfg.pid_output_types.pwm_ctrl_out_type = eomc_ctrl_out_type_pwm;
629 if (_cur_pids[joint].enabled)
631 _jsets[s].cfg.pid_output_types.cur_ctrl_out_type = eomc_ctrl_out_type_cur;
635 _jsets[s].cfg.pid_output_types.cur_ctrl_out_type = eomc_ctrl_out_type_n_a;
644 bool embObjMotionControl::saveCouplingsData(
void)
646 eOmc_4jomo_coupling_t *jc_dest;
648 static eOmc_4jomo_coupling_t dummyjomocoupling = {};
650 switch(serviceConfig.
ethservice.configuration.type)
652 case eomn_serv_MC_foc:
654 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.foc_based.jomocoupling);
657 case eomn_serv_MC_mc4plus:
659 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc4plus_based.jomocoupling);
662 case eomn_serv_MC_mc4plusmais:
664 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc4plusmais_based.jomocoupling);
668 case eomn_serv_MC_mc2pluspsc:
670 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc2pluspsc.jomocoupling);
674 case eomn_serv_MC_mc4plusfaps:
676 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc4plusfaps.jomocoupling);
680 case eomn_serv_MC_advfoc:
682 jc_dest = &dummyjomocoupling;
685 case eomn_serv_MC_mc4:
690 case eomn_serv_MC_generic:
702 memset(jc_dest, 0,
sizeof(eOmc_4jomo_coupling_t));
706 for(
int i=0; i<4; i++)
708 jc_dest->joint2set[i] = eomc_jointSetNum_none;
711 if(_joint2set.size() > 4 )
713 yError() <<
"embObjMC "<< getBoardInfo() <<
"Jointsset size is bigger than 4. I can't send jointset information to fw.";
717 for(
size_t i=0; i<_joint2set.size(); i++)
719 jc_dest->joint2set[i] = _joint2set[i];
722 for(
int i=0; i<4; i++)
724 for(
int j=0; j<4; j++)
726 jc_dest->joint2motor[i][j] = eo_common_float_to_Q17_14((
float)_couplingInfo.
matrixJ2M[4*i+j]);
727 jc_dest->motor2joint[i][j] = eo_common_float_to_Q17_14((
float)_couplingInfo.
matrixM2J[4*i+j]);
732 for(
int r=0; r<4; r++)
734 for(
int c=0; c<6; c++)
736 jc_dest->encoder2joint[r][c] = eo_common_float_to_Q17_14((
float)_couplingInfo.
matrixE2J[6*r+c]);
740 for(
size_t s=0; s< _jsets.size(); s++)
742 eOmc_jointset_configuration_t* cfg_ptr = _jsets[s].getConfiguration();
743 memcpy(&(jc_dest->jsetcfg[s]), cfg_ptr,
sizeof(eOmc_jointset_configuration_t));
747 if(eomn_serv_MC_advfoc == serviceConfig.
ethservice.configuration.type)
750 eOmc_adv4jomo_coupling_t *ajc = &serviceConfig.
ethservice.configuration.data.mc.advfoc.adv4jomocoupling;
751 ajc->type = eommccoupling_traditional4x4;
753 std::memmove(&ajc->data.coupling4x4.joint2set[0], &jc_dest->joint2set[0], 4*
sizeof(uint8_t));
754 std::memmove(&ajc->data.coupling4x4.jsetcfg[0], &jc_dest->jsetcfg[0], 4*
sizeof(eOmc_jointset_configuration_t));
755 std::memmove(&ajc->data.coupling4x4.joint2motor, &jc_dest->joint2motor,
sizeof(eOmc_4x4_matrix_t));
756 std::memmove(&ajc->data.coupling4x4.motor2joint, &jc_dest->motor2joint,
sizeof(eOmc_4x4_matrix_t));
758 for(uint8_t r=0; r<4; r++)
760 for(uint8_t c=0; c<4; c++)
762 ajc->data.coupling4x4.encoder2joint4x4[r][c] = jc_dest->encoder2joint[r][c];
772 bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config)
779 if(iNeedCouplingsInfo())
805 for (i = 0; i < _njoints; i++)
807 measConvFactors.angleToEncoder[i] = 1;
828 if(eomn_serv_MC_mc4 != (eOmn_serv_type_t)serviceConfig.
ethservice.configuration.type)
830 int* useMotorSpeedFbk = 0;
831 useMotorSpeedFbk =
new int[_njoints];
834 delete[] useMotorSpeedFbk;
839 if (!verifyUseMotorSpeedFbkInJointSet(useMotorSpeedFbk))
841 delete[] useMotorSpeedFbk;
844 delete[] useMotorSpeedFbk;
846 bool deadzoneIsAvailable;
849 if(!deadzoneIsAvailable)
851 updateDeadZoneWithDefaultValues();
863 bool lowLevPidisMandatory =
false;
865 if((serviceConfig.
ethservice.configuration.type == eomn_serv_MC_foc) || (serviceConfig.
ethservice.configuration.type == eomn_serv_MC_advfoc))
867 lowLevPidisMandatory =
true;
870 if(!_mcparser->
parsePids(config, _trj_pids, _trq_pids, _cur_pids, _spd_pids, lowLevPidisMandatory))
887 updatedJointsetsCfgWithControlInfo();
890 for (i = 0; i < _njoints; i++)
892 measConvFactors.newtonsToSensor[i] = 1000000.0f;
894 measConvFactors.bemf2raw[i] = measConvFactors.newtonsToSensor[i] / measConvFactors.angleToEncoder[i];
895 if (_trq_pids->
out_PidUnits == yarp::dev::PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT)
897 measConvFactors.ktau2raw[i] = measConvFactors.dutycycleToPWM[i] / measConvFactors.newtonsToSensor[i];
899 else if (_trq_pids->
out_PidUnits == yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS)
901 measConvFactors.ktau2raw[i] = 1.0 / measConvFactors.newtonsToSensor[i];
905 yError() <<
"Invalid ktau units";
return false;
910 _measureConverter =
new ControlBoardHelper(_njoints, _axisMap, measConvFactors.angleToEncoder, NULL, measConvFactors.newtonsToSensor, measConvFactors.ampsToSensor,
nullptr, measConvFactors.dutycycleToPWM , measConvFactors.bemf2raw, measConvFactors.ktau2raw);
911 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, _trj_pids->
fbk_PidUnits, _trj_pids->
out_PidUnits);
913 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, _trq_pids->
fbk_PidUnits, _trq_pids->
out_PidUnits);
914 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, _cur_pids->
fbk_PidUnits, _cur_pids->
out_PidUnits);
915 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, _spd_pids->
fbk_PidUnits, _spd_pids->
out_PidUnits);
928 initializeInterfaces(measConvFactors);
929 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, _trj_pids->
fbk_PidUnits, _trj_pids->
out_PidUnits);
931 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, _trq_pids->
fbk_PidUnits, _trq_pids->
out_PidUnits);
932 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, _cur_pids->
fbk_PidUnits, _cur_pids->
out_PidUnits);
933 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, _spd_pids->
fbk_PidUnits, _spd_pids->
out_PidUnits);
937 if(!saveCouplingsData())
944 yError() <<
"embObjMC " << getBoardInfo() <<
"IMPEDANCE section: error detected in parameters syntax";
950 for(j=0; j<_njoints; j++)
953 _impedance_limits[j].
min_damp= 0.001;
954 _impedance_limits[j].
max_damp= 9.888;
957 _impedance_limits[j].
param_a= 0.011;
958 _impedance_limits[j].
param_b= 0.012;
959 _impedance_limits[j].
param_c= 0.013;
980 eOmn_serv_type_t servtype =
static_cast<eOmn_serv_type_t
>(serviceConfig.
ethservice.configuration.type);
982 if((eomn_serv_MC_foc == servtype) || (eomn_serv_MC_advfoc == servtype))
984 std::string groupName = {};
986 if(eomn_serv_MC_foc == servtype)
989 eObrd_type_t brd =
static_cast<eObrd_type_t
>(serviceConfig.
ethservice.configuration.data.mc.foc_based.type);
990 groupName = (eobrd_foc == brd) ?
"2FOC" :
"AMCBLDC";
992 else if(eomn_serv_MC_advfoc == servtype)
996 groupName =
"ADVFOC";
999 if(!_mcparser->
parseFocGroup(config, _foc_based_info, groupName, _temperatureSensorsVector))
1002 for (j = 0; j < _njoints; j++)
1004 if (((_temperatureSensorsVector.at(j)->getType() !=
motor_temperature_sensor_none )) && ((_temperatureLimits[j].hardwareTemperatureLimit == 0) || (_temperatureLimits[j].warningTemperatureLimit == 0)))
1006 yError() <<
"In" << getBoardInfo() <<
"joint" << j <<
": inconsistent configuration, please update it. If Temperature limits are not set then TemperatureSensorType must be NONE or not set and/or HasTempSensor must be zero. Aborting...";
1012 yInfo() <<
"embObjMC " << getBoardInfo() <<
"joint " << j <<
" has motor not provided with any available type of temperature sensor. If needed update the configurations file accordingly";
1018 for (j = 0; j < _njoints; j++)
1020 _temperatureSensorsVector.at(j) = std::make_unique<eomc::TemperatureSensorNONE>();
1036 bool embObjMotionControl::verifyUseMotorSpeedFbkInJointSet(
int useMotorSpeedFbk [])
1038 for(
size_t s=0; s< _jsets.size(); s++)
1040 int numofjointsinset = _jsets[s].getNumberofJoints();
1041 if(numofjointsinset == 0 )
1043 yError() <<
"embObjMC " << getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
1047 int firstjointofset = _jsets[s].joints[0];
1048 for(
int j=1; j<numofjointsinset; j++)
1050 int joint = _jsets[s].joints[j];
1051 if(useMotorSpeedFbk[firstjointofset] != useMotorSpeedFbk[joint])
1053 yError() <<
"embObjMC " << getBoardInfo() <<
". Param useMotorSpeedFbk should have same value for joints belong same set. See joint " << firstjointofset <<
" and " << joint;
1058 _jsets[s].setUseSpeedFeedbackFromMotors(useMotorSpeedFbk[firstjointofset]);
1065 bool embObjMotionControl::verifyTorquePidshasSameUnitTypes(yarp::dev::PidFeedbackUnitsEnum &fbk_pidunits, yarp::dev::PidOutputUnitsEnum& out_pidunits)
1067 fbk_pidunits = yarp::dev::PidFeedbackUnitsEnum::RAW_MACHINE_UNITS;
1068 out_pidunits = yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS;
1070 int firstjoint = -1;
1071 for(
int i=0; i<_njoints; i++)
1073 if(_trq_pids[i].enabled)
1083 for(
int i=firstjoint+1; i<_njoints; i++)
1085 if(_trq_pids[i].enabled)
1087 if(_trq_pids[firstjoint].fbk_PidUnits != _trq_pids[i].fbk_PidUnits ||
1088 _trq_pids[firstjoint].out_PidUnits != _trq_pids[i].out_PidUnits)
1090 yError() <<
"embObjMC " << getBoardInfo() <<
"all joints with torque enabled should have same controlunits type. Joint " << firstjoint <<
" differs from joint " << i;
1101 bool embObjMotionControl::isTorqueControlEnabled(
int joint)
1103 return (_trq_pids[joint].enabled);
1106 bool embObjMotionControl::isVelocityControlEnabled(
int joint)
1109 return (_trj_pids[joint].enabled);
1113 void embObjMotionControl::updateDeadZoneWithDefaultValues(
void)
1115 for(
int i=0; i<_njoints; i++)
1117 switch(_jointEncs[i].
type)
1125 case eomc_enc_aksim2:
1131 case eomc_enc_absanalog:
1134 case eomc_enc_hallmotor:
1135 case eomc_enc_spichainof2:
1136 case eomc_enc_spichainof3:
1146 bool embObjMotionControl::fromConfig_readServiceCfg(yarp::os::Searchable &config)
1149 if(
false == parser->
parseService(config, serviceConfig))
1151 yError() <<
"embObjMC " << getBoardInfo() <<
"cannot parse service" ;
1155 if(eomn_serv_MC_generic == serviceConfig.
ethservice.configuration.type)
1157 yError() <<
"embObjMC " << getBoardInfo() <<
"it is no longer possible use eomn_serv_MC_generic, because firmware cannot configure itself!" ;
1165 for(
int i=0; i<_njoints; i++)
1170 if(NULL == jointEncoder_ptr)
1172 _jointEncs[i].resolution = 1;
1173 _jointEncs[i].type = eomc_enc_none;
1174 _jointEncs[i].tolerance = 0;
1178 _jointEncs[i].resolution = jointEncoder_ptr->
resolution;
1179 _jointEncs[i].type = (eOmc_encoder_t)jointEncoder_ptr->
desc.type;
1180 _jointEncs[i].tolerance = jointEncoder_ptr->
tolerance;
1184 if(NULL == motorEncoder_ptr)
1186 _motorEncs[i].resolution = 1;
1187 _motorEncs[i].type = eomc_enc_none;
1188 _motorEncs[i].tolerance = 0;
1192 _motorEncs[i].resolution = motorEncoder_ptr->
resolution;
1193 _motorEncs[i].type = (eOmc_encoder_t)motorEncoder_ptr->
desc.type;
1194 _motorEncs[i].tolerance = motorEncoder_ptr->
tolerance;
1206 bool embObjMotionControl::fromConfig(yarp::os::Searchable &config)
1209 _njoints = fromConfig_NumOfJoints(config);
1213 yError() <<
"embObjMC"<< getBoardInfo() <<
"fromConfig(): detected _njoints = " << _njoints;
1218 if(!alloc(_njoints))
1220 yError() <<
"embObjMC"<< getBoardInfo() <<
"fromConfig(): alloc() failed for _njoints = " << _njoints;
1228 int currentMCversion =0;
1234 yError() <<
"embObjMC" << getBoardInfo() <<
"------ ATTENTION!!!! Wrong value of <MotioncontrolVersion> parameter !!!! ---------------------------------------------------------------------------------------";
1235 yError() <<
"embObjMC" << getBoardInfo() <<
"------ This means that the configuration files of this device are not compatible with my parser, so I cannot start. ";
1236 yError() <<
"embObjMC" << getBoardInfo() <<
"------ I need version " <<
PARSER_MOTION_CONTROL_VERSION <<
", but in configuration files have version " << currentMCversion <<
".";
1237 yError() <<
"embObjMC" << getBoardInfo() <<
"------ Please update configuration files in robots-configuration repository. (see http://wiki.icub.org/wiki/Robot_configuration for more information). ";
1238 yError() <<
"embObjMC" << getBoardInfo() <<
"------ If the problem persists contact icub-support@iit.it DO NOT DO IT BY YOURSELF.";
1239 yError() <<
"embObjMC" << getBoardInfo() <<
"----------------------------------------------------------------------------------------------------------------------------------------------------------------";
1245 yTrace() << config.toString().c_str();
1250 if(
false == fromConfig_readServiceCfg(config))
1262 if(
false == fromConfig_Step2(config))
1274 bool embObjMotionControl::init()
1276 eOprotID32_t protid = 0;
1282 for(
int logico=0; logico< _njoints; logico++)
1284 int fisico = _axisMap[logico];
1285 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, fisico, eoprot_tag_mc_joint_cmmnds_controlmode);
1286 eOenum08_t controlMode = eomc_controlmode_cmd_idle;
1290 yError() <<
"embObjMotionControl::init() had an error while setting eomc_controlmode_cmd_idle in "<< getBoardInfo();
1296 SystemClock::delaySystem(0.010);
1303 vector<eOprotID32_t> id32v(0);
1304 for(
int n=0;
n<_njoints;
n++)
1306 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint,
n, eoprot_tag_mc_joint_status_core);
1307 id32v.push_back(protid);
1308 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor,
n, eoprot_tag_mc_motor_status);
1309 id32v.push_back(protid);
1312 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
1314 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, 0, eoprot_tag_mc_joint_status_debug);
1315 id32v.push_back(protid);
1316 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, 1, eoprot_tag_mc_joint_status_debug);
1317 id32v.push_back(protid);
1323 yError() <<
"embObjMotionControl::init() fails to add its variables to regulars in "<< getBoardInfo() <<
": cannot proceed any further";
1330 yDebug() <<
"embObjMotionControl::init() added" << id32v.size() <<
"regular rops to "<< getBoardInfo();
1332 for(
unsigned int r=0; r<id32v.size(); r++)
1334 uint32_t id32 = id32v.at(r);
1335 eoprot_ID2information(id32, nvinfo,
sizeof(nvinfo));
1336 yDebug() <<
"\t it added regular rop for" << nvinfo;
1341 SystemClock::delaySystem(0.005);
1348 for(
int logico=0; logico< _njoints; logico++)
1350 int fisico = _axisMap[logico];
1351 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, fisico, eoprot_tag_mc_joint_config);
1353 eOmc_joint_config_t jconfig = {0};
1354 memset(&jconfig, 0,
sizeof(eOmc_joint_config_t));
1356 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_POSITION,_trj_pids[logico].pid, fisico);
1360 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_TORQUE, _trq_pids[logico].pid, fisico);
1364 jconfig.impedance.damping = (eOmeas_damping_t) _measureConverter->impN2S(_impedance_params[logico].damping, fisico);
1365 jconfig.impedance.stiffness = (eOmeas_stiffness_t) _measureConverter->impN2S(_impedance_params[logico].stiffness, fisico);
1366 jconfig.impedance.offset = 0;
1368 _cacheImpedance[logico].stiffness = jconfig.impedance.stiffness;
1369 _cacheImpedance[logico].damping = jconfig.impedance.damping;
1370 _cacheImpedance[logico].offset = jconfig.impedance.offset;
1372 jconfig.userlimits.max = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posMax, fisico));
1373 jconfig.userlimits.min = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posMin, fisico));
1375 jconfig.hardwarelimits.max = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posHwMax, fisico));
1376 jconfig.hardwarelimits.min = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posHwMin, fisico));
1379 jconfig.maxvelocityofjoint =
S_32(_measureConverter->posA2E(_jointsLimits[logico].velMax, fisico));
1380 jconfig.velocitysetpointtimeout = (eOmeas_time_t)
U_16(_timeouts[logico].velocity);
1382 jconfig.jntEncoderResolution = _jointEncs[logico].resolution;
1383 jconfig.jntEncoderType = _jointEncs[logico].type;
1384 jconfig.jntEncTolerance = _jointEncs[logico].tolerance;
1386 jconfig.motor_params.bemf_value = _measureConverter->bemf_user2raw(_trq_pids[logico].kbemf, fisico);
1387 jconfig.motor_params.bemf_scale = 0;
1388 jconfig.motor_params.ktau_value = _measureConverter->ktau_user2raw(_trq_pids[logico].ktau, fisico);
1389 jconfig.motor_params.ktau_scale = 0;
1390 jconfig.motor_params.friction.viscous_pos_val = _measureConverter->viscousPos_user2raw(_trq_pids[logico].viscousPos, fisico);
1391 jconfig.motor_params.friction.viscous_neg_val = _measureConverter->viscousNeg_user2raw(_trq_pids[logico].viscousNeg, fisico);
1392 jconfig.motor_params.friction.coulomb_pos_val = _measureConverter->coulombPos_user2raw(_trq_pids[logico].coulombPos, fisico);
1393 jconfig.motor_params.friction.coulomb_neg_val = _measureConverter->coulombNeg_user2raw(_trq_pids[logico].coulombNeg, fisico);
1394 jconfig.motor_params.friction.velocityThres_val = _measureConverter->velocityThres_user2raw(_trq_pids[logico].velocityThres, fisico);
1396 jconfig.gearbox_E2J = _gearbox_E2J[logico];
1398 jconfig.deadzone = _measureConverter->posA2E(_deadzone[logico], fisico);
1400 jconfig.tcfiltertype=_trq_pids[logico].
filterType;
1402 jconfig.kalman_params.enabled = _kalman_params[logico].enabled;
1403 for(
int i=0; i<_kalman_params[logico].x0.size(); i++) jconfig.kalman_params.x0[i] = _kalman_params[logico].x0.at(i);
1404 for(
int i=0; i<_kalman_params[logico].Q.size(); i++) jconfig.kalman_params.Q[i] = _kalman_params[logico].Q.at(i);
1405 jconfig.kalman_params.R = _kalman_params[logico].R;
1406 jconfig.kalman_params.P0 = _kalman_params[logico].P0;
1410 yError() <<
"FATAL: embObjMotionControl::init() had an error while calling setcheckRemoteValue() for joint config fisico #" << fisico <<
"in "<< getBoardInfo();
1417 yDebug() <<
"embObjMotionControl::init() correctly configured joint config fisico #" << fisico <<
"in "<< getBoardInfo();
1428 for(
int logico=0; logico<_njoints; logico++)
1430 int fisico = _axisMap[logico];
1432 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, fisico, eoprot_tag_mc_motor_config);
1433 eOmc_motor_config_t motor_cfg = {0};
1434 motor_cfg.maxvelocityofmotor = 0;
1435 motor_cfg.currentLimits.nominalCurrent = _currentLimits[logico].nominalCurrent;
1436 motor_cfg.currentLimits.overloadCurrent = _currentLimits[logico].overloadCurrent;
1437 motor_cfg.currentLimits.peakCurrent = _currentLimits[logico].peakCurrent;
1438 motor_cfg.gearbox_M2J = _gearbox_M2J[logico];
1439 motor_cfg.rotorEncoderResolution = _motorEncs[logico].resolution;
1440 motor_cfg.rotEncTolerance = _motorEncs[logico].tolerance;
1441 motor_cfg.hasHallSensor = _foc_based_info[logico].
hasHallSensor;
1443 motor_cfg.hasTempSensor = _foc_based_info[logico].
hasTempSensor;
1446 motor_cfg.verbose = _foc_based_info[logico].
verbose;
1447 motor_cfg.motorPoles = _foc_based_info[logico].
motorPoles;
1449 motor_cfg.rotorEncoderType = _motorEncs[logico].type;
1450 motor_cfg.pwmLimit =_rotorsLimits[logico].pwmMax;
1451 motor_cfg.temperatureLimit = (eOmeas_temperature_t)
S_16(_temperatureSensorsVector.at(logico)->convertTempCelsiusToRaw(_temperatureLimits.at(logico).hardwareTemperatureLimit));
1452 motor_cfg.limitsofrotor.max = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMax, fisico ));
1453 motor_cfg.limitsofrotor.min = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMin, fisico ));
1456 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_CURRENT, _cur_pids[logico].pid, fisico);
1459 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_VELOCITY, _spd_pids[logico].pid, fisico);
1464 yError() <<
"FATAL: embObjMotionControl::init() had an error while calling setcheckRemoteValue() for motor config fisico #" << fisico <<
"in "<< getBoardInfo();
1471 yDebug() <<
"embObjMotionControl::init() correctly configured motor config fisico #" << fisico <<
"in "<< getBoardInfo();
1482 yTrace() <<
"embObjMotionControl::init(): correctly instantiated for " << getBoardInfo();
1490 yTrace() <<
" embObjMotionControl::close()";
1492 ImplementControlMode::uninitialize();
1493 ImplementEncodersTimed::uninitialize();
1494 ImplementMotorEncoders::uninitialize();
1495 ImplementPositionControl::uninitialize();
1496 ImplementVelocityControl::uninitialize();
1497 ImplementPidControl::uninitialize();
1498 ImplementControlCalibration::uninitialize();
1499 ImplementAmplifierControl::uninitialize();
1500 ImplementImpedanceControl::uninitialize();
1501 ImplementControlLimits::uninitialize();
1502 ImplementTorqueControl::uninitialize();
1503 ImplementPositionDirect::uninitialize();
1504 ImplementInteractionMode::uninitialize();
1505 ImplementRemoteVariables::uninitialize();
1506 ImplementAxisInfo::uninitialize();
1507 ImplementCurrentControl::uninitialize();
1508 ImplementPWMControl::uninitialize();
1509 ImplementJointFault::uninitialize();
1511 if (_measureConverter) {
delete _measureConverter; _measureConverter=0;}
1514 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
1517 for(
size_t i=0; i<mcdiagnostics.
ports.size(); i++)
1519 mcdiagnostics.
ports[i]->close();
1520 delete mcdiagnostics.
ports[i];
1522 mcdiagnostics.
ports.clear();
1524 mcdiagnostics.
config.mode = eomn_serv_diagn_mode_NONE;
1525 mcdiagnostics.
config.par16 = 0;
1528 delete event_downsampler;
1542 void embObjMotionControl::cleanup(
void)
1544 if(ethManager == NULL)
return;
1563 size_t joint = eoprot_ID2index(id32);
1564 eOprotEntity_t ent = eoprot_ID2entity(id32);
1565 eOprotTag_t tag = eoprot_ID2tag(id32);
1579 std::lock_guard<std::mutex> lck(_mutex);
1580 _encodersStamp[joint] = timestamp;
1584 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
1586 char str[128] =
"boh";
1588 eoprot_ID2information(id32, str,
sizeof(str));
1590 if((eoprot_entity_mc_joint == ent) && (eoprot_tag_mc_joint_status_debug == tag) && (joint < mcdiagnostics.
ports.size()))
1593 eOprotID32_t id32sc = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core);
1594 eOmc_joint_status_core_t jcore = {};
1598 int32_t *debug32 =
reinterpret_cast<int32_t*
>(rxdata);
1601 Bottle& output = mcdiagnostics.
ports[joint]->prepare();
1604 output.addString(
"[yt, amo, reg, pos]");
1605 output.addFloat64(timestamp);
1606 output.addInt32(debug32[0]);
1607 output.addInt32(debug32[1]);
1608 output.addInt32(jcore.measures.meas_position);
1609 mcdiagnostics.
ports[joint]->write();
1613 if((eoprot_entity_mc_motor == ent) && (eoprot_tag_mc_motor_status == tag))
1618 uint8_t motor = eoprot_ID2index(id32);
1622 eOmc_motor_status_t *mc_motor_status =
reinterpret_cast<eOmc_motor_status_t*
>(rxdata);
1626 double tmp = _temperatureSensorsVector.at(motor)->convertRawToTempCelsius((
double)mc_motor_status->basic.mot_temperature);
1628 if (
tmp > _temperatureLimits[motor].warningTemperatureLimit)
1630 if(! _temperatureExceededLimitWatchdog.at(motor).isStarted())
1632 yWarning() << getBoardInfo() <<
"Motor" << motor <<
"The temperature (" <<
tmp <<
"[ ℃ ] ) exceeds the warning limit (" << _temperatureLimits[motor].warningTemperatureLimit <<
"[ ℃ ] ). Processes not stopped but it is strongly recommended decreasing motor usage or reducing currents and PWMs to not risk motor damaging";
1633 _temperatureExceededLimitWatchdog.at(motor).start();
1637 if(_temperatureExceededLimitWatchdog.at(motor).isExpired())
1639 yWarning() << getBoardInfo() <<
"Motor" << motor <<
"The temperature (" <<
tmp <<
"[ ℃ ] ) exceeds the warning limit (" << _temperatureLimits[motor].warningTemperatureLimit <<
"[ ℃ ] ) again!. Processes not stopped but it is strongly recommended decreasing motor usage or reducing currents and PWMs to not risk motor damaging";
1640 _temperatureExceededLimitWatchdog.at(motor).start();
1642 _temperatureExceededLimitWatchdog.at(motor).increment();
1647 _temperatureExceededLimitWatchdog.at(motor).clear();
1652 if(! _temperatureSensorErrorWatchdog.at(motor).isStarted())
1654 yError() << getBoardInfo() <<
"At timestamp" << yarp::os::Time::now() <<
"In motor" << motor <<
"cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable";
1655 _temperatureSensorErrorWatchdog.at(motor).start();
1659 _temperatureSensorErrorWatchdog.at(motor).increment();
1660 if( _temperatureSensorErrorWatchdog.at(motor).isExpired())
1662 yError()<< getBoardInfo() <<
"Motor" << motor <<
"failed to read temperature for" << yarp::os::Time::now() - _temperatureSensorErrorWatchdog.at(motor).getStartTime() <<
"seconds";
1663 _temperatureSensorErrorWatchdog.at(motor).start();
1692 case VOCAB_PIDTYPE_POSITION:
1693 helper_setPosPidRaw(j,pid);
1695 case VOCAB_PIDTYPE_VELOCITY:
1697 helper_setSpdPidRaw(j, pid);
1699 case VOCAB_PIDTYPE_TORQUE:
1700 helper_setTrqPidRaw(j, pid);
1702 case VOCAB_PIDTYPE_CURRENT:
1703 helper_setCurPidRaw(j,pid);
1706 yError()<<
"Invalid pidtype:"<<pidtype;
1716 case VOCAB_PIDTYPE_POSITION:
1717 helper_getPosPidRaw(axis,pid);
1719 case VOCAB_PIDTYPE_VELOCITY:
1721 helper_getSpdPidRaw(axis, pid);
1723 case VOCAB_PIDTYPE_TORQUE:
1724 helper_getTrqPidRaw(axis, pid);
1726 case VOCAB_PIDTYPE_CURRENT:
1727 helper_getCurPidRaw(axis,pid);
1730 yError()<<
"Invalid pidtype:"<<pidtype;
1736 bool embObjMotionControl::helper_setPosPidRaw(
int j,
const Pid &pid)
1738 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtrajectory);
1747 yError() <<
"while setting position PIDs for " << getBoardInfo() <<
" joint " << j;
1757 for(
int j=0; j< _njoints; j++)
1772 for(
int j=0, index=0; j< _njoints; j++, index++)
1794 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
1795 eOmc_joint_status_core_t jcore = {0};
1802 case VOCAB_PIDTYPE_POSITION:
1804 if((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
1805 (eomc_controlmode_openloop == jcore.modes.controlmodestatus) ||
1806 (eomc_controlmode_current == jcore.modes.controlmodestatus))
1809 *err = (double) jcore.ofpid.generic.error1;
1820 case VOCAB_PIDTYPE_TORQUE:
1822 if ((eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus) &&
1823 (eomc_controlmode_position == jcore.modes.controlmodestatus))
1825 *err = (double) jcore.ofpid.complpos.errtrq;
1828 if(eomc_controlmode_torque == jcore.modes.controlmodestatus)
1830 *err = (double) jcore.ofpid.torque.errtrq;
1834 case VOCAB_PIDTYPE_VELOCITY:
1840 case VOCAB_PIDTYPE_CURRENT:
1848 yError()<<
"Invalid pidtype:"<<pidtype;
1858 for(
int j=0; j< _njoints; j++)
1865 bool embObjMotionControl::helper_getPosPidRaw(
int j, Pid *pid)
1867 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtrajectory);
1870 eOmc_PID_t eoPID = {0};
1873 #ifdef NETWORK_PERFORMANCE_BENCHMARK
1874 double start = yarp::os::Time::now();
1877 bool ret = askRemoteValue(protid, &eoPID, size);
1879 #ifdef NETWORK_PERFORMANCE_BENCHMARK
1880 double end = yarp::os::Time::now();
1881 m_responseTimingVerifier.tick(end-start, start);
1894 bool embObjMotionControl::helper_getPosPidsRaw(Pid *pid)
1896 std::vector<eOmc_PID_t> eoPIDList(_njoints);
1897 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_pidtrajectory, eoPIDList);
1900 yError() <<
"failed helper_getPosPidsRaw for" << getBoardInfo();
1904 for(
int j=0; j<_njoints; j++)
1918 case VOCAB_PIDTYPE_POSITION:
1919 helper_getPosPidsRaw(pids);
1924 case VOCAB_PIDTYPE_TORQUE:
1925 helper_getTrqPidsRaw(pids);
1927 case VOCAB_PIDTYPE_CURRENT:
1928 helper_getCurPidsRaw(pids);
1930 case VOCAB_PIDTYPE_VELOCITY:
1931 helper_getSpdPidsRaw(pids);
1934 yError()<<
"Invalid pidtype:"<<pidtype;
1942 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
1943 eOmc_joint_status_core_t jcore = {0};
1950 case VOCAB_PIDTYPE_POSITION:
1952 if((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
1953 (eomc_controlmode_openloop == jcore.modes.controlmodestatus) ||
1954 (eomc_controlmode_current == jcore.modes.controlmodestatus))
1955 { *ref = 0; yError() <<
"Invalid getPidReferenceRaw() request for current control mode";
return true; }
1956 *ref = (double) jcore.ofpid.generic.reference1;
1965 case VOCAB_PIDTYPE_TORQUE:
1971 case VOCAB_PIDTYPE_CURRENT:
1977 case VOCAB_PIDTYPE_VELOCITY:
1986 yError()<<
"Invalid pidtype:"<<pidtype;
1999 for(
int j=0; j< _njoints; j++)
2044 if( (mode != VOCAB_CM_VELOCITY) &&
2045 (mode != VOCAB_CM_MIXED) &&
2046 (mode != VOCAB_CM_IMPEDANCE_VEL) &&
2047 (mode != VOCAB_CM_IDLE))
2051 yError() <<
"velocityMoveRaw: skipping command because " << getBoardInfo() <<
" joint " << j <<
" is not in VOCAB_CM_VELOCITY mode";
2056 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
2058 _ref_command_speeds[j] = sp ;
2060 eOmc_setpoint_t setpoint;
2061 setpoint.type = eomc_setpoint_velocity;
2062 setpoint.to.velocity.value = (eOmeas_velocity_t)
S_32(_ref_command_speeds[j]);
2063 setpoint.to.velocity.withacceleration = (eOmeas_acceleration_t)
S_32(_ref_accs[j]);
2068 yError() <<
"while setting velocity mode";
2077 eOmc_setpoint_t setpoint;
2079 setpoint.type = eomc_setpoint_velocity;
2081 for(
int j=0; j<_njoints; j++)
2096 yTrace() <<
"setCalibrationParametersRaw for " << getBoardInfo() <<
"joint" << j;
2098 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_calibration);
2099 eOmc_calibrator_t calib;
2100 memset(&calib, 0x00,
sizeof(calib));
2101 calib.type = params.type;
2106 case eomc_calibration_type0_hard_stops:
2107 calib.params.type0.pwmlimit = (int16_t)
S_16(params.param1);
2108 calib.params.type0.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2109 calib.params.type0.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2113 case eomc_calibration_type1_abs_sens_analog:
2114 calib.params.type1.position = (int16_t)
S_16(params.param1);
2115 calib.params.type1.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2116 calib.params.type1.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2120 case eomc_calibration_type2_hard_stops_diff:
2121 calib.params.type2.pwmlimit = (int16_t)
S_16(params.param1);
2122 calib.params.type2.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2123 calib.params.type2.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2127 case eomc_calibration_type3_abs_sens_digital:
2128 calib.params.type3.position = (int16_t)
S_16(params.param1);
2129 calib.params.type3.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2130 calib.params.type3.offset = (int32_t)
S_32(params.param3);
2131 calib.params.type3.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2135 case eomc_calibration_type4_abs_and_incremental:
2136 calib.params.type4.position = (int16_t)
S_16(params.param1);
2137 calib.params.type4.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2138 calib.params.type4.maxencoder = (int32_t)
S_32(params.param3);
2139 calib.params.type4.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2143 case eomc_calibration_type5_hard_stops:
2144 calib.params.type5.pwmlimit = (int32_t)
S_32(params.param1);
2145 calib.params.type5.final_pos = (int32_t)
S_32(params.param2);
2146 calib.params.type5.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2150 case eomc_calibration_type6_mais:
2151 calib.params.type6.position = (int32_t)
S_32(params.param1);
2152 calib.params.type6.velocity = (int32_t)
S_32(params.param2);
2153 calib.params.type6.current = (int32_t)
S_32(params.param3);
2154 calib.params.type6.vmin = (int32_t)
S_32(params.param4);
2155 calib.params.type6.vmax = (int32_t)
S_32(params.param5);
2156 calib.params.type6.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2160 case eomc_calibration_type7_hall_sensor:
2161 calib.params.type7.position = (int32_t)
S_32(params.param1);
2162 calib.params.type7.velocity = (int32_t)
S_32(params.param2);
2164 calib.params.type7.vmin = (int32_t)
S_32(params.param4);
2165 calib.params.type7.vmax = (int32_t)
S_32(params.param5);
2166 calib.params.type7.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2170 case eomc_calibration_type8_tripod_internal_hard_stop:
2171 calib.params.type8.pwmlimit = (int32_t)
S_32(params.param1);
2172 calib.params.type8.max_delta = (int32_t)
S_32(params.param2);
2173 calib.params.type8.calibrationZero = (int32_t)
S_32(params.paramZero );
2176 case eomc_calibration_type9_tripod_external_hard_stop:
2177 calib.params.type9.pwmlimit = (int32_t)
S_32(params.param1);
2178 calib.params.type9.max_delta = (int32_t)
S_32(params.param2);
2179 calib.params.type9.calibrationZero = (int32_t)
S_32(params.paramZero );
2182 case eomc_calibration_type10_abs_hard_stop:
2183 calib.params.type10.pwmlimit = (int32_t)
S_32(params.param1);
2184 calib.params.type10.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2187 case eomc_calibration_type11_cer_hands:
2188 calib.params.type11.offset0 = (int32_t)
S_32(params.param1);
2189 calib.params.type11.offset1 = (int32_t)
S_32(params.param2);
2190 calib.params.type11.offset2 = (int32_t)
S_32(params.param3);
2191 calib.params.type11.cable_range = (int32_t)
S_32(params.param4);
2192 calib.params.type11.pwm = (int32_t)
S_32(params.param5);
2196 case eomc_calibration_type12_absolute_sensor:
2197 calib.params.type12.rawValueAtZeroPos = (int32_t)
S_32(params.param1);
2198 calib.params.type12.calibrationDelta = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2201 case eomc_calibration_type13_cer_hands_2:
2202 calib.params.type13.rawValueAtZeroPos0 = (int32_t)
S_32(params.param1);
2203 calib.params.type13.rawValueAtZeroPos1 = (int32_t)
S_32(params.param2);
2204 calib.params.type13.rawValueAtZeroPos2 = (int32_t)
S_32(params.param3);
2205 calib.params.type13.rawValueAtZeroPos3 = (int32_t)
S_32(params.param4);
2208 case eomc_calibration_type14_qenc_hard_stop_and_fap:
2209 calib.params.type14.pwmlimit = (int32_t)
S_32(params.param1);
2210 calib.params.type14.final_pos = (int32_t)
S_32(params.param2);
2211 calib.params.type14.invertdirection = (uint8_t)
U_32(params.param3);
2212 calib.params.type14.rotation = (int32_t)
S_32(params.param4);
2214 if (calib.params.type14.invertdirection != 0 && calib.params.type14.invertdirection != 1)
2216 yError() <<
"Error in param3 of calibartion type 14 for joint " << j <<
"Admitted values are: 0=FALSE and 1=TRUE";
2221 if(!checkCalib14RotationParam(calib.params.type14.rotation))
2223 yError() <<
"Error in param4 of calibartion type 14 for joint " << j <<
"Admitted values are: 0, 32768, 16384, -16384 [0, 180, 90, -90] in iCubDegree";
2226 calib.params.type14.offset = (int32_t)
S_32(params.param5);
2227 calib.params.type14.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2229 yDebug() <<
"***** calib.params.type14.pwmlimit = " <<calib.params.type14.pwmlimit;
2230 yDebug() <<
"***** calib.params.type14.final_pos = " <<calib.params.type14.final_pos;
2231 yDebug() <<
"***** calib.params.type14.invertdirection = " <<calib.params.type14.invertdirection;
2232 yDebug() <<
"***** calib.params.type14.rotation = " <<calib.params.type14.rotation;
2233 yDebug() <<
"***** calib.params.type14.offset = " <<calib.params.type14.offset;
2234 yDebug() <<
"***** calib.params.type14.calibrationZero = " <<calib.params.type14.calibrationZero;
2238 yError() <<
"Calibration type unknown!! (embObjMotionControl)\n";
2245 yError() <<
"while setting velocity mode";
2249 _calibrated[j] =
true;
2254 bool embObjMotionControl::checkCalib14RotationParam(int32_t calib_param4)
2256 eOmc_calib14_ROT_t urotation = eomc_int2calib14_ROT(calib_param4);
2258 if (urotation == eOmc_calib14_ROT_zero ||
2259 urotation == eOmc_calib14_ROT_plus180 ||
2260 urotation == eOmc_calib14_ROT_plus090 ||
2261 urotation == eOmc_calib14_ROT_minus090)
2271 yTrace() <<
"calibrateRaw for" << getBoardInfo() <<
"joint" << j;
2289 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_calibration);
2290 eOmc_calibrator_t calib;
2291 memset(&calib, 0x00,
sizeof(calib));
2297 case eomc_calibration_type0_hard_stops:
2298 calib.params.type0.pwmlimit = (int16_t)
S_16(p1);
2299 calib.params.type0.velocity = (eOmeas_velocity_t)
S_32(p2);
2303 case eomc_calibration_type1_abs_sens_analog:
2304 calib.params.type1.position = (int16_t)
S_16(p1);
2305 calib.params.type1.velocity = (eOmeas_velocity_t)
S_32(p2);
2309 case eomc_calibration_type2_hard_stops_diff:
2310 calib.params.type2.pwmlimit = (int16_t)
S_16(p1);
2311 calib.params.type2.velocity = (eOmeas_velocity_t)
S_32(p2);
2315 case eomc_calibration_type3_abs_sens_digital:
2316 calib.params.type3.position = (int16_t)
S_16(p1);
2317 calib.params.type3.velocity = (eOmeas_velocity_t)
S_32(p2);
2318 calib.params.type3.offset = (int32_t)
S_32(p3);
2322 case eomc_calibration_type4_abs_and_incremental:
2323 calib.params.type4.position = (int16_t)
S_16(p1);
2324 calib.params.type4.velocity = (eOmeas_velocity_t)
S_32(p2);
2325 calib.params.type4.maxencoder = (int32_t)
S_32(p3);
2329 yError () <<
"Calibration type unknown!! (embObjMotionControl)\n";
2336 yError() <<
"while setting velocity mode";
2340 _calibrated[j ] =
true;
2348 bool result =
false;
2349 eOenum08_t temp = 0;
2351 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_core_modes_controlmodestatus);
2352 if(
false == askRemoteValue(id32, &temp, size))
2354 yError () <<
"Failure of askRemoteValue() inside embObjMotionControl::doneRaw(axis=" << axis <<
") for " << getBoardInfo();
2358 eOmc_controlmode_t
type = (eOmc_controlmode_t) temp;
2362 if (eomc_controlmode_idle ==
type)
2366 else if (eomc_controlmode_calib ==
type)
2370 else if (eomc_controlmode_hwFault ==
type)
2372 yError(
"unable to complete calibration: joint %d in 'hw_fault status' inside doneRaw() function", axis);
2375 else if (eomc_controlmode_notConfigured ==
type)
2377 yError(
"unable to complete calibration: joint %d in 'not_configured' status inside doneRaw() function", axis);
2380 else if (eomc_controlmode_unknownError ==
type)
2382 yError(
"unable to complete calibration: joint %d in 'unknownError' status inside doneRaw() function", axis);
2385 else if (eomc_controlmode_configured ==
type)
2387 yError(
"unable to complete calibration: joint %d in 'configured' status inside doneRaw() function", axis);
2412 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.";
2414 _last_position_move_time[j] = yarp::os::Time::now();
2418 if( (mode != VOCAB_CM_POSITION) &&
2419 (mode != VOCAB_CM_MIXED) &&
2420 (mode != VOCAB_CM_IMPEDANCE_POS) &&
2421 (mode != VOCAB_CM_IDLE))
2425 yError() <<
"positionMoveRaw: skipping command because " << getBoardInfo() <<
" joint " << j <<
" is not in VOCAB_CM_POSITION mode";
2430 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
2431 _ref_command_positions[j] = ref;
2433 eOmc_setpoint_t setpoint;
2435 setpoint.type = (eOenum08_t) eomc_setpoint_position;
2436 setpoint.to.position.value = (eOmeas_position_t)
S_32(_ref_command_positions[j]);
2437 setpoint.to.position.withvelocity = (eOmeas_velocity_t)
S_32(_ref_speeds[j]);
2446 for(
int j=0, index=0; j< _njoints; j++, index++)
2466 eObool_t ismotiondone = eobool_false;
2469 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core_modes_ismotiondone);
2470 if(
false == askRemoteValue(id32, &ismotiondone, size))
2472 yError () <<
"Failure of askRemoteValue() inside embObjMotionControl::checkMotionDoneRaw(j=" << j <<
") for " << getBoardInfo();
2477 *flag = ismotiondone;
2484 std::vector <eObool_t> ismotiondoneList(_njoints);
2485 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_core_modes_ismotiondone, ismotiondoneList);
2488 yError () <<
"Failure of askRemoteValues() inside embObjMotionControl::checkMotionDoneRaw for all joints of" << getBoardInfo();
2492 for(
int j=0; j<_njoints; j++)
2494 *flag &= ismotiondoneList[j];
2504 _ref_speeds[index] = sp;
2512 for(
int j=0, index=0; j< _njoints; j++, index++)
2514 _ref_speeds[index] = spds[index];
2526 _ref_accs[j ] = 1e6;
2528 else if (
acc < -1e6)
2530 _ref_accs[j ] = -1e6;
2534 _ref_accs[j ] =
acc;
2544 for(
int j=0, index=0; j< _njoints; j++, index++)
2548 _ref_accs[index] = 1e6;
2550 else if (accs[j] < -1e6)
2552 _ref_accs[index] = -1e6;
2556 _ref_accs[index] = accs[j];
2564 if (j<0 || j>_njoints)
return false;
2565 #if ASK_REFERENCE_TO_FIRMWARE
2566 *spd = _ref_speeds[j];
2569 *spd = _ref_speeds[j];
2576 memcpy(spds, _ref_speeds,
sizeof(
double) * _njoints);
2582 *
acc = _ref_accs[j];
2588 memcpy(accs, _ref_accs,
sizeof(
double) * _njoints);
2594 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_stoptrajectory);
2596 eObool_t stop = eobool_true;
2604 for(
int j=0; j< _njoints; j++)
2619 for(
int j=0; j<n_joint; j++)
2629 for(
int j=0; j<n_joint; j++)
2640 for(
int j=0; j<n_joint; j++)
2642 if(joints[j] >= _njoints)
2644 yError() << getBoardInfo() <<
":checkMotionDoneRaw required for not existing joint ( " << joints[j] <<
")";
2650 std::vector <eObool_t> ismotiondoneList(_njoints);
2651 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_core_modes_ismotiondone, ismotiondoneList);
2654 yError () << getBoardInfo() <<
"Failure of askRemoteValues() inside embObjMotionControl::checkMotionDoneRaw for a group of joint"; getBoardInfo();
2659 bool tot_val =
true;
2660 for(
int j=0; j<n_joint; j++)
2662 tot_val &= ismotiondoneList[joints[j]];
2673 for(
int j=0; j<n_joint; j++)
2683 for(
int j=0; j<n_joint; j++)
2693 for(
int j=0; j<n_joint; j++)
2703 for(
int j=0; j<n_joint; j++)
2713 for(
int j=0; j<n_joint; j++)
2715 ret = ret &&
stopRaw(joints[j]);
2726 eOmc_joint_status_core_t jcore = {0};
2727 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2731 eOmc_controlmode_t
type = (eOmc_controlmode_t) jcore.modes.controlmodestatus;
2741 for(
int j=0; j< _njoints; j++)
2751 for(
int j=0; j< n_joint; j++)
2766 eOenum08_t controlmodecommand = 0;
2768 if((_mode == VOCAB_CM_TORQUE) && (_trq_pids[j].enabled ==
false))
2770 yError()<<
"Torque control is disabled. Check your configuration parameters";
2776 yError() <<
"SetControlMode: received unknown control mode for " << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
2780 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_controlmode);
2783 yError() <<
"setControlModeRaw failed for " << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
2788 ret = checkRemoteControlModeStatus(j, _mode);
2792 yError() <<
"In embObjMotionControl::setControlModeRaw(j=" << j <<
", mode=" << yarp::os::Vocab32::decode(_mode).c_str() <<
") for " << getBoardInfo() <<
" has failed checkRemoteControlModeStatus()";
2802 eOenum08_t controlmodecommand = 0;
2805 for(
int i=0; i<n_joint; i++)
2807 if ((modes[i] == VOCAB_CM_TORQUE) && (_trq_pids[i].enabled ==
false)) {yError()<<
"Torque control is disabled. Check your configuration parameters";
continue;}
2811 yError() <<
"SetControlModesRaw(): received unknown control mode for " << getBoardInfo() <<
" joint " << joints[i] <<
" mode " << Vocab32::decode(modes[i]);
2816 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joints[i], eoprot_tag_mc_joint_cmmnds_controlmode);
2819 yError() <<
"setControlModesRaw() could not send set<cmmnds_controlmode> for " << getBoardInfo() <<
" joint " << joints[i] <<
" mode " << Vocab32::decode(modes[i]);
2824 bool tmpresult = checkRemoteControlModeStatus(joints[i], modes[i]);
2825 if(
false == tmpresult)
2827 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]);
2830 ret = ret && tmpresult;
2840 eOenum08_t controlmodecommand = 0;
2842 for(
int i=0; i<_njoints; i++)
2845 if ((modes[i] == VOCAB_CM_TORQUE) && (_trq_pids[i].enabled ==
false))
2847 yError()<<
"Torque control is disabled. Check your configuration parameters";
2853 yError() <<
"SetControlMode: received unknown control mode for" << getBoardInfo() <<
" joint " << i <<
" mode " << Vocab32::decode(modes[i]);
2857 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, i, eoprot_tag_mc_joint_cmmnds_controlmode);
2860 yError() <<
"setControlModesRaw failed for " << getBoardInfo() <<
" joint " << i <<
" mode " << Vocab32::decode(modes[i]);
2864 bool tmpresult = checkRemoteControlModeStatus(i, modes[i]);
2865 if(
false == tmpresult)
2867 yError() <<
"setControlModesRaw(int *modes) could not check with checkRemoteControlModeStatus() for" << getBoardInfo() <<
" joint " << i <<
" mode " << Vocab32::decode(modes[i]);
2870 ret = ret && tmpresult;
2903 eOmc_joint_status_core_t core;
2904 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2910 *value = (double) core.measures.meas_position;
2914 yError() <<
"embObjMotionControl while reading encoder";
2924 for(
int j=0; j< _njoints; j++)
2934 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2935 eOmc_joint_status_core_t core;
2942 *sp = (double) core.measures.meas_velocity;
2949 for(
int j=0; j< _njoints; j++)
2958 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2959 eOmc_joint_status_core_t core;
2965 *
acc = (double) core.measures.meas_acceleration;
2972 for(
int j=0; j< _njoints; j++)
2984 std::lock_guard<std::mutex> lck(_mutex);
2985 for(
int i=0; i<_njoints; i++)
2986 stamps[i] = _encodersStamp[i];
2993 std::lock_guard<std::mutex> lck(_mutex);
2994 *stamp = _encodersStamp[j];
3038 eOmc_motor_status_basic_t status;
3039 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
3044 *value = (double) status.mot_position;
3048 yError() <<
"embObjMotionControl while reading motor encoder position";
3058 for(
int j=0; j< _njoints; j++)
3068 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
3069 eOmc_motor_status_basic_t tmpMotorStatus;
3073 *sp = (double) tmpMotorStatus.mot_velocity;
3077 yError() <<
"embObjMotionControl while reading motor encoder speed";
3086 for(
int j=0; j< _njoints; j++)
3095 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
3096 eOmc_motor_status_basic_t tmpMotorStatus;
3100 *
acc = (double) tmpMotorStatus.mot_acceleration;
3104 yError() <<
"embObjMotionControl while reading motor encoder acceleration";
3113 for(
int j=0; j< _njoints; j++)
3123 std::lock_guard<std::mutex> lck(_mutex);
3124 for(
int i=0; i<_njoints; i++)
3125 stamps[i] = _encodersStamp[i];
3132 std::lock_guard<std::mutex> lck(_mutex);
3133 *stamp = _encodersStamp[m];
3152 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
3153 eOmc_motor_status_basic_t tmpMotorStatus;
3156 *value = (double) tmpMotorStatus.mot_current;
3163 for(
int j=0; j< _njoints; j++)
3172 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_currentlimits);
3174 eOmc_current_limits_params_t currentlimits = {0};
3176 if(!askRemoteValue(protid, ¤tlimits, size))
3178 yError() <<
"embObjMotionControl::setMaxCurrentRaw() could not read max current for " << getBoardInfo() <<
"joint " << j;
3183 currentlimits.overloadCurrent = (eOmeas_current_t)
S_16(val);
3191 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_currentlimits);
3193 eOmc_current_limits_params_t currentlimits = {0};
3196 if(!askRemoteValue(protid, ¤tlimits, size))
3198 yError() <<
"embObjMotionControl::getMaxCurrentRaw() could not read max current for " << getBoardInfo() <<
"joint " << j;
3202 *val = (double) currentlimits.overloadCurrent;
3210 (_enabledAmp[j ]) ? *st = 1 : *st = 0;
3217 for(
int j=0; j<_njoints; j++)
3219 sts[j] = _enabledAmp[j];
3225 #ifdef IMPLEMENT_DEBUG_INTERFACE
3230 bool embObjMotionControl::setParameterRaw(
int j,
unsigned int type,
double value) {
return NOT_YET_IMPLEMENTED(
"setParameterRaw"); }
3231 bool embObjMotionControl::getParameterRaw(
int j,
unsigned int type,
double* value) {
return NOT_YET_IMPLEMENTED(
"getParameterRaw"); }
3232 bool embObjMotionControl::getDebugParameterRaw(
int j,
unsigned int index,
double* value) {
return NOT_YET_IMPLEMENTED(
"getDebugParameterRaw"); }
3233 bool embObjMotionControl::setDebugParameterRaw(
int j,
unsigned int index,
double value) {
return NOT_YET_IMPLEMENTED(
"setDebugParameterRaw"); }
3234 bool embObjMotionControl::setDebugReferencePositionRaw(
int j,
double value) {
return NOT_YET_IMPLEMENTED(
"setDebugReferencePositionRaw"); }
3235 bool embObjMotionControl::getDebugReferencePositionRaw(
int j,
double* value) {
return NOT_YET_IMPLEMENTED(
"getDebugReferencePositionRaw");}
3243 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_userlimits);
3245 eOmeas_position_limits_t limits;
3246 limits.max = (eOmeas_position_t)
S_32(
max);
3247 limits.min = (eOmeas_position_t)
S_32(
min);
3254 yError() <<
"while setting position limits for joint" << j <<
" \n";
3261 eOmeas_position_limits_t limits;
3262 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_userlimits);
3265 if(! askRemoteValue(protoid, &limits, size))
3268 *
min = (double)limits.min + SAFETY_THRESHOLD;
3269 *
max = (
double)limits.max - SAFETY_THRESHOLD;
3275 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3277 eOmc_motor_config_t motor_cfg;
3278 if(! askRemoteValue(protoid, &motor_cfg, size))
3282 *gearbox = (double)motor_cfg.gearbox_M2J;
3289 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3291 eOmc_motor_config_t motor_cfg;
3292 if(! askRemoteValue(protoid, &motor_cfg, size))
3294 *rotorMax = (double)( motor_cfg.limitsofrotor.max);
3295 *rotorMin = (double)( motor_cfg.limitsofrotor.min);
3301 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3303 eOmc_joint_config_t joint_cfg;
3304 if(! askRemoteValue(protoid, &joint_cfg, size))
3308 type = (int)joint_cfg.tcfiltertype;
3314 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3316 eOmc_motor_config_t motor_cfg;
3317 if(! askRemoteValue(protoid, &motor_cfg, size))
3321 rotres = (double)motor_cfg.rotorEncoderResolution;
3328 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3330 eOmc_joint_config_t joint_cfg;
3331 if(! askRemoteValue(protoid, &joint_cfg, size))
3335 jntres = (double)joint_cfg.jntEncoderResolution;
3342 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3344 eOmc_joint_config_t joint_cfg;
3345 if(! askRemoteValue(protoid, &joint_cfg, size))
3349 type = (int)joint_cfg.jntEncoderType;
3356 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3358 eOmc_motor_config_t motor_cfg;
3359 if(! askRemoteValue(protoid, &motor_cfg, size))
3363 type = (int)motor_cfg.rotorEncoderType;
3370 yError(
"getKinematicMJRaw not yet implemented");
3396 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3398 eOmc_motor_config_t motor_cfg;
3399 if(! askRemoteValue(protoid, &motor_cfg, size))
3403 ret = (int)motor_cfg.hasTempSensor;
3410 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3412 eOmc_motor_config_t motor_cfg;
3413 if(! askRemoteValue(protoid, &motor_cfg, size))
3417 ret = (int)motor_cfg.hasHallSensor;
3424 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3426 eOmc_motor_config_t motor_cfg;
3427 if(! askRemoteValue(protoid, &motor_cfg, size))
3431 ret = (int)motor_cfg.hasRotorEncoder;
3438 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3440 eOmc_motor_config_t motor_cfg;
3441 if(! askRemoteValue(protoid, &motor_cfg, size))
3445 ret = (int)motor_cfg.hasRotorEncoderIndex;
3452 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3455 eOmc_motor_config_t motor_cfg;
3456 if(! askRemoteValue(protoid, &motor_cfg, size))
3460 poles = (int)motor_cfg.motorPoles;
3467 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3469 eOmc_motor_config_t motor_cfg;
3470 if(! askRemoteValue(protoid, &motor_cfg, size))
3474 rotorOffset = (double)motor_cfg.rotorIndexOffset;
3481 if (axis >= 0 && axis < _njoints)
3483 name = _axesInfo[axis].name;
3495 if (axis >= 0 && axis < _njoints)
3497 type = _axesInfo[axis].type;
3506 bool embObjMotionControl::getJointDeadZoneRaw(
int j,
double &jntDeadZone)
3508 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3510 eOmc_joint_config_t joint_cfg;
3511 if(! askRemoteValue(protoid, &joint_cfg, size))
3515 jntDeadZone = _measureConverter->posE2A((
double)joint_cfg.deadzone, _axisMap[j]);
3524 if (key ==
"kinematic_mj")
3527 Bottle& ret = val.addList();
3529 eOmn_serv_type_t mc_serv_type = (eOmn_serv_type_t)serviceConfig.
ethservice.configuration.type;
3530 if(iNeedCouplingsInfo())
3532 for (
int r=0; r<_njoints; r++)
3534 for (
int c = 0; c < _njoints; c++)
3538 ret.addFloat64(_couplingInfo.
matrixJ2M[4 * r + c]);
3544 ret.addFloat64(0.0);
3548 else if (key ==
"encoders")
3550 Bottle& r = val.addList();
for (
int i = 0; i < _njoints; i++) { r.addFloat64(_measureConverter->posA2E(1.0, i)); }
3553 else if (key ==
"rotorEncoderResolution")
3558 else if (key ==
"jointEncoderResolution")
3563 else if (key ==
"gearbox_M2J")
3568 else if (key ==
"gearbox_E2J")
3570 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp=0; getGerabox_E2J(i, &
tmp); r.addFloat64(
tmp); }
3573 else if (key ==
"hasHallSensor")
3578 else if (key ==
"hasTempSensor")
3583 else if (key ==
"TemperatureSensorType")
3588 else if (key ==
"hasRotorEncoder")
3593 else if (key ==
"hasRotorEncoderIndex")
3598 else if (key ==
"rotorIndexOffset")
3603 else if (key ==
"motorPoles")
3608 else if (key ==
"pidCurrentKp")
3610 Bottle& r = val.addList();
for (
int i = 0; i < _njoints; i++) { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p); r.addFloat64(
p.kp); }
3613 else if (key ==
"pidCurrentKi")
3615 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p); r.addFloat64(
p.ki); }
3618 else if (key ==
"pidCurrentShift")
3620 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p); r.addFloat64(
p.scale); }
3623 else if (key ==
"pidCurrentOutput")
3625 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); }
3628 else if (key ==
"jointEncoderType")
3630 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++)
3636 yError(
"Invalid jointEncoderType");
3642 else if (key ==
"rotorEncoderType")
3644 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++)
3650 yError(
"Invalid motorEncoderType");
3656 else if (key ==
"coulombThreshold")
3658 val.addString(
"not implemented yet");
3661 else if (key ==
"torqueControlFilterType")
3666 else if (key ==
"torqueControlEnabled")
3669 Bottle& r = val.addList();
3670 for(
int i = 0; i<_njoints; i++)
3672 r.addInt32((
int)_trq_pids[i].enabled );
3676 else if (key ==
"PWMLimit")
3678 Bottle& r = val.addList();
for (
int i = 0; i< _njoints; i++) {
double tmp = 0;
getPWMLimitRaw(i, &
tmp); r.addFloat64(
tmp); }
3681 else if (key ==
"motOverloadCurr")
3683 Bottle& r = val.addList();
for (
int i = 0; i< _njoints; i++) {
double tmp = 0;
getMaxCurrentRaw(i, &
tmp); r.addFloat64(
tmp); }
3686 else if (key ==
"motNominalCurr")
3691 else if (key ==
"motPeakCurr")
3693 Bottle& r = val.addList();
for (
int i = 0; i< _njoints; i++) {
double tmp = 0;
getPeakCurrentRaw(i, &
tmp); r.addFloat64(
tmp); }
3696 else if (key ==
"PowerSuppVoltage")
3701 else if (key ==
"rotorMax")
3704 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getRotorLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp2); }
3707 else if (key ==
"rotorMin")
3710 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getRotorLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp1); }
3713 else if (key ==
"jointMax")
3716 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp2); }
3719 else if (key ==
"jointMin")
3722 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp1); }
3725 else if (key ==
"jointEncTolerance")
3728 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0; getJointEncTolerance(i, &tmp1); r.addFloat64(tmp1); }
3731 else if (key ==
"motorEncTolerance")
3734 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0; getMotorEncTolerance(i, &tmp1); r.addFloat64(tmp1); }
3737 else if (key ==
"jointDeadZone")
3740 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0; getJointDeadZoneRaw(i, tmp1); r.addFloat64(tmp1); }
3743 else if (key ==
"readonly_position_PIDraw")
3745 Bottle& r = val.addList();
3746 for (
int i = 0; i < _njoints; i++)
3748 getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, i, &
p);
3750 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);
3755 else if (key ==
"readonly_velocity_PIDraw")
3757 Bottle& r = val.addList();
3758 for (
int i = 0; i < _njoints; i++)
3759 { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, i, &
p);
3761 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);
3766 else if (key ==
"readonly_torque_PIDraw")
3768 Bottle& r = val.addList();
3769 for (
int i = 0; i < _njoints; i++)
3770 { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, i, &
p);
3772 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);
3777 else if (key ==
"readonly_current_PIDraw")
3779 Bottle& r = val.addList();
3780 for (
int i = 0; i < _njoints; i++)
3781 { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p);
3783 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);
3788 else if (key ==
"readonly_llspeed_PIDraw")
3790 Bottle& r = val.addList();
3791 for (
int i = 0; i < _njoints; i++)
3793 Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, i, &
p);
3795 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);
3800 else if (key ==
"readonly_motor_torque_params_raw")
3802 Bottle& r = val.addList();
3803 for (
int i = 0; i < _njoints; i++)
3805 MotorTorqueParameters params;
3808 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 velocityThres %+3.3f", i, params.bemf, params.bemf_scale, params.ktau, params.ktau_scale, params.viscousPos, params.viscousNeg, params.coulombPos, params.coulombNeg, params.velocityThres);
3813 yWarning(
"getRemoteVariable(): Unknown variable %s", key.c_str());
3819 string s1 = val.toString();
3820 if (val.size() != _njoints)
3822 yWarning(
"setRemoteVariable(): Protocol error %s", s1.c_str());
3826 if (key ==
"kinematic_mj")
3828 yWarning(
"setRemoteVariable(): Impossible to set kinematic_mj parameter at runtime.");
3841 else if (key ==
"PWMLimit")
3843 for (
int i = 0; i < _njoints; i++)
setPWMLimitRaw(i, val.get(i).asFloat64());
3848 else if (key ==
"jointMax")
3851 for (
int i = 0; i < _njoints; i++)
3858 else if (key ==
"jointMin")
3861 for (
int i = 0; i < _njoints; i++)
3868 yWarning(
"setRemoteVariable(): Unknown variable %s", key.c_str());
3874 listOfKeys->clear();
3875 listOfKeys->addString(
"kinematic_mj");
3876 listOfKeys->addString(
"encoders");
3877 listOfKeys->addString(
"gearbox_M2J");
3878 listOfKeys->addString(
"gearbox_E2J");
3879 listOfKeys->addString(
"hasHallSensor");
3880 listOfKeys->addString(
"hasTempSensor");
3881 listOfKeys->addString(
"TemperatureSensorType");
3882 listOfKeys->addString(
"hasRotorEncoder");
3883 listOfKeys->addString(
"hasRotorEncoderIndex");
3884 listOfKeys->addString(
"rotorIndexOffset");
3885 listOfKeys->addString(
"rotorEncoderResolution");
3886 listOfKeys->addString(
"jointEncoderResolution");
3887 listOfKeys->addString(
"motorPoles");
3888 listOfKeys->addString(
"pidCurrentKp");
3889 listOfKeys->addString(
"pidCurrentKi");
3890 listOfKeys->addString(
"pidCurrentShift");
3891 listOfKeys->addString(
"pidCurrentOutput");
3892 listOfKeys->addString(
"coulombThreshold");
3893 listOfKeys->addString(
"torqueControlFilterType");
3894 listOfKeys->addString(
"jointEncoderType");
3895 listOfKeys->addString(
"rotorEncoderType");
3896 listOfKeys->addString(
"PWMLimit");
3897 listOfKeys->addString(
"motOverloadCurr");
3898 listOfKeys->addString(
"motNominalCurr");
3899 listOfKeys->addString(
"motPeakCurr");
3900 listOfKeys->addString(
"PowerSuppVoltage");
3901 listOfKeys->addString(
"rotorMax");
3902 listOfKeys->addString(
"rotorMin");
3903 listOfKeys->addString(
"jointMax");
3904 listOfKeys->addString(
"jointMin");
3905 listOfKeys->addString(
"jointEncTolerance");
3906 listOfKeys->addString(
"motorEncTolerance");
3907 listOfKeys->addString(
"jointDeadZone");
3908 listOfKeys->addString(
"readonly_position_PIDraw");
3909 listOfKeys->addString(
"readonly_velocity_PIDraw");
3910 listOfKeys->addString(
"readonly_current_PIDraw");
3911 listOfKeys->addString(
"readonly_torque_PIDraw");
3912 listOfKeys->addString(
"readonly_motor_torque_params_raw");
3924 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_config);
3926 eOmc_joint_config_t joint_cfg;
3927 if(! askRemoteValue(protoid, &joint_cfg, size))
3930 *
max = joint_cfg.maxvelocityofjoint;
3946 return VAS_status::VAS_OK;
3958 for(
int j=0; j< _njoints; j++)
3967 int j = _axisMap[userLevel_jointNumber];
3969 eOmeas_torque_t meas_torque = 0;
3970 static double curr_time = Time::now();
3971 static int count_saturation=0;
3973 meas_torque = (eOmeas_torque_t)
S_32(_measureConverter->trqN2S(fTorque, j));
3975 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_inputs_externallymeasuredtorque);
3990 eOmc_joint_status_core_t jstatus;
3991 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
3993 *t = (double) _measureConverter->trqS2N(jstatus.measures.meas_torque, j);
4000 for(
int j=0; j<_njoints; j++)
4018 for(
int j=0; j<_njoints && ret; j++)
4025 eOmc_setpoint_t setpoint;
4026 setpoint.type = (eOenum08_t) eomc_setpoint_torque;
4027 setpoint.to.torque.value = (eOmeas_torque_t)
S_32(t);
4029 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
4036 for(
int j=0; j< n_joint; j++)
4046 for(
int j=0; j<_njoints && ret; j++)
4053 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
4054 eOmc_joint_status_core_t jcore = {0};
4060 yError() <<
"embObjMotionControl::getRefTorqueRaw() could not read pid torque reference pos for " << getBoardInfo() <<
"joint " << j;
4064 if ((eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus) &&
4065 (eomc_controlmode_position == jcore.modes.controlmodestatus))
4067 *t = (double) jcore.ofpid.complpos.reftrq;
4070 if(eomc_controlmode_torque == jcore.modes.controlmodestatus)
4072 *t = (double) jcore.ofpid.torque.reftrq;
4078 bool embObjMotionControl::helper_setTrqPidRaw(
int j,
const Pid &pid)
4086 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtorque);
4090 bool embObjMotionControl::helper_getTrqPidRaw(
int j, Pid *pid)
4092 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtorque);
4096 if(! askRemoteValue(protoid, &eoPID, size))
4105 bool embObjMotionControl::helper_getTrqPidsRaw(Pid *pid)
4107 std::vector<eOmc_PID_t> eoPIDList (_njoints);
4108 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_pidtorque, eoPIDList);
4111 for(
int j=0; j< _njoints; j++)
4123 eOmc_impedance_t val;
4128 *stiffness = (double) (val.stiffness);
4129 *damping = (double) (val.damping);
4137 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
4139 if(! askRemoteValue(protoid, &imped, size))
4143 _cacheImpedance->damping = imped.damping;
4144 _cacheImpedance->stiffness = imped.stiffness;
4145 _cacheImpedance->offset = imped.offset;
4152 eOmc_impedance_t val;
4160 _cacheImpedance[j].stiffness = (eOmeas_stiffness_t) stiffness;
4161 _cacheImpedance[j].damping = (eOmeas_damping_t) damping;
4163 val.stiffness = _cacheImpedance[j].stiffness;
4164 val.damping = _cacheImpedance[j].damping;
4165 val.offset = _cacheImpedance[j].offset;
4167 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
4177 eOmc_impedance_t val;
4183 _cacheImpedance[j].offset = (eOmeas_torque_t)
S_32(
offset);
4184 val.stiffness = _cacheImpedance[j].stiffness;
4185 val.damping = _cacheImpedance[j].damping;
4186 val.offset = _cacheImpedance[j].offset;
4188 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
4198 eOmc_impedance_t val;
4209 *min_stiff = _impedance_limits[j].
min_stiff;
4210 *max_stiff = _impedance_limits[j].
max_stiff;
4211 *min_damp = _impedance_limits[j].
min_damp;
4212 *max_damp = _impedance_limits[j].
max_damp;
4218 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_motor_params);
4221 eOmc_motor_params_t eo_params = {0};
4222 if(! askRemoteValue(protoid, &eo_params, size))
4225 params->bemf = eo_params.bemf_value;
4226 params->bemf_scale = eo_params.bemf_scale;
4227 params->ktau = eo_params.ktau_value;
4228 params->ktau_scale = eo_params.ktau_scale;
4229 params->viscousPos = eo_params.friction.viscous_pos_val;
4230 params->viscousNeg = eo_params.friction.viscous_neg_val ;
4231 params->coulombPos = eo_params.friction.coulomb_pos_val;
4232 params->coulombNeg = eo_params.friction.coulomb_neg_val;
4233 params->velocityThres = eo_params.friction.velocityThres_val;
4242 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_motor_params);
4243 eOmc_motor_params_t eo_params = {0};
4247 eo_params.bemf_value = (float) params.bemf;
4248 eo_params.bemf_scale = (uint8_t) params.bemf_scale;
4249 eo_params.ktau_value = (float) params.ktau;
4250 eo_params.ktau_scale = (uint8_t) params.ktau_scale;
4251 eo_params.friction.viscous_pos_val =
static_cast<float32_t
>(params.viscousPos);
4252 eo_params.friction.viscous_neg_val =
static_cast<float32_t
>(params.viscousNeg);
4253 eo_params.friction.coulomb_pos_val =
static_cast<float32_t
>(params.coulombPos);
4254 eo_params.friction.coulomb_neg_val =
static_cast<float32_t
>(params.coulombNeg);
4255 eo_params.friction.velocityThres_val =
static_cast<float32_t
>(params.velocityThres);
4260 yError() <<
"embObjMotionControl::setMotorTorqueParamsRaw() could not send set message for" << getBoardInfo() <<
"joint " << j;
4272 for(
int j=0; j< n_joint; j++)
4306 bool embObjMotionControl::helper_getVelPidRaw(
int j, Pid *pid)
4308 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_piddirect);
4311 if(! askRemoteValue(protoid, &eoPID, size))
4321 bool embObjMotionControl::helper_getVelPidsRaw(Pid *pid)
4323 std::vector <eOmc_PID_t> eoPIDList (_njoints);
4324 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_piddirect, eoPIDList);
4328 for(
int j=0; j<_njoints; j++)
4343 if (mode != VOCAB_CM_POSITION_DIRECT &&
4344 mode != VOCAB_CM_IDLE)
4348 yError() <<
"setReferenceRaw: skipping command because" << getBoardInfo() <<
" joint " << j <<
" is not in VOCAB_CM_POSITION_DIRECT mode";
4353 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
4354 eOmc_setpoint_t setpoint = {0};
4356 _ref_positions[j] = ref;
4357 setpoint.type = (eOenum08_t) eomc_setpoint_positionraw;
4358 setpoint.to.position.value = (eOmeas_position_t)
S_32(ref);
4359 setpoint.to.position.withvelocity = 0;
4367 for(
int i=0; i<n_joint; i++)
4377 for (
int i = 0; i<_njoints; i++)
4387 if (axis<0 || axis>_njoints)
return false;
4388 #if ASK_REFERENCE_TO_FIRMWARE
4389 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4394 eOmc_joint_status_target_t target = {0};
4395 if(!askRemoteValue(id32, &target, size))
4397 yError() <<
"embObjMotionControl::getTargetPositionRaw() could not read reference pos for " << getBoardInfo() <<
"joint " << axis;
4401 *ref = (double) target.trgt_position;
4405 *ref = _ref_command_positions[axis];
4413 for (
int i = 0; i<_njoints; i++)
4423 for (
int i = 0; i<nj; i++)
4432 if (axis<0 || axis>_njoints)
return false;
4433 #if ASK_REFERENCE_TO_FIRMWARE
4434 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4439 eOmc_joint_status_target_t target = {0};
4440 if(!askRemoteValue(id32, &target, size))
4442 yError() <<
"embObjMotionControl::getRefVelocityRaw() could not read reference vel for " << getBoardInfo() <<
"joint " << axis;
4445 *ref = (double) target.trgt_velocity;
4448 *ref = _ref_command_speeds[axis];
4455 #if ASK_REFERENCE_TO_FIRMWARE
4456 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
4457 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
4460 yError() <<
"embObjMotionControl::getRefVelocitiesRaw() could not read reference vel for " << getBoardInfo() ;
4464 for(
int j=0; j<_njoints; j++)
4466 refs[j] = (double) targetList[j].trgt_velocity;
4470 for(
int j=0; j<_njoints; j++)
4472 refs[j] = _ref_command_speeds[j];
4480 std::vector <double> refsList(_njoints);
4484 for (
int i = 0; i<nj; i++)
4486 if(
jnts[i]>= _njoints)
4488 yError() << getBoardInfo() <<
"getRefVelocitiesRaw: joint " <<
jnts[i] <<
"doesn't exist";
4491 refs[i] = refsList[
jnts[i]];
4498 if (axis<0 || axis>_njoints)
return false;
4499 #if ASK_REFERENCE_TO_FIRMWARE
4500 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4504 eOmc_joint_status_target_t target = {0};
4505 if(!askRemoteValue(id32, &target, size))
4507 yError() <<
"embObjMotionControl::getRefPositionRaw() could not read reference pos for " << getBoardInfo() <<
"joint " << axis;
4511 *ref = (double) target.trgt_positionraw;
4514 *ref = _ref_positions[axis];
4521 #if ASK_REFERENCE_TO_FIRMWARE
4522 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
4523 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
4526 yError() <<
"embObjMotionControl::getRefPositionRaw() could not read reference pos for " << getBoardInfo();
4530 for(
int j=0; j< _njoints; j++)
4531 refs[j] = (
double) targetList[j].trgt_positionraw;
4534 for(
int j=0; j< _njoints; j++)
4535 refs[j] = _ref_positions[j];
4543 for (
int i = 0; i<nj; i++)
4556 eOenum08_t interactionmodestatus;
4559 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core_modes_interactionmodestatus);
4563 int tmp = (int) *_mode;
4567 *_mode = (yarp::dev::InteractionModeEnum)
tmp;
4586 for(
int j=0; j<_njoints; j++)
4596 eOenum08_t interactionmodecommand = 0;
4601 if (_mode == VOCAB_IM_COMPLIANT && _trq_pids[j].enabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
return false;}
4605 yError() <<
"setInteractionModeRaw: received unknown mode for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
4608 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4610 if(
false == res->
setRemoteValue(protid, &interactionmodecommand) )
4612 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
4618 eOenum08_t interactionmodestatus = 0;
4620 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4621 bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4623 if((
false == ret) || (interactionmodecommand != interactionmodestatus))
4625 yError() <<
"check of embObjMotionControl::setInteractionModeRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
4638 eOenum08_t interactionmodecommand = 0;
4640 for(
int j=0; j<n_joints; j++)
4642 if (modes[j] == VOCAB_IM_COMPLIANT && _trq_pids[j].enabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
continue;}
4646 yError() <<
"embObjMotionControl::setInteractionModesRaw(): received unknown interactionMode for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]) <<
" " << modes[j];
4650 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4651 if(
false == res->
setRemoteValue(protid, &interactionmodecommand) )
4653 yError() <<
"embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4659 eOenum08_t interactionmodestatus = 0;
4661 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4662 bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4664 if((
false == ret) || (interactionmodecommand != interactionmodestatus))
4668 yError() <<
"check of embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4674 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4675 << Vocab32::decode(modes[j]) <<
" Got " << Vocab32::decode(
tmp);
4677 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4678 << Vocab32::decode(modes[j]) <<
" Got an unknown value!";
4691 eOenum08_t interactionmodecommand = 0;
4693 for(
int j=0; j<_njoints; j++)
4695 if ((modes[j] == VOCAB_IM_COMPLIANT) && (_trq_pids[j].enabled ==
false))
4697 yError()<<
"Torque control is disabled. Check your configuration parameters";
4703 yError() <<
"setInteractionModeRaw: received unknown interactionMode for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4707 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4708 if(
false == res->
setRemoteValue(protid, &interactionmodecommand) )
4710 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4716 eOenum08_t interactionmodestatus = 0;
4718 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4719 bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4721 if((
false == ret) || (interactionmodecommand != interactionmodestatus))
4725 yError() <<
"check of embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4731 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4732 << Vocab32::decode(modes[j]) <<
" Got " << Vocab32::decode(
tmp);
4734 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4735 << Vocab32::decode(modes[j]) <<
" Got an unknown value!";
4748 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
4749 eOmc_joint_status_core_t jcore = {0};
4756 case VOCAB_PIDTYPE_POSITION:
4757 if((eomc_controlmode_torque == jcore.modes.controlmodestatus) || (eomc_controlmode_current == jcore.modes.controlmodestatus))
4760 *
out = (double) jcore.ofpid.generic.output;
4765 case VOCAB_PIDTYPE_TORQUE:
4766 if ((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
4767 ((eomc_controlmode_position == jcore.modes.controlmodestatus) && (eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus)))
4768 *
out = jcore.ofpid.generic.output;
4772 case VOCAB_PIDTYPE_CURRENT:
4775 case VOCAB_PIDTYPE_VELOCITY:
4779 yError()<<
"Invalid pidtype:"<<pidtype;
4788 for(
int j=0; j< _njoints; j++)
4808 eOmc_motor_status_basic_t status;
4809 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
4820 yError() << getBoardInfo() <<
"At timestamp" << yarp::os::Time::now() <<
"In motor" << m <<
"embObjMotionControl::getTemperatureRaw failed to complete getLocalValue()";
4829 *val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius((
double)status.mot_temperature);
4838 for(
int j=0; j< _njoints; j++)
4859 *temp= _temperatureLimits[m].warningTemperatureLimit;
4866 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_temperaturelimit);
4867 eOmeas_temperature_t temperatureLimit = (eOmeas_pwm_t)
S_16(temp);
4874 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4876 eOmc_current_limits_params_t currentlimits = {0};
4878 if(!askRemoteValue(protid, ¤tlimits, size))
4880 yError() <<
"embObjMotionControl::getPeakCurrentRaw() can't read current limits for" << getBoardInfo() <<
" motor " << m;
4884 *val = (double) currentlimits.peakCurrent ;
4890 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4893 eOmc_current_limits_params_t currentlimits = {0};
4894 if(!askRemoteValue(protid, ¤tlimits, size))
4896 yError() <<
"embObjMotionControl::setPeakCurrentRaw can't read current limits for" << getBoardInfo() <<
" motor " << m ;
4901 currentlimits.peakCurrent = (eOmeas_current_t)
S_16(val);
4907 yError() <<
"embObjMotionControl::setPeakCurrentRaw failed sending new value for" << getBoardInfo() <<
" motor " << m ;
4914 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4916 eOmc_current_limits_params_t currentlimits = {0};
4918 if(!askRemoteValue(protid, ¤tlimits, size))
4920 yError() <<
"embObjMotionControl::getNominalCurrentRaw() can't read current limits for" << getBoardInfo() <<
" motor " << m;
4924 *val = (double) currentlimits.nominalCurrent ;
4930 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4934 eOmc_current_limits_params_t currentlimits = {0};
4935 if(!askRemoteValue(protid, ¤tlimits, size))
4937 yError() <<
"embObjMotionControl::setNominalCurrentRaw can't read current limits for" << getBoardInfo() <<
" motor " << m ;
4942 currentlimits.nominalCurrent = (eOmeas_current_t)
S_16(val);
4948 yError() <<
"embObjMotionControl::setNominalCurrentRaw failed sending new value for" << getBoardInfo() <<
" motor " << m ;
4956 eOmc_motor_status_basic_t status;
4957 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
4962 *val = (double) status.mot_pwm;
4966 yError() <<
"embObjMotionControl::getPWMRaw failed for" << getBoardInfo() <<
" motor " << j ;
4975 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pwmlimit);
4977 eOmeas_pwm_t motorPwmLimit;
4979 bool ret = askRemoteValue(protid, &motorPwmLimit, size);
4982 *val = (double) motorPwmLimit;
4986 yError() <<
"embObjMotionControl::getPWMLimitRaw failed for" << getBoardInfo() <<
" motor " << j ;
4997 yError() <<
"embObjMotionControl::setPWMLimitRaw failed because pwmLimit is negative for" << getBoardInfo() <<
" motor " << j ;
5000 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pwmlimit);
5001 eOmeas_pwm_t motorPwmLimit = (eOmeas_pwm_t)
S_16(val);
5007 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_controller, 0, eoprot_tag_mc_controller_status);
5009 eOmc_controller_status_t controllerStatus;
5011 bool ret = askRemoteValue(protid, &controllerStatus, size);
5014 *val = (double) controllerStatus.supplyVoltage;
5018 yError() <<
"embObjMotionControl::getPowerSupplyVoltageRaw failed for" << getBoardInfo() <<
" motor " << j ;
5025 bool embObjMotionControl::askRemoteValue(eOprotID32_t id32,
void* value, uint16_t& size)
5032 bool embObjMotionControl::askRemoteValues(eOprotEndpoint_t ep, eOprotEntity_t entity, eOprotTag_t tag, std::vector<T>& values)
5034 std::vector<eOprotID32_t> idList;
5035 std::vector<void*> valueList;
5038 for(
int j=0; j<_njoints; j++)
5040 eOprotID32_t protoId = eoprot_ID_get(ep, entity, j, tag);
5041 idList.push_back(protoId);
5042 valueList.push_back((
void*)&values[j]);
5048 yError() <<
"embObjMotionControl::askRemoteValues failed for all joints of" << getBoardInfo();
5057 bool embObjMotionControl::checkRemoteControlModeStatus(
int joint,
int target_mode)
5060 eOenum08_t temp = 0;
5063 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core_modes_controlmodestatus);
5064 const double timeout = 0.250;
5065 const int maxretries = 25;
5066 const double delaybetweenqueries = 0.010;
5070 double timeofstart = yarp::os::Time::now();
5073 for( attempt = 0; attempt < maxretries; attempt++)
5075 ret = askRemoteValue(id32, &temp, size);
5078 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());
5082 if(current_mode == target_mode)
5087 if((current_mode == VOCAB_CM_IDLE) && (target_mode == VOCAB_CM_FORCE_IDLE))
5092 if(current_mode == VOCAB_CM_HW_FAULT)
5094 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()); }
5099 if((yarp::os::Time::now()-timeofstart) > timeout)
5102 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());
5107 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());
5109 SystemClock::delaySystem(delaybetweenqueries);
5114 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);
5122 bool embObjMotionControl::iNeedCouplingsInfo(
void)
5124 eOmn_serv_type_t mc_serv_type = (eOmn_serv_type_t)serviceConfig.
ethservice.configuration.type;
5125 if( (mc_serv_type == eomn_serv_MC_foc) ||
5126 (mc_serv_type == eomn_serv_MC_mc4plus) ||
5127 (mc_serv_type == eomn_serv_MC_mc4plusmais) ||
5128 (mc_serv_type == eomn_serv_MC_mc2pluspsc) ||
5129 (mc_serv_type == eomn_serv_MC_mc4plusfaps) ||
5130 (mc_serv_type == eomn_serv_MC_advfoc)
5140 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
5142 eOmc_setpoint_t setpoint;
5144 setpoint.type = (eOenum08_t)eomc_setpoint_openloop;
5145 setpoint.to.openloop.value = (eOmeas_pwm_t)
S_16(v);
5153 for (
int j = 0; j<_njoints; j++)
5162 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_target);
5165 eOmc_joint_status_target_t target = { 0 };
5168 if (!askRemoteValue(protoId, &target, size))
5170 yError() <<
"embObjMotionControl::getRefDutyCycleRaw() could not read openloop reference for " << getBoardInfo() <<
"joint " << j;
5174 *v = (double)target.trgt_pwm;
5181 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
5182 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
5185 yError() <<
"embObjMotionControl::getDutyCyclesRaw failed for all joints of" << getBoardInfo();
5188 for (
int j = 0; j<_njoints; j++)
5190 v[j]= targetList[j].trgt_pwm;
5197 eOmc_motor_status_basic_t status;
5198 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
5203 *v = (double)status.mot_pwm;
5207 yError() <<
"embObjMotionControl::getDutyCycleRaw failed for" << getBoardInfo() <<
" motor " << j;
5217 for (
int j = 0; j< _njoints; j++)
5239 for (
int j = 0; j< _njoints; j++)
5249 for (
int j = 0; j<_njoints; j++)
5258 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
5260 eOmc_setpoint_t setpoint;
5262 setpoint.type = (eOenum08_t)eomc_setpoint_current;
5263 setpoint.to.current.value = (eOmeas_pwm_t)
S_16(t);
5271 for (
int j = 0; j<n_joint; j++)
5280 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
5281 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
5284 yError() <<
"embObjMotionControl::getDutyCyclesRaw failed for all joints of" << getBoardInfo();
5287 for (
int j = 0; j<_njoints; j++)
5289 t[j] = targetList[j].trgt_current;
5296 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_target);
5299 eOmc_joint_status_target_t target = { 0 };
5302 if (!askRemoteValue(protoId, &target, size))
5304 yError() <<
"embObjMotionControl::getRefDutyCycleRaw() could not read openloop reference for " << getBoardInfo() <<
"joint " << j;
5308 *t = (double)target.trgt_current;
5313 bool embObjMotionControl::helper_setCurPidRaw(
int j,
const Pid &pid)
5315 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pidcurrent);
5319 if (!_cur_pids[j].enabled)
5321 yError() <<
"eoMc " << getBoardInfo() <<
": it is not possible set current pid for motor " << j <<
", because current pid is not enabled in xml files";
5329 yError() <<
"while setting velocity PIDs for" << getBoardInfo() <<
" joint " << j;
5338 bool embObjMotionControl::helper_setSpdPidRaw(
int j,
const Pid &pid)
5340 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pidspeed);
5344 if (!_cur_pids[j].enabled)
5346 yError() <<
"eoMc " << getBoardInfo() <<
": it is not possible set speed pid for motor " << j <<
", because speed pid is not enabled in xml files";
5354 yError() <<
"while setting velocity PIDs for" << getBoardInfo() <<
" joint " << j;
5363 bool embObjMotionControl::helper_getCurPidRaw(
int j, Pid *pid)
5365 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
5367 eOmc_motor_config_t motor_cfg;
5368 if(! askRemoteValue(protoid, &motor_cfg, size))
5372 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg.pidcurrent;
5378 bool embObjMotionControl::helper_getSpdPidRaw(
int j, Pid *pid)
5380 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
5382 eOmc_motor_config_t motor_cfg;
5383 if (!askRemoteValue(protoid, &motor_cfg, size))
5387 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg.pidspeed;
5393 bool embObjMotionControl::helper_getCurPidsRaw(Pid *pid)
5395 std::vector <eOmc_motor_config_t> motor_cfg_list(_njoints);
5396 bool ret = askRemoteValues<eOmc_motor_config_t>(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, eoprot_tag_mc_motor_config, motor_cfg_list);
5400 for(
int j=0; j<_njoints; j++)
5402 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg_list[j].pidcurrent;
5408 bool embObjMotionControl::helper_getSpdPidsRaw(Pid *pid)
5410 std::vector <eOmc_motor_config_t> motor_cfg_list(_njoints);
5411 bool ret = askRemoteValues<eOmc_motor_config_t>(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, eoprot_tag_mc_motor_config, motor_cfg_list);
5415 for (
int j = 0; j<_njoints; j++)
5417 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg_list[j].pidspeed;
5423 bool embObjMotionControl::getJointConfiguration(
int joint, eOmc_joint_config_t *jntCfg_ptr)
5425 uint32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_config);
5427 if(!askRemoteValue(protoid, jntCfg_ptr, size))
5435 bool embObjMotionControl::getMotorConfiguration(
int axis, eOmc_motor_config_t *motCfg_ptr)
5437 uint32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, axis, eoprot_tag_mc_motor_config);
5439 if(!askRemoteValue(protoid, motCfg_ptr, size))
5448 bool embObjMotionControl::getGerabox_E2J(
int joint,
double *gearbox_E2J_ptr)
5450 eOmc_joint_config_t jntCfg;
5452 if(!getJointConfiguration(joint, &jntCfg))
5457 *gearbox_E2J_ptr = jntCfg.gearbox_E2J;
5461 bool embObjMotionControl::getJointEncTolerance(
int joint,
double *jEncTolerance_ptr)
5463 eOmc_joint_config_t jntCfg;
5465 if(!getJointConfiguration(joint, &jntCfg))
5470 *jEncTolerance_ptr = jntCfg.jntEncTolerance;
5474 bool embObjMotionControl::getMotorEncTolerance(
int axis,
double *mEncTolerance_ptr)
5476 eOmc_motor_config_t motorCfg;
5477 if(!getMotorConfiguration(axis, &motorCfg))
5482 *mEncTolerance_ptr = motorCfg.rotEncTolerance;
5488 eOmc_motor_status_t status;
5490 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol,
5491 eoprot_entity_mc_motor, j,
5492 eoprot_tag_mc_motor_status);
5501 message =
"Could not retrieve the fault state.";
5505 if (status.mc_fault_state == EOERROR_CODE_DUMMY)
5507 fault = EOERROR_CODE_DUMMY;
5508 message =
"No fault detected.";
5513 fault = eoerror_code2value(status.mc_fault_state);
5514 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 bool getEntityName(uint32_t entityId, std::string &entityName)
virtual eth::iethresType_t type()
bool getTemperatureSensorTypeRaw(int j, std::string &ret)
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 parseFocGroup(yarp::os::Searchable &config, focBasedSpecificInfo_t *foc_based_info, std::string groupName, std::vector< std::unique_ptr< eomc::ITemperatureSensor >> &temperatureSensorsVector)
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 parseTemperatureLimits(yarp::os::Searchable &config, std::vector< temperatureLimits_t > &temperatureLimits)
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 constexpr double const temperatureErrorValue_s
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)
@ motor_temperature_sensor_pt100
@ motor_temperature_sensor_pt1000
@ motor_temperature_sensor_none
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