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