27 #define NEW_JSTATUS_STRUCT 1
28 #define VELOCITY_WATCHDOG 0.1
29 #define OPENLOOP_WATCHDOG 0.1
30 #define PWM_CONSTANT 0.1
38 for (
int i=0;i <_njoints ;i++)
43 if (this->_command_speeds[i]!=0)
46 double increment = _command_speeds[i]*elapsed;
53 this->_command_speeds[i]=0.0;
59 if (this->refpwm[i]!=0)
74 pos[i] = _posDir_references[i];
78 pos[i] = _posCtrl_references[i];
108 yCError(FAKEMOTIONCONTROL) << txt <<
"is not yet implemented for FakeMotionControl";
114 yCError(FAKEMOTIONCONTROL) << txt <<
"has been deprecated for FakeMotionControl";
127 std::ostringstream oss;
134 bool FakeMotionControl::extractGroup(
Bottle &input,
Bottle &out,
const std::string &key1,
const std::string &txt,
int size)
141 yCError(FAKEMOTIONCONTROL) << key1.c_str() <<
"parameter not found";
145 if(tmp.
size()!=(
size_t) size)
147 yCError(FAKEMOTIONCONTROL) << key1.c_str() <<
"incorrect number of entries";
157 pos.resize(_njoints);
158 dpos.resize(_njoints);
159 vel.resize(_njoints);
160 speed.resize(_njoints);
161 acc.resize(_njoints);
162 loc.resize(_njoints);
163 amp.resize(_njoints);
165 current.resize(_njoints);
166 nominalCurrent.resize(_njoints);
167 maxCurrent.resize(_njoints);
168 peakCurrent.resize(_njoints);
169 pwm.resize(_njoints);
170 refpwm.resize(_njoints);
171 pwmLimit.resize(_njoints);
172 supplyVoltage.resize(_njoints);
173 last_velocity_command.resize(_njoints);
174 last_pwm_command.resize(_njoints);
185 nominalCurrent.zero();
192 supplyVoltage.zero();
197 _axisMap = allocAndCheck<int>(nj);
198 _controlModes = allocAndCheck<int>(nj);
199 _interactMode = allocAndCheck<int>(nj);
200 _angleToEncoder = allocAndCheck<double>(nj);
201 _dutycycleToPWM = allocAndCheck<double>(nj);
202 _ampsToSensor = allocAndCheck<double>(nj);
203 _encodersStamp = allocAndCheck<double>(nj);
204 _DEPRECATED_encoderconversionoffset = allocAndCheck<float>(nj);
205 _DEPRECATED_encoderconversionfactor = allocAndCheck<float>(nj);
208 _jointEncoderRes = allocAndCheck<int>(nj);
209 _rotorEncoderRes = allocAndCheck<int>(nj);
210 _gearbox = allocAndCheck<double>(nj);
211 _torqueSensorId= allocAndCheck<int>(nj);
212 _torqueSensorChan= allocAndCheck<int>(nj);
213 _maxTorque=allocAndCheck<double>(nj);
214 _torques = allocAndCheck<double>(nj);
215 _maxJntCmdVelocity = allocAndCheck<double>(nj);
216 _maxMotorVelocity = allocAndCheck<double>(nj);
217 _newtonsToSensor=allocAndCheck<double>(nj);
218 _hasHallSensor = allocAndCheck<bool>(nj);
219 _hasTempSensor = allocAndCheck<bool>(nj);
220 _hasRotorEncoder = allocAndCheck<bool>(nj);
221 _hasRotorEncoderIndex = allocAndCheck<bool>(nj);
222 _rotorIndexOffset = allocAndCheck<int>(nj);
223 _motorPoles = allocAndCheck<int>(nj);
224 _rotorlimits_max = allocAndCheck<double>(nj);
225 _rotorlimits_min = allocAndCheck<double>(nj);
226 _hwfault_code = allocAndCheck<int>(nj);
227 _hwfault_message = allocAndCheck<std::string>(nj);
230 _ppids=allocAndCheck<Pid>(nj);
231 _tpids=allocAndCheck<Pid>(nj);
232 _cpids = allocAndCheck<Pid>(nj);
233 _vpids = allocAndCheck<Pid>(nj);
234 _ppids_ena=allocAndCheck<bool>(nj);
235 _tpids_ena=allocAndCheck<bool>(nj);
236 _cpids_ena = allocAndCheck<bool>(nj);
237 _vpids_ena = allocAndCheck<bool>(nj);
238 _ppids_lim=allocAndCheck<double>(nj);
239 _tpids_lim=allocAndCheck<double>(nj);
240 _cpids_lim = allocAndCheck<double>(nj);
241 _vpids_lim = allocAndCheck<double>(nj);
242 _ppids_ref=allocAndCheck<double>(nj);
243 _tpids_ref=allocAndCheck<double>(nj);
244 _cpids_ref = allocAndCheck<double>(nj);
245 _vpids_ref = allocAndCheck<double>(nj);
249 _axisName =
new std::string[nj];
252 _limitsMax=allocAndCheck<double>(nj);
253 _limitsMin=allocAndCheck<double>(nj);
254 _kinematic_mj=allocAndCheck<double>(16);
256 _motorPwmLimits=allocAndCheck<double>(nj);
257 checking_motiondone=allocAndCheck<bool>(nj);
259 _velocityShifts=allocAndCheck<int>(nj);
260 _velocityTimeout=allocAndCheck<int>(nj);
261 _kbemf=allocAndCheck<double>(nj);
262 _ktau=allocAndCheck<double>(nj);
263 _kbemf_scale = allocAndCheck<int>(nj);
264 _ktau_scale = allocAndCheck<int>(nj);
265 _filterType=allocAndCheck<int>(nj);
266 _last_position_move_time=allocAndCheck<double>(nj);
269 _posCtrl_references = allocAndCheck<double>(nj);
270 _posDir_references = allocAndCheck<double>(nj);
271 _command_speeds = allocAndCheck<double>(nj);
272 _ref_speeds = allocAndCheck<double>(nj);
273 _ref_accs = allocAndCheck<double>(nj);
274 _ref_torques = allocAndCheck<double>(nj);
275 _ref_currents = allocAndCheck<double>(nj);
276 _enabledAmp = allocAndCheck<bool>(nj);
277 _enabledPid = allocAndCheck<bool>(nj);
278 _calibrated = allocAndCheck<bool>(nj);
286 bool FakeMotionControl::dealloc()
391 _angleToEncoder(nullptr),
392 _encodersStamp (nullptr),
393 _ampsToSensor (nullptr),
394 _dutycycleToPWM(nullptr),
395 _DEPRECATED_encoderconversionfactor (nullptr),
396 _DEPRECATED_encoderconversionoffset (nullptr),
397 _jointEncoderRes (nullptr),
398 _rotorEncoderRes (nullptr),
400 _hasHallSensor (nullptr),
401 _hasTempSensor (nullptr),
402 _hasRotorEncoder (nullptr),
403 _hasRotorEncoderIndex (nullptr),
404 _rotorIndexOffset (nullptr),
405 _motorPoles (nullptr),
406 _rotorlimits_max (nullptr),
407 _rotorlimits_min (nullptr),
412 _ppids_ena (nullptr),
413 _tpids_ena (nullptr),
414 _cpids_ena (nullptr),
415 _vpids_ena (nullptr),
416 _ppids_lim (nullptr),
417 _tpids_lim (nullptr),
418 _cpids_lim (nullptr),
419 _vpids_lim (nullptr),
420 _ppids_ref (nullptr),
421 _tpids_ref (nullptr),
422 _cpids_ref (nullptr),
423 _vpids_ref (nullptr),
425 _jointType (nullptr),
426 _limitsMin (nullptr),
427 _limitsMax (nullptr),
428 _kinematic_mj (nullptr),
429 _maxJntCmdVelocity (nullptr),
430 _maxMotorVelocity (nullptr),
431 _velocityShifts (nullptr),
432 _velocityTimeout (nullptr),
435 _kbemf_scale (nullptr),
436 _ktau_scale (nullptr),
437 _filterType (nullptr),
438 _torqueSensorId (nullptr),
439 _torqueSensorChan (nullptr),
440 _maxTorque (nullptr),
441 _newtonsToSensor (nullptr),
442 checking_motiondone (nullptr),
443 _last_position_move_time(nullptr),
444 _motorPwmLimits (nullptr),
446 useRawEncoderData (false),
447 _pwmIsLimited (false),
448 _torqueControlEnabled (false),
449 _torqueControlUnits (T_MACHINE_UNITS),
450 _positionControlUnits (P_MACHINE_UNITS),
453 verbose (VERY_VERBOSE)
457 verbosewhenok = (tmp !=
"") ? (
bool)yarp::conf::numeric::from_string<int>(tmp) :
475 for(
int i=0; i<_njoints; i++)
478 pwmLimit[i] = (33+i)*10;
479 current[i] = (33+i)*100;
480 maxCurrent[i] = (33+i)*1000;
481 peakCurrent[i] = (33+i)*2;
482 nominalCurrent[i] = (33+i)*20;
483 supplyVoltage[i] = (33+i)*200;
484 last_velocity_command[i] = -1;
485 last_pwm_command[i] = -1;
487 _maxJntCmdVelocity[i]=50.0;
529 yCTrace(FAKEMOTIONCONTROL) << str;
534 _njoints = config.
findGroup(
"GENERAL").
check(
"Joints",
Value(1),
"Number of degrees of freedom").asInt32();
535 yCInfo(FAKEMOTIONCONTROL,
"Using %d joint%s", _njoints, ((_njoints != 1) ?
"s" :
""));
539 yCError(FAKEMOTIONCONTROL) <<
"Malloc failed";
544 for (
int i = 0; i < _njoints; i++) {
545 _newtonsToSensor[i] = 1;
550 yCError(FAKEMOTIONCONTROL) <<
"Missing parameters in config file";
562 ControlBoardHelper cb(_njoints, _axisMap, _angleToEncoder,
nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM);
564 ImplementControlCalibration::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr);
565 ImplementAmplifierControl::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr);
566 ImplementEncodersTimed::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr);
567 ImplementMotorEncoders::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr);
568 ImplementPositionControl::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr);
569 ImplementPidControl::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM);
572 ImplementControlMode::initialize(_njoints, _axisMap);
573 ImplementVelocityControl::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr);
574 ImplementControlLimits::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr);
575 ImplementImpedanceControl::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr, _newtonsToSensor);
576 ImplementTorqueControl::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM, bemfToRaw.
data(), ktauToRaw.
data());
577 ImplementPositionDirect::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr);
578 ImplementInteractionMode::initialize(_njoints, _axisMap, _angleToEncoder,
nullptr);
579 ImplementMotor::initialize(_njoints, _axisMap);
580 ImplementAxisInfo::initialize(_njoints, _axisMap);
581 ImplementPWMControl::initialize(_njoints, _axisMap, _dutycycleToPWM);
582 ImplementCurrentControl::initialize(_njoints, _axisMap, _ampsToSensor);
583 ImplementVirtualAnalogSensor::initialize(_njoints, _axisMap, _newtonsToSensor);
584 ImplementJointFault::initialize(_njoints, _axisMap);
587 bool init = this->
start();
590 yCError(FAKEMOTIONCONTROL) <<
"open() has an error in call of FakeMotionControl::init() for board" ;
597 yCDebug(FAKEMOTIONCONTROL) <<
"init() has successfully initted board ";
610 if (!extractGroup(pidsGroup, xtmp,
"stiffness",
"stiffness parameter", _njoints)) {
613 for (j=0; j<_njoints; j++) {
617 if (!extractGroup(pidsGroup, xtmp,
"damping",
"damping parameter", _njoints)) {
620 for (j=0; j<_njoints; j++) {
627 bool FakeMotionControl::parsePositionPidsGroup(
Bottle& pidsGroup,
Pid myPid[])
632 if (!extractGroup(pidsGroup, xtmp,
"kp",
"Pid kp parameter", _njoints)) {
635 for (j=0; j<_njoints; j++) {
639 if (!extractGroup(pidsGroup, xtmp,
"kd",
"Pid kd parameter", _njoints)) {
642 for (j=0; j<_njoints; j++) {
646 if (!extractGroup(pidsGroup, xtmp,
"ki",
"Pid kp parameter", _njoints)) {
649 for (j=0; j<_njoints; j++) {
653 if (!extractGroup(pidsGroup, xtmp,
"maxInt",
"Pid maxInt parameter", _njoints)) {
656 for (j=0; j<_njoints; j++) {
660 if (!extractGroup(pidsGroup, xtmp,
"maxPwm",
"Pid maxPwm parameter", _njoints)) {
663 for (j=0; j<_njoints; j++) {
667 if (!extractGroup(pidsGroup, xtmp,
"shift",
"Pid shift parameter", _njoints)) {
670 for (j=0; j<_njoints; j++) {
674 if (!extractGroup(pidsGroup, xtmp,
"ko",
"Pid ko parameter", _njoints)) {
677 for (j=0; j<_njoints; j++) {
681 if (!extractGroup(pidsGroup, xtmp,
"stictionUp",
"Pid stictionUp", _njoints)) {
684 for (j=0; j<_njoints; j++) {
688 if (!extractGroup(pidsGroup, xtmp,
"stictionDwn",
"Pid stictionDwn", _njoints)) {
691 for (j=0; j<_njoints; j++) {
695 if (!extractGroup(pidsGroup, xtmp,
"kff",
"Pid kff parameter", _njoints)) {
698 for (j=0; j<_njoints; j++) {
703 if (_positionControlUnits==P_METRIC_UNITS)
705 for (j=0; j<_njoints; j++)
707 myPid[j].
kp = myPid[j].
kp / _angleToEncoder[j];
708 myPid[j].
ki = myPid[j].
ki / _angleToEncoder[j];
709 myPid[j].
kd = myPid[j].
kd / _angleToEncoder[j];
720 if (!extractGroup(pidsGroup, xtmp,
"limPwm",
"Limited PWD", _njoints))
722 yCError(FAKEMOTIONCONTROL) <<
"The PID parameter limPwm was requested but was not correctly set in the configuration file, please fill it.";
726 yCInfo(FAKEMOTIONCONTROL) <<
"Using LIMITED PWM!!";
727 for (j = 0; j < _njoints; j++) {
735 bool FakeMotionControl::parseTorquePidsGroup(
Bottle& pidsGroup,
Pid myPid[],
double kbemf[],
double ktau[],
int filterType[])
739 if (!extractGroup(pidsGroup, xtmp,
"kp",
"Pid kp parameter", _njoints)) {
742 for (j=0; j<_njoints; j++) {
746 if (!extractGroup(pidsGroup, xtmp,
"kd",
"Pid kd parameter", _njoints)) {
749 for (j=0; j<_njoints; j++) {
753 if (!extractGroup(pidsGroup, xtmp,
"ki",
"Pid kp parameter", _njoints)) {
756 for (j=0; j<_njoints; j++) {
760 if (!extractGroup(pidsGroup, xtmp,
"maxInt",
"Pid maxInt parameter", _njoints)) {
763 for (j=0; j<_njoints; j++) {
767 if (!extractGroup(pidsGroup, xtmp,
"maxPwm",
"Pid maxPwm parameter", _njoints)) {
770 for (j=0; j<_njoints; j++) {
774 if (!extractGroup(pidsGroup, xtmp,
"shift",
"Pid shift parameter", _njoints)) {
777 for (j=0; j<_njoints; j++) {
781 if (!extractGroup(pidsGroup, xtmp,
"ko",
"Pid ko parameter", _njoints)) {
784 for (j=0; j<_njoints; j++) {
788 if (!extractGroup(pidsGroup, xtmp,
"stictionUp",
"Pid stictionUp", _njoints)) {
791 for (j=0; j<_njoints; j++) {
795 if (!extractGroup(pidsGroup, xtmp,
"stictionDwn",
"Pid stictionDwn", _njoints)) {
798 for (j=0; j<_njoints; j++) {
802 if (!extractGroup(pidsGroup, xtmp,
"kff",
"Pid kff parameter", _njoints)) {
805 for (j=0; j<_njoints; j++) {
809 if (!extractGroup(pidsGroup, xtmp,
"kbemf",
"kbemf parameter", _njoints)) {
812 for (j=0; j<_njoints; j++) {
816 if (!extractGroup(pidsGroup, xtmp,
"ktau",
"ktau parameter", _njoints)) {
819 for (j=0; j<_njoints; j++) {
823 if (!extractGroup(pidsGroup, xtmp,
"filterType",
"filterType param", _njoints)) {
826 for (j=0; j<_njoints; j++) {
843 if (!extractGroup(pidsGroup, xtmp,
"limPwm",
"Limited PWM", _njoints))
845 yCError(FAKEMOTIONCONTROL) <<
"The PID parameter limPwm was requested but was not correctly set in the configuration file, please fill it.";
849 yCInfo(FAKEMOTIONCONTROL) <<
"Using LIMITED PWM!!";
850 for (j = 0; j < _njoints; j++) {
865 if(general.
check(
"AxisMap"))
867 if(extractGroup(general, xtmp,
"AxisMap",
"a list of reordered indices for the axes", _njoints))
878 yCInfo(FAKEMOTIONCONTROL) <<
"Using default AxisMap";
879 for (i = 0; i < _njoints; i++) {
884 if(general.
check(
"AxisName"))
886 if (extractGroup(general, xtmp,
"AxisName",
"a list of strings representing the axes names", _njoints))
891 _axisName[_axisMap[i - 1]] = xtmp.
get(i).
asString();
899 yCInfo(FAKEMOTIONCONTROL) <<
"Using default AxisName";
900 for (i = 0; i < _njoints; i++)
902 _axisName[_axisMap[i]] =
"joint" +
toString(i);
905 if(general.
check(
"AxisType"))
907 if (extractGroup(general, xtmp,
"AxisType",
"a list of strings representing the axes type (revolute/prismatic)", _njoints))
913 if (typeString ==
"revolute") {
915 }
else if (typeString ==
"prismatic") {
918 yCError(FAKEMOTIONCONTROL,
"Unknown AxisType value %s!", typeString.c_str());
929 yCInfo(FAKEMOTIONCONTROL) <<
"Using default AxisType (revolute)";
930 for (i = 0; i < _njoints; i++)
937 if (general.
check(
"ampsToSensor"))
939 if (extractGroup(general, xtmp,
"ampsToSensor",
"a list of scales for the ampsToSensor conversion factors", _njoints))
954 yCInfo(FAKEMOTIONCONTROL) <<
"Using default ampsToSensor";
955 for (i = 0; i < _njoints; i++)
957 _ampsToSensor[i] = 1.0;
962 if (general.
check(
"fullscalePWM"))
964 if (extractGroup(general, xtmp,
"fullscalePWM",
"a list of scales for the fullscalePWM conversion factors", _njoints))
970 _dutycycleToPWM[i - 1] = xtmp.
get(i).
asFloat64() / 100.0;
979 yCInfo(FAKEMOTIONCONTROL) <<
"Using default dutycycleToPWM=1.0";
980 for (i = 0; i < _njoints; i++) {
981 _dutycycleToPWM[i] = 1.0;
987 if(general.
check(
"Encoder"))
989 if (extractGroup(general, xtmp,
"Encoder",
"a list of scales for the encoders", _njoints))
1001 yCInfo(FAKEMOTIONCONTROL) <<
"Using default Encoder";
1002 for (i = 0; i < _njoints; i++) {
1003 _angleToEncoder[i] = 1;
1441 yCTrace(FAKEMOTIONCONTROL) <<
" close()";
1443 ImplementControlMode::uninitialize();
1444 ImplementEncodersTimed::uninitialize();
1445 ImplementMotorEncoders::uninitialize();
1446 ImplementPositionControl::uninitialize();
1447 ImplementVelocityControl::uninitialize();
1448 ImplementPidControl::uninitialize();
1449 ImplementControlCalibration::uninitialize();
1450 ImplementAmplifierControl::uninitialize();
1451 ImplementImpedanceControl::uninitialize();
1452 ImplementControlLimits::uninitialize();
1453 ImplementTorqueControl::uninitialize();
1454 ImplementPositionDirect::uninitialize();
1455 ImplementInteractionMode::uninitialize();
1456 ImplementAxisInfo::uninitialize();
1457 ImplementVirtualAnalogSensor::uninitialize();
1464 void FakeMotionControl::cleanup()
1475 yCDebug(FAKEMOTIONCONTROL) <<
"setPidRaw" << pidtype << j << pid.
kp;
1499 for(
int j=0; j< _njoints; j++)
1531 for(
int j=0, index=0; j< _njoints; j++, index++)
1543 _ppids_lim[j]=limit;
1546 _vpids_lim[j]=limit;
1549 _cpids_lim[j]=limit;
1552 _tpids_lim[j]=limit;
1563 for(
int j=0, index=0; j< _njoints; j++, index++)
1595 for(
int j=0; j< _njoints; j++)
1621 yCDebug(FAKEMOTIONCONTROL) <<
"getPidRaw" << pidtype << j << pid->
kp;
1631 for(
int j=0, index=0; j<_njoints; j++, index++)
1666 for(
int j=0; j< _njoints; j++)
1678 *limit=_ppids_lim[j];
1681 *limit=_vpids_lim[j];
1684 *limit=_cpids_lim[j];
1687 *limit=_tpids_lim[j];
1698 for(
int j=0, index=0; j<_njoints; j++, index++)
1715 _ppids_ena[j]=
false;
1718 _vpids_ena[j]=
false;
1721 _cpids_ena[j]=
false;
1724 _tpids_ena[j]=
false;
1756 yCDebug(FAKEMOTIONCONTROL) <<
"setPidOffsetRaw" << pidtype << j << v;
1782 *enabled=_ppids_ena[j];
1785 *enabled=_vpids_ena[j];
1788 *enabled=_cpids_ena[j];
1791 *enabled=_tpids_ena[j];
1818 yCDebug(FAKEMOTIONCONTROL) <<
"getPidOutputRaw" << pidtype << j << *out;
1825 for(
int j=0; j< _njoints; j++)
1845 yCError(FAKEMOTIONCONTROL) <<
"velocityMoveRaw: skipping command because board " <<
" joint " << j <<
" is not in VOCAB_CM_VELOCITY mode";
1847 _command_speeds[j] = sp;
1856 for (
int i = 0; i < _njoints; i++) {
1869 yCTrace(FAKEMOTIONCONTROL) <<
"setCalibrationParametersRaw for joint" << j;
1875 yCTrace(FAKEMOTIONCONTROL) <<
"calibrateRaw for joint" << j;
1881 bool result =
false;
1899 if (verbose >= VERY_VERBOSE) {
1900 yCTrace(FAKEMOTIONCONTROL) <<
"j " << j <<
" ref " << ref;
1916 yCError(FAKEMOTIONCONTROL) <<
"positionMoveRaw: skipping command because joint " << j <<
" is not in VOCAB_CM_POSITION mode";
1918 _posCtrl_references[j] = ref;
1924 if (verbose >= VERY_VERBOSE) {
1929 for(
int j=0, index=0; j< _njoints; j++, index++)
1938 if (verbose >= VERY_VERBOSE) {
1939 yCTrace(FAKEMOTIONCONTROL) <<
"j " << j <<
" ref " << delta;
1954 yCError(FAKEMOTIONCONTROL) <<
"relativeMoveRaw: skipping command because joint " << j <<
" is not in VOCAB_CM_POSITION mode";
1956 _posCtrl_references[j] += delta;
1962 if (verbose >= VERY_VERBOSE) {
1967 for(
int j=0, index=0; j< _njoints; j++, index++)
1977 if (verbose >= VERY_VERBOSE) {
1978 yCTrace(FAKEMOTIONCONTROL) <<
"j ";
1987 if (verbose >= VERY_VERBOSE) {
1992 bool val, tot_res =
true;
1994 for(
int j=0, index=0; j< _njoints; j++, index++)
2008 _ref_speeds[index] = sp;
2016 for(
int j=0, index=0; j< _njoints; j++, index++)
2018 _ref_speeds[index] = spds[index];
2030 _ref_accs[j ] = 1e6;
2032 else if (acc < -1e6)
2034 _ref_accs[j ] = -1e6;
2038 _ref_accs[j ] = acc;
2048 for(
int j=0, index=0; j< _njoints; j++, index++)
2052 _ref_accs[index] = 1e6;
2054 else if (accs[j] < -1e6)
2056 _ref_accs[index] = -1e6;
2060 _ref_accs[index] = accs[j];
2068 *spd = _ref_speeds[j];
2074 memcpy(spds, _ref_speeds,
sizeof(
double) * _njoints);
2080 *acc = _ref_accs[j];
2086 memcpy(accs, _ref_accs,
sizeof(
double) * _njoints);
2098 for(
int j=0; j< _njoints; j++)
2112 if (verbose >= VERY_VERBOSE) {
2113 yCTrace(FAKEMOTIONCONTROL) <<
" -> n_joint " << n_joint;
2116 for(
int j=0; j<n_joint; j++)
2118 yCDebug(FAKEMOTIONCONTROL,
"j: %d; ref %f;\n", joints[j], refs[j]); fflush(stdout);
2122 for(
int j=0; j<n_joint; j++)
2131 if (verbose >= VERY_VERBOSE) {
2132 yCTrace(FAKEMOTIONCONTROL) <<
"n_joint " << _njoints;
2136 for(
int j=0; j<n_joint; j++)
2145 if (verbose >= VERY_VERBOSE) {
2146 yCTrace(FAKEMOTIONCONTROL) <<
"n_joint " << _njoints;
2151 bool tot_val =
true;
2153 for(
int j=0; j<n_joint; j++)
2164 if (verbose >= VERY_VERBOSE) {
2165 yCTrace(FAKEMOTIONCONTROL) <<
"n_joint " << _njoints;
2169 for(
int j=0; j<n_joint; j++)
2178 if (verbose >= VERY_VERBOSE) {
2179 yCTrace(FAKEMOTIONCONTROL) <<
"n_joint " << _njoints;
2183 for(
int j=0; j<n_joint; j++)
2192 if (verbose >= VERY_VERBOSE) {
2193 yCTrace(FAKEMOTIONCONTROL) <<
"n_joint " << _njoints;
2197 for(
int j=0; j<n_joint; j++)
2206 if (verbose >= VERY_VERBOSE) {
2207 yCTrace(FAKEMOTIONCONTROL) <<
"n_joint " << _njoints;
2211 for(
int j=0; j<n_joint; j++)
2220 if (verbose >= VERY_VERBOSE) {
2221 yCTrace(FAKEMOTIONCONTROL) <<
"n_joint " << _njoints;
2225 for(
int j=0; j<n_joint; j++)
2239 if (verbose > VERY_VERY_VERBOSE) {
2240 yCTrace(FAKEMOTIONCONTROL) <<
"j: " << j;
2243 *v = _controlModes[j];
2251 for(
int j=0; j< _njoints; j++)
2261 for(
int j=0; j< n_joint; j++)
2275 if (verbose >= VERY_VERBOSE) {
2285 _controlModes[j] = _mode;
2287 _posCtrl_references[j] = pos[j];
2294 if (verbose >= VERY_VERBOSE) {
2295 yCTrace(FAKEMOTIONCONTROL) <<
"n_joints: " << n_joint;
2299 for(
int i=0; i<n_joint; i++)
2308 if (verbose >= VERY_VERBOSE) {
2313 for(
int i=0; i<_njoints; i++)
2357 for(
int j=0; j< _njoints; j++)
2376 for(
int j=0; j< _njoints; j++)
2394 for(
int j=0; j< _njoints; j++)
2407 for (
int i = 0; i < _njoints; i++) {
2408 stamps[i] = _encodersStamp[i];
2418 *stamp = _encodersStamp[j];
2471 for(
int j=0; j< _njoints; j++)
2488 for(
int j=0; j< _njoints; j++)
2504 for(
int j=0; j< _njoints; j++)
2515 for (
int i = 0; i < _njoints; i++) {
2516 stamps[i] = _encodersStamp[i];
2527 *stamp = _encodersStamp[m];
2549 *value = current[j];
2557 for(
int j=0; j< _njoints; j++)
2566 maxCurrent[m] = val;
2572 *val = maxCurrent[m];
2578 (_enabledAmp[j ]) ? *st = 1 : *st = 0;
2585 for(
int j=0; j<_njoints; j++)
2587 sts[j] = _enabledAmp[j];
2594 *val = peakCurrent[m];
2600 peakCurrent[m] = val;
2606 *val = nominalCurrent[m];
2612 nominalCurrent[m] = val;
2636 *val = supplyVoltage[m];
2685 yCError(FAKEMOTIONCONTROL,
"getKinematicMJRaw not yet implemented");
2721 if (axis >= 0 && axis < _njoints)
2723 name = _axisName[axis];
2735 if (axis >= 0 && axis < _njoints)
2737 type = _jointType[axis];
2755 *max = _maxJntCmdVelocity[axis];
2769 for (
int i = 0; i < _njoints; i++)
2789 for (
int j = 0; j < _njoints &&
ret; j++) {
2800 if (
t>1.0 ||
t< -1.0)
2802 yCError(FAKEMOTIONCONTROL) <<
"Joint received a high torque command, and was put in hardware fault";
2803 _hwfault_code[j] = 1;
2819 for (
int j = 0; j < _njoints &&
ret; j++) {
2828 *
t = _ref_torques[j];
2860 params->
bemf = _kbemf[j];
2862 params->
ktau = _ktau[j];
2870 _kbemf[j] = params.
bemf;
2871 _ktau[j] = params.
ktau;
2882 for(
int i=0; i<n_joint; i++)
2892 _posDir_references[j] = ref;
2898 for(
int i=0; i< n_joint; i++)
2900 _posDir_references[joints[i]] = refs[i];
2907 for(
int i=0; i< _njoints; i++)
2909 _posDir_references[i] = refs[i];
2917 if (verbose >= VERY_VERBOSE) {
2918 yCTrace(FAKEMOTIONCONTROL) <<
"j " << axis <<
" ref " << _posCtrl_references[axis];
2927 yCWarning(FAKEMOTIONCONTROL) <<
"getTargetPosition: Joint " << axis <<
" is not in POSITION mode, therefore the value returned by " <<
2928 "this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
2930 *ref = _posCtrl_references[axis];
2937 for (
int i = 0; i < _njoints; i++) {
2946 for (
int i = 0; i<nj; i++)
2955 *ref = _command_speeds[axis];
2962 for (
int i = 0; i<_njoints; i++)
2972 for (
int i = 0; i<nj; i++)
2988 yCWarning(FAKEMOTIONCONTROL) <<
"getRefPosition: Joint " << axis <<
" is not in POSITION_DIRECT mode, therefore the value returned by \
2989 this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
2992 *ref = _posDir_references[axis];
3000 for (
int i = 0; i<_njoints; i++)
3010 for (
int i = 0; i<nj; i++)
3021 if (verbose > VERY_VERY_VERBOSE) {
3022 yCTrace(FAKEMOTIONCONTROL) <<
"j: " << j;
3031 for(
int j=0; j< n_joints; j++)
3042 for (
int j = 0; j < _njoints; j++) {
3053 if (verbose >= VERY_VERBOSE) {
3057 _interactMode[j] = _mode;
3066 for(
int i=0; i<n_joints; i++)
3076 for(
int i=0; i<_njoints; i++)
3098 for(
int j=0; j< _njoints; j++)
3126 for (
int i = 0; i < _njoints; i++)
3141 for (
int i = 0; i < _njoints; i++)
3156 for (
int i = 0; i < _njoints; i++)
3178 *min = _ref_currents[j] / 100;
3179 *max = _ref_currents[j] * 100;
3186 for (
int i = 0; i < _njoints; i++)
3188 min[i] = _ref_currents[i] / 100;
3189 max[i] = _ref_currents[i] * 100;
3196 for (
int i = 0; i < _njoints; i++)
3198 _ref_currents[i] =
t[i];
3199 current[i] =
t[i] / 2;
3206 _ref_currents[j] =
t;
3214 for (
int j = 0; j<n_joint; j++)
3223 for (
int i = 0; i < _njoints; i++)
3225 t[i] = _ref_currents[i];
3232 *
t = _ref_currents[j];
3253 for (
int i = 0; i < _njoints; i++)
3255 measure[i] = _torques[i];
3262 _torques[ch] = measure;
3269 fault = _hwfault_code[j];
3270 message = _hwfault_message[j];
void checkAndDestroy(T *&p)
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_IMPEDANCE_VEL
constexpr yarp::conf::vocab32_t VOCAB_CM_IDLE
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION
constexpr yarp::conf::vocab32_t VOCAB_CM_IMPEDANCE_POS
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_CM_VELOCITY
constexpr yarp::conf::vocab32_t VOCAB_CM_FORCE_IDLE
bool getPowerSupplyVoltageRaw(int j, double *val) override
bool getRefSpeedRaw(int j, double *ref) override
Get reference speed for a joint.
virtual bool getHasTempSensorsRaw(int j, int &ret)
bool setRefCurrentsRaw(const double *t) override
Set the reference value of the currents for all motors.
bool setRefTorqueRaw(int j, double t) override
Set the reference value of the torque for a given joint.
bool getCurrentsRaw(double *vals) override
bool getImpedanceOffsetRaw(int j, double *offset) override
Get current force Offset for a specific joint.
bool getTargetPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
bool getTorqueRangeRaw(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
bool getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
Get the current impedandance limits for a specific joint.
bool setTemperatureLimitRaw(int m, const double temp) override
Set the temperature limit for a specific motor.
bool getPidErrorRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
bool setPeakCurrentRaw(int m, const double val) override
bool velocityMoveRaw(int j, double sp) override
Start motion at a given speed, single joint.
bool checkMotionDoneRaw(bool *flag) override
Check if the current trajectory is terminated.
bool getMotorEncoderSpeedRaw(int m, double *sp) override
Read the istantaneous speed of a motor encoder.
virtual bool getJointEncoderTypeRaw(int j, int &type)
bool setRefDutyCycleRaw(int j, double v) override
Sets the reference dutycycle of a single motor.
bool getTemperatureRaw(int m, double *val) override
Get temperature of a motor.
bool getEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of an axis.
bool getRefAccelerationRaw(int j, double *acc) override
Get reference acceleration for a joint.
bool getMotorEncoderSpeedsRaw(double *spds) override
Read the instantaneous speed of all motor encoders.
bool setPidsRaw(const yarp::dev::PidControlTypeEnum &pidtype, const yarp::dev::Pid *pids) override
Set new pid value on multiple axes.
bool 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...
bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
bool setMotorEncoderRaw(int m, const double val) override
Set the value of the motor encoder for a given motor.
bool getPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *limit) override
Get the error limit for the controller on a specific joint.
bool getPidOutputRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
bool setControlModeRaw(const int j, const int mode) override
bool getMotorEncoderAccelerationRaw(int m, double *spds) override
Read the instantaneous acceleration of a motor encoder.
bool open(yarp::os::Searchable &par) override
Open the DeviceDriver.
virtual bool getHasHallSensorRaw(int j, int &ret)
bool getTemperatureLimitRaw(int m, double *temp) override
Retreives the current temperature limit for a specific motor.
bool getNumberOfMotorsRaw(int *num) override
Retrieves the number of controlled motors from the current physical interface.
bool disableAmpRaw(int j) override
Disable the amplifier on a specific joint.
bool setMotorTorqueParamsRaw(int j, const yarp::dev::MotorTorqueParameters params) override
Set the motor parameters.
bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
Start calibration, this method is very often platform specific.
bool setRefAccelerationRaw(int j, double acc) override
Set reference acceleration for a joint.
virtual bool initialised()
bool setEncodersRaw(const double *vals) override
Set the value of all encoders.
bool getPidOutputsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
bool isPidEnabledRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, bool *enabled) override
Get the current status (enabled/disabled) of the pid controller.
bool setRefSpeedsRaw(const double *spds) override
Set reference speed on all joints.
bool getTargetPositionsRaw(double *refs) override
Get the last position reference for all axes.
bool getRefDutyCycleRaw(int j, double *v) override
Gets the last reference sent using the setRefDutyCycleRaw function.
bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
bool setPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum &pidtype, const double *limits) override
Get the error limit for the controller on all joints.
bool getPidReferenceRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *ref) override
Get the current reference of the pid controller for a specific joint.
bool getNominalCurrentRaw(int m, double *val) override
bool getControlModeRaw(int j, int *v) override
bool calibrationDoneRaw(int j) override
Check if the calibration is terminated, on a particular joint.
bool threadInit() override
Initialization method.
bool getRefDutyCyclesRaw(double *v) override
Gets the last reference sent using the setRefDutyCyclesRaw function.
bool getRefVelocityRaw(const int joint, double *ref) override
Get the last reference speed set by velocityMove for single joint.
bool getEncoderTimedRaw(int j, double *encs, double *stamp) override
Read the instantaneous acceleration of all axes.
bool disablePidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Disable the pid computation for a joint.
yarp::dev::VAS_status getVirtualAnalogSensorStatusRaw(int ch) override
Check the status of a given channel.
bool getMotorEncodersRaw(double *encs) override
Read the position of all motor encoders.
bool getTorqueRangesRaw(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
bool getRefCurrentsRaw(double *t) override
Get the reference value of the currents for all motors.
bool setRefDutyCyclesRaw(const double *v) override
Sets the reference dutycycle for all motors.
bool getEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of an axis.
int getVirtualAnalogSensorChannelsRaw() override
Get the number of channels of the virtual sensor.
bool 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 getCurrentRangesRaw(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
bool getEncoderRaw(int j, double *v) override
Read the value of an encoder.
bool getPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *limits) override
Get the error limit for all controllers.
bool enableAmpRaw(int j) override
Enable the amplifier on a specific joint.
bool fromConfig(yarp::os::Searchable &config)
bool getPidsRaw(const yarp::dev::PidControlTypeEnum &pidtype, yarp::dev::Pid *pids) override
Get current pid value for a specific joint.
bool getRefPositionsRaw(double *refs) override
Get the last position reference for all axes.
bool getMotorTorqueParamsRaw(int j, yarp::dev::MotorTorqueParameters *params) override
Get the motor parameters.
bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
bool setRefSpeedRaw(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
virtual bool getHasRotorEncoderIndexRaw(int j, int &ret)
bool updateVirtualAnalogSensorMeasureRaw(yarp::sig::Vector &measure) override
Set a vector of torque values for virtual sensor.
void resizeBuffers()
Resize previously allocated buffers.
void threadRelease() override
Release method.
bool getAmpStatusRaw(int *st) override
bool getLastJointFaultRaw(int j, int &fault, std::string &message) override
bool setImpedanceRaw(int j, double stiffness, double damping) override
Set current impedance parameters (stiffness,damping) for a specific joint.
bool setPWMLimitRaw(int j, const double val) override
bool relativeMoveRaw(int j, double delta) override
Set relative position.
virtual bool getMotorPolesRaw(int j, int &poles)
bool getPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::Pid *pid) override
Get current pid value for a specific joint.
bool getCurrentRaw(int j, double *val) override
bool getPeakCurrentRaw(int m, double *val) override
bool getDutyCyclesRaw(double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
bool setEncoderRaw(int j, double val) override
Set the value of the encoder for a given joint.
bool getRefPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
bool getLimitsRaw(int axis, double *min, double *max) override
Get the software limits for a particular axis.
virtual bool getRotorIndexOffsetRaw(int j, double &rotorOffset)
bool getVelLimitsRaw(int axis, double *min, double *max) override
Get the software speed limits for a particular axis.
bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
Set new reference point for all axes.
bool setPidReferenceRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
bool resetEncodersRaw() override
Reset encoders.
bool getPWMRaw(int j, double *val) override
bool getAxisNameRaw(int axis, std::string &name) override
bool setCalibrationParametersRaw(int axis, const yarp::dev::CalibrationParameters ¶ms) override
Start calibration, this method is very often platform specific.
bool setPidOffsetRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override
Set an offset value on the ourput of pid controller.
bool getInteractionModeRaw(int j, yarp::dev::InteractionModeEnum *_mode) override
Get the current interaction mode of the robot, values can be stiff or compliant.
bool getCurrentRangeRaw(int j, double *min, double *max) override
Get the full scale of the current measurement for a given motor (e.g.
bool stopRaw() override
Stop motion, multiple joints.
virtual bool getKinematicMJRaw(int j, double &rotres)
bool getRefSpeedsRaw(double *spds) override
Get reference speed of all joints.
bool getRefAccelerationsRaw(double *accs) override
Get reference acceleration of all joints.
virtual bool getJointEncoderResolutionRaw(int m, double &jntres)
bool getAxes(int *ax) override
Get the number of controlled axes.
bool setMaxCurrentRaw(int j, double val) override
bool alloc(int njoints)
Allocated buffers.
bool resetMotorEncodersRaw() override
Reset motor encoders.
bool setRefTorquesRaw(const double *t) override
Set the reference value of the torque for all joints.
bool getEncodersRaw(double *encs) override
Read the position of all axes.
bool setRefAccelerationsRaw(const double *accs) override
Set reference acceleration on all joints.
bool getControlModesRaw(int *v) override
bool setPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, const yarp::dev::Pid &pid) override
Set new pid value for a joint axis.
bool getPWMLimitRaw(int j, double *val) override
virtual bool getRotorEncoderTypeRaw(int j, int &type)
bool getRefCurrentRaw(int j, double *t) override
Get the reference value of the current for a single motor.
bool getDutyCycleRaw(int j, double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
bool setPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double limit) override
Set the error limit for the controller on a specific joint.
bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
bool positionMoveRaw(int j, double ref) override
Set new reference point for a single axis.
bool getMotorEncoderCountsPerRevolutionRaw(int m, double *v) override
Gets number of counts per revolution for motor encoder m.
bool setImpedanceOffsetRaw(int j, double offset) override
Set current force Offset for a specific joint.
bool setMotorEncodersRaw(const double *vals) override
Set the value of all motor encoders.
bool getPidErrorsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
bool getGearboxRatioRaw(int m, double *gearbox) override
Get the gearbox ratio for a specific motor.
bool getMaxCurrentRaw(int j, double *val) override
Returns the maximum electric current allowed for a given motor.
bool close() override
Close the DeviceDriver.
bool getRefTorquesRaw(double *t) override
Get the reference value of the torque for all joints.
bool setRefCurrentRaw(int j, double t) override
Set the reference value of the current for a single motor.
bool getNumberOfMotorEncodersRaw(int *num) override
Get the number of available motor encoders.
bool getPidReferencesRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all pid controllers.
bool 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).
bool getImpedanceRaw(int j, double *stiffness, double *damping) override
Get current impedance parameters (stiffness,damping,offset) for a specific joint.
bool enablePidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Enable the pid computation for a joint.
bool getRefVelocitiesRaw(double *refs) override
Get the last reference speed set by velocityMove for all joints.
bool setPositionRaw(int j, double ref) override
Set new position for a single axis.
bool getEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all axes.
bool setPidReferencesRaw(const yarp::dev::PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
bool getMotorEncoderRaw(int m, double *v) override
Read the value of a motor encoder.
bool getRefTorqueRaw(int j, double *t) override
Set the reference value of the torque for a given joint.
bool getMotorEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all motor encoders.
bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
Set the interaction mode of the robot for a set of joints, values can be stiff or compliant.
bool getTemperaturesRaw(double *vals) override
Get temperature of all the motors.
bool resetMotorEncoderRaw(int m) override
Reset motor encoder, single motor.
bool setNominalCurrentRaw(int m, const double val) override
virtual bool getRotorEncoderResolutionRaw(int m, double &rotres)
virtual bool getHasRotorEncoderRaw(int j, int &ret)
bool getMotorEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous position of all motor encoders.
bool resetEncoderRaw(int j) override
Reset encoder, single joint.
bool getTorquesRaw(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
bool setLimitsRaw(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...
bool setInteractionModeRaw(int j, yarp::dev::InteractionModeEnum _mode) override
Set the interaction mode of the robot, values can be stiff or compliant.
void run() override
Loop function.
bool getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
Read the instantaneous position of a motor encoder.
bool getEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous acceleration of all axes.
bool getEncoderSpeedsRaw(double *spds) override
Read the instantaneous acceleration of an axis.
virtual bool getTorqueControlFilterType(int j, int &type)
class ImplementControlLimits; class StubImplControlLimitsRaw;
Interface settings the way the robot interacts with the environment: basic interaction types are Stif...
Default implementation of the IPositionControl interface.
Default implementation of the IPositionDirect interface.
Contains the parameters for a PID.
double scale
scale for the pid output
double offset
pwm offset added to the pid output
double stiction_down_val
down stiction offset added to the pid output
double stiction_up_val
up stiction offset added to the pid output
double max_output
max output
double kff
feedforward gain
double ki
integrative gain
double max_int
saturation threshold for the integrator
double kp
proportional gain
A simple collection of objects that can be described and transmitted in a portable way.
size_type size() const
Gets the number of elements in the bottle.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Bottle & findGroup(const std::string &key) const override
Gets a list corresponding to a given keyword.
bool check(const std::string &key) const override
Check if there exists a property of the given name.
bool isNull() const override
Checks if the object is invalid.
An abstraction for a periodic thread.
bool start()
Call this to start the thread.
A base class for nested structures that can be searched.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
A single value (typically within a Bottle).
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
virtual std::int32_t asInt32() const
Get 32-bit integer value.
virtual bool isFloat64() const
Checks if value is a 64-bit floating point number.
virtual bool isInt32() const
Checks if value is a 32-bit integer.
virtual std::string asString() const
Get string value.
void resize(size_t size) override
Resize the vector.
T * data()
Return a pointer to the first element of the vector.
#define OPENLOOP_WATCHDOG
std::string toString(const T &value)
convert an arbitrary type to string.
static bool NOT_YET_IMPLEMENTED(const char *txt)
static bool DEPRECATED(const char *txt)
#define VELOCITY_WATCHDOG
#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.
std::string to_string(IntegerType x)
For streams capable of holding different kinds of content, check what they actually have.
@ VOCAB_JOINTTYPE_REVOLUTE
@ VOCAB_JOINTTYPE_PRISMATIC
@ VOCAB_JOINTTYPE_UNKNOWN
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.
VectorOf< double > Vector