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