17#include <yarp/os/Bottle.h>
18#include <yarp/os/Time.h>
21#include <FeatureInterface.h>
22#include <yarp/conf/environment.h>
24#include <yarp/os/LogStream.h>
26#include <yarp/os/NetType.h>
27#include <yarp/dev/ControlBoardHelper.h>
32#include "EoProtocol.h"
33#include "EoProtocolMN.h"
34#include "EoProtocolMC.h"
35#include "EoProtocolAS.h"
41using namespace yarp::os;
42using namespace yarp::os::impl;
50#define ASK_REFERENCE_TO_FIRMWARE 1
52#define PARSER_MOTION_CONTROL_VERSION 6
57 yErrorOnce() << txt <<
" is not yet implemented for embObjMotionControl";
63 yErrorOnce() << txt <<
" has been deprecated for embObjMotionControl";
67#define NV_NOT_FOUND return nv_not_found();
71 yError () <<
" nv_not_found!! This may mean that this variable is not handled by this EMS\n";
77std::string embObjMotionControl::getBoardInfo(
void)
81 return " BOARD name_unknown (IP unknown) ";
90bool embObjMotionControl::alloc(
int nj)
92 _axisMap = allocAndCheck<int>(nj);
94 _encodersStamp = allocAndCheck<double>(nj);
95 _gearbox_M2J = allocAndCheck<double>(nj);
96 _gearbox_E2J = allocAndCheck<double>(nj);
97 _deadzone = allocAndCheck<double>(nj);
98 _foc_based_info= allocAndCheck<eomc::focBasedSpecificInfo_t>(nj);
104 _impedance_limits=allocAndCheck<eomc::impedanceLimits_t>(nj);
105 checking_motiondone=allocAndCheck<bool>(nj);
106 _last_position_move_time=allocAndCheck<double>(nj);
109 _ref_command_positions = allocAndCheck<double>(nj);
110 _ref_positions = allocAndCheck<double>(nj);
111 _ref_command_speeds = allocAndCheck<double>(nj);
112 _ref_speeds = allocAndCheck<double>(nj);
113 _ref_accs = allocAndCheck<double>(nj);
115 _enabledAmp = allocAndCheck<bool>(nj);
116 _enabledPid = allocAndCheck<bool>(nj);
117 _calibrated = allocAndCheck<bool>(nj);
118 _cacheImpedance = allocAndCheck<eOmc_impedance_t>(nj);
120 _rotorsLimits.resize(nj);
121 _jointsLimits.resize(nj);
122 _currentLimits.resize(nj);
123 _temperatureLimits.resize(nj);
125 _joint2set.resize(nj);
126 _timeouts.resize(nj);
127 _impedance_params.resize(nj);
128 _lugre_params.resize(nj);
129 _axesInfo.resize(nj);
130 _jointEncs.resize(nj);
131 _motorEncs.resize(nj);
132 _kalman_params.resize(nj);
133 _temperatureSensorsVector.resize(nj);
134 _temperatureExceededLimitWatchdog.resize(nj);
135 _temperatureSensorErrorWatchdog.resize(nj);
136 _temperatureSpikesFilter.resize(nj);
140 for(
int i = 0; i < nj; ++i)
142 _temperatureExceededLimitWatchdog.at(i).setThreshold(txrate);
143 _temperatureSensorErrorWatchdog.at(i).setThreshold(txrate);
149bool embObjMotionControl::dealloc()
151 checkAndDestroy(_axisMap);
152 checkAndDestroy(_encodersStamp);
153 checkAndDestroy(_gearbox_M2J);
154 checkAndDestroy(_gearbox_E2J);
155 checkAndDestroy(_deadzone);
156 checkAndDestroy(_impedance_limits);
157 checkAndDestroy(checking_motiondone);
158 checkAndDestroy(_ref_command_positions);
159 checkAndDestroy(_ref_positions);
160 checkAndDestroy(_ref_command_speeds);
161 checkAndDestroy(_ref_speeds);
162 checkAndDestroy(_ref_accs);
164 checkAndDestroy(_enabledAmp);
165 checkAndDestroy(_enabledPid);
166 checkAndDestroy(_calibrated);
167 checkAndDestroy(_foc_based_info);
189 ImplementControlCalibration(this),
190 ImplementAmplifierControl(this),
191 ImplementPidControl(this),
192 ImplementEncodersTimed(this),
193 ImplementPositionControl(this),
194 ImplementVelocityControl(this),
195 ImplementControlMode(this),
196 ImplementImpedanceControl(this),
197 ImplementMotorEncoders(this),
198#ifdef IMPLEMENT_DEBUG_INTERFACE
201 ImplementTorqueControl(this),
202 ImplementControlLimits(this),
203 ImplementPositionDirect(this),
204 ImplementInteractionMode(this),
205 ImplementMotor(this),
206 ImplementRemoteVariables(this),
207 ImplementAxisInfo(this),
208 ImplementPWMControl(this),
209 ImplementCurrentControl(this),
210 ImplementJointFault(this),
211 SAFETY_THRESHOLD(2.0),
215 _temperatureLimits(0),
219 _impedance_params(0),
225 _temperatureSensorsVector(0),
226 _temperatureExceededLimitWatchdog(0),
227 _temperatureSensorErrorWatchdog(0),
228 _temperatureSpikesFilter(0),
229 _rawDataAuxVector(0),
230 _rawValuesMetadataMap({})
244 _encodersStamp = NULL;
245 _foc_based_info = NULL;
246 _cacheImpedance = NULL;
247 _impedance_limits = NULL;
249 _ref_command_speeds = NULL;
250 _ref_command_positions= NULL;
251 _ref_positions = NULL;
253 _measureConverter = NULL;
255 checking_motiondone = NULL;
266 _last_position_move_time = NULL;
268 behFlags.useRawEncoderData =
false;
269 behFlags.pwmIsLimited =
false;
271 _maintenanceModeCfg.enableSkipRecalibration =
false;
273 std::string tmp = yarp::conf::environment::get_string(
"ETH_VERBOSEWHENOK");
276 behFlags.verbosewhenok = (bool)yarp::conf::numeric::from_string(tmp, 0U);
280 behFlags.verbosewhenok =
false;
285#ifdef NETWORK_PERFORMANCE_BENCHMARK
289 m_responseTimingVerifier.init(0.003, 0.00025, 0.001, 0.01, 0.0005, 30);
297 yTrace() <<
"embObjMotionControl::~embObjMotionControl()";
305 if(NULL != _mcparser)
323 ImplementControlCalibration::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
324 ImplementAmplifierControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.ampsToSensor);
325 ImplementEncodersTimed::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
326 ImplementMotorEncoders::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
327 ImplementPositionControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
328 ImplementPidControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.newtonsToSensor,
f.ampsToSensor,
f.dutycycleToPWM);
329 ImplementControlMode::initialize(_njoints, _axisMap);
330 ImplementVelocityControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
331 ImplementControlLimits::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
332 ImplementImpedanceControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.newtonsToSensor);
333 ImplementTorqueControl::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL,
f.newtonsToSensor,
f.ampsToSensor,
f.dutycycleToPWM,
f.bemf2raw,
f.ktau2raw);
334 ImplementPositionDirect::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
335 ImplementInteractionMode::initialize(_njoints, _axisMap,
f.angleToEncoder, NULL);
336 ImplementMotor::initialize(_njoints, _axisMap);
337 ImplementRemoteVariables::initialize(_njoints, _axisMap);
338 ImplementAxisInfo::initialize(_njoints, _axisMap);
339 ImplementCurrentControl::initialize(_njoints, _axisMap,
f.ampsToSensor);
340 ImplementPWMControl::initialize(_njoints, _axisMap,
f.dutycycleToPWM);
341 ImplementJointFault::initialize(_njoints, _axisMap);
352 if(NULL == ethManager)
354 yFatal() <<
"embObjMotionControl::open() fails to instantiate ethManager";
358 eOipv4addr_t ipv4addr;
359 string boardIPstring;
361 if(
false == ethManager->
verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
363 yError() <<
"embObjMotionControl::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
370 yError() << getBoardInfo() <<
"embObjMotionControl::open(): eth::parser fails to read board configuration data from xml file";
387 yError() <<
"embObjMotionControl::open() fails because could not instantiate the ethResource for " << getBoardInfo() <<
" ... unable to continue";
391 if(!fromConfig(config))
393 yError() << getBoardInfo() <<
"Missing motion control parameters in config file";
399 yError() <<
"embObjMotionControl: failed verifyEPprotocol. Cannot continue!";
405 const eOmn_serv_parameter_t* servparam = &serviceConfig.
ethservice;
406 if(eomn_serv_MC_generic == serviceConfig.
ethservice.configuration.type)
412 mcdiagnostics.
config.mode = serviceConfig.
ethservice.configuration.diagnosticsmode;
413 mcdiagnostics.
config.par16 = serviceConfig.
ethservice.configuration.diagnosticsparam;
414 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
417 mcdiagnostics.
ports.resize(2);
418 for(
size_t i=0; i<mcdiagnostics.
ports.size(); i++)
420 mcdiagnostics.
ports[i] =
new BufferedPort<Bottle>;
431 event_downsampler->
config.
info = getBoardInfo();
432 event_downsampler->
start();
436 yError() <<
"embObjMotionControl::open() has an error in call of ethResources::serviceVerifyActivate() for" << getBoardInfo();
441 yDebug() <<
"embObjMotionControl:serviceVerifyActivate OK!";
446 yError() <<
"embObjMotionControl::open() has an error in call of embObjMotionControl::init() for" << getBoardInfo();
453 yDebug() <<
"embObjMotionControl::init() has succesfully initted" << getBoardInfo();
460 yError() <<
"embObjMotionControl::open() fails to start mc service for" << getBoardInfo() <<
": cannot continue";
468 yDebug() <<
"embObjMotionControl::open() correctly starts mc service of" << getBoardInfo();
476 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
507 SystemClock::delaySystem(0.001*mcdiagnostics.
config.par16);
514int embObjMotionControl::fromConfig_NumOfJoints(yarp::os::Searchable &config)
519 int jn = config.findGroup(
"GENERAL").check(
"Joints", Value(1),
"Number of degrees of freedom").asInt32();
526void embObjMotionControl::debugUtil_printJointsetInfo(
void)
529 yError() <<
"****** DEBUG PRINTS **********";
530 yError() <<
"joint to set:";
531 for(
int x=0;
x< _njoints;
x++)
532 yError() <<
" /t j " <<
x <<
": set " <<_joint2set[
x];
533 yError() <<
"jointmap:";
535 yError() <<
" number of sets" << _jsets.size();
536 for(
size_t x=0;
x< _jsets.size();
x++)
538 yError() <<
"set " <<
x<<
"has size " <<_jsets[
x].getNumberofJoints();
539 for(
int y=0;
y<_jsets[
x].getNumberofJoints();
y++)
540 yError() <<
"set " <<
x <<
": " << _jsets[
x].joints[
y];
542 yError() <<
"********* END ****************";
549bool embObjMotionControl::verifyUserControlLawConsistencyInJointSet(
eomc::PidInfo *pidInfo)
552 for(
size_t s=0; s<_jsets.size(); s++)
554 int numofjoints = _jsets[s].getNumberofJoints();
558 yError() <<
"embObjMC" << getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
561 int firstjoint = _jsets[s].joints[0];
563 for(
int k=1; k<numofjoints; k++)
565 int otherjoint = _jsets[s].joints[k];
567 if(pidInfo[firstjoint].usernamePidSelected != pidInfo[otherjoint].usernamePidSelected)
569 yError() <<
"embObjMC "<< getBoardInfo() <<
"Joints beloning to same set must be have same control law. Joint " << otherjoint <<
" differs from " << firstjoint <<
"Set num " << s ;
582bool embObjMotionControl::verifyUserControlLawConsistencyInJointSet(
eomc::TrqPidInfo *pidInfo)
584 for(
size_t s=0; s<_jsets.size(); s++)
586 int numofjoints = _jsets[s].getNumberofJoints();
590 yError() <<
"embObjMC "<< getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
593 int firstjoint = _jsets[s].joints[0];
595 for(
int k=1; k<numofjoints; k++)
597 int otherjoint = _jsets[s].joints[k];
599 if(pidInfo[firstjoint].usernamePidSelected != pidInfo[otherjoint].usernamePidSelected)
601 yError() <<
"embObjMC"<< getBoardInfo() <<
"Joints beloning to same set must be have same control law. Joint " << otherjoint <<
" differs from " << firstjoint <<
"Set num " << s ;
611bool embObjMotionControl::updatedJointsetsCfgWithControlInfo()
615 for(
size_t s=0; s<_jsets.size(); s++)
617 if(_jsets[s].getNumberofJoints() == 0)
619 yError() <<
"embObjMC"<< getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
623 int joint = _jsets[s].joints[0];
633 _jsets[s].cfg.pid_output_types.postrj_ctrl_out_type = _trj_pids[joint].
out_type;
634 _jsets[s].cfg.pid_output_types.veltrj_ctrl_out_type = _trj_pids[joint].
out_type;
635 _jsets[s].cfg.pid_output_types.mixtrj_ctrl_out_type = _trj_pids[joint].
out_type;
639 _jsets[s].cfg.pid_output_types.posdir_ctrl_out_type = _trj_pids[joint].
out_type;
640 _jsets[s].cfg.pid_output_types.veldir_ctrl_out_type = _trj_pids[joint].
out_type;
642 _jsets[s].cfg.pid_output_types.torque_ctrl_out_type = _trq_pids[joint].
out_type;
644 _jsets[s].cfg.pid_output_types.pwm_ctrl_out_type = eomc_ctrl_out_type_pwm;
646 if (_cur_pids[joint].enabled)
648 _jsets[s].cfg.pid_output_types.cur_ctrl_out_type = eomc_ctrl_out_type_cur;
652 _jsets[s].cfg.pid_output_types.cur_ctrl_out_type = eomc_ctrl_out_type_n_a;
661bool embObjMotionControl::saveCouplingsData(
void)
663 eOmc_4jomo_coupling_t *jc_dest;
665 static eOmc_4jomo_coupling_t dummyjomocoupling = {};
667 switch(serviceConfig.
ethservice.configuration.type)
669 case eomn_serv_MC_foc:
671 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.foc_based.jomocoupling);
674 case eomn_serv_MC_mc4plus:
676 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc4plus_based.jomocoupling);
679 case eomn_serv_MC_mc4plusmais:
681 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc4plusmais_based.jomocoupling);
685 case eomn_serv_MC_mc2pluspsc:
687 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc2pluspsc.jomocoupling);
691 case eomn_serv_MC_mc4plusfaps:
693 jc_dest = &(serviceConfig.
ethservice.configuration.data.mc.mc4plusfaps.jomocoupling);
697 case eomn_serv_MC_advfoc:
699 jc_dest = &dummyjomocoupling;
702 case eomn_serv_MC_mc4:
707 case eomn_serv_MC_generic:
719 memset(jc_dest, 0,
sizeof(eOmc_4jomo_coupling_t));
723 for(
int i=0; i<4; i++)
725 jc_dest->joint2set[i] = eomc_jointSetNum_none;
728 if(_joint2set.size() > 4 )
730 yError() <<
"embObjMC "<< getBoardInfo() <<
"Jointsset size is bigger than 4. I can't send jointset information to fw.";
734 for(
size_t i=0; i<_joint2set.size(); i++)
736 jc_dest->joint2set[i] = _joint2set[i];
739 for(
int i=0; i<4; i++)
741 for(
int j=0; j<4; j++)
743 jc_dest->joint2motor[i][j] = eo_common_float_to_Q17_14((
float)_couplingInfo.
matrixJ2M[4*i+j]);
744 jc_dest->motor2joint[i][j] = eo_common_float_to_Q17_14((
float)_couplingInfo.
matrixM2J[4*i+j]);
749 for(
int r=0; r<4; r++)
751 for(
int c=0; c<6; c++)
753 jc_dest->encoder2joint[r][c] = eo_common_float_to_Q17_14((
float)_couplingInfo.
matrixE2J[6*r+c]);
757 for(
size_t s=0; s< _jsets.size(); s++)
759 eOmc_jointset_configuration_t* cfg_ptr = _jsets[s].getConfiguration();
760 memcpy(&(jc_dest->jsetcfg[s]), cfg_ptr,
sizeof(eOmc_jointset_configuration_t));
764 if(eomn_serv_MC_advfoc == serviceConfig.
ethservice.configuration.type)
767 eOmc_adv4jomo_coupling_t *ajc = &serviceConfig.
ethservice.configuration.data.mc.advfoc.adv4jomocoupling;
768 ajc->type = eommccoupling_traditional4x4;
770 std::memmove(&ajc->data.coupling4x4.joint2set[0], &jc_dest->joint2set[0], 4*
sizeof(uint8_t));
771 std::memmove(&ajc->data.coupling4x4.jsetcfg[0], &jc_dest->jsetcfg[0], 4*
sizeof(eOmc_jointset_configuration_t));
772 std::memmove(&ajc->data.coupling4x4.joint2motor, &jc_dest->joint2motor,
sizeof(eOmc_4x4_matrix_t));
773 std::memmove(&ajc->data.coupling4x4.motor2joint, &jc_dest->motor2joint,
sizeof(eOmc_4x4_matrix_t));
775 for(uint8_t r=0; r<4; r++)
777 for(uint8_t c=0; c<4; c++)
779 ajc->data.coupling4x4.encoder2joint4x4[r][c] = jc_dest->encoder2joint[r][c];
789bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config)
796 if(iNeedCouplingsInfo())
822 for (i = 0; i < _njoints; i++)
824 measConvFactors.angleToEncoder[i] = 1;
845 if(eomn_serv_MC_mc4 != (eOmn_serv_type_t)serviceConfig.
ethservice.configuration.type)
847 int* useMotorSpeedFbk = 0;
848 useMotorSpeedFbk =
new int[_njoints];
851 delete[] useMotorSpeedFbk;
856 if (!verifyUseMotorSpeedFbkInJointSet(useMotorSpeedFbk))
858 delete[] useMotorSpeedFbk;
861 delete[] useMotorSpeedFbk;
863 bool deadzoneIsAvailable;
866 if(!deadzoneIsAvailable)
868 updateDeadZoneWithDefaultValues();
880 bool lowLevPidisMandatory =
false;
882 if((serviceConfig.
ethservice.configuration.type == eomn_serv_MC_foc) || (serviceConfig.
ethservice.configuration.type == eomn_serv_MC_advfoc))
884 lowLevPidisMandatory =
true;
887 if(!_mcparser->
parsePids(config, _trj_pids, _trq_pids, _cur_pids, _spd_pids, lowLevPidisMandatory))
904 updatedJointsetsCfgWithControlInfo();
907 for (i = 0; i < _njoints; i++)
909 measConvFactors.newtonsToSensor[i] = 1000000.0f;
911 measConvFactors.bemf2raw[i] = measConvFactors.newtonsToSensor[i] / measConvFactors.angleToEncoder[i];
912 if (_trq_pids->
out_PidUnits == yarp::dev::PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT)
914 measConvFactors.ktau2raw[i] = measConvFactors.dutycycleToPWM[i] / measConvFactors.newtonsToSensor[i];
916 else if (_trq_pids->
out_PidUnits == yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS)
918 measConvFactors.ktau2raw[i] = 1.0 / measConvFactors.newtonsToSensor[i];
922 yError() <<
"Invalid ktau units";
return false;
927 _measureConverter =
new ControlBoardHelper(_njoints, _axisMap, measConvFactors.angleToEncoder, NULL, measConvFactors.newtonsToSensor, measConvFactors.ampsToSensor,
nullptr, measConvFactors.dutycycleToPWM , measConvFactors.bemf2raw, measConvFactors.ktau2raw);
928 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, _trj_pids->
fbk_PidUnits, _trj_pids->
out_PidUnits);
930 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, _trq_pids->
fbk_PidUnits, _trq_pids->
out_PidUnits);
931 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, _cur_pids->
fbk_PidUnits, _cur_pids->
out_PidUnits);
932 _measureConverter->set_pid_conversion_units(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, _spd_pids->
fbk_PidUnits, _spd_pids->
out_PidUnits);
945 initializeInterfaces(measConvFactors);
946 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, _trj_pids->
fbk_PidUnits, _trj_pids->
out_PidUnits);
948 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, _trq_pids->
fbk_PidUnits, _trq_pids->
out_PidUnits);
949 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, _cur_pids->
fbk_PidUnits, _cur_pids->
out_PidUnits);
950 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, _spd_pids->
fbk_PidUnits, _spd_pids->
out_PidUnits);
954 if(!saveCouplingsData())
961 yError() <<
"embObjMC " << getBoardInfo() <<
"IMPEDANCE section: error detected in parameters syntax";
968 yError() <<
"embObjMC " << getBoardInfo() <<
"LUGRE section: error detected in parameters syntax";
973 for(j=0; j<_njoints; j++)
976 _impedance_limits[j].
min_damp= 0.001;
977 _impedance_limits[j].
max_damp= 9.888;
980 _impedance_limits[j].
param_a= 0.011;
981 _impedance_limits[j].
param_b= 0.012;
982 _impedance_limits[j].
param_c= 0.013;
1003 eOmn_serv_type_t servtype =
static_cast<eOmn_serv_type_t
>(serviceConfig.
ethservice.configuration.type);
1005 if((eomn_serv_MC_foc == servtype) || (eomn_serv_MC_advfoc == servtype))
1007 std::string groupName = {};
1009 if(eomn_serv_MC_foc == servtype)
1012 eObrd_type_t brd =
static_cast<eObrd_type_t
>(serviceConfig.
ethservice.configuration.data.mc.foc_based.type);
1013 groupName = (eobrd_foc == brd) ?
"2FOC" :
"AMCBLDC";
1015 else if(eomn_serv_MC_advfoc == servtype)
1019 groupName =
"ADVFOC";
1022 if(!_mcparser->
parseFocGroup(config, _foc_based_info, groupName, _temperatureSensorsVector))
1025 for (j = 0; j < _njoints; j++)
1027 if (((_temperatureSensorsVector.at(j)->getType() !=
motor_temperature_sensor_none )) && ((_temperatureLimits[j].hardwareTemperatureLimit == 0) || (_temperatureLimits[j].warningTemperatureLimit == 0)))
1029 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...";
1035 yInfo() <<
"embObjMC " << getBoardInfo() <<
"joint " << j <<
" has motor not provided with any available type of temperature sensor. If needed update the configurations file accordingly";
1041 for (j = 0; j < _njoints; j++)
1043 _temperatureSensorsVector.at(j) = std::make_unique<eomc::TemperatureSensorNONE>();
1047 int defaultTimeout = 100;
1049 if (this->serviceConfig.
ethservice.configuration.type == eomn_serv_MC_advfoc)
1053 defaultTimeout = 300;
1066bool embObjMotionControl::verifyUseMotorSpeedFbkInJointSet(
int useMotorSpeedFbk [])
1068 for(
size_t s=0; s< _jsets.size(); s++)
1070 int numofjointsinset = _jsets[s].getNumberofJoints();
1071 if(numofjointsinset == 0 )
1073 yError() <<
"embObjMC " << getBoardInfo() <<
"Jointsset " << s <<
"hasn't joints!!! I should be never stay here!!!";
1077 int firstjointofset = _jsets[s].joints[0];
1078 for(
int j=1; j<numofjointsinset; j++)
1080 int joint = _jsets[s].joints[j];
1081 if(useMotorSpeedFbk[firstjointofset] != useMotorSpeedFbk[joint])
1083 yError() <<
"embObjMC " << getBoardInfo() <<
". Param useMotorSpeedFbk should have same value for joints belong same set. See joint " << firstjointofset <<
" and " << joint;
1088 _jsets[s].setUseSpeedFeedbackFromMotors(useMotorSpeedFbk[firstjointofset]);
1095bool embObjMotionControl::verifyTorquePidshasSameUnitTypes(yarp::dev::PidFeedbackUnitsEnum &fbk_pidunits, yarp::dev::PidOutputUnitsEnum& out_pidunits)
1097 fbk_pidunits = yarp::dev::PidFeedbackUnitsEnum::RAW_MACHINE_UNITS;
1098 out_pidunits = yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS;
1100 int firstjoint = -1;
1101 for(
int i=0; i<_njoints; i++)
1103 if(_trq_pids[i].enabled)
1113 for(
int i=firstjoint+1; i<_njoints; i++)
1115 if(_trq_pids[i].enabled)
1117 if(_trq_pids[firstjoint].fbk_PidUnits != _trq_pids[i].fbk_PidUnits ||
1118 _trq_pids[firstjoint].out_PidUnits != _trq_pids[i].out_PidUnits)
1120 yError() <<
"embObjMC " << getBoardInfo() <<
"all joints with torque enabled should have same controlunits type. Joint " << firstjoint <<
" differs from joint " << i;
1131bool embObjMotionControl::isTorqueControlEnabled(
int joint)
1133 return (_trq_pids[joint].enabled);
1136bool embObjMotionControl::isVelocityControlEnabled(
int joint)
1139 return (_trj_pids[joint].enabled);
1143void embObjMotionControl::updateDeadZoneWithDefaultValues(
void)
1145 for(
int i=0; i<_njoints; i++)
1147 switch(_jointEncs[i].
type)
1155 case eomc_enc_aksim2:
1161 case eomc_enc_absanalog:
1164 case eomc_enc_hallmotor:
1165 case eomc_enc_spichainof2:
1166 case eomc_enc_spichainof3:
1176bool embObjMotionControl::fromConfig_readServiceCfg(yarp::os::Searchable &config)
1179 if(
false == parser->
parseService(config, serviceConfig))
1181 yError() <<
"embObjMC " << getBoardInfo() <<
"cannot parse service" ;
1185 if(eomn_serv_MC_generic == serviceConfig.
ethservice.configuration.type)
1187 yError() <<
"embObjMC " << getBoardInfo() <<
"it is no longer possible use eomn_serv_MC_generic, because firmware cannot configure itself!" ;
1195 for(
int i=0; i<_njoints; i++)
1200 if(NULL == jointEncoder_ptr)
1202 _jointEncs[i].resolution = 1;
1203 _jointEncs[i].type = eomc_enc_none;
1204 _jointEncs[i].tolerance = 0;
1208 _jointEncs[i].resolution = jointEncoder_ptr->
resolution;
1209 _jointEncs[i].type = (eOmc_encoder_t)jointEncoder_ptr->
desc.type;
1210 _jointEncs[i].tolerance = jointEncoder_ptr->
tolerance;
1214 if(NULL == motorEncoder_ptr)
1216 _motorEncs[i].resolution = 1;
1217 _motorEncs[i].type = eomc_enc_none;
1218 _motorEncs[i].tolerance = 0;
1222 _motorEncs[i].resolution = motorEncoder_ptr->
resolution;
1223 _motorEncs[i].type = (eOmc_encoder_t)motorEncoder_ptr->
desc.type;
1224 _motorEncs[i].tolerance = motorEncoder_ptr->
tolerance;
1236bool embObjMotionControl::fromConfig(yarp::os::Searchable &config)
1239 _njoints = fromConfig_NumOfJoints(config);
1243 yError() <<
"embObjMC"<< getBoardInfo() <<
"fromConfig(): detected _njoints = " << _njoints;
1248 if(!alloc(_njoints))
1250 yError() <<
"embObjMC"<< getBoardInfo() <<
"fromConfig(): alloc() failed for _njoints = " << _njoints;
1258 int currentMCversion =0;
1264 yError() <<
"embObjMC" << getBoardInfo() <<
"------ ATTENTION!!!! Wrong value of <MotioncontrolVersion> parameter !!!! ---------------------------------------------------------------------------------------";
1265 yError() <<
"embObjMC" << getBoardInfo() <<
"------ This means that the configuration files of this device are not compatible with my parser, so I cannot start. ";
1266 yError() <<
"embObjMC" << getBoardInfo() <<
"------ I need version " <<
PARSER_MOTION_CONTROL_VERSION <<
", but in configuration files have version " << currentMCversion <<
".";
1267 yError() <<
"embObjMC" << getBoardInfo() <<
"------ Please update configuration files in robots-configuration repository. (see https://icub-tech-iit.github.io/documentation/icub_robot_configuration/icub_robot_configuration_index/ for more information). ";
1268 yError() <<
"embObjMC" << getBoardInfo() <<
"------ If the problem persists contact icub-support@iit.it DO NOT DO IT BY YOURSELF.";
1269 yError() <<
"embObjMC" << getBoardInfo() <<
"----------------------------------------------------------------------------------------------------------------------------------------------------------------";
1275 yTrace() << config.toString().c_str();
1280 if(
false == fromConfig_readServiceCfg(config))
1296 if(
false == fromConfig_Step2(config))
1308bool embObjMotionControl::init()
1310 eOprotID32_t protid = 0;
1316 for(
int logico=0; logico< _njoints; logico++)
1318 int fisico = _axisMap[logico];
1319 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, fisico, eoprot_tag_mc_joint_cmmnds_controlmode);
1320 eOenum08_t controlMode = eomc_controlmode_cmd_idle;
1324 yError() <<
"embObjMotionControl::init() had an error while setting eomc_controlmode_cmd_idle in "<< getBoardInfo();
1330 SystemClock::delaySystem(0.010);
1337 vector<eOprotID32_t> id32v(0);
1338 for(
int n=0;
n<_njoints;
n++)
1340 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint,
n, eoprot_tag_mc_joint_status_core);
1341 id32v.push_back(protid);
1342 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint,
n, eoprot_tag_mc_joint_status_addinfo_multienc);
1343 id32v.push_back(protid);
1344 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor,
n, eoprot_tag_mc_motor_status);
1345 id32v.push_back(protid);
1348 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
1350 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, 0, eoprot_tag_mc_joint_status_debug);
1351 id32v.push_back(protid);
1352 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, 1, eoprot_tag_mc_joint_status_debug);
1353 id32v.push_back(protid);
1359 yError() <<
"embObjMotionControl::init() fails to add its variables to regulars in "<< getBoardInfo() <<
": cannot proceed any further";
1366 yDebug() <<
"embObjMotionControl::init() added" << id32v.size() <<
"regular rops to "<< getBoardInfo();
1368 for(
unsigned int r=0; r<id32v.size(); r++)
1370 uint32_t id32 = id32v.at(r);
1371 eoprot_ID2information(id32, nvinfo,
sizeof(nvinfo));
1372 yDebug() <<
"\t it added regular rop for" << nvinfo;
1377 SystemClock::delaySystem(0.005);
1384 for(
int logico=0; logico< _njoints; logico++)
1386 int fisico = _axisMap[logico];
1387 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, fisico, eoprot_tag_mc_joint_config);
1389 eOmc_joint_config_t jconfig = {0};
1390 memset(&jconfig, 0,
sizeof(eOmc_joint_config_t));
1392 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_POSITION,_trj_pids[logico].pid, fisico);
1396 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_TORQUE, _trq_pids[logico].pid, fisico);
1400 jconfig.impedance.damping = (eOmeas_damping_t) _measureConverter->impN2S(_impedance_params[logico].damping, fisico);
1401 jconfig.impedance.stiffness = (eOmeas_stiffness_t) _measureConverter->impN2S(_impedance_params[logico].stiffness, fisico);
1402 jconfig.impedance.offset = 0;
1404 _cacheImpedance[logico].stiffness = jconfig.impedance.stiffness;
1405 _cacheImpedance[logico].damping = jconfig.impedance.damping;
1406 _cacheImpedance[logico].offset = jconfig.impedance.offset;
1408 jconfig.userlimits.max = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posMax, fisico));
1409 jconfig.userlimits.min = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posMin, fisico));
1411 jconfig.hardwarelimits.max = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posHwMax, fisico));
1412 jconfig.hardwarelimits.min = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_jointsLimits[logico].posHwMin, fisico));
1415 jconfig.maxvelocityofjoint =
S_32(_measureConverter->posA2E(_jointsLimits[logico].velMax, fisico));
1416 jconfig.velocitysetpointtimeout = (eOmeas_time_t)
U_16(_timeouts[logico].velocity_ref);
1417 jconfig.currentsetpointtimeout = (eOmeas_time_t)
U_16(_timeouts[logico].current_ref);
1418 jconfig.openloopsetpointtimeout = (eOmeas_time_t)
U_16(_timeouts[logico].pwm_ref);
1419 jconfig.torquesetpointtimeout = (eOmeas_time_t)
U_16(_timeouts[logico].torque_ref);
1420 jconfig.torquefeedbacktimeout = (eOmeas_time_t)
U_16(_timeouts[logico].torque_fbk);
1422 jconfig.jntEncoderResolution = _jointEncs[logico].resolution;
1423 jconfig.jntEncoderType = _jointEncs[logico].type;
1424 jconfig.jntEncTolerance = _jointEncs[logico].tolerance;
1426 jconfig.motor_params.bemf_value = _measureConverter->bemf_user2raw(_trq_pids[logico].kbemf, fisico);
1427 jconfig.motor_params.bemf_scale = 0;
1428 jconfig.motor_params.ktau_value = _measureConverter->ktau_user2raw(_trq_pids[logico].ktau, fisico);
1429 jconfig.motor_params.ktau_scale = 0;
1430 jconfig.motor_params.friction.viscous_pos_val = _measureConverter->viscousPos_user2raw(_trq_pids[logico].viscousPos, fisico);
1431 jconfig.motor_params.friction.viscous_neg_val = _measureConverter->viscousNeg_user2raw(_trq_pids[logico].viscousNeg, fisico);
1432 jconfig.motor_params.friction.coulomb_pos_val = _measureConverter->coulombPos_user2raw(_trq_pids[logico].coulombPos, fisico);
1433 jconfig.motor_params.friction.coulomb_neg_val = _measureConverter->coulombNeg_user2raw(_trq_pids[logico].coulombNeg, fisico);
1434 jconfig.motor_params.friction.velocityThres_val = _measureConverter->velocityThres_user2raw(_trq_pids[logico].velocityThres, fisico);
1436 jconfig.gearbox_E2J = _gearbox_E2J[logico];
1438 jconfig.deadzone = _measureConverter->posA2E(_deadzone[logico], fisico);
1440 jconfig.tcfiltertype=_trq_pids[logico].
filterType;
1442 jconfig.kalman_params.enabled = _kalman_params[logico].enabled;
1443 for(
int i=0; i<_kalman_params[logico].x0.size(); i++) jconfig.kalman_params.x0[i] = _kalman_params[logico].x0.at(i);
1444 for(
int i=0; i<_kalman_params[logico].Q.size(); i++) jconfig.kalman_params.Q[i] = _kalman_params[logico].Q.at(i);
1445 jconfig.kalman_params.R = _kalman_params[logico].R;
1446 jconfig.kalman_params.P0 = _kalman_params[logico].P0;
1450 yError() <<
"FATAL: embObjMotionControl::init() had an error while calling setcheckRemoteValue() for joint config fisico #" << fisico <<
"in "<< getBoardInfo();
1457 yDebug() <<
"embObjMotionControl::init() correctly configured joint config fisico #" << fisico <<
"in "<< getBoardInfo();
1468 for(
int logico=0; logico<_njoints; logico++)
1470 int fisico = _axisMap[logico];
1472 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, fisico, eoprot_tag_mc_motor_config);
1473 eOmc_motor_config_t motor_cfg = {0};
1474 motor_cfg.maxvelocityofmotor = 0;
1475 motor_cfg.currentLimits.nominalCurrent = _currentLimits[logico].nominalCurrent;
1476 motor_cfg.currentLimits.overloadCurrent = _currentLimits[logico].overloadCurrent;
1477 motor_cfg.currentLimits.peakCurrent = _currentLimits[logico].peakCurrent;
1478 motor_cfg.gearbox_M2J = _gearbox_M2J[logico];
1479 motor_cfg.rotorEncoderResolution = _motorEncs[logico].resolution;
1480 motor_cfg.rotEncTolerance = _motorEncs[logico].tolerance;
1481 motor_cfg.hasHallSensor = _foc_based_info[logico].
hasHallSensor;
1483 motor_cfg.hasTempSensor = _foc_based_info[logico].
hasTempSensor;
1486 motor_cfg.verbose = _foc_based_info[logico].
verbose;
1487 motor_cfg.motorPoles = _foc_based_info[logico].
motorPoles;
1489 motor_cfg.rotorEncoderType = _motorEncs[logico].type;
1490 motor_cfg.pwmLimit =_rotorsLimits[logico].pwmMax;
1491 motor_cfg.temperatureLimit = (eOmeas_temperature_t)
S_16(_temperatureSensorsVector.at(logico)->convertTempCelsiusToRaw(_temperatureLimits.at(logico).hardwareTemperatureLimit));
1492 motor_cfg.limitsofrotor.max = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMax, fisico ));
1493 motor_cfg.limitsofrotor.min = (eOmeas_position_t)
S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMin, fisico ));
1495 motor_cfg.LuGre_params.Km = _lugre_params[logico].Km;
1496 motor_cfg.LuGre_params.Kw = _lugre_params[logico].Kw;
1497 motor_cfg.LuGre_params.S0 = _lugre_params[logico].S0;
1498 motor_cfg.LuGre_params.S1 = _lugre_params[logico].S1;
1499 motor_cfg.LuGre_params.Vth = _lugre_params[logico].Vth;
1500 motor_cfg.LuGre_params.Fc_pos = _lugre_params[logico].Fc_pos;
1501 motor_cfg.LuGre_params.Fc_neg = _lugre_params[logico].Fc_neg;
1502 motor_cfg.LuGre_params.Fs_pos = _lugre_params[logico].Fs_pos;
1503 motor_cfg.LuGre_params.Fs_neg = _lugre_params[logico].Fs_neg;
1506 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_CURRENT, _cur_pids[logico].pid, fisico);
1509 tmp = _measureConverter->convert_pid_to_machine(yarp::dev::VOCAB_PIDTYPE_VELOCITY, _spd_pids[logico].pid, fisico);
1514 yError() <<
"FATAL: embObjMotionControl::init() had an error while calling setcheckRemoteValue() for motor config fisico #" << fisico <<
"in "<< getBoardInfo();
1521 yDebug() <<
"embObjMotionControl::init() correctly configured motor config fisico #" << fisico <<
"in "<< getBoardInfo();
1530 protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_controller, 0, eoprot_tag_mc_controller_config);
1532 eOmc_controller_config_t controller_cfg = {0};
1533 memset(&controller_cfg, 0,
sizeof(eOmc_controller_config_t));
1534 controller_cfg.durationofctrlloop = (uint32_t)bdata.
settings.
txconfig.cycletime;
1539 yError() <<
"FATAL: embObjMotionControl::init() had an error while calling setcheckRemoteValue() for the controller " <<
"in "<< getBoardInfo();
1546 yDebug() <<
"embObjMotionControl::init() correctly configured controller config " <<
"in "<< getBoardInfo();
1553 const char* tag = eoprot_TAG2string(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_addinfo_multienc);
1555 _rawValuesMetadataMap.insert({{tag,
rawValuesKeyMetadata({}, {}, _njoints * eOmc_joint_multienc_maxnum)}});
1556 for (
auto &[k, v] : _rawValuesMetadataMap)
1558 std::string auxstring =
"";
1560 for (
int i = 0; i < _njoints; i++)
1565 v.axesNames.push_back(auxstring);
1566 v.rawValueNames.insert(v.rawValueNames.end(),
1567 {auxstring+
"_primary_encoder_raw_value",
1568 auxstring+
"_secondary_encoder_raw_value",
1569 auxstring+
"_primary_encoder_diagnostic"}
1575 yTrace() <<
"embObjMotionControl::init(): correctly instantiated for " << getBoardInfo();
1583 yTrace() <<
" embObjMotionControl::close()";
1585 ImplementControlMode::uninitialize();
1586 ImplementEncodersTimed::uninitialize();
1587 ImplementMotorEncoders::uninitialize();
1588 ImplementPositionControl::uninitialize();
1589 ImplementVelocityControl::uninitialize();
1590 ImplementPidControl::uninitialize();
1591 ImplementControlCalibration::uninitialize();
1592 ImplementAmplifierControl::uninitialize();
1593 ImplementImpedanceControl::uninitialize();
1594 ImplementControlLimits::uninitialize();
1595 ImplementTorqueControl::uninitialize();
1596 ImplementPositionDirect::uninitialize();
1597 ImplementInteractionMode::uninitialize();
1598 ImplementRemoteVariables::uninitialize();
1599 ImplementAxisInfo::uninitialize();
1600 ImplementCurrentControl::uninitialize();
1601 ImplementPWMControl::uninitialize();
1602 ImplementJointFault::uninitialize();
1604 if (_measureConverter) {
delete _measureConverter; _measureConverter=0;}
1607 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
1610 for(
size_t i=0; i<mcdiagnostics.
ports.size(); i++)
1612 mcdiagnostics.
ports[i]->close();
1613 delete mcdiagnostics.
ports[i];
1615 mcdiagnostics.
ports.clear();
1617 mcdiagnostics.
config.mode = eomn_serv_diagn_mode_NONE;
1618 mcdiagnostics.
config.par16 = 0;
1621 delete event_downsampler;
1635void embObjMotionControl::cleanup(
void)
1637 if(ethManager == NULL)
return;
1656 size_t joint = eoprot_ID2index(id32);
1657 eOprotEntity_t ent = eoprot_ID2entity(id32);
1658 eOprotTag_t tag = eoprot_ID2tag(id32);
1672 std::lock_guard<std::mutex> lck(_mutex);
1673 _encodersStamp[joint] = timestamp;
1677 if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.
config.mode)
1679 char str[128] =
"boh";
1681 eoprot_ID2information(id32, str,
sizeof(str));
1683 if((eoprot_entity_mc_joint == ent) && (eoprot_tag_mc_joint_status_debug == tag) && (joint < mcdiagnostics.
ports.size()))
1686 eOprotID32_t id32sc = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core);
1687 eOmc_joint_status_core_t jcore = {};
1691 int32_t *debug32 =
reinterpret_cast<int32_t*
>(rxdata);
1694 Bottle& output = mcdiagnostics.
ports[joint]->prepare();
1697 output.addString(
"[yt, amo, reg, pos]");
1698 output.addFloat64(timestamp);
1699 output.addInt32(debug32[0]);
1700 output.addInt32(debug32[1]);
1701 output.addInt32(jcore.measures.meas_position);
1702 mcdiagnostics.
ports[joint]->write();
1706 if((eoprot_entity_mc_motor == ent) && (eoprot_tag_mc_motor_status == tag))
1711 uint8_t motor = eoprot_ID2index(id32);
1715 eOmc_motor_status_t *mc_motor_status =
reinterpret_cast<eOmc_motor_status_t*
>(rxdata);
1717 if((
double)mc_motor_status->basic.mot_temperature < 0 )
1719 if(! _temperatureSensorErrorWatchdog.at(motor).isStarted())
1721 yWarning() << getBoardInfo() <<
"At time" << (yarp::os::Time::now() - _temperatureSensorErrorWatchdog.at(motor).getAbsoluteTime()) <<
"In motor" << motor <<
"cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable";
1722 _temperatureSensorErrorWatchdog.at(motor).start();
1726 _temperatureSensorErrorWatchdog.at(motor).increment();
1727 if( _temperatureSensorErrorWatchdog.at(motor).isExpired())
1729 yWarning()<< getBoardInfo() <<
"Motor" << motor <<
"failed to read" << _temperatureSensorErrorWatchdog.at(motor).getCount() <<
"temperature readings for" << yarp::os::Time::now() - _temperatureSensorErrorWatchdog.at(motor).getStartTime() <<
"seconds";
1730 _temperatureSensorErrorWatchdog.at(motor).start();
1737 double delta_tmp = 0;
1738 double tmp = _temperatureSensorsVector.at(motor)->convertRawToTempCelsius((
double)mc_motor_status->basic.mot_temperature);
1742 if(!_temperatureSpikesFilter.at(motor).isStarted())
1744 _temperatureSpikesFilter.at(motor).start(tmp);
1749 delta_tmp = std::abs(tmp - _temperatureSpikesFilter.at(motor).getPrevTemperature());
1752 if(delta_tmp > _temperatureSpikesFilter.at(motor).getTemperatureThreshold())
1758 _temperatureSpikesFilter.at(motor).updatePrevTemperature(tmp);
1761 if(tmp > _temperatureLimits[motor].warningTemperatureLimit)
1763 if(! _temperatureExceededLimitWatchdog.at(motor).isStarted())
1765 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";
1766 _temperatureExceededLimitWatchdog.at(motor).start();
1770 if(_temperatureExceededLimitWatchdog.at(motor).isExpired())
1772 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";
1773 _temperatureExceededLimitWatchdog.at(motor).start();
1775 _temperatureExceededLimitWatchdog.at(motor).increment();
1780 _temperatureExceededLimitWatchdog.at(motor).clear();
1804 encoderTypeName.clear();
1806 if ((jomoId >= 0) && (jomoId < _njoints))
1810 case eomc_pos_atjoint:
1811 encoderTypeName = eomc_encoder2string(_jointEncs[jomoId].
type, eobool_true);
1813 case eomc_pos_atmotor:
1814 encoderTypeName = eomc_encoder2string(_motorEncs[jomoId].
type, eobool_true);
1816 case eomc_pos_unknown:
1817 encoderTypeName =
"UNKNOWN";
1821 encoderTypeName =
"NONE";
1828 encoderTypeName =
"ERROR";
1838 case VOCAB_PIDTYPE_POSITION:
1839 helper_setPosPidRaw(j,pid);
1841 case VOCAB_PIDTYPE_VELOCITY:
1843 helper_setSpdPidRaw(j, pid);
1845 case VOCAB_PIDTYPE_TORQUE:
1846 helper_setTrqPidRaw(j, pid);
1848 case VOCAB_PIDTYPE_CURRENT:
1849 helper_setCurPidRaw(j,pid);
1852 yError()<<
"Invalid pidtype:"<<pidtype;
1862 case VOCAB_PIDTYPE_POSITION:
1863 helper_getPosPidRaw(axis,pid);
1865 case VOCAB_PIDTYPE_VELOCITY:
1867 helper_getSpdPidRaw(axis, pid);
1869 case VOCAB_PIDTYPE_TORQUE:
1870 helper_getTrqPidRaw(axis, pid);
1872 case VOCAB_PIDTYPE_CURRENT:
1873 helper_getCurPidRaw(axis,pid);
1876 yError()<<
"Invalid pidtype:"<<pidtype;
1882bool embObjMotionControl::helper_setPosPidRaw(
int j,
const Pid &pid)
1884 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtrajectory);
1893 yError() <<
"while setting position PIDs for " << getBoardInfo() <<
" joint " << j;
1903 for(
int j=0; j< _njoints; j++)
1918 for(
int j=0, index=0; j< _njoints; j++, index++)
1940 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
1941 eOmc_joint_status_core_t jcore = {0};
1948 case VOCAB_PIDTYPE_POSITION:
1950 if((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
1951 (eomc_controlmode_openloop == jcore.modes.controlmodestatus) ||
1952 (eomc_controlmode_current == jcore.modes.controlmodestatus))
1955 *err = (double) jcore.ofpid.generic.error1;
1966 case VOCAB_PIDTYPE_TORQUE:
1968 if ((eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus) &&
1969 (eomc_controlmode_position == jcore.modes.controlmodestatus))
1971 *err = (double) jcore.ofpid.complpos.errtrq;
1974 if(eomc_controlmode_torque == jcore.modes.controlmodestatus)
1976 *err = (double) jcore.ofpid.torque.errtrq;
1980 case VOCAB_PIDTYPE_VELOCITY:
1986 case VOCAB_PIDTYPE_CURRENT:
1994 yError()<<
"Invalid pidtype:"<<pidtype;
2004 for(
int j=0; j< _njoints; j++)
2011bool embObjMotionControl::helper_getPosPidRaw(
int j, Pid *pid)
2013 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtrajectory);
2016 eOmc_PID_t eoPID = {0};
2019#ifdef NETWORK_PERFORMANCE_BENCHMARK
2020 double start = yarp::os::Time::now();
2023 bool ret = askRemoteValue(protid, &eoPID, size);
2025#ifdef NETWORK_PERFORMANCE_BENCHMARK
2026 double end = yarp::os::Time::now();
2027 m_responseTimingVerifier.tick(end-start, start);
2040bool embObjMotionControl::helper_getPosPidsRaw(Pid *pid)
2042 std::vector<eOmc_PID_t> eoPIDList(_njoints);
2043 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_pidtrajectory, eoPIDList);
2046 yError() <<
"failed helper_getPosPidsRaw for" << getBoardInfo();
2050 for(
int j=0; j<_njoints; j++)
2064 case VOCAB_PIDTYPE_POSITION:
2065 helper_getPosPidsRaw(pids);
2070 case VOCAB_PIDTYPE_TORQUE:
2071 helper_getTrqPidsRaw(pids);
2073 case VOCAB_PIDTYPE_CURRENT:
2074 helper_getCurPidsRaw(pids);
2076 case VOCAB_PIDTYPE_VELOCITY:
2077 helper_getSpdPidsRaw(pids);
2080 yError()<<
"Invalid pidtype:"<<pidtype;
2088 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2089 eOmc_joint_status_core_t jcore = {0};
2096 case VOCAB_PIDTYPE_POSITION:
2098 if((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
2099 (eomc_controlmode_openloop == jcore.modes.controlmodestatus) ||
2100 (eomc_controlmode_current == jcore.modes.controlmodestatus))
2101 { *ref = 0; yError() <<
"Invalid getPidReferenceRaw() request for current control mode";
return true; }
2102 *ref = (double) jcore.ofpid.generic.reference1;
2111 case VOCAB_PIDTYPE_TORQUE:
2117 case VOCAB_PIDTYPE_CURRENT:
2123 case VOCAB_PIDTYPE_VELOCITY:
2132 yError()<<
"Invalid pidtype:"<<pidtype;
2145 for(
int j=0; j< _njoints; j++)
2190 if( (mode != VOCAB_CM_VELOCITY) &&
2191 (mode != VOCAB_CM_MIXED) &&
2192 (mode != VOCAB_CM_IMPEDANCE_VEL) &&
2193 (mode != VOCAB_CM_IDLE))
2197 yError() <<
"velocityMoveRaw: skipping command because " << getBoardInfo() <<
" joint " << j <<
" is not in VOCAB_CM_VELOCITY mode";
2202 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
2204 _ref_command_speeds[j] = sp ;
2206 eOmc_setpoint_t setpoint;
2207 setpoint.type = eomc_setpoint_velocity;
2208 setpoint.to.velocity.value = (eOmeas_velocity_t)
S_32(_ref_command_speeds[j]);
2209 setpoint.to.velocity.withacceleration = (eOmeas_acceleration_t)
S_32(_ref_accs[j]);
2214 yError() <<
"while setting velocity mode";
2223 eOmc_setpoint_t setpoint;
2225 setpoint.type = eomc_setpoint_velocity;
2227 for(
int j=0; j<_njoints; j++)
2242 yTrace() <<
"setCalibrationParametersRaw for " << getBoardInfo() <<
"joint" << j;
2244 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_calibration);
2245 eOmc_calibrator_t calib;
2246 memset(&calib, 0x00,
sizeof(calib));
2247 calib.type = params.type;
2252 case eomc_calibration_type0_hard_stops:
2253 calib.params.type0.pwmlimit = (int16_t)
S_16(params.param1);
2254 calib.params.type0.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2255 calib.params.type0.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2259 case eomc_calibration_type1_abs_sens_analog:
2260 calib.params.type1.position = (int16_t)
S_16(params.param1);
2261 calib.params.type1.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2262 calib.params.type1.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2266 case eomc_calibration_type2_hard_stops_diff:
2267 calib.params.type2.pwmlimit = (int16_t)
S_16(params.param1);
2268 calib.params.type2.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2269 calib.params.type2.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2273 case eomc_calibration_type3_abs_sens_digital:
2274 calib.params.type3.position = (int16_t)
S_16(params.param1);
2275 calib.params.type3.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2276 calib.params.type3.offset = (int32_t)
S_32(params.param3);
2277 calib.params.type3.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2281 case eomc_calibration_type4_abs_and_incremental:
2282 calib.params.type4.position = (int16_t)
S_16(params.param1);
2283 calib.params.type4.velocity = (eOmeas_velocity_t)
S_32(params.param2);
2284 calib.params.type4.maxencoder = (int32_t)
S_32(params.param3);
2285 calib.params.type4.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2289 case eomc_calibration_type5_hard_stops:
2290 calib.params.type5.pwmlimit = (int32_t)
S_32(params.param1);
2291 calib.params.type5.final_pos = (int32_t)
S_32(params.param2);
2292 calib.params.type5.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2296 case eomc_calibration_type6_mais:
2297 calib.params.type6.position = (int32_t)
S_32(params.param1);
2298 calib.params.type6.velocity = (int32_t)
S_32(params.param2);
2299 calib.params.type6.current = (int32_t)
S_32(params.param3);
2300 calib.params.type6.vmin = (int32_t)
S_32(params.param4);
2301 calib.params.type6.vmax = (int32_t)
S_32(params.param5);
2302 calib.params.type6.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2306 case eomc_calibration_type7_hall_sensor:
2307 calib.params.type7.position = (int32_t)
S_32(params.param1);
2308 calib.params.type7.velocity = (int32_t)
S_32(params.param2);
2310 calib.params.type7.vmin = (int32_t)
S_32(params.param4);
2311 calib.params.type7.vmax = (int32_t)
S_32(params.param5);
2312 calib.params.type7.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2316 case eomc_calibration_type8_tripod_internal_hard_stop:
2317 calib.params.type8.pwmlimit = (int32_t)
S_32(params.param1);
2318 calib.params.type8.max_delta = (int32_t)
S_32(params.param2);
2319 calib.params.type8.calibrationZero = (int32_t)
S_32(params.paramZero );
2322 case eomc_calibration_type9_tripod_external_hard_stop:
2323 calib.params.type9.pwmlimit = (int32_t)
S_32(params.param1);
2324 calib.params.type9.max_delta = (int32_t)
S_32(params.param2);
2325 calib.params.type9.calibrationZero = (int32_t)
S_32(params.paramZero );
2328 case eomc_calibration_type10_abs_hard_stop:
2329 calib.params.type10.pwmlimit = (int32_t)
S_32(params.param1);
2330 calib.params.type10.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2333 case eomc_calibration_type11_cer_hands:
2334 calib.params.type11.offset0 = (int32_t)
S_32(params.param1);
2335 calib.params.type11.offset1 = (int32_t)
S_32(params.param2);
2336 calib.params.type11.offset2 = (int32_t)
S_32(params.param3);
2337 calib.params.type11.cable_range = (int32_t)
S_32(params.param4);
2338 calib.params.type11.pwm = (int32_t)
S_32(params.param5);
2342 case eomc_calibration_type12_absolute_sensor:
2343 calib.params.type12.rawValueAtZeroPos = (int32_t)
S_32(params.param1);
2344 calib.params.type12.calibrationDelta = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2347 case eomc_calibration_type13_cer_hands_2:
2348 calib.params.type13.rawValueAtZeroPos0 = (int32_t)
S_32(params.param1);
2349 calib.params.type13.rawValueAtZeroPos1 = (int32_t)
S_32(params.param2);
2350 calib.params.type13.rawValueAtZeroPos2 = (int32_t)
S_32(params.param3);
2351 calib.params.type13.rawValueAtZeroPos3 = (int32_t)
S_32(params.param4);
2354 case eomc_calibration_type14_qenc_hard_stop_and_fap:
2355 calib.params.type14.pwmlimit = (int32_t)
S_32(params.param1);
2356 calib.params.type14.final_pos = (int32_t)
S_32(params.param2);
2357 calib.params.type14.invertdirection = (uint8_t)
U_32(params.param3);
2358 calib.params.type14.rotation = (int32_t)
S_32(params.param4);
2360 if (calib.params.type14.invertdirection != 0 && calib.params.type14.invertdirection != 1)
2362 yError() <<
"Error in param3 of calibartion type 14 for joint " << j <<
"Admitted values are: 0=FALSE and 1=TRUE";
2367 if(!checkCalib14RotationParam(calib.params.type14.rotation))
2369 yError() <<
"Error in param4 of calibartion type 14 for joint " << j <<
"Admitted values are: 0, 32768, 16384, -16384 [0, 180, 90, -90] in iCubDegree";
2372 calib.params.type14.offset = (int32_t)
S_32(params.param5);
2373 calib.params.type14.calibrationZero = (int32_t)
S_32(_measureConverter->posA2E(params.paramZero, j));
2378 yError() <<
"Calibration type unknown!! (embObjMotionControl)\n";
2385 yError() <<
"while setting velocity mode";
2389 _calibrated[j] =
true;
2394bool embObjMotionControl::checkCalib14RotationParam(int32_t calib_param4)
2396 eOmc_calib14_ROT_t urotation = eomc_int2calib14_ROT(calib_param4);
2398 if (urotation == eOmc_calib14_ROT_zero ||
2399 urotation == eOmc_calib14_ROT_plus180 ||
2400 urotation == eOmc_calib14_ROT_plus090 ||
2401 urotation == eOmc_calib14_ROT_minus090)
2411 yTrace() <<
"calibrateRaw for" << getBoardInfo() <<
"joint" << j;
2429 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_calibration);
2430 eOmc_calibrator_t calib;
2431 memset(&calib, 0x00,
sizeof(calib));
2437 case eomc_calibration_type0_hard_stops:
2438 calib.params.type0.pwmlimit = (int16_t)
S_16(p1);
2439 calib.params.type0.velocity = (eOmeas_velocity_t)
S_32(p2);
2443 case eomc_calibration_type1_abs_sens_analog:
2444 calib.params.type1.position = (int16_t)
S_16(p1);
2445 calib.params.type1.velocity = (eOmeas_velocity_t)
S_32(p2);
2449 case eomc_calibration_type2_hard_stops_diff:
2450 calib.params.type2.pwmlimit = (int16_t)
S_16(p1);
2451 calib.params.type2.velocity = (eOmeas_velocity_t)
S_32(p2);
2455 case eomc_calibration_type3_abs_sens_digital:
2456 calib.params.type3.position = (int16_t)
S_16(p1);
2457 calib.params.type3.velocity = (eOmeas_velocity_t)
S_32(p2);
2458 calib.params.type3.offset = (int32_t)
S_32(p3);
2462 case eomc_calibration_type4_abs_and_incremental:
2463 calib.params.type4.position = (int16_t)
S_16(p1);
2464 calib.params.type4.velocity = (eOmeas_velocity_t)
S_32(p2);
2465 calib.params.type4.maxencoder = (int32_t)
S_32(p3);
2469 yError () <<
"Calibration type unknown!! (embObjMotionControl)\n";
2476 yError() <<
"while setting velocity mode";
2480 _calibrated[j ] =
true;
2488 bool result =
false;
2489 eOmc_joint_status_core_t jcore = {0};
2490 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_core);
2493 yError () <<
"Failure of getLocalValue() inside embObjMotionControl::calibrationDoneRaw(axis=" << axis <<
") for " << getBoardInfo();
2497 eOmc_controlmode_t
type = (eOmc_controlmode_t) jcore.modes.controlmodestatus;
2501 if (eomc_controlmode_idle ==
type)
2505 else if (eomc_controlmode_calib ==
type)
2509 else if (eomc_controlmode_hwFault ==
type)
2511 yError(
"unable to complete calibration: joint %d in 'hw_fault status' inside calibrationDoneRaw() function", axis);
2514 else if (eomc_controlmode_notConfigured ==
type)
2516 yError(
"unable to complete calibration: joint %d in 'not_configured' status inside calibrationDoneRaw() function", axis);
2519 else if (eomc_controlmode_unknownError ==
type)
2521 yError(
"unable to complete calibration: joint %d in 'unknownError' status inside calibrationDoneRaw() function", axis);
2524 else if (eomc_controlmode_configured ==
type)
2526 yError(
"unable to complete calibration: joint %d in 'configured' status inside calibrationDoneRaw() function", axis);
2551 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.";
2553 _last_position_move_time[j] = yarp::os::Time::now();
2557 if( (mode != VOCAB_CM_POSITION) &&
2558 (mode != VOCAB_CM_MIXED) &&
2559 (mode != VOCAB_CM_IMPEDANCE_POS) &&
2560 (mode != VOCAB_CM_IDLE))
2564 yError() <<
"positionMoveRaw: skipping command because " << getBoardInfo() <<
" joint " << j <<
" is not in VOCAB_CM_POSITION mode";
2569 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
2570 _ref_command_positions[j] = ref;
2572 eOmc_setpoint_t setpoint;
2574 setpoint.type = (eOenum08_t) eomc_setpoint_position;
2575 setpoint.to.position.value = (eOmeas_position_t)
S_32(_ref_command_positions[j]);
2576 setpoint.to.position.withvelocity = (eOmeas_velocity_t)
S_32(_ref_speeds[j]);
2585 for(
int j=0, index=0; j< _njoints; j++, index++)
2605 eObool_t ismotiondone = eobool_false;
2608 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core_modes_ismotiondone);
2609 if(
false == askRemoteValue(id32, &ismotiondone, size))
2611 yError () <<
"Failure of askRemoteValue() inside embObjMotionControl::checkMotionDoneRaw(j=" << j <<
") for " << getBoardInfo();
2616 *flag = ismotiondone;
2623 std::vector <eObool_t> ismotiondoneList(_njoints);
2624 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_core_modes_ismotiondone, ismotiondoneList);
2627 yError () <<
"Failure of askRemoteValues() inside embObjMotionControl::checkMotionDoneRaw for all joints of" << getBoardInfo();
2631 for(
int j=0; j<_njoints; j++)
2633 *flag &= ismotiondoneList[j];
2643 _ref_speeds[index] = sp;
2651 for(
int j=0, index=0; j< _njoints; j++, index++)
2653 _ref_speeds[index] = spds[index];
2665 _ref_accs[j ] = 1e6;
2667 else if (
acc < -1e6)
2669 _ref_accs[j ] = -1e6;
2673 _ref_accs[j ] =
acc;
2683 for(
int j=0, index=0; j< _njoints; j++, index++)
2687 _ref_accs[index] = 1e6;
2689 else if (accs[j] < -1e6)
2691 _ref_accs[index] = -1e6;
2695 _ref_accs[index] = accs[j];
2703 if (j<0 || j>_njoints)
return false;
2704#if ASK_REFERENCE_TO_FIRMWARE
2705 *spd = _ref_speeds[j];
2708 *spd = _ref_speeds[j];
2715 memcpy(spds, _ref_speeds,
sizeof(
double) * _njoints);
2721 *
acc = _ref_accs[j];
2727 memcpy(accs, _ref_accs,
sizeof(
double) * _njoints);
2733 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_stoptrajectory);
2735 eObool_t stop = eobool_true;
2743 for(
int j=0; j< _njoints; j++)
2758 for(
int j=0; j<n_joint; j++)
2768 for(
int j=0; j<n_joint; j++)
2779 for(
int j=0; j<n_joint; j++)
2781 if(joints[j] >= _njoints)
2783 yError() << getBoardInfo() <<
":checkMotionDoneRaw required for not existing joint ( " << joints[j] <<
")";
2789 std::vector <eObool_t> ismotiondoneList(_njoints);
2790 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_core_modes_ismotiondone, ismotiondoneList);
2793 yError () << getBoardInfo() <<
"Failure of askRemoteValues() inside embObjMotionControl::checkMotionDoneRaw for a group of joint"; getBoardInfo();
2798 bool tot_val =
true;
2799 for(
int j=0; j<n_joint; j++)
2801 tot_val &= ismotiondoneList[joints[j]];
2812 for(
int j=0; j<n_joint; j++)
2822 for(
int j=0; j<n_joint; j++)
2832 for(
int j=0; j<n_joint; j++)
2842 for(
int j=0; j<n_joint; j++)
2852 for(
int j=0; j<n_joint; j++)
2854 ret = ret &&
stopRaw(joints[j]);
2865 eOmc_joint_status_core_t jcore = {0};
2866 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
2870 eOmc_controlmode_t
type = (eOmc_controlmode_t) jcore.modes.controlmodestatus;
2880 for(
int j=0; j< _njoints; j++)
2890 for(
int j=0; j< n_joint; j++)
2905 eOenum08_t controlmodecommand = 0;
2907 if((_mode == VOCAB_CM_TORQUE) && (_trq_pids[j].enabled ==
false))
2909 yError()<<
"Torque control is disabled. Check your configuration parameters";
2915 yError() <<
"SetControlMode: received unknown control mode for " << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
2919 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_controlmode);
2922 yError() <<
"setControlModeRaw failed for " << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
2927 ret = checkRemoteControlModeStatus(j, _mode);
2931 yError() <<
"In embObjMotionControl::setControlModeRaw(j=" << j <<
", mode=" << yarp::os::Vocab32::decode(_mode).c_str() <<
") for " << getBoardInfo() <<
" has failed checkRemoteControlModeStatus()";
2941 eOenum08_t controlmodecommand = 0;
2944 for(
int i=0; i<n_joint; i++)
2946 if ((modes[i] == VOCAB_CM_TORQUE) && (_trq_pids[i].enabled ==
false)) {yError()<<
"Torque control is disabled. Check your configuration parameters";
continue;}
2950 yError() <<
"SetControlModesRaw(): received unknown control mode for " << getBoardInfo() <<
" joint " << joints[i] <<
" mode " << Vocab32::decode(modes[i]);
2955 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joints[i], eoprot_tag_mc_joint_cmmnds_controlmode);
2958 yError() <<
"setControlModesRaw() could not send set<cmmnds_controlmode> for " << getBoardInfo() <<
" joint " << joints[i] <<
" mode " << Vocab32::decode(modes[i]);
2963 bool tmpresult = checkRemoteControlModeStatus(joints[i], modes[i]);
2964 if(
false == tmpresult)
2966 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]);
2969 ret = ret && tmpresult;
2979 eOenum08_t controlmodecommand = 0;
2981 for(
int i=0; i<_njoints; i++)
2984 if ((modes[i] == VOCAB_CM_TORQUE) && (_trq_pids[i].enabled ==
false))
2986 yError()<<
"Torque control is disabled. Check your configuration parameters";
2992 yError() <<
"SetControlMode: received unknown control mode for" << getBoardInfo() <<
" joint " << i <<
" mode " << Vocab32::decode(modes[i]);
2996 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, i, eoprot_tag_mc_joint_cmmnds_controlmode);
2999 yError() <<
"setControlModesRaw failed for " << getBoardInfo() <<
" joint " << i <<
" mode " << Vocab32::decode(modes[i]);
3003 bool tmpresult = checkRemoteControlModeStatus(i, modes[i]);
3004 if(
false == tmpresult)
3006 yError() <<
"setControlModesRaw(int *modes) could not check with checkRemoteControlModeStatus() for" << getBoardInfo() <<
" joint " << i <<
" mode " << Vocab32::decode(modes[i]);
3009 ret = ret && tmpresult;
3042 eOmc_joint_status_core_t core;
3043 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
3049 *value = (double) core.measures.meas_position;
3053 yError() <<
"embObjMotionControl while reading encoder";
3063 for(
int j=0; j< _njoints; j++)
3073 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
3074 eOmc_joint_status_core_t core;
3081 *sp = (double) core.measures.meas_velocity;
3088 for(
int j=0; j< _njoints; j++)
3097 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
3098 eOmc_joint_status_core_t core;
3104 *
acc = (double) core.measures.meas_acceleration;
3111 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[j];
3177 eOmc_motor_status_basic_t status;
3178 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
3183 *value = (double) status.mot_position;
3187 yError() <<
"embObjMotionControl while reading motor encoder position";
3197 for(
int j=0; j< _njoints; j++)
3207 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
3208 eOmc_motor_status_basic_t tmpMotorStatus;
3212 *sp = (double) tmpMotorStatus.mot_velocity;
3216 yError() <<
"embObjMotionControl while reading motor encoder speed";
3225 for(
int j=0; j< _njoints; j++)
3234 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
3235 eOmc_motor_status_basic_t tmpMotorStatus;
3239 *
acc = (double) tmpMotorStatus.mot_acceleration;
3243 yError() <<
"embObjMotionControl while reading motor encoder acceleration";
3252 for(
int j=0; j< _njoints; j++)
3262 std::lock_guard<std::mutex> lck(_mutex);
3263 for(
int i=0; i<_njoints; i++)
3264 stamps[i] = _encodersStamp[i];
3271 std::lock_guard<std::mutex> lck(_mutex);
3272 *stamp = _encodersStamp[m];
3291 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
3292 eOmc_motor_status_basic_t tmpMotorStatus;
3295 *value = (double) tmpMotorStatus.mot_current;
3302 for(
int j=0; j< _njoints; j++)
3311 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_currentlimits);
3313 eOmc_current_limits_params_t currentlimits = {0};
3315 if(!askRemoteValue(protid, ¤tlimits, size))
3317 yError() <<
"embObjMotionControl::setMaxCurrentRaw() could not read max current for " << getBoardInfo() <<
"joint " << j;
3322 currentlimits.overloadCurrent = (eOmeas_current_t)
S_16(val);
3330 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_currentlimits);
3332 eOmc_current_limits_params_t currentlimits = {0};
3335 if(!askRemoteValue(protid, ¤tlimits, size))
3337 yError() <<
"embObjMotionControl::getMaxCurrentRaw() could not read max current for " << getBoardInfo() <<
"joint " << j;
3341 *val = (double) currentlimits.overloadCurrent;
3349 (_enabledAmp[j ]) ? *st = 1 : *st = 0;
3356 for(
int j=0; j<_njoints; j++)
3358 sts[j] = _enabledAmp[j];
3364#ifdef IMPLEMENT_DEBUG_INTERFACE
3369bool embObjMotionControl::setParameterRaw(
int j,
unsigned int type,
double value) {
return NOT_YET_IMPLEMENTED(
"setParameterRaw"); }
3370bool embObjMotionControl::getParameterRaw(
int j,
unsigned int type,
double* value) {
return NOT_YET_IMPLEMENTED(
"getParameterRaw"); }
3371bool embObjMotionControl::getDebugParameterRaw(
int j,
unsigned int index,
double* value) {
return NOT_YET_IMPLEMENTED(
"getDebugParameterRaw"); }
3372bool embObjMotionControl::setDebugParameterRaw(
int j,
unsigned int index,
double value) {
return NOT_YET_IMPLEMENTED(
"setDebugParameterRaw"); }
3373bool embObjMotionControl::setDebugReferencePositionRaw(
int j,
double value) {
return NOT_YET_IMPLEMENTED(
"setDebugReferencePositionRaw"); }
3374bool embObjMotionControl::getDebugReferencePositionRaw(
int j,
double* value) {
return NOT_YET_IMPLEMENTED(
"getDebugReferencePositionRaw");}
3382 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_userlimits);
3384 eOmeas_position_limits_t limits;
3385 limits.max = (eOmeas_position_t)
S_32(max);
3386 limits.min = (eOmeas_position_t)
S_32(min);
3393 yError() <<
"while setting position limits for joint" << j <<
" \n";
3400 eOmeas_position_limits_t limits;
3401 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_userlimits);
3404 if(! askRemoteValue(protoid, &limits, size))
3407 *min = (double)limits.min + SAFETY_THRESHOLD;
3408 *max = (double)limits.max - SAFETY_THRESHOLD;
3414 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3416 eOmc_motor_config_t motor_cfg;
3417 if(! askRemoteValue(protoid, &motor_cfg, size))
3421 *gearbox = (double)motor_cfg.gearbox_M2J;
3428 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3430 eOmc_motor_config_t motor_cfg;
3431 if(! askRemoteValue(protoid, &motor_cfg, size))
3433 *rotorMax = (double)( motor_cfg.limitsofrotor.max);
3434 *rotorMin = (double)( motor_cfg.limitsofrotor.min);
3440 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3442 eOmc_joint_config_t joint_cfg;
3443 if(! askRemoteValue(protoid, &joint_cfg, size))
3447 type = (int)joint_cfg.tcfiltertype;
3453 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 rotres = (double)motor_cfg.rotorEncoderResolution;
3467 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3469 eOmc_joint_config_t joint_cfg;
3470 if(! askRemoteValue(protoid, &joint_cfg, size))
3474 jntres = (double)joint_cfg.jntEncoderResolution;
3481 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3483 eOmc_joint_config_t joint_cfg;
3484 if(! askRemoteValue(protoid, &joint_cfg, size))
3488 type = (int)joint_cfg.jntEncoderType;
3495 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3497 eOmc_motor_config_t motor_cfg;
3498 if(! askRemoteValue(protoid, &motor_cfg, size))
3502 type = (int)motor_cfg.rotorEncoderType;
3509 yError(
"getKinematicMJRaw not yet implemented");
3535 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3537 eOmc_motor_config_t motor_cfg;
3538 if(! askRemoteValue(protoid, &motor_cfg, size))
3542 ret = (int)motor_cfg.hasTempSensor;
3549 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3551 eOmc_motor_config_t motor_cfg;
3552 if(! askRemoteValue(protoid, &motor_cfg, size))
3556 ret = (int)motor_cfg.hasHallSensor;
3563 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3565 eOmc_motor_config_t motor_cfg;
3566 if(! askRemoteValue(protoid, &motor_cfg, size))
3570 ret = (int)motor_cfg.hasRotorEncoder;
3577 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3579 eOmc_motor_config_t motor_cfg;
3580 if(! askRemoteValue(protoid, &motor_cfg, size))
3584 ret = (int)motor_cfg.hasRotorEncoderIndex;
3591 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3594 eOmc_motor_config_t motor_cfg;
3595 if(! askRemoteValue(protoid, &motor_cfg, size))
3599 poles = (int)motor_cfg.motorPoles;
3606 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
3608 eOmc_motor_config_t motor_cfg;
3609 if(! askRemoteValue(protoid, &motor_cfg, size))
3613 rotorOffset = (double)motor_cfg.rotorIndexOffset;
3620 if (axis >= 0 && axis < _njoints)
3622 name = _axesInfo[axis].name;
3634 if (axis >= 0 && axis < _njoints)
3636 type = _axesInfo[axis].type;
3645bool embObjMotionControl::getJointDeadZoneRaw(
int j,
double &jntDeadZone)
3647 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config);
3649 eOmc_joint_config_t joint_cfg;
3650 if(! askRemoteValue(protoid, &joint_cfg, size))
3654 jntDeadZone = _measureConverter->posE2A((
double)joint_cfg.deadzone, _axisMap[j]);
3663 if (key ==
"kinematic_mj")
3666 Bottle& ret = val.addList();
3668 eOmn_serv_type_t mc_serv_type = (eOmn_serv_type_t)serviceConfig.
ethservice.configuration.type;
3669 if(iNeedCouplingsInfo())
3671 for (
int r=0; r<_njoints; r++)
3673 for (
int c = 0; c < _njoints; c++)
3677 ret.addFloat64(_couplingInfo.
matrixJ2M[4 * r + c]);
3683 ret.addFloat64(0.0);
3687 else if (key ==
"encoders")
3689 Bottle& r = val.addList();
for (
int i = 0; i < _njoints; i++) { r.addFloat64(_measureConverter->posA2E(1.0, i)); }
3692 else if (key ==
"rotorEncoderResolution")
3694 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getRotorEncoderResolutionRaw(i, tmp); r.addFloat64(tmp); }
3697 else if (key ==
"jointEncoderResolution")
3699 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getJointEncoderResolutionRaw(i, tmp); r.addFloat64(tmp); }
3702 else if (key ==
"gearbox_M2J")
3704 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp=0;
getGearboxRatioRaw(i, &tmp); r.addFloat64(tmp); }
3707 else if (key ==
"gearbox_E2J")
3709 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp=0; getGerabox_E2J(i, &tmp); r.addFloat64(tmp); }
3712 else if (key ==
"hasHallSensor")
3714 Bottle& r = val.addList();
for (
int i = 0; i < _njoints; i++) {
int tmp = 0;
getHasHallSensorRaw(i, tmp); r.addInt32(tmp); }
3717 else if (key ==
"hasTempSensor")
3719 Bottle& r = val.addList();
for (
int i = 0; i < _njoints; i++) {
int tmp = 0;
getHasTempSensorsRaw(i, tmp); r.addInt32(tmp); }
3722 else if (key ==
"TemperatureSensorType")
3724 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) { std::string tmp =
"";
getTemperatureSensorTypeRaw(i, tmp); r.addString(tmp); }
3727 else if (key ==
"hasRotorEncoder")
3729 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
int tmp = 0;
getHasRotorEncoderRaw(i, tmp); r.addInt32(tmp); }
3732 else if (key ==
"hasRotorEncoderIndex")
3734 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
int tmp = 0;
getHasRotorEncoderIndexRaw(i, tmp); r.addInt32(tmp); }
3737 else if (key ==
"rotorIndexOffset")
3739 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getRotorIndexOffsetRaw(i, tmp); r.addFloat64(tmp); }
3742 else if (key ==
"motorPoles")
3744 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
int tmp = 0;
getMotorPolesRaw(i, tmp); r.addInt32(tmp); }
3747 else if (key ==
"pidCurrentKp")
3749 Bottle& r = val.addList();
for (
int i = 0; i < _njoints; i++) { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p); r.addFloat64(
p.kp); }
3752 else if (key ==
"pidCurrentKi")
3754 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p); r.addFloat64(
p.ki); }
3757 else if (key ==
"pidCurrentShift")
3759 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p); r.addFloat64(
p.scale); }
3762 else if (key ==
"pidCurrentOutput")
3764 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); }
3767 else if (key ==
"jointEncoderType")
3769 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++)
3775 yError(
"Invalid jointEncoderType");
3781 else if (key ==
"rotorEncoderType")
3783 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++)
3789 yError(
"Invalid motorEncoderType");
3795 else if (key ==
"coulombThreshold")
3797 val.addString(
"not implemented yet");
3800 else if (key ==
"torqueControlFilterType")
3805 else if (key ==
"torqueControlEnabled")
3808 Bottle& r = val.addList();
3809 for(
int i = 0; i<_njoints; i++)
3811 r.addInt32((
int)_trq_pids[i].enabled );
3815 else if (key ==
"PWMLimit")
3817 Bottle& r = val.addList();
for (
int i = 0; i< _njoints; i++) {
double tmp = 0;
getPWMLimitRaw(i, &tmp); r.addFloat64(tmp); }
3820 else if (key ==
"motOverloadCurr")
3822 Bottle& r = val.addList();
for (
int i = 0; i< _njoints; i++) {
double tmp = 0;
getMaxCurrentRaw(i, &tmp); r.addFloat64(tmp); }
3825 else if (key ==
"motNominalCurr")
3827 Bottle& r = val.addList();
for (
int i = 0; i< _njoints; i++) {
double tmp = 0;
getNominalCurrentRaw(i, &tmp); r.addFloat64(tmp); }
3830 else if (key ==
"motPeakCurr")
3832 Bottle& r = val.addList();
for (
int i = 0; i< _njoints; i++) {
double tmp = 0;
getPeakCurrentRaw(i, &tmp); r.addFloat64(tmp); }
3835 else if (key ==
"PowerSuppVoltage")
3837 Bottle& r = val.addList();
for (
int i = 0; i< _njoints; i++) {
double tmp = 0;
getPowerSupplyVoltageRaw(i, &tmp); r.addFloat64(tmp); }
3840 else if (key ==
"rotorMax")
3843 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getRotorLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp2); }
3846 else if (key ==
"rotorMin")
3849 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getRotorLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp1); }
3852 else if (key ==
"jointMax")
3855 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp2); }
3858 else if (key ==
"jointMin")
3861 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0;
getLimitsRaw(i, &tmp1, &tmp2); r.addFloat64(tmp1); }
3864 else if (key ==
"jointEncTolerance")
3867 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0; getJointEncTolerance(i, &tmp1); r.addFloat64(tmp1); }
3870 else if (key ==
"motorEncTolerance")
3873 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0; getMotorEncTolerance(i, &tmp1); r.addFloat64(tmp1); }
3876 else if (key ==
"jointDeadZone")
3879 Bottle& r = val.addList();
for (
int i = 0; i<_njoints; i++) {
double tmp = 0; getJointDeadZoneRaw(i, tmp1); r.addFloat64(tmp1); }
3882 else if (key ==
"readonly_position_PIDraw")
3884 Bottle& r = val.addList();
3885 for (
int i = 0; i < _njoints; i++)
3887 getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, i, &
p);
3889 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);
3894 else if (key ==
"readonly_velocity_PIDraw")
3896 Bottle& r = val.addList();
3897 for (
int i = 0; i < _njoints; i++)
3898 { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, i, &
p);
3900 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);
3905 else if (key ==
"readonly_torque_PIDraw")
3907 Bottle& r = val.addList();
3908 for (
int i = 0; i < _njoints; i++)
3909 { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, i, &
p);
3911 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);
3916 else if (key ==
"readonly_current_PIDraw")
3918 Bottle& r = val.addList();
3919 for (
int i = 0; i < _njoints; i++)
3920 { Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT, i, &
p);
3922 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);
3927 else if (key ==
"readonly_llspeed_PIDraw")
3929 Bottle& r = val.addList();
3930 for (
int i = 0; i < _njoints; i++)
3932 Pid
p;
getPidRaw(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY, i, &
p);
3934 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);
3939 else if (key ==
"readonly_motor_torque_params_raw")
3941 Bottle& r = val.addList();
3942 for (
int i = 0; i < _njoints; i++)
3944 MotorTorqueParameters params;
3947 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);
3952 yWarning(
"getRemoteVariable(): Unknown variable %s", key.c_str());
3958 string s1 = val.toString();
3959 if (val.size() != _njoints)
3961 yWarning(
"setRemoteVariable(): Protocol error %s", s1.c_str());
3965 if (key ==
"kinematic_mj")
3967 yWarning(
"setRemoteVariable(): Impossible to set kinematic_mj parameter at runtime.");
3980 else if (key ==
"PWMLimit")
3982 for (
int i = 0; i < _njoints; i++)
setPWMLimitRaw(i, val.get(i).asFloat64());
3987 else if (key ==
"jointMax")
3990 for (
int i = 0; i < _njoints; i++)
3997 else if (key ==
"jointMin")
4000 for (
int i = 0; i < _njoints; i++)
4007 yWarning(
"setRemoteVariable(): Unknown variable %s", key.c_str());
4013 listOfKeys->clear();
4014 listOfKeys->addString(
"kinematic_mj");
4015 listOfKeys->addString(
"encoders");
4016 listOfKeys->addString(
"gearbox_M2J");
4017 listOfKeys->addString(
"gearbox_E2J");
4018 listOfKeys->addString(
"hasHallSensor");
4019 listOfKeys->addString(
"hasTempSensor");
4020 listOfKeys->addString(
"TemperatureSensorType");
4021 listOfKeys->addString(
"hasRotorEncoder");
4022 listOfKeys->addString(
"hasRotorEncoderIndex");
4023 listOfKeys->addString(
"rotorIndexOffset");
4024 listOfKeys->addString(
"rotorEncoderResolution");
4025 listOfKeys->addString(
"jointEncoderResolution");
4026 listOfKeys->addString(
"motorPoles");
4027 listOfKeys->addString(
"pidCurrentKp");
4028 listOfKeys->addString(
"pidCurrentKi");
4029 listOfKeys->addString(
"pidCurrentShift");
4030 listOfKeys->addString(
"pidCurrentOutput");
4031 listOfKeys->addString(
"coulombThreshold");
4032 listOfKeys->addString(
"torqueControlFilterType");
4033 listOfKeys->addString(
"jointEncoderType");
4034 listOfKeys->addString(
"rotorEncoderType");
4035 listOfKeys->addString(
"PWMLimit");
4036 listOfKeys->addString(
"motOverloadCurr");
4037 listOfKeys->addString(
"motNominalCurr");
4038 listOfKeys->addString(
"motPeakCurr");
4039 listOfKeys->addString(
"PowerSuppVoltage");
4040 listOfKeys->addString(
"rotorMax");
4041 listOfKeys->addString(
"rotorMin");
4042 listOfKeys->addString(
"jointMax");
4043 listOfKeys->addString(
"jointMin");
4044 listOfKeys->addString(
"jointEncTolerance");
4045 listOfKeys->addString(
"motorEncTolerance");
4046 listOfKeys->addString(
"jointDeadZone");
4047 listOfKeys->addString(
"readonly_position_PIDraw");
4048 listOfKeys->addString(
"readonly_velocity_PIDraw");
4049 listOfKeys->addString(
"readonly_current_PIDraw");
4050 listOfKeys->addString(
"readonly_torque_PIDraw");
4051 listOfKeys->addString(
"readonly_motor_torque_params_raw");
4063 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_config);
4065 eOmc_joint_config_t joint_cfg;
4066 if(! askRemoteValue(protoid, &joint_cfg, size))
4069 *max = joint_cfg.maxvelocityofjoint;
4085 return VAS_status::VAS_OK;
4097 for(
int j=0; j< _njoints; j++)
4106 int j = _axisMap[userLevel_jointNumber];
4108 eOmeas_torque_t meas_torque = 0;
4109 static double curr_time = Time::now();
4110 static int count_saturation=0;
4112 meas_torque = (eOmeas_torque_t)
S_32(_measureConverter->trqN2S(fTorque, j));
4114 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_inputs_externallymeasuredtorque);
4129 eOmc_joint_status_core_t jstatus;
4130 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
4132 *t = (double) _measureConverter->trqS2N(jstatus.measures.meas_torque, j);
4139 for(
int j=0; j<_njoints; j++)
4157 for(
int j=0; j<_njoints && ret; j++)
4164 eOmc_setpoint_t setpoint;
4165 setpoint.type = (eOenum08_t) eomc_setpoint_torque;
4166 setpoint.to.torque.value = (eOmeas_torque_t)
S_32(t);
4168 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
4175 for(
int j=0; j< n_joint; j++)
4185 for(
int j=0; j<_njoints && ret; j++)
4192 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
4193 eOmc_joint_status_core_t jcore = {0};
4199 yError() <<
"embObjMotionControl::getRefTorqueRaw() could not read pid torque reference pos for " << getBoardInfo() <<
"joint " << j;
4203 if ((eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus) &&
4204 (eomc_controlmode_position == jcore.modes.controlmodestatus))
4206 *t = (double) jcore.ofpid.complpos.reftrq;
4209 if(eomc_controlmode_torque == jcore.modes.controlmodestatus)
4211 *t = (double) jcore.ofpid.torque.reftrq;
4217bool embObjMotionControl::helper_setTrqPidRaw(
int j,
const Pid &pid)
4225 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtorque);
4229bool embObjMotionControl::helper_getTrqPidRaw(
int j, Pid *pid)
4231 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_pidtorque);
4235 if(! askRemoteValue(protoid, &eoPID, size))
4244bool embObjMotionControl::helper_getTrqPidsRaw(Pid *pid)
4246 std::vector<eOmc_PID_t> eoPIDList (_njoints);
4247 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_pidtorque, eoPIDList);
4250 for(
int j=0; j< _njoints; j++)
4262 eOmc_impedance_t val;
4267 *stiffness = (double) (val.stiffness);
4268 *damping = (double) (val.damping);
4276 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
4278 if(! askRemoteValue(protoid, &imped, size))
4282 _cacheImpedance->damping = imped.damping;
4283 _cacheImpedance->stiffness = imped.stiffness;
4284 _cacheImpedance->offset = imped.offset;
4291 eOmc_impedance_t val;
4299 _cacheImpedance[j].stiffness = (eOmeas_stiffness_t) stiffness;
4300 _cacheImpedance[j].damping = (eOmeas_damping_t) damping;
4302 val.stiffness = _cacheImpedance[j].stiffness;
4303 val.damping = _cacheImpedance[j].damping;
4304 val.offset = _cacheImpedance[j].offset;
4306 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
4316 eOmc_impedance_t val;
4322 _cacheImpedance[j].offset = (eOmeas_torque_t)
S_32(
offset);
4323 val.stiffness = _cacheImpedance[j].stiffness;
4324 val.damping = _cacheImpedance[j].damping;
4325 val.offset = _cacheImpedance[j].offset;
4327 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_impedance);
4337 eOmc_impedance_t val;
4348 *min_stiff = _impedance_limits[j].
min_stiff;
4349 *max_stiff = _impedance_limits[j].
max_stiff;
4350 *min_damp = _impedance_limits[j].
min_damp;
4351 *max_damp = _impedance_limits[j].
max_damp;
4357 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_motor_params);
4360 eOmc_motor_params_t eo_params = {0};
4361 if(! askRemoteValue(protoid, &eo_params, size))
4364 params->bemf = eo_params.bemf_value;
4365 params->bemf_scale = eo_params.bemf_scale;
4366 params->ktau = eo_params.ktau_value;
4367 params->ktau_scale = eo_params.ktau_scale;
4368 params->viscousPos = eo_params.friction.viscous_pos_val;
4369 params->viscousNeg = eo_params.friction.viscous_neg_val ;
4370 params->coulombPos = eo_params.friction.coulomb_pos_val;
4371 params->coulombNeg = eo_params.friction.coulomb_neg_val;
4372 params->velocityThres = eo_params.friction.velocityThres_val;
4381 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_motor_params);
4382 eOmc_motor_params_t eo_params = {0};
4386 eo_params.bemf_value = (float) params.bemf;
4387 eo_params.bemf_scale = (uint8_t) params.bemf_scale;
4388 eo_params.ktau_value = (float) params.ktau;
4389 eo_params.ktau_scale = (uint8_t) params.ktau_scale;
4390 eo_params.friction.viscous_pos_val =
static_cast<float32_t
>(params.viscousPos);
4391 eo_params.friction.viscous_neg_val =
static_cast<float32_t
>(params.viscousNeg);
4392 eo_params.friction.coulomb_pos_val =
static_cast<float32_t
>(params.coulombPos);
4393 eo_params.friction.coulomb_neg_val =
static_cast<float32_t
>(params.coulombNeg);
4394 eo_params.friction.velocityThres_val =
static_cast<float32_t
>(params.velocityThres);
4399 yError() <<
"embObjMotionControl::setMotorTorqueParamsRaw() could not send set message for" << getBoardInfo() <<
"joint " << j;
4411 for(
int j=0; j< n_joint; j++)
4445bool embObjMotionControl::helper_getVelPidRaw(
int j, Pid *pid)
4447 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_config_piddirect);
4450 if(! askRemoteValue(protoid, &eoPID, size))
4460bool embObjMotionControl::helper_getVelPidsRaw(Pid *pid)
4462 std::vector <eOmc_PID_t> eoPIDList (_njoints);
4463 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_config_piddirect, eoPIDList);
4467 for(
int j=0; j<_njoints; j++)
4482 if (mode != VOCAB_CM_POSITION_DIRECT &&
4483 mode != VOCAB_CM_IDLE)
4487 yError() <<
"setReferenceRaw: skipping command because" << getBoardInfo() <<
" joint " << j <<
" is not in VOCAB_CM_POSITION_DIRECT mode";
4492 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
4493 eOmc_setpoint_t setpoint = {0};
4495 _ref_positions[j] = ref;
4496 setpoint.type = (eOenum08_t) eomc_setpoint_positionraw;
4497 setpoint.to.position.value = (eOmeas_position_t)
S_32(ref);
4498 setpoint.to.position.withvelocity = 0;
4506 for(
int i=0; i<n_joint; i++)
4516 for (
int i = 0; i<_njoints; i++)
4526 if (axis<0 || axis>_njoints)
return false;
4527#if ASK_REFERENCE_TO_FIRMWARE
4528 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4533 eOmc_joint_status_target_t target = {0};
4534 if(!askRemoteValue(id32, &target, size))
4536 yError() <<
"embObjMotionControl::getTargetPositionRaw() could not read reference pos for " << getBoardInfo() <<
"joint " << axis;
4540 *ref = (double) target.trgt_position;
4544 *ref = _ref_command_positions[axis];
4552 for (
int i = 0; i<_njoints; i++)
4562 for (
int i = 0; i<nj; i++)
4571 if (axis<0 || axis>_njoints)
return false;
4572#if ASK_REFERENCE_TO_FIRMWARE
4573 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4578 eOmc_joint_status_target_t target = {0};
4579 if(!askRemoteValue(id32, &target, size))
4581 yError() <<
"embObjMotionControl::getRefVelocityRaw() could not read reference vel for " << getBoardInfo() <<
"joint " << axis;
4584 *ref = (double) target.trgt_velocity;
4587 *ref = _ref_command_speeds[axis];
4594 #if ASK_REFERENCE_TO_FIRMWARE
4595 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
4596 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
4599 yError() <<
"embObjMotionControl::getRefVelocitiesRaw() could not read reference vel for " << getBoardInfo() ;
4603 for(
int j=0; j<_njoints; j++)
4605 refs[j] = (double) targetList[j].trgt_velocity;
4609 for(
int j=0; j<_njoints; j++)
4611 refs[j] = _ref_command_speeds[j];
4619 std::vector <double> refsList(_njoints);
4623 for (
int i = 0; i<nj; i++)
4625 if(jnts[i]>= _njoints)
4627 yError() << getBoardInfo() <<
"getRefVelocitiesRaw: joint " << jnts[i] <<
"doesn't exist";
4630 refs[i] = refsList[jnts[i]];
4637 if (axis<0 || axis>_njoints)
return false;
4638#if ASK_REFERENCE_TO_FIRMWARE
4639 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, axis, eoprot_tag_mc_joint_status_target);
4643 eOmc_joint_status_target_t target = {0};
4644 if(!askRemoteValue(id32, &target, size))
4646 yError() <<
"embObjMotionControl::getRefPositionRaw() could not read reference pos for " << getBoardInfo() <<
"joint " << axis;
4650 *ref = (double) target.trgt_positionraw;
4653 *ref = _ref_positions[axis];
4660 #if ASK_REFERENCE_TO_FIRMWARE
4661 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
4662 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
4665 yError() <<
"embObjMotionControl::getRefPositionRaw() could not read reference pos for " << getBoardInfo();
4669 for(
int j=0; j< _njoints; j++)
4670 refs[j] = (
double) targetList[j].trgt_positionraw;
4673 for(
int j=0; j< _njoints; j++)
4674 refs[j] = _ref_positions[j];
4682 for (
int i = 0; i<nj; i++)
4695 eOenum08_t interactionmodestatus;
4698 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core_modes_interactionmodestatus);
4702 int tmp = (int) *_mode;
4706 *_mode = (yarp::dev::InteractionModeEnum) tmp;
4725 for(
int j=0; j<_njoints; j++)
4735 eOenum08_t interactionmodecommand = 0;
4740 if (_mode == VOCAB_IM_COMPLIANT && _trq_pids[j].enabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
return false;}
4744 yError() <<
"setInteractionModeRaw: received unknown mode for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
4747 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4749 if(
false == res->
setRemoteValue(protid, &interactionmodecommand) )
4751 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
4757 eOenum08_t interactionmodestatus = 0;
4759 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4760 bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4762 if((
false == ret) || (interactionmodecommand != interactionmodestatus))
4764 yError() <<
"check of embObjMotionControl::setInteractionModeRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(_mode);
4777 eOenum08_t interactionmodecommand = 0;
4779 for(
int j=0; j<n_joints; j++)
4781 if (modes[j] == VOCAB_IM_COMPLIANT && _trq_pids[j].enabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
continue;}
4785 yError() <<
"embObjMotionControl::setInteractionModesRaw(): received unknown interactionMode for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]) <<
" " << modes[j];
4789 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4790 if(
false == res->
setRemoteValue(protid, &interactionmodecommand) )
4792 yError() <<
"embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4798 eOenum08_t interactionmodestatus = 0;
4800 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4801 bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4803 if((
false == ret) || (interactionmodecommand != interactionmodestatus))
4807 yError() <<
"check of embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4813 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4814 << Vocab32::decode(modes[j]) <<
" Got " << Vocab32::decode(tmp);
4816 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4817 << Vocab32::decode(modes[j]) <<
" Got an unknown value!";
4830 eOenum08_t interactionmodecommand = 0;
4832 for(
int j=0; j<_njoints; j++)
4834 if ((modes[j] == VOCAB_IM_COMPLIANT) && (_trq_pids[j].enabled ==
false))
4836 yError()<<
"Torque control is disabled. Check your configuration parameters";
4842 yError() <<
"setInteractionModeRaw: received unknown interactionMode for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4846 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_interactionmode);
4847 if(
false == res->
setRemoteValue(protid, &interactionmodecommand) )
4849 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4855 eOenum08_t interactionmodestatus = 0;
4857 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_interactionmodestatus);
4858 bool ret = askRemoteValue(id32, &interactionmodestatus, size);
4860 if((
false == ret) || (interactionmodecommand != interactionmodestatus))
4864 yError() <<
"check of embObjMotionControl::setInteractionModesRaw() failed for" << getBoardInfo() <<
" joint " << j <<
" mode " << Vocab32::decode(modes[j]);
4870 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4871 << Vocab32::decode(modes[j]) <<
" Got " << Vocab32::decode(tmp);
4873 yError() <<
"setInteractionModeRaw failed for" << getBoardInfo() <<
" joint " << j <<
" because of interactionMode mismatching \n\tSet " \
4874 << Vocab32::decode(modes[j]) <<
" Got an unknown value!";
4887 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_core);
4888 eOmc_joint_status_core_t jcore = {0};
4895 case VOCAB_PIDTYPE_POSITION:
4896 if((eomc_controlmode_torque == jcore.modes.controlmodestatus) || (eomc_controlmode_current == jcore.modes.controlmodestatus))
4899 *
out = (double) jcore.ofpid.generic.output;
4904 case VOCAB_PIDTYPE_TORQUE:
4905 if ((eomc_controlmode_torque == jcore.modes.controlmodestatus) ||
4906 ((eomc_controlmode_position == jcore.modes.controlmodestatus) && (eOmc_interactionmode_compliant == jcore.modes.interactionmodestatus)))
4907 *
out = jcore.ofpid.generic.output;
4911 case VOCAB_PIDTYPE_CURRENT:
4914 case VOCAB_PIDTYPE_VELOCITY:
4918 yError()<<
"Invalid pidtype:"<<pidtype;
4927 for(
int j=0; j< _njoints; j++)
4947 eOmc_motor_status_basic_t status;
4948 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);
4958 yError() << getBoardInfo() <<
"At timestamp" << yarp::os::Time::now() <<
"In motor" << m <<
"embObjMotionControl::getTemperatureRaw failed to complete getLocalValue()";
4962 *val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius((
double)status.mot_temperature);
4971 for(
int j=0; j< _njoints; j++)
4980 *temp= _temperatureLimits[m].warningTemperatureLimit;
4987 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_temperaturelimit);
4988 eOmeas_temperature_t temperatureLimit = (eOmeas_pwm_t)
S_16(temp);
4995 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
4997 eOmc_current_limits_params_t currentlimits = {0};
4999 if(!askRemoteValue(protid, ¤tlimits, size))
5001 yError() <<
"embObjMotionControl::getPeakCurrentRaw() can't read current limits for" << getBoardInfo() <<
" motor " << m;
5005 *val = (double) currentlimits.peakCurrent ;
5011 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
5014 eOmc_current_limits_params_t currentlimits = {0};
5015 if(!askRemoteValue(protid, ¤tlimits, size))
5017 yError() <<
"embObjMotionControl::setPeakCurrentRaw can't read current limits for" << getBoardInfo() <<
" motor " << m ;
5022 currentlimits.peakCurrent = (eOmeas_current_t)
S_16(val);
5028 yError() <<
"embObjMotionControl::setPeakCurrentRaw failed sending new value for" << getBoardInfo() <<
" motor " << m ;
5035 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
5037 eOmc_current_limits_params_t currentlimits = {0};
5039 if(!askRemoteValue(protid, ¤tlimits, size))
5041 yError() <<
"embObjMotionControl::getNominalCurrentRaw() can't read current limits for" << getBoardInfo() <<
" motor " << m;
5045 *val = (double) currentlimits.nominalCurrent ;
5051 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_currentlimits);
5055 eOmc_current_limits_params_t currentlimits = {0};
5056 if(!askRemoteValue(protid, ¤tlimits, size))
5058 yError() <<
"embObjMotionControl::setNominalCurrentRaw can't read current limits for" << getBoardInfo() <<
" motor " << m ;
5063 currentlimits.nominalCurrent = (eOmeas_current_t)
S_16(val);
5069 yError() <<
"embObjMotionControl::setNominalCurrentRaw failed sending new value for" << getBoardInfo() <<
" motor " << m ;
5077 eOmc_motor_status_basic_t status;
5078 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
5083 *val = (double) status.mot_pwm;
5087 yError() <<
"embObjMotionControl::getPWMRaw failed for" << getBoardInfo() <<
" motor " << j ;
5096 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pwmlimit);
5098 eOmeas_pwm_t motorPwmLimit;
5100 bool ret = askRemoteValue(protid, &motorPwmLimit, size);
5103 *val = (double) motorPwmLimit;
5107 yError() <<
"embObjMotionControl::getPWMLimitRaw failed for" << getBoardInfo() <<
" motor " << j ;
5118 yError() <<
"embObjMotionControl::setPWMLimitRaw failed because pwmLimit is negative for" << getBoardInfo() <<
" motor " << j ;
5121 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pwmlimit);
5122 eOmeas_pwm_t motorPwmLimit = (eOmeas_pwm_t)
S_16(val);
5128 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_controller, 0, eoprot_tag_mc_controller_status);
5130 eOmc_controller_status_t controllerStatus;
5132 bool ret = askRemoteValue(protid, &controllerStatus, size);
5135 *val = (double) controllerStatus.supplyVoltage;
5139 yError() <<
"embObjMotionControl::getPowerSupplyVoltageRaw failed for" << getBoardInfo() <<
" motor " << j ;
5146bool embObjMotionControl::askRemoteValue(eOprotID32_t id32,
void* value, uint16_t& size)
5153bool embObjMotionControl::askRemoteValues(eOprotEndpoint_t ep, eOprotEntity_t entity, eOprotTag_t tag, std::vector<T>& values)
5155 std::vector<eOprotID32_t> idList;
5156 std::vector<void*> valueList;
5159 for(
int j=0; j<_njoints; j++)
5161 eOprotID32_t protoId = eoprot_ID_get(ep, entity, j, tag);
5162 idList.push_back(protoId);
5163 valueList.push_back((
void*)&values[j]);
5169 yError() <<
"embObjMotionControl::askRemoteValues failed for all joints of" << getBoardInfo();
5178bool embObjMotionControl::checkRemoteControlModeStatus(
int joint,
int target_mode)
5181 eOenum08_t temp = 0;
5184 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core_modes_controlmodestatus);
5185 const double timeout = 0.250;
5186 const int maxretries = 25;
5187 const double delaybetweenqueries = 0.010;
5191 double timeofstart = yarp::os::Time::now();
5194 for( attempt = 0; attempt < maxretries; attempt++)
5196 ret = askRemoteValue(id32, &temp, size);
5199 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());
5203 if(current_mode == target_mode)
5208 if((current_mode == VOCAB_CM_IDLE) && (target_mode == VOCAB_CM_FORCE_IDLE))
5213 if(current_mode == VOCAB_CM_HW_FAULT)
5215 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()); }
5220 if((yarp::os::Time::now()-timeofstart) > timeout)
5223 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());
5228 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());
5230 SystemClock::delaySystem(delaybetweenqueries);
5235 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);
5243bool embObjMotionControl::iNeedCouplingsInfo(
void)
5245 eOmn_serv_type_t mc_serv_type = (eOmn_serv_type_t)serviceConfig.
ethservice.configuration.type;
5246 if( (mc_serv_type == eomn_serv_MC_foc) ||
5247 (mc_serv_type == eomn_serv_MC_mc4plus) ||
5248 (mc_serv_type == eomn_serv_MC_mc4plusmais) ||
5249 (mc_serv_type == eomn_serv_MC_mc2pluspsc) ||
5250 (mc_serv_type == eomn_serv_MC_mc4plusfaps) ||
5251 (mc_serv_type == eomn_serv_MC_advfoc)
5261 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
5263 eOmc_setpoint_t setpoint;
5265 setpoint.type = (eOenum08_t)eomc_setpoint_openloop;
5266 setpoint.to.openloop.value = (eOmeas_pwm_t)
S_16(v);
5274 for (
int j = 0; j<_njoints; j++)
5283 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_target);
5286 eOmc_joint_status_target_t target = { 0 };
5289 if (!askRemoteValue(protoId, &target, size))
5291 yError() <<
"embObjMotionControl::getRefDutyCycleRaw() could not read openloop reference for " << getBoardInfo() <<
"joint " << j;
5295 *v = (double)target.trgt_pwm;
5302 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
5303 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
5306 yError() <<
"embObjMotionControl::getDutyCyclesRaw failed for all joints of" << getBoardInfo();
5309 for (
int j = 0; j<_njoints; j++)
5311 v[j]= targetList[j].trgt_pwm;
5318 eOmc_motor_status_basic_t status;
5319 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_status_basic);
5324 *v = (double)status.mot_pwm;
5328 yError() <<
"embObjMotionControl::getDutyCycleRaw failed for" << getBoardInfo() <<
" motor " << j;
5338 for (
int j = 0; j< _njoints; j++)
5360 for (
int j = 0; j< _njoints; j++)
5370 for (
int j = 0; j<_njoints; j++)
5379 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_cmmnds_setpoint);
5381 eOmc_setpoint_t setpoint;
5383 setpoint.type = (eOenum08_t)eomc_setpoint_current;
5384 setpoint.to.current.value = (eOmeas_pwm_t)
S_16(t);
5392 for (
int j = 0; j<n_joint; j++)
5401 std::vector <eOmc_joint_status_target_t> targetList(_njoints);
5402 bool ret = askRemoteValues(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, eoprot_tag_mc_joint_status_target, targetList);
5405 yError() <<
"embObjMotionControl::getDutyCyclesRaw failed for all joints of" << getBoardInfo();
5408 for (
int j = 0; j<_njoints; j++)
5410 t[j] = targetList[j].trgt_current;
5417 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_target);
5420 eOmc_joint_status_target_t target = { 0 };
5423 if (!askRemoteValue(protoId, &target, size))
5425 yError() <<
"embObjMotionControl::getRefDutyCycleRaw() could not read openloop reference for " << getBoardInfo() <<
"joint " << j;
5429 *t = (double)target.trgt_current;
5434bool embObjMotionControl::helper_setCurPidRaw(
int j,
const Pid &pid)
5436 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pidcurrent);
5440 if (!_cur_pids[j].enabled)
5442 yError() <<
"eoMc " << getBoardInfo() <<
": it is not possible set current pid for motor " << j <<
", because current pid is not enabled in xml files";
5450 yError() <<
"while setting velocity PIDs for" << getBoardInfo() <<
" joint " << j;
5459bool embObjMotionControl::helper_setSpdPidRaw(
int j,
const Pid &pid)
5461 eOprotID32_t protoId = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config_pidspeed);
5465 if (!_cur_pids[j].enabled)
5467 yError() <<
"eoMc " << getBoardInfo() <<
": it is not possible set speed pid for motor " << j <<
", because speed pid is not enabled in xml files";
5475 yError() <<
"while setting velocity PIDs for" << getBoardInfo() <<
" joint " << j;
5484bool embObjMotionControl::helper_getCurPidRaw(
int j, Pid *pid)
5486 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
5488 eOmc_motor_config_t motor_cfg;
5489 if(! askRemoteValue(protoid, &motor_cfg, size))
5493 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg.pidcurrent;
5499bool embObjMotionControl::helper_getSpdPidRaw(
int j, Pid *pid)
5501 eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config);
5503 eOmc_motor_config_t motor_cfg;
5504 if (!askRemoteValue(protoid, &motor_cfg, size))
5508 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg.pidspeed;
5514bool embObjMotionControl::helper_getCurPidsRaw(Pid *pid)
5516 std::vector <eOmc_motor_config_t> motor_cfg_list(_njoints);
5517 bool ret = askRemoteValues<eOmc_motor_config_t>(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, eoprot_tag_mc_motor_config, motor_cfg_list);
5521 for(
int j=0; j<_njoints; j++)
5523 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg_list[j].pidcurrent;
5529bool embObjMotionControl::helper_getSpdPidsRaw(Pid *pid)
5531 std::vector <eOmc_motor_config_t> motor_cfg_list(_njoints);
5532 bool ret = askRemoteValues<eOmc_motor_config_t>(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, eoprot_tag_mc_motor_config, motor_cfg_list);
5536 for (
int j = 0; j<_njoints; j++)
5538 eOmc_PID_t
tmp = (eOmc_PID_t)motor_cfg_list[j].pidspeed;
5544bool embObjMotionControl::getJointConfiguration(
int joint, eOmc_joint_config_t *jntCfg_ptr)
5546 uint32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_config);
5548 if(!askRemoteValue(protoid, jntCfg_ptr, size))
5556bool embObjMotionControl::getMotorConfiguration(
int axis, eOmc_motor_config_t *motCfg_ptr)
5558 uint32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, axis, eoprot_tag_mc_motor_config);
5560 if(!askRemoteValue(protoid, motCfg_ptr, size))
5569bool embObjMotionControl::getGerabox_E2J(
int joint,
double *gearbox_E2J_ptr)
5571 eOmc_joint_config_t jntCfg;
5573 if(!getJointConfiguration(joint, &jntCfg))
5578 *gearbox_E2J_ptr = jntCfg.gearbox_E2J;
5582bool embObjMotionControl::getJointEncTolerance(
int joint,
double *jEncTolerance_ptr)
5584 eOmc_joint_config_t jntCfg;
5586 if(!getJointConfiguration(joint, &jntCfg))
5591 *jEncTolerance_ptr = jntCfg.jntEncTolerance;
5595bool embObjMotionControl::getMotorEncTolerance(
int axis,
double *mEncTolerance_ptr)
5597 eOmc_motor_config_t motorCfg;
5598 if(!getMotorConfiguration(axis, &motorCfg))
5603 *mEncTolerance_ptr = motorCfg.rotEncTolerance;
5609 eOmc_motor_status_t status;
5611 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol,
5612 eoprot_entity_mc_motor, j,
5613 eoprot_tag_mc_motor_status);
5622 message =
"Could not retrieve the fault state.";
5626 if (status.mc_fault_state == EOERROR_CODE_DUMMY)
5628 fault = EOERROR_CODE_DUMMY;
5629 message =
"No fault detected.";
5634 fault = eoerror_code2value(status.mc_fault_state);
5635 message = eoerror_code2string(status.mc_fault_state);
5640bool embObjMotionControl::getRawData_core(std::string key, std::vector<std::int32_t> &
data)
5645 for(
int j=0; j< _njoints; j++)
5647 eOmc_joint_status_additionalInfo_t addinfo;
5648 eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, j, eoprot_tag_mc_joint_status_addinfo_multienc);
5653 for (
int k = 0; k < std::size(addinfo.multienc); k++)
5655 data.push_back((int32_t)addinfo.multienc[k]);
5664 for (
auto it = _rawValuesMetadataMap.begin(); it != _rawValuesMetadataMap.end(); it++)
5666 if(!getRawData_core(it->first, _rawDataAuxVector))
5668 yError() << getBoardInfo() <<
"getRawData failed. Cannot retrieve all raw data from local memory";
5671 map.insert({it->first, _rawDataAuxVector});
5679 if(_rawValuesMetadataMap.find(key) != _rawValuesMetadataMap.end())
5681 getRawData_core(key,
data);
5685 yError() << getBoardInfo() <<
"Request key:" << key <<
"is not available. Cannot retrieve get raw data.";
5694 for (
const auto &
p : _rawValuesMetadataMap)
5696 keys.push_back(
p.first);
5704 return _rawValuesMetadataMap.size();
5709 if (_rawValuesMetadataMap.empty())
5711 yError() << getBoardInfo() <<
"embObjMotionControl Map is empty. Closing...";
5720 if(_rawValuesMetadataMap.find(key) != _rawValuesMetadataMap.end())
5722 meta = _rawValuesMetadataMap[key];
5726 yError() << getBoardInfo() <<
"Requested key" << key <<
"is not available in the map. Closing...";
5737 if (_rawValuesMetadataMap.find(key) != _rawValuesMetadataMap.end())
5739 axesNames.assign(_rawValuesMetadataMap[key].axesNames.begin(), _rawValuesMetadataMap[key].axesNames.end());
5743 yError() << getBoardInfo() <<
"Requested key" << key <<
"is not available in the map. Exiting";
bool NOT_YET_IMPLEMENTED(const char *txt)
bool DEPRECATED(const char *txt)
#define MAX_POSITION_MOVE_INTERVAL
servMC_encoder_t * getEncoderAtMotor(int index)
bool parseService(yarp::os::Searchable &config, servConfigMais_t &maisconfig)
servMC_encoder_t * getEncoderAtJoint(int index)
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 const Properties & getProperties()=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 getRawData(std::string key, std::vector< std::int32_t > &data) override
virtual bool setPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, const double *limits) override
virtual bool setRefSpeedsRaw(const double *spds) override
virtual bool getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos, std::string &encoderTypeName) 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 getRawDataMap(std::map< std::string, std::vector< std::int32_t > > &map) 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 getMetadataMap(rawValuesKeyMetadataMap &metamap) 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 getKeys(std::vector< std::string > &keys) 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 int getNumberOfKeys() 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 getAxesNames(std::string key, std::vector< std::string > &axesNames) 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 getKeyMetadata(std::string key, rawValuesKeyMetadata &meta) 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 parseTimeoutsGroup(yarp::os::Searchable &config, std::vector< timeouts_t > &timeouts, int defaultTimeout)
bool parseMaintenanceModeGroup(yarp::os::Searchable &config, bool &skipRecalibrationEnabled)
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 parseLugreGroup(yarp::os::Searchable &config, std::vector< lugreParameters_t > &lugre)
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 parseCurrentLimits(yarp::os::Searchable &config, std::vector< motorCurrentLimits_t > &currLimits)
bool parseEncoderFactor(yarp::os::Searchable &config, double encoderFactor[])
bool parsefullscalePWM(yarp::os::Searchable &config, double dutycycleToPWM[])
bool parseRotorsLimits(yarp::os::Searchable &config, std::vector< rotorLimits_t > &rotorsLimits)
bool parseImpedanceGroup(yarp::os::Searchable &config, std::vector< impedanceParameters_t > &impedance)
bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector< kalmanFilterParams_t > &kalmanFilterParams)
bool parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J[], double gearbox_E2J[])
bool parseBehaviourFalgs(yarp::os::Searchable &config, bool &useRawEncoderData, bool &pwmIsLimited)
std::string usernamePidSelected
yarp::dev::PidFeedbackUnitsEnum fbk_PidUnits
eOmc_ctrl_out_type_t out_type
yarp::dev::PidOutputUnitsEnum out_PidUnits
static uint32_t idx[BOARD_NUM]
static bool nv_not_found(void)
static bool NOT_YET_IMPLEMENTED(const char *txt)
#define PARSER_MOTION_CONTROL_VERSION
static bool DEPRECATED(const char *txt)
const double jointWithAKSIM2
const double jointWithAEA3
const double jointWithAMO
const double jointWithAEA
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
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_appl_config_t txconfig
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
bool enableSkipRecalibration