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