YARP
Yet Another Robot Platform
fakeMotionControl.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 #include "fakeMotionControl.h"
10 
11 #include <yarp/conf/environment.h>
12 
13 #include <yarp/os/Bottle.h>
14 #include <yarp/os/Time.h>
15 #include <yarp/os/LogComponent.h>
16 #include <yarp/os/LogStream.h>
17 #include <yarp/os/NetType.h>
18 #include <yarp/dev/Drivers.h>
19 #include <yarp/math/Math.h>
20 
21 #include <sstream>
22 #include <cstring>
23 
24 using namespace std;
25 using namespace yarp::dev;
26 using namespace yarp::os;
27 using namespace yarp::os::impl;
28 using namespace yarp::math;
29 
30 // macros
31 #define NEW_JSTATUS_STRUCT 1
32 #define VELOCITY_WATCHDOG 0.1
33 #define OPENLOOP_WATCHDOG 0.1
34 #define PWM_CONSTANT 0.1
35 
36 namespace {
37 YARP_LOG_COMPONENT(FAKEMOTIONCONTROL, "yarp.device.fakeMotionControl")
38 }
39 
41 {
42  for (int i=0;i <_njoints ;i++)
43  {
44  if (_controlModes[i] == VOCAB_CM_VELOCITY)
45  {
46  //increment joint position
47  if (this->_command_speeds[i]!=0)
48  {
49  double elapsed = yarp::os::Time::now()-prev_time;
50  double increment = _command_speeds[i]*elapsed;
51  pos[i]+=increment;
52  }
53 
54  //velocity watchdog
55  if (yarp::os::Time::now()-last_velocity_command[i]>=VELOCITY_WATCHDOG)
56  {
57  this->_command_speeds[i]=0.0;
58  }
59  }
60  else if (_controlModes[i] == VOCAB_CM_PWM)
61  {
62  //increment joint position
63  if (this->refpwm[i]!=0)
64  {
65  double elapsed = yarp::os::Time::now()-prev_time;
66  double increment = refpwm[i]*elapsed*PWM_CONSTANT;
67  pos[i]+=increment;
68  }
69 
70  //pwm watchdog
71  if (yarp::os::Time::now()-last_pwm_command[i]>=OPENLOOP_WATCHDOG)
72  {
73  this->refpwm[i]=0.0;
74  }
75  }
76  else if (_controlModes[i] == VOCAB_CM_POSITION_DIRECT)
77  {
78  pos[i] = _posDir_references[i];
79  }
80  else if (_controlModes[i] == VOCAB_CM_POSITION)
81  {
82  pos[i] = _posCtrl_references[i];
83  //do something with _ref_speeds[i];
84  }
85  else if (_controlModes[i] == VOCAB_CM_IDLE)
86  {
87  //do nothing
88  }
89  else if (_controlModes[i] == VOCAB_CM_MIXED)
90  {
91  //not yet implemented
92  }
93  else
94  {
95  //unsupported control mode
96  yCWarning(FAKEMOTIONCONTROL) << "Unsupported control mode, joint " << i << " " << yarp::os::Vocab::decode(_controlModes[i]);
97  }
98  }
99  prev_time = yarp::os::Time::now();
100 }
101 
102 static inline bool NOT_YET_IMPLEMENTED(const char *txt)
103 {
104  yCError(FAKEMOTIONCONTROL) << txt << "is not yet implemented for FakeMotionControl";
105  return true;
106 }
107 
108 static inline bool DEPRECATED(const char *txt)
109 {
110  yCError(FAKEMOTIONCONTROL) << txt << "has been deprecated for FakeMotionControl";
111  return true;
112 }
113 
114 
115 // replace with to_string as soon as C++11 is required by YARP
120 template<typename T>
121 std::string toString(const T& value)
122 {
123  std::ostringstream oss;
124  oss << value;
125  return oss.str();
126 }
127 
128 //generic function that check is key1 is present in input bottle and that the result has size elements
129 // return true/false
130 bool FakeMotionControl::extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
131 {
132  size++;
133  Bottle &tmp=input.findGroup(key1, txt);
134 
135  if (tmp.isNull())
136  {
137  yCError(FAKEMOTIONCONTROL) << key1.c_str() << "parameter not found";
138  return false;
139  }
140 
141  if(tmp.size()!=(size_t) size)
142  {
143  yCError(FAKEMOTIONCONTROL) << key1.c_str() << "incorrect number of entries";
144  return false;
145  }
146 
147  out=tmp;
148  return true;
149 }
150 
152 {
153  pos.resize(_njoints);
154  dpos.resize(_njoints);
155  vel.resize(_njoints);
156  speed.resize(_njoints);
157  acc.resize(_njoints);
158  loc.resize(_njoints);
159  amp.resize(_njoints);
160 
161  current.resize(_njoints);
162  nominalCurrent.resize(_njoints);
163  maxCurrent.resize(_njoints);
164  peakCurrent.resize(_njoints);
165  pwm.resize(_njoints);
166  refpwm.resize(_njoints);
167  pwmLimit.resize(_njoints);
168  supplyVoltage.resize(_njoints);
169  last_velocity_command.resize(_njoints);
170  last_pwm_command.resize(_njoints);
171 
172  pos.zero();
173  dpos.zero();
174  vel.zero();
175  speed.zero();
176  acc.zero();
177  loc.zero();
178  amp.zero();
179 
180  current.zero();
181  nominalCurrent.zero();
182  maxCurrent.zero();
183  peakCurrent.zero();
184 
185  pwm.zero();
186  refpwm.zero();
187  pwmLimit.zero();
188  supplyVoltage.zero();
189 }
190 
192 {
193  _axisMap = allocAndCheck<int>(nj);
194  _controlModes = allocAndCheck<int>(nj);
195  _interactMode = allocAndCheck<int>(nj);
196  _angleToEncoder = allocAndCheck<double>(nj);
197  _dutycycleToPWM = allocAndCheck<double>(nj);
198  _ampsToSensor = allocAndCheck<double>(nj);
199  _encodersStamp = allocAndCheck<double>(nj);
200  _DEPRECATED_encoderconversionoffset = allocAndCheck<float>(nj);
201  _DEPRECATED_encoderconversionfactor = allocAndCheck<float>(nj);
202 // _jointEncoderType = allocAndCheck<uint8_t>(nj);
203 // _rotorEncoderType = allocAndCheck<uint8_t>(nj);
204  _jointEncoderRes = allocAndCheck<int>(nj);
205  _rotorEncoderRes = allocAndCheck<int>(nj);
206  _gearbox = allocAndCheck<double>(nj);
207  _torqueSensorId= allocAndCheck<int>(nj);
208  _torqueSensorChan= allocAndCheck<int>(nj);
209  _maxTorque=allocAndCheck<double>(nj);
210  _torques = allocAndCheck<double>(nj);
211  _maxJntCmdVelocity = allocAndCheck<double>(nj);
212  _maxMotorVelocity = allocAndCheck<double>(nj);
213  _newtonsToSensor=allocAndCheck<double>(nj);
214  _hasHallSensor = allocAndCheck<bool>(nj);
215  _hasTempSensor = allocAndCheck<bool>(nj);
216  _hasRotorEncoder = allocAndCheck<bool>(nj);
217  _hasRotorEncoderIndex = allocAndCheck<bool>(nj);
218  _rotorIndexOffset = allocAndCheck<int>(nj);
219  _motorPoles = allocAndCheck<int>(nj);
220  _rotorlimits_max = allocAndCheck<double>(nj);
221  _rotorlimits_min = allocAndCheck<double>(nj);
222 
223  _ppids=allocAndCheck<Pid>(nj);
224  _tpids=allocAndCheck<Pid>(nj);
225  _cpids = allocAndCheck<Pid>(nj);
226  _vpids = allocAndCheck<Pid>(nj);
227  _ppids_ena=allocAndCheck<bool>(nj);
228  _tpids_ena=allocAndCheck<bool>(nj);
229  _cpids_ena = allocAndCheck<bool>(nj);
230  _vpids_ena = allocAndCheck<bool>(nj);
231  _ppids_lim=allocAndCheck<double>(nj);
232  _tpids_lim=allocAndCheck<double>(nj);
233  _cpids_lim = allocAndCheck<double>(nj);
234  _vpids_lim = allocAndCheck<double>(nj);
235  _ppids_ref=allocAndCheck<double>(nj);
236  _tpids_ref=allocAndCheck<double>(nj);
237  _cpids_ref = allocAndCheck<double>(nj);
238  _vpids_ref = allocAndCheck<double>(nj);
239 
240 // _impedance_params=allocAndCheck<ImpedanceParameters>(nj);
241 // _impedance_limits=allocAndCheck<ImpedanceLimits>(nj);
242  _axisName = new string[nj];
243  _jointType = new JointTypeEnum[nj];
244 
245  _limitsMax=allocAndCheck<double>(nj);
246  _limitsMin=allocAndCheck<double>(nj);
247  _kinematic_mj=allocAndCheck<double>(16);
248 // _currentLimits=allocAndCheck<MotorCurrentLimits>(nj);
249  _motorPwmLimits=allocAndCheck<double>(nj);
250  checking_motiondone=allocAndCheck<bool>(nj);
251 
252  _velocityShifts=allocAndCheck<int>(nj);
253  _velocityTimeout=allocAndCheck<int>(nj);
254  _kbemf=allocAndCheck<double>(nj);
255  _ktau=allocAndCheck<double>(nj);
256  _kbemf_scale = allocAndCheck<int>(nj);
257  _ktau_scale = allocAndCheck<int>(nj);
258  _filterType=allocAndCheck<int>(nj);
259  _last_position_move_time=allocAndCheck<double>(nj);
260 
261  // Reserve space for data stored locally. values are initialized to 0
262  _posCtrl_references = allocAndCheck<double>(nj);
263  _posDir_references = allocAndCheck<double>(nj);
264  _command_speeds = allocAndCheck<double>(nj);
265  _ref_speeds = allocAndCheck<double>(nj);
266  _ref_accs = allocAndCheck<double>(nj);
267  _ref_torques = allocAndCheck<double>(nj);
268  _ref_currents = allocAndCheck<double>(nj);
269  _enabledAmp = allocAndCheck<bool>(nj);
270  _enabledPid = allocAndCheck<bool>(nj);
271  _calibrated = allocAndCheck<bool>(nj);
272 // _cacheImpedance = allocAndCheck<eOmc_impedance_t>(nj);
273 
274  resizeBuffers();
275 
276  return true;
277 }
278 
279 bool FakeMotionControl::dealloc()
280 {
281  checkAndDestroy(_axisMap);
282  checkAndDestroy(_controlModes);
283  checkAndDestroy(_interactMode);
284  checkAndDestroy(_angleToEncoder);
285  checkAndDestroy(_ampsToSensor);
286  checkAndDestroy(_dutycycleToPWM);
287  checkAndDestroy(_encodersStamp);
288  checkAndDestroy(_DEPRECATED_encoderconversionoffset);
289  checkAndDestroy(_DEPRECATED_encoderconversionfactor);
290  checkAndDestroy(_jointEncoderRes);
291  checkAndDestroy(_rotorEncoderRes);
292 // checkAndDestroy(_jointEncoderType);
293 // checkAndDestroy(_rotorEncoderType);
294  checkAndDestroy(_gearbox);
295  checkAndDestroy(_torqueSensorId);
296  checkAndDestroy(_torqueSensorChan);
297  checkAndDestroy(_maxTorque);
298  checkAndDestroy(_maxJntCmdVelocity);
299  checkAndDestroy(_maxMotorVelocity);
300  checkAndDestroy(_newtonsToSensor);
301  checkAndDestroy(_ppids);
302  checkAndDestroy(_tpids);
303  checkAndDestroy(_cpids);
304  checkAndDestroy(_vpids);
305  checkAndDestroy(_ppids_ena);
306  checkAndDestroy(_tpids_ena);
307  checkAndDestroy(_cpids_ena);
308  checkAndDestroy(_vpids_ena);
309  checkAndDestroy(_ppids_lim);
310  checkAndDestroy(_tpids_lim);
311  checkAndDestroy(_cpids_lim);
312  checkAndDestroy(_vpids_lim);
313  checkAndDestroy(_ppids_ref);
314  checkAndDestroy(_tpids_ref);
315  checkAndDestroy(_cpids_ref);
316  checkAndDestroy(_vpids_ref);
317 // checkAndDestroy(_impedance_params);
318 // checkAndDestroy(_impedance_limits);
319  checkAndDestroy(_limitsMax);
320  checkAndDestroy(_limitsMin);
321  checkAndDestroy(_kinematic_mj);
322 // checkAndDestroy(_currentLimits);
323  checkAndDestroy(_motorPwmLimits);
324  checkAndDestroy(checking_motiondone);
325  checkAndDestroy(_velocityShifts);
326  checkAndDestroy(_velocityTimeout);
327  checkAndDestroy(_kbemf);
328  checkAndDestroy(_ktau);
329  checkAndDestroy(_kbemf_scale);
330  checkAndDestroy(_ktau_scale);
331  checkAndDestroy(_filterType);
332  checkAndDestroy(_posCtrl_references);
333  checkAndDestroy(_posDir_references);
334  checkAndDestroy(_command_speeds);
335  checkAndDestroy(_ref_speeds);
336  checkAndDestroy(_ref_accs);
337  checkAndDestroy(_ref_torques);
338  checkAndDestroy(_ref_currents);
339  checkAndDestroy(_enabledAmp);
340  checkAndDestroy(_enabledPid);
341  checkAndDestroy(_calibrated);
342  checkAndDestroy(_hasHallSensor);
343  checkAndDestroy(_hasTempSensor);
344  checkAndDestroy(_hasRotorEncoder);
345  checkAndDestroy(_hasRotorEncoderIndex);
346  checkAndDestroy(_rotorIndexOffset);
347  checkAndDestroy(_motorPoles);
348  checkAndDestroy(_axisName);
349  checkAndDestroy(_jointType);
350  checkAndDestroy(_rotorlimits_max);
351  checkAndDestroy(_rotorlimits_min);
352  checkAndDestroy(_last_position_move_time);
353  checkAndDestroy(_torques);
354 
355  return true;
356 }
357 
359  PeriodicThread(0.01),
362  ImplementPidControl(this),
366  ImplementControlMode(this),
374  ImplementPWMControl(this),
375  ImplementMotor(this),
376  ImplementAxisInfo(this),
378  _mutex(),
379  _njoints (0),
380  _axisMap (nullptr),
381  _angleToEncoder(nullptr),
382  _encodersStamp (nullptr),
383  _ampsToSensor (nullptr),
384  _dutycycleToPWM(nullptr),
385  _DEPRECATED_encoderconversionfactor (nullptr),
386  _DEPRECATED_encoderconversionoffset (nullptr),
387  _jointEncoderRes (nullptr),
388  _rotorEncoderRes (nullptr),
389  _gearbox (nullptr),
390  _hasHallSensor (nullptr),
391  _hasTempSensor (nullptr),
392  _hasRotorEncoder (nullptr),
393  _hasRotorEncoderIndex (nullptr),
394  _rotorIndexOffset (nullptr),
395  _motorPoles (nullptr),
396  _rotorlimits_max (nullptr),
397  _rotorlimits_min (nullptr),
398  _ppids (nullptr),
399  _tpids (nullptr),
400  _cpids (nullptr),
401  _vpids (nullptr),
402  _ppids_ena (nullptr),
403  _tpids_ena (nullptr),
404  _cpids_ena (nullptr),
405  _vpids_ena (nullptr),
406  _ppids_lim (nullptr),
407  _tpids_lim (nullptr),
408  _cpids_lim (nullptr),
409  _vpids_lim (nullptr),
410  _ppids_ref (nullptr),
411  _tpids_ref (nullptr),
412  _cpids_ref (nullptr),
413  _vpids_ref (nullptr),
414  _axisName (nullptr),
415  _jointType (nullptr),
416  _limitsMin (nullptr),
417  _limitsMax (nullptr),
418  _kinematic_mj (nullptr),
419  _maxJntCmdVelocity (nullptr),
420  _maxMotorVelocity (nullptr),
421  _velocityShifts (nullptr),
422  _velocityTimeout (nullptr),
423  _kbemf (nullptr),
424  _ktau (nullptr),
425  _kbemf_scale (nullptr),
426  _ktau_scale (nullptr),
427  _filterType (nullptr),
428  _torqueSensorId (nullptr),
429  _torqueSensorChan (nullptr),
430  _maxTorque (nullptr),
431  _newtonsToSensor (nullptr),
432  checking_motiondone (nullptr),
433  _last_position_move_time(nullptr),
434  _motorPwmLimits (nullptr),
435  _torques (nullptr),
436  useRawEncoderData (false),
437  _pwmIsLimited (false),
438  _torqueControlEnabled (false),
439  _torqueControlUnits (T_MACHINE_UNITS),
440  _positionControlUnits (P_MACHINE_UNITS),
441  _controlModes (nullptr),
442  _interactMode (nullptr),
443  _enabledAmp (nullptr),
444  _enabledPid (nullptr),
445  _calibrated (nullptr),
446  _posCtrl_references (nullptr),
447  _posDir_references (nullptr),
448  _ref_speeds (nullptr),
449  _command_speeds (nullptr),
450  _ref_accs (nullptr),
451  _ref_torques (nullptr),
452  _ref_currents (nullptr),
453  prev_time (0.0),
454  opened (false),
455  verbose (VERY_VERBOSE)
456 {
457  resizeBuffers();
458  std::string tmp = yarp::conf::environment::getEnvironment("VERBOSE_STICA");
459  verbosewhenok = (tmp != "") ? (bool)NetType::toInt(tmp) :
460  false;
461 }
462 
464 {
465  yCTrace(FAKEMOTIONCONTROL);
466  dealloc();
467 }
468 
470 {
471  return opened;
472 }
473 
475 {
476  yCTrace(FAKEMOTIONCONTROL);
477  for(int i=0; i<_njoints; i++)
478  {
479  pwm[i] = 33+i;
480  pwmLimit[i] = (33+i)*10;
481  current[i] = (33+i)*100;
482  maxCurrent[i] = (33+i)*1000;
483  peakCurrent[i] = (33+i)*2;
484  nominalCurrent[i] = (33+i)*20;
485  supplyVoltage[i] = (33+i)*200;
486  last_velocity_command[i] = -1;
487  last_pwm_command[i] = -1;
488  _controlModes[i] = VOCAB_CM_POSITION;
489  _maxJntCmdVelocity[i]=50.0;
490  }
491  prev_time = yarp::os::Time::now();
492  return true;
493 }
494 
496 {
497 }
498 
500 {
501  std::string str;
502 
503 // if (!config.findGroup("GENERAL").find("MotioncontrolVersion").isInt32())
504 // {
505 // yCError(FAKEMOTIONCONTROL) << "Missing MotioncontrolVersion parameter. yarprobotinterface cannot start. Please contact icub-support@iit.it";
506 // return false;
507 // }
508 // else
509 // {
510 // int mcv = config.findGroup("GENERAL").find("MotioncontrolVersion").asInt32();
511 // if (mcv != 2)
512 // {
513 // yCError(FAKEMOTIONCONTROL) << "Wrong MotioncontrolVersion parameter. yarprobotinterface cannot start. Please contact icub-support@iit.it";
514 // return false;
515 // }
516 // }
517 
518 // if(!config.findGroup("GENERAL").find("verbose").isBool())
519 // {
520 // yCError(FAKEMOTIONCONTROL) << "open() detects that general->verbose bool param is different from accepted values (true / false). Assuming false";
521 // str=" ";
522 // }
523 // else
524 // {
525 // if(config.findGroup("GENERAL").find("verbose").asBool())
526 // str=config.toString().c_str();
527 // else
528 // str=" ";
529 // }
530  str=config.toString();
531  yCTrace(FAKEMOTIONCONTROL) << str;
532 
533  //
534  // Read Configuration params from file
535  //
536  _njoints = config.findGroup("GENERAL").check("Joints",Value(1), "Number of degrees of freedom").asInt32();
537 
538  if(!alloc(_njoints))
539  {
540  yCError(FAKEMOTIONCONTROL) << "Malloc failed";
541  return false;
542  }
543 
544  // Default value
545  for(int i=0; i<_njoints; i++)
546  _newtonsToSensor[i] = 1;
547 
548  if(!fromConfig(config))
549  {
550  yCError(FAKEMOTIONCONTROL) << "Missing parameters in config file";
551  return false;
552  }
553 
554  // INIT ALL INTERFACES
555  yarp::sig::Vector tmpZeros; tmpZeros.resize (_njoints, 0.0);
556  yarp::sig::Vector tmpOnes; tmpOnes.resize (_njoints, 1.0);
557  yarp::sig::Vector bemfToRaw; bemfToRaw.resize(_njoints, 1.0);
558  yarp::sig::Vector ktauToRaw; ktauToRaw.resize(_njoints, 1.0);
559  bemfToRaw = yarp::sig::Vector(_njoints, _newtonsToSensor) / yarp::sig::Vector(_njoints, _angleToEncoder);
560  ktauToRaw = yarp::sig::Vector(_njoints, _dutycycleToPWM) / yarp::sig::Vector(_njoints, _newtonsToSensor);
561 
562  ControlBoardHelper cb(_njoints, _axisMap, _angleToEncoder, nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM);
563  ControlBoardHelper cb_copy_test(cb);
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);
570  ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
571  ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
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 
585  //start the rateThread
586  bool init = this->start();
587  if(!init)
588  {
589  yCError(FAKEMOTIONCONTROL) << "open() has an error in call of FakeMotionControl::init() for board" ;
590  return false;
591  }
592  else
593  {
594  if(verbosewhenok)
595  {
596  yCDebug(FAKEMOTIONCONTROL) << "init() has successfully initted board ";
597  }
598  }
599  opened = true;
600 
601  return true;
602 }
603 
604 bool FakeMotionControl::parseImpedanceGroup_NewFormat(Bottle& pidsGroup, ImpedanceParameters vals[])
605 {
606  int j=0;
607  Bottle xtmp;
608 
609  if (!extractGroup(pidsGroup, xtmp, "stiffness", "stiffness parameter", _njoints)) {
610  return false;
611  }
612  for (j=0; j<_njoints; j++) {
613  vals[j].stiffness = xtmp.get(j+1).asFloat64();
614  }
615 
616  if (!extractGroup(pidsGroup, xtmp, "damping", "damping parameter", _njoints)) {
617  return false;
618  }
619  for (j=0; j<_njoints; j++) {
620  vals[j].damping = xtmp.get(j+1).asFloat64();
621  }
622 
623  return true;
624 }
625 
626 bool FakeMotionControl::parsePositionPidsGroup(Bottle& pidsGroup, Pid myPid[])
627 {
628  int j=0;
629  Bottle xtmp;
630 
631  if (!extractGroup(pidsGroup, xtmp, "kp", "Pid kp parameter", _njoints)) {
632  return false;
633  }
634  for (j=0; j<_njoints; j++) {
635  myPid[j].kp = xtmp.get(j+1).asFloat64();
636  }
637 
638  if (!extractGroup(pidsGroup, xtmp, "kd", "Pid kd parameter", _njoints)) {
639  return false;
640  }
641  for (j=0; j<_njoints; j++) {
642  myPid[j].kd = xtmp.get(j+1).asFloat64();
643  }
644 
645  if (!extractGroup(pidsGroup, xtmp, "ki", "Pid kp parameter", _njoints)) {
646  return false;
647  }
648  for (j=0; j<_njoints; j++) {
649  myPid[j].ki = xtmp.get(j+1).asFloat64();
650  }
651 
652  if (!extractGroup(pidsGroup, xtmp, "maxInt", "Pid maxInt parameter", _njoints)) {
653  return false;
654  }
655  for (j=0; j<_njoints; j++) {
656  myPid[j].max_int = xtmp.get(j+1).asFloat64();
657  }
658 
659  if (!extractGroup(pidsGroup, xtmp, "maxPwm", "Pid maxPwm parameter", _njoints)) {
660  return false;
661  }
662  for (j=0; j<_njoints; j++) {
663  myPid[j].max_output = xtmp.get(j+1).asFloat64();
664  }
665 
666  if (!extractGroup(pidsGroup, xtmp, "shift", "Pid shift parameter", _njoints)) {
667  return false;
668  }
669  for (j=0; j<_njoints; j++) {
670  myPid[j].scale = xtmp.get(j+1).asFloat64();
671  }
672 
673  if (!extractGroup(pidsGroup, xtmp, "ko", "Pid ko parameter", _njoints)) {
674  return false;
675  }
676  for (j=0; j<_njoints; j++) {
677  myPid[j].offset = xtmp.get(j+1).asFloat64();
678  }
679 
680  if (!extractGroup(pidsGroup, xtmp, "stictionUp", "Pid stictionUp", _njoints)) {
681  return false;
682  }
683  for (j=0; j<_njoints; j++) {
684  myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
685  }
686 
687  if (!extractGroup(pidsGroup, xtmp, "stictionDwn", "Pid stictionDwn", _njoints)) {
688  return false;
689  }
690  for (j=0; j<_njoints; j++) {
691  myPid[j].stiction_down_val = xtmp.get(j+1).asFloat64();
692  }
693 
694  if (!extractGroup(pidsGroup, xtmp, "kff", "Pid kff parameter", _njoints)) {
695  return false;
696  }
697  for (j=0; j<_njoints; j++) {
698  myPid[j].kff = xtmp.get(j+1).asFloat64();
699  }
700 
701  //conversion from metric to machine units (if applicable)
702  if (_positionControlUnits==P_METRIC_UNITS)
703  {
704  for (j=0; j<_njoints; j++)
705  {
706  myPid[j].kp = myPid[j].kp / _angleToEncoder[j]; //[PWM/deg]
707  myPid[j].ki = myPid[j].ki / _angleToEncoder[j]; //[PWM/deg]
708  myPid[j].kd = myPid[j].kd / _angleToEncoder[j]; //[PWM/deg]
709  }
710  }
711  else
712  {
713  //do nothing
714  }
715 
716  //optional PWM limit
717  if(_pwmIsLimited)
718  { // check for value in the file
719  if (!extractGroup(pidsGroup, xtmp, "limPwm", "Limited PWD", _njoints))
720  {
721  yCError(FAKEMOTIONCONTROL) << "The PID parameter limPwm was requested but was not correctly set in the configuration file, please fill it.";
722  return false;
723  }
724 
725  yCInfo(FAKEMOTIONCONTROL) << "Using LIMITED PWM!!";
726  for (j=0; j<_njoints; j++)
727  myPid[j].max_output = xtmp.get(j+1).asFloat64();
728  }
729 
730  return true;
731 }
732 
733 bool FakeMotionControl::parseTorquePidsGroup(Bottle& pidsGroup, Pid myPid[], double kbemf[], double ktau[], int filterType[])
734 {
735  int j=0;
736  Bottle xtmp;
737  if (!extractGroup(pidsGroup, xtmp, "kp", "Pid kp parameter", _njoints)) {
738  return false;
739  }
740  for (j=0; j<_njoints; j++) {
741  myPid[j].kp = xtmp.get(j+1).asFloat64();
742  }
743 
744  if (!extractGroup(pidsGroup, xtmp, "kd", "Pid kd parameter", _njoints)) {
745  return false;
746  }
747  for (j=0; j<_njoints; j++) {
748  myPid[j].kd = xtmp.get(j+1).asFloat64();
749  }
750 
751  if (!extractGroup(pidsGroup, xtmp, "ki", "Pid kp parameter", _njoints)) {
752  return false;
753  }
754  for (j=0; j<_njoints; j++) {
755  myPid[j].ki = xtmp.get(j+1).asFloat64();
756  }
757 
758  if (!extractGroup(pidsGroup, xtmp, "maxInt", "Pid maxInt parameter", _njoints)) {
759  return false;
760  }
761  for (j=0; j<_njoints; j++) {
762  myPid[j].max_int = xtmp.get(j+1).asFloat64();
763  }
764 
765  if (!extractGroup(pidsGroup, xtmp, "maxPwm", "Pid maxPwm parameter", _njoints)) {
766  return false;
767  }
768  for (j=0; j<_njoints; j++) {
769  myPid[j].max_output = xtmp.get(j+1).asFloat64();
770  }
771 
772  if (!extractGroup(pidsGroup, xtmp, "shift", "Pid shift parameter", _njoints)) {
773  return false;
774  }
775  for (j=0; j<_njoints; j++) {
776  myPid[j].scale = xtmp.get(j+1).asFloat64();
777  }
778 
779  if (!extractGroup(pidsGroup, xtmp, "ko", "Pid ko parameter", _njoints)) {
780  return false;
781  }
782  for (j=0; j<_njoints; j++) {
783  myPid[j].offset = xtmp.get(j+1).asFloat64();
784  }
785 
786  if (!extractGroup(pidsGroup, xtmp, "stictionUp", "Pid stictionUp", _njoints)) {
787  return false;
788  }
789  for (j=0; j<_njoints; j++) {
790  myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
791  }
792 
793  if (!extractGroup(pidsGroup, xtmp, "stictionDwn", "Pid stictionDwn", _njoints)) {
794  return false;
795  }
796  for (j=0; j<_njoints; j++) {
797  myPid[j].stiction_down_val = xtmp.get(j+1).asFloat64();
798  }
799 
800  if (!extractGroup(pidsGroup, xtmp, "kff", "Pid kff parameter", _njoints)) {
801  return false;
802  }
803  for (j=0; j<_njoints; j++) {
804  myPid[j].kff = xtmp.get(j+1).asFloat64();
805  }
806 
807  if (!extractGroup(pidsGroup, xtmp, "kbemf", "kbemf parameter", _njoints)) {
808  return false;
809  }
810  for (j=0; j<_njoints; j++) {
811  kbemf[j] = xtmp.get(j+1).asFloat64();
812  }
813 
814  if (!extractGroup(pidsGroup, xtmp, "ktau", "ktau parameter", _njoints)) {
815  return false;
816  }
817  for (j=0; j<_njoints; j++) {
818  ktau[j] = xtmp.get(j+1).asFloat64();
819  }
820 
821  if (!extractGroup(pidsGroup, xtmp, "filterType", "filterType param", _njoints)) {
822  return false;
823  }
824  for (j=0; j<_njoints; j++) {
825  filterType[j] = xtmp.get(j+1).asInt32();
826  }
827 
828  //conversion from metric to machine units (if applicable)
829 // for (j=0; j<_njoints; j++)
830 // {
831 // myPid[j].kp = myPid[j].kp / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
832 // myPid[j].ki = myPid[j].ki / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
833 // myPid[j].kd = myPid[j].kd / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
834 // myPid[j].stiction_up_val = myPid[j].stiction_up_val * _torqueControlHelper->getNewtonsToSensor(j); //[Nm]
835 // myPid[j].stiction_down_val = myPid[j].stiction_down_val * _torqueControlHelper->getNewtonsToSensor(j); //[Nm]
836 // }
837 
838  //optional PWM limit
839  if(_pwmIsLimited)
840  { // check for value in the file
841  if (!extractGroup(pidsGroup, xtmp, "limPwm", "Limited PWM", _njoints))
842  {
843  yCError(FAKEMOTIONCONTROL) << "The PID parameter limPwm was requested but was not correctly set in the configuration file, please fill it.";
844  return false;
845  }
846 
847  yCInfo(FAKEMOTIONCONTROL) << "Using LIMITED PWM!!";
848  for (j=0; j<_njoints; j++) myPid[j].max_output = xtmp.get(j+1).asFloat64();
849  }
850 
851  return true;
852 }
853 
855 {
856  Bottle xtmp;
857  int i;
858  Bottle general = config.findGroup("GENERAL");
859 
860  // read AxisMap values from file
861  if(general.check("AxisMap"))
862  {
863  if(extractGroup(general, xtmp, "AxisMap", "a list of reordered indices for the axes", _njoints))
864  {
865  for (i = 1; (size_t) i < xtmp.size(); i++)
866  _axisMap[i - 1] = xtmp.get(i).asInt32();
867  }
868  else
869  return false;
870  }
871  else
872  {
873  yCInfo(FAKEMOTIONCONTROL) << "Using default AxisMap";
874  for (i = 0; i < _njoints; i++)
875  _axisMap[i] = i;
876  }
877 
878  if(general.check("AxisName"))
879  {
880  if (extractGroup(general, xtmp, "AxisName", "a list of strings representing the axes names", _njoints))
881  {
882  //beware: axis name has to be remapped here because they are not set using the toHw() helper function
883  for (i = 1; (size_t) i < xtmp.size(); i++)
884  {
885  _axisName[_axisMap[i - 1]] = xtmp.get(i).asString();
886  }
887  }
888  else
889  return false;
890  }
891  else
892  {
893  yCInfo(FAKEMOTIONCONTROL) << "Using default AxisName";
894  for (i = 0; i < _njoints; i++)
895  {
896  _axisName[_axisMap[i]] = "joint" + toString(i);
897  }
898  }
899  if(general.check("AxisType"))
900  {
901  if (extractGroup(general, xtmp, "AxisType", "a list of strings representing the axes type (revolute/prismatic)", _njoints))
902  {
903  //beware: axis type has to be remapped here because they are not set using the toHw() helper function
904  for (i = 1; (size_t) i < xtmp.size(); i++)
905  {
906  string typeString = xtmp.get(i).asString();
907  if (typeString == "revolute") _jointType[_axisMap[i - 1]] = VOCAB_JOINTTYPE_REVOLUTE;
908  else if (typeString == "prismatic") _jointType[_axisMap[i - 1]] = VOCAB_JOINTTYPE_PRISMATIC;
909  else
910  {
911  yCError(FAKEMOTIONCONTROL, "Unknown AxisType value %s!", typeString.c_str());
912  _jointType[_axisMap[i - 1]] = VOCAB_JOINTTYPE_UNKNOWN;
913  return false;
914  }
915  }
916  }
917  else
918  return false;
919  }
920  else
921  {
922  yCInfo(FAKEMOTIONCONTROL) << "Using default AxisType (revolute)";
923  for (i = 0; i < _njoints; i++)
924  {
925  _jointType[_axisMap[i]] = VOCAB_JOINTTYPE_REVOLUTE;
926  }
927  }
928 
929  // current conversions factor
930  if (general.check("ampsToSensor"))
931  {
932  if (extractGroup(general, xtmp, "ampsToSensor", "a list of scales for the ampsToSensor conversion factors", _njoints))
933  {
934  for (i = 1; (size_t) i < xtmp.size(); i++)
935  {
936  if (xtmp.get(i).isFloat64())
937  {
938  _ampsToSensor[i - 1] = xtmp.get(i).asFloat64();
939  }
940  }
941  }
942  else
943  return false;
944  }
945  else
946  {
947  yCInfo(FAKEMOTIONCONTROL) << "Using default ampsToSensor";
948  for (i = 0; i < _njoints; i++)
949  {
950  _ampsToSensor[i] = 1.0;
951  }
952  }
953 
954  // pwm conversions factor
955  if (general.check("fullscalePWM"))
956  {
957  if (extractGroup(general, xtmp, "fullscalePWM", "a list of scales for the fullscalePWM conversion factors", _njoints))
958  {
959  for (i = 1; (size_t) i < xtmp.size(); i++)
960  {
961  if (xtmp.get(i).isFloat64() || xtmp.get(i).isInt32())
962  {
963  _dutycycleToPWM[i - 1] = xtmp.get(i).asFloat64() / 100.0;
964  }
965  }
966  }
967  else
968  return false;
969  }
970  else
971  {
972  yCInfo(FAKEMOTIONCONTROL) << "Using default dutycycleToPWM=1.0";
973  for (i = 0; i < _njoints; i++)
974  _dutycycleToPWM[i] = 1.0;
975  }
976 
977 // double tmp_A2E;
978  // Encoder scales
979  if(general.check("Encoder"))
980  {
981  if (extractGroup(general, xtmp, "Encoder", "a list of scales for the encoders", _njoints))
982  {
983  for (i = 1; (size_t) i < xtmp.size(); i++)
984  {
985  _angleToEncoder[i-1] = xtmp.get(i).asFloat64();
986  }
987  }
988  else
989  return false;
990  }
991  else
992  {
993  yCInfo(FAKEMOTIONCONTROL) << "Using default Encoder";
994  for (i = 0; i < _njoints; i++)
995  _angleToEncoder[i] = 1;
996  }
997 
998  // Joint encoder resolution
999  /*if (!extractGroup(general, xtmp, "JointEncoderRes", "the resolution of the joint encoder", _njoints))
1000  {
1001  return false;
1002  }
1003  else
1004  {
1005  int test = xtmp.size();
1006  for (i = 1; i < xtmp.size(); i++)
1007  _jointEncoderRes[i - 1] = xtmp.get(i).asInt32();
1008  }
1009 
1010  // Joint encoder type
1011  if (!extractGroup(general, xtmp, "JointEncoderType", "JointEncoderType", _njoints))
1012  {
1013  return false;
1014  }
1015  else
1016  {
1017  int test = xtmp.size();
1018  for (i = 1; i < xtmp.size(); i++)
1019  {
1020  uint8_t val;
1021  string s = xtmp.get(i).asString();
1022  bool b = EncoderType_iCub2eo(&s, &val);
1023  if (b == false)
1024  {
1025  yCError(FAKEMOTIONCONTROL, "Invalid JointEncoderType: %s!", s.c_str()); return false;
1026  }
1027 // _jointEncoderType[i - 1] = val;
1028  }
1029  }
1030 */
1031 
1032  // Motor capabilities
1033 /* if (!extractGroup(general, xtmp, "HasHallSensor", "HasHallSensor 0/1 ", _njoints))
1034  {
1035  return false;
1036  }
1037  else
1038  {
1039  int test = xtmp.size();
1040  for (i = 1; i < xtmp.size(); i++)
1041  _hasHallSensor[i - 1] = xtmp.get(i).asInt32();
1042  }
1043  if (!extractGroup(general, xtmp, "HasTempSensor", "HasTempSensor 0/1 ", _njoints))
1044  {
1045  return false;
1046  }
1047  else
1048  {
1049  int test = xtmp.size();
1050  for (i = 1; i < xtmp.size(); i++)
1051  _hasTempSensor[i - 1] = xtmp.get(i).asInt32();
1052  }
1053  if (!extractGroup(general, xtmp, "HasRotorEncoder", "HasRotorEncoder 0/1 ", _njoints))
1054  {
1055  return false;
1056  }
1057  else
1058  {
1059  int test = xtmp.size();
1060  for (i = 1; i < xtmp.size(); i++)
1061  _hasRotorEncoder[i - 1] = xtmp.get(i).asInt32();
1062  }
1063  if (!extractGroup(general, xtmp, "HasRotorEncoderIndex", "HasRotorEncoderIndex 0/1 ", _njoints))
1064  {
1065  return false;
1066  }
1067  else
1068  {
1069  int test = xtmp.size();
1070  for (i = 1; i < xtmp.size(); i++)
1071  _hasRotorEncoderIndex[i - 1] = xtmp.get(i).asInt32();
1072  }
1073 
1074  // Rotor encoder res
1075  if (!extractGroup(general, xtmp, "RotorEncoderRes", "a list of scales for the rotor encoders", _njoints))
1076  {
1077  return false;
1078  }
1079  else
1080  {
1081  int test = xtmp.size();
1082  for (i = 1; i < xtmp.size(); i++)
1083  _rotorEncoderRes[i - 1] = xtmp.get(i).asInt32();
1084  }
1085 
1086  // joint encoder res
1087  if (!extractGroup(general, xtmp, "JointEncoderRes", "a list of scales for the joint encoders", _njoints))
1088  {
1089  return false;
1090  }
1091  else
1092  {
1093  int test = xtmp.size();
1094  for (i = 1; i < xtmp.size(); i++)
1095  _jointEncoderRes[i - 1] = xtmp.get(i).asInt32();
1096  }
1097 */
1098  // Number of motor poles
1099  /*if (!extractGroup(general, xtmp, "MotorPoles", "MotorPoles", _njoints))
1100  {
1101  return false;
1102  }
1103  else
1104  {
1105  int test = xtmp.size();
1106  for (i = 1; i < xtmp.size(); i++)
1107  _motorPoles[i - 1] = xtmp.get(i).asInt32();
1108  }
1109 
1110  // Rotor encoder index
1111  if (!extractGroup(general, xtmp, "RotorIndexOffset", "RotorIndexOffset", _njoints))
1112  {
1113  return false;
1114  }
1115  else
1116  {
1117  int test = xtmp.size();
1118  for (i = 1; i < xtmp.size(); i++)
1119  _rotorIndexOffset[i - 1] = xtmp.get(i).asInt32();
1120  }
1121 */
1122 
1123  // Rotor encoder type
1124 /*
1125  if (!extractGroup(general, xtmp, "RotorEncoderType", "RotorEncoderType", _njoints))
1126  {
1127  return false;
1128  }
1129  else
1130  {
1131  int test = xtmp.size();
1132  for (i = 1; i < xtmp.size(); i++)
1133  {
1134  uint8_t val;
1135  string s = xtmp.get(i).asString();
1136  bool b = EncoderType_iCub2eo(&s, &val);
1137  if (b == false)
1138  {
1139  yCError(FAKEMOTIONCONTROL, "Invalid RotorEncoderType: %s", s.c_str()); return false;
1140  }
1141  _rotorEncoderType[i - 1] = val;
1142  }
1143  }
1144 */
1145 /*
1146  // Gearbox
1147  if (!extractGroup(general, xtmp, "Gearbox", "The gearbox reduction ratio", _njoints))
1148  {
1149  return false;
1150  }
1151  else
1152  {
1153  int test = xtmp.size();
1154  for (i = 1; i < xtmp.size(); i++)
1155  {
1156  _gearbox[i-1] = xtmp.get(i).asFloat64();
1157  if (_gearbox[i-1]==0) {yCError(FAKEMOTIONCONTROL) << "Using a gearbox value = 0 may cause problems! Check your configuration files"; return false;}
1158  }
1159  }
1160 
1161  // Torque sensors stuff
1162  if (!extractGroup(general, xtmp, "TorqueId","a list of associated joint torque sensor ids", _njoints))
1163  {
1164  yCWarning(FAKEMOTIONCONTROL, "Using default value = 0 (disabled)");
1165  for(i=1; i<_njoints+1; i++)
1166  _torqueSensorId[i-1] = 0;
1167  }
1168  else
1169  {
1170  for (i = 1; i < xtmp.size(); i++) _torqueSensorId[i-1] = xtmp.get(i).asInt32();
1171  }
1172 
1173 
1174  if (!extractGroup(general, xtmp, "TorqueChan","a list of associated joint torque sensor channels", _njoints))
1175  {
1176  yCWarning(FAKEMOTIONCONTROL) << "fromConfig() detected that TorqueChan is not present: using default value = 0 (disabled)";
1177  for(i=1; i<_njoints+1; i++)
1178  _torqueSensorChan[i-1] = 0;
1179  }
1180  else
1181  {
1182  for (i = 1; i < xtmp.size(); i++) _torqueSensorChan[i-1] = xtmp.get(i).asInt32();
1183  }
1184 
1185 
1186  if (!extractGroup(general, xtmp, "TorqueMax","full scale value for a joint torque sensor", _njoints))
1187  {
1188  return false;
1189  }
1190  else
1191  {
1192  for (i = 1; i < xtmp.size(); i++)
1193  {
1194  _maxTorque[i-1] = xtmp.get(i).asInt32();
1195  _newtonsToSensor[i-1] = 1000.0f; // conversion from Nm into milliNm
1196  }
1197  }
1198 */
1199 
1201 /*
1202  {
1203  Bottle posPidsGroup;
1204  posPidsGroup=config.findGroup("POSITION_CONTROL", "Position Pid parameters new format");
1205  if (posPidsGroup.isNull()==false)
1206  {
1207  Value &controlUnits=posPidsGroup.find("controlUnits");
1208  if (controlUnits.isNull() == false && controlUnits.isString() == true)
1209  {
1210  if (controlUnits.toString()==string("metric_units"))
1211  {
1212  yCDebug(FAKEMOTIONCONTROL, "POSITION_CONTROL: using metric_units"); _positionControlUnits=P_METRIC_UNITS;
1213  }
1214  else if (controlUnits.toString()==string("machine_units"))
1215  {
1216  yCDebug(FAKEMOTIONCONTROL, "POSITION_CONTROL: using machine_units"); _positionControlUnits=P_MACHINE_UNITS;
1217  }
1218  else
1219  {
1220  yCError(FAKEMOTIONCONTROL) << "fromConfig(): POSITION_CONTROL section: invalid controlUnits value";
1221  return false;
1222  }
1223  }
1224  else
1225  {
1226  yCError(FAKEMOTIONCONTROL) << "fromConfig(): POSITION_CONTROL section: missing controlUnits parameter. Assuming machine_units. Please fix your configuration file.";
1227  _positionControlUnits=P_MACHINE_UNITS;
1228  }
1229 
1230 // Value &controlLaw=posPidsGroup.find("controlLaw");
1231 // if (controlLaw.isNull() == false && controlLaw.isString() == true)
1232 // {
1233 // string s_controlaw = controlLaw.toString();
1234 // if (s_controlaw==string("joint_pid_v1"))
1235 // {
1236 // if (!parsePositionPidsGroup (posPidsGroup, _pids))
1237 // {
1238 // yCError(FAKEMOTIONCONTROL) << "fromConfig(): POSITION_CONTROL section: error detected in parameters syntax";
1239 // return false;
1240 // }
1241 // else
1242 // {
1243 // yCDebug(FAKEMOTIONCONTROL, "POSITION_CONTROL: using control law joint_pid_v1");
1244 // }
1245 // }
1246 // else if (s_controlaw==string("not_implemented"))
1247 // {
1248 // yCDebug(FAKEMOTIONCONTROL) << "found 'not_impelemented' in position control_law. This will terminate yarprobotinterface execution.";
1249 // return false;
1250 // }
1251 // else if (s_controlaw==string("disabled"))
1252 // {
1253 // yCDebug(FAKEMOTIONCONTROL) << "found 'disabled' in position control_law. This will terminate yarprobotinterface execution.";
1254 // return false;
1255 // }
1256 // else
1257 // {
1258 // yCError(FAKEMOTIONCONTROL) << "Unable to use control law " << s_controlaw << " por position control. Quitting.";
1259 // return false;
1260 // }
1261 // }
1262  }
1263  else
1264  {
1265  yCError(FAKEMOTIONCONTROL) <<"fromConfig(): Error: no POS_PIDS group found in config file, returning";
1266  return false;
1267  }
1268  }
1269 */
1270 
1271 
1273 /*
1274  {
1275  Bottle trqPidsGroup;
1276  trqPidsGroup=config.findGroup("TORQUE_CONTROL", "Torque control parameters new format");
1277  if (trqPidsGroup.isNull()==false)
1278  {
1279  Value &controlUnits=trqPidsGroup.find("controlUnits");
1280  if (controlUnits.isNull() == false && controlUnits.isString() == true)
1281  {
1282  if (controlUnits.toString()==string("metric_units")) {yCDebug(FAKEMOTIONCONTROL, "TORQUE_CONTROL: using metric_units"); _torqueControlUnits=T_METRIC_UNITS;}
1283  else if (controlUnits.toString()==string("machine_units")) {yCDebug(FAKEMOTIONCONTROL, "TORQUE_CONTROL: using metric_units"); _torqueControlUnits=T_MACHINE_UNITS;}
1284  else {yCError(FAKEMOTIONCONTROL) << "fromConfig(): TORQUE_CONTROL section: invalid controlUnits value";
1285  return false;}
1286  }
1287  else
1288  {
1289  yCError(FAKEMOTIONCONTROL) << "fromConfig(): TORQUE_CONTROL section: missing controlUnits parameter. Assuming machine_units. Please fix your configuration file.";
1290  _torqueControlUnits=T_MACHINE_UNITS;
1291  }
1292 
1293  if(_torqueControlUnits==T_MACHINE_UNITS)
1294  {
1295  yarp::sig::Vector tmpOnes; tmpOnes.resize(_njoints,1.0);
1296  }
1297  else if (_torqueControlUnits==T_METRIC_UNITS)
1298  {
1299  }
1300  else
1301  {
1302  yCError(FAKEMOTIONCONTROL) << "fromConfig(): TORQUE_CONTROL section: invalid controlUnits value (_torqueControlUnits=" << _torqueControlUnits << ")";
1303  return false;
1304  }
1305  }
1306  else
1307  {
1308  yCError(FAKEMOTIONCONTROL) <<"fromConfig(): Error: no TORQUE_CONTROL group found in config file";
1309  _torqueControlEnabled = false;
1310  return false; //torque control group is mandatory
1311  }
1312  }
1313 */
1314 
1316 /*
1317  for(j=0; j<_njoints; j++)
1318  {
1319  // got from canBusMotionControl, ask to Randazzo Marco
1320  _impedance_limits[j].min_damp= 0.001;
1321  _impedance_limits[j].max_damp= 9.888;
1322  _impedance_limits[j].min_stiff= 0.002;
1323  _impedance_limits[j].max_stiff= 9.889;
1324  _impedance_limits[j].param_a= 0.011;
1325  _impedance_limits[j].param_b= 0.012;
1326  _impedance_limits[j].param_c= 0.013;
1327  }
1328 */
1329 
1331 /*
1332  if (_njoints<=4)
1333  {
1334  Bottle &coupling=config.findGroup("JOINTS_COUPLING");
1335  if (coupling.isNull())
1336  {
1337  yCWarning(FAKEMOTIONCONTROL) << "fromConfig() detected that Group JOINTS_COUPLING is not found in configuration file";
1338  //return false;
1339  }
1340  // current limit
1341  if (!extractGroup(coupling, xtmp, "kinematic_mj","the kinematic matrix 4x4 which transforms from joint space to motor space", 16))
1342  {
1343  for(i=1; i<xtmp.size(); i++) _kinematic_mj[i-1]=0.0;
1344  }
1345  else
1346  for(i=1; i<xtmp.size(); i++) _kinematic_mj[i-1]=xtmp.get(i).asFloat64();
1347  }
1348  else
1349  {
1350  //we are skipping JOINTS_COUPLING for EMS boards which control MC4 boards (for now)
1351  }
1352 */
1353 
1355 /*
1356  Bottle &limits=config.findGroup("LIMITS");
1357  if (limits.isNull())
1358  {
1359  yCWarning(FAKEMOTIONCONTROL) << "fromConfig() detected that Group LIMITS is not found in configuration file";
1360  return false;
1361  }
1362  // current limit
1363  if (!extractGroup(limits, xtmp, "OverloadCurrents","a list of current limits", _njoints))
1364  return false;
1365  else
1366  for(i=1; i<xtmp.size(); i++) _currentLimits[i-1].overloadCurrent=xtmp.get(i).asFloat64();
1367 
1368  // nominal current
1369  if (!extractGroup(limits, xtmp, "MotorNominalCurrents","a list of nominal current limits", _njoints))
1370  return false;
1371  else
1372  for(i=1; i<xtmp.size(); i++) _currentLimits[i-1].nominalCurrent =xtmp.get(i).asFloat64();
1373 
1374  // nominal current
1375  if (!extractGroup(limits, xtmp, "MotorPeakCurrents","a list of peak current limits", _njoints))
1376  return false;
1377  else
1378  for(i=1; i<xtmp.size(); i++) _currentLimits[i-1].peakCurrent=xtmp.get(i).asFloat64();
1379 
1380  // max limit
1381  if (!extractGroup(limits, xtmp, "Max","a list of maximum angles (in degrees)", _njoints))
1382  return false;
1383  else
1384  for(i=1; i<xtmp.size(); i++) _limitsMax[i-1]=xtmp.get(i).asFloat64();
1385 
1386  // min limit
1387  if (!extractGroup(limits, xtmp, "Min","a list of minimum angles (in degrees)", _njoints))
1388  return false;
1389  else
1390  for(i=1; i<xtmp.size(); i++) _limitsMin[i-1]=xtmp.get(i).asFloat64();
1391 
1392  // Rotor max limit
1393  if (!extractGroup(limits, xtmp, "RotorMax","a list of maximum rotor angles (in degrees)", _njoints))
1394  return false;
1395  else
1396  for(i=1; i<xtmp.size(); i++) _rotorlimits_max[i-1]=xtmp.get(i).asFloat64();
1397 
1398  // joint Velocity command max limit
1399  if (!extractGroup(limits, xtmp, "JntVelocityMax", "a list of maximum velocities for the joints (in degrees/s)", _njoints))
1400  return false;
1401  else
1402  for (i = 1; i<xtmp.size(); i++) _maxJntCmdVelocity[i - 1] = xtmp.get(i).asFloat64();
1403 
1404  // Rotor min limit
1405  if (!extractGroup(limits, xtmp, "RotorMin","a list of minimum roto angles (in degrees)", _njoints))
1406  return false;
1407  else
1408  for(i=1; i<xtmp.size(); i++) _rotorlimits_min[i-1]=xtmp.get(i).asFloat64();
1409 
1410  // Motor pwm limit
1411  if (!extractGroup(limits, xtmp, "MotorPwmLimit","a list of motor PWM limits", _njoints))
1412  return false;
1413  else
1414  {
1415  for(i=1; i<xtmp.size(); i++)
1416  {
1417  _motorPwmLimits[i-1]=xtmp.get(i).asFloat64();
1418  if(_motorPwmLimits[i-1]<0)
1419  {
1420  yCError(FAKEMOTIONCONTROL) << "MotorPwmLimit should be a positive value";
1421  return false;
1422  }
1423  }
1424  }
1425 */
1426  return true;
1427 }
1428 
1429 
1431 {
1432  yCTrace(FAKEMOTIONCONTROL) << " close()";
1433 
1434  ImplementControlMode::uninitialize();
1435  ImplementEncodersTimed::uninitialize();
1436  ImplementMotorEncoders::uninitialize();
1437  ImplementPositionControl::uninitialize();
1438  ImplementVelocityControl::uninitialize();
1439  ImplementPidControl::uninitialize();
1440  ImplementControlCalibration::uninitialize();
1441  ImplementAmplifierControl::uninitialize();
1442  ImplementImpedanceControl::uninitialize();
1443  ImplementControlLimits::uninitialize();
1444  ImplementTorqueControl::uninitialize();
1445  ImplementPositionDirect::uninitialize();
1446  ImplementInteractionMode::uninitialize();
1447  ImplementAxisInfo::uninitialize();
1448  ImplementVirtualAnalogSensor::uninitialize();
1449 
1450 // cleanup();
1451 
1452  return true;
1453 }
1454 
1455 void FakeMotionControl::cleanup()
1456 {
1457 
1458 }
1459 
1460 
1461 
1463 
1464 bool FakeMotionControl::setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid)
1465 {
1466  yCDebug(FAKEMOTIONCONTROL) << "setPidRaw" << pidtype << j << pid.kp;
1467  switch (pidtype)
1468  {
1470  _ppids[j]=pid;
1471  break;
1473  _vpids[j]=pid;
1474  break;
1475  case VOCAB_PIDTYPE_CURRENT:
1476  _cpids[j]=pid;
1477  break;
1478  case VOCAB_PIDTYPE_TORQUE:
1479  _tpids[j]=pid;
1480  break;
1481  default:
1482  break;
1483  }
1484  return true;
1485 }
1486 
1487 bool FakeMotionControl::setPidsRaw(const PidControlTypeEnum& pidtype, const Pid *pids)
1488 {
1489  bool ret = true;
1490  for(int j=0; j< _njoints; j++)
1491  {
1492  ret &= setPidRaw(pidtype, j, pids[j]);
1493  }
1494  return ret;
1495 }
1496 
1497 bool FakeMotionControl::setPidReferenceRaw(const PidControlTypeEnum& pidtype, int j, double ref)
1498 {
1499  switch (pidtype)
1500  {
1502  _ppids_ref[j]=ref;
1503  break;
1505  _vpids_ref[j]=ref;
1506  break;
1507  case VOCAB_PIDTYPE_CURRENT:
1508  _cpids_ref[j]=ref;
1509  break;
1510  case VOCAB_PIDTYPE_TORQUE:
1511  _tpids_ref[j]=ref;
1512  break;
1513  default:
1514  break;
1515  }
1516  return true;
1517 }
1518 
1519 bool FakeMotionControl::setPidReferencesRaw(const PidControlTypeEnum& pidtype, const double *refs)
1520 {
1521  bool ret = true;
1522  for(int j=0, index=0; j< _njoints; j++, index++)
1523  {
1524  ret &= setPidReferenceRaw(pidtype, j, refs[index]);
1525  }
1526  return ret;
1527 }
1528 
1529 bool FakeMotionControl::setPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int j, double limit)
1530 {
1531  switch (pidtype)
1532  {
1534  _ppids_lim[j]=limit;
1535  break;
1537  _vpids_lim[j]=limit;
1538  break;
1539  case VOCAB_PIDTYPE_CURRENT:
1540  _cpids_lim[j]=limit;
1541  break;
1542  case VOCAB_PIDTYPE_TORQUE:
1543  _tpids_lim[j]=limit;
1544  break;
1545  default:
1546  break;
1547  }
1548  return true;
1549 }
1550 
1551 bool FakeMotionControl::setPidErrorLimitsRaw(const PidControlTypeEnum& pidtype, const double *limits)
1552 {
1553  bool ret = true;
1554  for(int j=0, index=0; j< _njoints; j++, index++)
1555  {
1556  ret &= setPidErrorLimitRaw(pidtype, j, limits[index]);
1557  }
1558  return ret;
1559 }
1560 
1561 bool FakeMotionControl::getPidErrorRaw(const PidControlTypeEnum& pidtype, int j, double *err)
1562 {
1563  switch (pidtype)
1564  {
1566  *err=0.1;
1567  break;
1569  *err=0.2;
1570  break;
1571  case VOCAB_PIDTYPE_CURRENT:
1572  *err=0.3;
1573  break;
1574  case VOCAB_PIDTYPE_TORQUE:
1575  *err=0.4;
1576  break;
1577  default:
1578  break;
1579  }
1580  return true;
1581 }
1582 
1583 bool FakeMotionControl::getPidErrorsRaw(const PidControlTypeEnum& pidtype, double *errs)
1584 {
1585  bool ret = true;
1586  for(int j=0; j< _njoints; j++)
1587  {
1588  ret &= getPidErrorRaw(pidtype, j, &errs[j]);
1589  }
1590  return ret;
1591 }
1592 
1593 bool FakeMotionControl::getPidRaw(const PidControlTypeEnum& pidtype, int j, Pid *pid)
1594 {
1595  switch (pidtype)
1596  {
1598  *pid=_ppids[j];
1599  break;
1601  *pid=_vpids[j];
1602  break;
1603  case VOCAB_PIDTYPE_CURRENT:
1604  *pid=_cpids[j];
1605  break;
1606  case VOCAB_PIDTYPE_TORQUE:
1607  *pid=_tpids[j];
1608  break;
1609  default:
1610  break;
1611  }
1612  yCDebug(FAKEMOTIONCONTROL) << "getPidRaw" << pidtype << j << pid->kp;
1613  return true;
1614 }
1615 
1617 {
1618  bool ret = true;
1619 
1620  // just one joint at time, wait answer before getting to the next.
1621  // This is because otherwise too many msg will be placed into can queue
1622  for(int j=0, index=0; j<_njoints; j++, index++)
1623  {
1624  ret &=getPidRaw(pidtype, j, &pids[j]);
1625  }
1626  return ret;
1627 }
1628 
1629 bool FakeMotionControl::getPidReferenceRaw(const PidControlTypeEnum& pidtype, int j, double *ref)
1630 {
1631  switch (pidtype)
1632  {
1634  *ref=_ppids_ref[j];
1635  break;
1637  *ref=_vpids_ref[j];
1638  break;
1639  case VOCAB_PIDTYPE_CURRENT:
1640  *ref=_cpids_ref[j];
1641  break;
1642  case VOCAB_PIDTYPE_TORQUE:
1643  *ref=_tpids_ref[j];
1644  break;
1645  default:
1646  break;
1647  }
1648  return true;
1649 }
1650 
1652 {
1653  bool ret = true;
1654 
1655  // just one joint at time, wait answer before getting to the next.
1656  // This is because otherwise too many msg will be placed into can queue
1657  for(int j=0; j< _njoints; j++)
1658  {
1659  ret &= getPidReferenceRaw(pidtype, j, &refs[j]);
1660  }
1661  return ret;
1662 }
1663 
1664 bool FakeMotionControl::getPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int j, double *limit)
1665 {
1666  switch (pidtype)
1667  {
1669  *limit=_ppids_lim[j];
1670  break;
1672  *limit=_vpids_lim[j];
1673  break;
1674  case VOCAB_PIDTYPE_CURRENT:
1675  *limit=_cpids_lim[j];
1676  break;
1677  case VOCAB_PIDTYPE_TORQUE:
1678  *limit=_tpids_lim[j];
1679  break;
1680  default:
1681  break;
1682  }
1683  return true;
1684 }
1685 
1687 {
1688  bool ret = true;
1689  for(int j=0, index=0; j<_njoints; j++, index++)
1690  {
1691  ret &=getPidErrorLimitRaw(pidtype, j, &limits[j]);
1692  }
1693  return ret;
1694 }
1695 
1697 {
1698  return true;
1699 }
1700 
1702 {
1703  switch (pidtype)
1704  {
1706  _ppids_ena[j]=false;
1707  break;
1709  _vpids_ena[j]=false;
1710  break;
1711  case VOCAB_PIDTYPE_CURRENT:
1712  _cpids_ena[j]=false;
1713  break;
1714  case VOCAB_PIDTYPE_TORQUE:
1715  _tpids_ena[j]=false;
1716  break;
1717  default:
1718  break;
1719  }
1720  return true;
1721 }
1722 
1724 {
1725  switch (pidtype)
1726  {
1728  _ppids_ena[j]=true;
1729  break;
1731  _vpids_ena[j]=true;
1732  break;
1733  case VOCAB_PIDTYPE_CURRENT:
1734  _cpids_ena[j]=true;
1735  break;
1736  case VOCAB_PIDTYPE_TORQUE:
1737  _tpids_ena[j]=true;
1738  break;
1739  default:
1740  break;
1741  }
1742  return true;
1743 }
1744 
1745 bool FakeMotionControl::setPidOffsetRaw(const PidControlTypeEnum& pidtype, int j, double v)
1746 {
1747  yCDebug(FAKEMOTIONCONTROL) << "setPidOffsetRaw" << pidtype << j << v;
1748  switch (pidtype)
1749  {
1751  _ppids[j].offset=v;
1752  break;
1754  _vpids[j].offset=v;
1755  break;
1756  case VOCAB_PIDTYPE_CURRENT:
1757  _cpids[j].offset=v;
1758  break;
1759  case VOCAB_PIDTYPE_TORQUE:
1760  _tpids[j].offset=v;
1761  break;
1762  default:
1763  break;
1764  }
1765  return true;
1766 }
1767 
1768 bool FakeMotionControl::isPidEnabledRaw(const PidControlTypeEnum& pidtype, int j, bool* enabled)
1769 {
1770  switch (pidtype)
1771  {
1773  *enabled=_ppids_ena[j];
1774  break;
1776  *enabled=_vpids_ena[j];
1777  break;
1778  case VOCAB_PIDTYPE_CURRENT:
1779  *enabled=_cpids_ena[j];
1780  break;
1781  case VOCAB_PIDTYPE_TORQUE:
1782  *enabled=_tpids_ena[j];
1783  break;
1784  default:
1785  break;
1786  }
1787  return true;
1788 }
1789 
1790 bool FakeMotionControl::getPidOutputRaw(const PidControlTypeEnum& pidtype, int j, double *out)
1791 {
1792  switch (pidtype)
1793  {
1795  *out=1.1 + j * 10;
1796  break;
1798  *out=1.2 + j * 10;
1799  break;
1800  case VOCAB_PIDTYPE_CURRENT:
1801  *out=1.3 + j * 10;
1802  break;
1803  case VOCAB_PIDTYPE_TORQUE:
1804  *out=1.4 + j * 10;
1805  break;
1806  default:
1807  break;
1808  }
1809  yCDebug(FAKEMOTIONCONTROL) << "getPidOutputRaw" << pidtype << j << *out;
1810  return true;
1811 }
1812 
1814 {
1815  bool ret = true;
1816  for(int j=0; j< _njoints; j++)
1817  {
1818  ret &= getPidOutputRaw(pidtype, j, &outs[j]);
1819  }
1820  return ret;
1821 }
1822 
1824 // Velocity control interface raw //
1826 
1828 {
1829  int mode=0;
1830  getControlModeRaw(j, &mode);
1831  if( (mode != VOCAB_CM_VELOCITY) &&
1832  (mode != VOCAB_CM_MIXED) &&
1833  (mode != VOCAB_CM_IMPEDANCE_VEL) &&
1834  (mode != VOCAB_CM_IDLE))
1835  {
1836  yCError(FAKEMOTIONCONTROL) << "velocityMoveRaw: skipping command because board " << " joint " << j << " is not in VOCAB_CM_VELOCITY mode";
1837  }
1838  _command_speeds[j] = sp;
1839  last_velocity_command[j]=yarp::os::Time::now();
1840  return true;
1841 }
1842 
1844 {
1845  yCTrace(FAKEMOTIONCONTROL);
1846  bool ret = true;
1847  for(int i=0; i<_njoints; i++)
1848  ret &= velocityMoveRaw(i, sp[i]);
1849  return ret;
1850 }
1851 
1852 
1854 // Calibration control interface //
1856 
1858 {
1859  yCTrace(FAKEMOTIONCONTROL) << "setCalibrationParametersRaw for joint" << j;
1860  return true;
1861 }
1862 
1863 bool FakeMotionControl::calibrateAxisWithParamsRaw(int j, unsigned int type, double p1, double p2, double p3)
1864 {
1865  yCTrace(FAKEMOTIONCONTROL) << "calibrateRaw for joint" << j;
1866  return true;
1867 }
1868 
1870 {
1871  bool result = false;
1872 
1873  return result;
1874 }
1875 
1877 // Position control interface //
1879 
1881 {
1882  *ax=_njoints;
1883 
1884  return true;
1885 }
1886 
1887 bool FakeMotionControl::positionMoveRaw(int j, double ref)
1888 {
1889  if(verbose >= VERY_VERBOSE)
1890  yCTrace(FAKEMOTIONCONTROL) << "j " << j << " ref " << ref;
1891 
1892 // if (yarp::os::Time::now()-_last_position_move_time[j]<MAX_POSITION_MOVE_INTERVAL)
1893 // {
1894 // yCWarning(FAKEMOTIONCONTROL) << "Performance warning: You are using positionMove commands at high rate (<"<< MAX_POSITION_MOVE_INTERVAL*1000.0 <<" ms). Probably position control mode is not the right control mode to use.";
1895 // }
1896 // _last_position_move_time[j] = yarp::os::Time::now();
1897 
1898  int mode = 0;
1899  getControlModeRaw(j, &mode);
1900  if( (mode != VOCAB_CM_POSITION) &&
1901  (mode != VOCAB_CM_MIXED) &&
1902  (mode != VOCAB_CM_IMPEDANCE_POS) &&
1903  (mode != VOCAB_CM_IDLE))
1904  {
1905  yCError(FAKEMOTIONCONTROL) << "positionMoveRaw: skipping command because joint " << j << " is not in VOCAB_CM_POSITION mode";
1906  }
1907  _posCtrl_references[j] = ref;
1908  return true;
1909 }
1910 
1911 bool FakeMotionControl::positionMoveRaw(const double *refs)
1912 {
1913  if(verbose >= VERY_VERBOSE)
1914  yCTrace(FAKEMOTIONCONTROL);
1915 
1916  bool ret = true;
1917  for(int j=0, index=0; j< _njoints; j++, index++)
1918  {
1919  ret &= positionMoveRaw(j, refs[index]);
1920  }
1921  return ret;
1922 }
1923 
1924 bool FakeMotionControl::relativeMoveRaw(int j, double delta)
1925 {
1926  if(verbose >= VERY_VERBOSE)
1927  yCTrace(FAKEMOTIONCONTROL) << "j " << j << " ref " << delta;
1928 // if (yarp::os::Time::now()-_last_position_move_time[j]<MAX_POSITION_MOVE_INTERVAL)
1929 // {
1930 // yCWarning(FAKEMOTIONCONTROL) << "Performance warning: You are using positionMove commands at high rate (<"<< MAX_POSITION_MOVE_INTERVAL*1000.0 <<" ms). Probably position control mode is not the right control mode to use.";
1931 // }
1932 // _last_position_move_time[j] = yarp::os::Time::now();
1933 
1934  int mode = 0;
1935  getControlModeRaw(j, &mode);
1936  if( (mode != VOCAB_CM_POSITION) &&
1937  (mode != VOCAB_CM_MIXED) &&
1938  (mode != VOCAB_CM_IMPEDANCE_POS) &&
1939  (mode != VOCAB_CM_IDLE))
1940  {
1941  yCError(FAKEMOTIONCONTROL) << "relativeMoveRaw: skipping command because joint " << j << " is not in VOCAB_CM_POSITION mode";
1942  }
1943  _posCtrl_references[j] += delta;
1944  return false;
1945 }
1946 
1947 bool FakeMotionControl::relativeMoveRaw(const double *deltas)
1948 {
1949  if(verbose >= VERY_VERBOSE)
1950  yCTrace(FAKEMOTIONCONTROL);
1951 
1952  bool ret = true;
1953  for(int j=0, index=0; j< _njoints; j++, index++)
1954  {
1955  ret &= relativeMoveRaw(j, deltas[index]);
1956  }
1957  return ret;
1958 }
1959 
1960 
1962 {
1963  if(verbose >= VERY_VERBOSE)
1964  yCTrace(FAKEMOTIONCONTROL) << "j ";
1965 
1966  *flag = false;
1967  return false;
1968 }
1969 
1971 {
1972  if(verbose >= VERY_VERBOSE)
1973  yCTrace(FAKEMOTIONCONTROL);
1974 
1975  bool ret = true;
1976  bool val, tot_res = true;
1977 
1978  for(int j=0, index=0; j< _njoints; j++, index++)
1979  {
1980  ret &= checkMotionDoneRaw(&val);
1981  tot_res &= val;
1982  }
1983  *flag = tot_res;
1984  return ret;
1985 }
1986 
1988 {
1989  // Velocity is expressed in iDegrees/s
1990  // save internally the new value of speed; it'll be used in the positionMove
1991  int index = j ;
1992  _ref_speeds[index] = sp;
1993  return true;
1994 }
1995 
1996 bool FakeMotionControl::setRefSpeedsRaw(const double *spds)
1997 {
1998  // Velocity is expressed in iDegrees/s
1999  // save internally the new value of speed; it'll be used in the positionMove
2000  for(int j=0, index=0; j< _njoints; j++, index++)
2001  {
2002  _ref_speeds[index] = spds[index];
2003  }
2004  return true;
2005 }
2006 
2008 {
2009  // Acceleration is expressed in iDegrees/s^2
2010  // save internally the new value of the acceleration; it'll be used in the velocityMove command
2011 
2012  if (acc > 1e6)
2013  {
2014  _ref_accs[j ] = 1e6;
2015  }
2016  else if (acc < -1e6)
2017  {
2018  _ref_accs[j ] = -1e6;
2019  }
2020  else
2021  {
2022  _ref_accs[j ] = acc;
2023  }
2024 
2025  return true;
2026 }
2027 
2029 {
2030  // Acceleration is expressed in iDegrees/s^2
2031  // save internally the new value of the acceleration; it'll be used in the velocityMove command
2032  for(int j=0, index=0; j< _njoints; j++, index++)
2033  {
2034  if (accs[j] > 1e6)
2035  {
2036  _ref_accs[index] = 1e6;
2037  }
2038  else if (accs[j] < -1e6)
2039  {
2040  _ref_accs[index] = -1e6;
2041  }
2042  else
2043  {
2044  _ref_accs[index] = accs[j];
2045  }
2046  }
2047  return true;
2048 }
2049 
2050 bool FakeMotionControl::getRefSpeedRaw(int j, double *spd)
2051 {
2052  *spd = _ref_speeds[j];
2053  return true;
2054 }
2055 
2057 {
2058  memcpy(spds, _ref_speeds, sizeof(double) * _njoints);
2059  return true;
2060 }
2061 
2063 {
2064  *acc = _ref_accs[j];
2065  return true;
2066 }
2067 
2069 {
2070  memcpy(accs, _ref_accs, sizeof(double) * _njoints);
2071  return true;
2072 }
2073 
2075 {
2076  return false;
2077 }
2078 
2080 {
2081  bool ret = true;
2082  for(int j=0; j< _njoints; j++)
2083  {
2084  ret &= stopRaw(j);
2085  }
2086  return ret;
2087 }
2089 
2091 // Position control2 interface //
2093 
2094 bool FakeMotionControl::positionMoveRaw(const int n_joint, const int *joints, const double *refs)
2095 {
2096  if(verbose >= VERY_VERBOSE)
2097  yCTrace(FAKEMOTIONCONTROL) << " -> n_joint " << n_joint;
2098 
2099  for(int j=0; j<n_joint; j++)
2100  {
2101  yCDebug(FAKEMOTIONCONTROL, "j: %d; ref %f;\n", joints[j], refs[j]); fflush(stdout);
2102  }
2103 
2104  bool ret = true;
2105  for(int j=0; j<n_joint; j++)
2106  {
2107  ret = ret &&positionMoveRaw(joints[j], refs[j]);
2108  }
2109  return ret;
2110 }
2111 
2112 bool FakeMotionControl::relativeMoveRaw(const int n_joint, const int *joints, const double *deltas)
2113 {
2114  if(verbose >= VERY_VERBOSE)
2115  yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2116 
2117  bool ret = true;
2118  for(int j=0; j<n_joint; j++)
2119  {
2120  ret = ret &&relativeMoveRaw(joints[j], deltas[j]);
2121  }
2122  return ret;
2123 }
2124 
2125 bool FakeMotionControl::checkMotionDoneRaw(const int n_joint, const int *joints, bool *flag)
2126 {
2127  if(verbose >= VERY_VERBOSE)
2128  yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2129 
2130  bool ret = true;
2131  bool val = true;
2132  bool tot_val = true;
2133 
2134  for(int j=0; j<n_joint; j++)
2135  {
2136  ret = ret && checkMotionDoneRaw(joints[j], &val);
2137  tot_val &= val;
2138  }
2139  *flag = tot_val;
2140  return ret;
2141 }
2142 
2143 bool FakeMotionControl::setRefSpeedsRaw(const int n_joint, const int *joints, const double *spds)
2144 {
2145  if(verbose >= VERY_VERBOSE)
2146  yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2147 
2148  bool ret = true;
2149  for(int j=0; j<n_joint; j++)
2150  {
2151  ret = ret &&setRefSpeedRaw(joints[j], spds[j]);
2152  }
2153  return ret;
2154 }
2155 
2156 bool FakeMotionControl::setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs)
2157 {
2158  if(verbose >= VERY_VERBOSE)
2159  yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2160 
2161  bool ret = true;
2162  for(int j=0; j<n_joint; j++)
2163  {
2164  ret = ret &&setRefAccelerationRaw(joints[j], accs[j]);
2165  }
2166  return ret;
2167 }
2168 
2169 bool FakeMotionControl::getRefSpeedsRaw(const int n_joint, const int *joints, double *spds)
2170 {
2171  if(verbose >= VERY_VERBOSE)
2172  yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2173 
2174  bool ret = true;
2175  for(int j=0; j<n_joint; j++)
2176  {
2177  ret = ret && getRefSpeedRaw(joints[j], &spds[j]);
2178  }
2179  return ret;
2180 }
2181 
2182 bool FakeMotionControl::getRefAccelerationsRaw(const int n_joint, const int *joints, double *accs)
2183 {
2184  if(verbose >= VERY_VERBOSE)
2185  yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2186 
2187  bool ret = true;
2188  for(int j=0; j<n_joint; j++)
2189  {
2190  ret = ret && getRefAccelerationRaw(joints[j], &accs[j]);
2191  }
2192  return ret;
2193 }
2194 
2195 bool FakeMotionControl::stopRaw(const int n_joint, const int *joints)
2196 {
2197  if(verbose >= VERY_VERBOSE)
2198  yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2199 
2200  bool ret = true;
2201  for(int j=0; j<n_joint; j++)
2202  {
2203  ret = ret &&stopRaw(joints[j]);
2204  }
2205  return ret;
2206 }
2207 
2209 
2210 // ControlMode
2211 
2212 // puo' essere richiesto con get
2214 {
2215  if(verbose > VERY_VERY_VERBOSE)
2216  yCTrace(FAKEMOTIONCONTROL) << "j: " << j;
2217 
2218  *v = _controlModes[j];
2219  return true;
2220 }
2221 
2222 // IControl Mode 2
2224 {
2225  bool ret = true;
2226  for(int j=0; j< _njoints; j++)
2227  {
2228  ret = ret && getControlModeRaw(j, &v[j]);
2229  }
2230  return ret;
2231 }
2232 
2233 bool FakeMotionControl::getControlModesRaw(const int n_joint, const int *joints, int *modes)
2234 {
2235  bool ret = true;
2236  for(int j=0; j< n_joint; j++)
2237  {
2238  ret = ret && getControlModeRaw(joints[j], &modes[j]);
2239  }
2240  return ret;
2241 }
2242 
2243 
2244 
2245 // marco.accame: con alberto cardellino abbiamo parlato della correttezza di effettuare la verifica di quanto imposto (in setControlModeRaw() ed affini)
2246 // andando a rileggere il valore nella scheda eth fino a che esso non sia quello atteso. si deve fare oppure no?
2247 // con il control mode il can ora lo fa ma e' giusto? era cosi' anche in passato?
2248 bool FakeMotionControl::setControlModeRaw(const int j, const int _mode)
2249 {
2250  if(verbose >= VERY_VERBOSE)
2251  yCTrace(FAKEMOTIONCONTROL) << "j: " << j << " mode: " << yarp::os::Vocab::decode(_mode);
2252 
2253  if (_mode==VOCAB_CM_FORCE_IDLE)
2254  {
2255  _controlModes[j] = VOCAB_CM_IDLE;
2256  }
2257  else
2258  {
2259  _controlModes[j] = _mode;
2260  }
2261  _posCtrl_references[j] = pos[j];
2262  return true;
2263 }
2264 
2265 
2266 bool FakeMotionControl::setControlModesRaw(const int n_joint, const int *joints, int *modes)
2267 {
2268  if(verbose >= VERY_VERBOSE)
2269  yCTrace(FAKEMOTIONCONTROL) << "n_joints: " << n_joint;
2270 
2271  bool ret = true;
2272  for(int i=0; i<n_joint; i++)
2273  {
2274  ret &= setControlModeRaw(joints[i], modes[i]);
2275  }
2276  return ret;
2277 }
2278 
2280 {
2281  if(verbose >= VERY_VERBOSE)
2282  yCTrace(FAKEMOTIONCONTROL);
2283 
2284  bool ret = true;
2285  for(int i=0; i<_njoints; i++)
2286  {
2287  ret &= setControlModeRaw(i, modes[i]);
2288  }
2289  return ret;
2290 }
2291 
2292 
2294 
2295 bool FakeMotionControl::setEncoderRaw(int j, double val)
2296 {
2297  return NOT_YET_IMPLEMENTED("setEncoder");
2298 }
2299 
2300 bool FakeMotionControl::setEncodersRaw(const double *vals)
2301 {
2302  return NOT_YET_IMPLEMENTED("setEncoders");
2303 }
2304 
2306 {
2307  return NOT_YET_IMPLEMENTED("resetEncoder");
2308 }
2309 
2311 {
2312  return NOT_YET_IMPLEMENTED("resetEncoders");
2313 }
2314 
2315 bool FakeMotionControl::getEncoderRaw(int j, double *value)
2316 {
2317  bool ret = true;
2318 
2319  // To simulate a real controlboard, we assume that the joint
2320  // encoders is exactly the last set by setPosition(s) or positionMove
2321  *value = pos[j];
2322 
2323  return ret;
2324 }
2325 
2327 {
2328  bool ret = true;
2329  for(int j=0; j< _njoints; j++)
2330  {
2331  bool ok = getEncoderRaw(j, &encs[j]);
2332  ret = ret && ok;
2333 
2334  }
2335  return ret;
2336 }
2337 
2339 {
2340  // To avoid returning uninitialized memory, we set the encoder speed to 0
2341  *sp = 0.0;
2342  return true;
2343 }
2344 
2346 {
2347  bool ret = true;
2348  for(int j=0; j< _njoints; j++)
2349  {
2350  ret &= getEncoderSpeedRaw(j, &spds[j]);
2351  }
2352  return ret;
2353 }
2354 
2356 {
2357  // To avoid returning uninitialized memory, we set the encoder acc to 0
2358  *acc = 0.0;
2359 
2360  return true;
2361 }
2362 
2364 {
2365  bool ret = true;
2366  for(int j=0; j< _njoints; j++)
2367  {
2368  ret &= getEncoderAccelerationRaw(j, &accs[j]);
2369  }
2370  return ret;
2371 }
2372 
2374 
2375 bool FakeMotionControl::getEncodersTimedRaw(double *encs, double *stamps)
2376 {
2377  bool ret = getEncodersRaw(encs);
2378  _mutex.lock();
2379  for(int i=0; i<_njoints; i++)
2380  stamps[i] = _encodersStamp[i];
2381  _mutex.unlock();
2382  return ret;
2383 }
2384 
2385 bool FakeMotionControl::getEncoderTimedRaw(int j, double *encs, double *stamp)
2386 {
2387  bool ret = getEncoderRaw(j, encs);
2388  _mutex.lock();
2389  *stamp = _encodersStamp[j];
2390  _mutex.unlock();
2391 
2392  return ret;
2393 }
2394 
2396 
2398 {
2399  *num=_njoints;
2400  return true;
2401 }
2402 
2403 bool FakeMotionControl::setMotorEncoderRaw(int m, const double val)
2404 {
2405  return NOT_YET_IMPLEMENTED("setMotorEncoder");
2406 }
2407 
2409 {
2410  return NOT_YET_IMPLEMENTED("setMotorEncoders");
2411 }
2412 
2414 {
2415  return NOT_YET_IMPLEMENTED("setMotorEncoderCountsPerRevolutionRaw");
2416 }
2417 
2419 {
2420  return NOT_YET_IMPLEMENTED("getMotorEncoderCountsPerRevolutionRaw");
2421 }
2422 
2424 {
2425  return NOT_YET_IMPLEMENTED("resetMotorEncoder");
2426 }
2427 
2429 {
2430  return NOT_YET_IMPLEMENTED("reseMotortEncoders");
2431 }
2432 
2433 bool FakeMotionControl::getMotorEncoderRaw(int m, double *value)
2434 {
2435  *value = 0.0;
2436  return true;
2437 }
2438 
2440 {
2441  bool ret = true;
2442  for(int j=0; j< _njoints; j++)
2443  {
2444  ret &= getMotorEncoderRaw(j, &encs[j]);
2445 
2446  }
2447  return ret;
2448 }
2449 
2451 {
2452  *sp = 0.0;
2453  return true;
2454 }
2455 
2457 {
2458  bool ret = true;
2459  for(int j=0; j< _njoints; j++)
2460  {
2461  ret &= getMotorEncoderSpeedRaw(j, &spds[j]);
2462  }
2463  return ret;
2464 }
2465 
2467 {
2468  *acc = 0.0;
2469  return true;
2470 }
2471 
2473 {
2474  bool ret = true;
2475  for(int j=0; j< _njoints; j++)
2476  {
2477  ret &= getMotorEncoderAccelerationRaw(j, &accs[j]);
2478  }
2479  return ret;
2480 }
2481 
2482 bool FakeMotionControl::getMotorEncodersTimedRaw(double *encs, double *stamps)
2483 {
2484  bool ret = getMotorEncodersRaw(encs);
2485  _mutex.lock();
2486  for(int i=0; i<_njoints; i++)
2487  stamps[i] = _encodersStamp[i];
2488  _mutex.unlock();
2489 
2490  return ret;
2491 }
2492 
2493 bool FakeMotionControl::getMotorEncoderTimedRaw(int m, double *encs, double *stamp)
2494 {
2495  bool ret = getMotorEncoderRaw(m, encs);
2496  _mutex.lock();
2497  *stamp = _encodersStamp[m];
2498  _mutex.unlock();
2499 
2500  return ret;
2501 }
2503 
2505 
2507 {
2508  return DEPRECATED("enableAmpRaw");
2509 }
2510 
2512 {
2513  return DEPRECATED("disableAmpRaw");
2514 }
2515 
2516 bool FakeMotionControl::getCurrentRaw(int j, double *value)
2517 {
2518  //just for testing purposes, this is not a real implementation
2519  *value = current[j];
2520  return true;
2521 }
2522 
2524 {
2525  //just for testing purposes, this is not a real implementation
2526  bool ret = true;
2527  for(int j=0; j< _njoints; j++)
2528  {
2529  ret &= getCurrentRaw(j, &vals[j]);
2530  }
2531  return ret;
2532 }
2533 
2535 {
2536  maxCurrent[m] = val;
2537  return true;
2538 }
2539 
2540 bool FakeMotionControl::getMaxCurrentRaw(int m, double *val)
2541 {
2542  *val = maxCurrent[m];
2543  return true;
2544 }
2545 
2547 {
2548  (_enabledAmp[j ]) ? *st = 1 : *st = 0;
2549  return true;
2550 }
2551 
2553 {
2554  bool ret = true;
2555  for(int j=0; j<_njoints; j++)
2556  {
2557  sts[j] = _enabledAmp[j];
2558  }
2559  return ret;
2560 }
2561 
2563 {
2564  *val = peakCurrent[m];
2565  return true;
2566 }
2567 
2568 bool FakeMotionControl::setPeakCurrentRaw(int m, const double val)
2569 {
2570  peakCurrent[m] = val;
2571  return true;
2572 }
2573 
2575 {
2576  *val = nominalCurrent[m];
2577  return true;
2578 }
2579 
2580 bool FakeMotionControl::setNominalCurrentRaw(int m, const double val)
2581 {
2582  nominalCurrent[m] = val;
2583  return true;
2584 }
2585 
2586 bool FakeMotionControl::getPWMRaw(int m, double *val)
2587 {
2588  *val = pwm[m];
2589  return true;
2590 }
2591 
2592 bool FakeMotionControl::getPWMLimitRaw(int m, double* val)
2593 {
2594  *val = pwmLimit[m];
2595  return true;
2596 }
2597 
2598 bool FakeMotionControl::setPWMLimitRaw(int m, const double val)
2599 {
2600  pwmLimit[m] = val;
2601  return true;
2602 }
2603 
2605 {
2606  *val = supplyVoltage[m];
2607  return true;
2608 }
2609 
2610 
2611 // Limit interface
2612 bool FakeMotionControl::setLimitsRaw(int j, double min, double max)
2613 {
2614  bool ret = true;
2615  return ret;
2616 }
2617 
2618 bool FakeMotionControl::getLimitsRaw(int j, double *min, double *max)
2619 {
2620  return false;
2621 }
2622 
2623 bool FakeMotionControl::getGearboxRatioRaw(int j, double *gearbox)
2624 {
2625  return true;
2626 }
2627 
2629 {
2630  return true;
2631 }
2632 
2634 {
2635  return true;
2636 }
2637 
2639 {
2640  return true;
2641 }
2642 
2644 {
2645  return true;
2646 }
2647 
2649 {
2650  return true;
2651 }
2652 
2653 bool FakeMotionControl::getKinematicMJRaw(int j, double &rotres)
2654 {
2655  yCError(FAKEMOTIONCONTROL, "getKinematicMJRaw not yet implemented");
2656  return false;
2657 }
2658 
2660 {
2661  return true;
2662 }
2663 
2665 {
2666  return true;
2667 }
2668 
2670 {
2671  return true;
2672 }
2673 
2675 {
2676  return true;
2677 }
2678 
2680 {
2681  return true;
2682 }
2683 
2684 bool FakeMotionControl::getRotorIndexOffsetRaw(int j, double& rotorOffset)
2685 {
2686  return true;
2687 }
2688 
2689 bool FakeMotionControl::getAxisNameRaw(int axis, std::string& name)
2690 {
2691  if (axis >= 0 && axis < _njoints)
2692  {
2693  name = _axisName[axis];
2694  return true;
2695  }
2696  else
2697  {
2698  name = "ERROR";
2699  return false;
2700  }
2701 }
2702 
2704 {
2705  if (axis >= 0 && axis < _njoints)
2706  {
2707  type = _jointType[axis];
2708  return true;
2709  }
2710  else
2711  {
2712  return false;
2713  }
2714 }
2715 
2716 // IControlLimits2
2717 bool FakeMotionControl::setVelLimitsRaw(int axis, double min, double max)
2718 {
2719  return NOT_YET_IMPLEMENTED("setVelLimitsRaw");
2720 }
2721 
2722 bool FakeMotionControl::getVelLimitsRaw(int axis, double *min, double *max)
2723 {
2724  *min = 0.0;
2725  *max = _maxJntCmdVelocity[axis];
2726  return true;
2727 }
2728 
2729 
2730 // Torque control
2732 {
2733  *t = _torques[j];
2734  return true;
2735 }
2736 
2738 {
2739  for (int i = 0; i < _njoints; i++)
2740  {
2741  t[i]= _torques[i];
2742  }
2743  return true;
2744 }
2745 
2746 bool FakeMotionControl::getTorqueRangeRaw(int j, double *min, double *max)
2747 {
2748  return NOT_YET_IMPLEMENTED("getTorqueRangeRaw");
2749 }
2750 
2751 bool FakeMotionControl::getTorqueRangesRaw(double *min, double *max)
2752 {
2753  return NOT_YET_IMPLEMENTED("getTorqueRangesRaw");
2754 }
2755 
2757 {
2758  bool ret = true;
2759  for(int j=0; j<_njoints && ret; j++)
2760  ret &= setRefTorqueRaw(j, t[j]);
2761  return ret;
2762 }
2763 
2765 {
2766  return false;
2767 }
2768 
2769 bool FakeMotionControl::setRefTorquesRaw(const int n_joint, const int *joints, const double *t)
2770 {
2771  return false;
2772 }
2773 
2775 {
2776  return false;
2777 }
2778 
2780 {
2781  return false;
2782 }
2783 
2784 bool FakeMotionControl::getImpedanceRaw(int j, double *stiffness, double *damping)
2785 {
2786  return false;
2787 }
2788 
2789 bool FakeMotionControl::setImpedanceRaw(int j, double stiffness, double damping)
2790 {
2791  return false;
2792 }
2793 
2795 {
2796  return false;
2797 }
2798 
2799 bool FakeMotionControl::getImpedanceOffsetRaw(int j, double *offset)
2800 {
2801  return false;
2802 }
2803 
2804 bool FakeMotionControl::getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
2805 {
2806  return false;
2807 }
2808 
2810 {
2811  params->bemf = _kbemf[j];
2812  params->bemf_scale = _kbemf_scale[j];
2813  params->ktau = _ktau[j];
2814  params->ktau_scale = _ktau_scale[j];
2815  yCDebug(FAKEMOTIONCONTROL) << "getMotorTorqueParamsRaw" << params->bemf << params->bemf_scale << params->ktau << params->ktau_scale;
2816  return true;
2817 }
2818 
2820 {
2821  _kbemf[j] = params.bemf;
2822  _ktau[j] = params.ktau;
2823  _kbemf_scale[j] = params.bemf_scale;
2824  _ktau_scale[j] = params.ktau_scale;
2825  yCDebug(FAKEMOTIONCONTROL) << "setMotorTorqueParamsRaw" << params.bemf << params.bemf_scale << params.ktau << params.ktau_scale;
2826  return true;
2827 }
2828 
2829 // IVelocityControl2
2830 bool FakeMotionControl::velocityMoveRaw(const int n_joint, const int *joints, const double *spds)
2831 {
2832  bool ret = true;
2833  for(int i=0; i<n_joint; i++)
2834  {
2835  ret &= velocityMoveRaw(joints[i], spds[i]);
2836  }
2837  return ret;
2838 }
2839 
2840 // PositionDirect Interface
2841 bool FakeMotionControl::setPositionRaw(int j, double ref)
2842 {
2843  _posDir_references[j] = ref;
2844  return true;
2845 }
2846 
2847 bool FakeMotionControl::setPositionsRaw(const int n_joint, const int *joints, const double *refs)
2848 {
2849  for(int i=0; i< n_joint; i++)
2850  {
2851  _posDir_references[joints[i]] = refs[i];
2852  }
2853  return true;
2854 }
2855 
2856 bool FakeMotionControl::setPositionsRaw(const double *refs)
2857 {
2858  for(int i=0; i< _njoints; i++)
2859  {
2860  _posDir_references[i] = refs[i];
2861  }
2862  return true;
2863 }
2864 
2865 
2866 bool FakeMotionControl::getTargetPositionRaw(int axis, double *ref)
2867 {
2868  if(verbose >= VERY_VERBOSE)
2869  yCTrace(FAKEMOTIONCONTROL) << "j " << axis << " ref " << _posCtrl_references[axis];
2870 
2871  int mode = 0;
2872  getControlModeRaw(axis, &mode);
2873  if( (mode != VOCAB_CM_POSITION) &&
2874  (mode != VOCAB_CM_MIXED) &&
2875  (mode != VOCAB_CM_IMPEDANCE_POS))
2876  {
2877  yCWarning(FAKEMOTIONCONTROL) << "getTargetPosition: Joint " << axis << " is not in POSITION mode, therefore the value returned by " <<
2878  "this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
2879  }
2880  *ref = _posCtrl_references[axis];
2881  return true;
2882 }
2883 
2885 {
2886  bool ret = true;
2887  for(int i=0; i<_njoints; i++)
2888  ret &= getTargetPositionRaw(i, &refs[i]);
2889  return ret;
2890 }
2891 
2892 bool FakeMotionControl::getTargetPositionsRaw(int nj, const int * jnts, double *refs)
2893 {
2894  bool ret = true;
2895  for (int i = 0; i<nj; i++)
2896  {
2897  ret &= getTargetPositionRaw(jnts[i], &refs[i]);
2898  }
2899  return ret;
2900 }
2901 
2902 bool FakeMotionControl::getRefVelocityRaw(int axis, double *ref)
2903 {
2904  *ref = _command_speeds[axis];
2905  return true;
2906 }
2907 
2909 {
2910  bool ret = true;
2911  for (int i = 0; i<_njoints; i++)
2912  {
2913  ret &= getRefVelocityRaw(i, &refs[i]);
2914  }
2915  return ret;
2916 }
2917 
2918 bool FakeMotionControl::getRefVelocitiesRaw(int nj, const int * jnts, double *refs)
2919 {
2920  bool ret = true;
2921  for (int i = 0; i<nj; i++)
2922  {
2923  ret &= getRefVelocityRaw(jnts[i], &refs[i]);
2924  }
2925  return ret;
2926 }
2927 
2928 bool FakeMotionControl::getRefPositionRaw(int axis, double *ref)
2929 {
2930  int mode = 0;
2931  getControlModeRaw(axis, &mode);
2932  if( (mode != VOCAB_CM_POSITION) &&
2933  (mode != VOCAB_CM_MIXED) &&
2934  (mode != VOCAB_CM_IMPEDANCE_POS) &&
2935  (mode != VOCAB_CM_POSITION_DIRECT) )
2936  {
2937  yCWarning(FAKEMOTIONCONTROL) << "getRefPosition: Joint " << axis << " is not in POSITION_DIRECT mode, therefore the value returned by \
2938  this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
2939  }
2940 
2941  *ref = _posDir_references[axis];
2942 
2943  return true;
2944 }
2945 
2947 {
2948  bool ret = true;
2949  for (int i = 0; i<_njoints; i++)
2950  {
2951  ret &= getRefPositionRaw(i, &refs[i]);
2952  }
2953  return ret;
2954 }
2955 
2956 bool FakeMotionControl::getRefPositionsRaw(int nj, const int * jnts, double *refs)
2957 {
2958  bool ret = true;
2959  for (int i = 0; i<nj; i++)
2960  {
2961  ret &= getRefPositionRaw(jnts[i], &refs[i]);
2962  }
2963  return ret;
2964 }
2965 
2966 
2967 // InteractionMode
2969 {
2970  if(verbose > VERY_VERY_VERBOSE)
2971  yCTrace(FAKEMOTIONCONTROL) << "j: " << j;
2972 
2973  *_mode = (yarp::dev::InteractionModeEnum)_interactMode[j];
2974  return true;}
2975 
2977 {
2978  bool ret = true;
2979  for(int j=0; j< n_joints; j++)
2980  {
2981  ret = ret && getInteractionModeRaw(joints[j], &modes[j]);
2982  }
2983  return ret;
2984 
2985 }
2986 
2988 {
2989  bool ret = true;
2990  for(int j=0; j<_njoints; j++)
2991  ret = ret && getInteractionModeRaw(j, &modes[j]);
2992  return ret;
2993 }
2994 
2995 // marco.accame: con alberto cardellino abbiamo parlato della correttezza di effettuare la verifica di quanto imposto (in setInteractionModeRaw() ed affini)
2996 // andando a rileggere il valore nella scheda eth fino a che esso non sia quello atteso. si deve fare oppure no?
2997 // con il interaction mode il can ora non lo fa. mentre lo fa per il control mode. perche' diverso?
2999 {
3000  if(verbose >= VERY_VERBOSE)
3001  yCTrace(FAKEMOTIONCONTROL) << "j: " << j << " interaction mode: " << yarp::os::Vocab::decode(_mode);
3002 
3003  _interactMode[j] = _mode;
3004 
3005  return true;
3006 }
3007 
3008 
3010 {
3011  bool ret = true;
3012  for(int i=0; i<n_joints; i++)
3013  {
3014  ret &= setInteractionModeRaw(joints[i], modes[i]);
3015  }
3016  return ret;
3017 }
3018 
3020 {
3021  bool ret = true;
3022  for(int i=0; i<_njoints; i++)
3023  {
3024  ret &= setInteractionModeRaw(i, modes[i]);
3025  }
3026  return ret;
3027 
3028 }
3029 
3031 {
3032  *num=_njoints;
3033  return true;
3034 }
3035 
3037 {
3038  return false;
3039 }
3040 
3042 {
3043  bool ret = true;
3044  for(int j=0; j< _njoints; j++)
3045  {
3046  ret &= getTemperatureRaw(j, &vals[j]);
3047  }
3048  return ret;
3049 }
3050 
3052 {
3053  return false;
3054 }
3055 
3056 bool FakeMotionControl::setTemperatureLimitRaw(int m, const double temp)
3057 {
3058  return false;
3059 }
3060 
3061 //PWM interface
3063 {
3064  refpwm[j] = v;
3065  pwm[j] = v;
3066  last_pwm_command[j]=yarp::os::Time::now();
3067  return true;
3068 }
3069 
3071 {
3072  for (int i = 0; i < _njoints; i++)
3073  {
3074  setRefDutyCycleRaw(i,v[i]);
3075  }
3076  return true;
3077 }
3078 
3080 {
3081  *v = refpwm[j];
3082  return true;
3083 }
3084 
3086 {
3087  for (int i = 0; i < _njoints; i++)
3088  {
3089  v[i] = refpwm[i];
3090  }
3091  return true;
3092 }
3093 
3095 {
3096  *v = pwm[j];
3097  return true;
3098 }
3099 
3101 {
3102  for (int i = 0; i < _njoints; i++)
3103  {
3104  v[i] = pwm[i];
3105  }
3106  return true;
3107 }
3108 
3109 // Current interface
3110 /*bool FakeMotionControl::getCurrentRaw(int j, double *t)
3111 {
3112  return NOT_YET_IMPLEMENTED("getCurrentRaw");
3113 }
3114 
3115 bool FakeMotionControl::getCurrentsRaw(double *t)
3116 {
3117  return NOT_YET_IMPLEMENTED("getCurrentsRaw");
3118 }
3119 */
3120 
3121 bool FakeMotionControl::getCurrentRangeRaw(int j, double *min, double *max)
3122 {
3123  //just for testing purposes, this is not a real implementation
3124  *min = _ref_currents[j] / 100;
3125  *max = _ref_currents[j] * 100;
3126  return true;
3127 }
3128 
3129 bool FakeMotionControl::getCurrentRangesRaw(double *min, double *max)
3130 {
3131  //just for testing purposes, this is not a real implementation
3132  for (int i = 0; i < _njoints; i++)
3133  {
3134  min[i] = _ref_currents[i] / 100;
3135  max[i] = _ref_currents[i] * 100;
3136  }
3137  return true;
3138 }
3139 
3141 {
3142  for (int i = 0; i < _njoints; i++)
3143  {
3144  _ref_currents[i] = t[i];
3145  current[i] = t[i] / 2;
3146  }
3147  return true;
3148 }
3149 
3151 {
3152  _ref_currents[j] = t;
3153  current[j] = t / 2;
3154  return true;
3155 }
3156 
3157 bool FakeMotionControl::setRefCurrentsRaw(const int n_joint, const int *joints, const double *t)
3158 {
3159  bool ret = true;
3160  for (int j = 0; j<n_joint; j++)
3161  {
3162  ret = ret &&setRefCurrentRaw(joints[j], t[j]);
3163  }
3164  return ret;
3165 }
3166 
3168 {
3169  for (int i = 0; i < _njoints; i++)
3170  {
3171  t[i] = _ref_currents[i];
3172  }
3173  return true;
3174 }
3175 
3177 {
3178  *t = _ref_currents[j];
3179  return true;
3180 }
3181 
3182 // bool FakeMotionControl::checkRemoteControlModeStatus(int joint, int target_mode)
3183 // {
3184 // return false;
3185 // }
3186 
3188 {
3190 }
3191 
3193 {
3194  return _njoints;
3195 }
3196 
3198 {
3199  for (int i = 0; i < _njoints; i++)
3200  {
3201  measure[i] = _torques[i];
3202  }
3203  return true;
3204 }
3205 
3207 {
3208  _torques[ch] = measure;
3209  return true;
3210 }
3211 
3212 
3213 // eof
LogStream.h
FakeMotionControl::getHasHallSensorRaw
virtual bool getHasHallSensorRaw(int j, int &ret)
Definition: fakeMotionControl.cpp:2664
FakeMotionControl::getTorqueRangesRaw
bool getTorqueRangesRaw(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
Definition: fakeMotionControl.cpp:2751
FakeMotionControl::getPidRaw
bool getPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::Pid *pid) override
Get current pid value for a specific joint.
Definition: fakeMotionControl.cpp:1593
yarp::os::Bottle
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:72
FakeMotionControl::setPositionRaw
bool setPositionRaw(int j, double ref) override
Set new position for a single axis.
Definition: fakeMotionControl.cpp:2841
FakeMotionControl::setPidErrorLimitRaw
bool setPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double limit) override
Set the error limit for the controller on a specific joint.
Definition: fakeMotionControl.cpp:1529
FakeMotionControl::checkMotionDoneRaw
bool checkMotionDoneRaw(bool *flag) override
Check if the current trajectory is terminated.
Definition: fakeMotionControl.cpp:1970
yarp::dev::ImplementMotorEncoders
Definition: ImplementMotorEncoders.h:31
FakeMotionControl::getNominalCurrentRaw
bool getNominalCurrentRaw(int m, double *val) override
Definition: fakeMotionControl.cpp:2574
FakeMotionControl::getMotorEncoderTimedRaw
bool getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
Read the instantaneous position of a motor encoder.
Definition: fakeMotionControl.cpp:2493
yarp::dev::MotorTorqueParameters::bemf
double bemf
Definition: ITorqueControl.h:26
yarp::dev::ImplementCurrentControl
Definition: ImplementCurrentControl.h:32
yarp::sig::VectorOf::resize
void resize(size_t size) override
Resize the vector.
Definition: Vector.h:254
FakeMotionControl::getEncoderAccelerationRaw
bool getEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of an axis.
Definition: fakeMotionControl.cpp:2355
FakeMotionControl::setEncodersRaw
bool setEncodersRaw(const double *vals) override
Set the value of all encoders.
Definition: fakeMotionControl.cpp:2300
FakeMotionControl::getPidErrorLimitsRaw
bool getPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *limits) override
Get the error limit for all controllers.
Definition: fakeMotionControl.cpp:1686
yarp::os::Searchable
A base class for nested structures that can be searched.
Definition: Searchable.h:68
FakeMotionControl::getRefAccelerationRaw
bool getRefAccelerationRaw(int j, double *acc) override
Get reference acceleration for a joint.
Definition: fakeMotionControl.cpp:2062
FakeMotionControl::getPidErrorRaw
bool getPidErrorRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
Definition: fakeMotionControl.cpp:1561
yarp::dev::ImplementControlMode
Definition: ImplementControlMode.h:32
yarp::dev::ImplementPositionControl
Default implementation of the IPositionControl interface.
Definition: ImplementPositionControl.h:38
yarp::dev::Pid::stiction_up_val
double stiction_up_val
up stiction offset added to the pid output
Definition: ControlBoardPid.h:38
yarp::dev::VOCAB_PIDTYPE_CURRENT
@ VOCAB_PIDTYPE_CURRENT
Definition: PidEnums.h:25
yarp::os::Bottle::size
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:254
FakeMotionControl::getRefDutyCycleRaw
bool getRefDutyCycleRaw(int j, double *v) override
Gets the last reference sent using the setRefDutyCycleRaw function.
Definition: fakeMotionControl.cpp:3079
yarp::dev::ImplementAmplifierControl
Definition: ImplementAmplifierControl.h:23
yarp::dev::MotorTorqueParameters::ktau_scale
double ktau_scale
Definition: ITorqueControl.h:29
FakeMotionControl::setMotorEncodersRaw
bool setMotorEncodersRaw(const double *vals) override
Set the value of all motor encoders.
Definition: fakeMotionControl.cpp:2408
FakeMotionControl::setPidReferenceRaw
bool setPidReferenceRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
Definition: fakeMotionControl.cpp:1497
Drivers.h
t
float t
Definition: FfmpegWriter.cpp:74
VELOCITY_WATCHDOG
#define VELOCITY_WATCHDOG
Definition: fakeMotionControl.cpp:32
FakeMotionControl::resetMotorEncodersRaw
bool resetMotorEncodersRaw() override
Reset motor encoders.
Definition: fakeMotionControl.cpp:2428
FakeMotionControl::setRefCurrentRaw
bool setRefCurrentRaw(int j, double t) override
Set the reference value of the current for a single motor.
Definition: fakeMotionControl.cpp:3150
FakeMotionControl::getAxes
bool getAxes(int *ax) override
Get the number of controlled axes.
Definition: fakeMotionControl.cpp:1880
FakeMotionControl::run
void run() override
Loop function.
Definition: fakeMotionControl.cpp:40
FakeMotionControl::getInteractionModeRaw
bool getInteractionModeRaw(int j, yarp::dev::InteractionModeEnum *_mode) override
Get the current interaction mode of the robot, values can be stiff or compliant.
Definition: fakeMotionControl.cpp:2968
yarp::dev::Pid::max_output
double max_output
max output
Definition: ControlBoardPid.h:36
NOT_YET_IMPLEMENTED
static bool NOT_YET_IMPLEMENTED(const char *txt)
Definition: fakeMotionControl.cpp:102
VOCAB_CM_IMPEDANCE_VEL
constexpr yarp::conf::vocab32_t VOCAB_CM_IMPEDANCE_VEL
Definition: IControlMode.h:134
yarp::dev::ImplementInteractionMode
Definition: ImplementInteractionMode.h:40
yarp::os::Searchable::findGroup
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
yarp::dev::VOCAB_PIDTYPE_POSITION
@ VOCAB_PIDTYPE_POSITION
Definition: PidEnums.h:22
yarp::dev::VOCAB_JOINTTYPE_REVOLUTE
@ VOCAB_JOINTTYPE_REVOLUTE
Definition: IAxisInfo.h:30
FakeMotionControl::setPidRaw
bool setPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, const yarp::dev::Pid &pid) override
Set new pid value for a joint axis.
Definition: fakeMotionControl.cpp:1464
yCWarning
#define yCWarning(component,...)
Definition: LogComponent.h:146
FakeMotionControl::getAmpStatusRaw
bool getAmpStatusRaw(int *st) override
Definition: fakeMotionControl.cpp:2552
FakeMotionControl::getHasRotorEncoderRaw
virtual bool getHasRotorEncoderRaw(int j, int &ret)
Definition: fakeMotionControl.cpp:2669
FakeMotionControl::setPWMLimitRaw
bool setPWMLimitRaw(int j, const double val) override
Definition: fakeMotionControl.cpp:2598
FakeMotionControl::velocityMoveRaw
bool velocityMoveRaw(int j, double sp) override
Start motion at a given speed, single joint.
Definition: fakeMotionControl.cpp:1827
FakeMotionControl::FakeMotionControl
FakeMotionControl()
Definition: fakeMotionControl.cpp:358
FakeMotionControl::setEncoderRaw
bool setEncoderRaw(int j, double val) override
Set the value of the encoder for a given joint.
Definition: fakeMotionControl.cpp:2295
yarp::dev::MotorTorqueParameters
Definition: ITorqueControl.h:23
yarp::conf::environment::getEnvironment
std::string getEnvironment(const char *key, bool *found=nullptr)
Read a variable from the environment.
Definition: environment.h:31
yarp::os::Searchable::toString
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
FakeMotionControl::getPidErrorLimitRaw
bool getPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *limit) override
Get the error limit for the controller on a specific joint.
Definition: fakeMotionControl.cpp:1664
FakeMotionControl::~FakeMotionControl
~FakeMotionControl()
Definition: fakeMotionControl.cpp:463
FakeMotionControl::stopRaw
bool stopRaw() override
Stop motion, multiple joints.
Definition: fakeMotionControl.cpp:2079
FakeMotionControl::enablePidRaw
bool enablePidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Enable the pid computation for a joint.
Definition: fakeMotionControl.cpp:1723
FakeMotionControl::resetPidRaw
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 ...
Definition: fakeMotionControl.cpp:1696
FakeMotionControl::getCurrentRangeRaw
bool getCurrentRangeRaw(int j, double *min, double *max) override
Get the full scale of the current measurement for a given motor (e.g.
Definition: fakeMotionControl.cpp:3121
FakeMotionControl::getEncoderAccelerationsRaw
bool getEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all axes.
Definition: fakeMotionControl.cpp:2363
FakeMotionControl::relativeMoveRaw
bool relativeMoveRaw(int j, double delta) override
Set relative position.
Definition: fakeMotionControl.cpp:1924
FakeMotionControl::threadInit
bool threadInit() override
Initialization method.
Definition: fakeMotionControl.cpp:474
yarp::dev::ImplementPidControl
Definition: ImplementPidControl.h:31
YARP_LOG_COMPONENT
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:80
FakeMotionControl::getCurrentRaw
bool getCurrentRaw(int j, double *val) override
Definition: fakeMotionControl.cpp:2516
FakeMotionControl::getPeakCurrentRaw
bool getPeakCurrentRaw(int m, double *val) override
Definition: fakeMotionControl.cpp:2562
FakeMotionControl::getPidsRaw
bool getPidsRaw(const yarp::dev::PidControlTypeEnum &pidtype, yarp::dev::Pid *pids) override
Get current pid value for a specific joint.
Definition: fakeMotionControl.cpp:1616
FakeMotionControl::getControlModesRaw
bool getControlModesRaw(int *v) override
Definition: fakeMotionControl.cpp:2223
FakeMotionControl::setPidErrorLimitsRaw
bool setPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum &pidtype, const double *limits) override
Get the error limit for the controller on all joints.
Definition: fakeMotionControl.cpp:1551
FakeMotionControl::resetEncodersRaw
bool resetEncodersRaw() override
Reset encoders.
Definition: fakeMotionControl.cpp:2310
FakeMotionControl::setRefDutyCycleRaw
bool setRefDutyCycleRaw(int j, double v) override
Sets the reference dutycycle of a single motor.
Definition: fakeMotionControl.cpp:3062
yarp::dev::Pid::scale
double scale
scale for the pid output
Definition: ControlBoardPid.h:35
yarp::dev::Pid::ki
double ki
integrative gain
Definition: ControlBoardPid.h:33
FakeMotionControl::setControlModesRaw
bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
Definition: fakeMotionControl.cpp:2266
ret
bool ret
Definition: ImplementAxisInfo.cpp:72
FakeMotionControl::getMotorEncodersRaw
bool getMotorEncodersRaw(double *encs) override
Read the position of all motor encoders.
Definition: fakeMotionControl.cpp:2439
FakeMotionControl::getVirtualAnalogSensorChannelsRaw
int getVirtualAnalogSensorChannelsRaw() override
Get the number of channels of the virtual sensor.
Definition: fakeMotionControl.cpp:3192
FakeMotionControl::getMotorPolesRaw
virtual bool getMotorPolesRaw(int j, int &poles)
Definition: fakeMotionControl.cpp:2679
FakeMotionControl::calibrateAxisWithParamsRaw
bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
Start calibration, this method is very often platform specific.
Definition: fakeMotionControl.cpp:1863
yarp::math
Definition: FrameTransform.h:18
FakeMotionControl::getTemperaturesRaw
bool getTemperaturesRaw(double *vals) override
Get temperature of all the motors.
Definition: fakeMotionControl.cpp:3041
FakeMotionControl::getRotorEncoderTypeRaw
virtual bool getRotorEncoderTypeRaw(int j, int &type)
Definition: fakeMotionControl.cpp:2648
FakeMotionControl::disableAmpRaw
bool disableAmpRaw(int j) override
Disable the amplifier on a specific joint.
Definition: fakeMotionControl.cpp:2511
yarp::dev::VAS_status::VAS_OK
@ VAS_OK
FakeMotionControl::setImpedanceRaw
bool setImpedanceRaw(int j, double stiffness, double damping) override
Set current impedance parameters (stiffness,damping) for a specific joint.
Definition: fakeMotionControl.cpp:2789
FakeMotionControl::getNumberOfMotorEncodersRaw
bool getNumberOfMotorEncodersRaw(int *num) override
Get the number of available motor encoders.
Definition: fakeMotionControl.cpp:2397
yarp::os::Time::now
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:124
yarp::dev
An interface for the device drivers.
Definition: audioBufferSizeData.cpp:17
yarp::dev::MotorTorqueParameters::ktau
double ktau
Definition: ITorqueControl.h:28
ImpedanceParameters
Definition: fakeMotionControl.h:75
FakeMotionControl::getEncoderRaw
bool getEncoderRaw(int j, double *v) override
Read the value of an encoder.
Definition: fakeMotionControl.cpp:2315
FakeMotionControl::setLimitsRaw
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...
Definition: fakeMotionControl.cpp:2612
FakeMotionControl::initialised
virtual bool initialised()
Definition: fakeMotionControl.cpp:469
VOCAB_CM_POSITION_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION_DIRECT
Definition: IControlMode.h:129
FakeMotionControl::getRotorEncoderResolutionRaw
virtual bool getRotorEncoderResolutionRaw(int m, double &rotres)
Definition: fakeMotionControl.cpp:2633
VOCAB_CM_POSITION
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION
Definition: IControlMode.h:128
FakeMotionControl::setMotorEncoderCountsPerRevolutionRaw
bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
Definition: fakeMotionControl.cpp:2413
FakeMotionControl::getTemperatureLimitRaw
bool getTemperatureLimitRaw(int m, double *temp) override
Retreives the current temperature limit for a specific motor.
Definition: fakeMotionControl.cpp:3051
FakeMotionControl::getNumberOfMotorsRaw
bool getNumberOfMotorsRaw(int *num) override
Retrieves the number of controlled motors from the current physical interface.
Definition: fakeMotionControl.cpp:3030
NetType.h
FakeMotionControl::setInteractionModesRaw
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.
Definition: fakeMotionControl.cpp:3009
FakeMotionControl::getHasTempSensorsRaw
virtual bool getHasTempSensorsRaw(int j, int &ret)
Definition: fakeMotionControl.cpp:2659
VOCAB_CM_FORCE_IDLE
constexpr yarp::conf::vocab32_t VOCAB_CM_FORCE_IDLE
Definition: IControlMode.h:141
yarp::os::Vocab::decode
std::string decode(NetInt32 code)
Convert a vocabulary identifier into a string.
Definition: Vocab.cpp:36
yarp::sig::VectorOf< double >
yarp::os::Bottle::check
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Definition: Bottle.cpp:280
FakeMotionControl::getPidErrorsRaw
bool getPidErrorsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
Definition: fakeMotionControl.cpp:1583
yarp::os::Bottle::findGroup
Bottle & findGroup(const std::string &key) const override
Gets a list corresponding to a given keyword.
Definition: Bottle.cpp:305
yarp::dev::ImplementAxisInfo
Definition: ImplementAxisInfo.h:20
FakeMotionControl::getHasRotorEncoderIndexRaw
virtual bool getHasRotorEncoderIndexRaw(int j, int &ret)
Definition: fakeMotionControl.cpp:2674
FakeMotionControl::getRefTorqueRaw
bool getRefTorqueRaw(int j, double *t) override
Set the reference value of the torque for a given joint.
Definition: fakeMotionControl.cpp:2779
FakeMotionControl::getImpedanceRaw
bool getImpedanceRaw(int j, double *stiffness, double *damping) override
Get current impedance parameters (stiffness,damping,offset) for a specific joint.
Definition: fakeMotionControl.cpp:2784
yarp::dev::VOCAB_JOINTTYPE_PRISMATIC
@ VOCAB_JOINTTYPE_PRISMATIC
Definition: IAxisInfo.h:31
yarp::dev::VOCAB_PIDTYPE_TORQUE
@ VOCAB_PIDTYPE_TORQUE
Definition: PidEnums.h:24
FakeMotionControl::getJointTypeRaw
bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
Definition: fakeMotionControl.cpp:2703
yarp::dev::Pid::offset
double offset
pwm offset added to the pid output
Definition: ControlBoardPid.h:37
FakeMotionControl::getDutyCycleRaw
bool getDutyCycleRaw(int j, double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
Definition: fakeMotionControl.cpp:3094
yarp::dev::Pid::max_int
double max_int
saturation threshold for the integrator
Definition: ControlBoardPid.h:34
FakeMotionControl::getRefAccelerationsRaw
bool getRefAccelerationsRaw(double *accs) override
Get reference acceleration of all joints.
Definition: fakeMotionControl.cpp:2068
FakeMotionControl::setInteractionModeRaw
bool setInteractionModeRaw(int j, yarp::dev::InteractionModeEnum _mode) override
Set the interaction mode of the robot, values can be stiff or compliant.
Definition: fakeMotionControl.cpp:2998
yarp::dev::ImplementVirtualAnalogSensor
Definition: ImplementVirtualAnalogSensor.h:23
FakeMotionControl::setPeakCurrentRaw
bool setPeakCurrentRaw(int m, const double val) override
Definition: fakeMotionControl.cpp:2568
FakeMotionControl::setRefSpeedRaw
bool setRefSpeedRaw(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
Definition: fakeMotionControl.cpp:1987
FakeMotionControl::updateVirtualAnalogSensorMeasureRaw
bool updateVirtualAnalogSensorMeasureRaw(yarp::sig::Vector &measure) override
Set a vector of torque values for virtual sensor.
Definition: fakeMotionControl.cpp:3197
yarp::dev::ImplementEncodersTimed
Definition: ImplementEncodersTimed.h:31
FakeMotionControl::resetMotorEncoderRaw
bool resetMotorEncoderRaw(int m) override
Reset motor encoder, single motor.
Definition: fakeMotionControl.cpp:2423
yarp::os::Bottle::get
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:249
FakeMotionControl::getTemperatureRaw
bool getTemperatureRaw(int m, double *val) override
Get temperature of a motor.
Definition: fakeMotionControl.cpp:3036
FakeMotionControl::getPidOutputRaw
bool getPidOutputRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
Definition: fakeMotionControl.cpp:1790
yarp::dev::PidControlTypeEnum
PidControlTypeEnum
Definition: PidEnums.h:20
Math.h
PWM_CONSTANT
#define PWM_CONSTANT
Definition: fakeMotionControl.cpp:34
yarp::os::Value::isFloat64
virtual bool isFloat64() const
Checks if value is a 64-bit floating point number.
Definition: Value.cpp:153
FakeMotionControl::setControlModeRaw
bool setControlModeRaw(const int j, const int mode) override
Definition: fakeMotionControl.cpp:2248
FakeMotionControl::getEncoderTimedRaw
bool getEncoderTimedRaw(int j, double *encs, double *stamp) override
Read the instantaneous acceleration of all axes.
Definition: fakeMotionControl.cpp:2385
yarp::dev::Pid::kd
double kd
derivative gain
Definition: ControlBoardPid.h:32
FakeMotionControl::fromConfig
bool fromConfig(yarp::os::Searchable &config)
Definition: fakeMotionControl.cpp:854
FakeMotionControl::close
bool close() override
Close the DeviceDriver.
Definition: fakeMotionControl.cpp:1430
VOCAB_CM_PWM
constexpr yarp::conf::vocab32_t VOCAB_CM_PWM
Definition: IControlMode.h:132
yarp::os::Value::asString
virtual std::string asString() const
Get string value.
Definition: Value.cpp:237
yarp::os::PeriodicThread::start
bool start()
Call this to start the thread.
Definition: PeriodicThread.cpp:311
yarp::dev::ImplementImpedanceControl
Definition: ImplementImpedanceControl.h:21
FakeMotionControl::setRefAccelerationRaw
bool setRefAccelerationRaw(int j, double acc) override
Set reference acceleration for a joint.
Definition: fakeMotionControl.cpp:2007
FakeMotionControl::getMotorEncoderAccelerationRaw
bool getMotorEncoderAccelerationRaw(int m, double *spds) override
Read the instantaneous acceleration of a motor encoder.
Definition: fakeMotionControl.cpp:2466
yarp::dev::ImplementPWMControl
Definition: ImplementPWMControl.h:34
yarp::dev::Pid::kp
double kp
proportional gain
Definition: ControlBoardPid.h:31
yarp::dev::InteractionModeEnum
InteractionModeEnum
Definition: IInteractionMode.h:20
FakeMotionControl::getTargetPositionsRaw
bool getTargetPositionsRaw(double *refs) override
Get the last position reference for all axes.
Definition: fakeMotionControl.cpp:2884
FakeMotionControl::isPidEnabledRaw
bool isPidEnabledRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, bool *enabled) override
Get the current status (enabled/disabled) of the pid controller.
Definition: fakeMotionControl.cpp:1768
yarp::dev::Pid::kff
double kff
feedforward gain
Definition: ControlBoardPid.h:40
OPENLOOP_WATCHDOG
#define OPENLOOP_WATCHDOG
Definition: fakeMotionControl.cpp:33
FakeMotionControl::positionMoveRaw
bool positionMoveRaw(int j, double ref) override
Set new reference point for a single axis.
Definition: fakeMotionControl.cpp:1887
FakeMotionControl::setPidOffsetRaw
bool setPidOffsetRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override
Set an offset value on the ourput of pid controller.
Definition: fakeMotionControl.cpp:1745
VOCAB_CM_IDLE
constexpr yarp::conf::vocab32_t VOCAB_CM_IDLE
Definition: IControlMode.h:126
FakeMotionControl::setPidReferencesRaw
bool setPidReferencesRaw(const yarp::dev::PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
Definition: fakeMotionControl.cpp:1519
FakeMotionControl::getGearboxRatioRaw
bool getGearboxRatioRaw(int m, double *gearbox) override
Get the gearbox ratio for a specific motor.
Definition: fakeMotionControl.cpp:2623
yarp::dev::VOCAB_PIDTYPE_VELOCITY
@ VOCAB_PIDTYPE_VELOCITY
Definition: PidEnums.h:23
FakeMotionControl::setCalibrationParametersRaw
bool setCalibrationParametersRaw(int axis, const yarp::dev::CalibrationParameters &params) override
Start calibration, this method is very often platform specific.
Definition: fakeMotionControl.cpp:1857
FakeMotionControl::setPidsRaw
bool setPidsRaw(const yarp::dev::PidControlTypeEnum &pidtype, const yarp::dev::Pid *pids) override
Set new pid value on multiple axes.
Definition: fakeMotionControl.cpp:1487
FakeMotionControl::getPidReferencesRaw
bool getPidReferencesRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all pid controllers.
Definition: fakeMotionControl.cpp:1651
FakeMotionControl::getRefCurrentsRaw
bool getRefCurrentsRaw(double *t) override
Get the reference value of the currents for all motors.
Definition: fakeMotionControl.cpp:3167
FakeMotionControl::getAxisNameRaw
bool getAxisNameRaw(int axis, std::string &name) override
Definition: fakeMotionControl.cpp:2689
FakeMotionControl::getRefDutyCyclesRaw
bool getRefDutyCyclesRaw(double *v) override
Gets the last reference sent using the setRefDutyCyclesRaw function.
Definition: fakeMotionControl.cpp:3085
FakeMotionControl::getEncodersTimedRaw
bool getEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous acceleration of all axes.
Definition: fakeMotionControl.cpp:2375
FakeMotionControl::setMotorTorqueParamsRaw
bool setMotorTorqueParamsRaw(int j, const yarp::dev::MotorTorqueParameters params) override
Set the motor parameters.
Definition: fakeMotionControl.cpp:2819
FakeMotionControl::getImpedanceOffsetRaw
bool getImpedanceOffsetRaw(int j, double *offset) override
Get current force Offset for a specific joint.
Definition: fakeMotionControl.cpp:2799
yarp::dev::ImplementMotor
Definition: ImplementMotor.h:32
FakeMotionControl::getJointEncoderResolutionRaw
virtual bool getJointEncoderResolutionRaw(int m, double &jntres)
Definition: fakeMotionControl.cpp:2638
FakeMotionControl::getMotorEncoderSpeedsRaw
bool getMotorEncoderSpeedsRaw(double *spds) override
Read the instantaneous speed of all motor encoders.
Definition: fakeMotionControl.cpp:2456
FakeMotionControl::getRefVelocitiesRaw
bool getRefVelocitiesRaw(double *refs) override
Get the last reference speed set by velocityMove for all joints.
Definition: fakeMotionControl.cpp:2908
yarp::dev::Pid::stiction_down_val
double stiction_down_val
down stiction offset added to the pid output
Definition: ControlBoardPid.h:39
yarp::os::PeriodicThread
Definition: PeriodicThread.h:24
LogComponent.h
FakeMotionControl::alloc
bool alloc(int njoints)
Allocated buffers.
Definition: fakeMotionControl.cpp:191
yarp::os::Bottle::isNull
bool isNull() const override
Checks if the object is invalid.
Definition: Bottle.cpp:373
checkAndDestroy
void checkAndDestroy(T *&p)
Definition: ControlBoardHelper.h:45
FakeMotionControl::getMotorEncoderRaw
bool getMotorEncoderRaw(int m, double *v) override
Read the value of a motor encoder.
Definition: fakeMotionControl.cpp:2433
FakeMotionControl::setRefDutyCyclesRaw
bool setRefDutyCyclesRaw(const double *v) override
Sets the reference dutycycle for all motors.
Definition: fakeMotionControl.cpp:3070
FakeMotionControl::setRefTorqueRaw
bool setRefTorqueRaw(int j, double t) override
Set the reference value of the torque for a given joint.
Definition: fakeMotionControl.cpp:2764
FakeMotionControl::getMotorEncoderSpeedRaw
bool getMotorEncoderSpeedRaw(int m, double *sp) override
Read the istantaneous speed of a motor encoder.
Definition: fakeMotionControl.cpp:2450
FakeMotionControl::setVelLimitsRaw
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...
Definition: fakeMotionControl.cpp:2717
FakeMotionControl::getRefPositionsRaw
bool getRefPositionsRaw(double *refs) override
Get the last position reference for all axes.
Definition: fakeMotionControl.cpp:2946
FakeMotionControl::getTargetPositionRaw
bool getTargetPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
Definition: fakeMotionControl.cpp:2866
yarp::sig::VectorOf::data
T * data()
Return a pointer to the first element of the vector.
Definition: Vector.h:239
yarp::dev::VAS_status
VAS_status
Definition: IVirtualAnalogSensor.h:30
FakeMotionControl::getMaxCurrentRaw
bool getMaxCurrentRaw(int j, double *val) override
Returns the maximum electric current allowed for a given motor.
Definition: fakeMotionControl.cpp:2540
yarp::dev::MotorTorqueParameters::bemf_scale
double bemf_scale
Definition: ITorqueControl.h:27
FakeMotionControl::getLimitsRaw
bool getLimitsRaw(int axis, double *min, double *max) override
Get the software limits for a particular axis.
Definition: fakeMotionControl.cpp:2618
FakeMotionControl::resetEncoderRaw
bool resetEncoderRaw(int j) override
Reset encoder, single joint.
Definition: fakeMotionControl.cpp:2305
yCError
#define yCError(component,...)
Definition: LogComponent.h:157
FakeMotionControl::getRotorIndexOffsetRaw
virtual bool getRotorIndexOffsetRaw(int j, double &rotorOffset)
Definition: fakeMotionControl.cpp:2684
yarp::os::Value::asInt32
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:207
FakeMotionControl::getTorqueRaw
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).
Definition: fakeMotionControl.cpp:2731
FakeMotionControl::getEncoderSpeedsRaw
bool getEncoderSpeedsRaw(double *spds) override
Read the instantaneous acceleration of an axis.
Definition: fakeMotionControl.cpp:2345
FakeMotionControl::getPWMRaw
bool getPWMRaw(int j, double *val) override
Definition: fakeMotionControl.cpp:2586
FakeMotionControl::getVelLimitsRaw
bool getVelLimitsRaw(int axis, double *min, double *max) override
Get the software speed limits for a particular axis.
Definition: fakeMotionControl.cpp:2722
ImpedanceParameters::stiffness
double stiffness
Definition: fakeMotionControl.h:77
yCInfo
#define yCInfo(component,...)
Definition: LogComponent.h:135
FakeMotionControl::setRefCurrentsRaw
bool setRefCurrentsRaw(const double *t) override
Set the reference value of the currents for all motors.
Definition: fakeMotionControl.cpp:3140
yarp::sig::Vector
VectorOf< double > Vector
Definition: Vector.h:33
yarp::os
Definition: AbstractCarrier.h:17
yCDebug
#define yCDebug(component,...)
Definition: LogComponent.h:112
FakeMotionControl::enableAmpRaw
bool enableAmpRaw(int j) override
Enable the amplifier on a specific joint.
Definition: fakeMotionControl.cpp:2506
yarp::dev::ImplementVelocityControl
Definition: ImplementVelocityControl.h:34
FakeMotionControl::getKinematicMJRaw
virtual bool getKinematicMJRaw(int j, double &rotres)
Definition: fakeMotionControl.cpp:2653
FakeMotionControl::setMaxCurrentRaw
bool setMaxCurrentRaw(int j, double val) override
Definition: fakeMotionControl.cpp:2534
FakeMotionControl::getJointEncoderTypeRaw
virtual bool getJointEncoderTypeRaw(int j, int &type)
Definition: fakeMotionControl.cpp:2643
VOCAB_CM_MIXED
constexpr yarp::conf::vocab32_t VOCAB_CM_MIXED
Definition: IControlMode.h:138
toString
std::string toString(const T &value)
convert an arbitrary type to string.
Definition: fakeMotionControl.cpp:121
FakeMotionControl::getRefVelocityRaw
bool getRefVelocityRaw(const int joint, double *ref) override
Get the last reference speed set by velocityMove for single joint.
Definition: fakeMotionControl.cpp:2902
yarp::dev::ControlBoardHelper
Definition: ControlBoardHelper.h:59
yarp::dev::ImplementControlLimits
Definition: ImplementControlLimits.h:28
environment.h
FakeMotionControl::getEncodersRaw
bool getEncodersRaw(double *encs) override
Read the position of all axes.
Definition: fakeMotionControl.cpp:2326
FakeMotionControl::calibrationDoneRaw
bool calibrationDoneRaw(int j) override
Check if the calibration is terminated, on a particular joint.
Definition: fakeMotionControl.cpp:1869
FakeMotionControl::getPidReferenceRaw
bool getPidReferenceRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *ref) override
Get the current reference of the pid controller for a specific joint.
Definition: fakeMotionControl.cpp:1629
FakeMotionControl::getRefSpeedsRaw
bool getRefSpeedsRaw(double *spds) override
Get reference speed of all joints.
Definition: fakeMotionControl.cpp:2056
FakeMotionControl::setRefSpeedsRaw
bool setRefSpeedsRaw(const double *spds) override
Set reference speed on all joints.
Definition: fakeMotionControl.cpp:1996
FakeMotionControl::setTemperatureLimitRaw
bool setTemperatureLimitRaw(int m, const double temp) override
Set the temperature limit for a specific motor.
Definition: fakeMotionControl.cpp:3056
yarp::dev::ImplementControlCalibration
Definition: ImplementControlCalibration.h:23
FakeMotionControl::setRefTorquesRaw
bool setRefTorquesRaw(const double *t) override
Set the reference value of the torque for all joints.
Definition: fakeMotionControl.cpp:2756
yarp::dev::Pid
Contains the parameters for a PID.
Definition: ControlBoardPid.h:28
FakeMotionControl::getInteractionModesRaw
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.
Definition: fakeMotionControl.cpp:2976
Time.h
DEPRECATED
static bool DEPRECATED(const char *txt)
Definition: fakeMotionControl.cpp:108
FakeMotionControl::getRefTorquesRaw
bool getRefTorquesRaw(double *t) override
Get the reference value of the torque for all joints.
Definition: fakeMotionControl.cpp:2774
yarp::dev::CalibrationParameters
Definition: IControlCalibration.h:31
FakeMotionControl::getDutyCyclesRaw
bool getDutyCyclesRaw(double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
Definition: fakeMotionControl.cpp:3100
FakeMotionControl::getTorqueControlFilterType
virtual bool getTorqueControlFilterType(int j, int &type)
Definition: fakeMotionControl.cpp:2628
FakeMotionControl::getControlModeRaw
bool getControlModeRaw(int j, int *v) override
Definition: fakeMotionControl.cpp:2213
yarp::dev::ImplementTorqueControl
Definition: ImplementTorqueControl.h:32
FakeMotionControl::getMotorEncoderCountsPerRevolutionRaw
bool getMotorEncoderCountsPerRevolutionRaw(int m, double *v) override
Gets number of counts per revolution for motor encoder m.
Definition: fakeMotionControl.cpp:2418
FakeMotionControl::getCurrentRangesRaw
bool getCurrentRangesRaw(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
Definition: fakeMotionControl.cpp:3129
FakeMotionControl::getMotorEncoderAccelerationsRaw
bool getMotorEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all motor encoders.
Definition: fakeMotionControl.cpp:2472
FakeMotionControl::getVirtualAnalogSensorStatusRaw
yarp::dev::VAS_status getVirtualAnalogSensorStatusRaw(int ch) override
Check the status of a given channel.
Definition: fakeMotionControl.cpp:3187
yCTrace
#define yCTrace(component,...)
Definition: LogComponent.h:88
FakeMotionControl::getEncoderSpeedRaw
bool getEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of an axis.
Definition: fakeMotionControl.cpp:2338
FakeMotionControl::getTorqueRangeRaw
bool getTorqueRangeRaw(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
Definition: fakeMotionControl.cpp:2746
FakeMotionControl::open
bool open(yarp::os::Searchable &par) override
Open the DeviceDriver.
Definition: fakeMotionControl.cpp:499
FakeMotionControl::getCurrentImpedanceLimitRaw
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.
Definition: fakeMotionControl.cpp:2804
FakeMotionControl::getTorquesRaw
bool getTorquesRaw(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
Definition: fakeMotionControl.cpp:2737
yarp::os::Value
A single value (typically within a Bottle).
Definition: Value.h:46
FakeMotionControl::getRefCurrentRaw
bool getRefCurrentRaw(int j, double *t) override
Get the reference value of the current for a single motor.
Definition: fakeMotionControl.cpp:3176
FakeMotionControl::getRefPositionRaw
bool getRefPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
Definition: fakeMotionControl.cpp:2928
FakeMotionControl::getPowerSupplyVoltageRaw
bool getPowerSupplyVoltageRaw(int j, double *val) override
Definition: fakeMotionControl.cpp:2604
FakeMotionControl::setPositionsRaw
bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
Set new reference point for all axes.
Definition: fakeMotionControl.cpp:2847
FakeMotionControl::setRefAccelerationsRaw
bool setRefAccelerationsRaw(const double *accs) override
Set reference acceleration on all joints.
Definition: fakeMotionControl.cpp:2028
FakeMotionControl::setImpedanceOffsetRaw
bool setImpedanceOffsetRaw(int j, double offset) override
Set current force Offset for a specific joint.
Definition: fakeMotionControl.cpp:2794
FakeMotionControl::getMotorEncodersTimedRaw
bool getMotorEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous position of all motor encoders.
Definition: fakeMotionControl.cpp:2482
FakeMotionControl::disablePidRaw
bool disablePidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Disable the pid computation for a joint.
Definition: fakeMotionControl.cpp:1701
FakeMotionControl::threadRelease
void threadRelease() override
Release method.
Definition: fakeMotionControl.cpp:495
yarp::os::impl
FakeMotionControl::getPidOutputsRaw
bool getPidOutputsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
Definition: fakeMotionControl.cpp:1813
yarp::os::Value::asFloat64
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:225
yarp::dev::ImplementPositionDirect
Default implementation of the IPositionDirect interface.
Definition: ImplementPositionDirect.h:38
FakeMotionControl::setNominalCurrentRaw
bool setNominalCurrentRaw(int m, const double val) override
Definition: fakeMotionControl.cpp:2580
yarp::dev::VOCAB_JOINTTYPE_UNKNOWN
@ VOCAB_JOINTTYPE_UNKNOWN
Definition: IAxisInfo.h:32
FakeMotionControl::getCurrentsRaw
bool getCurrentsRaw(double *vals) override
Definition: fakeMotionControl.cpp:2523
yarp::dev::JointTypeEnum
JointTypeEnum
Definition: IAxisInfo.h:28
FakeMotionControl::resizeBuffers
void resizeBuffers()
Resize previously allocated buffers.
Definition: fakeMotionControl.cpp:151
FakeMotionControl::getRefSpeedRaw
bool getRefSpeedRaw(int j, double *ref) override
Get reference speed for a joint.
Definition: fakeMotionControl.cpp:2050
FakeMotionControl::getMotorTorqueParamsRaw
bool getMotorTorqueParamsRaw(int j, yarp::dev::MotorTorqueParameters *params) override
Get the motor parameters.
Definition: fakeMotionControl.cpp:2809
Bottle.h
VOCAB_CM_IMPEDANCE_POS
constexpr yarp::conf::vocab32_t VOCAB_CM_IMPEDANCE_POS
Definition: IControlMode.h:133
FakeMotionControl::getPWMLimitRaw
bool getPWMLimitRaw(int j, double *val) override
Definition: fakeMotionControl.cpp:2592
fakeMotionControl.h
VOCAB_CM_VELOCITY
constexpr yarp::conf::vocab32_t VOCAB_CM_VELOCITY
Definition: IControlMode.h:130
FakeMotionControl::setMotorEncoderRaw
bool setMotorEncoderRaw(int m, const double val) override
Set the value of the motor encoder for a given motor.
Definition: fakeMotionControl.cpp:2403
yarp::os::Value::isInt32
virtual bool isInt32() const
Checks if value is a 32-bit integer.
Definition: Value.cpp:135
ImpedanceParameters::damping
double damping
Definition: fakeMotionControl.h:78