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