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
9
10#include <yarp/os/Bottle.h>
11#include <yarp/os/Time.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
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
134/*
135bool FakeMotionControl::extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
136{
137 size++;
138 Bottle &tmp=input.findGroup(key1, txt);
139
140 if (tmp.isNull())
141 {
142 yCError(FAKEMOTIONCONTROL) << key1.c_str() << "parameter not found";
143 return false;
144 }
145
146 if(tmp.size()!=(size_t) size)
147 {
148 yCError(FAKEMOTIONCONTROL) << key1.c_str() << "incorrect number of entries";
149 return false;
150 }
151
152 out=tmp;
153 return true;
154}
155*/
156
158{
159 pos.resize(_njoints);
160 dpos.resize(_njoints);
161 vel.resize(_njoints);
162 speed.resize(_njoints);
163 acc.resize(_njoints);
164 loc.resize(_njoints);
165 amp.resize(_njoints);
166
167 current.resize(_njoints);
168 nominalCurrent.resize(_njoints);
169 maxCurrent.resize(_njoints);
170 peakCurrent.resize(_njoints);
171 pwm.resize(_njoints);
172 refpwm.resize(_njoints);
173 pwmLimit.resize(_njoints);
174 supplyVoltage.resize(_njoints);
175 last_velocity_command.resize(_njoints);
176 last_pwm_command.resize(_njoints);
177
178 pos.zero();
179 dpos.zero();
180 vel.zero();
181 speed.zero();
182 acc.zero();
183 loc.zero();
184 amp.zero();
185
186 current.zero();
187 nominalCurrent.zero();
188 maxCurrent.zero();
189 peakCurrent.zero();
190
191 pwm.zero();
192 refpwm.zero();
193 pwmLimit.zero();
194 supplyVoltage.zero();
195}
196
198{
199 _axisMap = allocAndCheck<int>(nj);
200 _controlModes = allocAndCheck<int>(nj);
201 _interactMode = allocAndCheck<int>(nj);
202 _angleToEncoder = allocAndCheck<double>(nj);
203 _dutycycleToPWM = allocAndCheck<double>(nj);
204 _ampsToSensor = allocAndCheck<double>(nj);
205 _encodersStamp = allocAndCheck<double>(nj);
206 _DEPRECATED_encoderconversionoffset = allocAndCheck<float>(nj);
207 _DEPRECATED_encoderconversionfactor = allocAndCheck<float>(nj);
208 _jointEncoderRes = allocAndCheck<int>(nj);
209 _rotorEncoderRes = allocAndCheck<int>(nj);
210 _gearbox = allocAndCheck<double>(nj);
211 _torqueSensorId= allocAndCheck<int>(nj);
212 _torqueSensorChan= allocAndCheck<int>(nj);
213 _maxTorque=allocAndCheck<double>(nj);
214 _torques = allocAndCheck<double>(nj);
215 _maxJntCmdVelocity = allocAndCheck<double>(nj);
216 _maxMotorVelocity = allocAndCheck<double>(nj);
217 _newtonsToSensor=allocAndCheck<double>(nj);
218 _hasHallSensor = allocAndCheck<bool>(nj);
219 _hasTempSensor = allocAndCheck<bool>(nj);
220 _hasRotorEncoder = allocAndCheck<bool>(nj);
221 _hasRotorEncoderIndex = allocAndCheck<bool>(nj);
222 _rotorIndexOffset = allocAndCheck<int>(nj);
223 _motorPoles = allocAndCheck<int>(nj);
224 _rotorlimits_max = allocAndCheck<double>(nj);
225 _rotorlimits_min = allocAndCheck<double>(nj);
226 _hwfault_code = allocAndCheck<int>(nj);
227 _hwfault_message = allocAndCheck<std::string>(nj);
228
229
230 _ppids=allocAndCheck<Pid>(nj);
231 _tpids=allocAndCheck<Pid>(nj);
232 _cpids = allocAndCheck<Pid>(nj);
233 _vpids = allocAndCheck<Pid>(nj);
234 _ppids_ena=allocAndCheck<bool>(nj);
235 _tpids_ena=allocAndCheck<bool>(nj);
236 _cpids_ena = allocAndCheck<bool>(nj);
237 _vpids_ena = allocAndCheck<bool>(nj);
238 _ppids_lim=allocAndCheck<double>(nj);
239 _tpids_lim=allocAndCheck<double>(nj);
240 _cpids_lim = allocAndCheck<double>(nj);
241 _vpids_lim = allocAndCheck<double>(nj);
242 _ppids_ref=allocAndCheck<double>(nj);
243 _tpids_ref=allocAndCheck<double>(nj);
244 _cpids_ref = allocAndCheck<double>(nj);
245 _vpids_ref = allocAndCheck<double>(nj);
246
247// _impedance_params=allocAndCheck<ImpedanceParameters>(nj);
248// _impedance_limits=allocAndCheck<ImpedanceLimits>(nj);
249 _axisName = new std::string[nj];
250 _jointType = new JointTypeEnum[nj];
251
252 _limitsMax=allocAndCheck<double>(nj);
253 _limitsMin=allocAndCheck<double>(nj);
254 _kinematic_mj=allocAndCheck<double>(16);
255// _currentLimits=allocAndCheck<MotorCurrentLimits>(nj);
256 _motorPwmLimits=allocAndCheck<double>(nj);
257 checking_motiondone=allocAndCheck<bool>(nj);
258
259 _velocityShifts=allocAndCheck<int>(nj);
260 _velocityTimeout=allocAndCheck<int>(nj);
261 _kbemf=allocAndCheck<double>(nj);
262 _ktau=allocAndCheck<double>(nj);
263 _kbemf_scale = allocAndCheck<int>(nj);
264 _ktau_scale = allocAndCheck<int>(nj);
265 _filterType=allocAndCheck<int>(nj);
266 _last_position_move_time=allocAndCheck<double>(nj);
267 _viscousPos=allocAndCheck<double>(nj);
268 _viscousNeg=allocAndCheck<double>(nj);
269 _coulombPos=allocAndCheck<double>(nj);
270 _coulombNeg=allocAndCheck<double>(nj);
271 _velocityThres=allocAndCheck<double>(nj);
272
273 // Reserve space for data stored locally. values are initialized to 0
274 _posCtrl_references = allocAndCheck<double>(nj);
275 _posDir_references = allocAndCheck<double>(nj);
276 _command_speeds = allocAndCheck<double>(nj);
277 _ref_speeds = allocAndCheck<double>(nj);
278 _ref_accs = allocAndCheck<double>(nj);
279 _ref_torques = allocAndCheck<double>(nj);
280 _ref_currents = allocAndCheck<double>(nj);
281 _enabledAmp = allocAndCheck<bool>(nj);
282 _enabledPid = allocAndCheck<bool>(nj);
283 _calibrated = allocAndCheck<bool>(nj);
284// _cacheImpedance = allocAndCheck<eOmc_impedance_t>(nj);
285
287
288 return true;
289}
290
291bool FakeMotionControl::dealloc()
292{
293 checkAndDestroy(_axisMap);
294 checkAndDestroy(_controlModes);
295 checkAndDestroy(_interactMode);
296 checkAndDestroy(_angleToEncoder);
297 checkAndDestroy(_ampsToSensor);
298 checkAndDestroy(_dutycycleToPWM);
299 checkAndDestroy(_encodersStamp);
300 checkAndDestroy(_DEPRECATED_encoderconversionoffset);
301 checkAndDestroy(_DEPRECATED_encoderconversionfactor);
302 checkAndDestroy(_jointEncoderRes);
303 checkAndDestroy(_rotorEncoderRes);
304// checkAndDestroy(_jointEncoderType);
305// checkAndDestroy(_rotorEncoderType);
306 checkAndDestroy(_gearbox);
307 checkAndDestroy(_torqueSensorId);
308 checkAndDestroy(_torqueSensorChan);
309 checkAndDestroy(_maxTorque);
310 checkAndDestroy(_maxJntCmdVelocity);
311 checkAndDestroy(_maxMotorVelocity);
312 checkAndDestroy(_newtonsToSensor);
313 checkAndDestroy(_ppids);
314 checkAndDestroy(_tpids);
315 checkAndDestroy(_cpids);
316 checkAndDestroy(_vpids);
317 checkAndDestroy(_ppids_ena);
318 checkAndDestroy(_tpids_ena);
319 checkAndDestroy(_cpids_ena);
320 checkAndDestroy(_vpids_ena);
321 checkAndDestroy(_ppids_lim);
322 checkAndDestroy(_tpids_lim);
323 checkAndDestroy(_cpids_lim);
324 checkAndDestroy(_vpids_lim);
325 checkAndDestroy(_ppids_ref);
326 checkAndDestroy(_tpids_ref);
327 checkAndDestroy(_cpids_ref);
328 checkAndDestroy(_vpids_ref);
329// checkAndDestroy(_impedance_params);
330// checkAndDestroy(_impedance_limits);
331 checkAndDestroy(_limitsMax);
332 checkAndDestroy(_limitsMin);
333 checkAndDestroy(_kinematic_mj);
334// checkAndDestroy(_currentLimits);
335 checkAndDestroy(_motorPwmLimits);
336 checkAndDestroy(checking_motiondone);
337 checkAndDestroy(_velocityShifts);
338 checkAndDestroy(_velocityTimeout);
339 checkAndDestroy(_kbemf);
340 checkAndDestroy(_ktau);
341 checkAndDestroy(_kbemf_scale);
342 checkAndDestroy(_ktau_scale);
343 checkAndDestroy(_filterType);
344 checkAndDestroy(_viscousPos);
345 checkAndDestroy(_viscousNeg);
346 checkAndDestroy(_coulombPos);
347 checkAndDestroy(_coulombNeg);
348 checkAndDestroy(_velocityThres);
349 checkAndDestroy(_posCtrl_references);
350 checkAndDestroy(_posDir_references);
351 checkAndDestroy(_command_speeds);
352 checkAndDestroy(_ref_speeds);
353 checkAndDestroy(_ref_accs);
354 checkAndDestroy(_ref_torques);
355 checkAndDestroy(_ref_currents);
356 checkAndDestroy(_enabledAmp);
357 checkAndDestroy(_enabledPid);
358 checkAndDestroy(_calibrated);
359 checkAndDestroy(_hasHallSensor);
360 checkAndDestroy(_hasTempSensor);
361 checkAndDestroy(_hasRotorEncoder);
362 checkAndDestroy(_hasRotorEncoderIndex);
363 checkAndDestroy(_rotorIndexOffset);
364 checkAndDestroy(_motorPoles);
365 checkAndDestroy(_axisName);
366 checkAndDestroy(_jointType);
367 checkAndDestroy(_rotorlimits_max);
368 checkAndDestroy(_rotorlimits_min);
369 checkAndDestroy(_last_position_move_time);
370 checkAndDestroy(_torques);
371 checkAndDestroy(_hwfault_code);
372 checkAndDestroy(_hwfault_message);
373
374 return true;
375}
376
378 PeriodicThread(0.01),
398 _mutex(),
399 _njoints (0),
400 _axisMap (nullptr),
401 _angleToEncoder(nullptr),
402 _encodersStamp (nullptr),
403 _ampsToSensor (nullptr),
404 _dutycycleToPWM(nullptr),
405 _DEPRECATED_encoderconversionfactor (nullptr),
406 _DEPRECATED_encoderconversionoffset (nullptr),
407 _jointEncoderRes (nullptr),
408 _rotorEncoderRes (nullptr),
409 _gearbox (nullptr),
410 _hasHallSensor (nullptr),
411 _hasTempSensor (nullptr),
412 _hasRotorEncoder (nullptr),
413 _hasRotorEncoderIndex (nullptr),
414 _rotorIndexOffset (nullptr),
415 _motorPoles (nullptr),
416 _rotorlimits_max (nullptr),
417 _rotorlimits_min (nullptr),
418 _ppids (nullptr),
419 _tpids (nullptr),
420 _cpids (nullptr),
421 _vpids (nullptr),
422 _ppids_ena (nullptr),
423 _tpids_ena (nullptr),
424 _cpids_ena (nullptr),
425 _vpids_ena (nullptr),
426 _ppids_lim (nullptr),
427 _tpids_lim (nullptr),
428 _cpids_lim (nullptr),
429 _vpids_lim (nullptr),
430 _ppids_ref (nullptr),
431 _tpids_ref (nullptr),
432 _cpids_ref (nullptr),
433 _vpids_ref (nullptr),
434 _axisName (nullptr),
435 _jointType (nullptr),
436 _limitsMin (nullptr),
437 _limitsMax (nullptr),
438 _kinematic_mj (nullptr),
439 _maxJntCmdVelocity (nullptr),
440 _maxMotorVelocity (nullptr),
441 _velocityShifts (nullptr),
442 _velocityTimeout (nullptr),
443 _kbemf (nullptr),
444 _ktau (nullptr),
445 _kbemf_scale (nullptr),
446 _ktau_scale (nullptr),
447 _viscousPos (nullptr),
448 _viscousNeg (nullptr),
449 _coulombPos (nullptr),
450 _coulombNeg (nullptr),
451 _velocityThres (nullptr),
452 _filterType (nullptr),
453 _torqueSensorId (nullptr),
454 _torqueSensorChan (nullptr),
455 _maxTorque (nullptr),
456 _newtonsToSensor (nullptr),
457 checking_motiondone (nullptr),
458 _last_position_move_time(nullptr),
459 _motorPwmLimits (nullptr),
460 _torques (nullptr),
461 useRawEncoderData (false),
462 _pwmIsLimited (false),
463 _torqueControlEnabled (false),
464 _torqueControlUnits (T_MACHINE_UNITS),
465 _positionControlUnits (P_MACHINE_UNITS),
466 prev_time (0.0),
467 opened (false),
468 verbose (VERY_VERBOSE)
469{
471 std::string tmp = yarp::conf::environment::get_string("VERBOSE_STICA");
472 verbosewhenok = (tmp != "") ? (bool)yarp::conf::numeric::from_string<int>(tmp) :
473 false;
474}
475
481
483{
484 return opened;
485}
486
488{
490 for(int i=0; i<_njoints; i++)
491 {
492 pwm[i] = 33+i;
493 pwmLimit[i] = (33+i)*10;
494 current[i] = (33+i)*100;
495 maxCurrent[i] = (33+i)*1000;
496 peakCurrent[i] = (33+i)*2;
497 nominalCurrent[i] = (33+i)*20;
498 supplyVoltage[i] = (33+i)*200;
499 last_velocity_command[i] = -1;
500 last_pwm_command[i] = -1;
501 _controlModes[i] = VOCAB_CM_POSITION;
502 _maxJntCmdVelocity[i]=50.0;
503 }
504 prev_time = yarp::os::Time::now();
505 return true;
506}
507
511
513{
514 if (!this->parseParams(config)) {return false;}
515
516 std::string str;
517
518 //
519 // Read Configuration params from file
520 //
521 _njoints = m_GENERAL_Joints;
522
523 if(!alloc(_njoints))
524 {
525 yCError(FAKEMOTIONCONTROL) << "Malloc failed";
526 return false;
527 }
528
529 // Default value
530 for (int i = 0; i < _njoints; i++) {
531 _newtonsToSensor[i] = 1;
532 }
533
534 if(!fromConfig(config))
535 {
536 yCError(FAKEMOTIONCONTROL) << "Missing parameters in config file";
537 return false;
538 }
539
540 // INIT ALL INTERFACES
541 yarp::sig::Vector tmpZeros; tmpZeros.resize (_njoints, 0.0);
542 yarp::sig::Vector tmpOnes; tmpOnes.resize (_njoints, 1.0);
543 yarp::sig::Vector bemfToRaw; bemfToRaw.resize(_njoints, 1.0);
544 yarp::sig::Vector ktauToRaw; ktauToRaw.resize(_njoints, 1.0);
545 for (size_t i = 0; i < _njoints; i++) { bemfToRaw [i] = _newtonsToSensor[i] / _angleToEncoder[i];}
546 for (size_t i = 0; i < _njoints; i++) { ktauToRaw[i] = _dutycycleToPWM[i] / _newtonsToSensor[i]; }
547 ControlBoardHelper cb(_njoints, _axisMap, _angleToEncoder, nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM);
549 ImplementControlCalibration::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
550 ImplementAmplifierControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
551 ImplementEncodersTimed::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
552 ImplementMotorEncoders::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
553 ImplementPositionControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
554 ImplementPidControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM);
555 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
556 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
557 ImplementControlMode::initialize(_njoints, _axisMap);
558 ImplementVelocityControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
559 ImplementControlLimits::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
560 ImplementImpedanceControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr, _newtonsToSensor);
561 ImplementTorqueControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM, bemfToRaw.data(), ktauToRaw.data());
562 ImplementPositionDirect::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
563 ImplementInteractionMode::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
564 ImplementMotor::initialize(_njoints, _axisMap);
565 ImplementAxisInfo::initialize(_njoints, _axisMap);
566 ImplementPWMControl::initialize(_njoints, _axisMap, _dutycycleToPWM);
567 ImplementCurrentControl::initialize(_njoints, _axisMap, _ampsToSensor);
568 ImplementVirtualAnalogSensor::initialize(_njoints, _axisMap, _newtonsToSensor);
569 ImplementJointFault::initialize(_njoints, _axisMap);
570
571 //start the rateThread
572 bool init = this->start();
573 if(!init)
574 {
575 yCError(FAKEMOTIONCONTROL) << "open() has an error in call of FakeMotionControl::init() for board" ;
576 return false;
577 }
578 else
579 {
580 if(verbosewhenok)
581 {
582 yCDebug(FAKEMOTIONCONTROL) << "init() has successfully initted board ";
583 }
584 }
585 opened = true;
586
587 return true;
588}
589
591{
592 size_t i;
593
594 // AxisMap
595 if (!m_GENERAL_AxisMap.empty())
596 {
597 for (i = 0; i < m_GENERAL_AxisMap.size(); i++) {
598 _axisMap[i] = m_GENERAL_AxisMap[i];
599 }
600 }
601 else
602 {
603 for (i = 0; i < _njoints; i++) {
604 _axisMap[i] = i;
605 }
606 }
607 for (i = 0; i < _njoints; i++) {yDebug() << "_axisMap: " << _axisMap[i] << " "; }
608
609 // AxisName
610 if (!m_GENERAL_AxisName.empty())
611 {
612 for (i = 0; i < m_GENERAL_AxisName.size(); i++) {
613 _axisName[_axisMap[i]] = m_GENERAL_AxisName[i];
614 }
615 }
616 else
617 {
618 for (i = 0; i < _njoints; i++) {
619 _axisName[_axisMap[i]] = "joint" + toString(i);
620 }
621 }
622 for (i = 0; i < _njoints; i++) { yDebug() << "_axisName: " << _axisName[_axisMap[i]] << " "; }
623
624 // Axis Type
625 if (!m_GENERAL_AxisType.empty())
626 {
627 //beware: axis type has to be remapped here because they are not set using the toHw() helper function
628 for (i = 0; i < m_GENERAL_AxisType.size(); i++)
629 {
630 std::string typeString = m_GENERAL_AxisType[i];
631 if (typeString == "revolute") {
632 _jointType[_axisMap[i]] = VOCAB_JOINTTYPE_REVOLUTE;
633 }
634 else if (typeString == "prismatic") {
635 _jointType[_axisMap[i]] = VOCAB_JOINTTYPE_PRISMATIC;
636 }
637 else {
638 yCError(FAKEMOTIONCONTROL, "Unknown AxisType value %s!", typeString.c_str());
639 _jointType[_axisMap[i]] = VOCAB_JOINTTYPE_UNKNOWN;
640 return false;
641 }
642 }
643 }
644 else
645 {
646 yCInfo(FAKEMOTIONCONTROL) << "Using default AxisType (revolute)";
647 for (i = 0; i < _njoints; i++) {
648 _jointType[_axisMap[i]] = VOCAB_JOINTTYPE_REVOLUTE;
649 }
650 }
651
652 // current conversions factor
653 if (!m_GENERAL_ampsToSensor.empty())
654 {
655 for (i = 0; i < m_GENERAL_ampsToSensor.size(); i++) {
656 _ampsToSensor[i] = m_GENERAL_ampsToSensor[i];
657 }
658 }
659 else
660 {
661 yCInfo(FAKEMOTIONCONTROL) << "Using default ampsToSensor=1";
662 for (i = 0; i < _njoints; i++) {
663 _ampsToSensor[i] = 1;
664 }
665 }
666
667
668 // pwm conversions factor
669 if (!m_GENERAL_fullscalePWM.empty())
670 {
671 for (i = 0; i < m_GENERAL_fullscalePWM.size(); i++) {
672 _dutycycleToPWM[i] = m_GENERAL_fullscalePWM[i]/100;
673 }
674 }
675 else
676 {
677 yCInfo(FAKEMOTIONCONTROL) << "Using default fullscalePWM=1";
678 for (i = 0; i < _njoints; i++) {
679 _dutycycleToPWM[i] = 1;
680 }
681 }
682
683// double tmp_A2E;
684 // Encoder scales
685 if(!m_GENERAL_Encoder.empty())
686 {
687 for (i = 0; i < m_GENERAL_Encoder.size(); i++) {
688 _angleToEncoder[i] = m_GENERAL_Encoder[i]; }
689 }
690 else
691 {
692 yCInfo(FAKEMOTIONCONTROL) << "Using default Encoder=1";
693 for (i = 0; i < _njoints; i++) {
694 _angleToEncoder[i] = 1; }
695 }
696
698 if (!m_LIMITS_Max.empty())
699 {
700 for (i = 0; i < m_LIMITS_Max.size(); i++) {
701 _limitsMax[i] = m_LIMITS_Max[i];
702 }
703 }
704 else
705 {
706 yCInfo(FAKEMOTIONCONTROL) << "Using default m_LIMITS_Max=100";
707 for (i = 0; i < _njoints; i++) {
708 _limitsMax[i] = 100;
709 }
710 }
711
712 if (!m_LIMITS_Min.empty())
713 {
714 for (i = 0; i < m_LIMITS_Min.size(); i++) {
715 _limitsMin[i] = m_LIMITS_Min[i];
716 }
717 }
718 else
719 {
720 yCInfo(FAKEMOTIONCONTROL) << "Using default m_LIMITS_Min=0";
721 for (i = 0; i < _njoints; i++) {
722 _limitsMin[i] = 0;
723 }
724 }
725
726 return true;
727}
728
729
758
759void FakeMotionControl::cleanup()
760{
761
762}
763
764
765
767
769{
770 yCDebug(FAKEMOTIONCONTROL) << "setPidRaw" << pidtype << j << pid.kp;
771 switch (pidtype)
772 {
774 _ppids[j]=pid;
775 break;
777 _vpids[j]=pid;
778 break;
780 _cpids[j]=pid;
781 break;
783 _tpids[j]=pid;
784 break;
785 default:
786 break;
787 }
788 return true;
789}
790
792{
793 bool ret = true;
794 for(int j=0; j< _njoints; j++)
795 {
796 ret &= setPidRaw(pidtype, j, pids[j]);
797 }
798 return ret;
799}
800
802{
803 switch (pidtype)
804 {
806 _ppids_ref[j]=ref;
807 break;
809 _vpids_ref[j]=ref;
810 break;
812 _cpids_ref[j]=ref;
813 break;
815 _tpids_ref[j]=ref;
816 break;
817 default:
818 break;
819 }
820 return true;
821}
822
824{
825 bool ret = true;
826 for(int j=0, index=0; j< _njoints; j++, index++)
827 {
828 ret &= setPidReferenceRaw(pidtype, j, refs[index]);
829 }
830 return ret;
831}
832
834{
835 switch (pidtype)
836 {
838 _ppids_lim[j]=limit;
839 break;
841 _vpids_lim[j]=limit;
842 break;
844 _cpids_lim[j]=limit;
845 break;
847 _tpids_lim[j]=limit;
848 break;
849 default:
850 break;
851 }
852 return true;
853}
854
856{
857 bool ret = true;
858 for(int j=0, index=0; j< _njoints; j++, index++)
859 {
860 ret &= setPidErrorLimitRaw(pidtype, j, limits[index]);
861 }
862 return ret;
863}
864
866{
867 switch (pidtype)
868 {
870 *err=0.1;
871 break;
873 *err=0.2;
874 break;
876 *err=0.3;
877 break;
879 *err=0.4;
880 break;
881 default:
882 break;
883 }
884 return true;
885}
886
888{
889 bool ret = true;
890 for(int j=0; j< _njoints; j++)
891 {
892 ret &= getPidErrorRaw(pidtype, j, &errs[j]);
893 }
894 return ret;
895}
896
898{
899 switch (pidtype)
900 {
902 *pid=_ppids[j];
903 break;
905 *pid=_vpids[j];
906 break;
908 *pid=_cpids[j];
909 break;
911 *pid=_tpids[j];
912 break;
913 default:
914 break;
915 }
916 yCDebug(FAKEMOTIONCONTROL) << "getPidRaw" << pidtype << j << pid->kp;
917 return true;
918}
919
921{
922 bool ret = true;
923
924 // just one joint at time, wait answer before getting to the next.
925 // This is because otherwise too many msg will be placed into can queue
926 for(int j=0, index=0; j<_njoints; j++, index++)
927 {
928 ret &=getPidRaw(pidtype, j, &pids[j]);
929 }
930 return ret;
931}
932
934{
935 switch (pidtype)
936 {
938 *ref=_ppids_ref[j];
939 break;
941 *ref=_vpids_ref[j];
942 break;
944 *ref=_cpids_ref[j];
945 break;
947 *ref=_tpids_ref[j];
948 break;
949 default:
950 break;
951 }
952 return true;
953}
954
956{
957 bool ret = true;
958
959 // just one joint at time, wait answer before getting to the next.
960 // This is because otherwise too many msg will be placed into can queue
961 for(int j=0; j< _njoints; j++)
962 {
964 }
965 return ret;
966}
967
969{
970 switch (pidtype)
971 {
973 *limit=_ppids_lim[j];
974 break;
976 *limit=_vpids_lim[j];
977 break;
979 *limit=_cpids_lim[j];
980 break;
982 *limit=_tpids_lim[j];
983 break;
984 default:
985 break;
986 }
987 return true;
988}
989
991{
992 bool ret = true;
993 for(int j=0, index=0; j<_njoints; j++, index++)
994 {
995 ret &=getPidErrorLimitRaw(pidtype, j, &limits[j]);
996 }
997 return ret;
998}
999
1001{
1002 return true;
1003}
1004
1006{
1007 switch (pidtype)
1008 {
1010 _ppids_ena[j]=false;
1011 break;
1013 _vpids_ena[j]=false;
1014 break;
1016 _cpids_ena[j]=false;
1017 break;
1019 _tpids_ena[j]=false;
1020 break;
1021 default:
1022 break;
1023 }
1024 return true;
1025}
1026
1028{
1029 switch (pidtype)
1030 {
1032 _ppids_ena[j]=true;
1033 break;
1035 _vpids_ena[j]=true;
1036 break;
1038 _cpids_ena[j]=true;
1039 break;
1041 _tpids_ena[j]=true;
1042 break;
1043 default:
1044 break;
1045 }
1046 return true;
1047}
1048
1050{
1051 yCDebug(FAKEMOTIONCONTROL) << "setPidOffsetRaw" << pidtype << j << v;
1052 switch (pidtype)
1053 {
1055 _ppids[j].offset=v;
1056 break;
1058 _vpids[j].offset=v;
1059 break;
1061 _cpids[j].offset=v;
1062 break;
1064 _tpids[j].offset=v;
1065 break;
1066 default:
1067 break;
1068 }
1069 return true;
1070}
1071
1073{
1074 switch (pidtype)
1075 {
1077 *enabled=_ppids_ena[j];
1078 break;
1080 *enabled=_vpids_ena[j];
1081 break;
1083 *enabled=_cpids_ena[j];
1084 break;
1086 *enabled=_tpids_ena[j];
1087 break;
1088 default:
1089 break;
1090 }
1091 return true;
1092}
1093
1095{
1096 switch (pidtype)
1097 {
1099 *out=1.1 + j * 10;
1100 break;
1102 *out=1.2 + j * 10;
1103 break;
1105 *out=1.3 + j * 10;
1106 break;
1108 *out=1.4 + j * 10;
1109 break;
1110 default:
1111 break;
1112 }
1113 yCDebug(FAKEMOTIONCONTROL) << "getPidOutputRaw" << pidtype << j << *out;
1114 return true;
1115}
1116
1118{
1119 bool ret = true;
1120 for(int j=0; j< _njoints; j++)
1121 {
1122 ret &= getPidOutputRaw(pidtype, j, &outs[j]);
1123 }
1124 return ret;
1125}
1126
1128// Velocity control interface raw //
1130
1132{
1133 int mode=0;
1134 getControlModeRaw(j, &mode);
1135 if( (mode != VOCAB_CM_VELOCITY) &&
1136 (mode != VOCAB_CM_MIXED) &&
1137 (mode != VOCAB_CM_IMPEDANCE_VEL) &&
1138 (mode != VOCAB_CM_IDLE))
1139 {
1140 yCError(FAKEMOTIONCONTROL) << "velocityMoveRaw: skipping command because board " << " joint " << j << " is not in VOCAB_CM_VELOCITY mode";
1141 }
1142 _command_speeds[j] = sp;
1143 last_velocity_command[j]=yarp::os::Time::now();
1144 return true;
1145}
1146
1148{
1150 bool ret = true;
1151 for (int i = 0; i < _njoints; i++) {
1152 ret &= velocityMoveRaw(i, sp[i]);
1153 }
1154 return ret;
1155}
1156
1157
1159// Calibration control interface //
1161
1163{
1164 yCTrace(FAKEMOTIONCONTROL) << "setCalibrationParametersRaw for joint" << j;
1165 return true;
1166}
1167
1168bool FakeMotionControl::calibrateAxisWithParamsRaw(int j, unsigned int type, double p1, double p2, double p3)
1169{
1170 yCTrace(FAKEMOTIONCONTROL) << "calibrateRaw for joint" << j;
1171 return true;
1172}
1173
1175{
1176 return NOT_YET_IMPLEMENTED("calibrationDoneRaw");
1177}
1178
1180// Position control interface //
1182
1184{
1185 *ax=_njoints;
1186
1187 return true;
1188}
1189
1191{
1192 if (verbose >= VERY_VERBOSE) {
1193 yCTrace(FAKEMOTIONCONTROL) << "j " << j << " ref " << ref;
1194 }
1195
1196// if (yarp::os::Time::now()-_last_position_move_time[j]<MAX_POSITION_MOVE_INTERVAL)
1197// {
1198// 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.";
1199// }
1200// _last_position_move_time[j] = yarp::os::Time::now();
1201
1202 int mode = 0;
1203 getControlModeRaw(j, &mode);
1204 if( (mode != VOCAB_CM_POSITION) &&
1205 (mode != VOCAB_CM_MIXED) &&
1206 (mode != VOCAB_CM_IMPEDANCE_POS) &&
1207 (mode != VOCAB_CM_IDLE))
1208 {
1209 yCError(FAKEMOTIONCONTROL) << "positionMoveRaw: skipping command because joint " << j << " is not in VOCAB_CM_POSITION mode";
1210 }
1211 _posCtrl_references[j] = ref;
1212 return true;
1213}
1214
1216{
1217 if (verbose >= VERY_VERBOSE) {
1219 }
1220
1221 bool ret = true;
1222 for(int j=0, index=0; j< _njoints; j++, index++)
1223 {
1224 ret &= positionMoveRaw(j, refs[index]);
1225 }
1226 return ret;
1227}
1228
1230{
1231 if (verbose >= VERY_VERBOSE) {
1232 yCTrace(FAKEMOTIONCONTROL) << "j " << j << " ref " << delta;
1233 }
1234// if (yarp::os::Time::now()-_last_position_move_time[j]<MAX_POSITION_MOVE_INTERVAL)
1235// {
1236// 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.";
1237// }
1238// _last_position_move_time[j] = yarp::os::Time::now();
1239
1240 int mode = 0;
1241 getControlModeRaw(j, &mode);
1242 if( (mode != VOCAB_CM_POSITION) &&
1243 (mode != VOCAB_CM_MIXED) &&
1244 (mode != VOCAB_CM_IMPEDANCE_POS) &&
1245 (mode != VOCAB_CM_IDLE))
1246 {
1247 yCError(FAKEMOTIONCONTROL) << "relativeMoveRaw: skipping command because joint " << j << " is not in VOCAB_CM_POSITION mode";
1248 }
1249 _posCtrl_references[j] += delta;
1250 return true;
1251}
1252
1254{
1255 if (verbose >= VERY_VERBOSE) {
1257 }
1258
1259 bool ret = true;
1260 for(int j=0, index=0; j< _njoints; j++, index++)
1261 {
1262 ret &= relativeMoveRaw(j, deltas[index]);
1263 }
1264 return ret;
1265}
1266
1267
1269{
1270 if (verbose >= VERY_VERBOSE) {
1271 yCTrace(FAKEMOTIONCONTROL) << "j ";
1272 }
1273
1274 *flag = false;
1275 return true;
1276}
1277
1279{
1280 if (verbose >= VERY_VERBOSE) {
1282 }
1283
1284 bool ret = true;
1285 bool val, tot_res = true;
1286
1287 for(int j=0, index=0; j< _njoints; j++, index++)
1288 {
1289 ret &= checkMotionDoneRaw(j, &val);
1290 tot_res &= val;
1291 }
1292 *flag = tot_res;
1293 return ret;
1294}
1295
1297{
1298 // Velocity is expressed in iDegrees/s
1299 // save internally the new value of speed; it'll be used in the positionMove
1300 int index = j ;
1301 _ref_speeds[index] = sp;
1302 return true;
1303}
1304
1306{
1307 // Velocity is expressed in iDegrees/s
1308 // save internally the new value of speed; it'll be used in the positionMove
1309 for(int j=0, index=0; j< _njoints; j++, index++)
1310 {
1311 _ref_speeds[index] = spds[index];
1312 }
1313 return true;
1314}
1315
1317{
1318 // Acceleration is expressed in iDegrees/s^2
1319 // save internally the new value of the acceleration; it'll be used in the velocityMove command
1320
1321 if (acc > 1e6)
1322 {
1323 _ref_accs[j ] = 1e6;
1324 }
1325 else if (acc < -1e6)
1326 {
1327 _ref_accs[j ] = -1e6;
1328 }
1329 else
1330 {
1331 _ref_accs[j ] = acc;
1332 }
1333
1334 return true;
1335}
1336
1338{
1339 // Acceleration is expressed in iDegrees/s^2
1340 // save internally the new value of the acceleration; it'll be used in the velocityMove command
1341 for(int j=0, index=0; j< _njoints; j++, index++)
1342 {
1343 if (accs[j] > 1e6)
1344 {
1345 _ref_accs[index] = 1e6;
1346 }
1347 else if (accs[j] < -1e6)
1348 {
1349 _ref_accs[index] = -1e6;
1350 }
1351 else
1352 {
1353 _ref_accs[index] = accs[j];
1354 }
1355 }
1356 return true;
1357}
1358
1360{
1361 *spd = _ref_speeds[j];
1362 return true;
1363}
1364
1366{
1367 memcpy(spds, _ref_speeds, sizeof(double) * _njoints);
1368 return true;
1369}
1370
1372{
1373 *acc = _ref_accs[j];
1374 return true;
1375}
1376
1378{
1379 memcpy(accs, _ref_accs, sizeof(double) * _njoints);
1380 return true;
1381}
1382
1384{
1385 velocityMoveRaw(j,0);
1386 return true;
1387}
1388
1390{
1391 bool ret = true;
1392 for(int j=0; j< _njoints; j++)
1393 {
1394 ret &= stopRaw(j);
1395 }
1396 return ret;
1397}
1399
1401// Position control2 interface //
1403
1404bool FakeMotionControl::positionMoveRaw(const int n_joint, const int *joints, const double *refs)
1405{
1406 if (verbose >= VERY_VERBOSE) {
1407 yCTrace(FAKEMOTIONCONTROL) << " -> n_joint " << n_joint;
1408 }
1409
1410 for(int j=0; j<n_joint; j++)
1411 {
1412 yCDebug(FAKEMOTIONCONTROL, "j: %d; ref %f;\n", joints[j], refs[j]); fflush(stdout);
1413 }
1414
1415 bool ret = true;
1416 for(int j=0; j<n_joint; j++)
1417 {
1419 }
1420 return ret;
1421}
1422
1423bool FakeMotionControl::relativeMoveRaw(const int n_joint, const int *joints, const double *deltas)
1424{
1425 if (verbose >= VERY_VERBOSE) {
1426 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
1427 }
1428
1429 bool ret = true;
1430 for(int j=0; j<n_joint; j++)
1431 {
1433 }
1434 return ret;
1435}
1436
1438{
1439 if (verbose >= VERY_VERBOSE) {
1440 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
1441 }
1442
1443 bool ret = true;
1444 bool val = true;
1445 bool tot_val = true;
1446
1447 for(int j=0; j<n_joint; j++)
1448 {
1449 ret = ret && checkMotionDoneRaw(joints[j], &val);
1450 tot_val &= val;
1451 }
1452 *flag = tot_val;
1453 return ret;
1454}
1455
1456bool FakeMotionControl::setRefSpeedsRaw(const int n_joint, const int *joints, const double *spds)
1457{
1458 if (verbose >= VERY_VERBOSE) {
1459 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
1460 }
1461
1462 bool ret = true;
1463 for(int j=0; j<n_joint; j++)
1464 {
1466 }
1467 return ret;
1468}
1469
1470bool FakeMotionControl::setRefAccelerationsRaw(const int n_joint, const int *joints, const double *accs)
1471{
1472 if (verbose >= VERY_VERBOSE) {
1473 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
1474 }
1475
1476 bool ret = true;
1477 for(int j=0; j<n_joint; j++)
1478 {
1480 }
1481 return ret;
1482}
1483
1484bool FakeMotionControl::getRefSpeedsRaw(const int n_joint, const int *joints, double *spds)
1485{
1486 if (verbose >= VERY_VERBOSE) {
1487 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
1488 }
1489
1490 bool ret = true;
1491 for(int j=0; j<n_joint; j++)
1492 {
1493 ret = ret && getRefSpeedRaw(joints[j], &spds[j]);
1494 }
1495 return ret;
1496}
1497
1499{
1500 if (verbose >= VERY_VERBOSE) {
1501 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
1502 }
1503
1504 bool ret = true;
1505 for(int j=0; j<n_joint; j++)
1506 {
1508 }
1509 return ret;
1510}
1511
1512bool FakeMotionControl::stopRaw(const int n_joint, const int *joints)
1513{
1514 if (verbose >= VERY_VERBOSE) {
1515 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
1516 }
1517
1518 bool ret = true;
1519 for(int j=0; j<n_joint; j++)
1520 {
1521 ret = ret &&stopRaw(joints[j]);
1522 }
1523 return ret;
1524}
1525
1527
1528// ControlMode
1529
1530// puo' essere richiesto con get
1532{
1533 if (verbose > VERY_VERY_VERBOSE) {
1534 yCTrace(FAKEMOTIONCONTROL) << "j: " << j;
1535 }
1536
1537 *v = _controlModes[j];
1538 return true;
1539}
1540
1541// IControl Mode 2
1543{
1544 bool ret = true;
1545 for(int j=0; j< _njoints; j++)
1546 {
1547 ret = ret && getControlModeRaw(j, &v[j]);
1548 }
1549 return ret;
1550}
1551
1553{
1554 bool ret = true;
1555 for(int j=0; j< n_joint; j++)
1556 {
1558 }
1559 return ret;
1560}
1561
1563{
1564 if (verbose >= VERY_VERBOSE) {
1565 yCTrace(FAKEMOTIONCONTROL) << "j: " << j << " mode: " << yarp::os::Vocab32::decode(_mode);
1566 }
1567
1569 {
1570 _controlModes[j] = VOCAB_CM_IDLE;
1571 }
1572 else
1573 {
1574 _controlModes[j] = _mode;
1575 }
1576 _posCtrl_references[j] = pos[j];
1577 return true;
1578}
1579
1580
1582{
1583 if (verbose >= VERY_VERBOSE) {
1584 yCTrace(FAKEMOTIONCONTROL) << "n_joints: " << n_joint;
1585 }
1586
1587 bool ret = true;
1588 for(int i=0; i<n_joint; i++)
1589 {
1591 }
1592 return ret;
1593}
1594
1596{
1597 if (verbose >= VERY_VERBOSE) {
1599 }
1600
1601 bool ret = true;
1602 for(int i=0; i<_njoints; i++)
1603 {
1605 }
1606 return ret;
1607}
1608
1609
1611
1613{
1614 return NOT_YET_IMPLEMENTED("setEncoder");
1615}
1616
1618{
1619 return NOT_YET_IMPLEMENTED("setEncoders");
1620}
1621
1623{
1624 return NOT_YET_IMPLEMENTED("resetEncoder");
1625}
1626
1628{
1629 return NOT_YET_IMPLEMENTED("resetEncoders");
1630}
1631
1632bool FakeMotionControl::getEncoderRaw(int j, double *value)
1633{
1634 bool ret = true;
1635
1636 // To simulate a real controlboard, we assume that the joint
1637 // encoders is exactly the last set by setPosition(s) or positionMove
1638 *value = pos[j];
1639
1640 return ret;
1641}
1642
1644{
1645 bool ret = true;
1646 for(int j=0; j< _njoints; j++)
1647 {
1648 bool ok = getEncoderRaw(j, &encs[j]);
1649 ret = ret && ok;
1650
1651 }
1652 return ret;
1653}
1654
1656{
1657 // To avoid returning uninitialized memory, we set the encoder speed to 0
1658 *sp = 0.0;
1659 return true;
1660}
1661
1663{
1664 bool ret = true;
1665 for(int j=0; j< _njoints; j++)
1666 {
1667 ret &= getEncoderSpeedRaw(j, &spds[j]);
1668 }
1669 return ret;
1670}
1671
1673{
1674 // To avoid returning uninitialized memory, we set the encoder acc to 0
1675 *acc = 0.0;
1676
1677 return true;
1678}
1679
1681{
1682 bool ret = true;
1683 for(int j=0; j< _njoints; j++)
1684 {
1686 }
1687 return ret;
1688}
1689
1691
1693{
1694 bool ret = getEncodersRaw(encs);
1695 _mutex.lock();
1696 for (int i = 0; i < _njoints; i++) {
1697 stamps[i] = _encodersStamp[i];
1698 }
1699 _mutex.unlock();
1700 return ret;
1701}
1702
1703bool FakeMotionControl::getEncoderTimedRaw(int j, double *encs, double *stamp)
1704{
1705 bool ret = getEncoderRaw(j, encs);
1706 _mutex.lock();
1707 *stamp = _encodersStamp[j];
1708 _mutex.unlock();
1709
1710 return ret;
1711}
1712
1714
1716{
1717 *num=_njoints;
1718 return true;
1719}
1720
1721bool FakeMotionControl::setMotorEncoderRaw(int m, const double val)
1722{
1723 return NOT_YET_IMPLEMENTED("setMotorEncoder");
1724}
1725
1727{
1728 return NOT_YET_IMPLEMENTED("setMotorEncoders");
1729}
1730
1732{
1733 return NOT_YET_IMPLEMENTED("setMotorEncoderCountsPerRevolutionRaw");
1734}
1735
1737{
1738 return NOT_YET_IMPLEMENTED("getMotorEncoderCountsPerRevolutionRaw");
1739}
1740
1742{
1743 return NOT_YET_IMPLEMENTED("resetMotorEncoder");
1744}
1745
1747{
1748 return NOT_YET_IMPLEMENTED("reseMotortEncoders");
1749}
1750
1752{
1753 *value = pos[m]*10;
1754 return true;
1755}
1756
1758{
1759 bool ret = true;
1760 for(int j=0; j< _njoints; j++)
1761 {
1762 ret &= getMotorEncoderRaw(j, &encs[j]);
1763
1764 }
1765 return ret;
1766}
1767
1769{
1770 *sp = 0.0;
1771 return true;
1772}
1773
1775{
1776 bool ret = true;
1777 for(int j=0; j< _njoints; j++)
1778 {
1780 }
1781 return ret;
1782}
1783
1785{
1786 *acc = 0.0;
1787 return true;
1788}
1789
1791{
1792 bool ret = true;
1793 for(int j=0; j< _njoints; j++)
1794 {
1796 }
1797 return ret;
1798}
1799
1801{
1803 _mutex.lock();
1804 for (int i = 0; i < _njoints; i++) {
1805 stamps[i] = _encodersStamp[i];
1806 }
1807 _mutex.unlock();
1808
1809 return ret;
1810}
1811
1812bool FakeMotionControl::getMotorEncoderTimedRaw(int m, double *encs, double *stamp)
1813{
1814 bool ret = getMotorEncoderRaw(m, encs);
1815 _mutex.lock();
1816 *stamp = _encodersStamp[m];
1817 _mutex.unlock();
1818
1819 return ret;
1820}
1822
1824
1826{
1827 return DEPRECATED("enableAmpRaw");
1828}
1829
1831{
1832 return DEPRECATED("disableAmpRaw");
1833}
1834
1835bool FakeMotionControl::getCurrentRaw(int j, double *value)
1836{
1837 //just for testing purposes, this is not a real implementation
1838 *value = current[j];
1839 return true;
1840}
1841
1843{
1844 //just for testing purposes, this is not a real implementation
1845 bool ret = true;
1846 for(int j=0; j< _njoints; j++)
1847 {
1848 ret &= getCurrentRaw(j, &vals[j]);
1849 }
1850 return ret;
1851}
1852
1854{
1855 maxCurrent[m] = val;
1856 return true;
1857}
1858
1860{
1861 *val = maxCurrent[m];
1862 return true;
1863}
1864
1866{
1867 (_enabledAmp[j ]) ? *st = 1 : *st = 0;
1868 return true;
1869}
1870
1872{
1873 bool ret = true;
1874 for(int j=0; j<_njoints; j++)
1875 {
1876 sts[j] = _enabledAmp[j];
1877 }
1878 return ret;
1879}
1880
1882{
1883 *val = peakCurrent[m];
1884 return true;
1885}
1886
1887bool FakeMotionControl::setPeakCurrentRaw(int m, const double val)
1888{
1889 peakCurrent[m] = val;
1890 return true;
1891}
1892
1894{
1895 *val = nominalCurrent[m];
1896 return true;
1897}
1898
1899bool FakeMotionControl::setNominalCurrentRaw(int m, const double val)
1900{
1901 nominalCurrent[m] = val;
1902 return true;
1903}
1904
1905bool FakeMotionControl::getPWMRaw(int m, double *val)
1906{
1907 *val = pwm[m];
1908 return true;
1909}
1910
1912{
1913 *val = pwmLimit[m];
1914 return true;
1915}
1916
1917bool FakeMotionControl::setPWMLimitRaw(int m, const double val)
1918{
1919 pwmLimit[m] = val;
1920 return true;
1921}
1922
1924{
1925 *val = supplyVoltage[m];
1926 return true;
1927}
1928
1929
1930// Limit interface
1931bool FakeMotionControl::setLimitsRaw(int j, double min, double max)
1932{
1933 bool ret = true;
1934 return ret;
1935}
1936
1937bool FakeMotionControl::getLimitsRaw(int j, double *min, double *max)
1938{
1939 *min = _limitsMin[j];
1940 *max = _limitsMax[j];
1941 return true;
1942}
1943
1945{
1946 return NOT_YET_IMPLEMENTED("getGearboxRatioRaw");
1947}
1948
1949bool FakeMotionControl::setGearboxRatioRaw(int m, const double val)
1950{
1951 return NOT_YET_IMPLEMENTED("setGearboxRatioRaw");
1952}
1953
1955{
1956 return NOT_YET_IMPLEMENTED("getTorqueControlFilterType");
1957}
1958
1960{
1961 return NOT_YET_IMPLEMENTED("getRotorEncoderResolutionRaw");
1962}
1963
1965{
1966 return NOT_YET_IMPLEMENTED("getJointEncoderResolutionRaw");
1967}
1968
1970{
1971 return NOT_YET_IMPLEMENTED("getJointEncoderTypeRaw");
1972}
1973
1975{
1976 return NOT_YET_IMPLEMENTED("getRotorEncoderTypeRaw");
1977}
1978
1980{
1981 return NOT_YET_IMPLEMENTED("getKinematicMJRaw");
1982}
1983
1985{
1986 return NOT_YET_IMPLEMENTED("getHasTempSensorsRaw");
1987}
1988
1990{
1991 return NOT_YET_IMPLEMENTED("getHasHallSensorRaw");
1992}
1993
1995{
1996 return NOT_YET_IMPLEMENTED("getHasRotorEncoderRaw");
1997}
1998
2000{
2001 return NOT_YET_IMPLEMENTED("getHasRotorEncoderIndexRaw");
2002}
2003
2005{
2006 return NOT_YET_IMPLEMENTED("getMotorPolesRaw");
2007}
2008
2010{
2011 return NOT_YET_IMPLEMENTED("getRotorIndexOffsetRaw");
2012}
2013
2014bool FakeMotionControl::getAxisNameRaw(int axis, std::string& name)
2015{
2016 if (axis >= 0 && axis < _njoints)
2017 {
2018 name = _axisName[axis];
2019 return true;
2020 }
2021 else
2022 {
2023 name = "ERROR";
2024 return false;
2025 }
2026}
2027
2029{
2030 if (axis >= 0 && axis < _njoints)
2031 {
2032 type = _jointType[axis];
2033 return true;
2034 }
2035 else
2036 {
2037 return false;
2038 }
2039}
2040
2041// IControlLimits
2042bool FakeMotionControl::setVelLimitsRaw(int axis, double min, double max)
2043{
2044 return NOT_YET_IMPLEMENTED("setVelLimitsRaw");
2045}
2046
2047bool FakeMotionControl::getVelLimitsRaw(int axis, double *min, double *max)
2048{
2049 *min = 0.0;
2050 *max = _maxJntCmdVelocity[axis];
2051 return true;
2052}
2053
2054
2055// Torque control
2057{
2058 *t = _torques[j];
2059 return true;
2060}
2061
2063{
2064 for (int i = 0; i < _njoints; i++)
2065 {
2066 t[i]= _torques[i];
2067 }
2068 return true;
2069}
2070
2071bool FakeMotionControl::getTorqueRangeRaw(int j, double *min, double *max)
2072{
2073 return NOT_YET_IMPLEMENTED("getTorqueRangeRaw");
2074}
2075
2076bool FakeMotionControl::getTorqueRangesRaw(double *min, double *max)
2077{
2078 return NOT_YET_IMPLEMENTED("getTorqueRangesRaw");
2079}
2080
2082{
2083 bool ret = true;
2084 for (int j = 0; j < _njoints && ret; j++) {
2085 ret &= setRefTorqueRaw(j, t[j]);
2086 }
2087 return ret;
2088}
2089
2091{
2092 _mutex.lock();
2093 _ref_torques[j]=t;
2094
2095 if (t>1.0 || t< -1.0)
2096 {
2097 yCError(FAKEMOTIONCONTROL) << "Joint received a high torque command, and was put in hardware fault";
2098 _hwfault_code[j] = 1;
2099 _hwfault_message[j] = "test" + std::to_string(j) + " torque";
2100 _controlModes[j] = VOCAB_CM_HW_FAULT;
2101 }
2102 _mutex.unlock();
2103 return true;
2104}
2105
2106bool FakeMotionControl::setRefTorquesRaw(const int n_joint, const int *joints, const double *t)
2107{
2108 return NOT_YET_IMPLEMENTED("setRefTorquesRaw");
2109}
2110
2112{
2113 bool ret = true;
2114 for (int j = 0; j < _njoints && ret; j++) {
2115 ret &= getRefTorqueRaw(j, &_ref_torques[j]);
2116 }
2117 return true;
2118}
2119
2121{
2122 _mutex.lock();
2123 *t = _ref_torques[j];
2124 _mutex.unlock();
2125 return true;
2126}
2127
2128bool FakeMotionControl::getImpedanceRaw(int j, double *stiffness, double *damping)
2129{
2130 return NOT_YET_IMPLEMENTED("getImpedanceRaw");
2131}
2132
2133bool FakeMotionControl::setImpedanceRaw(int j, double stiffness, double damping)
2134{
2135 return NOT_YET_IMPLEMENTED("setImpedanceRaw");
2136}
2137
2139{
2140 return NOT_YET_IMPLEMENTED("setImpedanceOffsetRaw");
2141}
2142
2144{
2145 return NOT_YET_IMPLEMENTED("getImpedanceOffsetRaw");
2146}
2147
2148bool FakeMotionControl::getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
2149{
2150 return NOT_YET_IMPLEMENTED("getCurrentImpedanceLimitRaw");
2151}
2152
2154{
2155 params->bemf = _kbemf[j];
2156 params->bemf_scale = _kbemf_scale[j];
2157 params->ktau = _ktau[j];
2158 params->ktau_scale = _ktau_scale[j];
2159 params->viscousPos = _viscousPos[j];
2160 params->viscousNeg = _viscousNeg[j];
2161 params->coulombPos = _coulombPos[j];
2162 params->coulombNeg = _coulombNeg[j];
2163 params->velocityThres = _velocityThres[j];
2164 yCDebug(FAKEMOTIONCONTROL) << "getMotorTorqueParamsRaw" << params->bemf
2165 << params->bemf_scale
2166 << params->ktau
2167 << params->ktau_scale
2168 << params->viscousPos
2169 << params->viscousNeg
2170 << params->coulombPos
2171 << params->coulombNeg
2172 << params->velocityThres;
2173 return true;
2174}
2175
2177{
2178 _kbemf[j] = params.bemf;
2179 _ktau[j] = params.ktau;
2180 _kbemf_scale[j] = params.bemf_scale;
2181 _ktau_scale[j] = params.ktau_scale;
2182 _viscousPos[j] = params.viscousPos;
2183 _viscousNeg[j] = params.viscousNeg;
2184 _coulombPos[j] = params.coulombPos;
2185 _coulombNeg[j] = params.coulombNeg;
2186 _velocityThres[j] = params.velocityThres;
2187 yCDebug(FAKEMOTIONCONTROL) << "setMotorTorqueParamsRaw" << params.bemf
2188 << params.bemf_scale
2189 << params.ktau
2190 << params.ktau_scale
2191 << params.viscousPos
2192 << params.viscousNeg
2193 << params.coulombPos
2194 << params.coulombNeg
2195 << params.velocityThres;
2196 return true;
2197}
2198
2199// IVelocityControl interface
2200bool FakeMotionControl::velocityMoveRaw(const int n_joint, const int *joints, const double *spds)
2201{
2202 bool ret = true;
2203 for(int i=0; i<n_joint; i++)
2204 {
2206 }
2207 return ret;
2208}
2209
2210// PositionDirect Interface
2212{
2213 _posDir_references[j] = ref;
2214 return true;
2215}
2216
2217bool FakeMotionControl::setPositionsRaw(const int n_joint, const int *joints, const double *refs)
2218{
2219 for(int i=0; i< n_joint; i++)
2220 {
2221 _posDir_references[joints[i]] = refs[i];
2222 }
2223 return true;
2224}
2225
2227{
2228 for(int i=0; i< _njoints; i++)
2229 {
2230 _posDir_references[i] = refs[i];
2231 }
2232 return true;
2233}
2234
2235
2237{
2238 if (verbose >= VERY_VERBOSE) {
2239 yCTrace(FAKEMOTIONCONTROL) << "j " << axis << " ref " << _posCtrl_references[axis];
2240 }
2241
2242 int mode = 0;
2243 getControlModeRaw(axis, &mode);
2244 if( (mode != VOCAB_CM_POSITION) &&
2245 (mode != VOCAB_CM_MIXED) &&
2246 (mode != VOCAB_CM_IMPEDANCE_POS))
2247 {
2248 yCWarning(FAKEMOTIONCONTROL) << "getTargetPosition: Joint " << axis << " is not in POSITION mode, therefore the value returned by " <<
2249 "this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
2250 }
2251 *ref = _posCtrl_references[axis];
2252 return true;
2253}
2254
2256{
2257 bool ret = true;
2258 for (int i = 0; i < _njoints; i++) {
2260 }
2261 return ret;
2262}
2263
2264bool FakeMotionControl::getTargetPositionsRaw(int nj, const int * jnts, double *refs)
2265{
2266 bool ret = true;
2267 for (int i = 0; i<nj; i++)
2268 {
2270 }
2271 return ret;
2272}
2273
2275{
2276 *ref = _command_speeds[axis];
2277 return true;
2278}
2279
2281{
2282 bool ret = true;
2283 for (int i = 0; i<_njoints; i++)
2284 {
2285 ret &= getRefVelocityRaw(i, &refs[i]);
2286 }
2287 return ret;
2288}
2289
2290bool FakeMotionControl::getRefVelocitiesRaw(int nj, const int * jnts, double *refs)
2291{
2292 bool ret = true;
2293 for (int i = 0; i<nj; i++)
2294 {
2295 ret &= getRefVelocityRaw(jnts[i], &refs[i]);
2296 }
2297 return ret;
2298}
2299
2301{
2302 int mode = 0;
2303 getControlModeRaw(axis, &mode);
2304 if( (mode != VOCAB_CM_POSITION) &&
2305 (mode != VOCAB_CM_MIXED) &&
2306 (mode != VOCAB_CM_IMPEDANCE_POS) &&
2307 (mode != VOCAB_CM_POSITION_DIRECT) )
2308 {
2309 yCWarning(FAKEMOTIONCONTROL) << "getRefPosition: Joint " << axis << " is not in POSITION_DIRECT mode, therefore the value returned by \
2310 this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
2311 }
2312
2313 *ref = _posDir_references[axis];
2314
2315 return true;
2316}
2317
2319{
2320 bool ret = true;
2321 for (int i = 0; i<_njoints; i++)
2322 {
2323 ret &= getRefPositionRaw(i, &refs[i]);
2324 }
2325 return ret;
2326}
2327
2328bool FakeMotionControl::getRefPositionsRaw(int nj, const int * jnts, double *refs)
2329{
2330 bool ret = true;
2331 for (int i = 0; i<nj; i++)
2332 {
2333 ret &= getRefPositionRaw(jnts[i], &refs[i]);
2334 }
2335 return ret;
2336}
2337
2338
2339// InteractionMode
2341{
2342 if (verbose > VERY_VERY_VERBOSE) {
2343 yCTrace(FAKEMOTIONCONTROL) << "j: " << j;
2344 }
2345
2346 *_mode = (yarp::dev::InteractionModeEnum)_interactMode[j];
2347 return true;}
2348
2350{
2351 bool ret = true;
2352 for(int j=0; j< n_joints; j++)
2353 {
2355 }
2356 return ret;
2357
2358}
2359
2361{
2362 bool ret = true;
2363 for (int j = 0; j < _njoints; j++) {
2365 }
2366 return ret;
2367}
2368
2369// marco.accame: con alberto cardellino abbiamo parlato della correttezza di effettuare la verifica di quanto imposto (in setInteractionModeRaw() ed affini)
2370// andando a rileggere il valore nella scheda eth fino a che esso non sia quello atteso. si deve fare oppure no?
2371// con il interaction mode il can ora non lo fa. mentre lo fa per il control mode. perche' diverso?
2373{
2374 if (verbose >= VERY_VERBOSE) {
2375 yCTrace(FAKEMOTIONCONTROL) << "j: " << j << " interaction mode: " << yarp::os::Vocab32::decode(_mode);
2376 }
2377
2378 _interactMode[j] = _mode;
2379
2380 return true;
2381}
2382
2383
2385{
2386 bool ret = true;
2387 for(int i=0; i<n_joints; i++)
2388 {
2390 }
2391 return ret;
2392}
2393
2395{
2396 bool ret = true;
2397 for(int i=0; i<_njoints; i++)
2398 {
2400 }
2401 return ret;
2402
2403}
2404
2406{
2407 *num=_njoints;
2408 return true;
2409}
2410
2412{
2413 *val = 37.5+double(m);
2414 return true;
2415}
2416
2418{
2419 bool ret = true;
2420 for(int j=0; j< _njoints; j++)
2421 {
2422 ret &= getTemperatureRaw(j, &vals[j]);
2423 }
2424 return ret;
2425}
2426
2428{
2429 return NOT_YET_IMPLEMENTED("getTemperatureLimitRaw");
2430}
2431
2432bool FakeMotionControl::setTemperatureLimitRaw(int m, const double temp)
2433{
2434 return NOT_YET_IMPLEMENTED("setTemperatureLimitRaw");
2435}
2436
2437//PWM interface
2439{
2440 refpwm[j] = v;
2441 pwm[j] = v;
2442 last_pwm_command[j]=yarp::os::Time::now();
2443 return true;
2444}
2445
2447{
2448 for (int i = 0; i < _njoints; i++)
2449 {
2451 }
2452 return true;
2453}
2454
2456{
2457 *v = refpwm[j];
2458 return true;
2459}
2460
2462{
2463 for (int i = 0; i < _njoints; i++)
2464 {
2465 v[i] = refpwm[i];
2466 }
2467 return true;
2468}
2469
2471{
2472 *v = pwm[j];
2473 return true;
2474}
2475
2477{
2478 for (int i = 0; i < _njoints; i++)
2479 {
2480 v[i] = pwm[i];
2481 }
2482 return true;
2483}
2484
2485// Current interface
2486/*bool FakeMotionControl::getCurrentRaw(int j, double *t)
2487{
2488 return NOT_YET_IMPLEMENTED("getCurrentRaw");
2489}
2490
2491bool FakeMotionControl::getCurrentsRaw(double *t)
2492{
2493 return NOT_YET_IMPLEMENTED("getCurrentsRaw");
2494}
2495*/
2496
2497bool FakeMotionControl::getCurrentRangeRaw(int j, double *min, double *max)
2498{
2499 //just for testing purposes, this is not a real implementation
2500 *min = _ref_currents[j] / 100;
2501 *max = _ref_currents[j] * 100;
2502 return true;
2503}
2504
2505bool FakeMotionControl::getCurrentRangesRaw(double *min, double *max)
2506{
2507 //just for testing purposes, this is not a real implementation
2508 for (int i = 0; i < _njoints; i++)
2509 {
2510 min[i] = _ref_currents[i] / 100;
2511 max[i] = _ref_currents[i] * 100;
2512 }
2513 return true;
2514}
2515
2517{
2518 for (int i = 0; i < _njoints; i++)
2519 {
2520 _ref_currents[i] = t[i];
2521 current[i] = t[i] / 2;
2522 }
2523 return true;
2524}
2525
2527{
2528 _ref_currents[j] = t;
2529 current[j] = t / 2;
2530 return true;
2531}
2532
2533bool FakeMotionControl::setRefCurrentsRaw(const int n_joint, const int *joints, const double *t)
2534{
2535 bool ret = true;
2536 for (int j = 0; j<n_joint; j++)
2537 {
2538 ret = ret &&setRefCurrentRaw(joints[j], t[j]);
2539 }
2540 return ret;
2541}
2542
2544{
2545 for (int i = 0; i < _njoints; i++)
2546 {
2547 t[i] = _ref_currents[i];
2548 }
2549 return true;
2550}
2551
2553{
2554 *t = _ref_currents[j];
2555 return true;
2556}
2557
2562
2564{
2565 return _njoints;
2566}
2567
2569{
2570 for (int i = 0; i < _njoints; i++)
2571 {
2572 measure[i] = _torques[i];
2573 }
2574 return true;
2575}
2576
2578{
2579 _torques[ch] = measure;
2580 return true;
2581}
2582
2583bool FakeMotionControl::getLastJointFaultRaw(int j, int& fault, std::string& message)
2584{
2585 _mutex.lock();
2586 fault = _hwfault_code[j];
2587 message = _hwfault_message[j];
2588 _mutex.unlock();
2589 return true;
2590}
2591
2592
2593/*
2594bool FakeMotionControl::parseImpedanceGroup_NewFormat(Bottle& pidsGroup, ImpedanceParameters vals[])
2595{
2596 int j=0;
2597 Bottle xtmp;
2598
2599 if (!extractGroup(pidsGroup, xtmp, "stiffness", "stiffness parameter", _njoints)) {
2600 return false;
2601 }
2602 for (j=0; j<_njoints; j++) {
2603 vals[j].stiffness = xtmp.get(j+1).asFloat64();
2604 }
2605
2606 if (!extractGroup(pidsGroup, xtmp, "damping", "damping parameter", _njoints)) {
2607 return false;
2608 }
2609 for (j=0; j<_njoints; j++) {
2610 vals[j].damping = xtmp.get(j+1).asFloat64();
2611 }
2612
2613 return true;
2614}
2615*/
2616
2617/*
2618bool FakeMotionControl::parsePositionPidsGroup(Bottle& pidsGroup, Pid myPid[])
2619{
2620 int j=0;
2621 Bottle xtmp;
2622
2623 if (!extractGroup(pidsGroup, xtmp, "kp", "Pid kp parameter", _njoints)) {
2624 return false;
2625 }
2626 for (j=0; j<_njoints; j++) {
2627 myPid[j].kp = xtmp.get(j+1).asFloat64();
2628 }
2629
2630 if (!extractGroup(pidsGroup, xtmp, "kd", "Pid kd parameter", _njoints)) {
2631 return false;
2632 }
2633 for (j=0; j<_njoints; j++) {
2634 myPid[j].kd = xtmp.get(j+1).asFloat64();
2635 }
2636
2637 if (!extractGroup(pidsGroup, xtmp, "ki", "Pid kp parameter", _njoints)) {
2638 return false;
2639 }
2640 for (j=0; j<_njoints; j++) {
2641 myPid[j].ki = xtmp.get(j+1).asFloat64();
2642 }
2643
2644 if (!extractGroup(pidsGroup, xtmp, "maxInt", "Pid maxInt parameter", _njoints)) {
2645 return false;
2646 }
2647 for (j=0; j<_njoints; j++) {
2648 myPid[j].max_int = xtmp.get(j+1).asFloat64();
2649 }
2650
2651 if (!extractGroup(pidsGroup, xtmp, "maxPwm", "Pid maxPwm parameter", _njoints)) {
2652 return false;
2653 }
2654 for (j=0; j<_njoints; j++) {
2655 myPid[j].max_output = xtmp.get(j+1).asFloat64();
2656 }
2657
2658 if (!extractGroup(pidsGroup, xtmp, "shift", "Pid shift parameter", _njoints)) {
2659 return false;
2660 }
2661 for (j=0; j<_njoints; j++) {
2662 myPid[j].scale = xtmp.get(j+1).asFloat64();
2663 }
2664
2665 if (!extractGroup(pidsGroup, xtmp, "ko", "Pid ko parameter", _njoints)) {
2666 return false;
2667 }
2668 for (j=0; j<_njoints; j++) {
2669 myPid[j].offset = xtmp.get(j+1).asFloat64();
2670 }
2671
2672 if (!extractGroup(pidsGroup, xtmp, "stictionUp", "Pid stictionUp", _njoints)) {
2673 return false;
2674 }
2675 for (j=0; j<_njoints; j++) {
2676 myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
2677 }
2678
2679 if (!extractGroup(pidsGroup, xtmp, "stictionDwn", "Pid stictionDwn", _njoints)) {
2680 return false;
2681 }
2682 for (j=0; j<_njoints; j++) {
2683 myPid[j].stiction_down_val = xtmp.get(j+1).asFloat64();
2684 }
2685
2686 if (!extractGroup(pidsGroup, xtmp, "kff", "Pid kff parameter", _njoints)) {
2687 return false;
2688 }
2689 for (j=0; j<_njoints; j++) {
2690 myPid[j].kff = xtmp.get(j+1).asFloat64();
2691 }
2692
2693 //conversion from metric to machine units (if applicable)
2694 if (_positionControlUnits==P_METRIC_UNITS)
2695 {
2696 for (j=0; j<_njoints; j++)
2697 {
2698 myPid[j].kp = myPid[j].kp / _angleToEncoder[j]; //[PWM/deg]
2699 myPid[j].ki = myPid[j].ki / _angleToEncoder[j]; //[PWM/deg]
2700 myPid[j].kd = myPid[j].kd / _angleToEncoder[j]; //[PWM/deg]
2701 }
2702 }
2703 else
2704 {
2705 //do nothing
2706 }
2707
2708 //optional PWM limit
2709 if(_pwmIsLimited)
2710 { // check for value in the file
2711 if (!extractGroup(pidsGroup, xtmp, "limPwm", "Limited PWD", _njoints))
2712 {
2713 yCError(FAKEMOTIONCONTROL) << "The PID parameter limPwm was requested but was not correctly set in the configuration file, please fill it.";
2714 return false;
2715 }
2716
2717 yCInfo(FAKEMOTIONCONTROL) << "Using LIMITED PWM!!";
2718 for (j = 0; j < _njoints; j++) {
2719 myPid[j].max_output = xtmp.get(j + 1).asFloat64();
2720 }
2721 }
2722
2723 return true;
2724}
2725*/
2726
2727/*
2728bool FakeMotionControl::parseTorquePidsGroup(Bottle& pidsGroup, Pid myPid[], double kbemf[], double ktau[], int filterType[], double viscousPos[], double viscousNeg[], double coulombPos[], double coulombNeg[], double velocityThres[])
2729{
2730 int j=0;
2731 Bottle xtmp;
2732 if (!extractGroup(pidsGroup, xtmp, "kp", "Pid kp parameter", _njoints)) {
2733 return false;
2734 }
2735 for (j=0; j<_njoints; j++) {
2736 myPid[j].kp = xtmp.get(j+1).asFloat64();
2737 }
2738
2739 if (!extractGroup(pidsGroup, xtmp, "kd", "Pid kd parameter", _njoints)) {
2740 return false;
2741 }
2742 for (j=0; j<_njoints; j++) {
2743 myPid[j].kd = xtmp.get(j+1).asFloat64();
2744 }
2745
2746 if (!extractGroup(pidsGroup, xtmp, "ki", "Pid kp parameter", _njoints)) {
2747 return false;
2748 }
2749 for (j=0; j<_njoints; j++) {
2750 myPid[j].ki = xtmp.get(j+1).asFloat64();
2751 }
2752
2753 if (!extractGroup(pidsGroup, xtmp, "maxInt", "Pid maxInt parameter", _njoints)) {
2754 return false;
2755 }
2756 for (j=0; j<_njoints; j++) {
2757 myPid[j].max_int = xtmp.get(j+1).asFloat64();
2758 }
2759
2760 if (!extractGroup(pidsGroup, xtmp, "maxPwm", "Pid maxPwm parameter", _njoints)) {
2761 return false;
2762 }
2763 for (j=0; j<_njoints; j++) {
2764 myPid[j].max_output = xtmp.get(j+1).asFloat64();
2765 }
2766
2767 if (!extractGroup(pidsGroup, xtmp, "shift", "Pid shift parameter", _njoints)) {
2768 return false;
2769 }
2770 for (j=0; j<_njoints; j++) {
2771 myPid[j].scale = xtmp.get(j+1).asFloat64();
2772 }
2773
2774 if (!extractGroup(pidsGroup, xtmp, "ko", "Pid ko parameter", _njoints)) {
2775 return false;
2776 }
2777 for (j=0; j<_njoints; j++) {
2778 myPid[j].offset = xtmp.get(j+1).asFloat64();
2779 }
2780
2781 if (!extractGroup(pidsGroup, xtmp, "stictionUp", "Pid stictionUp", _njoints)) {
2782 return false;
2783 }
2784 for (j=0; j<_njoints; j++) {
2785 myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
2786 }
2787
2788 if (!extractGroup(pidsGroup, xtmp, "stictionDwn", "Pid stictionDwn", _njoints)) {
2789 return false;
2790 }
2791 for (j=0; j<_njoints; j++) {
2792 myPid[j].stiction_down_val = xtmp.get(j+1).asFloat64();
2793 }
2794
2795 if (!extractGroup(pidsGroup, xtmp, "kff", "Pid kff parameter", _njoints)) {
2796 return false;
2797 }
2798 for (j=0; j<_njoints; j++) {
2799 myPid[j].kff = xtmp.get(j+1).asFloat64();
2800 }
2801
2802 if (!extractGroup(pidsGroup, xtmp, "kbemf", "kbemf parameter", _njoints)) {
2803 return false;
2804 }
2805 for (j=0; j<_njoints; j++) {
2806 kbemf[j] = xtmp.get(j+1).asFloat64();
2807 }
2808
2809 if (!extractGroup(pidsGroup, xtmp, "ktau", "ktau parameter", _njoints)) {
2810 return false;
2811 }
2812 for (j=0; j<_njoints; j++) {
2813 ktau[j] = xtmp.get(j+1).asFloat64();
2814 }
2815
2816 if (!extractGroup(pidsGroup, xtmp, "filterType", "filterType param", _njoints)) {
2817 return false;
2818 }
2819 for (j=0; j<_njoints; j++) {
2820 filterType[j] = xtmp.get(j+1).asInt32();
2821 }
2822
2823 if (!extractGroup(pidsGroup, xtmp, "viscousPos", "viscous pos parameter", _njoints)) {
2824 return false;
2825 }
2826 for (j=0; j<_njoints; j++) {
2827 viscousPos[j] = xtmp.get(j+1).asFloat64();
2828 }
2829
2830 if (!extractGroup(pidsGroup, xtmp, "viscousNeg", "viscous neg parameter", _njoints)) {
2831 return false;
2832 }
2833 for (j=0; j<_njoints; j++) {
2834 viscousNeg[j] = xtmp.get(j+1).asFloat64();
2835 }
2836
2837 if (!extractGroup(pidsGroup, xtmp, "coulombPos", "coulomb pos parameter", _njoints)) {
2838 return false;
2839 }
2840 for (j=0; j<_njoints; j++) {
2841 coulombPos[j] = xtmp.get(j+1).asFloat64();
2842 }
2843
2844 if (!extractGroup(pidsGroup, xtmp, "coulombNeg", "coulomb neg parameter", _njoints)) {
2845 return false;
2846 }
2847 for (j=0; j<_njoints; j++) {
2848 coulombNeg[j] = xtmp.get(j+1).asFloat64();
2849 }
2850
2851 if (!extractGroup(pidsGroup, xtmp, "velocityThres", "velocity threshold parameter", _njoints)) {
2852 return false;
2853 }
2854 for (j=0; j<_njoints; j++) {
2855 velocityThres[j] = xtmp.get(j+1).asFloat64();
2856 }
2857
2858
2859 //conversion from metric to machine units (if applicable)
2860// for (j=0; j<_njoints; j++)
2861// {
2862// myPid[j].kp = myPid[j].kp / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
2863// myPid[j].ki = myPid[j].ki / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
2864// myPid[j].kd = myPid[j].kd / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
2865// myPid[j].stiction_up_val = myPid[j].stiction_up_val * _torqueControlHelper->getNewtonsToSensor(j); //[Nm]
2866// myPid[j].stiction_down_val = myPid[j].stiction_down_val * _torqueControlHelper->getNewtonsToSensor(j); //[Nm]
2867// }
2868
2869 //optional PWM limit
2870 if(_pwmIsLimited)
2871 { // check for value in the file
2872 if (!extractGroup(pidsGroup, xtmp, "limPwm", "Limited PWM", _njoints))
2873 {
2874 yCError(FAKEMOTIONCONTROL) << "The PID parameter limPwm was requested but was not correctly set in the configuration file, please fill it.";
2875 return false;
2876 }
2877
2878 yCInfo(FAKEMOTIONCONTROL) << "Using LIMITED PWM!!";
2879 for (j = 0; j < _njoints; j++) {
2880 myPid[j].max_output = xtmp.get(j + 1).asFloat64();
2881 }
2882 }
2883
2884 return true;
2885}
2886*/
2887
2888// eof
void checkAndDestroy(T *&p)
bool NOT_YET_IMPLEMENTED(const char *txt)
#define OPENLOOP_WATCHDOG
std::string toString(const T &value)
convert an arbitrary type to string.
#define PWM_CONSTANT
static bool NOT_YET_IMPLEMENTED(const char *txt)
static bool DEPRECATED(const char *txt)
#define VELOCITY_WATCHDOG
float t
constexpr yarp::conf::vocab32_t VOCAB_CM_MIXED
constexpr yarp::conf::vocab32_t VOCAB_CM_TORQUE
constexpr yarp::conf::vocab32_t VOCAB_CM_PWM
constexpr yarp::conf::vocab32_t VOCAB_CM_HW_FAULT
constexpr yarp::conf::vocab32_t VOCAB_CM_IMPEDANCE_VEL
constexpr yarp::conf::vocab32_t VOCAB_CM_IDLE
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION
constexpr yarp::conf::vocab32_t VOCAB_CM_IMPEDANCE_POS
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_CM_VELOCITY
constexpr yarp::conf::vocab32_t VOCAB_CM_FORCE_IDLE
bool ret
#define yDebug(...)
Definition Log.h:275
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool getPowerSupplyVoltageRaw(int j, double *val) override
bool getRefSpeedRaw(int j, double *ref) override
Get reference speed for a joint.
virtual bool getHasTempSensorsRaw(int j, int &ret)
bool setRefCurrentsRaw(const double *t) override
Set the reference value of the currents for all motors.
bool setRefTorqueRaw(int j, double t) override
Set the reference value of the torque for a given joint.
bool getCurrentsRaw(double *vals) override
bool getImpedanceOffsetRaw(int j, double *offset) override
Get current force Offset for a specific joint.
bool getTargetPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
bool getTorqueRangeRaw(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
bool getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
Get the current impedandance limits for a specific joint.
bool setTemperatureLimitRaw(int m, const double temp) override
Set the temperature limit for a specific motor.
bool getPidErrorRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
bool setPeakCurrentRaw(int m, const double val) override
bool velocityMoveRaw(int j, double sp) override
Start motion at a given speed, single joint.
bool checkMotionDoneRaw(bool *flag) override
Check if the current trajectory is terminated.
bool getMotorEncoderSpeedRaw(int m, double *sp) override
Read the istantaneous speed of a motor encoder.
virtual bool getJointEncoderTypeRaw(int j, int &type)
bool setRefDutyCycleRaw(int j, double v) override
Sets the reference dutycycle of a single motor.
bool getTemperatureRaw(int m, double *val) override
Get temperature of a motor.
bool getEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of an axis.
bool getRefAccelerationRaw(int j, double *acc) override
Get reference acceleration for a joint.
bool getMotorEncoderSpeedsRaw(double *spds) override
Read the instantaneous speed of all motor encoders.
bool setPidsRaw(const yarp::dev::PidControlTypeEnum &pidtype, const yarp::dev::Pid *pids) override
Set new pid value on multiple axes.
bool setVelLimitsRaw(int axis, double min, double max) override
Set the software speed limits for a particular axis, the behavior of the control card when these limi...
bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
bool setMotorEncoderRaw(int m, const double val) override
Set the value of the motor encoder for a given motor.
bool getPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *limit) override
Get the error limit for the controller on a specific joint.
bool getPidOutputRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
bool setControlModeRaw(const int j, const int mode) override
bool getMotorEncoderAccelerationRaw(int m, double *spds) override
Read the instantaneous acceleration of a motor encoder.
bool open(yarp::os::Searchable &par) override
Open the DeviceDriver.
virtual bool getHasHallSensorRaw(int j, int &ret)
bool getTemperatureLimitRaw(int m, double *temp) override
Retreives the current temperature limit for a specific motor.
bool getNumberOfMotorsRaw(int *num) override
Get the number of available motors.
bool disableAmpRaw(int j) override
Disable the amplifier on a specific joint.
bool setMotorTorqueParamsRaw(int j, const yarp::dev::MotorTorqueParameters params) override
Set the motor parameters.
bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
Start calibration, this method is very often platform specific.
bool setRefAccelerationRaw(int j, double acc) override
Set reference acceleration for a joint.
virtual bool initialised()
bool setGearboxRatioRaw(int m, const double val) override
Set the gearbox ratio for a specific motor.
bool setEncodersRaw(const double *vals) override
Set the value of all encoders.
bool getPidOutputsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
bool isPidEnabledRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, bool *enabled) override
Get the current status (enabled/disabled) of the pid controller.
bool setRefSpeedsRaw(const double *spds) override
Set reference speed on all joints.
bool getTargetPositionsRaw(double *refs) override
Get the last position reference for all axes.
bool getRefDutyCycleRaw(int j, double *v) override
Gets the last reference sent using the setRefDutyCycleRaw function.
bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
bool setPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum &pidtype, const double *limits) override
Get the error limit for the controller on all joints.
bool getPidReferenceRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *ref) override
Get the current reference of the pid controller for a specific joint.
bool getNominalCurrentRaw(int m, double *val) override
bool getControlModeRaw(int j, int *v) override
bool calibrationDoneRaw(int j) override
Check if the calibration is terminated, on a particular joint.
bool threadInit() override
Initialization method.
bool getRefDutyCyclesRaw(double *v) override
Gets the last reference sent using the setRefDutyCyclesRaw function.
bool getRefVelocityRaw(const int joint, double *ref) override
Get the last reference speed set by velocityMove for single joint.
bool getEncoderTimedRaw(int j, double *encs, double *stamp) override
Read the instantaneous acceleration of all axes.
bool disablePidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Disable the pid computation for a joint.
yarp::dev::VAS_status getVirtualAnalogSensorStatusRaw(int ch) override
Check the status of a given channel.
bool getMotorEncodersRaw(double *encs) override
Read the position of all motor encoders.
bool getTorqueRangesRaw(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
bool getRefCurrentsRaw(double *t) override
Get the reference value of the currents for all motors.
bool setRefDutyCyclesRaw(const double *v) override
Sets the reference dutycycle for all motors.
bool getEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of an axis.
int getVirtualAnalogSensorChannelsRaw() override
Get the number of channels of the virtual sensor.
bool resetPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Reset the controller of a given joint, usually sets the current status of the joint as the reference ...
bool getCurrentRangesRaw(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
bool getEncoderRaw(int j, double *v) override
Read the value of an encoder.
bool getPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *limits) override
Get the error limit for all controllers.
bool enableAmpRaw(int j) override
Enable the amplifier on a specific joint.
bool fromConfig(yarp::os::Searchable &config)
bool getPidsRaw(const yarp::dev::PidControlTypeEnum &pidtype, yarp::dev::Pid *pids) override
Get current pid value for a specific joint.
bool getRefPositionsRaw(double *refs) override
Get the last position reference for all axes.
bool getMotorTorqueParamsRaw(int j, yarp::dev::MotorTorqueParameters *params) override
Get the motor parameters.
bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
bool setRefSpeedRaw(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
virtual bool getHasRotorEncoderIndexRaw(int j, int &ret)
bool updateVirtualAnalogSensorMeasureRaw(yarp::sig::Vector &measure) override
Set a vector of torque values for virtual sensor.
void resizeBuffers()
Resize previously allocated buffers.
void threadRelease() override
Release method.
bool getAmpStatusRaw(int *st) override
bool getLastJointFaultRaw(int j, int &fault, std::string &message) override
bool setImpedanceRaw(int j, double stiffness, double damping) override
Set current impedance parameters (stiffness,damping) for a specific joint.
bool setPWMLimitRaw(int j, const double val) override
bool relativeMoveRaw(int j, double delta) override
Set relative position.
virtual bool getMotorPolesRaw(int j, int &poles)
bool getPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::Pid *pid) override
Get current pid value for a specific joint.
bool getCurrentRaw(int j, double *val) override
bool getPeakCurrentRaw(int m, double *val) override
bool getDutyCyclesRaw(double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
bool setEncoderRaw(int j, double val) override
Set the value of the encoder for a given joint.
bool getRefPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
bool getLimitsRaw(int axis, double *min, double *max) override
Get the software limits for a particular axis.
virtual bool getRotorIndexOffsetRaw(int j, double &rotorOffset)
bool getVelLimitsRaw(int axis, double *min, double *max) override
Get the software speed limits for a particular axis.
bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
Set new reference point for all axes.
bool setPidReferenceRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
bool resetEncodersRaw() override
Reset encoders.
bool getPWMRaw(int j, double *val) override
bool getAxisNameRaw(int axis, std::string &name) override
bool setCalibrationParametersRaw(int axis, const yarp::dev::CalibrationParameters &params) override
Start calibration, this method is very often platform specific.
bool setPidOffsetRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override
Set an offset value on the ourput of pid controller.
bool getInteractionModeRaw(int j, yarp::dev::InteractionModeEnum *_mode) override
Get the current interaction mode of the robot, values can be stiff or compliant.
bool getCurrentRangeRaw(int j, double *min, double *max) override
Get the full scale of the current measurement for a given motor (e.g.
bool stopRaw() override
Stop motion, multiple joints.
virtual bool getKinematicMJRaw(int j, double &rotres)
bool getRefSpeedsRaw(double *spds) override
Get reference speed of all joints.
bool getRefAccelerationsRaw(double *accs) override
Get reference acceleration of all joints.
virtual bool getJointEncoderResolutionRaw(int m, double &jntres)
bool getAxes(int *ax) override
Get the number of controlled axes.
bool setMaxCurrentRaw(int j, double val) override
bool alloc(int njoints)
Allocated buffers.
bool resetMotorEncodersRaw() override
Reset motor encoders.
bool setRefTorquesRaw(const double *t) override
Set the reference value of the torque for all joints.
bool getEncodersRaw(double *encs) override
Read the position of all axes.
bool setRefAccelerationsRaw(const double *accs) override
Set reference acceleration on all joints.
bool getControlModesRaw(int *v) override
bool setPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, const yarp::dev::Pid &pid) override
Set new pid value for a joint axis.
bool getPWMLimitRaw(int j, double *val) override
virtual bool getRotorEncoderTypeRaw(int j, int &type)
bool getRefCurrentRaw(int j, double *t) override
Get the reference value of the current for a single motor.
bool getDutyCycleRaw(int j, double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
bool setPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double limit) override
Set the error limit for the controller on a specific joint.
bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
bool positionMoveRaw(int j, double ref) override
Set new reference point for a single axis.
bool getMotorEncoderCountsPerRevolutionRaw(int m, double *v) override
Gets number of counts per revolution for motor encoder m.
bool setImpedanceOffsetRaw(int j, double offset) override
Set current force Offset for a specific joint.
bool setMotorEncodersRaw(const double *vals) override
Set the value of all motor encoders.
bool getPidErrorsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
bool getGearboxRatioRaw(int m, double *gearbox) override
Get the gearbox ratio for a specific motor.
bool getMaxCurrentRaw(int j, double *val) override
Returns the maximum electric current allowed for a given motor.
bool close() override
Close the DeviceDriver.
bool getRefTorquesRaw(double *t) override
Get the reference value of the torque for all joints.
bool setRefCurrentRaw(int j, double t) override
Set the reference value of the current for a single motor.
bool getNumberOfMotorEncodersRaw(int *num) override
Get the number of available motor encoders.
bool getPidReferencesRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all pid controllers.
bool getTorqueRaw(int j, double *t) override
Get the value of the torque on a given joint (this is the feedback if you have a torque sensor).
bool getImpedanceRaw(int j, double *stiffness, double *damping) override
Get current impedance parameters (stiffness,damping,offset) for a specific joint.
bool enablePidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Enable the pid computation for a joint.
bool getRefVelocitiesRaw(double *refs) override
Get the last reference speed set by velocityMove for all joints.
bool setPositionRaw(int j, double ref) override
Set new position for a single axis.
bool getEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all axes.
bool setPidReferencesRaw(const yarp::dev::PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
bool getMotorEncoderRaw(int m, double *v) override
Read the value of a motor encoder.
bool getRefTorqueRaw(int j, double *t) override
Set the reference value of the torque for a given joint.
bool getMotorEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all motor encoders.
bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
Set the interaction mode of the robot for a set of joints, values can be stiff or compliant.
bool getTemperaturesRaw(double *vals) override
Get temperature of all the motors.
bool resetMotorEncoderRaw(int m) override
Reset motor encoder, single motor.
bool setNominalCurrentRaw(int m, const double val) override
virtual bool getRotorEncoderResolutionRaw(int m, double &rotres)
virtual bool getHasRotorEncoderRaw(int j, int &ret)
bool getMotorEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous position of all motor encoders.
bool resetEncoderRaw(int j) override
Reset encoder, single joint.
bool getTorquesRaw(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
bool setLimitsRaw(int axis, double min, double max) override
Set the software limits for a particular axis, the behavior of the control card when these limits are...
bool setInteractionModeRaw(int j, yarp::dev::InteractionModeEnum _mode) override
Set the interaction mode of the robot, values can be stiff or compliant.
void run() override
Loop function.
bool getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
Read the instantaneous position of a motor encoder.
bool getEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous acceleration of all axes.
bool getEncoderSpeedsRaw(double *spds) override
Read the instantaneous acceleration of an axis.
virtual bool getTorqueControlFilterType(int j, int &type)
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.
bool uninitialize()
Clean up internal data and memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
class ImplementControlLimits; class StubImplControlLimitsRaw;
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool initialize(int k, const int *amap)
bool initialize(int size, const int *amap, const double *ampsToSens)
Initialize the internal data and alloc memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos, const double *nw)
Initialize the internal data and alloc memory.
Interface settings the way the robot interacts with the environment: basic interaction types are Stif...
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory, smaller version.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool initialize(int k, const int *amap, const double *dutyToPWM)
bool uninitialize()
Clean up internal data and memory.
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.
bool setConversionUnits(const PidControlTypeEnum &pidtype, const PidFeedbackUnitsEnum fbk_conv_units, const PidOutputUnitsEnum out_conv_units)
Default implementation of the IPositionControl interface.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
Default implementation of the IPositionDirect interface.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
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.
bool uninitialize()
Clean up internal data and memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool initialize(int k, const int *amap, const double *userToRaw)
Contains the parameters for a PID.
double offset
pwm offset added to the pid output
double kp
proportional gain
A mini-server for performing network communication in the background.
An abstraction for a periodic thread.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A base class for nested structures that can be searched.
Definition Searchable.h:31
void resize(size_t size) override
Resize the vector.
Definition Vector.h:221
void zero()
Zero the elements of the vector.
Definition Vector.h:344
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
std::string get_string(const std::string &key, bool *found=nullptr)
Read a string from an environment variable.
Definition environment.h:66
For streams capable of holding different kinds of content, check what they actually have.
PidControlTypeEnum
Definition PidEnums.h:15
@ VOCAB_PIDTYPE_TORQUE
Definition PidEnums.h:18
@ VOCAB_PIDTYPE_VELOCITY
Definition PidEnums.h:17
@ VOCAB_PIDTYPE_POSITION
Definition PidEnums.h:16
@ VOCAB_PIDTYPE_CURRENT
Definition PidEnums.h:19
@ VOCAB_JOINTTYPE_REVOLUTE
Definition IAxisInfo.h:24
@ VOCAB_JOINTTYPE_PRISMATIC
Definition IAxisInfo.h:25
@ VOCAB_JOINTTYPE_UNKNOWN
Definition IAxisInfo.h:26
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
std::string decode(NetInt32 code)
Convert a vocabulary identifier into a string.
Definition Vocab.cpp:33
The components from which ports and connections are built.
An interface to the operating system, including Port based communication.