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>
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 _cycleTimestamp = yarp::os::Time::now();
39
40 for (int i=0;i <_njoints ;i++)
41 {
42 if (_controlModes[i] == VOCAB_CM_VELOCITY)
43 {
44 //increment joint position
45 if (this->_command_speeds[i]!=0)
46 {
47 double elapsed = yarp::os::Time::now()-prev_time;
48 double increment = _command_speeds[i]*elapsed;
49 pos[i]+=increment;
50 }
51
52 //velocity watchdog
53 if (velocity_watchdog_enabled && yarp::os::Time::now()-last_velocity_command[i]>=VELOCITY_WATCHDOG)
54 {
55 this->_command_speeds[i]=0.0;
56 }
57 }
58 else if (_controlModes[i] == VOCAB_CM_PWM)
59 {
60 //increment joint position
61 if (this->refpwm[i]!=0)
62 {
63 double elapsed = yarp::os::Time::now()-prev_time;
64 double increment = refpwm[i]*elapsed*PWM_CONSTANT;
65 pos[i]+=increment;
66 }
67
68 //pwm watchdog
69 if (openloop_watchdog_enabled && yarp::os::Time::now()-last_pwm_command[i]>=OPENLOOP_WATCHDOG)
70 {
71 this->refpwm[i]=0.0;
72 }
73 }
74 else if (_controlModes[i] == VOCAB_CM_POSITION_DIRECT)
75 {
76 pos[i] = _posDir_references[i];
77 }
78 else if (_controlModes[i] == VOCAB_CM_POSITION)
79 {
80 pos[i] = _posCtrl_references[i];
81 //do something with _ref_speeds[i];
82 }
83 else if (_controlModes[i] == VOCAB_CM_IDLE)
84 {
85 //do nothing
86 }
87 else if (_controlModes[i] == VOCAB_CM_CURRENT)
88 {
89 //do nothing
90 }
91 else if (_controlModes[i] == VOCAB_CM_VELOCITY_DIRECT)
92 {
93 //not yet implemented
94 }
95 else if (_controlModes[i] == VOCAB_CM_MIXED)
96 {
97 //not yet implemented
98 }
99 else if (_controlModes[i] == VOCAB_CM_TORQUE)
100 {
101 //not yet implemented
102 }
103 else if (_controlModes[i] == VOCAB_CM_HW_FAULT)
104 {
105 //not yet implemented
106 }
107 else
108 {
109 //unsupported control mode
110 yCWarning(FAKEMOTIONCONTROL) << "Unsupported control mode, joint " << i << " " << yarp::os::Vocab32::decode(_controlModes[i]);
111 }
112 }
113 prev_time = yarp::os::Time::now();
114}
115
116// replace with to_string as soon as C++11 is required by YARP
121template<typename T>
122std::string toString(const T& value)
123{
124 std::ostringstream oss;
125 oss << value;
126 return oss.str();
127}
128
129//generic function that check is key1 is present in input bottle and that the result has size elements
130// return true/false
131/*
132bool FakeMotionControl::extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
133{
134 size++;
135 Bottle &tmp=input.findGroup(key1, txt);
136
137 if (tmp.isNull())
138 {
139 yCError(FAKEMOTIONCONTROL) << key1.c_str() << "parameter not found";
140 return false;
141 }
142
143 if(tmp.size()!=(size_t) size)
144 {
145 yCError(FAKEMOTIONCONTROL) << key1.c_str() << "incorrect number of entries";
146 return false;
147 }
148
149 out=tmp;
150 return true;
151}
152*/
153
155{
156 pos.resize(_njoints);
157 dpos.resize(_njoints);
158 vel.resize(_njoints);
159 speed.resize(_njoints);
160 acc.resize(_njoints);
161 loc.resize(_njoints);
162 amp.resize(_njoints);
163
164 current.resize(_njoints);
165 nominalCurrent.resize(_njoints);
166 maxCurrent.resize(_njoints);
167 peakCurrent.resize(_njoints);
168 pwm.resize(_njoints);
169 refpwm.resize(_njoints);
170 pwmLimit.resize(_njoints);
171 supplyVoltage.resize(_njoints);
172 last_velocity_command.resize(_njoints);
173 last_pwm_command.resize(_njoints);
174
175 pos.zero();
176 dpos.zero();
177 vel.zero();
178 speed.zero();
179 acc.zero();
180 loc.zero();
181 amp.zero();
182
183 current.zero();
184 nominalCurrent.zero();
185 maxCurrent.zero();
186 peakCurrent.zero();
187
188 pwm.zero();
189 refpwm.zero();
190 pwmLimit.zero();
191 supplyVoltage.zero();
192}
193
194
195void FakeMotionControl::setInfoPPids(int j)
196{
197 _ppids[j].at(0).pidExtraInfo.description.pid_description = "Position PID 0";
198 _ppids[j].at(0).pidExtraInfo.description.input_data_description = "Encoder Value";
199 _ppids[j].at(0).pidExtraInfo.description.output_data_description = "PWM";
200
201 _ppids[j].at(1).pidExtraInfo.description.pid_description = "Position PID 1";
202 _ppids[j].at(1).pidExtraInfo.description.input_data_description = "Encoder Value";
203 _ppids[j].at(1).pidExtraInfo.description.output_data_description = "Velocity";
204
205 _ppids[j].at(2).pidExtraInfo.description.pid_description = "Position PID 2";
206 _ppids[j].at(2).pidExtraInfo.description.input_data_description = "Encoder Value";
207 _ppids[j].at(2).pidExtraInfo.description.output_data_description = "Current";
208}
209
210void FakeMotionControl::setInfoPDPids(int j)
211{
212 _pdpids[j].at(0).pidExtraInfo.description.pid_description = "Position Direct PID 0";
213 _pdpids[j].at(0).pidExtraInfo.description.input_data_description = "Encoder Value";
214 _pdpids[j].at(0).pidExtraInfo.description.output_data_description = "PWM";
215
216 _pdpids[j].at(1).pidExtraInfo.description.pid_description = "Position Direct PID 1";
217 _pdpids[j].at(1).pidExtraInfo.description.input_data_description = "Encoder Value";
218 _pdpids[j].at(1).pidExtraInfo.description.output_data_description = "Velocity";
219
220 _pdpids[j].at(2).pidExtraInfo.description.pid_description = "Position Direct PID 2";
221 _pdpids[j].at(2).pidExtraInfo.description.input_data_description = "Encoder Value";
222 _pdpids[j].at(2).pidExtraInfo.description.output_data_description = "Current";
223}
224
225void FakeMotionControl::setInfoTPids(int j)
226{
227 _tpids[j].at(0).pidExtraInfo.description.pid_description = "Torque PID 0";
228 _tpids[j].at(0).pidExtraInfo.description.input_data_description = "Torque";
229 _tpids[j].at(0).pidExtraInfo.description.output_data_description = "PWM";
230
231 _tpids[j].at(1).pidExtraInfo.description.pid_description = "Torque PID 1";
232 _tpids[j].at(1).pidExtraInfo.description.input_data_description = "Torque";
233 _tpids[j].at(1).pidExtraInfo.description.output_data_description = "Velocity";
234
235 _tpids[j].at(2).pidExtraInfo.description.pid_description = "Torque PID 2";
236 _tpids[j].at(2).pidExtraInfo.description.input_data_description = "Torque";
237 _tpids[j].at(2).pidExtraInfo.description.output_data_description = "Current";
238}
239
240void FakeMotionControl::setInfoVPids(int j)
241{
242 _vpids[j].at(0).pidExtraInfo.description.pid_description = "Velocity PID 0";
243 _vpids[j].at(0).pidExtraInfo.description.input_data_description = "Velocity Value";
244 _vpids[j].at(0).pidExtraInfo.description.output_data_description = "PWM";
245
246 _vpids[j].at(1).pidExtraInfo.description.pid_description = "Velocity PID 1";
247 _vpids[j].at(1).pidExtraInfo.description.input_data_description = "Velocity Value";
248 _vpids[j].at(1).pidExtraInfo.description.output_data_description = "PWM";
249
250 _vpids[j].at(2).pidExtraInfo.description.pid_description = "Velocity PID 2";
251 _vpids[j].at(2).pidExtraInfo.description.input_data_description = "Velocity Value";
252 _vpids[j].at(2).pidExtraInfo.description.output_data_description = "Current";
253}
254
255void FakeMotionControl::setInfoVDPids(int j)
256{
257 _vdpids[j].at(0).pidExtraInfo.description.pid_description = "Velocity Direct PID 0";
258 _vdpids[j].at(0).pidExtraInfo.description.input_data_description = "Velocity Value";
259 _vdpids[j].at(0).pidExtraInfo.description.output_data_description = "PWM";
260
261 _vdpids[j].at(1).pidExtraInfo.description.pid_description = "Velocity Direct PID 1";
262 _vdpids[j].at(1).pidExtraInfo.description.input_data_description = "Velocity Value";
263 _vdpids[j].at(1).pidExtraInfo.description.output_data_description = "PWM";
264
265 _vdpids[j].at(2).pidExtraInfo.description.pid_description = "Velocity Direct PID 2";
266 _vdpids[j].at(2).pidExtraInfo.description.input_data_description = "Velocity Value";
267 _vdpids[j].at(2).pidExtraInfo.description.output_data_description = "Current";
268}
269
270void FakeMotionControl::setInfoCPids(int j)
271{
272 _cpids[j].at(0).pidExtraInfo.description.pid_description = "Current PID 0";
273 _cpids[j].at(0).pidExtraInfo.description.input_data_description = "Current Value";
274 _cpids[j].at(0).pidExtraInfo.description.output_data_description = "PWM";
275
276 _cpids[j].at(1).pidExtraInfo.description.pid_description = "Current PID 1";
277 _cpids[j].at(1).pidExtraInfo.description.input_data_description = "Current Value";
278 _cpids[j].at(1).pidExtraInfo.description.output_data_description = "PWM";
279
280 _cpids[j].at(2).pidExtraInfo.description.pid_description = "Current PID 2";
281 _cpids[j].at(2).pidExtraInfo.description.input_data_description = "Current Value";
282 _cpids[j].at(2).pidExtraInfo.description.output_data_description = "PWM";
283}
284
285void FakeMotionControl::setInfoMPids(int j)
286{
287 _mpids[j].at(0).pidExtraInfo.description.pid_description = "Mixed PID 0";
288 _mpids[j].at(0).pidExtraInfo.description.input_data_description = "Encoder Value";
289 _mpids[j].at(0).pidExtraInfo.description.output_data_description = "PWM";
290
291 _mpids[j].at(1).pidExtraInfo.description.pid_description = "Mixed PID 1";
292 _mpids[j].at(1).pidExtraInfo.description.input_data_description = "Encoder Value";
293 _mpids[j].at(1).pidExtraInfo.description.output_data_description = "PWM";
294
295 _mpids[j].at(2).pidExtraInfo.description.pid_description = "Current PID 2";
296 _mpids[j].at(2).pidExtraInfo.description.input_data_description = "Encoder Value";
297 _mpids[j].at(2).pidExtraInfo.description.output_data_description = "PWM";
298}
299
301{
302 _axisMap = allocAndCheck<int>(nj);
303 _controlModes = allocAndCheck<int>(nj);
305 _angleToEncoder = allocAndCheck<double>(nj);
306 _dutycycleToPWM = allocAndCheck<double>(nj);
307 _ampsToSensor = allocAndCheck<double>(nj);
308 _encodersStamp = allocAndCheck<double>(nj);
309 _DEPRECATED_encoderconversionoffset = allocAndCheck<float>(nj);
310 _DEPRECATED_encoderconversionfactor = allocAndCheck<float>(nj);
311 _jointEncoderRes = allocAndCheck<int>(nj);
312 _rotorEncoderRes = allocAndCheck<int>(nj);
313 _gearbox = allocAndCheck<double>(nj);
314 _torqueSensorId= allocAndCheck<int>(nj);
315 _torqueSensorChan= allocAndCheck<int>(nj);
316 _maxTorque=allocAndCheck<double>(nj);
317 _torques = allocAndCheck<double>(nj);
318 _minJntCmdVelocity = allocAndCheck<double>(nj);
319 _maxJntCmdVelocity = allocAndCheck<double>(nj);
320 _maxMotorVelocity = allocAndCheck<double>(nj);
321 _newtonsToSensor=allocAndCheck<double>(nj);
322 _hasHallSensor = allocAndCheck<bool>(nj);
323 _hasTempSensor = allocAndCheck<bool>(nj);
324 _hasRotorEncoder = allocAndCheck<bool>(nj);
325 _hasRotorEncoderIndex = allocAndCheck<bool>(nj);
326 _rotorIndexOffset = allocAndCheck<int>(nj);
327 _motorPoles = allocAndCheck<int>(nj);
328 _rotorlimits_max = allocAndCheck<double>(nj);
329 _rotorlimits_min = allocAndCheck<double>(nj);
330 _hwfault_code = allocAndCheck<int>(nj);
331 _hwfault_message = allocAndCheck<std::string>(nj);
332 _braked = allocAndCheck<bool>(nj);
333 _autobraked = allocAndCheck<bool>(nj);
334 _tempLimit = allocAndCheck<double>(nj);
335
336 _ppids = allocAndCheck<std::vector<PidWithExtraInfo>>(nj); for (int i = 0; i < nj; ++i) {_ppids[i].resize(npids); setInfoPPids(i);}
337 _tpids = allocAndCheck<std::vector<PidWithExtraInfo>>(nj); for (int i = 0; i < nj; ++i) {_tpids[i].resize(npids); setInfoTPids(i);}
338 _cpids = allocAndCheck<std::vector<PidWithExtraInfo>>(nj); for (int i = 0; i < nj; ++i) {_cpids[i].resize(npids); setInfoCPids(i);}
339 _vpids = allocAndCheck<std::vector<PidWithExtraInfo>>(nj); for (int i = 0; i < nj; ++i) {_vpids[i].resize(npids); setInfoVPids(i);}
340 _mpids = allocAndCheck<std::vector<PidWithExtraInfo>>(nj); for (int i = 0; i < nj; ++i) {_mpids[i].resize(npids); setInfoMPids(i);}
341 _pdpids = allocAndCheck<std::vector<PidWithExtraInfo>>(nj); for (int i = 0; i < nj; ++i) {_pdpids[i].resize(npids); setInfoPDPids(i);}
342 _vdpids = allocAndCheck<std::vector<PidWithExtraInfo>>(nj); for (int i = 0; i < nj; ++i) {_vdpids[i].resize(npids); setInfoVDPids(i);}
343
344 _ppids_ena = allocAndCheck<std::vector<bool>>(nj); for (int i = 0; i < nj; ++i) _ppids_ena[i].resize(npids);
345 _tpids_ena = allocAndCheck<std::vector<bool>>(nj); for (int i = 0; i < nj; ++i) _tpids_ena[i].resize(npids);
346 _cpids_ena = allocAndCheck<std::vector<bool>>(nj); for (int i = 0; i < nj; ++i) _cpids_ena[i].resize(npids);
347 _vpids_ena = allocAndCheck<std::vector<bool>>(nj); for (int i = 0; i < nj; ++i) _vpids_ena[i].resize(npids);
348 _mpids_ena = allocAndCheck<std::vector<bool>>(nj); for (int i = 0; i < nj; ++i) _mpids_ena[i].resize(npids);
349 _pdpids_ena = allocAndCheck<std::vector<bool>>(nj); for (int i = 0; i < nj; ++i) _pdpids_ena[i].resize(npids);
350 _vdpids_ena = allocAndCheck<std::vector<bool>>(nj); for (int i = 0; i < nj; ++i) _vdpids_ena[i].resize(npids);
351
352 _ppids_lim = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _ppids_lim[i].resize(npids);
353 _tpids_lim = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _tpids_lim[i].resize(npids);
354 _cpids_lim = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _cpids_lim[i].resize(npids);
355 _vpids_lim = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _vpids_lim[i].resize(npids);
356 _mpids_lim = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _mpids_lim[i].resize(npids);
357 _pdpids_lim = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _pdpids_lim[i].resize(npids);
358 _vdpids_lim = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _vdpids_lim[i].resize(npids);
359
360 _ppids_ref = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _ppids_ref[i].resize(npids);
361 _tpids_ref = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _tpids_ref[i].resize(npids);
362 _cpids_ref = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _cpids_ref[i].resize(npids);
363 _vpids_ref = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _vpids_ref[i].resize(npids);
364 _mpids_ref = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _mpids_ref[i].resize(npids);
365 _pdpids_ref = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _pdpids_ref[i].resize(npids);
366 _vdpids_ref = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _vdpids_ref[i].resize(npids);
367
368 _ppids_ffd = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _ppids_ffd[i].resize(npids);
369 _tpids_ffd = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _tpids_ffd[i].resize(npids);
370 _cpids_ffd = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _cpids_ffd[i].resize(npids);
371 _vpids_ffd = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _vpids_ffd[i].resize(npids);
372 _mpids_ffd = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _mpids_ffd[i].resize(npids);
373 _pdpids_ffd = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _pdpids_ffd[i].resize(npids);
374 _vdpids_ffd = allocAndCheck<std::vector<double>>(nj); for (int i = 0; i < nj; ++i) _vdpids_ffd[i].resize(npids);
376 for (int i = 0; i < nj; ++i)
377 {
378 _availableControlModes[i].push_back(yarp::dev::SelectableControlModeEnum::VOCAB_CM_IDLE);
379 _availableControlModes[i].push_back(yarp::dev::SelectableControlModeEnum::VOCAB_CM_TORQUE);
380 _availableControlModes[i].push_back(yarp::dev::SelectableControlModeEnum::VOCAB_CM_POSITION);
381 _availableControlModes[i].push_back(yarp::dev::SelectableControlModeEnum::VOCAB_CM_POSITION_DIRECT);
382 _availableControlModes[i].push_back(yarp::dev::SelectableControlModeEnum::VOCAB_CM_VELOCITY);
383 _availableControlModes[i].push_back(yarp::dev::SelectableControlModeEnum::VOCAB_CM_VELOCITY_DIRECT);
384 _availableControlModes[i].push_back(yarp::dev::SelectableControlModeEnum::VOCAB_CM_CURRENT);
385 _availableControlModes[i].push_back(yarp::dev::SelectableControlModeEnum::VOCAB_CM_PWM);
386 _availableControlModes[i].push_back(yarp::dev::SelectableControlModeEnum::VOCAB_CM_MIXED);
387 _availableControlModes[i].push_back(yarp::dev::SelectableControlModeEnum::VOCAB_CM_FORCE_IDLE);
388 }
390 for (int i = 0; i < nj; ++i)
391 {
404 _availablePids[i].push_back(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1);
405 _availablePids[i].push_back(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2);
406 _availablePids[i].push_back(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3);
413 }
414
415// _impedance_params=allocAndCheck<ImpedanceParameters>(nj);
416// _impedance_limits=allocAndCheck<ImpedanceLimits>(nj);
417 _axisName = new std::string[nj];
418 _jointType = new JointTypeEnum[nj];
419
420 _limitsMax=allocAndCheck<double>(nj);
421 _limitsMin=allocAndCheck<double>(nj);
422 _kinematic_mj=allocAndCheck<double>(16);
423// _currentLimits=allocAndCheck<MotorCurrentLimits>(nj);
424 _motorPwmLimits=allocAndCheck<double>(nj);
425 checking_motiondone=allocAndCheck<bool>(nj);
426
427 _velocityShifts=allocAndCheck<int>(nj);
428 _velocityTimeout=allocAndCheck<int>(nj);
429 _kbemf=allocAndCheck<double>(nj);
430 _ktau=allocAndCheck<double>(nj);
431 _kbemf_scale = allocAndCheck<int>(nj);
432 _ktau_scale = allocAndCheck<int>(nj);
433 _filterType=allocAndCheck<int>(nj);
434 _last_position_move_time=allocAndCheck<double>(nj);
435 _viscousPos=allocAndCheck<double>(nj);
436 _viscousNeg=allocAndCheck<double>(nj);
437 _coulombPos=allocAndCheck<double>(nj);
438 _coulombNeg=allocAndCheck<double>(nj);
439 _velocityThres=allocAndCheck<double>(nj);
440
441 // Reserve space for data stored locally. values are initialized to 0
442 _posCtrl_references = allocAndCheck<double>(nj);
443 _posDir_references = allocAndCheck<double>(nj);
444 _command_speeds = allocAndCheck<double>(nj);
445 _dir_vel_commands = allocAndCheck<double>(nj);
446 _ref_speeds = allocAndCheck<double>(nj);
447 _ref_accs = allocAndCheck<double>(nj);
448 _ref_torques = allocAndCheck<double>(nj);
449 _ref_currents = allocAndCheck<double>(nj);
450 _enabledAmp = allocAndCheck<bool>(nj);
451 _enabledPid = allocAndCheck<bool>(nj);
452 _calibrated = allocAndCheck<bool>(nj);
453// _cacheImpedance = allocAndCheck<eOmc_impedance_t>(nj);
454
455 _stiffness = allocAndCheck<double>(nj);
456 _damping = allocAndCheck<double>(nj);
457 _force_offset = allocAndCheck<double>(nj);
458
460
461 return true;
462}
463
464bool FakeMotionControl::dealloc()
465{
466 checkAndDestroy(_axisMap);
467 checkAndDestroy(_controlModes);
468 checkAndDestroy(_interactMode);
469 checkAndDestroy(_angleToEncoder);
470 checkAndDestroy(_ampsToSensor);
471 checkAndDestroy(_dutycycleToPWM);
472 checkAndDestroy(_encodersStamp);
473 checkAndDestroy(_DEPRECATED_encoderconversionoffset);
474 checkAndDestroy(_DEPRECATED_encoderconversionfactor);
475 checkAndDestroy(_jointEncoderRes);
476 checkAndDestroy(_rotorEncoderRes);
477// checkAndDestroy(_jointEncoderType);
478// checkAndDestroy(_rotorEncoderType);
479 checkAndDestroy(_gearbox);
480 checkAndDestroy(_torqueSensorId);
481 checkAndDestroy(_torqueSensorChan);
482 checkAndDestroy(_braked);
483 checkAndDestroy(_autobraked);
484 checkAndDestroy(_maxTorque);
485 checkAndDestroy(_minJntCmdVelocity);
486 checkAndDestroy(_maxJntCmdVelocity);
487 checkAndDestroy(_maxMotorVelocity);
488 checkAndDestroy(_newtonsToSensor);
489 checkAndDestroy(_tempLimit);
490
491 checkAndDestroy(_ppids);
492 checkAndDestroy(_tpids);
493 checkAndDestroy(_cpids);
494 checkAndDestroy(_vpids);
495 checkAndDestroy(_mpids);
496 checkAndDestroy(_pdpids);
497 checkAndDestroy(_vdpids);
498
499 checkAndDestroy(_ppids_ena);
500 checkAndDestroy(_tpids_ena);
501 checkAndDestroy(_cpids_ena);
502 checkAndDestroy(_vpids_ena);
503 checkAndDestroy(_mpids_ena);
504 checkAndDestroy(_pdpids_ena);
505 checkAndDestroy(_vdpids_ena);
506
507 checkAndDestroy(_ppids_lim);
508 checkAndDestroy(_tpids_lim);
509 checkAndDestroy(_cpids_lim);
510 checkAndDestroy(_vpids_lim);
511 checkAndDestroy(_mpids_lim);
512 checkAndDestroy(_pdpids_lim);
513 checkAndDestroy(_vdpids_lim);
514
515 checkAndDestroy(_ppids_ref);
516 checkAndDestroy(_tpids_ref);
517 checkAndDestroy(_cpids_ref);
518 checkAndDestroy(_vpids_ref);
519 checkAndDestroy(_mpids_ref);
520 checkAndDestroy(_pdpids_ref);
521 checkAndDestroy(_vdpids_ref);
522
523 checkAndDestroy(_ppids_ffd);
524 checkAndDestroy(_tpids_ffd);
525 checkAndDestroy(_cpids_ffd);
526 checkAndDestroy(_vpids_ffd);
527 checkAndDestroy(_mpids_ffd);
528 checkAndDestroy(_pdpids_ffd);
529 checkAndDestroy(_vdpids_ffd);
530
531// checkAndDestroy(_impedance_params);
532// checkAndDestroy(_impedance_limits);
533 checkAndDestroy(_limitsMax);
534 checkAndDestroy(_limitsMin);
535 checkAndDestroy(_kinematic_mj);
536// checkAndDestroy(_currentLimits);
537 checkAndDestroy(_motorPwmLimits);
538 checkAndDestroy(checking_motiondone);
539 checkAndDestroy(_velocityShifts);
540 checkAndDestroy(_velocityTimeout);
541 checkAndDestroy(_kbemf);
542 checkAndDestroy(_ktau);
543 checkAndDestroy(_kbemf_scale);
544 checkAndDestroy(_ktau_scale);
545 checkAndDestroy(_filterType);
546 checkAndDestroy(_viscousPos);
547 checkAndDestroy(_viscousNeg);
548 checkAndDestroy(_coulombPos);
549 checkAndDestroy(_coulombNeg);
550 checkAndDestroy(_velocityThres);
551 checkAndDestroy(_posCtrl_references);
552 checkAndDestroy(_posDir_references);
553 checkAndDestroy(_command_speeds);
554 checkAndDestroy(_dir_vel_commands);
555 checkAndDestroy(_ref_speeds);
556 checkAndDestroy(_ref_accs);
557 checkAndDestroy(_ref_torques);
558 checkAndDestroy(_ref_currents);
559 checkAndDestroy(_enabledAmp);
560 checkAndDestroy(_enabledPid);
561 checkAndDestroy(_calibrated);
562 checkAndDestroy(_hasHallSensor);
563 checkAndDestroy(_hasTempSensor);
564 checkAndDestroy(_hasRotorEncoder);
565 checkAndDestroy(_hasRotorEncoderIndex);
566 checkAndDestroy(_rotorIndexOffset);
567 checkAndDestroy(_motorPoles);
568 checkAndDestroy(_axisName);
569 checkAndDestroy(_jointType);
570 checkAndDestroy(_rotorlimits_max);
571 checkAndDestroy(_rotorlimits_min);
572 checkAndDestroy(_last_position_move_time);
573 checkAndDestroy(_torques);
574 checkAndDestroy(_hwfault_code);
575 checkAndDestroy(_hwfault_message);
576 checkAndDestroy(_stiffness);
577 checkAndDestroy(_damping);
578 checkAndDestroy(_force_offset);
579 checkAndDestroy(_availableControlModes);
580 checkAndDestroy(_availablePids);
581
582 return true;
583}
584
586 PeriodicThread(0.01),
608 _mutex(),
609 _njoints (0),
610 _axisMap (nullptr),
611 _angleToEncoder(nullptr),
612 _encodersStamp (nullptr),
613 _ampsToSensor (nullptr),
614 _dutycycleToPWM(nullptr),
615 _DEPRECATED_encoderconversionfactor (nullptr),
616 _DEPRECATED_encoderconversionoffset (nullptr),
617 _jointEncoderRes (nullptr),
618 _rotorEncoderRes (nullptr),
619 _gearbox (nullptr),
620 _hasHallSensor (nullptr),
621 _hasTempSensor (nullptr),
622 _hasRotorEncoder (nullptr),
623 _hasRotorEncoderIndex (nullptr),
624 _rotorIndexOffset (nullptr),
625 _motorPoles (nullptr),
626 _rotorlimits_max (nullptr),
627 _rotorlimits_min (nullptr),
628 _axisName (nullptr),
629 _jointType (nullptr),
630 _limitsMin (nullptr),
631 _limitsMax (nullptr),
632 _kinematic_mj (nullptr),
633 _maxMotorVelocity (nullptr),
634 _velocityShifts (nullptr),
635 _velocityTimeout (nullptr),
636 _kbemf (nullptr),
637 _ktau (nullptr),
638 _kbemf_scale (nullptr),
639 _ktau_scale (nullptr),
640 _viscousPos (nullptr),
641 _viscousNeg (nullptr),
642 _coulombPos (nullptr),
643 _coulombNeg (nullptr),
644 _velocityThres (nullptr),
645 _filterType (nullptr),
646 _torqueSensorId (nullptr),
647 _torqueSensorChan (nullptr),
648 _maxTorque (nullptr),
649 _newtonsToSensor (nullptr),
650 checking_motiondone (nullptr),
651 _last_position_move_time(nullptr),
652 _motorPwmLimits (nullptr),
653 _torques (nullptr),
654 useRawEncoderData (false),
655 _pwmIsLimited (false),
656 _torqueControlEnabled (false),
657 _torqueControlUnits (T_MACHINE_UNITS),
658 _positionControlUnits (P_MACHINE_UNITS),
659 prev_time (0.0),
660 opened (false),
661 verbose (VERY_VERBOSE)
662{
664 std::string tmp = yarp::conf::environment::get_string("VERBOSE_STICA");
665 verbosewhenok = (tmp != "") ? (bool)yarp::conf::numeric::from_string<int>(tmp) :
666 false;
667}
668
674
676{
677 return opened;
678}
679
681{
683 for(int i=0; i<_njoints; i++)
684 {
685 pwm[i] = 33+i;
686 pwmLimit[i] = (33+i)*10;
687 current[i] = (33+i)*100;
688 maxCurrent[i] = (33+i)*1000;
689 peakCurrent[i] = (33+i)*2;
690 nominalCurrent[i] = (33+i)*20;
691 supplyVoltage[i] = (33+i)*200;
692 last_velocity_command[i] = -1;
693 last_pwm_command[i] = -1;
694 _controlModes[i] = VOCAB_CM_POSITION;
695 _minJntCmdVelocity[i]=-50.0;
696 _maxJntCmdVelocity[i]=50.0;
697 }
698 prev_time = yarp::os::Time::now();
699 return true;
700}
701
705
707{
708 if (!this->parseParams(config)) {return false;}
709
710 std::string str;
711
712 //
713 // Read Configuration params from file
714 //
715 _njoints = m_GENERAL_Joints;
716
717 if(!alloc(_njoints))
718 {
719 yCError(FAKEMOTIONCONTROL) << "Malloc failed";
720 return false;
721 }
722
723 // Default value
724 for (int i = 0; i < _njoints; i++) {
725 _newtonsToSensor[i] = 1;
726 }
727
728 if(!fromConfig(config))
729 {
730 yCError(FAKEMOTIONCONTROL) << "Missing parameters in config file";
731 return false;
732 }
733
734 // INIT ALL INTERFACES
735 yarp::sig::Vector tmpZeros; tmpZeros.resize (_njoints, 0.0);
736 yarp::sig::Vector tmpOnes; tmpOnes.resize (_njoints, 1.0);
737 yarp::sig::Vector bemfToRaw; bemfToRaw.resize(_njoints, 1.0);
738 yarp::sig::Vector ktauToRaw; ktauToRaw.resize(_njoints, 1.0);
739 for (size_t i = 0; i < _njoints; i++) { bemfToRaw [i] = _newtonsToSensor[i] / _angleToEncoder[i];}
740 for (size_t i = 0; i < _njoints; i++) { ktauToRaw[i] = _dutycycleToPWM[i] / _newtonsToSensor[i]; }
741 ControlBoardHelper cb(_njoints, _axisMap, _angleToEncoder, nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM);
743 ImplementControlCalibration::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
744 ImplementAmplifierControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
745 ImplementEncodersTimed::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
746 ImplementMotorEncoders::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
747 ImplementPositionControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
748 ImplementPidControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM);
749 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
750 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
751 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
752 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
753 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
754 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
755 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
756 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
757 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
758 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
759 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
760 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
761 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
762 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
763 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
764 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
765 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
766 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
767 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
768 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
769 ImplementPidControl::setConversionUnits(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3, PidFeedbackUnitsEnum::METRIC, PidOutputUnitsEnum::DUTYCYCLE_PWM_PERCENT);
770 ImplementControlMode::initialize(_njoints, _axisMap);
771 ImplementVelocityControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
772 ImplementVelocityDirect::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
773 ImplementControlLimits::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
774 ImplementImpedanceControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr, _newtonsToSensor);
775 ImplementTorqueControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM, bemfToRaw.data(), ktauToRaw.data());
776 ImplementPositionDirect::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
777 ImplementInteractionMode::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
778 ImplementMotor::initialize(_njoints, _axisMap);
779 ImplementAxisInfo::initialize(_njoints, _axisMap);
780 ImplementPWMControl::initialize(_njoints, _axisMap, _dutycycleToPWM);
781 ImplementCurrentControl::initialize(_njoints, _axisMap, _ampsToSensor);
782 ImplementVirtualAnalogSensor::initialize(_njoints, _axisMap, _newtonsToSensor);
783 ImplementJointFault::initialize(_njoints, _axisMap);
784 ImplementJointBrake::initialize(_njoints, _axisMap);
785
786 //start the rateThread
787 bool init = this->start();
788 if(!init)
789 {
790 yCError(FAKEMOTIONCONTROL) << "open() has an error in call of FakeMotionControl::init() for board" ;
791 return false;
792 }
793 else
794 {
795 if(verbosewhenok)
796 {
797 yCDebug(FAKEMOTIONCONTROL) << "init() has successfully initted board ";
798 }
799 }
800 opened = true;
801
802 return true;
803}
804
806{
807 size_t i;
808
809 // AxisMap
810 if (!m_GENERAL_AxisMap.empty())
811 {
812 for (i = 0; i < m_GENERAL_AxisMap.size(); i++) {
813 _axisMap[i] = m_GENERAL_AxisMap[i];
814 }
815 }
816 else
817 {
818 for (i = 0; i < _njoints; i++) {
819 _axisMap[i] = i;
820 }
821 }
822 for (i = 0; i < _njoints; i++) {yDebug() << "_axisMap: " << _axisMap[i] << " "; }
823
824 // AxisName
825 if (!m_GENERAL_AxisName.empty())
826 {
827 for (i = 0; i < m_GENERAL_AxisName.size(); i++) {
828 _axisName[_axisMap[i]] = m_GENERAL_AxisName[i];
829 }
830 }
831 else
832 {
833 for (i = 0; i < _njoints; i++) {
834 _axisName[_axisMap[i]] = "joint" + toString(i);
835 }
836 }
837 for (i = 0; i < _njoints; i++) { yDebug() << "_axisName: " << _axisName[_axisMap[i]] << " "; }
838
839 // Axis Type
840 if (!m_GENERAL_AxisType.empty())
841 {
842 //beware: axis type has to be remapped here because they are not set using the toHw() helper function
843 for (i = 0; i < m_GENERAL_AxisType.size(); i++)
844 {
845 std::string typeString = m_GENERAL_AxisType[i];
846 if (typeString == "revolute") {
847 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_REVOLUTE;
848 }
849 else if (typeString == "prismatic") {
850 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_PRISMATIC;
851 }
852 else {
853 yCError(FAKEMOTIONCONTROL, "Unknown AxisType value %s!", typeString.c_str());
854 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_UNKNOWN;
855 return false;
856 }
857 }
858 }
859 else
860 {
861 yCInfo(FAKEMOTIONCONTROL) << "Using default AxisType (revolute)";
862 for (i = 0; i < _njoints; i++) {
863 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_REVOLUTE;
864 }
865 }
866
867 // current conversions factor
868 if (!m_GENERAL_ampsToSensor.empty())
869 {
870 for (i = 0; i < m_GENERAL_ampsToSensor.size(); i++) {
871 _ampsToSensor[i] = m_GENERAL_ampsToSensor[i];
872 }
873 }
874 else
875 {
876 yCInfo(FAKEMOTIONCONTROL) << "Using default ampsToSensor=1";
877 for (i = 0; i < _njoints; i++) {
878 _ampsToSensor[i] = 1;
879 }
880 }
881
882
883 // pwm conversions factor
884 if (!m_GENERAL_fullscalePWM.empty())
885 {
886 for (i = 0; i < m_GENERAL_fullscalePWM.size(); i++) {
887 _dutycycleToPWM[i] = m_GENERAL_fullscalePWM[i]/100;
888 }
889 }
890 else
891 {
892 yCInfo(FAKEMOTIONCONTROL) << "Using default fullscalePWM=1";
893 for (i = 0; i < _njoints; i++) {
894 _dutycycleToPWM[i] = 1;
895 }
896 }
897
898// double tmp_A2E;
899 // Encoder scales
900 if(!m_GENERAL_Encoder.empty())
901 {
902 for (i = 0; i < m_GENERAL_Encoder.size(); i++) {
903 _angleToEncoder[i] = m_GENERAL_Encoder[i]; }
904 }
905 else
906 {
907 yCInfo(FAKEMOTIONCONTROL) << "Using default Encoder=1";
908 for (i = 0; i < _njoints; i++) {
909 _angleToEncoder[i] = 1; }
910 }
911
913 if (!m_LIMITS_Max.empty())
914 {
915 for (i = 0; i < m_LIMITS_Max.size(); i++) {
916 _limitsMax[i] = m_LIMITS_Max[i];
917 }
918 }
919 else
920 {
921 yCInfo(FAKEMOTIONCONTROL) << "Using default m_LIMITS_Max=100";
922 for (i = 0; i < _njoints; i++) {
923 _limitsMax[i] = 100;
924 }
925 }
926
927 if (!m_LIMITS_Min.empty())
928 {
929 for (i = 0; i < m_LIMITS_Min.size(); i++) {
930 _limitsMin[i] = m_LIMITS_Min[i];
931 }
932 }
933 else
934 {
935 yCInfo(FAKEMOTIONCONTROL) << "Using default m_LIMITS_Min=0";
936 for (i = 0; i < _njoints; i++) {
937 _limitsMin[i] = 0;
938 }
939 }
940
941 return true;
942}
943
944
976
977void FakeMotionControl::cleanup()
978{
979
980}
981
982
983
985ReturnValue FakeMotionControl::getAvailablePidsRaw(int j, std::vector<yarp::dev::PidControlTypeEnum>& avail)
986{
987 if (verbose > VERY_VERY_VERBOSE) {
988 yCTrace(FAKEMOTIONCONTROL) << "j: " << j;
989 }
990
991 avail = _availablePids[j];
992 return ReturnValue_ok;
993}
994
996{
997 yCDebug(FAKEMOTIONCONTROL) << "setPidRaw" << (yarp::conf::vocab32_t)(pidtype) << jnt << pid.kp;
998 size_t index = PidControlTypeEnum2Index(pidtype);
999 switch (pidtype)
1000 {
1001 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1002 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1003 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1004 _ppids[jnt].at(index)=pid;
1005 break;
1006 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1007 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1008 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1009 _pdpids[jnt].at(index)=pid;
1010 break;
1011 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1012 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1013 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1014 _vpids[jnt].at(index)=pid;
1015 break;
1016 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1017 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1018 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1019 _vdpids[jnt].at(index)=pid;
1020 break;
1021 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1022 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1023 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1024 _mpids[jnt].at(index)=pid;
1025 break;
1026 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1027 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1028 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1029 _cpids[jnt].at(index)=pid;
1030 break;
1031 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1032 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1033 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1034 _tpids[jnt].at(index)=pid;
1035 break;
1036 default:
1037 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1038 return ReturnValue::return_code::return_value_error_method_failed;
1039 break;
1040 }
1041 return ReturnValue_ok;
1042}
1043
1045{
1047 for(int j=0; j< _njoints; j++)
1048 {
1049 ret &= setPidRaw(pidtype, j, pids[j]);
1050 }
1051 return ret;
1052}
1053
1055{
1056 size_t index = PidControlTypeEnum2Index(pidtype);
1057 switch (pidtype)
1058 {
1059 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1060 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1061 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1062 _ppids_ref[j].at(index)=ref;
1063 break;
1064 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1065 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1066 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1067 _pdpids_ref[j].at(index)=ref;
1068 break;
1069 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1070 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1071 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1072 _vpids_ref[j].at(index)=ref;
1073 break;
1074 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1075 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1076 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1077 _vdpids_ref[j].at(index)=ref;
1078 break;
1079 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1080 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1081 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1082 _mpids_ref[j].at(index)=ref;
1083 break;
1084 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1085 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1086 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1087 _cpids_ref[j].at(index)=ref;
1088 break;
1089 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1090 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1091 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1092 _tpids_ref[j].at(index)=ref;
1093 break;
1094 default:
1095 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1096 return ReturnValue::return_code::return_value_error_method_failed;
1097 break;
1098 }
1099 return ReturnValue_ok;
1100}
1101
1103{
1105 for(int j=0, index=0; j< _njoints; j++, index++)
1106 {
1107 ret &= setPidReferenceRaw(pidtype, j, refs[index]);
1108 }
1109 return ret;
1110}
1111
1113{
1114 size_t index = PidControlTypeEnum2Index(pidtype);
1115 switch (pidtype)
1116 {
1117 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1118 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1119 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1120 _ppids_lim[j].at(index)=limit;
1121 break;
1122 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1123 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1124 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1125 _pdpids_lim[j].at(index)=limit;
1126 break;
1127 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1128 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1129 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1130 _vpids_lim[j].at(index)=limit;
1131 break;
1132 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1133 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1134 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1135 _vdpids_lim[j].at(index)=limit;
1136 break;
1137 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1138 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1139 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1140 _mpids_lim[j].at(index)=limit;
1141 break;
1142 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1143 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1144 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1145 _cpids_lim[j].at(index)=limit;
1146 break;
1147 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1148 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1149 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1150 _tpids_lim[j].at(index)=limit;
1151 break;
1152 default:
1153 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1154 return ReturnValue::return_code::return_value_error_method_failed;
1155 break;
1156 }
1157 return ReturnValue_ok;
1158}
1159
1161{
1163 for(int j=0, index=0; j< _njoints; j++, index++)
1164 {
1165 ret &= setPidErrorLimitRaw(pidtype, j, limits[index]);
1166 }
1167 return ret;
1168}
1169
1171{
1172 switch (pidtype)
1173 {
1174 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1175 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1176 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1177 *err=0.1;
1178 break;
1179 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1180 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1181 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1182 *err=0.1;
1183 break;
1184 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1185 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1186 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1187 *err=0.2;
1188 break;
1189 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1190 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1191 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1192 *err=0.2;
1193 break;
1194 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1195 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1196 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1197 *err=0.2;
1198 break;
1199 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1200 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1201 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1202 *err=0.3;
1203 break;
1204 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1205 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1206 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1207 *err=0.4;
1208 break;
1209 default:
1210 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1211 return ReturnValue::return_code::return_value_error_method_failed;
1212 break;
1213 }
1214 return ReturnValue_ok;
1215}
1216
1218{
1220 for(int j=0; j< _njoints; j++)
1221 {
1222 ret &= getPidErrorRaw(pidtype, j, &errs[j]);
1223 }
1224 return ret;
1225}
1226
1228{
1229 size_t index = PidControlTypeEnum2Index(pidtype);
1230 switch (pidtype)
1231 {
1232 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1233 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1234 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1235 *pid=_ppids[jnt].at(index).pid;
1236 break;
1237 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1238 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1239 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1240 *pid=_pdpids[jnt].at(index).pid;
1241 break;
1242 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1243 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1244 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1245 *pid=_vpids[jnt].at(index).pid;
1246 break;
1247 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1248 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1249 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1250 *pid=_vdpids[jnt].at(index).pid;
1251 break;
1252 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1253 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1254 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1255 *pid=_mpids[jnt].at(index).pid;
1256 break;
1257 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1258 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1259 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1260 *pid=_cpids[jnt].at(index).pid;
1261 break;
1262 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1263 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1264 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1265 *pid=_tpids[jnt].at(index).pid;
1266 break;
1267 default:
1268 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1269 return ReturnValue::return_code::return_value_error_method_failed;
1270 break;
1271 }
1272 yCDebug(FAKEMOTIONCONTROL) << "getPidRaw" << (yarp::conf::vocab32_t)(pidtype) << jnt << pid->kp;
1273 return ReturnValue_ok;
1274}
1275
1277{
1279
1280 for(int j=0, index=0; j<_njoints; j++, index++)
1281 {
1282 ret &=getPidRaw(pidtype, j, &pids[j]);
1283 }
1284 return ret;
1285}
1286
1288{
1289 size_t index = PidControlTypeEnum2Index(pidtype);
1290 switch (pidtype)
1291 {
1292 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1293 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1294 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1295 *ref=_ppids_ref[j].at(index);
1296 break;
1297 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1298 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1299 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1300 *ref=_pdpids_ref[j].at(index);
1301 break;
1302 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1303 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1304 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1305 *ref=_vpids_ref[j].at(index);
1306 break;
1307 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1308 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1309 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1310 *ref=_vdpids_ref[j].at(index);
1311 break;
1312 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1313 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1314 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1315 *ref=_mpids_ref[j].at(index);
1316 break;
1317 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1318 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1319 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1320 *ref=_cpids_ref[j].at(index);
1321 break;
1322 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1323 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1324 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1325 *ref=_tpids_ref[j].at(index);
1326 break;
1327 default:
1328 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1329 return ReturnValue::return_code::return_value_error_method_failed;
1330 break;
1331 }
1332 return ReturnValue_ok;
1333}
1334
1336{
1338
1339 // just one joint at time, wait answer before getting to the next.
1340 // This is because otherwise too many msg will be placed into can queue
1341 for(int j=0; j< _njoints; j++)
1342 {
1343 ret &= getPidReferenceRaw(pidtype, j, &refs[j]);
1344 }
1345 return ret;
1346}
1347
1349{
1350 size_t index = PidControlTypeEnum2Index(pidtype);
1351 switch (pidtype)
1352 {
1353 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1354 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1355 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1356 *limit=_ppids_lim[j].at(index);
1357 break;
1358 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1359 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1360 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1361 *limit=_pdpids_lim[j].at(index);
1362 break;
1363 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1364 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1365 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1366 *limit=_vpids_lim[j].at(index);
1367 break;
1368 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1369 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1370 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1371 *limit=_vdpids_lim[j].at(index);
1372 break;
1373 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1374 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1375 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1376 *limit=_mpids_lim[j].at(index);
1377 break;
1378 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1379 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1380 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1381 *limit=_cpids_lim[j].at(index);
1382 break;
1383 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1384 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1385 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1386 *limit=_tpids_lim[j].at(index);
1387 break;
1388 default:
1389 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1390 return ReturnValue::return_code::return_value_error_method_failed;
1391 break;
1392 }
1393 return ReturnValue_ok;
1394}
1395
1397{
1399 for(int j=0, index=0; j<_njoints; j++, index++)
1400 {
1401 ret &=getPidErrorLimitRaw(pidtype, j, &limits[j]);
1402 }
1403 return ret;
1404}
1405
1407{
1408 return ReturnValue_ok;
1409}
1410
1412{
1413 size_t index = PidControlTypeEnum2Index(pidtype);
1414 switch (pidtype)
1415 {
1416 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1417 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1418 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1419 _ppids_ena[j].at(index)=false;
1420 break;
1421 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1422 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1423 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1424 _pdpids_ena[j].at(index)=false;
1425 break;
1426 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1427 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1428 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1429 _vpids_ena[j].at(index)=false;
1430 break;
1431 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1432 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1433 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1434 _vdpids_ena[j].at(index)=false;
1435 break;
1436 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1437 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1438 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1439 _mpids_ena[j].at(index)=false;
1440 break;
1441 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1442 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1443 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1444 _cpids_ena[j].at(index)=false;
1445 break;
1446 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1447 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1448 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1449 _tpids_ena[j].at(index)=false;
1450 break;
1451 default:
1452 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1453 return ReturnValue::return_code::return_value_error_method_failed;
1454 break;
1455 }
1456 return ReturnValue_ok;
1457}
1458
1460{
1461 size_t index = PidControlTypeEnum2Index(pidtype);
1462 switch (pidtype)
1463 {
1464 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1465 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1466 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1467 _ppids_ena[j].at(index)=true;
1468 break;
1469 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1470 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1471 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1472 _pdpids_ena[j].at(index)=true;
1473 break;
1474 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1475 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1476 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1477 _vpids_ena[j].at(index)=true;
1478 break;
1479 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1480 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1481 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1482 _vdpids_ena[j].at(index)=true;
1483 break;
1484 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1485 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1486 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1487 _mpids_ena[j].at(index)=true;
1488 break;
1489 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1490 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1491 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1492 _cpids_ena[j].at(index)=true;
1493 break;
1494 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1495 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1496 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1497 _tpids_ena[j].at(index)=true;
1498 break;
1499 default:
1500 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1501 return ReturnValue::return_code::return_value_error_method_failed;
1502 break;
1503 }
1504 return ReturnValue_ok;
1505}
1506
1508{
1509 yCDebug(FAKEMOTIONCONTROL) << "setPidOffsetRaw" << (yarp::conf::vocab32_t)(pidtype) << jnt << v;
1510 size_t index = PidControlTypeEnum2Index(pidtype);
1511 switch (pidtype)
1512 {
1513 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1514 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1515 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1516 _ppids[jnt].at(index).pid.offset=v;
1517 break;
1518 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1519 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1520 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1521 _pdpids[jnt].at(index).pid.offset=v;
1522 break;
1523 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1524 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1525 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1526 _vpids[jnt].at(index).pid.offset=v;
1527 break;
1528 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1529 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1530 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1531 _vdpids[jnt].at(index).pid.offset=v;
1532 break;
1533 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1534 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1535 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1536 _mpids[jnt].at(index).pid.offset=v;
1537 break;
1538 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1539 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1540 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1541 _cpids[jnt].at(index).pid.offset=v;
1542 break;
1543 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1544 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1545 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1546 _tpids[jnt].at(index).pid.offset=v;
1547 break;
1548 default:
1549 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1550 return ReturnValue::return_code::return_value_error_method_failed;
1551 break;
1552 }
1553 return ReturnValue_ok;
1554}
1555
1557{
1558 size_t index = PidControlTypeEnum2Index(pidtype);
1559 switch (pidtype)
1560 {
1561 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1562 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1563 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1564 v = _ppids[jnt].at(index).pid.offset;
1565 break;
1566 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1567 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1568 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1569 v = _pdpids[jnt].at(index).pid.offset;
1570 break;
1571 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1572 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1573 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1574 v = _vpids[jnt].at(index).pid.offset;
1575 break;
1576 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1577 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1578 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1579 v = _vdpids[jnt].at(index).pid.offset;
1580 break;
1581 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1582 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1583 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1584 v = _mpids[jnt].at(index).pid.offset;
1585 break;
1586 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1587 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1588 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1589 v = _cpids[jnt].at(index).pid.offset;
1590 break;
1591 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1592 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1593 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1594 v = _tpids[jnt].at(index).pid.offset;
1595 break;
1596 default:
1597 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1598 return ReturnValue::return_code::return_value_error_method_failed;
1599 break;
1600 }
1601 yCDebug(FAKEMOTIONCONTROL) << "getPidOffsetRaw" << (yarp::conf::vocab32_t)(pidtype) << jnt << v;
1602 return ReturnValue_ok;
1603}
1604
1606{
1607 yCDebug(FAKEMOTIONCONTROL) << "setPidFeedforwardRaw" << (yarp::conf::vocab32_t)(pidtype) << jnt << v;
1608 size_t index = PidControlTypeEnum2Index(pidtype);
1609 switch (pidtype)
1610 {
1611 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1612 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1613 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1614 _ppids_ffd[jnt].at(index)=v;
1615 break;
1616 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1617 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1618 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1619 _pdpids_ffd[jnt].at(index)=v;
1620 break;
1621 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1622 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1623 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1624 _vpids_ffd[jnt].at(index)=v;
1625 break;
1626 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1627 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1628 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1629 _vdpids_ffd[jnt].at(index)=v;
1630 break;
1631 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1632 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1633 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1634 _mpids_ffd[jnt].at(index)=v;
1635 break;
1636 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1637 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1638 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1639 _cpids_ffd[jnt].at(index)=v;
1640 break;
1641 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1642 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1643 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1644 _tpids_ffd[jnt].at(index)=v;
1645 break;
1646 default:
1647 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1648 return ReturnValue::return_code::return_value_error_method_failed;
1649 break;
1650 }
1651 return ReturnValue_ok;
1652}
1653
1655{
1656 size_t index = PidControlTypeEnum2Index(pidtype);
1657 switch (pidtype)
1658 {
1659 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1660 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1661 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1662 v = _ppids_ffd[jnt].at(index);
1663 break;
1664 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1665 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1666 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1667 v = _pdpids_ffd[jnt].at(index);
1668 break;
1669 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1670 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1671 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1672 v = _vpids_ffd[jnt].at(index);
1673 break;
1674 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1675 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1676 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1677 v = _vdpids_ffd[jnt].at(index);
1678 break;
1679 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1680 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1681 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1682 v = _mpids_ffd[jnt].at(index);
1683 break;
1684 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1685 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1686 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1687 v = _cpids_ffd[jnt].at(index);
1688 break;
1689 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1690 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1691 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1692 v = _tpids_ffd[jnt].at(index);
1693 break;
1694 default:
1695 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1696 return ReturnValue::return_code::return_value_error_method_failed;
1697 break;
1698 }
1699 yCDebug(FAKEMOTIONCONTROL) << "getPidFeedforwardRaw" << (yarp::conf::vocab32_t)(pidtype) << jnt << v;
1700 return ReturnValue_ok;
1701}
1702
1704{
1705 size_t index = PidControlTypeEnum2Index(pidtype);
1706 switch (pidtype)
1707 {
1708 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1709 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1710 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1711 enabled=_ppids_ena[j].at(index);
1712 break;
1713 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1714 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1715 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1716 enabled=_pdpids_ena[j].at(index);
1717 break;
1718 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1719 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1720 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1721 enabled=_vpids_ena[j].at(index);
1722 break;
1723 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1724 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1725 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1726 enabled=_vdpids_ena[j].at(index);
1727 break;
1728 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1729 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1730 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1731 enabled=_mpids_ena[j].at(index);
1732 break;
1733 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1734 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1735 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1736 enabled=_cpids_ena[j].at(index);
1737 break;
1738 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1739 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1740 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1741 enabled=_tpids_ena[j].at(index);
1742 break;
1743 default:
1744 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1745 return ReturnValue::return_code::return_value_error_method_failed;
1746 break;
1747 }
1748 return ReturnValue_ok;
1749}
1750
1752{
1753 // The fake device has no real control loop: the PID output is simulated
1754 // as the PID offset, so that tests can set a value (setPidOffset) and read it back.
1755 ReturnValue ret = getPidOffsetRaw(pidtype, j, *out);
1756 if (!ret) {
1757 return ret;
1758 }
1759 yCDebug(FAKEMOTIONCONTROL) << "getPidOutputRaw" << (yarp::conf::vocab32_t)(pidtype) << j << *out;
1760 return ReturnValue_ok;
1761}
1762
1764{
1766 for(int j=0; j< _njoints; j++)
1767 {
1768 ret &= getPidOutputRaw(pidtype, j, &outs[j]);
1769 }
1770 return ret;
1771}
1772
1774// Velocity control interface raw //
1776
1778{
1780 auto ret = getControlModeRaw(j, mode);
1785 {
1786 yCError(FAKEMOTIONCONTROL) << "velocityMoveRaw: skipping command because board " << " joint " << j << " is not in VOCAB_CM_VELOCITY mode";
1787 }
1788 _command_speeds[j] = sp;
1789 last_velocity_command[j]=yarp::os::Time::now();
1790 return ReturnValue_ok;
1791}
1792
1794{
1797 for (int i = 0; i < _njoints; i++) {
1798 ret &= velocityMoveRaw(i, sp[i]);
1799 }
1800 return ret;
1801}
1802
1803
1805// Calibration control interface //
1807
1809{
1810 yCTrace(FAKEMOTIONCONTROL) << "setCalibrationParametersRaw for joint" << j;
1811 return ReturnValue_ok;
1812}
1813
1814ReturnValue FakeMotionControl::calibrateAxisWithParamsRaw(int j, unsigned int type, double p1, double p2, double p3)
1815{
1816 yCTrace(FAKEMOTIONCONTROL) << "calibrateRaw for joint" << j;
1817 return ReturnValue_ok;
1818}
1819
1824
1826// Position control interface //
1828
1830{
1831 ax=_njoints;
1832
1833 return ReturnValue_ok;
1834}
1835
1837{
1838 if (verbose >= VERY_VERBOSE) {
1839 yCTrace(FAKEMOTIONCONTROL) << "j " << j << " ref " << ref;
1840 }
1841
1842// if (yarp::os::Time::now()-_last_position_move_time[j]<MAX_POSITION_MOVE_INTERVAL)
1843// {
1844// 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.";
1845// }
1846// _last_position_move_time[j] = yarp::os::Time::now();
1847
1849 auto ret = getControlModeRaw(j, mode);
1854 {
1855 yCError(FAKEMOTIONCONTROL) << "positionMoveRaw: skipping command because joint " << j << " is not in VOCAB_CM_POSITION mode";
1856 }
1857 _posCtrl_references[j] = ref;
1858 return ReturnValue_ok;
1859}
1860
1862{
1863 if (verbose >= VERY_VERBOSE) {
1865 }
1866
1868 for(int j=0, index=0; j< _njoints; j++, index++)
1869 {
1870 ret &= positionMoveRaw(j, refs[index]);
1871 }
1872 return ret;
1873}
1874
1876{
1877 if (verbose >= VERY_VERBOSE) {
1878 yCTrace(FAKEMOTIONCONTROL) << "j " << j << " ref " << delta;
1879 }
1880// if (yarp::os::Time::now()-_last_position_move_time[j]<MAX_POSITION_MOVE_INTERVAL)
1881// {
1882// 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.";
1883// }
1884// _last_position_move_time[j] = yarp::os::Time::now();
1885
1887 auto ret = getControlModeRaw(j, mode);
1892 {
1893 yCError(FAKEMOTIONCONTROL) << "relativeMoveRaw: skipping command because joint " << j << " is not in VOCAB_CM_POSITION mode";
1894 }
1895 _posCtrl_references[j] += delta;
1896 return ReturnValue_ok;
1897}
1898
1900{
1901 if (verbose >= VERY_VERBOSE) {
1903 }
1904
1906 for(int j=0, index=0; j< _njoints; j++, index++)
1907 {
1908 ret &= relativeMoveRaw(j, deltas[index]);
1909 }
1910 return ret;
1911}
1912
1913
1915{
1916 if (verbose >= VERY_VERBOSE) {
1917 yCTrace(FAKEMOTIONCONTROL) << "j ";
1918 }
1919
1920 flag = false;
1921 return ReturnValue_ok;
1922}
1923
1925{
1926 if (verbose >= VERY_VERBOSE) {
1928 }
1929
1931 bool val, tot_res = true;
1932
1933 for(int j=0, index=0; j< _njoints; j++, index++)
1934 {
1935 ret &= checkMotionDoneRaw(j, val);
1936 tot_res &= val;
1937 }
1938 flag = tot_res;
1939 return ret;
1940}
1941
1943{
1944 // Velocity is expressed in iDegrees/s
1945 // save internally the new value of speed; it'll be used in the positionMove
1946 int index = j ;
1947 _ref_speeds[index] = sp;
1948 return ReturnValue_ok;
1949}
1950
1952{
1953 // Velocity is expressed in iDegrees/s
1954 // save internally the new value of speed; it'll be used in the positionMove
1955 for(int j=0, index=0; j< _njoints; j++, index++)
1956 {
1957 _ref_speeds[index] = spds[index];
1958 }
1959 return ReturnValue_ok;
1960}
1961
1963{
1964 // Acceleration is expressed in iDegrees/s^2
1965 // save internally the new value of the acceleration; it'll be used in the velocityMove command
1966
1967 if (acc > 1e6)
1968 {
1969 _ref_accs[j ] = 1e6;
1970 }
1971 else if (acc < -1e6)
1972 {
1973 _ref_accs[j ] = -1e6;
1974 }
1975 else
1976 {
1977 _ref_accs[j ] = acc;
1978 }
1979
1980 return ReturnValue_ok;
1981}
1982
1984{
1985 // Acceleration is expressed in iDegrees/s^2
1986 // save internally the new value of the acceleration; it'll be used in the velocityMove command
1987 for(int j=0, index=0; j< _njoints; j++, index++)
1988 {
1989 if (accs[j] > 1e6)
1990 {
1991 _ref_accs[index] = 1e6;
1992 }
1993 else if (accs[j] < -1e6)
1994 {
1995 _ref_accs[index] = -1e6;
1996 }
1997 else
1998 {
1999 _ref_accs[index] = accs[j];
2000 }
2001 }
2002 return ReturnValue_ok;
2003}
2004
2006{
2007 *spd = _ref_speeds[j];
2008 return ReturnValue_ok;
2009}
2010
2012{
2013 memcpy(spds, _ref_speeds, sizeof(double) * _njoints);
2014 return ReturnValue_ok;
2015}
2016
2018{
2019 *acc = _ref_accs[j];
2020 return ReturnValue_ok;
2021}
2022
2024{
2025 memcpy(accs, _ref_accs, sizeof(double) * _njoints);
2026 return ReturnValue_ok;
2027}
2028
2034
2036{
2038 for(int j=0; j< _njoints; j++)
2039 {
2040 ret &= stopRaw(j);
2041 }
2042 return ret;
2043}
2045
2047// Position control2 interface //
2049
2050ReturnValue FakeMotionControl::positionMoveRaw(const int n_joint, const int *joints, const double *refs)
2051{
2052 if (verbose >= VERY_VERBOSE) {
2053 yCTrace(FAKEMOTIONCONTROL) << " -> n_joint " << n_joint;
2054 }
2055
2056 for(int j=0; j<n_joint; j++)
2057 {
2058 yCDebug(FAKEMOTIONCONTROL, "j: %d; ref %f;\n", joints[j], refs[j]); fflush(stdout);
2059 }
2060
2062 for(int j=0; j<n_joint; j++)
2063 {
2064 ret = ret &&positionMoveRaw(joints[j], refs[j]);
2065 }
2066 return ret;
2067}
2068
2069ReturnValue FakeMotionControl::relativeMoveRaw(const int n_joint, const int *joints, const double *deltas)
2070{
2071 if (verbose >= VERY_VERBOSE) {
2072 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2073 }
2074
2076 for(int j=0; j<n_joint; j++)
2077 {
2078 ret = ret &&relativeMoveRaw(joints[j], deltas[j]);
2079 }
2080 return ret;
2081}
2082
2083ReturnValue FakeMotionControl::checkMotionDoneRaw(const std::vector<int>& joints, bool&flag)
2084{
2085 if (verbose >= VERY_VERBOSE) {
2086 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2087 }
2088
2090 bool val = true;
2091 bool tot_val = true;
2092
2093 for(int j=0; j<joints.size(); j++)
2094 {
2095 ret = ret && checkMotionDoneRaw(joints[j], val);
2096 tot_val &= val;
2097 }
2098 flag = tot_val;
2099 return ret;
2100}
2101
2102ReturnValue FakeMotionControl::setTrajSpeedsRaw(const int n_joint, const int *joints, const double *spds)
2103{
2104 if (verbose >= VERY_VERBOSE) {
2105 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2106 }
2107
2109 for(int j=0; j<n_joint; j++)
2110 {
2111 ret = ret &&setTrajSpeedRaw(joints[j], spds[j]);
2112 }
2113 return ret;
2114}
2115
2116ReturnValue FakeMotionControl::setTrajAccelerationsRaw(const int n_joint, const int *joints, const double *accs)
2117{
2118 if (verbose >= VERY_VERBOSE) {
2119 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2120 }
2121
2123 for(int j=0; j<n_joint; j++)
2124 {
2125 ret = ret &&setTrajAccelerationRaw(joints[j], accs[j]);
2126 }
2127 return ret;
2128}
2129
2130ReturnValue FakeMotionControl::getTrajSpeedsRaw(const int n_joint, const int *joints, double *spds)
2131{
2132 if (verbose >= VERY_VERBOSE) {
2133 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2134 }
2135
2137 for(int j=0; j<n_joint; j++)
2138 {
2139 ret = ret && getTrajSpeedRaw(joints[j], &spds[j]);
2140 }
2141 return ret;
2142}
2143
2144ReturnValue FakeMotionControl::getTrajAccelerationsRaw(const int n_joint, const int *joints, double *accs)
2145{
2146 if (verbose >= VERY_VERBOSE) {
2147 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2148 }
2149
2151 for(int j=0; j<n_joint; j++)
2152 {
2153 ret = ret && getTrajAccelerationRaw(joints[j], &accs[j]);
2154 }
2155 return ret;
2156}
2157
2159{
2160 if (verbose >= VERY_VERBOSE) {
2161 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2162 }
2163
2165 for(int j=0; j<n_joint; j++)
2166 {
2167 ret = ret &&stopRaw(joints[j]);
2168 }
2169 return ret;
2170}
2171
2173
2174// ControlMode
2175
2176ReturnValue FakeMotionControl::getAvailableControlModesRaw(int j, std::vector<yarp::dev::SelectableControlModeEnum>& avail)
2177{
2178 if (verbose > VERY_VERY_VERBOSE) {
2179 yCTrace(FAKEMOTIONCONTROL) << "j: " << j;
2180 }
2181
2182 avail = _availableControlModes[j];
2183 return ReturnValue_ok;
2184}
2185
2187{
2188 if (verbose > VERY_VERY_VERBOSE) {
2189 yCTrace(FAKEMOTIONCONTROL) << "j: " << j;
2190 }
2191
2192 mode = (yarp::dev::ControlModeEnum)_controlModes[j];
2193 return ReturnValue_ok;
2194}
2195
2196// IControl Mode 2
2197ReturnValue FakeMotionControl::getControlModesRaw(std::vector<yarp::dev::ControlModeEnum>& mode)
2198{
2200 for(int j=0; j< _njoints; j++)
2201 {
2202 ret = ret && getControlModeRaw(j, mode[j]);
2203 }
2204 return ret;
2205}
2206
2207ReturnValue FakeMotionControl::getControlModesRaw(const std::vector<int>& joints, std::vector<yarp::dev::ControlModeEnum>& modes)
2208{
2210 for(int j=0; j< joints.size(); j++)
2211 {
2212 ret = ret && getControlModeRaw(joints[j], modes[j]);
2213 }
2214 return ret;
2215}
2216
2218{
2219 if (verbose >= VERY_VERBOSE) {
2220 yCTrace(FAKEMOTIONCONTROL) << "j: " << j << " mode: " << yarp::os::Vocab32::decode((int)mode);
2221 }
2222
2224 {
2225 _controlModes[j] = VOCAB_CM_IDLE;
2226 }
2227 else
2228 {
2229 _controlModes[j] = (int)mode;
2230 }
2231 _posCtrl_references[j] = pos[j];
2232 return ReturnValue_ok;
2233}
2234
2235
2236ReturnValue FakeMotionControl::setControlModesRaw(const std::vector<int>& joints, const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
2237{
2238 if (verbose >= VERY_VERBOSE) {
2239 yCTrace(FAKEMOTIONCONTROL) << "n_joints: " << joints.size();
2240 }
2241
2243 for(int i=0; i<joints.size(); i++)
2244 {
2245 ret &= setControlModeRaw(joints[i], modes[i]);
2246 }
2247 return ret;
2248}
2249
2250ReturnValue FakeMotionControl::setControlModesRaw(const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
2251{
2252 if (verbose >= VERY_VERBOSE) {
2254 }
2255
2257 for(int i=0; i<_njoints; i++)
2258 {
2259 ret &= setControlModeRaw(i, modes[i]);
2260 }
2261 return ret;
2262}
2263
2264
2266
2268{
2269 pos[j] = val;
2270 return ReturnValue_ok;
2271}
2272
2274{
2275 for(int j=0; j< _njoints; j++)
2276 {
2277 pos[j] = vals[j];
2278 }
2279 return ReturnValue_ok;
2280}
2281
2283{
2284 pos[j] = 0;
2285 return ReturnValue_ok;
2286}
2287
2289{
2290 for(int j=0; j< _njoints; j++)
2291 {
2292 pos[j] = 0;
2293 }
2294 return ReturnValue_ok;
2295}
2296
2298{
2300
2301 // To simulate a real controlboard, we assume that the joint
2302 // encoders is exactly the last set by setPosition(s) or positionMove
2303 *value = pos[j];
2304
2305 return ret;
2306}
2307
2309{
2311 for(int j=0; j< _njoints; j++)
2312 {
2313 ReturnValue ok = getEncoderRaw(j, &encs[j]);
2314 ret = ret && ok;
2315
2316 }
2317 return ret;
2318}
2319
2321{
2322 // To avoid returning uninitialized memory, we set the encoder speed to 0
2323 *sp = 0.0;
2324 return ReturnValue_ok;
2325}
2326
2328{
2330 for(int j=0; j< _njoints; j++)
2331 {
2332 ret &= getEncoderSpeedRaw(j, &spds[j]);
2333 }
2334 return ret;
2335}
2336
2338{
2339 // To avoid returning uninitialized memory, we set the encoder acc to 0
2340 *acc = 0.0;
2341
2342 return ReturnValue_ok;
2343}
2344
2346{
2348 for(int j=0; j< _njoints; j++)
2349 {
2350 ret &= getEncoderAccelerationRaw(j, &accs[j]);
2351 }
2352 return ret;
2353}
2354
2356
2358{
2359 ReturnValue ret = getEncodersRaw(encs);
2360 _mutex.lock();
2361 for (int i = 0; i < _njoints; i++) {
2362 stamps[i] = _encodersStamp[i] = _cycleTimestamp;
2363 }
2364 _mutex.unlock();
2365 return ret;
2366}
2367
2368ReturnValue FakeMotionControl::getEncoderTimedRaw(int j, double *encs, double *stamp)
2369{
2370 ReturnValue ret = getEncoderRaw(j, encs);
2371 _mutex.lock();
2372 *stamp = _encodersStamp[j] = _cycleTimestamp;
2373 _mutex.unlock();
2374
2375 return ret;
2376}
2377
2379
2381{
2382 *num=_njoints;
2383 return ReturnValue_ok;
2384}
2385
2390
2395
2400
2405
2410
2415
2417{
2418 *value = pos[m]*10;
2419 return ReturnValue_ok;
2420}
2421
2423{
2425 for(int j=0; j< _njoints; j++)
2426 {
2427 ret &= getMotorEncoderRaw(j, &encs[j]);
2428
2429 }
2430 return ret;
2431}
2432
2434{
2435 *sp = 0.0;
2436 return ReturnValue_ok;
2437}
2438
2440{
2442 for(int j=0; j< _njoints; j++)
2443 {
2444 ret &= getMotorEncoderSpeedRaw(j, &spds[j]);
2445 }
2446 return ret;
2447}
2448
2450{
2451 *acc = 0.0;
2452 return ReturnValue_ok;
2453}
2454
2456{
2458 for(int j=0; j< _njoints; j++)
2459 {
2460 ret &= getMotorEncoderAccelerationRaw(j, &accs[j]);
2461 }
2462 return ret;
2463}
2464
2466{
2467 ReturnValue ret = getMotorEncodersRaw(encs);
2468 _mutex.lock();
2469 for (int i = 0; i < _njoints; i++) {
2470 stamps[i] = _encodersStamp[i] = _cycleTimestamp;
2471 }
2472 _mutex.unlock();
2473
2474 return ret;
2475}
2476
2478{
2479 ReturnValue ret = getMotorEncoderRaw(m, encs);
2480 _mutex.lock();
2481 *stamp = _encodersStamp[m] = _cycleTimestamp;
2482 _mutex.unlock();
2483
2484 return ret;
2485}
2487
2489
2494
2499
2501{
2502 //just for testing purposes, this is not a real implementation
2503 *value = current[j];
2504 return ReturnValue_ok;
2505}
2506
2508{
2509 //just for testing purposes, this is not a real implementation
2511 for(int j=0; j< _njoints; j++)
2512 {
2513 ret &= getCurrentRaw(j, &vals[j]);
2514 }
2515 return ret;
2516}
2517
2519{
2520 maxCurrent[m] = val;
2521 return ReturnValue_ok;
2522}
2523
2525{
2526 *val = maxCurrent[m];
2527 return ReturnValue_ok;
2528}
2529
2531{
2532 (_enabledAmp[j ]) ? *st = 1 : *st = 0;
2533 return ReturnValue_ok;
2534}
2535
2537{
2539 for(int j=0; j<_njoints; j++)
2540 {
2541 sts[j] = _enabledAmp[j];
2542 }
2543 return ReturnValue_ok;
2544}
2545
2547{
2548 *val = peakCurrent[m];
2549 return ReturnValue_ok;
2550}
2551
2553{
2554 peakCurrent[m] = val;
2555 return ReturnValue_ok;
2556}
2557
2559{
2560 *val = nominalCurrent[m];
2561 return ReturnValue_ok;
2562}
2563
2565{
2566 nominalCurrent[m] = val;
2567 return ReturnValue_ok;
2568}
2569
2571{
2572 *val = pwm[m];
2573 return ReturnValue_ok;
2574}
2575
2577{
2578 *val = pwmLimit[m];
2579 return ReturnValue_ok;
2580}
2581
2583{
2584 pwmLimit[m] = val;
2585 return ReturnValue_ok;
2586}
2587
2589{
2590 *val = supplyVoltage[m];
2591 return ReturnValue_ok;
2592}
2593
2594
2595// Limit interface
2597{
2598 _limitsMin[j] = min;
2599 _limitsMax[j] = max;
2600 return ReturnValue_ok;
2601}
2602
2603ReturnValue FakeMotionControl::getPosLimitsRaw(int j, double *min, double *max)
2604{
2605 *min = _limitsMin[j];
2606 *max = _limitsMax[j];
2607 return ReturnValue_ok;
2608}
2609
2611{
2612 *gearbox = _gearbox[j];
2613 return ReturnValue_ok;
2614}
2615
2617{
2618 _gearbox[j] = val;
2619 return ReturnValue_ok;
2620}
2621
2626
2631
2636
2638{
2640}
2641
2643{
2645}
2646
2651
2653{
2655}
2656
2658{
2660}
2661
2663{
2665}
2666
2671
2676
2681
2683{
2684 name = _axisName[j];
2685 return ReturnValue_ok;
2686}
2687
2689{
2690 type = _jointType[j];
2691 return ReturnValue_ok;
2692}
2693
2694// IControlLimits
2695ReturnValue FakeMotionControl::setVelLimitsRaw(int axis, double min, double max)
2696{
2697 _minJntCmdVelocity[axis] = min;
2698 _maxJntCmdVelocity[axis] = max;
2699 return ReturnValue_ok;
2700}
2701
2702ReturnValue FakeMotionControl::getVelLimitsRaw(int axis, double *min, double *max)
2703{
2704 *min = _minJntCmdVelocity[axis];
2705 *max = _maxJntCmdVelocity[axis];
2706 return ReturnValue_ok;
2707}
2708
2709
2710// Torque control
2712{
2713 *t = _torques[j];
2714 return ReturnValue_ok;
2715}
2716
2718{
2719 for (int i = 0; i < _njoints; i++)
2720 {
2721 t[i]= _torques[i];
2722 }
2723 return ReturnValue_ok;
2724}
2725
2727{
2728 *min = -100;
2729 *max = 100;
2730 return ReturnValue_ok;
2731}
2732
2734{
2736 for (int j = 0; j < _njoints && ret; j++) {
2737 ret &= getTorqueRangeRaw(j, &min[j], &max[j]);
2738 }
2739 return ret;
2740}
2741
2743{
2745 for (int j = 0; j < _njoints && ret; j++) {
2746 ret &= setRefTorqueRaw(j, t[j]);
2747 }
2748 return ret;
2749}
2750
2752{
2753 _mutex.lock();
2754 _ref_torques[j]=t;
2755
2756 if (t>1.0 || t< -1.0)
2757 {
2758 yCError(FAKEMOTIONCONTROL) << "Joint received a high torque command, and was put in hardware fault";
2759 _hwfault_code[j] = 1;
2760 _hwfault_message[j] = "test" + std::to_string(j) + " torque";
2761 _controlModes[j] = VOCAB_CM_HW_FAULT;
2762 }
2763 _mutex.unlock();
2764 return ReturnValue_ok;
2765}
2766
2767ReturnValue FakeMotionControl::setRefTorquesRaw(const int n_joint, const int *joints, const double *t)
2768{
2770 for (int j = 0; j < n_joint && ret; j++) {
2771 ret &= setRefTorqueRaw(joints[j], t[j]);
2772 }
2773 return ret;
2774}
2775
2777{
2779 for (int j = 0; j < _njoints && ret; j++) {
2780 ret &= getRefTorqueRaw(j, &_ref_torques[j]);
2781 }
2782 return ReturnValue_ok;
2783}
2784
2786{
2787 _mutex.lock();
2788 *t = _ref_torques[j];
2789 _mutex.unlock();
2790 return ReturnValue_ok;
2791}
2792
2793ReturnValue FakeMotionControl::getImpedanceRaw(int j, double *stiffness, double *damping)
2794{
2795 _mutex.lock();
2796 *stiffness = _stiffness[j];
2797 *damping = _damping[j];
2798 _mutex.unlock();
2799 return ReturnValue_ok;
2800}
2801
2802ReturnValue FakeMotionControl::setImpedanceRaw(int j, double stiffness, double damping)
2803{
2804 _mutex.lock();
2805 _stiffness[j] = stiffness;
2806 _damping[j] = damping;
2807 _mutex.unlock();
2808 return ReturnValue_ok;
2809}
2810
2812{
2813 _mutex.lock();
2814 _force_offset[j] = offset;
2815 _mutex.unlock();
2816 return ReturnValue_ok;
2817}
2818
2820{
2821 _mutex.lock();
2822 *offset = _force_offset[j];
2823 _mutex.unlock();
2824 return ReturnValue_ok;
2825}
2826
2827ReturnValue FakeMotionControl::getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
2828{
2829 *min_stiff=1.0;
2830 *max_stiff=10.0;
2831 *min_damp=2.0;
2832 *max_damp=20.0;
2833 return ReturnValue_ok;
2834}
2835
2837{
2838 params->bemf = _kbemf[j];
2839 params->bemf_scale = _kbemf_scale[j];
2840 params->ktau = _ktau[j];
2841 params->ktau_scale = _ktau_scale[j];
2842 params->viscousPos = _viscousPos[j];
2843 params->viscousNeg = _viscousNeg[j];
2844 params->coulombPos = _coulombPos[j];
2845 params->coulombNeg = _coulombNeg[j];
2846 params->velocityThres = _velocityThres[j];
2847 yCDebug(FAKEMOTIONCONTROL) << "getMotorTorqueParamsRaw" << params->bemf
2848 << params->bemf_scale
2849 << params->ktau
2850 << params->ktau_scale
2851 << params->viscousPos
2852 << params->viscousNeg
2853 << params->coulombPos
2854 << params->coulombNeg
2855 << params->velocityThres;
2856 return ReturnValue_ok;
2857}
2858
2860{
2861 _kbemf[j] = params.bemf;
2862 _ktau[j] = params.ktau;
2863 _kbemf_scale[j] = params.bemf_scale;
2864 _ktau_scale[j] = params.ktau_scale;
2865 _viscousPos[j] = params.viscousPos;
2866 _viscousNeg[j] = params.viscousNeg;
2867 _coulombPos[j] = params.coulombPos;
2868 _coulombNeg[j] = params.coulombNeg;
2869 _velocityThres[j] = params.velocityThres;
2870 yCDebug(FAKEMOTIONCONTROL) << "setMotorTorqueParamsRaw" << params.bemf
2871 << params.bemf_scale
2872 << params.ktau
2873 << params.ktau_scale
2874 << params.viscousPos
2875 << params.viscousNeg
2876 << params.coulombPos
2877 << params.coulombNeg
2878 << params.velocityThres;
2879 return ReturnValue_ok;
2880}
2881
2882// IVelocityControl interface
2883ReturnValue FakeMotionControl::velocityMoveRaw(const int n_joint, const int *joints, const double *spds)
2884{
2886 for(int i=0; i<n_joint; i++)
2887 {
2888 ret &= velocityMoveRaw(joints[i], spds[i]);
2889 }
2890 return ret;
2891}
2892
2893// PositionDirect Interface
2895{
2896 _posDir_references[j] = ref;
2897 return ReturnValue_ok;
2898}
2899
2900ReturnValue FakeMotionControl::setPositionsRaw(const int n_joint, const int *joints, const double *refs)
2901{
2902 for(int i=0; i< n_joint; i++)
2903 {
2904 _posDir_references[joints[i]] = refs[i];
2905 }
2906 return ReturnValue_ok;
2907}
2908
2910{
2911 for(int i=0; i< _njoints; i++)
2912 {
2913 _posDir_references[i] = refs[i];
2914 }
2915 return ReturnValue_ok;
2916}
2917
2918
2920{
2921 if (verbose >= VERY_VERBOSE) {
2922 yCTrace(FAKEMOTIONCONTROL) << "j " << axis << " ref " << _posCtrl_references[axis];
2923 }
2924
2926 auto ret = getControlModeRaw(axis, mode);
2930 {
2931 yCWarning(FAKEMOTIONCONTROL) << "getTargetPosition: Joint " << axis << " is not in POSITION mode, therefore the value returned by " <<
2932 "this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
2933 }
2934 *ref = _posCtrl_references[axis];
2935 return ReturnValue_ok;
2936}
2937
2939{
2941 for (int i = 0; i < _njoints; i++) {
2942 ret &= getTargetPositionRaw(i, &refs[i]);
2943 }
2944 return ret;
2945}
2946
2948{
2950 for (int i = 0; i<nj; i++)
2951 {
2952 ret &= getTargetPositionRaw(jnts[i], &refs[i]);
2953 }
2954 return ret;
2955}
2956
2958{
2959 *ref = _command_speeds[axis];
2960 return ReturnValue_ok;
2961}
2962
2964{
2966 for (int i = 0; i<_njoints; i++)
2967 {
2968 ret &= getTargetVelocityRaw(i, &refs[i]);
2969 }
2970 return ret;
2971}
2972
2974{
2976 for (int i = 0; i<nj; i++)
2977 {
2978 ret &= getTargetVelocityRaw(jnts[i], &refs[i]);
2979 }
2980 return ret;
2981}
2982
2984{
2986 auto ret = getControlModeRaw(axis, mode);
2991 {
2992 yCWarning(FAKEMOTIONCONTROL) << "getRefPosition: Joint " << axis << " is not in POSITION_DIRECT mode, therefore the value returned by \
2993 this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
2994 }
2995
2996 *ref = _posDir_references[axis];
2997
2998 return ReturnValue_ok;
2999}
3000
3002{
3004 for (int i = 0; i<_njoints; i++)
3005 {
3006 ret &= getRefPositionRaw(i, &refs[i]);
3007 }
3008 return ret;
3009}
3010
3012{
3014 for (int i = 0; i<nj; i++)
3015 {
3016 ret &= getRefPositionRaw(jnts[i], &refs[i]);
3017 }
3018 return ret;
3019}
3020
3021
3022// InteractionMode
3024{
3025 if (verbose > VERY_VERY_VERBOSE) {
3026 yCTrace(FAKEMOTIONCONTROL) << "j: " << j;
3027 }
3028
3029 _mode = (yarp::dev::InteractionModeEnum)_interactMode[j];
3030 return ReturnValue_ok;}
3031
3032ReturnValue FakeMotionControl::getInteractionModesRaw(const std::vector<int>& joints, std::vector<yarp::dev::InteractionModeEnum>& modes)
3033{
3035 for(int j=0; j< joints.size(); j++)
3036 {
3037 ret = ret && getInteractionModeRaw(joints[j], modes[j]);
3038 }
3039 return ret;
3040
3041}
3042
3043ReturnValue FakeMotionControl::getInteractionModesRaw(std::vector<yarp::dev::InteractionModeEnum>& modes)
3044{
3046 for (int j = 0; j < _njoints; j++) {
3047 ret = ret && getInteractionModeRaw(j, modes[j]);
3048 }
3049 return ret;
3050}
3051
3053{
3054 if (verbose >= VERY_VERBOSE) {
3055 yCTrace(FAKEMOTIONCONTROL) << "j: " << j << " interaction mode: " << yarp::os::Vocab32::decode(_mode);
3056 }
3057
3058 _interactMode[j] = _mode;
3059
3060 return ReturnValue_ok;
3061}
3062
3063
3064ReturnValue FakeMotionControl::setInteractionModesRaw(const std::vector<int>& joints, const std::vector<yarp::dev::InteractionModeEnum>& modes)
3065{
3067 for(int i=0; i<joints.size(); i++)
3068 {
3069 ret &= setInteractionModeRaw(joints[i], modes[i]);
3070 }
3071 return ret;
3072}
3073
3074ReturnValue FakeMotionControl::setInteractionModesRaw(const std::vector<yarp::dev::InteractionModeEnum>& modes)
3075{
3077 for(int i=0; i<_njoints; i++)
3078 {
3079 ret &= setInteractionModeRaw(i, modes[i]);
3080 }
3081 return ret;
3082
3083}
3084
3086{
3087 *num=_njoints;
3088 return ReturnValue_ok;
3089}
3090
3092{
3093 *val = 37.5+double(m);
3094 return ReturnValue_ok;
3095}
3096
3098{
3100 for(int j=0; j< _njoints; j++)
3101 {
3102 ret &= getTemperatureRaw(j, &vals[j]);
3103 }
3104 return ret;
3105}
3106
3108{
3109 *temp = _tempLimit[m];
3110 return ReturnValue_ok;
3111}
3112
3114{
3115 _tempLimit[m] = temp;
3116 return ReturnValue_ok;
3117}
3118
3119//PWM interface
3121{
3122 refpwm[j] = v;
3123 pwm[j] = v;
3124 last_pwm_command[j]=yarp::os::Time::now();
3125 return ReturnValue_ok;
3126}
3127
3129{
3130 for (int i = 0; i < _njoints; i++)
3131 {
3132 setRefDutyCycleRaw(i,v[i]);
3133 }
3134 return ReturnValue_ok;
3135}
3136
3138{
3139 *v = refpwm[j];
3140 return ReturnValue_ok;
3141}
3142
3144{
3145 for (int i = 0; i < _njoints; i++)
3146 {
3147 v[i] = refpwm[i];
3148 }
3149 return ReturnValue_ok;
3150}
3151
3153{
3154 *v = pwm[j];
3155 return ReturnValue_ok;
3156}
3157
3159{
3160 for (int i = 0; i < _njoints; i++)
3161 {
3162 v[i] = pwm[i];
3163 }
3164 return ReturnValue_ok;
3165}
3166
3167// Current interface
3168/*bool FakeMotionControl::getCurrentRaw(int j, double *t)
3169{
3170 return NOT_YET_IMPLEMENTED("getCurrentRaw");
3171}
3172
3173bool FakeMotionControl::getCurrentsRaw(double *t)
3174{
3175 return NOT_YET_IMPLEMENTED("getCurrentsRaw");
3176}
3177*/
3178
3180{
3181 //just for testing purposes, this is not a real implementation
3182 *min = -3.5;
3183 *max = +3.5;
3184 return ReturnValue_ok;
3185}
3186
3188{
3189 //just for testing purposes, this is not a real implementation
3190 for (int i = 0; i < _njoints; i++)
3191 {
3192 min[i] = -3.5;
3193 max[i] = +3.5;
3194 }
3195 return ReturnValue_ok;
3196}
3197
3199{
3200 for (int i = 0; i < _njoints; i++)
3201 {
3202 _ref_currents[i] = t[i];
3203 current[i] = t[i] / 2;
3204 }
3205 return ReturnValue_ok;
3206}
3207
3209{
3210 _ref_currents[j] = t;
3211 current[j] = t / 2;
3212 return ReturnValue_ok;
3213}
3214
3215ReturnValue FakeMotionControl::setRefCurrentsRaw(const int n_joint, const int *joints, const double *t)
3216{
3218 for (int j = 0; j<n_joint; j++)
3219 {
3220 ret = ret &&setRefCurrentRaw(joints[j], t[j]);
3221 }
3222 return ret;
3223}
3224
3226{
3227 for (int i = 0; i < _njoints; i++)
3228 {
3229 t[i] = _ref_currents[i];
3230 }
3231 return ReturnValue_ok;
3232}
3233
3235{
3236 *t = _ref_currents[j];
3237 return ReturnValue_ok;
3238}
3239
3244
3246{
3247 return _njoints;
3248}
3249
3251{
3252 for (int i = 0; i < _njoints; i++)
3253 {
3254 measure[i] = _torques[i];
3255 }
3256 return true;
3257}
3258
3260{
3261 _torques[ch] = measure;
3262 return true;
3263}
3264
3265ReturnValue FakeMotionControl::getLastJointFaultRaw(int j, int& fault, std::string& message)
3266{
3267 _mutex.lock();
3268 fault = _hwfault_code[j];
3269 message = _hwfault_message[j];
3270 _mutex.unlock();
3271 return ReturnValue_ok;
3272}
3273
3275{
3276 braked = _braked[j];
3277 return ReturnValue_ok;
3278}
3279
3281{
3282 _braked[j] = active;
3283 return ReturnValue_ok;
3284}
3285
3287{
3288 _autobraked[j] = enabled;
3289 return ReturnValue_ok;
3290}
3291
3293{
3294 enabled = _autobraked[j];
3295 return ReturnValue_ok;
3296}
3297
3299{
3301 if (jnt < 0 || jnt >= _njoints) {
3302 yCError(FAKEMOTIONCONTROL) << "setRefVelocityRaw: joint index out of bounds";
3303 return ReturnValue::return_code::return_value_error_method_failed;
3304 }
3305 if (vel < -_maxJntCmdVelocity[jnt] || vel > _maxJntCmdVelocity[jnt]) {
3306 yCError(FAKEMOTIONCONTROL) << "setRefVelocityRaw: velocity out of bounds for joint" << jnt;
3307 return ReturnValue::return_code::return_value_error_method_failed;
3308 }
3309 _mutex.lock();
3310 _dir_vel_commands[jnt] = vel;
3311 _mutex.unlock();
3312 return ReturnValue_ok;
3313}
3314
3316{
3319 for (int i = 0; i < _njoints; i++) {
3320 ret &= setRefVelocityRaw(i, vels[i]);
3321 }
3322 return ret;
3323}
3324
3325ReturnValue FakeMotionControl::setRefVelocityRaw(const std::vector<int>& jnts, const std::vector<double>& vels)
3326{
3328 if (jnts.size() != vels.size()) {
3329 yCError(FAKEMOTIONCONTROL) << "setRefVelocityRaw: jnts and vels vectors must have the same size";
3330 return ReturnValue::return_code::return_value_error_method_failed;
3331 }
3333 for (int i = 0; i < _njoints; i++) {
3334 ret &= setRefVelocityRaw(jnts[i], vels[i]);
3335 }
3336 return ret;
3337}
3338
3340{
3342 if (jnt < 0 || jnt >= _njoints) {
3343 yCError(FAKEMOTIONCONTROL) << "setRefVelocityRaw: joint index out of bounds";
3344 return ReturnValue::return_code::return_value_error_method_failed;
3345 }
3346 _mutex.lock();
3347 vel = _dir_vel_commands[jnt];
3348 _mutex.unlock();
3349 return ReturnValue_ok;
3350}
3351
3353{
3356 vels.resize(_njoints);
3357 for (int i = 0; i < _njoints; i++) {
3358 ret &= getRefVelocityRaw(i, vels[i]);
3359 }
3360 return ret;
3361}
3362
3363ReturnValue FakeMotionControl::getRefVelocityRaw(const std::vector<int>& jnts, std::vector<double>& vels)
3364{
3366 if (jnts.size() != vels.size()) {
3367 yCError(FAKEMOTIONCONTROL) << "getRefVelocityRaw: jnts and vels vectors must have the same size";
3368 return ReturnValue::return_code::return_value_error_method_failed;
3369 }
3371 vels.resize(_njoints);
3372 for (int i = 0; i < _njoints; i++) {
3373 ret &= getRefVelocityRaw(jnts[i], vels[i]);
3374 }
3375 return ret;
3376}
3377
3378
3380{
3383
3384 size_t index = PidControlTypeEnum2Index(pidtype);
3385 switch (pidtype)
3386 {
3387 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
3388 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
3389 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
3390 units=_ppids[j].at(index).pidExtraInfo;
3391 break;
3392 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
3393 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
3394 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
3395 units=_vpids[j].at(index).pidExtraInfo;
3396 break;
3397 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
3398 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
3399 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
3400 units=_cpids[j].at(index).pidExtraInfo;
3401 break;
3402 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
3403 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
3404 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
3405 units=_pdpids[j].at(index).pidExtraInfo;
3406 break;
3407 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
3408 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
3409 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
3410 units=_vdpids[j].at(index).pidExtraInfo;
3411 break;
3412 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
3413 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
3414 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
3415 units=_mpids[j].at(index).pidExtraInfo;
3416 break;
3417 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
3418 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
3419 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
3420 units=_tpids[j].at(index).pidExtraInfo;
3421 break;
3422 default:
3423 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
3424 return ReturnValue::return_code::return_value_error_method_failed;
3425 break;
3426 }
3427
3428 return ret;
3429}
3430
3431
3432ReturnValue FakeMotionControl::getPidExtraInfosRaw(const PidControlTypeEnum& pidtype, std::vector<yarp::dev::PidExtraInfo>& units)
3433{
3436 units.resize(_njoints);
3437 for (int i = 0; i < _njoints; i++) {
3438 ret &= getPidExtraInfoRaw(pidtype, i, units[i]);
3439 }
3440 return ret;
3441}
3442
3443
3444/*
3445bool FakeMotionControl::parseImpedanceGroup_NewFormat(Bottle& pidsGroup, ImpedanceParameters vals[])
3446{
3447 int j=0;
3448 Bottle xtmp;
3449
3450 if (!extractGroup(pidsGroup, xtmp, "stiffness", "stiffness parameter", _njoints)) {
3451 return false;
3452 }
3453 for (j=0; j<_njoints; j++) {
3454 vals[j].stiffness = xtmp.get(j+1).asFloat64();
3455 }
3456
3457 if (!extractGroup(pidsGroup, xtmp, "damping", "damping parameter", _njoints)) {
3458 return false;
3459 }
3460 for (j=0; j<_njoints; j++) {
3461 vals[j].damping = xtmp.get(j+1).asFloat64();
3462 }
3463
3464 return true;
3465}
3466*/
3467
3468/*
3469bool FakeMotionControl::parsePositionPidsGroup(Bottle& pidsGroup, Pid myPid[])
3470{
3471 int j=0;
3472 Bottle xtmp;
3473
3474 if (!extractGroup(pidsGroup, xtmp, "kp", "Pid kp parameter", _njoints)) {
3475 return false;
3476 }
3477 for (j=0; j<_njoints; j++) {
3478 myPid[j].kp = xtmp.get(j+1).asFloat64();
3479 }
3480
3481 if (!extractGroup(pidsGroup, xtmp, "kd", "Pid kd parameter", _njoints)) {
3482 return false;
3483 }
3484 for (j=0; j<_njoints; j++) {
3485 myPid[j].kd = xtmp.get(j+1).asFloat64();
3486 }
3487
3488 if (!extractGroup(pidsGroup, xtmp, "ki", "Pid kp parameter", _njoints)) {
3489 return false;
3490 }
3491 for (j=0; j<_njoints; j++) {
3492 myPid[j].ki = xtmp.get(j+1).asFloat64();
3493 }
3494
3495 if (!extractGroup(pidsGroup, xtmp, "maxInt", "Pid maxInt parameter", _njoints)) {
3496 return false;
3497 }
3498 for (j=0; j<_njoints; j++) {
3499 myPid[j].max_int = xtmp.get(j+1).asFloat64();
3500 }
3501
3502 if (!extractGroup(pidsGroup, xtmp, "maxPwm", "Pid maxPwm parameter", _njoints)) {
3503 return false;
3504 }
3505 for (j=0; j<_njoints; j++) {
3506 myPid[j].max_output = xtmp.get(j+1).asFloat64();
3507 }
3508
3509 if (!extractGroup(pidsGroup, xtmp, "shift", "Pid shift parameter", _njoints)) {
3510 return false;
3511 }
3512 for (j=0; j<_njoints; j++) {
3513 myPid[j].scale = xtmp.get(j+1).asFloat64();
3514 }
3515
3516 if (!extractGroup(pidsGroup, xtmp, "ko", "Pid ko parameter", _njoints)) {
3517 return false;
3518 }
3519 for (j=0; j<_njoints; j++) {
3520 myPid[j].offset = xtmp.get(j+1).asFloat64();
3521 }
3522
3523 if (!extractGroup(pidsGroup, xtmp, "stictionUp", "Pid stictionUp", _njoints)) {
3524 return false;
3525 }
3526 for (j=0; j<_njoints; j++) {
3527 myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
3528 }
3529
3530 if (!extractGroup(pidsGroup, xtmp, "stictionDwn", "Pid stictionDwn", _njoints)) {
3531 return false;
3532 }
3533 for (j=0; j<_njoints; j++) {
3534 myPid[j].stiction_down_val = xtmp.get(j+1).asFloat64();
3535 }
3536
3537 if (!extractGroup(pidsGroup, xtmp, "kff", "Pid kff parameter", _njoints)) {
3538 return false;
3539 }
3540 for (j=0; j<_njoints; j++) {
3541 myPid[j].kff = xtmp.get(j+1).asFloat64();
3542 }
3543
3544 //conversion from metric to machine units (if applicable)
3545 if (_positionControlUnits==P_METRIC_UNITS)
3546 {
3547 for (j=0; j<_njoints; j++)
3548 {
3549 myPid[j].kp = myPid[j].kp / _angleToEncoder[j]; //[PWM/deg]
3550 myPid[j].ki = myPid[j].ki / _angleToEncoder[j]; //[PWM/deg]
3551 myPid[j].kd = myPid[j].kd / _angleToEncoder[j]; //[PWM/deg]
3552 }
3553 }
3554 else
3555 {
3556 //do nothing
3557 }
3558
3559 //optional PWM limit
3560 if(_pwmIsLimited)
3561 { // check for value in the file
3562 if (!extractGroup(pidsGroup, xtmp, "limPwm", "Limited PWD", _njoints))
3563 {
3564 yCError(FAKEMOTIONCONTROL) << "The PID parameter limPwm was requested but was not correctly set in the configuration file, please fill it.";
3565 return false;
3566 }
3567
3568 yCInfo(FAKEMOTIONCONTROL) << "Using LIMITED PWM!!";
3569 for (j = 0; j < _njoints; j++) {
3570 myPid[j].max_output = xtmp.get(j + 1).asFloat64();
3571 }
3572 }
3573
3574 return true;
3575}
3576*/
3577
3578/*
3579bool FakeMotionControl::parseTorquePidsGroup(Bottle& pidsGroup, Pid myPid[], double kbemf[], double ktau[], int filterType[], double viscousPos[], double viscousNeg[], double coulombPos[], double coulombNeg[], double velocityThres[])
3580{
3581 int j=0;
3582 Bottle xtmp;
3583 if (!extractGroup(pidsGroup, xtmp, "kp", "Pid kp parameter", _njoints)) {
3584 return false;
3585 }
3586 for (j=0; j<_njoints; j++) {
3587 myPid[j].kp = xtmp.get(j+1).asFloat64();
3588 }
3589
3590 if (!extractGroup(pidsGroup, xtmp, "kd", "Pid kd parameter", _njoints)) {
3591 return false;
3592 }
3593 for (j=0; j<_njoints; j++) {
3594 myPid[j].kd = xtmp.get(j+1).asFloat64();
3595 }
3596
3597 if (!extractGroup(pidsGroup, xtmp, "ki", "Pid kp parameter", _njoints)) {
3598 return false;
3599 }
3600 for (j=0; j<_njoints; j++) {
3601 myPid[j].ki = xtmp.get(j+1).asFloat64();
3602 }
3603
3604 if (!extractGroup(pidsGroup, xtmp, "maxInt", "Pid maxInt parameter", _njoints)) {
3605 return false;
3606 }
3607 for (j=0; j<_njoints; j++) {
3608 myPid[j].max_int = xtmp.get(j+1).asFloat64();
3609 }
3610
3611 if (!extractGroup(pidsGroup, xtmp, "maxPwm", "Pid maxPwm parameter", _njoints)) {
3612 return false;
3613 }
3614 for (j=0; j<_njoints; j++) {
3615 myPid[j].max_output = xtmp.get(j+1).asFloat64();
3616 }
3617
3618 if (!extractGroup(pidsGroup, xtmp, "shift", "Pid shift parameter", _njoints)) {
3619 return false;
3620 }
3621 for (j=0; j<_njoints; j++) {
3622 myPid[j].scale = xtmp.get(j+1).asFloat64();
3623 }
3624
3625 if (!extractGroup(pidsGroup, xtmp, "ko", "Pid ko parameter", _njoints)) {
3626 return false;
3627 }
3628 for (j=0; j<_njoints; j++) {
3629 myPid[j].offset = xtmp.get(j+1).asFloat64();
3630 }
3631
3632 if (!extractGroup(pidsGroup, xtmp, "stictionUp", "Pid stictionUp", _njoints)) {
3633 return false;
3634 }
3635 for (j=0; j<_njoints; j++) {
3636 myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
3637 }
3638
3639 if (!extractGroup(pidsGroup, xtmp, "stictionDwn", "Pid stictionDwn", _njoints)) {
3640 return false;
3641 }
3642 for (j=0; j<_njoints; j++) {
3643 myPid[j].stiction_down_val = xtmp.get(j+1).asFloat64();
3644 }
3645
3646 if (!extractGroup(pidsGroup, xtmp, "kff", "Pid kff parameter", _njoints)) {
3647 return false;
3648 }
3649 for (j=0; j<_njoints; j++) {
3650 myPid[j].kff = xtmp.get(j+1).asFloat64();
3651 }
3652
3653 if (!extractGroup(pidsGroup, xtmp, "kbemf", "kbemf parameter", _njoints)) {
3654 return false;
3655 }
3656 for (j=0; j<_njoints; j++) {
3657 kbemf[j] = xtmp.get(j+1).asFloat64();
3658 }
3659
3660 if (!extractGroup(pidsGroup, xtmp, "ktau", "ktau parameter", _njoints)) {
3661 return false;
3662 }
3663 for (j=0; j<_njoints; j++) {
3664 ktau[j] = xtmp.get(j+1).asFloat64();
3665 }
3666
3667 if (!extractGroup(pidsGroup, xtmp, "filterType", "filterType param", _njoints)) {
3668 return false;
3669 }
3670 for (j=0; j<_njoints; j++) {
3671 filterType[j] = xtmp.get(j+1).asInt32();
3672 }
3673
3674 if (!extractGroup(pidsGroup, xtmp, "viscousPos", "viscous pos parameter", _njoints)) {
3675 return false;
3676 }
3677 for (j=0; j<_njoints; j++) {
3678 viscousPos[j] = xtmp.get(j+1).asFloat64();
3679 }
3680
3681 if (!extractGroup(pidsGroup, xtmp, "viscousNeg", "viscous neg parameter", _njoints)) {
3682 return false;
3683 }
3684 for (j=0; j<_njoints; j++) {
3685 viscousNeg[j] = xtmp.get(j+1).asFloat64();
3686 }
3687
3688 if (!extractGroup(pidsGroup, xtmp, "coulombPos", "coulomb pos parameter", _njoints)) {
3689 return false;
3690 }
3691 for (j=0; j<_njoints; j++) {
3692 coulombPos[j] = xtmp.get(j+1).asFloat64();
3693 }
3694
3695 if (!extractGroup(pidsGroup, xtmp, "coulombNeg", "coulomb neg parameter", _njoints)) {
3696 return false;
3697 }
3698 for (j=0; j<_njoints; j++) {
3699 coulombNeg[j] = xtmp.get(j+1).asFloat64();
3700 }
3701
3702 if (!extractGroup(pidsGroup, xtmp, "velocityThres", "velocity threshold parameter", _njoints)) {
3703 return false;
3704 }
3705 for (j=0; j<_njoints; j++) {
3706 velocityThres[j] = xtmp.get(j+1).asFloat64();
3707 }
3708
3709
3710 //conversion from metric to machine units (if applicable)
3711// for (j=0; j<_njoints; j++)
3712// {
3713// myPid[j].kp = myPid[j].kp / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
3714// myPid[j].ki = myPid[j].ki / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
3715// myPid[j].kd = myPid[j].kd / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
3716// myPid[j].stiction_up_val = myPid[j].stiction_up_val * _torqueControlHelper->getNewtonsToSensor(j); //[Nm]
3717// myPid[j].stiction_down_val = myPid[j].stiction_down_val * _torqueControlHelper->getNewtonsToSensor(j); //[Nm]
3718// }
3719
3720 //optional PWM limit
3721 if(_pwmIsLimited)
3722 { // check for value in the file
3723 if (!extractGroup(pidsGroup, xtmp, "limPwm", "Limited PWM", _njoints))
3724 {
3725 yCError(FAKEMOTIONCONTROL) << "The PID parameter limPwm was requested but was not correctly set in the configuration file, please fill it.";
3726 return false;
3727 }
3728
3729 yCInfo(FAKEMOTIONCONTROL) << "Using LIMITED PWM!!";
3730 for (j = 0; j < _njoints; j++) {
3731 myPid[j].max_output = xtmp.get(j + 1).asFloat64();
3732 }
3733 }
3734
3735 return true;
3736}
3737*/
3738
3739// eof
void checkAndDestroy(T *&p)
FeatureMode mode
#define OPENLOOP_WATCHDOG
std::string toString(const T &value)
convert an arbitrary type to string.
#define PWM_CONSTANT
#define VELOCITY_WATCHDOG
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_VELOCITY_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_CM_IDLE
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION
constexpr yarp::conf::vocab32_t VOCAB_CM_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_CM_VELOCITY
#define yDebug(...)
Definition Log.h:275
#define ReturnValue_ok
Definition ReturnValue.h:82
#define YARP_METHOD_NOT_YET_IMPLEMENTED()
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
yarp::dev::ReturnValue setPosLimitsRaw(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...
virtual bool getHasTempSensorsRaw(int j, int &ret)
yarp::dev::ReturnValue setInteractionModesRaw(const std::vector< int > &joints, const std::vector< yarp::dev::InteractionModeEnum > &modes) override
Set the interaction mode of the robot for a set of joints, values can be stiff or compliant.
yarp::dev::ReturnValue getInteractionModeRaw(int j, yarp::dev::InteractionModeEnum &_mode) override
Get the current interaction mode of the robot, values can be stiff or compliant.
yarp::dev::ReturnValue getPosLimitsRaw(int axis, double *min, double *max) override
Get the software limits for a particular axis.
yarp::dev::ReturnValue getRefDutyCycleRaw(int j, double *v) override
Gets the last reference sent using the setRefDutyCycleRaw function.
yarp::dev::ReturnValue setMotorEncoderRaw(int m, const double val) override
Set the value of the motor encoder for a given motor.
yarp::dev::ReturnValue getMotorEncoderSpeedsRaw(double *spds) override
Read the instantaneous speed of all motor encoders.
yarp::dev::ReturnValue getPidOutputRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
yarp::dev::ReturnValue setPidReferenceRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
yarp::dev::ReturnValue getEncoderTimedRaw(int j, double *encs, double *stamp) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue velocityMoveRaw(int j, double sp) override
Start motion at a given speed, single joint.
yarp::dev::ReturnValue getPidOffsetRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double &v) override
Get the currently set offset value for the specified PID controller.
yarp::dev::ReturnValue setTemperatureLimitRaw(int m, const double temp) override
Set the temperature limit for a specific motor.
yarp::dev::ReturnValue getImpedanceRaw(int j, double *stiffness, double *damping) override
Get current impedance parameters (stiffness,damping,offset) for a specific joint.
yarp::dev::ReturnValue setImpedanceRaw(int j, double stiffness, double damping) override
Set current impedance parameters (stiffness,damping) for a specific joint.
yarp::dev::ReturnValue 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...
yarp::dev::ReturnValue isPidEnabledRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, bool &enabled) override
Get the current status (enabled/disabled) of the PID controller.
yarp::dev::ReturnValue getEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue setPidOffsetRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override
Set an offset value for a given controller.
yarp::dev::ReturnValue getInteractionModesRaw(const std::vector< int > &joints, std::vector< yarp::dev::InteractionModeEnum > &modes) override
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
yarp::dev::ReturnValue setAutoBrakeEnabledRaw(int j, bool enabled) override
virtual bool getJointEncoderTypeRaw(int j, int &type)
yarp::dev::ReturnValue getEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getTemperatureLimitRaw(int m, double *temp) override
Retrieves the current temperature limit for a specific motor.
yarp::dev::ReturnValue setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
Set new reference point for all axes.
yarp::dev::ReturnValue getMaxCurrentRaw(int j, double *val) override
Returns the maximum electric current allowed for a given motor.
yarp::dev::ReturnValue setTrajSpeedsRaw(const double *spds) override
Set reference speed on all joints.
yarp::dev::ReturnValue setPidFeedforwardRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override
Set the feedforward value for the specified PID controller.
yarp::dev::ReturnValue getPidFeedforwardRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double &v) override
Get the currently set feedforward value for the specified PID controller.
yarp::dev::ReturnValue getCurrentRangesRaw(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
yarp::dev::ReturnValue getTrajSpeedRaw(int j, double *ref) override
Get reference speed for a joint.
bool open(yarp::os::Searchable &par) override
Open the DeviceDriver.
yarp::dev::ReturnValue getImpedanceOffsetRaw(int j, double *offset) override
Get current force Offset for a specific joint.
yarp::dev::ReturnValue resetMotorEncoderRaw(int m) override
Reset motor encoder, single motor.
yarp::dev::ReturnValue getRefCurrentsRaw(double *t) override
Get the reference value of the currents for all motors.
yarp::dev::ReturnValue getVelLimitsRaw(int axis, double *min, double *max) override
Get the software speed limits for a particular axis.
virtual bool getHasHallSensorRaw(int j, int &ret)
yarp::dev::ReturnValue getEncoderSpeedsRaw(double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue disablePidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Disable the PID computation for a joint.
yarp::dev::ReturnValue getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
Read the instantaneous position of a motor encoder.
yarp::dev::ReturnValue setPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double limit) override
Set the error limit for the controller on a specific joint.
yarp::dev::ReturnValue getMotorEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous position of all motor encoders.
virtual bool initialised()
yarp::dev::ReturnValue setTrajAccelerationsRaw(const double *accs) override
Set reference acceleration on all joints.
yarp::dev::ReturnValue setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
bool threadInit() override
Initialization method.
yarp::dev::ReturnValue calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
Start calibration, this method is very often platform specific.
yarp::dev::ReturnValue getRefDutyCyclesRaw(double *v) override
Gets the last reference sent using the setRefDutyCyclesRaw function.
yarp::dev::VAS_status getVirtualAnalogSensorStatusRaw(int ch) override
Check the status of a given channel.
yarp::dev::ReturnValue positionMoveRaw(int j, double ref) override
Set new reference point for a single axis.
yarp::dev::ReturnValue getMotorEncodersRaw(double *encs) override
Read the position of all motor encoders.
yarp::dev::ReturnValue 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).
yarp::dev::ReturnValue getLastJointFaultRaw(int j, int &fault, std::string &message) override
yarp::dev::ReturnValue setRefDutyCycleRaw(int j, double v) override
Sets the reference dutycycle of a single motor.
yarp::dev::ReturnValue setRefTorquesRaw(const double *t) override
Set the reference value of the torque for all joints.
int getVirtualAnalogSensorChannelsRaw() override
Get the number of channels of the virtual sensor.
yarp::dev::ReturnValue setMotorTorqueParamsRaw(int j, const yarp::dev::MotorTorqueParameters params) override
Set the motor parameters.
yarp::dev::ReturnValue getTrajSpeedsRaw(double *spds) override
Get reference speed of all joints.
yarp::dev::ReturnValue getTorqueRangesRaw(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
yarp::dev::ReturnValue setRefDutyCyclesRaw(const double *v) override
Sets the reference dutycycle for all motors.
yarp::dev::ReturnValue resetEncodersRaw() override
Reset encoders.
yarp::dev::ReturnValue getPidReferenceRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *ref) override
Get the current reference of the PID controller for a specific joint.
yarp::dev::ReturnValue getMotorEncoderAccelerationRaw(int m, double *spds) override
Read the instantaneous acceleration of a motor encoder.
yarp::dev::ReturnValue getTorqueRangeRaw(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
yarp::dev::ReturnValue getMotorTorqueParamsRaw(int j, yarp::dev::MotorTorqueParameters *params) override
Get the motor parameters.
yarp::dev::ReturnValue setRefTorqueRaw(int j, double t) override
Set the reference value of the torque for a given joint.
yarp::dev::ReturnValue setEncoderRaw(int j, double val) override
Set the value of the encoder for a given joint.
bool fromConfig(yarp::os::Searchable &config)
yarp::dev::ReturnValue getPidErrorsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
yarp::dev::ReturnValue setManualBrakeActiveRaw(int j, bool active) override
yarp::dev::ReturnValue setRefVelocityRaw(int jnt, double vel) override
Set the velocity of single joint.
yarp::dev::ReturnValue getRefTorquesRaw(double *t) override
Get the reference value of the torque for all joints.
yarp::dev::ReturnValue getAmpStatusRaw(int *st) override
yarp::dev::ReturnValue setMotorEncodersRaw(const double *vals) override
Set the value of all motor encoders.
yarp::dev::ReturnValue getRefPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getControlModeRaw(int j, yarp::dev::ControlModeEnum &mode) override
yarp::dev::ReturnValue setPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum &pidtype, const double *limits) override
Get the error limit for the controller on all joints.
yarp::dev::ReturnValue getTargetVelocityRaw(const int joint, double *ref) override
Get the last reference speed set by velocityMove for single joint.
virtual bool getHasRotorEncoderIndexRaw(int j, int &ret)
bool updateVirtualAnalogSensorMeasureRaw(yarp::sig::Vector &measure) override
Set a vector of torque values for virtual sensor.
yarp::dev::ReturnValue setTrajAccelerationRaw(int j, double acc) override
Set reference acceleration for a joint.
yarp::dev::ReturnValue getPidOutputsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
void resizeBuffers()
Resize previously allocated buffers.
yarp::dev::ReturnValue getGearboxRatioRaw(int m, double *gearbox) override
Get the gearbox ratio for a specific motor.
yarp::dev::ReturnValue isJointBrakedRaw(int j, bool &braked) const override
yarp::dev::ReturnValue getDutyCyclesRaw(double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
yarp::dev::ReturnValue getTargetPositionsRaw(double *refs) override
Get the last position reference for all axes.
void threadRelease() override
Release method.
yarp::dev::ReturnValue getCurrentsRaw(double *vals) override
yarp::dev::ReturnValue getEncodersRaw(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue getPidExtraInfosRaw(const yarp::dev::PidControlTypeEnum &pidtype, std::vector< yarp::dev::PidExtraInfo > &units) override
Get PID extra info and measurement units for a specific joint.
virtual bool getMotorPolesRaw(int j, int &poles)
yarp::dev::ReturnValue getTemperaturesRaw(double *vals) override
Get temperature of all the motors.
yarp::dev::ReturnValue getTargetPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getRefVelocityRaw(const int jnt, double &vel) override
Get the last reference velocity set by setRefVelocity() for a single joint.
yarp::dev::ReturnValue getNumberOfMotorsRaw(int *num) override
Get the number of available motors.
yarp::dev::ReturnValue setPidReferencesRaw(const yarp::dev::PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
yarp::dev::ReturnValue getEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of an axis.
yarp::dev::ReturnValue setEncodersRaw(const double *vals) override
Set the value of all encoders.
virtual bool getRotorIndexOffsetRaw(int j, double &rotorOffset)
yarp::dev::ReturnValue setRefCurrentRaw(int j, double t) override
Set the reference value of the current for a single motor.
yarp::dev::ReturnValue getTargetVelocitiesRaw(double *refs) override
Get the last reference speed set by velocityMove for all joints.
yarp::dev::ReturnValue checkMotionDoneRaw(bool &flag) override
Check if the current trajectory all joints is terminated.
yarp::dev::ReturnValue getDutyCycleRaw(int j, double *v) override
Gets the current dutycycle of the output of the amplifier (i.e.
yarp::dev::ReturnValue getMotorEncoderRaw(int m, double *v) override
Read the value of a motor encoder.
yarp::dev::ReturnValue getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
Get the current impedance limits for a specific joint.
yarp::dev::ReturnValue getRefCurrentRaw(int j, double *t) override
Get the reference value of the current for a single motor.
yarp::dev::ReturnValue getPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::Pid *pid) override
Get current PID value for a specific joint.
yarp::dev::ReturnValue getAutoBrakeEnabledRaw(int j, bool &enabled) const override
yarp::dev::ReturnValue getPowerSupplyVoltageRaw(int j, double *val) override
yarp::dev::ReturnValue setPWMLimitRaw(int j, const double val) override
virtual bool getKinematicMJRaw(int j, double &rotres)
yarp::dev::ReturnValue getRefTorqueRaw(int j, double *t) override
Set the reference value of the torque for a given joint.
virtual bool getJointEncoderResolutionRaw(int m, double &jntres)
yarp::dev::ReturnValue getPidReferencesRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all PID controllers.
yarp::dev::ReturnValue disableAmpRaw(int j) override
Disable the amplifier on a specific joint.
yarp::dev::ReturnValue setImpedanceOffsetRaw(int j, double offset) override
Set current force Offset for a specific joint.
yarp::dev::ReturnValue setCalibrationParametersRaw(int axis, const yarp::dev::CalibrationParameters &params) override
Start calibration, this method is very often platform specific.
yarp::dev::ReturnValue 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 alloc(int njoints)
Allocated buffers.
yarp::dev::ReturnValue getNumberOfMotorEncodersRaw(int *num) override
Get the number of available motor encoders.
yarp::dev::ReturnValue setPidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, const yarp::dev::Pid &pid) override
Set new PID value for a joint axis.
yarp::dev::ReturnValue getCurrentRaw(int j, double *val) override
yarp::dev::ReturnValue getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
yarp::dev::ReturnValue setPeakCurrentRaw(int m, const double val) override
virtual bool getRotorEncoderTypeRaw(int j, int &type)
yarp::dev::ReturnValue getEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getNominalCurrentRaw(int m, double *val) override
yarp::dev::ReturnValue calibrationDoneRaw(int j) override
Check if the calibration is terminated, on a particular joint.
yarp::dev::ReturnValue getPidErrorRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
yarp::dev::ReturnValue getTrajAccelerationRaw(int j, double *acc) override
Get reference acceleration for a joint.
yarp::dev::ReturnValue resetEncoderRaw(int j) override
Reset encoder, single joint.
yarp::dev::ReturnValue getTemperatureRaw(int m, double *val) override
Get temperature of a motor.
yarp::dev::ReturnValue getPidExtraInfoRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::PidExtraInfo &units) override
Get PID extra info and measurement units for a specific joint.
yarp::dev::ReturnValue setPidsRaw(const yarp::dev::PidControlTypeEnum &pidtype, const yarp::dev::Pid *pids) override
Set new PID value on multiple axes.
yarp::dev::ReturnValue setMaxCurrentRaw(int j, double val) override
yarp::dev::ReturnValue setControlModeRaw(int j, yarp::dev::SelectableControlModeEnum mode) override
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue relativeMoveRaw(int j, double delta) override
Set relative position.
yarp::dev::ReturnValue getMotorEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all motor encoders.
yarp::dev::ReturnValue getMotorEncoderSpeedRaw(int m, double *sp) override
Read the instantaneous speed of a motor encoder.
yarp::dev::ReturnValue resetMotorEncodersRaw() override
Reset motor encoders.
yarp::dev::ReturnValue getCurrentRangeRaw(int j, double *min, double *max) override
Get the full scale of the current measurement for a given motor (e.g.
yarp::dev::ReturnValue getMotorEncoderCountsPerRevolutionRaw(int m, double *v) override
Gets number of counts per revolution for motor encoder m.
yarp::dev::ReturnValue enablePidRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Enable the PID computation for a joint.
yarp::dev::ReturnValue getRefPositionsRaw(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue setTrajSpeedRaw(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
yarp::dev::ReturnValue getPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *limit) override
Get the error limit for the controller on a specific joint.
yarp::dev::ReturnValue setNominalCurrentRaw(int m, const double val) override
yarp::dev::ReturnValue setControlModesRaw(const std::vector< int > &joints, const std::vector< yarp::dev::SelectableControlModeEnum > &modes) override
yarp::dev::ReturnValue setRefCurrentsRaw(const double *t) override
Set the reference value of the currents for all motors.
yarp::dev::ReturnValue getPWMRaw(int j, double *val) override
yarp::dev::ReturnValue setInteractionModeRaw(int j, yarp::dev::InteractionModeEnum _mode) override
Set the interaction mode of the robot, values can be stiff or compliant.
yarp::dev::ReturnValue getTorquesRaw(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
yarp::dev::ReturnValue getAvailableControlModesRaw(int j, std::vector< yarp::dev::SelectableControlModeEnum > &avail) override
yarp::dev::ReturnValue enableAmpRaw(int j) override
Enable the amplifier on a specific joint.
yarp::dev::ReturnValue getPeakCurrentRaw(int m, double *val) override
yarp::dev::ReturnValue getControlModesRaw(const std::vector< int > &joints, std::vector< yarp::dev::ControlModeEnum > &modes) override
yarp::dev::ReturnValue setGearboxRatioRaw(int m, const double val) override
Set the gearbox ratio for a specific motor.
virtual bool getRotorEncoderResolutionRaw(int m, double &rotres)
virtual bool getHasRotorEncoderRaw(int j, int &ret)
yarp::dev::ReturnValue getPidsRaw(const yarp::dev::PidControlTypeEnum &pidtype, yarp::dev::Pid *pids) override
Get current PID value for a specific joint.
yarp::dev::ReturnValue getPWMLimitRaw(int j, double *val) override
yarp::dev::ReturnValue getAxisNameRaw(int axis, std::string &name) override
void run() override
Loop function.
yarp::dev::ReturnValue stopRaw() override
Stop motion, multiple joints.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue getEncoderRaw(int j, double *v) override
Read the value of an encoder.
virtual bool getTorqueControlFilterType(int j, int &type)
yarp::dev::ReturnValue getTrajAccelerationsRaw(double *accs) override
Get reference acceleration of all joints.
yarp::dev::ReturnValue getAvailablePidsRaw(int j, std::vector< yarp::dev::PidControlTypeEnum > &avail) override
Return a list of available pid types mode for the given joint.
yarp::dev::ReturnValue setPositionRaw(int j, double ref) override
Set new position for a single axis.
yarp::dev::ReturnValue getPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum &pidtype, double *limits) override
Get the error limit for all controllers.
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 uninitialize()
Clean up internal data and memory.
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)
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 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)
double kp
proportional gain
Contains the parameters for a PID.
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 zero()
Set to zero the elements of the vector.
Definition VectorBase.h:296
void resize(size_t size)
Resize the vector.
Definition VectorBase.h:106
#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:65
std::int32_t vocab32_t
Definition numeric.h:78
For streams capable of holding different kinds of content, check what they actually have.
SelectableControlModeEnum
size_t PidControlTypeEnum2Index(PidControlTypeEnum type)
Definition PidEnums.h:46
PidControlTypeEnum
Definition PidEnums.h:15
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 Vocab32.cpp:33
The components from which ports and connections are built.
An interface to the operating system, including Port based communication.