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 size_t index = PidControlTypeEnum2Index(pidtype);
1754 switch (pidtype)
1755 {
1756 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
1757 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
1758 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
1759 *out=1.1 + j * 10;
1760 break;
1761 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
1762 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
1763 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
1764 *out=1.1 + j * 10;
1765 break;
1766 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
1767 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
1768 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
1769 *out=1.2 + j * 10;
1770 break;
1771 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
1772 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
1773 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
1774 *out=1.2 + j * 10;
1775 break;
1776 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
1777 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
1778 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
1779 *out=1.2 + j * 10;
1780 break;
1781 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
1782 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
1783 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
1784 *out=1.3 + j * 10;
1785 break;
1786 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
1787 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
1788 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
1789 *out=1.4 + j * 10;
1790 break;
1791 default:
1792 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
1793 return ReturnValue::return_code::return_value_error_method_failed;
1794 break;
1795 }
1796 yCDebug(FAKEMOTIONCONTROL) << "getPidOutputRaw" << (yarp::conf::vocab32_t)(pidtype) << j << *out;
1797 return ReturnValue_ok;
1798}
1799
1801{
1803 for(int j=0; j< _njoints; j++)
1804 {
1805 ret &= getPidOutputRaw(pidtype, j, &outs[j]);
1806 }
1807 return ret;
1808}
1809
1811// Velocity control interface raw //
1813
1815{
1817 auto ret = getControlModeRaw(j, mode);
1822 {
1823 yCError(FAKEMOTIONCONTROL) << "velocityMoveRaw: skipping command because board " << " joint " << j << " is not in VOCAB_CM_VELOCITY mode";
1824 }
1825 _command_speeds[j] = sp;
1826 last_velocity_command[j]=yarp::os::Time::now();
1827 return ReturnValue_ok;
1828}
1829
1831{
1834 for (int i = 0; i < _njoints; i++) {
1835 ret &= velocityMoveRaw(i, sp[i]);
1836 }
1837 return ret;
1838}
1839
1840
1842// Calibration control interface //
1844
1846{
1847 yCTrace(FAKEMOTIONCONTROL) << "setCalibrationParametersRaw for joint" << j;
1848 return ReturnValue_ok;
1849}
1850
1851ReturnValue FakeMotionControl::calibrateAxisWithParamsRaw(int j, unsigned int type, double p1, double p2, double p3)
1852{
1853 yCTrace(FAKEMOTIONCONTROL) << "calibrateRaw for joint" << j;
1854 return ReturnValue_ok;
1855}
1856
1861
1863// Position control interface //
1865
1867{
1868 ax=_njoints;
1869
1870 return ReturnValue_ok;
1871}
1872
1874{
1875 if (verbose >= VERY_VERBOSE) {
1876 yCTrace(FAKEMOTIONCONTROL) << "j " << j << " ref " << ref;
1877 }
1878
1879// if (yarp::os::Time::now()-_last_position_move_time[j]<MAX_POSITION_MOVE_INTERVAL)
1880// {
1881// 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.";
1882// }
1883// _last_position_move_time[j] = yarp::os::Time::now();
1884
1886 auto ret = getControlModeRaw(j, mode);
1891 {
1892 yCError(FAKEMOTIONCONTROL) << "positionMoveRaw: skipping command because joint " << j << " is not in VOCAB_CM_POSITION mode";
1893 }
1894 _posCtrl_references[j] = ref;
1895 return ReturnValue_ok;
1896}
1897
1899{
1900 if (verbose >= VERY_VERBOSE) {
1902 }
1903
1905 for(int j=0, index=0; j< _njoints; j++, index++)
1906 {
1907 ret &= positionMoveRaw(j, refs[index]);
1908 }
1909 return ret;
1910}
1911
1913{
1914 if (verbose >= VERY_VERBOSE) {
1915 yCTrace(FAKEMOTIONCONTROL) << "j " << j << " ref " << delta;
1916 }
1917// if (yarp::os::Time::now()-_last_position_move_time[j]<MAX_POSITION_MOVE_INTERVAL)
1918// {
1919// 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.";
1920// }
1921// _last_position_move_time[j] = yarp::os::Time::now();
1922
1924 auto ret = getControlModeRaw(j, mode);
1929 {
1930 yCError(FAKEMOTIONCONTROL) << "relativeMoveRaw: skipping command because joint " << j << " is not in VOCAB_CM_POSITION mode";
1931 }
1932 _posCtrl_references[j] += delta;
1933 return ReturnValue_ok;
1934}
1935
1937{
1938 if (verbose >= VERY_VERBOSE) {
1940 }
1941
1943 for(int j=0, index=0; j< _njoints; j++, index++)
1944 {
1945 ret &= relativeMoveRaw(j, deltas[index]);
1946 }
1947 return ret;
1948}
1949
1950
1952{
1953 if (verbose >= VERY_VERBOSE) {
1954 yCTrace(FAKEMOTIONCONTROL) << "j ";
1955 }
1956
1957 flag = false;
1958 return ReturnValue_ok;
1959}
1960
1962{
1963 if (verbose >= VERY_VERBOSE) {
1965 }
1966
1968 bool val, tot_res = true;
1969
1970 for(int j=0, index=0; j< _njoints; j++, index++)
1971 {
1972 ret &= checkMotionDoneRaw(j, val);
1973 tot_res &= val;
1974 }
1975 flag = tot_res;
1976 return ret;
1977}
1978
1980{
1981 // Velocity is expressed in iDegrees/s
1982 // save internally the new value of speed; it'll be used in the positionMove
1983 int index = j ;
1984 _ref_speeds[index] = sp;
1985 return ReturnValue_ok;
1986}
1987
1989{
1990 // Velocity is expressed in iDegrees/s
1991 // save internally the new value of speed; it'll be used in the positionMove
1992 for(int j=0, index=0; j< _njoints; j++, index++)
1993 {
1994 _ref_speeds[index] = spds[index];
1995 }
1996 return ReturnValue_ok;
1997}
1998
2000{
2001 // Acceleration is expressed in iDegrees/s^2
2002 // save internally the new value of the acceleration; it'll be used in the velocityMove command
2003
2004 if (acc > 1e6)
2005 {
2006 _ref_accs[j ] = 1e6;
2007 }
2008 else if (acc < -1e6)
2009 {
2010 _ref_accs[j ] = -1e6;
2011 }
2012 else
2013 {
2014 _ref_accs[j ] = acc;
2015 }
2016
2017 return ReturnValue_ok;
2018}
2019
2021{
2022 // Acceleration is expressed in iDegrees/s^2
2023 // save internally the new value of the acceleration; it'll be used in the velocityMove command
2024 for(int j=0, index=0; j< _njoints; j++, index++)
2025 {
2026 if (accs[j] > 1e6)
2027 {
2028 _ref_accs[index] = 1e6;
2029 }
2030 else if (accs[j] < -1e6)
2031 {
2032 _ref_accs[index] = -1e6;
2033 }
2034 else
2035 {
2036 _ref_accs[index] = accs[j];
2037 }
2038 }
2039 return ReturnValue_ok;
2040}
2041
2043{
2044 *spd = _ref_speeds[j];
2045 return ReturnValue_ok;
2046}
2047
2049{
2050 memcpy(spds, _ref_speeds, sizeof(double) * _njoints);
2051 return ReturnValue_ok;
2052}
2053
2055{
2056 *acc = _ref_accs[j];
2057 return ReturnValue_ok;
2058}
2059
2061{
2062 memcpy(accs, _ref_accs, sizeof(double) * _njoints);
2063 return ReturnValue_ok;
2064}
2065
2071
2073{
2075 for(int j=0; j< _njoints; j++)
2076 {
2077 ret &= stopRaw(j);
2078 }
2079 return ret;
2080}
2082
2084// Position control2 interface //
2086
2087ReturnValue FakeMotionControl::positionMoveRaw(const int n_joint, const int *joints, const double *refs)
2088{
2089 if (verbose >= VERY_VERBOSE) {
2090 yCTrace(FAKEMOTIONCONTROL) << " -> n_joint " << n_joint;
2091 }
2092
2093 for(int j=0; j<n_joint; j++)
2094 {
2095 yCDebug(FAKEMOTIONCONTROL, "j: %d; ref %f;\n", joints[j], refs[j]); fflush(stdout);
2096 }
2097
2099 for(int j=0; j<n_joint; j++)
2100 {
2101 ret = ret &&positionMoveRaw(joints[j], refs[j]);
2102 }
2103 return ret;
2104}
2105
2106ReturnValue FakeMotionControl::relativeMoveRaw(const int n_joint, const int *joints, const double *deltas)
2107{
2108 if (verbose >= VERY_VERBOSE) {
2109 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2110 }
2111
2113 for(int j=0; j<n_joint; j++)
2114 {
2115 ret = ret &&relativeMoveRaw(joints[j], deltas[j]);
2116 }
2117 return ret;
2118}
2119
2120ReturnValue FakeMotionControl::checkMotionDoneRaw(const std::vector<int>& joints, bool&flag)
2121{
2122 if (verbose >= VERY_VERBOSE) {
2123 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2124 }
2125
2127 bool val = true;
2128 bool tot_val = true;
2129
2130 for(int j=0; j<joints.size(); j++)
2131 {
2132 ret = ret && checkMotionDoneRaw(joints[j], val);
2133 tot_val &= val;
2134 }
2135 flag = tot_val;
2136 return ret;
2137}
2138
2139ReturnValue FakeMotionControl::setTrajSpeedsRaw(const int n_joint, const int *joints, const double *spds)
2140{
2141 if (verbose >= VERY_VERBOSE) {
2142 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2143 }
2144
2146 for(int j=0; j<n_joint; j++)
2147 {
2148 ret = ret &&setTrajSpeedRaw(joints[j], spds[j]);
2149 }
2150 return ret;
2151}
2152
2153ReturnValue FakeMotionControl::setTrajAccelerationsRaw(const int n_joint, const int *joints, const double *accs)
2154{
2155 if (verbose >= VERY_VERBOSE) {
2156 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2157 }
2158
2160 for(int j=0; j<n_joint; j++)
2161 {
2162 ret = ret &&setTrajAccelerationRaw(joints[j], accs[j]);
2163 }
2164 return ret;
2165}
2166
2167ReturnValue FakeMotionControl::getTrajSpeedsRaw(const int n_joint, const int *joints, double *spds)
2168{
2169 if (verbose >= VERY_VERBOSE) {
2170 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2171 }
2172
2174 for(int j=0; j<n_joint; j++)
2175 {
2176 ret = ret && getTrajSpeedRaw(joints[j], &spds[j]);
2177 }
2178 return ret;
2179}
2180
2181ReturnValue FakeMotionControl::getTrajAccelerationsRaw(const int n_joint, const int *joints, double *accs)
2182{
2183 if (verbose >= VERY_VERBOSE) {
2184 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2185 }
2186
2188 for(int j=0; j<n_joint; j++)
2189 {
2190 ret = ret && getTrajAccelerationRaw(joints[j], &accs[j]);
2191 }
2192 return ret;
2193}
2194
2196{
2197 if (verbose >= VERY_VERBOSE) {
2198 yCTrace(FAKEMOTIONCONTROL) << "n_joint " << _njoints;
2199 }
2200
2202 for(int j=0; j<n_joint; j++)
2203 {
2204 ret = ret &&stopRaw(joints[j]);
2205 }
2206 return ret;
2207}
2208
2210
2211// ControlMode
2212
2213ReturnValue FakeMotionControl::getAvailableControlModesRaw(int j, std::vector<yarp::dev::SelectableControlModeEnum>& avail)
2214{
2215 if (verbose > VERY_VERY_VERBOSE) {
2216 yCTrace(FAKEMOTIONCONTROL) << "j: " << j;
2217 }
2218
2219 avail = _availableControlModes[j];
2220 return ReturnValue_ok;
2221}
2222
2224{
2225 if (verbose > VERY_VERY_VERBOSE) {
2226 yCTrace(FAKEMOTIONCONTROL) << "j: " << j;
2227 }
2228
2229 mode = (yarp::dev::ControlModeEnum)_controlModes[j];
2230 return ReturnValue_ok;
2231}
2232
2233// IControl Mode 2
2234ReturnValue FakeMotionControl::getControlModesRaw(std::vector<yarp::dev::ControlModeEnum>& mode)
2235{
2237 for(int j=0; j< _njoints; j++)
2238 {
2239 ret = ret && getControlModeRaw(j, mode[j]);
2240 }
2241 return ret;
2242}
2243
2244ReturnValue FakeMotionControl::getControlModesRaw(const std::vector<int>& joints, std::vector<yarp::dev::ControlModeEnum>& modes)
2245{
2247 for(int j=0; j< joints.size(); j++)
2248 {
2249 ret = ret && getControlModeRaw(joints[j], modes[j]);
2250 }
2251 return ret;
2252}
2253
2255{
2256 if (verbose >= VERY_VERBOSE) {
2257 yCTrace(FAKEMOTIONCONTROL) << "j: " << j << " mode: " << yarp::os::Vocab32::decode((int)mode);
2258 }
2259
2261 {
2262 _controlModes[j] = VOCAB_CM_IDLE;
2263 }
2264 else
2265 {
2266 _controlModes[j] = (int)mode;
2267 }
2268 _posCtrl_references[j] = pos[j];
2269 return ReturnValue_ok;
2270}
2271
2272
2273ReturnValue FakeMotionControl::setControlModesRaw(const std::vector<int>& joints, const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
2274{
2275 if (verbose >= VERY_VERBOSE) {
2276 yCTrace(FAKEMOTIONCONTROL) << "n_joints: " << joints.size();
2277 }
2278
2280 for(int i=0; i<joints.size(); i++)
2281 {
2282 ret &= setControlModeRaw(joints[i], modes[i]);
2283 }
2284 return ret;
2285}
2286
2287ReturnValue FakeMotionControl::setControlModesRaw(const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
2288{
2289 if (verbose >= VERY_VERBOSE) {
2291 }
2292
2294 for(int i=0; i<_njoints; i++)
2295 {
2296 ret &= setControlModeRaw(i, modes[i]);
2297 }
2298 return ret;
2299}
2300
2301
2303
2305{
2306 pos[j] = val;
2307 return ReturnValue_ok;
2308}
2309
2311{
2312 for(int j=0; j< _njoints; j++)
2313 {
2314 pos[j] = vals[j];
2315 }
2316 return ReturnValue_ok;
2317}
2318
2320{
2321 pos[j] = 0;
2322 return ReturnValue_ok;
2323}
2324
2326{
2327 for(int j=0; j< _njoints; j++)
2328 {
2329 pos[j] = 0;
2330 }
2331 return ReturnValue_ok;
2332}
2333
2335{
2337
2338 // To simulate a real controlboard, we assume that the joint
2339 // encoders is exactly the last set by setPosition(s) or positionMove
2340 *value = pos[j];
2341
2342 return ret;
2343}
2344
2346{
2348 for(int j=0; j< _njoints; j++)
2349 {
2350 ReturnValue ok = getEncoderRaw(j, &encs[j]);
2351 ret = ret && ok;
2352
2353 }
2354 return ret;
2355}
2356
2358{
2359 // To avoid returning uninitialized memory, we set the encoder speed to 0
2360 *sp = 0.0;
2361 return ReturnValue_ok;
2362}
2363
2365{
2367 for(int j=0; j< _njoints; j++)
2368 {
2369 ret &= getEncoderSpeedRaw(j, &spds[j]);
2370 }
2371 return ret;
2372}
2373
2375{
2376 // To avoid returning uninitialized memory, we set the encoder acc to 0
2377 *acc = 0.0;
2378
2379 return ReturnValue_ok;
2380}
2381
2383{
2385 for(int j=0; j< _njoints; j++)
2386 {
2387 ret &= getEncoderAccelerationRaw(j, &accs[j]);
2388 }
2389 return ret;
2390}
2391
2393
2395{
2396 ReturnValue ret = getEncodersRaw(encs);
2397 _mutex.lock();
2398 for (int i = 0; i < _njoints; i++) {
2399 stamps[i] = _encodersStamp[i] = _cycleTimestamp;
2400 }
2401 _mutex.unlock();
2402 return ret;
2403}
2404
2405ReturnValue FakeMotionControl::getEncoderTimedRaw(int j, double *encs, double *stamp)
2406{
2407 ReturnValue ret = getEncoderRaw(j, encs);
2408 _mutex.lock();
2409 *stamp = _encodersStamp[j] = _cycleTimestamp;
2410 _mutex.unlock();
2411
2412 return ret;
2413}
2414
2416
2418{
2419 *num=_njoints;
2420 return ReturnValue_ok;
2421}
2422
2427
2432
2437
2442
2447
2452
2454{
2455 *value = pos[m]*10;
2456 return ReturnValue_ok;
2457}
2458
2460{
2462 for(int j=0; j< _njoints; j++)
2463 {
2464 ret &= getMotorEncoderRaw(j, &encs[j]);
2465
2466 }
2467 return ret;
2468}
2469
2471{
2472 *sp = 0.0;
2473 return ReturnValue_ok;
2474}
2475
2477{
2479 for(int j=0; j< _njoints; j++)
2480 {
2481 ret &= getMotorEncoderSpeedRaw(j, &spds[j]);
2482 }
2483 return ret;
2484}
2485
2487{
2488 *acc = 0.0;
2489 return ReturnValue_ok;
2490}
2491
2493{
2495 for(int j=0; j< _njoints; j++)
2496 {
2497 ret &= getMotorEncoderAccelerationRaw(j, &accs[j]);
2498 }
2499 return ret;
2500}
2501
2503{
2504 ReturnValue ret = getMotorEncodersRaw(encs);
2505 _mutex.lock();
2506 for (int i = 0; i < _njoints; i++) {
2507 stamps[i] = _encodersStamp[i] = _cycleTimestamp;
2508 }
2509 _mutex.unlock();
2510
2511 return ret;
2512}
2513
2515{
2516 ReturnValue ret = getMotorEncoderRaw(m, encs);
2517 _mutex.lock();
2518 *stamp = _encodersStamp[m] = _cycleTimestamp;
2519 _mutex.unlock();
2520
2521 return ret;
2522}
2524
2526
2531
2536
2538{
2539 //just for testing purposes, this is not a real implementation
2540 *value = current[j];
2541 return ReturnValue_ok;
2542}
2543
2545{
2546 //just for testing purposes, this is not a real implementation
2548 for(int j=0; j< _njoints; j++)
2549 {
2550 ret &= getCurrentRaw(j, &vals[j]);
2551 }
2552 return ret;
2553}
2554
2556{
2557 maxCurrent[m] = val;
2558 return ReturnValue_ok;
2559}
2560
2562{
2563 *val = maxCurrent[m];
2564 return ReturnValue_ok;
2565}
2566
2568{
2569 (_enabledAmp[j ]) ? *st = 1 : *st = 0;
2570 return ReturnValue_ok;
2571}
2572
2574{
2576 for(int j=0; j<_njoints; j++)
2577 {
2578 sts[j] = _enabledAmp[j];
2579 }
2580 return ReturnValue_ok;
2581}
2582
2584{
2585 *val = peakCurrent[m];
2586 return ReturnValue_ok;
2587}
2588
2590{
2591 peakCurrent[m] = val;
2592 return ReturnValue_ok;
2593}
2594
2596{
2597 *val = nominalCurrent[m];
2598 return ReturnValue_ok;
2599}
2600
2602{
2603 nominalCurrent[m] = val;
2604 return ReturnValue_ok;
2605}
2606
2608{
2609 *val = pwm[m];
2610 return ReturnValue_ok;
2611}
2612
2614{
2615 *val = pwmLimit[m];
2616 return ReturnValue_ok;
2617}
2618
2620{
2621 pwmLimit[m] = val;
2622 return ReturnValue_ok;
2623}
2624
2626{
2627 *val = supplyVoltage[m];
2628 return ReturnValue_ok;
2629}
2630
2631
2632// Limit interface
2634{
2635 _limitsMin[j] = min;
2636 _limitsMax[j] = max;
2637 return ReturnValue_ok;
2638}
2639
2640ReturnValue FakeMotionControl::getPosLimitsRaw(int j, double *min, double *max)
2641{
2642 *min = _limitsMin[j];
2643 *max = _limitsMax[j];
2644 return ReturnValue_ok;
2645}
2646
2648{
2649 *gearbox = _gearbox[j];
2650 return ReturnValue_ok;
2651}
2652
2654{
2655 _gearbox[j] = val;
2656 return ReturnValue_ok;
2657}
2658
2663
2668
2673
2675{
2677}
2678
2680{
2682}
2683
2688
2690{
2692}
2693
2695{
2697}
2698
2700{
2702}
2703
2708
2713
2718
2720{
2721 name = _axisName[j];
2722 return ReturnValue_ok;
2723}
2724
2726{
2727 type = _jointType[j];
2728 return ReturnValue_ok;
2729}
2730
2731// IControlLimits
2732ReturnValue FakeMotionControl::setVelLimitsRaw(int axis, double min, double max)
2733{
2734 _minJntCmdVelocity[axis] = min;
2735 _maxJntCmdVelocity[axis] = max;
2736 return ReturnValue_ok;
2737}
2738
2739ReturnValue FakeMotionControl::getVelLimitsRaw(int axis, double *min, double *max)
2740{
2741 *min = _minJntCmdVelocity[axis];
2742 *max = _maxJntCmdVelocity[axis];
2743 return ReturnValue_ok;
2744}
2745
2746
2747// Torque control
2749{
2750 *t = _torques[j];
2751 return ReturnValue_ok;
2752}
2753
2755{
2756 for (int i = 0; i < _njoints; i++)
2757 {
2758 t[i]= _torques[i];
2759 }
2760 return ReturnValue_ok;
2761}
2762
2764{
2765 *min = -100;
2766 *max = 100;
2767 return ReturnValue_ok;
2768}
2769
2771{
2773 for (int j = 0; j < _njoints && ret; j++) {
2774 ret &= getTorqueRangeRaw(j, &min[j], &max[j]);
2775 }
2776 return ret;
2777}
2778
2780{
2782 for (int j = 0; j < _njoints && ret; j++) {
2783 ret &= setRefTorqueRaw(j, t[j]);
2784 }
2785 return ret;
2786}
2787
2789{
2790 _mutex.lock();
2791 _ref_torques[j]=t;
2792
2793 if (t>1.0 || t< -1.0)
2794 {
2795 yCError(FAKEMOTIONCONTROL) << "Joint received a high torque command, and was put in hardware fault";
2796 _hwfault_code[j] = 1;
2797 _hwfault_message[j] = "test" + std::to_string(j) + " torque";
2798 _controlModes[j] = VOCAB_CM_HW_FAULT;
2799 }
2800 _mutex.unlock();
2801 return ReturnValue_ok;
2802}
2803
2804ReturnValue FakeMotionControl::setRefTorquesRaw(const int n_joint, const int *joints, const double *t)
2805{
2807 for (int j = 0; j < n_joint && ret; j++) {
2808 ret &= setRefTorqueRaw(joints[j], t[j]);
2809 }
2810 return ret;
2811}
2812
2814{
2816 for (int j = 0; j < _njoints && ret; j++) {
2817 ret &= getRefTorqueRaw(j, &_ref_torques[j]);
2818 }
2819 return ReturnValue_ok;
2820}
2821
2823{
2824 _mutex.lock();
2825 *t = _ref_torques[j];
2826 _mutex.unlock();
2827 return ReturnValue_ok;
2828}
2829
2830ReturnValue FakeMotionControl::getImpedanceRaw(int j, double *stiffness, double *damping)
2831{
2832 _mutex.lock();
2833 *stiffness = _stiffness[j];
2834 *damping = _damping[j];
2835 _mutex.unlock();
2836 return ReturnValue_ok;
2837}
2838
2839ReturnValue FakeMotionControl::setImpedanceRaw(int j, double stiffness, double damping)
2840{
2841 _mutex.lock();
2842 _stiffness[j] = stiffness;
2843 _damping[j] = damping;
2844 _mutex.unlock();
2845 return ReturnValue_ok;
2846}
2847
2849{
2850 _mutex.lock();
2851 _force_offset[j] = offset;
2852 _mutex.unlock();
2853 return ReturnValue_ok;
2854}
2855
2857{
2858 _mutex.lock();
2859 *offset = _force_offset[j];
2860 _mutex.unlock();
2861 return ReturnValue_ok;
2862}
2863
2864ReturnValue FakeMotionControl::getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
2865{
2866 *min_stiff=1.0;
2867 *max_stiff=10.0;
2868 *min_damp=2.0;
2869 *max_damp=20.0;
2870 return ReturnValue_ok;
2871}
2872
2874{
2875 params->bemf = _kbemf[j];
2876 params->bemf_scale = _kbemf_scale[j];
2877 params->ktau = _ktau[j];
2878 params->ktau_scale = _ktau_scale[j];
2879 params->viscousPos = _viscousPos[j];
2880 params->viscousNeg = _viscousNeg[j];
2881 params->coulombPos = _coulombPos[j];
2882 params->coulombNeg = _coulombNeg[j];
2883 params->velocityThres = _velocityThres[j];
2884 yCDebug(FAKEMOTIONCONTROL) << "getMotorTorqueParamsRaw" << params->bemf
2885 << params->bemf_scale
2886 << params->ktau
2887 << params->ktau_scale
2888 << params->viscousPos
2889 << params->viscousNeg
2890 << params->coulombPos
2891 << params->coulombNeg
2892 << params->velocityThres;
2893 return ReturnValue_ok;
2894}
2895
2897{
2898 _kbemf[j] = params.bemf;
2899 _ktau[j] = params.ktau;
2900 _kbemf_scale[j] = params.bemf_scale;
2901 _ktau_scale[j] = params.ktau_scale;
2902 _viscousPos[j] = params.viscousPos;
2903 _viscousNeg[j] = params.viscousNeg;
2904 _coulombPos[j] = params.coulombPos;
2905 _coulombNeg[j] = params.coulombNeg;
2906 _velocityThres[j] = params.velocityThres;
2907 yCDebug(FAKEMOTIONCONTROL) << "setMotorTorqueParamsRaw" << params.bemf
2908 << params.bemf_scale
2909 << params.ktau
2910 << params.ktau_scale
2911 << params.viscousPos
2912 << params.viscousNeg
2913 << params.coulombPos
2914 << params.coulombNeg
2915 << params.velocityThres;
2916 return ReturnValue_ok;
2917}
2918
2919// IVelocityControl interface
2920ReturnValue FakeMotionControl::velocityMoveRaw(const int n_joint, const int *joints, const double *spds)
2921{
2923 for(int i=0; i<n_joint; i++)
2924 {
2925 ret &= velocityMoveRaw(joints[i], spds[i]);
2926 }
2927 return ret;
2928}
2929
2930// PositionDirect Interface
2932{
2933 _posDir_references[j] = ref;
2934 return ReturnValue_ok;
2935}
2936
2937ReturnValue FakeMotionControl::setPositionsRaw(const int n_joint, const int *joints, const double *refs)
2938{
2939 for(int i=0; i< n_joint; i++)
2940 {
2941 _posDir_references[joints[i]] = refs[i];
2942 }
2943 return ReturnValue_ok;
2944}
2945
2947{
2948 for(int i=0; i< _njoints; i++)
2949 {
2950 _posDir_references[i] = refs[i];
2951 }
2952 return ReturnValue_ok;
2953}
2954
2955
2957{
2958 if (verbose >= VERY_VERBOSE) {
2959 yCTrace(FAKEMOTIONCONTROL) << "j " << axis << " ref " << _posCtrl_references[axis];
2960 }
2961
2963 auto ret = getControlModeRaw(axis, mode);
2967 {
2968 yCWarning(FAKEMOTIONCONTROL) << "getTargetPosition: Joint " << axis << " is not in POSITION mode, therefore the value returned by " <<
2969 "this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
2970 }
2971 *ref = _posCtrl_references[axis];
2972 return ReturnValue_ok;
2973}
2974
2976{
2978 for (int i = 0; i < _njoints; i++) {
2979 ret &= getTargetPositionRaw(i, &refs[i]);
2980 }
2981 return ret;
2982}
2983
2985{
2987 for (int i = 0; i<nj; i++)
2988 {
2989 ret &= getTargetPositionRaw(jnts[i], &refs[i]);
2990 }
2991 return ret;
2992}
2993
2995{
2996 *ref = _command_speeds[axis];
2997 return ReturnValue_ok;
2998}
2999
3001{
3003 for (int i = 0; i<_njoints; i++)
3004 {
3005 ret &= getTargetVelocityRaw(i, &refs[i]);
3006 }
3007 return ret;
3008}
3009
3011{
3013 for (int i = 0; i<nj; i++)
3014 {
3015 ret &= getTargetVelocityRaw(jnts[i], &refs[i]);
3016 }
3017 return ret;
3018}
3019
3021{
3023 auto ret = getControlModeRaw(axis, mode);
3028 {
3029 yCWarning(FAKEMOTIONCONTROL) << "getRefPosition: Joint " << axis << " is not in POSITION_DIRECT mode, therefore the value returned by \
3030 this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
3031 }
3032
3033 *ref = _posDir_references[axis];
3034
3035 return ReturnValue_ok;
3036}
3037
3039{
3041 for (int i = 0; i<_njoints; i++)
3042 {
3043 ret &= getRefPositionRaw(i, &refs[i]);
3044 }
3045 return ret;
3046}
3047
3049{
3051 for (int i = 0; i<nj; i++)
3052 {
3053 ret &= getRefPositionRaw(jnts[i], &refs[i]);
3054 }
3055 return ret;
3056}
3057
3058
3059// InteractionMode
3061{
3062 if (verbose > VERY_VERY_VERBOSE) {
3063 yCTrace(FAKEMOTIONCONTROL) << "j: " << j;
3064 }
3065
3066 _mode = (yarp::dev::InteractionModeEnum)_interactMode[j];
3067 return ReturnValue_ok;}
3068
3069ReturnValue FakeMotionControl::getInteractionModesRaw(const std::vector<int>& joints, std::vector<yarp::dev::InteractionModeEnum>& modes)
3070{
3072 for(int j=0; j< joints.size(); j++)
3073 {
3074 ret = ret && getInteractionModeRaw(joints[j], modes[j]);
3075 }
3076 return ret;
3077
3078}
3079
3080ReturnValue FakeMotionControl::getInteractionModesRaw(std::vector<yarp::dev::InteractionModeEnum>& modes)
3081{
3083 for (int j = 0; j < _njoints; j++) {
3084 ret = ret && getInteractionModeRaw(j, modes[j]);
3085 }
3086 return ret;
3087}
3088
3090{
3091 if (verbose >= VERY_VERBOSE) {
3092 yCTrace(FAKEMOTIONCONTROL) << "j: " << j << " interaction mode: " << yarp::os::Vocab32::decode(_mode);
3093 }
3094
3095 _interactMode[j] = _mode;
3096
3097 return ReturnValue_ok;
3098}
3099
3100
3101ReturnValue FakeMotionControl::setInteractionModesRaw(const std::vector<int>& joints, const std::vector<yarp::dev::InteractionModeEnum>& modes)
3102{
3104 for(int i=0; i<joints.size(); i++)
3105 {
3106 ret &= setInteractionModeRaw(joints[i], modes[i]);
3107 }
3108 return ret;
3109}
3110
3111ReturnValue FakeMotionControl::setInteractionModesRaw(const std::vector<yarp::dev::InteractionModeEnum>& modes)
3112{
3114 for(int i=0; i<_njoints; i++)
3115 {
3116 ret &= setInteractionModeRaw(i, modes[i]);
3117 }
3118 return ret;
3119
3120}
3121
3123{
3124 *num=_njoints;
3125 return ReturnValue_ok;
3126}
3127
3129{
3130 *val = 37.5+double(m);
3131 return ReturnValue_ok;
3132}
3133
3135{
3137 for(int j=0; j< _njoints; j++)
3138 {
3139 ret &= getTemperatureRaw(j, &vals[j]);
3140 }
3141 return ret;
3142}
3143
3145{
3146 *temp = _tempLimit[m];
3147 return ReturnValue_ok;
3148}
3149
3151{
3152 _tempLimit[m] = temp;
3153 return ReturnValue_ok;
3154}
3155
3156//PWM interface
3158{
3159 refpwm[j] = v;
3160 pwm[j] = v;
3161 last_pwm_command[j]=yarp::os::Time::now();
3162 return ReturnValue_ok;
3163}
3164
3166{
3167 for (int i = 0; i < _njoints; i++)
3168 {
3169 setRefDutyCycleRaw(i,v[i]);
3170 }
3171 return ReturnValue_ok;
3172}
3173
3175{
3176 *v = refpwm[j];
3177 return ReturnValue_ok;
3178}
3179
3181{
3182 for (int i = 0; i < _njoints; i++)
3183 {
3184 v[i] = refpwm[i];
3185 }
3186 return ReturnValue_ok;
3187}
3188
3190{
3191 *v = pwm[j];
3192 return ReturnValue_ok;
3193}
3194
3196{
3197 for (int i = 0; i < _njoints; i++)
3198 {
3199 v[i] = pwm[i];
3200 }
3201 return ReturnValue_ok;
3202}
3203
3204// Current interface
3205/*bool FakeMotionControl::getCurrentRaw(int j, double *t)
3206{
3207 return NOT_YET_IMPLEMENTED("getCurrentRaw");
3208}
3209
3210bool FakeMotionControl::getCurrentsRaw(double *t)
3211{
3212 return NOT_YET_IMPLEMENTED("getCurrentsRaw");
3213}
3214*/
3215
3217{
3218 //just for testing purposes, this is not a real implementation
3219 *min = -3.5;
3220 *max = +3.5;
3221 return ReturnValue_ok;
3222}
3223
3225{
3226 //just for testing purposes, this is not a real implementation
3227 for (int i = 0; i < _njoints; i++)
3228 {
3229 min[i] = -3.5;
3230 max[i] = +3.5;
3231 }
3232 return ReturnValue_ok;
3233}
3234
3236{
3237 for (int i = 0; i < _njoints; i++)
3238 {
3239 _ref_currents[i] = t[i];
3240 current[i] = t[i] / 2;
3241 }
3242 return ReturnValue_ok;
3243}
3244
3246{
3247 _ref_currents[j] = t;
3248 current[j] = t / 2;
3249 return ReturnValue_ok;
3250}
3251
3252ReturnValue FakeMotionControl::setRefCurrentsRaw(const int n_joint, const int *joints, const double *t)
3253{
3255 for (int j = 0; j<n_joint; j++)
3256 {
3257 ret = ret &&setRefCurrentRaw(joints[j], t[j]);
3258 }
3259 return ret;
3260}
3261
3263{
3264 for (int i = 0; i < _njoints; i++)
3265 {
3266 t[i] = _ref_currents[i];
3267 }
3268 return ReturnValue_ok;
3269}
3270
3272{
3273 *t = _ref_currents[j];
3274 return ReturnValue_ok;
3275}
3276
3281
3283{
3284 return _njoints;
3285}
3286
3288{
3289 for (int i = 0; i < _njoints; i++)
3290 {
3291 measure[i] = _torques[i];
3292 }
3293 return true;
3294}
3295
3297{
3298 _torques[ch] = measure;
3299 return true;
3300}
3301
3302ReturnValue FakeMotionControl::getLastJointFaultRaw(int j, int& fault, std::string& message)
3303{
3304 _mutex.lock();
3305 fault = _hwfault_code[j];
3306 message = _hwfault_message[j];
3307 _mutex.unlock();
3308 return ReturnValue_ok;
3309}
3310
3312{
3313 braked = _braked[j];
3314 return ReturnValue_ok;
3315}
3316
3318{
3319 _braked[j] = active;
3320 return ReturnValue_ok;
3321}
3322
3324{
3325 _autobraked[j] = enabled;
3326 return ReturnValue_ok;
3327}
3328
3330{
3331 enabled = _autobraked[j];
3332 return ReturnValue_ok;
3333}
3334
3336{
3338 if (jnt < 0 || jnt >= _njoints) {
3339 yCError(FAKEMOTIONCONTROL) << "setRefVelocityRaw: joint index out of bounds";
3340 return ReturnValue::return_code::return_value_error_method_failed;
3341 }
3342 if (vel < -_maxJntCmdVelocity[jnt] || vel > _maxJntCmdVelocity[jnt]) {
3343 yCError(FAKEMOTIONCONTROL) << "setRefVelocityRaw: velocity out of bounds for joint" << jnt;
3344 return ReturnValue::return_code::return_value_error_method_failed;
3345 }
3346 _mutex.lock();
3347 _dir_vel_commands[jnt] = vel;
3348 _mutex.unlock();
3349 return ReturnValue_ok;
3350}
3351
3353{
3356 for (int i = 0; i < _njoints; i++) {
3357 ret &= setRefVelocityRaw(i, vels[i]);
3358 }
3359 return ret;
3360}
3361
3362ReturnValue FakeMotionControl::setRefVelocityRaw(const std::vector<int>& jnts, const std::vector<double>& vels)
3363{
3365 if (jnts.size() != vels.size()) {
3366 yCError(FAKEMOTIONCONTROL) << "setRefVelocityRaw: jnts and vels vectors must have the same size";
3367 return ReturnValue::return_code::return_value_error_method_failed;
3368 }
3370 for (int i = 0; i < _njoints; i++) {
3371 ret &= setRefVelocityRaw(jnts[i], vels[i]);
3372 }
3373 return ret;
3374}
3375
3377{
3379 if (jnt < 0 || jnt >= _njoints) {
3380 yCError(FAKEMOTIONCONTROL) << "setRefVelocityRaw: joint index out of bounds";
3381 return ReturnValue::return_code::return_value_error_method_failed;
3382 }
3383 _mutex.lock();
3384 vel = _dir_vel_commands[jnt];
3385 _mutex.unlock();
3386 return ReturnValue_ok;
3387}
3388
3390{
3393 vels.resize(_njoints);
3394 for (int i = 0; i < _njoints; i++) {
3395 ret &= getRefVelocityRaw(i, vels[i]);
3396 }
3397 return ret;
3398}
3399
3400ReturnValue FakeMotionControl::getRefVelocityRaw(const std::vector<int>& jnts, std::vector<double>& vels)
3401{
3403 if (jnts.size() != vels.size()) {
3404 yCError(FAKEMOTIONCONTROL) << "getRefVelocityRaw: jnts and vels vectors must have the same size";
3405 return ReturnValue::return_code::return_value_error_method_failed;
3406 }
3408 vels.resize(_njoints);
3409 for (int i = 0; i < _njoints; i++) {
3410 ret &= getRefVelocityRaw(jnts[i], vels[i]);
3411 }
3412 return ret;
3413}
3414
3415
3417{
3420
3421 size_t index = PidControlTypeEnum2Index(pidtype);
3422 switch (pidtype)
3423 {
3424 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1:
3425 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2:
3426 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3:
3427 units=_ppids[j].at(index).pidExtraInfo;
3428 break;
3429 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1:
3430 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2:
3431 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3:
3432 units=_vpids[j].at(index).pidExtraInfo;
3433 break;
3434 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1:
3435 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2:
3436 case PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3:
3437 units=_cpids[j].at(index).pidExtraInfo;
3438 break;
3439 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1:
3440 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2:
3441 case PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3:
3442 units=_pdpids[j].at(index).pidExtraInfo;
3443 break;
3444 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1:
3445 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2:
3446 case PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3:
3447 units=_vdpids[j].at(index).pidExtraInfo;
3448 break;
3449 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1:
3450 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2:
3451 case PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3:
3452 units=_mpids[j].at(index).pidExtraInfo;
3453 break;
3454 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1:
3455 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2:
3456 case PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3:
3457 units=_tpids[j].at(index).pidExtraInfo;
3458 break;
3459 default:
3460 yCError(FAKEMOTIONCONTROL) << "Invalid pid type";
3461 return ReturnValue::return_code::return_value_error_method_failed;
3462 break;
3463 }
3464
3465 return ret;
3466}
3467
3468
3469ReturnValue FakeMotionControl::getPidExtraInfosRaw(const PidControlTypeEnum& pidtype, std::vector<yarp::dev::PidExtraInfo>& units)
3470{
3473 units.resize(_njoints);
3474 for (int i = 0; i < _njoints; i++) {
3475 ret &= getPidExtraInfoRaw(pidtype, i, units[i]);
3476 }
3477 return ret;
3478}
3479
3480
3481/*
3482bool FakeMotionControl::parseImpedanceGroup_NewFormat(Bottle& pidsGroup, ImpedanceParameters vals[])
3483{
3484 int j=0;
3485 Bottle xtmp;
3486
3487 if (!extractGroup(pidsGroup, xtmp, "stiffness", "stiffness parameter", _njoints)) {
3488 return false;
3489 }
3490 for (j=0; j<_njoints; j++) {
3491 vals[j].stiffness = xtmp.get(j+1).asFloat64();
3492 }
3493
3494 if (!extractGroup(pidsGroup, xtmp, "damping", "damping parameter", _njoints)) {
3495 return false;
3496 }
3497 for (j=0; j<_njoints; j++) {
3498 vals[j].damping = xtmp.get(j+1).asFloat64();
3499 }
3500
3501 return true;
3502}
3503*/
3504
3505/*
3506bool FakeMotionControl::parsePositionPidsGroup(Bottle& pidsGroup, Pid myPid[])
3507{
3508 int j=0;
3509 Bottle xtmp;
3510
3511 if (!extractGroup(pidsGroup, xtmp, "kp", "Pid kp parameter", _njoints)) {
3512 return false;
3513 }
3514 for (j=0; j<_njoints; j++) {
3515 myPid[j].kp = xtmp.get(j+1).asFloat64();
3516 }
3517
3518 if (!extractGroup(pidsGroup, xtmp, "kd", "Pid kd parameter", _njoints)) {
3519 return false;
3520 }
3521 for (j=0; j<_njoints; j++) {
3522 myPid[j].kd = xtmp.get(j+1).asFloat64();
3523 }
3524
3525 if (!extractGroup(pidsGroup, xtmp, "ki", "Pid kp parameter", _njoints)) {
3526 return false;
3527 }
3528 for (j=0; j<_njoints; j++) {
3529 myPid[j].ki = xtmp.get(j+1).asFloat64();
3530 }
3531
3532 if (!extractGroup(pidsGroup, xtmp, "maxInt", "Pid maxInt parameter", _njoints)) {
3533 return false;
3534 }
3535 for (j=0; j<_njoints; j++) {
3536 myPid[j].max_int = xtmp.get(j+1).asFloat64();
3537 }
3538
3539 if (!extractGroup(pidsGroup, xtmp, "maxPwm", "Pid maxPwm parameter", _njoints)) {
3540 return false;
3541 }
3542 for (j=0; j<_njoints; j++) {
3543 myPid[j].max_output = xtmp.get(j+1).asFloat64();
3544 }
3545
3546 if (!extractGroup(pidsGroup, xtmp, "shift", "Pid shift parameter", _njoints)) {
3547 return false;
3548 }
3549 for (j=0; j<_njoints; j++) {
3550 myPid[j].scale = xtmp.get(j+1).asFloat64();
3551 }
3552
3553 if (!extractGroup(pidsGroup, xtmp, "ko", "Pid ko parameter", _njoints)) {
3554 return false;
3555 }
3556 for (j=0; j<_njoints; j++) {
3557 myPid[j].offset = xtmp.get(j+1).asFloat64();
3558 }
3559
3560 if (!extractGroup(pidsGroup, xtmp, "stictionUp", "Pid stictionUp", _njoints)) {
3561 return false;
3562 }
3563 for (j=0; j<_njoints; j++) {
3564 myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
3565 }
3566
3567 if (!extractGroup(pidsGroup, xtmp, "stictionDwn", "Pid stictionDwn", _njoints)) {
3568 return false;
3569 }
3570 for (j=0; j<_njoints; j++) {
3571 myPid[j].stiction_down_val = xtmp.get(j+1).asFloat64();
3572 }
3573
3574 if (!extractGroup(pidsGroup, xtmp, "kff", "Pid kff parameter", _njoints)) {
3575 return false;
3576 }
3577 for (j=0; j<_njoints; j++) {
3578 myPid[j].kff = xtmp.get(j+1).asFloat64();
3579 }
3580
3581 //conversion from metric to machine units (if applicable)
3582 if (_positionControlUnits==P_METRIC_UNITS)
3583 {
3584 for (j=0; j<_njoints; j++)
3585 {
3586 myPid[j].kp = myPid[j].kp / _angleToEncoder[j]; //[PWM/deg]
3587 myPid[j].ki = myPid[j].ki / _angleToEncoder[j]; //[PWM/deg]
3588 myPid[j].kd = myPid[j].kd / _angleToEncoder[j]; //[PWM/deg]
3589 }
3590 }
3591 else
3592 {
3593 //do nothing
3594 }
3595
3596 //optional PWM limit
3597 if(_pwmIsLimited)
3598 { // check for value in the file
3599 if (!extractGroup(pidsGroup, xtmp, "limPwm", "Limited PWD", _njoints))
3600 {
3601 yCError(FAKEMOTIONCONTROL) << "The PID parameter limPwm was requested but was not correctly set in the configuration file, please fill it.";
3602 return false;
3603 }
3604
3605 yCInfo(FAKEMOTIONCONTROL) << "Using LIMITED PWM!!";
3606 for (j = 0; j < _njoints; j++) {
3607 myPid[j].max_output = xtmp.get(j + 1).asFloat64();
3608 }
3609 }
3610
3611 return true;
3612}
3613*/
3614
3615/*
3616bool FakeMotionControl::parseTorquePidsGroup(Bottle& pidsGroup, Pid myPid[], double kbemf[], double ktau[], int filterType[], double viscousPos[], double viscousNeg[], double coulombPos[], double coulombNeg[], double velocityThres[])
3617{
3618 int j=0;
3619 Bottle xtmp;
3620 if (!extractGroup(pidsGroup, xtmp, "kp", "Pid kp parameter", _njoints)) {
3621 return false;
3622 }
3623 for (j=0; j<_njoints; j++) {
3624 myPid[j].kp = xtmp.get(j+1).asFloat64();
3625 }
3626
3627 if (!extractGroup(pidsGroup, xtmp, "kd", "Pid kd parameter", _njoints)) {
3628 return false;
3629 }
3630 for (j=0; j<_njoints; j++) {
3631 myPid[j].kd = xtmp.get(j+1).asFloat64();
3632 }
3633
3634 if (!extractGroup(pidsGroup, xtmp, "ki", "Pid kp parameter", _njoints)) {
3635 return false;
3636 }
3637 for (j=0; j<_njoints; j++) {
3638 myPid[j].ki = xtmp.get(j+1).asFloat64();
3639 }
3640
3641 if (!extractGroup(pidsGroup, xtmp, "maxInt", "Pid maxInt parameter", _njoints)) {
3642 return false;
3643 }
3644 for (j=0; j<_njoints; j++) {
3645 myPid[j].max_int = xtmp.get(j+1).asFloat64();
3646 }
3647
3648 if (!extractGroup(pidsGroup, xtmp, "maxPwm", "Pid maxPwm parameter", _njoints)) {
3649 return false;
3650 }
3651 for (j=0; j<_njoints; j++) {
3652 myPid[j].max_output = xtmp.get(j+1).asFloat64();
3653 }
3654
3655 if (!extractGroup(pidsGroup, xtmp, "shift", "Pid shift parameter", _njoints)) {
3656 return false;
3657 }
3658 for (j=0; j<_njoints; j++) {
3659 myPid[j].scale = xtmp.get(j+1).asFloat64();
3660 }
3661
3662 if (!extractGroup(pidsGroup, xtmp, "ko", "Pid ko parameter", _njoints)) {
3663 return false;
3664 }
3665 for (j=0; j<_njoints; j++) {
3666 myPid[j].offset = xtmp.get(j+1).asFloat64();
3667 }
3668
3669 if (!extractGroup(pidsGroup, xtmp, "stictionUp", "Pid stictionUp", _njoints)) {
3670 return false;
3671 }
3672 for (j=0; j<_njoints; j++) {
3673 myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
3674 }
3675
3676 if (!extractGroup(pidsGroup, xtmp, "stictionDwn", "Pid stictionDwn", _njoints)) {
3677 return false;
3678 }
3679 for (j=0; j<_njoints; j++) {
3680 myPid[j].stiction_down_val = xtmp.get(j+1).asFloat64();
3681 }
3682
3683 if (!extractGroup(pidsGroup, xtmp, "kff", "Pid kff parameter", _njoints)) {
3684 return false;
3685 }
3686 for (j=0; j<_njoints; j++) {
3687 myPid[j].kff = xtmp.get(j+1).asFloat64();
3688 }
3689
3690 if (!extractGroup(pidsGroup, xtmp, "kbemf", "kbemf parameter", _njoints)) {
3691 return false;
3692 }
3693 for (j=0; j<_njoints; j++) {
3694 kbemf[j] = xtmp.get(j+1).asFloat64();
3695 }
3696
3697 if (!extractGroup(pidsGroup, xtmp, "ktau", "ktau parameter", _njoints)) {
3698 return false;
3699 }
3700 for (j=0; j<_njoints; j++) {
3701 ktau[j] = xtmp.get(j+1).asFloat64();
3702 }
3703
3704 if (!extractGroup(pidsGroup, xtmp, "filterType", "filterType param", _njoints)) {
3705 return false;
3706 }
3707 for (j=0; j<_njoints; j++) {
3708 filterType[j] = xtmp.get(j+1).asInt32();
3709 }
3710
3711 if (!extractGroup(pidsGroup, xtmp, "viscousPos", "viscous pos parameter", _njoints)) {
3712 return false;
3713 }
3714 for (j=0; j<_njoints; j++) {
3715 viscousPos[j] = xtmp.get(j+1).asFloat64();
3716 }
3717
3718 if (!extractGroup(pidsGroup, xtmp, "viscousNeg", "viscous neg parameter", _njoints)) {
3719 return false;
3720 }
3721 for (j=0; j<_njoints; j++) {
3722 viscousNeg[j] = xtmp.get(j+1).asFloat64();
3723 }
3724
3725 if (!extractGroup(pidsGroup, xtmp, "coulombPos", "coulomb pos parameter", _njoints)) {
3726 return false;
3727 }
3728 for (j=0; j<_njoints; j++) {
3729 coulombPos[j] = xtmp.get(j+1).asFloat64();
3730 }
3731
3732 if (!extractGroup(pidsGroup, xtmp, "coulombNeg", "coulomb neg parameter", _njoints)) {
3733 return false;
3734 }
3735 for (j=0; j<_njoints; j++) {
3736 coulombNeg[j] = xtmp.get(j+1).asFloat64();
3737 }
3738
3739 if (!extractGroup(pidsGroup, xtmp, "velocityThres", "velocity threshold parameter", _njoints)) {
3740 return false;
3741 }
3742 for (j=0; j<_njoints; j++) {
3743 velocityThres[j] = xtmp.get(j+1).asFloat64();
3744 }
3745
3746
3747 //conversion from metric to machine units (if applicable)
3748// for (j=0; j<_njoints; j++)
3749// {
3750// myPid[j].kp = myPid[j].kp / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
3751// myPid[j].ki = myPid[j].ki / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
3752// myPid[j].kd = myPid[j].kd / _torqueControlHelper->getNewtonsToSensor(j); //[PWM/Nm]
3753// myPid[j].stiction_up_val = myPid[j].stiction_up_val * _torqueControlHelper->getNewtonsToSensor(j); //[Nm]
3754// myPid[j].stiction_down_val = myPid[j].stiction_down_val * _torqueControlHelper->getNewtonsToSensor(j); //[Nm]
3755// }
3756
3757 //optional PWM limit
3758 if(_pwmIsLimited)
3759 { // check for value in the file
3760 if (!extractGroup(pidsGroup, xtmp, "limPwm", "Limited PWM", _njoints))
3761 {
3762 yCError(FAKEMOTIONCONTROL) << "The PID parameter limPwm was requested but was not correctly set in the configuration file, please fill it.";
3763 return false;
3764 }
3765
3766 yCInfo(FAKEMOTIONCONTROL) << "Using LIMITED PWM!!";
3767 for (j = 0; j < _njoints; j++) {
3768 myPid[j].max_output = xtmp.get(j + 1).asFloat64();
3769 }
3770 }
3771
3772 return true;
3773}
3774*/
3775
3776// eof
void checkAndDestroy(T *&p)
#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 resize(size_t size) override
Resize the vector.
Definition Vector.h:231
void zero()
Zero the elements of the vector.
Definition Vector.h:373
#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.
Definition OSTestMsg.cpp:13