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