25#define NEW_JSTATUS_STRUCT 1
26#define VELOCITY_WATCHDOG 0.1
27#define OPENLOOP_WATCHDOG 0.1
28#define PWM_CONSTANT 0.1
36 std::lock_guard lock(_mutex);
40 for (
int i=0;i <_njoints ;i++)
45 if (this->_command_speeds[i]!=0)
55 this->_command_speeds[i]=0.0;
61 if (this->refpwm[i]!=0)
76 pos[i] = _posDir_references[i];
80 pos[i] = _posCtrl_references[i];
124 std::ostringstream oss;
165 nominalCurrent.
resize(_njoints);
166 maxCurrent.
resize(_njoints);
167 peakCurrent.
resize(_njoints);
170 pwmLimit.
resize(_njoints);
171 supplyVoltage.
resize(_njoints);
172 last_velocity_command.
resize(_njoints);
173 last_pwm_command.
resize(_njoints);
184 nominalCurrent.
zero();
191 supplyVoltage.
zero();
195void FakeMotionControl::setInfoPPids(
int j)
197 _ppids[j].at(0).pidExtraInfo.description.pid_description =
"Position PID 0";
198 _ppids[j].at(0).pidExtraInfo.description.input_data_description =
"Encoder Value";
199 _ppids[j].at(0).pidExtraInfo.description.output_data_description =
"PWM";
201 _ppids[j].at(1).pidExtraInfo.description.pid_description =
"Position PID 1";
202 _ppids[j].at(1).pidExtraInfo.description.input_data_description =
"Encoder Value";
203 _ppids[j].at(1).pidExtraInfo.description.output_data_description =
"Velocity";
205 _ppids[j].at(2).pidExtraInfo.description.pid_description =
"Position PID 2";
206 _ppids[j].at(2).pidExtraInfo.description.input_data_description =
"Encoder Value";
207 _ppids[j].at(2).pidExtraInfo.description.output_data_description =
"Current";
210void FakeMotionControl::setInfoPDPids(
int j)
212 _pdpids[j].at(0).pidExtraInfo.description.pid_description =
"Position Direct PID 0";
213 _pdpids[j].at(0).pidExtraInfo.description.input_data_description =
"Encoder Value";
214 _pdpids[j].at(0).pidExtraInfo.description.output_data_description =
"PWM";
216 _pdpids[j].at(1).pidExtraInfo.description.pid_description =
"Position Direct PID 1";
217 _pdpids[j].at(1).pidExtraInfo.description.input_data_description =
"Encoder Value";
218 _pdpids[j].at(1).pidExtraInfo.description.output_data_description =
"Velocity";
220 _pdpids[j].at(2).pidExtraInfo.description.pid_description =
"Position Direct PID 2";
221 _pdpids[j].at(2).pidExtraInfo.description.input_data_description =
"Encoder Value";
222 _pdpids[j].at(2).pidExtraInfo.description.output_data_description =
"Current";
225void FakeMotionControl::setInfoTPids(
int j)
227 _tpids[j].at(0).pidExtraInfo.description.pid_description =
"Torque PID 0";
228 _tpids[j].at(0).pidExtraInfo.description.input_data_description =
"Torque";
229 _tpids[j].at(0).pidExtraInfo.description.output_data_description =
"PWM";
231 _tpids[j].at(1).pidExtraInfo.description.pid_description =
"Torque PID 1";
232 _tpids[j].at(1).pidExtraInfo.description.input_data_description =
"Torque";
233 _tpids[j].at(1).pidExtraInfo.description.output_data_description =
"Velocity";
235 _tpids[j].at(2).pidExtraInfo.description.pid_description =
"Torque PID 2";
236 _tpids[j].at(2).pidExtraInfo.description.input_data_description =
"Torque";
237 _tpids[j].at(2).pidExtraInfo.description.output_data_description =
"Current";
240void FakeMotionControl::setInfoVPids(
int j)
242 _vpids[j].at(0).pidExtraInfo.description.pid_description =
"Velocity PID 0";
243 _vpids[j].at(0).pidExtraInfo.description.input_data_description =
"Velocity Value";
244 _vpids[j].at(0).pidExtraInfo.description.output_data_description =
"PWM";
246 _vpids[j].at(1).pidExtraInfo.description.pid_description =
"Velocity PID 1";
247 _vpids[j].at(1).pidExtraInfo.description.input_data_description =
"Velocity Value";
248 _vpids[j].at(1).pidExtraInfo.description.output_data_description =
"PWM";
250 _vpids[j].at(2).pidExtraInfo.description.pid_description =
"Velocity PID 2";
251 _vpids[j].at(2).pidExtraInfo.description.input_data_description =
"Velocity Value";
252 _vpids[j].at(2).pidExtraInfo.description.output_data_description =
"Current";
255void FakeMotionControl::setInfoVDPids(
int j)
257 _vdpids[j].at(0).pidExtraInfo.description.pid_description =
"Velocity Direct PID 0";
258 _vdpids[j].at(0).pidExtraInfo.description.input_data_description =
"Velocity Value";
259 _vdpids[j].at(0).pidExtraInfo.description.output_data_description =
"PWM";
261 _vdpids[j].at(1).pidExtraInfo.description.pid_description =
"Velocity Direct PID 1";
262 _vdpids[j].at(1).pidExtraInfo.description.input_data_description =
"Velocity Value";
263 _vdpids[j].at(1).pidExtraInfo.description.output_data_description =
"PWM";
265 _vdpids[j].at(2).pidExtraInfo.description.pid_description =
"Velocity Direct PID 2";
266 _vdpids[j].at(2).pidExtraInfo.description.input_data_description =
"Velocity Value";
267 _vdpids[j].at(2).pidExtraInfo.description.output_data_description =
"Current";
270void FakeMotionControl::setInfoCPids(
int j)
272 _cpids[j].at(0).pidExtraInfo.description.pid_description =
"Current PID 0";
273 _cpids[j].at(0).pidExtraInfo.description.input_data_description =
"Current Value";
274 _cpids[j].at(0).pidExtraInfo.description.output_data_description =
"PWM";
276 _cpids[j].at(1).pidExtraInfo.description.pid_description =
"Current PID 1";
277 _cpids[j].at(1).pidExtraInfo.description.input_data_description =
"Current Value";
278 _cpids[j].at(1).pidExtraInfo.description.output_data_description =
"PWM";
280 _cpids[j].at(2).pidExtraInfo.description.pid_description =
"Current PID 2";
281 _cpids[j].at(2).pidExtraInfo.description.input_data_description =
"Current Value";
282 _cpids[j].at(2).pidExtraInfo.description.output_data_description =
"PWM";
285void FakeMotionControl::setInfoMPids(
int j)
287 _mpids[j].at(0).pidExtraInfo.description.pid_description =
"Mixed PID 0";
288 _mpids[j].at(0).pidExtraInfo.description.input_data_description =
"Encoder Value";
289 _mpids[j].at(0).pidExtraInfo.description.output_data_description =
"PWM";
291 _mpids[j].at(1).pidExtraInfo.description.pid_description =
"Mixed PID 1";
292 _mpids[j].at(1).pidExtraInfo.description.input_data_description =
"Encoder Value";
293 _mpids[j].at(1).pidExtraInfo.description.output_data_description =
"PWM";
295 _mpids[j].at(2).pidExtraInfo.description.pid_description =
"Current PID 2";
296 _mpids[j].at(2).pidExtraInfo.description.input_data_description =
"Encoder Value";
297 _mpids[j].at(2).pidExtraInfo.description.output_data_description =
"PWM";
376 for (
int i = 0; i < nj; ++i)
390 for (
int i = 0; i < nj; ++i)
417 _axisName =
new std::string[nj];
464bool FakeMotionControl::dealloc()
611 _angleToEncoder(nullptr),
612 _encodersStamp (nullptr),
613 _ampsToSensor (nullptr),
614 _dutycycleToPWM(nullptr),
615 _DEPRECATED_encoderconversionfactor (nullptr),
616 _DEPRECATED_encoderconversionoffset (nullptr),
617 _jointEncoderRes (nullptr),
618 _rotorEncoderRes (nullptr),
620 _hasHallSensor (nullptr),
621 _hasTempSensor (nullptr),
622 _hasRotorEncoder (nullptr),
623 _hasRotorEncoderIndex (nullptr),
624 _rotorIndexOffset (nullptr),
625 _motorPoles (nullptr),
626 _rotorlimits_max (nullptr),
627 _rotorlimits_min (nullptr),
629 _jointType (nullptr),
630 _limitsMin (nullptr),
631 _limitsMax (nullptr),
632 _kinematic_mj (nullptr),
633 _maxMotorVelocity (nullptr),
634 _velocityShifts (nullptr),
635 _velocityTimeout (nullptr),
638 _kbemf_scale (nullptr),
639 _ktau_scale (nullptr),
640 _viscousPos (nullptr),
641 _viscousNeg (nullptr),
642 _coulombPos (nullptr),
643 _coulombNeg (nullptr),
644 _velocityThres (nullptr),
645 _filterType (nullptr),
646 _torqueSensorId (nullptr),
647 _torqueSensorChan (nullptr),
648 _maxTorque (nullptr),
649 _newtonsToSensor (nullptr),
650 checking_motiondone (nullptr),
651 _last_position_move_time(nullptr),
652 _motorPwmLimits (nullptr),
654 useRawEncoderData (
false),
655 _pwmIsLimited (
false),
656 _torqueControlEnabled (
false),
657 _torqueControlUnits (T_MACHINE_UNITS),
658 _positionControlUnits (P_MACHINE_UNITS),
661 verbose (VERY_VERBOSE)
665 verbosewhenok = (
tmp !=
"") ? (
bool)yarp::conf::numeric::from_string<int>(
tmp) :
683 for(
int i=0; i<_njoints; i++)
686 pwmLimit[i] = (33+i)*10;
687 current[i] = (33+i)*100;
688 maxCurrent[i] = (33+i)*1000;
689 peakCurrent[i] = (33+i)*2;
690 nominalCurrent[i] = (33+i)*20;
691 supplyVoltage[i] = (33+i)*200;
692 last_velocity_command[i] = -1;
693 last_pwm_command[i] = -1;
695 _minJntCmdVelocity[i]=-50.0;
696 _maxJntCmdVelocity[i]=50.0;
724 for (
int i = 0; i < _njoints; i++) {
725 _newtonsToSensor[i] = 1;
739 for (
size_t i = 0; i < _njoints; i++) {
bemfToRaw [i] = _newtonsToSensor[i] / _angleToEncoder[i];}
740 for (
size_t i = 0; i < _njoints; i++) {
ktauToRaw[i] = _dutycycleToPWM[i] / _newtonsToSensor[i]; }
741 ControlBoardHelper cb(_njoints, _axisMap, _angleToEncoder,
nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM);
787 bool init = this->
start();
818 for (i = 0; i < _njoints; i++) {
822 for (i = 0; i < _njoints; i++) {
yDebug() <<
"_axisMap: " << _axisMap[i] <<
" "; }
833 for (i = 0; i < _njoints; i++) {
834 _axisName[_axisMap[i]] =
"joint" +
toString(i);
837 for (i = 0; i < _njoints; i++) {
yDebug() <<
"_axisName: " << _axisName[_axisMap[i]] <<
" "; }
847 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_REVOLUTE;
850 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_PRISMATIC;
854 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_UNKNOWN;
862 for (i = 0; i < _njoints; i++) {
863 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_REVOLUTE;
877 for (i = 0; i < _njoints; i++) {
878 _ampsToSensor[i] = 1;
893 for (i = 0; i < _njoints; i++) {
894 _dutycycleToPWM[i] = 1;
908 for (i = 0; i < _njoints; i++) {
909 _angleToEncoder[i] = 1; }
922 for (i = 0; i < _njoints; i++) {
936 for (i = 0; i < _njoints; i++) {
947 std::lock_guard lock(_mutex);
977void FakeMotionControl::cleanup()
987 if (verbose > VERY_VERY_VERBOSE) {
991 avail = _availablePids[j];
1001 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1002 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1003 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1004 _ppids[
jnt].at(index)=pid;
1006 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1007 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1008 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1009 _pdpids[
jnt].at(index)=pid;
1011 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1012 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1013 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1014 _vpids[
jnt].at(index)=pid;
1016 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1017 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1018 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1019 _vdpids[
jnt].at(index)=pid;
1021 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1022 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1023 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1024 _mpids[
jnt].at(index)=pid;
1026 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1027 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1028 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1029 _cpids[
jnt].at(index)=pid;
1031 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1032 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1033 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1034 _tpids[
jnt].at(index)=pid;
1038 return ReturnValue::return_code::return_value_error_method_failed;
1047 for(
int j=0; j< _njoints; j++)
1059 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1060 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1061 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1062 _ppids_ref[j].at(index)=ref;
1064 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1065 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1066 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1067 _pdpids_ref[j].at(index)=ref;
1069 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1070 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1071 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1072 _vpids_ref[j].at(index)=ref;
1074 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1075 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1076 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1077 _vdpids_ref[j].at(index)=ref;
1079 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1080 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1081 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1082 _mpids_ref[j].at(index)=ref;
1084 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1085 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1086 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1087 _cpids_ref[j].at(index)=ref;
1089 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1090 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1091 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1092 _tpids_ref[j].at(index)=ref;
1096 return ReturnValue::return_code::return_value_error_method_failed;
1105 for(
int j=0, index=0; j< _njoints; j++, index++)
1117 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1118 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1119 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1120 _ppids_lim[j].at(index)=limit;
1122 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1123 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1124 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1125 _pdpids_lim[j].at(index)=limit;
1127 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1128 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1129 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1130 _vpids_lim[j].at(index)=limit;
1132 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1133 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1134 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1135 _vdpids_lim[j].at(index)=limit;
1137 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1138 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1139 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1140 _mpids_lim[j].at(index)=limit;
1142 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1143 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1144 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1145 _cpids_lim[j].at(index)=limit;
1147 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1148 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1149 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1150 _tpids_lim[j].at(index)=limit;
1154 return ReturnValue::return_code::return_value_error_method_failed;
1163 for(
int j=0, index=0; j< _njoints; j++, index++)
1174 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1175 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1176 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1179 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1180 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1181 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1184 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1185 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1186 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1189 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1190 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1191 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1194 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1195 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1196 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1199 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1200 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1201 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1204 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1205 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1206 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1211 return ReturnValue::return_code::return_value_error_method_failed;
1220 for(
int j=0; j< _njoints; j++)
1232 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1233 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1234 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1235 *pid=_ppids[
jnt].at(index).pid;
1237 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1238 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1239 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1240 *pid=_pdpids[
jnt].at(index).pid;
1242 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1243 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1244 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1245 *pid=_vpids[
jnt].at(index).pid;
1247 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1248 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1249 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1250 *pid=_vdpids[
jnt].at(index).pid;
1252 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1253 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1254 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1255 *pid=_mpids[
jnt].at(index).pid;
1257 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1258 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1259 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1260 *pid=_cpids[
jnt].at(index).pid;
1262 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1263 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1264 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1265 *pid=_tpids[
jnt].at(index).pid;
1269 return ReturnValue::return_code::return_value_error_method_failed;
1280 for(
int j=0, index=0; j<_njoints; j++, index++)
1292 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1293 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1294 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1295 *ref=_ppids_ref[j].at(index);
1297 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1298 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1299 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1300 *ref=_pdpids_ref[j].at(index);
1302 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1303 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1304 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1305 *ref=_vpids_ref[j].at(index);
1307 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1308 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1309 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1310 *ref=_vdpids_ref[j].at(index);
1312 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1313 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1314 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1315 *ref=_mpids_ref[j].at(index);
1317 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1318 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1319 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1320 *ref=_cpids_ref[j].at(index);
1322 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1323 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1324 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1325 *ref=_tpids_ref[j].at(index);
1329 return ReturnValue::return_code::return_value_error_method_failed;
1341 for(
int j=0; j< _njoints; j++)
1353 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1354 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1355 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1356 *limit=_ppids_lim[j].at(index);
1358 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1359 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1360 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1361 *limit=_pdpids_lim[j].at(index);
1363 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1364 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1365 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1366 *limit=_vpids_lim[j].at(index);
1368 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1369 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1370 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1371 *limit=_vdpids_lim[j].at(index);
1373 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1374 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1375 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1376 *limit=_mpids_lim[j].at(index);
1378 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1379 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1380 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1381 *limit=_cpids_lim[j].at(index);
1383 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1384 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1385 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1386 *limit=_tpids_lim[j].at(index);
1390 return ReturnValue::return_code::return_value_error_method_failed;
1399 for(
int j=0, index=0; j<_njoints; j++, index++)
1416 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1417 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1418 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1419 _ppids_ena[j].at(index)=
false;
1421 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1422 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1423 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1424 _pdpids_ena[j].at(index)=
false;
1426 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1427 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1428 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1429 _vpids_ena[j].at(index)=
false;
1431 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1432 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1433 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1434 _vdpids_ena[j].at(index)=
false;
1436 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1437 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1438 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1439 _mpids_ena[j].at(index)=
false;
1441 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1442 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1443 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1444 _cpids_ena[j].at(index)=
false;
1446 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1447 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1448 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1449 _tpids_ena[j].at(index)=
false;
1453 return ReturnValue::return_code::return_value_error_method_failed;
1464 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1465 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1466 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1467 _ppids_ena[j].at(index)=
true;
1469 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1470 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1471 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1472 _pdpids_ena[j].at(index)=
true;
1474 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1475 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1476 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1477 _vpids_ena[j].at(index)=
true;
1479 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1480 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1481 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1482 _vdpids_ena[j].at(index)=
true;
1484 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1485 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1486 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1487 _mpids_ena[j].at(index)=
true;
1489 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1490 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1491 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1492 _cpids_ena[j].at(index)=
true;
1494 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1495 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1496 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1497 _tpids_ena[j].at(index)=
true;
1501 return ReturnValue::return_code::return_value_error_method_failed;
1513 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1514 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1515 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1516 _ppids[
jnt].at(index).pid.offset=v;
1518 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1519 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1520 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1521 _pdpids[
jnt].at(index).pid.offset=v;
1523 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1524 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1525 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1526 _vpids[
jnt].at(index).pid.offset=v;
1528 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1529 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1530 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1531 _vdpids[
jnt].at(index).pid.offset=v;
1533 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1534 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1535 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1536 _mpids[
jnt].at(index).pid.offset=v;
1538 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1539 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1540 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1541 _cpids[
jnt].at(index).pid.offset=v;
1543 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1544 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1545 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1546 _tpids[
jnt].at(index).pid.offset=v;
1550 return ReturnValue::return_code::return_value_error_method_failed;
1561 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1562 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1563 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1564 v = _ppids[
jnt].at(index).pid.offset;
1566 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1567 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1568 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1569 v = _pdpids[
jnt].at(index).pid.offset;
1571 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1572 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1573 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1574 v = _vpids[
jnt].at(index).pid.offset;
1576 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1577 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1578 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1579 v = _vdpids[
jnt].at(index).pid.offset;
1581 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1582 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1583 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1584 v = _mpids[
jnt].at(index).pid.offset;
1586 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1587 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1588 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1589 v = _cpids[
jnt].at(index).pid.offset;
1591 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1592 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1593 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1594 v = _tpids[
jnt].at(index).pid.offset;
1598 return ReturnValue::return_code::return_value_error_method_failed;
1611 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1612 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1613 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1614 _ppids_ffd[
jnt].at(index)=v;
1616 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1617 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1618 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1619 _pdpids_ffd[
jnt].at(index)=v;
1621 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1622 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1623 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1624 _vpids_ffd[
jnt].at(index)=v;
1626 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1627 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1628 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1629 _vdpids_ffd[
jnt].at(index)=v;
1631 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1632 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1633 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1634 _mpids_ffd[
jnt].at(index)=v;
1636 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1637 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1638 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1639 _cpids_ffd[
jnt].at(index)=v;
1641 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1642 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1643 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1644 _tpids_ffd[
jnt].at(index)=v;
1648 return ReturnValue::return_code::return_value_error_method_failed;
1659 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1660 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1661 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1662 v = _ppids_ffd[
jnt].at(index);
1664 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1665 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1666 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1667 v = _pdpids_ffd[
jnt].at(index);
1669 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1670 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1671 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1672 v = _vpids_ffd[
jnt].at(index);
1674 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1675 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1676 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1677 v = _vdpids_ffd[
jnt].at(index);
1679 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1680 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1681 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1682 v = _mpids_ffd[
jnt].at(index);
1684 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1685 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1686 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1687 v = _cpids_ffd[
jnt].at(index);
1689 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1690 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1691 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1692 v = _tpids_ffd[
jnt].at(index);
1696 return ReturnValue::return_code::return_value_error_method_failed;
1708 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1709 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1710 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1711 enabled=_ppids_ena[j].at(index);
1713 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1714 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1715 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1716 enabled=_pdpids_ena[j].at(index);
1718 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1719 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1720 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1721 enabled=_vpids_ena[j].at(index);
1723 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1724 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1725 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1726 enabled=_vdpids_ena[j].at(index);
1728 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1729 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1730 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1731 enabled=_mpids_ena[j].at(index);
1733 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1734 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1735 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1736 enabled=_cpids_ena[j].at(index);
1738 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1739 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1740 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1741 enabled=_tpids_ena[j].at(index);
1745 return ReturnValue::return_code::return_value_error_method_failed;
1766 for(
int j=0; j< _njoints; j++)
1786 yCError(
FAKEMOTIONCONTROL) <<
"velocityMoveRaw: skipping command because board " <<
" joint " << j <<
" is not in VOCAB_CM_VELOCITY mode";
1788 _command_speeds[j] = sp;
1797 for (
int i = 0; i < _njoints; i++) {
1838 if (verbose >= VERY_VERBOSE) {
1855 yCError(
FAKEMOTIONCONTROL) <<
"positionMoveRaw: skipping command because joint " << j <<
" is not in VOCAB_CM_POSITION mode";
1857 _posCtrl_references[j] = ref;
1863 if (verbose >= VERY_VERBOSE) {
1868 for(
int j=0, index=0; j< _njoints; j++, index++)
1877 if (verbose >= VERY_VERBOSE) {
1893 yCError(
FAKEMOTIONCONTROL) <<
"relativeMoveRaw: skipping command because joint " << j <<
" is not in VOCAB_CM_POSITION mode";
1895 _posCtrl_references[j] += delta;
1901 if (verbose >= VERY_VERBOSE) {
1906 for(
int j=0, index=0; j< _njoints; j++, index++)
1916 if (verbose >= VERY_VERBOSE) {
1926 if (verbose >= VERY_VERBOSE) {
1933 for(
int j=0, index=0; j< _njoints; j++, index++)
1947 _ref_speeds[index] = sp;
1955 for(
int j=0, index=0; j< _njoints; j++, index++)
1957 _ref_speeds[index] = spds[index];
1969 _ref_accs[j ] = 1
e6;
1971 else if (acc < -1
e6)
1973 _ref_accs[j ] = -1
e6;
1977 _ref_accs[j ] = acc;
1987 for(
int j=0, index=0; j< _njoints; j++, index++)
1991 _ref_accs[index] = 1
e6;
1993 else if (accs[j] < -1
e6)
1995 _ref_accs[index] = -1
e6;
1999 _ref_accs[index] = accs[j];
2007 *
spd = _ref_speeds[j];
2013 memcpy(spds, _ref_speeds,
sizeof(
double) * _njoints);
2019 *acc = _ref_accs[j];
2025 memcpy(accs, _ref_accs,
sizeof(
double) * _njoints);
2038 for(
int j=0; j< _njoints; j++)
2052 if (verbose >= VERY_VERBOSE) {
2071 if (verbose >= VERY_VERBOSE) {
2085 if (verbose >= VERY_VERBOSE) {
2093 for(
int j=0; j<joints.size(); j++)
2104 if (verbose >= VERY_VERBOSE) {
2118 if (verbose >= VERY_VERBOSE) {
2132 if (verbose >= VERY_VERBOSE) {
2146 if (verbose >= VERY_VERBOSE) {
2160 if (verbose >= VERY_VERBOSE) {
2167 ret = ret &&
stopRaw(joints[j]);
2178 if (verbose > VERY_VERY_VERBOSE) {
2182 avail = _availableControlModes[j];
2188 if (verbose > VERY_VERY_VERBOSE) {
2200 for(
int j=0; j< _njoints; j++)
2210 for(
int j=0; j< joints.size(); j++)
2219 if (verbose >= VERY_VERBOSE) {
2231 _posCtrl_references[j] = pos[j];
2238 if (verbose >= VERY_VERBOSE) {
2243 for(
int i=0; i<joints.size(); i++)
2252 if (verbose >= VERY_VERBOSE) {
2257 for(
int i=0; i<_njoints; i++)
2275 for(
int j=0; j< _njoints; j++)
2290 for(
int j=0; j< _njoints; j++)
2311 for(
int j=0; j< _njoints; j++)
2330 for(
int j=0; j< _njoints; j++)
2348 for(
int j=0; j< _njoints; j++)
2361 for (
int i = 0; i < _njoints; i++) {
2362 stamps[i] = _encodersStamp[i] = _cycleTimestamp;
2372 *stamp = _encodersStamp[j] = _cycleTimestamp;
2425 for(
int j=0; j< _njoints; j++)
2442 for(
int j=0; j< _njoints; j++)
2458 for(
int j=0; j< _njoints; j++)
2469 for (
int i = 0; i < _njoints; i++) {
2470 stamps[i] = _encodersStamp[i] = _cycleTimestamp;
2481 *stamp = _encodersStamp[m] = _cycleTimestamp;
2503 *value = current[j];
2511 for(
int j=0; j< _njoints; j++)
2520 maxCurrent[m] = val;
2526 *val = maxCurrent[m];
2532 (_enabledAmp[j ]) ? *
st = 1 : *
st = 0;
2539 for(
int j=0; j<_njoints; j++)
2541 sts[j] = _enabledAmp[j];
2548 *val = peakCurrent[m];
2554 peakCurrent[m] = val;
2560 *val = nominalCurrent[m];
2566 nominalCurrent[m] = val;
2590 *val = supplyVoltage[m];
2598 _limitsMin[j] = min;
2599 _limitsMax[j] = max;
2605 *min = _limitsMin[j];
2606 *max = _limitsMax[j];
2684 name = _axisName[j];
2690 type = _jointType[j];
2697 _minJntCmdVelocity[axis] = min;
2698 _maxJntCmdVelocity[axis] = max;
2704 *min = _minJntCmdVelocity[axis];
2705 *max = _maxJntCmdVelocity[axis];
2719 for (
int i = 0; i < _njoints; i++)
2736 for (
int j = 0; j < _njoints && ret; j++) {
2745 for (
int j = 0; j < _njoints && ret; j++) {
2756 if (t>1.0 || t< -1.0)
2759 _hwfault_code[j] = 1;
2760 _hwfault_message[j] =
"test" + std::to_string(j) +
" torque";
2770 for (
int j = 0; j <
n_joint && ret; j++) {
2779 for (
int j = 0; j < _njoints && ret; j++) {
2788 *t = _ref_torques[j];
2796 *stiffness = _stiffness[j];
2797 *damping = _damping[j];
2805 _stiffness[j] = stiffness;
2806 _damping[j] = damping;
2814 _force_offset[j] = offset;
2822 *offset = _force_offset[j];
2838 params->
bemf = _kbemf[j];
2840 params->
ktau = _ktau[j];
2861 _kbemf[j] = params.
bemf;
2862 _ktau[j] = params.
ktau;
2896 _posDir_references[j] = ref;
2904 _posDir_references[joints[i]] = refs[i];
2911 for(
int i=0; i< _njoints; i++)
2913 _posDir_references[i] = refs[i];
2921 if (verbose >= VERY_VERBOSE) {
2931 yCWarning(
FAKEMOTIONCONTROL) <<
"getTargetPosition: Joint " << axis <<
" is not in POSITION mode, therefore the value returned by " <<
2932 "this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
2934 *ref = _posCtrl_references[axis];
2941 for (
int i = 0; i < _njoints; i++) {
2950 for (
int i = 0; i<nj; i++)
2959 *ref = _command_speeds[axis];
2966 for (
int i = 0; i<_njoints; i++)
2976 for (
int i = 0; i<nj; i++)
2992 yCWarning(
FAKEMOTIONCONTROL) <<
"getRefPosition: Joint " << axis <<
" is not in POSITION_DIRECT mode, therefore the value returned by \
2993 this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
2996 *ref = _posDir_references[axis];
3004 for (
int i = 0; i<_njoints; i++)
3014 for (
int i = 0; i<nj; i++)
3025 if (verbose > VERY_VERY_VERBOSE) {
3035 for(
int j=0; j< joints.size(); j++)
3046 for (
int j = 0; j < _njoints; j++) {
3054 if (verbose >= VERY_VERBOSE) {
3058 _interactMode[j] =
_mode;
3067 for(
int i=0; i<joints.size(); i++)
3077 for(
int i=0; i<_njoints; i++)
3100 for(
int j=0; j< _njoints; j++)
3109 *
temp = _tempLimit[m];
3115 _tempLimit[m] =
temp;
3130 for (
int i = 0; i < _njoints; i++)
3145 for (
int i = 0; i < _njoints; i++)
3160 for (
int i = 0; i < _njoints; i++)
3190 for (
int i = 0; i < _njoints; i++)
3200 for (
int i = 0; i < _njoints; i++)
3202 _ref_currents[i] = t[i];
3203 current[i] = t[i] / 2;
3210 _ref_currents[j] = t;
3218 for (
int j = 0; j<
n_joint; j++)
3227 for (
int i = 0; i < _njoints; i++)
3229 t[i] = _ref_currents[i];
3236 *t = _ref_currents[j];
3252 for (
int i = 0; i < _njoints; i++)
3268 fault = _hwfault_code[j];
3269 message = _hwfault_message[j];
3282 _braked[j] = active;
3288 _autobraked[j] = enabled;
3294 enabled = _autobraked[j];
3303 return ReturnValue::return_code::return_value_error_method_failed;
3305 if (vel < -_maxJntCmdVelocity[
jnt] || vel > _maxJntCmdVelocity[
jnt]) {
3307 return ReturnValue::return_code::return_value_error_method_failed;
3310 _dir_vel_commands[
jnt] = vel;
3319 for (
int i = 0; i < _njoints; i++) {
3330 return ReturnValue::return_code::return_value_error_method_failed;
3333 for (
int i = 0; i < _njoints; i++) {
3344 return ReturnValue::return_code::return_value_error_method_failed;
3347 vel = _dir_vel_commands[
jnt];
3356 vels.resize(_njoints);
3357 for (
int i = 0; i < _njoints; i++) {
3368 return ReturnValue::return_code::return_value_error_method_failed;
3371 vels.resize(_njoints);
3372 for (
int i = 0; i < _njoints; i++) {
3387 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
3388 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
3389 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
3390 units=_ppids[j].at(index).pidExtraInfo;
3392 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
3393 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
3394 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
3395 units=_vpids[j].at(index).pidExtraInfo;
3397 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
3398 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
3399 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
3400 units=_cpids[j].at(index).pidExtraInfo;
3402 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
3403 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
3404 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
3405 units=_pdpids[j].at(index).pidExtraInfo;
3407 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
3408 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
3409 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
3410 units=_vdpids[j].at(index).pidExtraInfo;
3412 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
3413 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
3414 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
3415 units=_mpids[j].at(index).pidExtraInfo;
3417 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
3418 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
3419 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
3420 units=_tpids[j].at(index).pidExtraInfo;
3424 return ReturnValue::return_code::return_value_error_method_failed;
3436 units.resize(_njoints);
3437 for (
int i = 0; i < _njoints; i++) {
void checkAndDestroy(T *&p)
#define OPENLOOP_WATCHDOG
std::string toString(const T &value)
convert an arbitrary type to string.
#define VELOCITY_WATCHDOG
constexpr yarp::conf::vocab32_t VOCAB_CM_MIXED
constexpr yarp::conf::vocab32_t VOCAB_CM_TORQUE
constexpr yarp::conf::vocab32_t VOCAB_CM_PWM
constexpr yarp::conf::vocab32_t VOCAB_CM_HW_FAULT
constexpr yarp::conf::vocab32_t VOCAB_CM_VELOCITY_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_CM_IDLE
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION
constexpr yarp::conf::vocab32_t VOCAB_CM_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_CM_VELOCITY
#define YARP_METHOD_NOT_YET_IMPLEMENTED()
std::vector< double > m_LIMITS_Min
std::vector< std::string > m_GENERAL_AxisName
std::vector< int > m_GENERAL_Encoder
std::vector< double > m_LIMITS_Max
std::vector< std::string > m_GENERAL_AxisType
std::vector< int > m_GENERAL_AxisMap
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
std::vector< double > m_GENERAL_fullscalePWM
std::vector< double > m_GENERAL_ampsToSensor
yarp::dev::ReturnValue setPosLimitsRaw(int axis, double min, double max) override
Set the software limits for a particular axis, the behavior of the control card when these limits are...
virtual bool getHasTempSensorsRaw(int j, int &ret)
yarp::dev::ReturnValue setInteractionModesRaw(const std::vector< int > &joints, const std::vector< yarp::dev::InteractionModeEnum > &modes) override
Set the interaction mode of the robot for a set of joints, values can be stiff or compliant.
yarp::dev::ReturnValue getInteractionModeRaw(int j, yarp::dev::InteractionModeEnum &_mode) override
Get the current interaction mode of the robot, values can be stiff or compliant.
yarp::dev::ReturnValue getPosLimitsRaw(int axis, double *min, double *max) override
Get the software limits for a particular axis.
yarp::dev::ReturnValue getRefDutyCycleRaw(int j, double *v) override
Gets the last reference sent using the setRefDutyCycleRaw function.
yarp::dev::ReturnValue setMotorEncoderRaw(int m, const double val) override
Set the value of the motor encoder for a given motor.
yarp::dev::ReturnValue getMotorEncoderSpeedsRaw(double *spds) override
Read the instantaneous speed of all motor encoders.
yarp::dev::ReturnValue getPidOutputRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
yarp::dev::ReturnValue setPidReferenceRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
yarp::dev::ReturnValue getEncoderTimedRaw(int j, double *encs, double *stamp) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue velocityMoveRaw(int j, double sp) override
Start motion at a given speed, single joint.
yarp::dev::ReturnValue getPidOffsetRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double &v) override
Get the currently set offset value for the specified PID controller.
yarp::dev::ReturnValue setTemperatureLimitRaw(int m, const double temp) override
Set the temperature limit for a specific motor.
yarp::dev::ReturnValue getImpedanceRaw(int j, double *stiffness, double *damping) override
Get current impedance parameters (stiffness,damping,offset) for a specific joint.
yarp::dev::ReturnValue setImpedanceRaw(int j, double stiffness, double damping) override
Set current impedance parameters (stiffness,damping) for a specific joint.
yarp::dev::ReturnValue setVelLimitsRaw(int axis, double min, double max) override
Set the software speed limits for a particular axis, the behavior of the control card when these limi...
yarp::dev::ReturnValue isPidEnabledRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, bool &enabled) override
Get the current status (enabled/disabled) of the PID controller.
yarp::dev::ReturnValue getEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue setPidOffsetRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override
Set an offset value for a given controller.
yarp::dev::ReturnValue getInteractionModesRaw(const std::vector< int > &joints, std::vector< yarp::dev::InteractionModeEnum > &modes) override
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
yarp::dev::ReturnValue setAutoBrakeEnabledRaw(int j, bool enabled) override
virtual bool getJointEncoderTypeRaw(int j, int &type)
yarp::dev::ReturnValue getEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getTemperatureLimitRaw(int m, double *temp) override
Retrieves the current temperature limit for a specific motor.
yarp::dev::ReturnValue setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
Set new reference point for all axes.
yarp::dev::ReturnValue getMaxCurrentRaw(int j, double *val) override
Returns the maximum electric current allowed for a given motor.
yarp::dev::ReturnValue setTrajSpeedsRaw(const double *spds) override
Set reference speed on all joints.
yarp::dev::ReturnValue setPidFeedforwardRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override
Set the feedforward value for the specified PID controller.
yarp::dev::ReturnValue getPidFeedforwardRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double &v) override
Get the currently set feedforward value for the specified PID controller.
yarp::dev::ReturnValue getCurrentRangesRaw(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
yarp::dev::ReturnValue getTrajSpeedRaw(int j, double *ref) override
Get reference speed for a joint.
bool open(yarp::os::Searchable &par) override
Open the DeviceDriver.
yarp::dev::ReturnValue getImpedanceOffsetRaw(int j, double *offset) override
Get current force Offset for a specific joint.
yarp::dev::ReturnValue resetMotorEncoderRaw(int m) override
Reset motor encoder, single motor.
yarp::dev::ReturnValue getRefCurrentsRaw(double *t) override
Get the reference value of the currents for all motors.
yarp::dev::ReturnValue getVelLimitsRaw(int axis, double *min, double *max) override
Get the software speed limits for a particular axis.
virtual bool getHasHallSensorRaw(int j, int &ret)
yarp::dev::ReturnValue getEncoderSpeedsRaw(double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue disablePidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Disable the PID computation for a joint.
yarp::dev::ReturnValue getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
Read the instantaneous position of a motor encoder.
yarp::dev::ReturnValue setPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double limit) override
Set the error limit for the controller on a specific joint.
yarp::dev::ReturnValue getMotorEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous position of all motor encoders.
virtual bool initialised()
yarp::dev::ReturnValue setTrajAccelerationsRaw(const double *accs) override
Set reference acceleration on all joints.
yarp::dev::ReturnValue setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
bool threadInit() override
Initialization method.
yarp::dev::ReturnValue calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
Start calibration, this method is very often platform specific.
yarp::dev::ReturnValue getRefDutyCyclesRaw(double *v) override
Gets the last reference sent using the setRefDutyCyclesRaw function.
yarp::dev::VAS_status getVirtualAnalogSensorStatusRaw(int ch) override
Check the status of a given channel.
yarp::dev::ReturnValue positionMoveRaw(int j, double ref) override
Set new reference point for a single axis.
yarp::dev::ReturnValue getMotorEncodersRaw(double *encs) override
Read the position of all motor encoders.
yarp::dev::ReturnValue getTorqueRaw(int j, double *t) override
Get the value of the torque on a given joint (this is the feedback if you have a torque sensor).
yarp::dev::ReturnValue getLastJointFaultRaw(int j, int &fault, std::string &message) override
yarp::dev::ReturnValue setRefDutyCycleRaw(int j, double v) override
Sets the reference dutycycle of a single motor.
yarp::dev::ReturnValue setRefTorquesRaw(const double *t) override
Set the reference value of the torque for all joints.
int getVirtualAnalogSensorChannelsRaw() override
Get the number of channels of the virtual sensor.
yarp::dev::ReturnValue setMotorTorqueParamsRaw(int j, const yarp::dev::MotorTorqueParameters params) override
Set the motor parameters.
yarp::dev::ReturnValue getTrajSpeedsRaw(double *spds) override
Get reference speed of all joints.
yarp::dev::ReturnValue getTorqueRangesRaw(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
yarp::dev::ReturnValue setRefDutyCyclesRaw(const double *v) override
Sets the reference dutycycle for all motors.
yarp::dev::ReturnValue resetEncodersRaw() override
Reset encoders.
yarp::dev::ReturnValue getPidReferenceRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *ref) override
Get the current reference of the PID controller for a specific joint.
yarp::dev::ReturnValue getMotorEncoderAccelerationRaw(int m, double *spds) override
Read the instantaneous acceleration of a motor encoder.
yarp::dev::ReturnValue getTorqueRangeRaw(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
yarp::dev::ReturnValue getMotorTorqueParamsRaw(int j, yarp::dev::MotorTorqueParameters *params) override
Get the motor parameters.
yarp::dev::ReturnValue setRefTorqueRaw(int j, double t) override
Set the reference value of the torque for a given joint.
yarp::dev::ReturnValue setEncoderRaw(int j, double val) override
Set the value of the encoder for a given joint.
bool fromConfig(yarp::os::Searchable &config)
yarp::dev::ReturnValue getPidErrorsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
yarp::dev::ReturnValue setManualBrakeActiveRaw(int j, bool active) override
yarp::dev::ReturnValue setRefVelocityRaw(int jnt, double vel) override
Set the velocity of single joint.
yarp::dev::ReturnValue getRefTorquesRaw(double *t) override
Get the reference value of the torque for all joints.
yarp::dev::ReturnValue getAmpStatusRaw(int *st) override
yarp::dev::ReturnValue setMotorEncodersRaw(const double *vals) override
Set the value of all motor encoders.
yarp::dev::ReturnValue getRefPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getControlModeRaw(int j, yarp::dev::ControlModeEnum &mode) override
yarp::dev::ReturnValue setPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum &pidtype, const double *limits) override
Get the error limit for the controller on all joints.
yarp::dev::ReturnValue getTargetVelocityRaw(const int joint, double *ref) override
Get the last reference speed set by velocityMove for single joint.
virtual bool getHasRotorEncoderIndexRaw(int j, int &ret)
bool updateVirtualAnalogSensorMeasureRaw(yarp::sig::Vector &measure) override
Set a vector of torque values for virtual sensor.
yarp::dev::ReturnValue setTrajAccelerationRaw(int j, double acc) override
Set reference acceleration for a joint.
yarp::dev::ReturnValue getPidOutputsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
void resizeBuffers()
Resize previously allocated buffers.
yarp::dev::ReturnValue getGearboxRatioRaw(int m, double *gearbox) override
Get the gearbox ratio for a specific motor.
yarp::dev::ReturnValue isJointBrakedRaw(int j, bool &braked) const override
yarp::dev::ReturnValue getDutyCyclesRaw(double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
yarp::dev::ReturnValue getTargetPositionsRaw(double *refs) override
Get the last position reference for all axes.
void threadRelease() override
Release method.
yarp::dev::ReturnValue getCurrentsRaw(double *vals) override
yarp::dev::ReturnValue getEncodersRaw(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue getPidExtraInfosRaw(const yarp::dev::PidControlTypeEnum &pidtype, std::vector< yarp::dev::PidExtraInfo > &units) override
Get PID extra info and measurement units for a specific joint.
virtual bool getMotorPolesRaw(int j, int &poles)
yarp::dev::ReturnValue getTemperaturesRaw(double *vals) override
Get temperature of all the motors.
yarp::dev::ReturnValue getTargetPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getRefVelocityRaw(const int jnt, double &vel) override
Get the last reference velocity set by setRefVelocity() for a single joint.
yarp::dev::ReturnValue getNumberOfMotorsRaw(int *num) override
Get the number of available motors.
yarp::dev::ReturnValue setPidReferencesRaw(const yarp::dev::PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
yarp::dev::ReturnValue getEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of an axis.
yarp::dev::ReturnValue setEncodersRaw(const double *vals) override
Set the value of all encoders.
virtual bool getRotorIndexOffsetRaw(int j, double &rotorOffset)
yarp::dev::ReturnValue setRefCurrentRaw(int j, double t) override
Set the reference value of the current for a single motor.
yarp::dev::ReturnValue getTargetVelocitiesRaw(double *refs) override
Get the last reference speed set by velocityMove for all joints.
yarp::dev::ReturnValue checkMotionDoneRaw(bool &flag) override
Check if the current trajectory all joints is terminated.
yarp::dev::ReturnValue getDutyCycleRaw(int j, double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
yarp::dev::ReturnValue getMotorEncoderRaw(int m, double *v) override
Read the value of a motor encoder.
yarp::dev::ReturnValue getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
Get the current impedance limits for a specific joint.
yarp::dev::ReturnValue getRefCurrentRaw(int j, double *t) override
Get the reference value of the current for a single motor.
yarp::dev::ReturnValue getPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::Pid *pid) override
Get current PID value for a specific joint.
yarp::dev::ReturnValue getAutoBrakeEnabledRaw(int j, bool &enabled) const override
yarp::dev::ReturnValue getPowerSupplyVoltageRaw(int j, double *val) override
yarp::dev::ReturnValue setPWMLimitRaw(int j, const double val) override
virtual bool getKinematicMJRaw(int j, double &rotres)
yarp::dev::ReturnValue getRefTorqueRaw(int j, double *t) override
Set the reference value of the torque for a given joint.
virtual bool getJointEncoderResolutionRaw(int m, double &jntres)
yarp::dev::ReturnValue getPidReferencesRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all PID controllers.
yarp::dev::ReturnValue disableAmpRaw(int j) override
Disable the amplifier on a specific joint.
yarp::dev::ReturnValue setImpedanceOffsetRaw(int j, double offset) override
Set current force Offset for a specific joint.
yarp::dev::ReturnValue setCalibrationParametersRaw(int axis, const yarp::dev::CalibrationParameters ¶ms) override
Start calibration, this method is very often platform specific.
yarp::dev::ReturnValue resetPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Reset the controller of a given joint, usually sets the current status of the joint as the reference ...
bool alloc(int njoints)
Allocated buffers.
yarp::dev::ReturnValue getNumberOfMotorEncodersRaw(int *num) override
Get the number of available motor encoders.
yarp::dev::ReturnValue setPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, const yarp::dev::Pid &pid) override
Set new PID value for a joint axis.
yarp::dev::ReturnValue getCurrentRaw(int j, double *val) override
yarp::dev::ReturnValue getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
yarp::dev::ReturnValue setPeakCurrentRaw(int m, const double val) override
virtual bool getRotorEncoderTypeRaw(int j, int &type)
yarp::dev::ReturnValue getEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getNominalCurrentRaw(int m, double *val) override
yarp::dev::ReturnValue calibrationDoneRaw(int j) override
Check if the calibration is terminated, on a particular joint.
yarp::dev::ReturnValue getPidErrorRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
yarp::dev::ReturnValue getTrajAccelerationRaw(int j, double *acc) override
Get reference acceleration for a joint.
yarp::dev::ReturnValue resetEncoderRaw(int j) override
Reset encoder, single joint.
yarp::dev::ReturnValue getTemperatureRaw(int m, double *val) override
Get temperature of a motor.
yarp::dev::ReturnValue getPidExtraInfoRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::PidExtraInfo &units) override
Get PID extra info and measurement units for a specific joint.
yarp::dev::ReturnValue setPidsRaw(const yarp::dev::PidControlTypeEnum &pidtype, const yarp::dev::Pid *pids) override
Set new PID value on multiple axes.
yarp::dev::ReturnValue setMaxCurrentRaw(int j, double val) override
yarp::dev::ReturnValue setControlModeRaw(int j, yarp::dev::SelectableControlModeEnum mode) override
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue relativeMoveRaw(int j, double delta) override
Set relative position.
yarp::dev::ReturnValue getMotorEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all motor encoders.
yarp::dev::ReturnValue getMotorEncoderSpeedRaw(int m, double *sp) override
Read the instantaneous speed of a motor encoder.
yarp::dev::ReturnValue resetMotorEncodersRaw() override
Reset motor encoders.
yarp::dev::ReturnValue getCurrentRangeRaw(int j, double *min, double *max) override
Get the full scale of the current measurement for a given motor (e.g.
yarp::dev::ReturnValue getMotorEncoderCountsPerRevolutionRaw(int m, double *v) override
Gets number of counts per revolution for motor encoder m.
yarp::dev::ReturnValue enablePidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Enable the PID computation for a joint.
yarp::dev::ReturnValue getRefPositionsRaw(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue setTrajSpeedRaw(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
yarp::dev::ReturnValue getPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *limit) override
Get the error limit for the controller on a specific joint.
yarp::dev::ReturnValue setNominalCurrentRaw(int m, const double val) override
yarp::dev::ReturnValue setControlModesRaw(const std::vector< int > &joints, const std::vector< yarp::dev::SelectableControlModeEnum > &modes) override
yarp::dev::ReturnValue setRefCurrentsRaw(const double *t) override
Set the reference value of the currents for all motors.
yarp::dev::ReturnValue getPWMRaw(int j, double *val) override
yarp::dev::ReturnValue setInteractionModeRaw(int j, yarp::dev::InteractionModeEnum _mode) override
Set the interaction mode of the robot, values can be stiff or compliant.
yarp::dev::ReturnValue getTorquesRaw(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
yarp::dev::ReturnValue getAvailableControlModesRaw(int j, std::vector< yarp::dev::SelectableControlModeEnum > &avail) override
yarp::dev::ReturnValue enableAmpRaw(int j) override
Enable the amplifier on a specific joint.
yarp::dev::ReturnValue getPeakCurrentRaw(int m, double *val) override
yarp::dev::ReturnValue getControlModesRaw(const std::vector< int > &joints, std::vector< yarp::dev::ControlModeEnum > &modes) override
yarp::dev::ReturnValue setGearboxRatioRaw(int m, const double val) override
Set the gearbox ratio for a specific motor.
virtual bool getRotorEncoderResolutionRaw(int m, double &rotres)
virtual bool getHasRotorEncoderRaw(int j, int &ret)
yarp::dev::ReturnValue getPidsRaw(const yarp::dev::PidControlTypeEnum &pidtype, yarp::dev::Pid *pids) override
Get current PID value for a specific joint.
yarp::dev::ReturnValue getPWMLimitRaw(int j, double *val) override
yarp::dev::ReturnValue getAxisNameRaw(int axis, std::string &name) override
void run() override
Loop function.
yarp::dev::ReturnValue stopRaw() override
Stop motion, multiple joints.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue getEncoderRaw(int j, double *v) override
Read the value of an encoder.
virtual bool getTorqueControlFilterType(int j, int &type)
yarp::dev::ReturnValue getTrajAccelerationsRaw(double *accs) override
Get reference acceleration of all joints.
yarp::dev::ReturnValue getAvailablePidsRaw(int j, std::vector< yarp::dev::PidControlTypeEnum > &avail) override
Return a list of available pid types mode for the given joint.
yarp::dev::ReturnValue setPositionRaw(int j, double ref) override
Set new position for a single axis.
yarp::dev::ReturnValue getPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *limits) override
Get the error limit for all controllers.
bool initialize(int size, const int *amap, const double *enc, const double *zos, const double *ampereFactor=NULL, const double *voltFactor=NULL)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
class ImplementControlLimits; class StubImplControlLimitsRaw;
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool initialize(int k, const int *amap)
bool initialize(int size, const int *amap, const double *ampsToSens)
Initialize the internal data and alloc memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos, const double *nw)
Initialize the internal data and alloc memory.
Interface settings the way the robot interacts with the environment: basic interaction types are Stif...
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory, smaller version.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool initialize(int k, const int *amap, const double *dutyToPWM)
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos, const double *newtons, const double *amps, const double *dutys)
Initialize the internal data and alloc memory.
bool setConversionUnits(const PidControlTypeEnum &pidtype, const PidFeedbackUnitsEnum fbk_conv_units, const PidOutputUnitsEnum out_conv_units)
Default implementation of the IPositionControl interface.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
Default implementation of the IPositionDirect interface.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos, const double *nw, const double *amps, const double *dutys, const double *bemfs, const double *ktaus)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool initialize(int k, const int *amap, const double *userToRaw)
double kp
proportional gain
Contains the parameters for a PID.
A mini-server for performing network communication in the background.
An abstraction for a periodic thread.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A base class for nested structures that can be searched.
void zero()
Set to zero the elements of the vector.
void resize(size_t size)
Resize the vector.
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
std::string get_string(const std::string &key, bool *found=nullptr)
Read a string from an environment variable.
For streams capable of holding different kinds of content, check what they actually have.
SelectableControlModeEnum
@ VOCAB_CM_VELOCITY_DIRECT
@ VOCAB_CM_POSITION_DIRECT
size_t PidControlTypeEnum2Index(PidControlTypeEnum type)
@ VOCAB_PIDTYPE_VELOCITY_DIRECT_2
@ VOCAB_PIDTYPE_VELOCITY_3
@ VOCAB_PIDTYPE_POSITION_DIRECT_1
@ VOCAB_PIDTYPE_CURRENT_3
@ VOCAB_PIDTYPE_CURRENT_1
@ VOCAB_PIDTYPE_VELOCITY_2
@ VOCAB_PIDTYPE_CURRENT_2
@ VOCAB_PIDTYPE_VELOCITY_DIRECT_1
@ VOCAB_PIDTYPE_POSITION_DIRECT_3
@ VOCAB_PIDTYPE_VELOCITY_1
@ VOCAB_PIDTYPE_VELOCITY_DIRECT_3
@ VOCAB_PIDTYPE_POSITION_1
@ VOCAB_PIDTYPE_POSITION_DIRECT_2
@ VOCAB_PIDTYPE_POSITION_2
@ VOCAB_PIDTYPE_POSITION_3
@ VOCAB_CM_POSITION_DIRECT
double now()
Return the current time in seconds, relative to an arbitrary starting point.
std::string decode(NetInt32 code)
Convert a vocabulary identifier into a string.
The components from which ports and connections are built.
An interface to the operating system, including Port based communication.