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