yarp-devices
Loading...
Searching...
No Matches
TechnosoftIposBase.hpp
1// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2
3#ifndef __TECHNOSOFT_IPOS_BASE_HPP__
4#define __TECHNOSOFT_IPOS_BASE_HPP__
5
6#include <cstdint>
7
8#include <atomic>
9#include <bitset>
10#include <memory>
11#include <string>
12
13#include <yarp/conf/numeric.h>
14#include <yarp/conf/version.h>
15
16#include <yarp/os/Timer.h>
17
18#include <yarp/dev/DeviceDriver.h>
19#include <yarp/dev/IAxisInfo.h>
20#include <yarp/dev/IControlLimits.h>
21#include <yarp/dev/IControlMode.h>
22#include <yarp/dev/ICurrentControl.h>
23#include <yarp/dev/IEncodersTimed.h>
24#include <yarp/dev/IImpedanceControl.h>
25#include <yarp/dev/IInteractionMode.h>
26#include <yarp/dev/IJointFault.h>
27#include <yarp/dev/IMotor.h>
28#include <yarp/dev/IMotorEncoders.h>
29#include <yarp/dev/IPidControl.h>
30#include <yarp/dev/IPositionControl.h>
31#include <yarp/dev/IPositionDirect.h>
32#include <yarp/dev/IRemoteVariables.h>
33#include <yarp/dev/ITorqueControl.h>
34#include <yarp/dev/IVelocityControl.h>
35#include <yarp/dev/PolyDriver.h>
36
37#include "CanOpenNode.hpp"
38#include "CommandBuffer.hpp"
39#include "EncoderRead.hpp"
40#include "ICanBusSharer.hpp"
41#include "PdoProtocol.hpp"
42#include "StateObserver.hpp"
43
44#include "TechnosoftIpos_ParamsParser.h"
45
46#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
47#define CHECK_JOINT(j) do { if (std::size_t ax; getAxes(ax), (j) < 0 || (j) >= ax) return yarp::dev::ReturnValue_error_input_out_of_bounds; } while (0)
48#define CHECK_MODE(mode) do { if ((mode) != actualControlMode) return yarp::dev::ReturnValue_error_not_ready; } while (0)
49#else
50#define CHECK_JOINT(j) do { if (int ax; getAxes(&ax), (j) < 0 || (j) >= ax) return false; } while (0)
51#define CHECK_MODE(mode) do { if ((mode) != actualControlMode) return false; } while (0)
52#endif
53
54namespace roboticslab
55{
56
61class TechnosoftIposBase : public yarp::dev::DeviceDriver,
62 public yarp::dev::IAxisInfoRaw,
63 public yarp::dev::IControlLimitsRaw,
64 public yarp::dev::IControlModeRaw,
65 public yarp::dev::ICurrentControlRaw,
66 public yarp::dev::IEncodersTimedRaw,
67 public yarp::dev::IImpedanceControlRaw,
68 public yarp::dev::IInteractionModeRaw,
69 public yarp::dev::IJointFaultRaw,
70 public yarp::dev::IMotorRaw,
71 public yarp::dev::IMotorEncodersRaw,
72 public yarp::dev::IPidControlRaw,
73 public yarp::dev::IPositionControlRaw,
74 public yarp::dev::IPositionDirectRaw,
75 public yarp::dev::IRemoteVariablesRaw,
76 public yarp::dev::ITorqueControlRaw,
77 public yarp::dev::IVelocityControlRaw,
78 public ICanBusSharer
79{
80public:
81
82#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
83 using return_t = yarp::dev::ReturnValue;
84
85 constexpr static auto ret_not_ok = return_t::return_code::return_value_error_not_implemented_by_device;
86#else
87 using return_t = bool;
88
89 constexpr static auto ret_not_ok = false;
90#endif
91
92 TechnosoftIposBase(const TechnosoftIpos_ParamsParser & _params)
93 : params(_params)
94 {}
95
96 // --------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp ---------
97
98 bool open(yarp::os::Searchable & config) override;
99 bool close() override;
100
101 // --------- ICanBusSharer declarations. Implementation in ICanBusSharerImpl.cpp ---------
102
103 unsigned int getId() override;
104 std::vector<unsigned int> getAdditionalIds() override;
105 bool notifyMessage(const can_message & message) override;
106 bool initialize() override;
107 bool finalize() override;
108 bool registerSender(ICanSenderDelegate * sender) override;
109
110 // --------- IAxisInfoRaw declarations. Implementation in IAxisInfoRawImpl.cpp ---------
111
112 return_t getAxisNameRaw(int axis, std::string & name) override;
113 return_t getJointTypeRaw(int axis, yarp::dev::JointTypeEnum & type) override;
114
115 // --------- IControlLimitsRaw declarations. Implementation in IControlLimitsRawImpl.cpp ---------
116
117#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
118 return_t setPosLimitsRaw(int axis, double min, double max) override;
119 return_t getPosLimitsRaw(int axis, double * min, double * max) override;
120#else
121 return_t setLimitsRaw(int axis, double min, double max) override;
122 return_t getLimitsRaw(int axis, double * min, double * max) override;
123#endif
124 return_t setVelLimitsRaw(int axis, double min, double max) override;
125 return_t getVelLimitsRaw(int axis, double * min, double * max) override;
126
127 // --------- IControlModeRaw declarations. Implementation in IControlModeRawImpl.cpp ---------
128
129#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
130 return_t getControlModesRaw(std::vector<yarp::dev::ControlModeEnum> & modes) override
131 { return getControlModeRaw(0, modes[0]); }
132
133 return_t getControlModesRaw(const std::vector<int> & joints, std::vector<yarp::dev::ControlModeEnum> & modes) override
134 { return getControlModeRaw(joints[0], modes[0]); }
135
136 return_t setControlModesRaw(const std::vector<yarp::dev::SelectableControlModeEnum> & modes) override
137 { return setControlModeRaw(0, modes[0]); }
138
139 return_t setControlModesRaw(const std::vector<int> & joints, const std::vector<yarp::dev::SelectableControlModeEnum> & modes) override
140 { return setControlModeRaw(joints[0], modes[0]); }
141#else
142 return_t getControlModesRaw(int * modes) override
143 { return getControlModeRaw(0, &modes[0]); }
144
145 return_t getControlModesRaw(int n_joint, const int * joints, int * modes) override
146 { return getControlModeRaw(joints[0], &modes[0]); }
147
148 return_t setControlModesRaw(int * modes) override
149 { return setControlModeRaw(0, modes[0]); }
150
151 return_t setControlModesRaw(int n_joint, const int * joints, int * modes) override
152 { return setControlModeRaw(joints[0], modes[0]); }
153#endif
154
155 // --------- ICurrentControlRaw declarations. Implementation in ICurrentControlRawImpl.cpp ---------
156
157 return_t getCurrentRaw(int m, double * curr) override;
158
159 return_t getCurrentsRaw(double * currs) override
160 { return getCurrentRaw(0, &currs[0]); }
161
162 return_t getCurrentRangeRaw(int m, double * min, double * max) override;
163
164 return_t getCurrentRangesRaw(double * min, double * max) override
165 { return getCurrentRangeRaw(0, min, max); }
166
167 return_t setRefCurrentRaw(int m, double curr) override;
168
169 return_t setRefCurrentsRaw(const double * currs) override
170 { return setRefCurrentRaw(0, currs[0]); }
171
172 return_t setRefCurrentsRaw(int n_motor, const int * motors, const double * currs) override
173 { return setRefCurrentRaw(motors[0], currs[0]); }
174
175 return_t getRefCurrentRaw(int m, double * curr) override;
176
177 return_t getRefCurrentsRaw(double * currs) override
178 { return getRefCurrentRaw(0, &currs[0]); }
179
180 // ---------- IEncodersRaw declarations. Implementation in IEncodersRawImpl.cpp ----------
181
182#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
183 return_t getAxes(std::size_t & ax) override;
184#else
185 return_t getAxes(int * ax) override;
186#endif
187
188 return_t resetEncoderRaw(int j) override;
189
190 return_t resetEncodersRaw() override
191 { return resetEncoderRaw(0); }
192
193 return_t setEncoderRaw(int j, double val) override;
194
195 return_t setEncodersRaw(const double * vals) override
196 { return setEncoderRaw(0, vals[0]); }
197
198 return_t getEncoderRaw(int j, double * v) override;
199
200 return_t getEncodersRaw(double * encs) override
201 { return getEncoderRaw(0, &encs[0]); }
202
203 return_t getEncoderSpeedRaw(int j, double * sp) override;
204
205 return_t getEncoderSpeedsRaw(double * spds) override
206 { return getEncoderSpeedRaw(0, &spds[0]); }
207
208 return_t getEncoderAccelerationRaw(int j, double * spds) override;
209
210 return_t getEncoderAccelerationsRaw(double * accs) override
211 { return getEncoderAccelerationRaw(0, &accs[0]); }
212
213 return_t getEncoderTimedRaw(int j, double * encs, double * time) override;
214
215 return_t getEncodersTimedRaw(double * encs, double * times) override
216 { return getEncoderTimedRaw(0, &encs[0], &times[0]); }
217
218 // ---------- IImpedanceControlRaw declarations. Implementation in IImpedanceControlRawImpl.cpp ----------
219
220 return_t getImpedanceRaw(int j, double * stiffness, double * damping) override
221 { return ret_not_ok; }
222
223 return_t setImpedanceRaw(int j, double stiffness, double damping) override
224 { return ret_not_ok; }
225
226 return_t setImpedanceOffsetRaw(int j, double offset) override
227 { return ret_not_ok; }
228
229 return_t getImpedanceOffsetRaw(int j, double * offset) override
230 { return ret_not_ok; }
231
232 return_t getCurrentImpedanceLimitRaw(int j, double * min_stiff, double * max_stiff, double * min_damp, double * max_damp) override
233 { return ret_not_ok; }
234
235 // ---------- IInteractionModeRaw declarations. Implementation in IInteractionModeRawImpl.cpp ----------
236
237#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
238 return_t getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum & mode) override
239 { return ret_not_ok; }
240
241 return_t getInteractionModesRaw(std::vector<yarp::dev::InteractionModeEnum> & modes) override
242 { return getInteractionModeRaw(0, modes[0]); }
243
244 return_t getInteractionModesRaw(const std::vector<int> & joints, std::vector<yarp::dev::InteractionModeEnum> & modes) override
245 { return getInteractionModeRaw(joints[0], modes[0]); }
246
247 return_t setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode) override
248 { return ret_not_ok; }
249
250 return_t setInteractionModesRaw(const std::vector<yarp::dev::InteractionModeEnum> & modes) override
251 { return setInteractionModeRaw(0, modes[0]); }
252
253 return_t setInteractionModesRaw(const std::vector<int> & joints, const std::vector<yarp::dev::InteractionModeEnum> & modes) override
254 { return setInteractionModeRaw(joints[0], modes[0]); }
255#else
256 return_t getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum * mode) override
257 { return ret_not_ok; }
258
259 return_t getInteractionModesRaw(int n_joints, int * joints, yarp::dev::InteractionModeEnum * modes) override
260 { return getInteractionModeRaw(joints[0], &modes[0]); }
261
262 return_t getInteractionModesRaw(yarp::dev::InteractionModeEnum * modes) override
263 { return getInteractionModeRaw(0, &modes[0]); }
264
265 return_t setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode) override
266 { return ret_not_ok; }
267
268 return_t setInteractionModesRaw(int n_joints, int * joints, yarp::dev::InteractionModeEnum * modes) override
269 { return setInteractionModeRaw(joints[0], modes[0]); }
270
271 return_t setInteractionModesRaw(yarp::dev::InteractionModeEnum * modes) override
272 { return setInteractionModeRaw(0, modes[0]); }
273#endif
274
275 // ---------- IJointFaultRaw declarations. Implementation in IJointFaultRawImpl.cpp ----------
276
277 return_t getLastJointFaultRaw(int j, int & fault, std::string & message) override;
278
279 // --------- IMotorRaw declarations. Implementation in IMotorRawImpl.cpp ---------
280
281 return_t getNumberOfMotorsRaw(int * num) override;
282 return_t getTemperatureRaw(int m, double * val) override;
283 return_t getTemperaturesRaw(double * vals) override;
284 return_t getTemperatureLimitRaw(int m, double * temp) override;
285 return_t setTemperatureLimitRaw(int m, double temp) override;
286 return_t getGearboxRatioRaw(int m, double * val) override;
287 return_t setGearboxRatioRaw(int m, double val) override;
288
289 // --------- IMotorEncodersRaw declarations. Implementation in IMotorEncodersRawImpl.cpp ---------
290
291 return_t getNumberOfMotorEncodersRaw(int * num) override;
292 return_t resetMotorEncoderRaw(int m) override;
293
294 return_t resetMotorEncodersRaw() override
295 { return resetMotorEncoderRaw(0); }
296
297 return_t setMotorEncoderCountsPerRevolutionRaw(int m, double cpr) override;
298 return_t getMotorEncoderCountsPerRevolutionRaw(int m, double * cpr) override;
299 return_t setMotorEncoderRaw(int m, double val) override;
300
301 return_t setMotorEncodersRaw(const double * vals) override
302 { return setMotorEncoderRaw(0, vals[0]); }
303
304 return_t getMotorEncoderRaw(int m, double * v) override;
305
306 return_t getMotorEncodersRaw(double * encs) override
307 { return getMotorEncoderSpeedRaw(0, &encs[0]); }
308
309 return_t getMotorEncoderTimedRaw(int m, double * encs, double * stamp) override;
310
311 return_t getMotorEncodersTimedRaw(double * encs, double * stamps) override
312 { return getMotorEncoderTimedRaw(0, &encs[0], &stamps[0]); }
313
314 return_t getMotorEncoderSpeedRaw(int m, double * sp) override;
315
316 return_t getMotorEncoderSpeedsRaw(double * spds) override
317 { return getMotorEncoderSpeedRaw(0, &spds[0]); }
318
319 return_t getMotorEncoderAccelerationRaw(int m, double * spds) override;
320
321 return_t getMotorEncoderAccelerationsRaw(double * accs) override
322 { return getMotorEncoderAccelerationRaw(0, &accs[0]); }
323
324 // --------- IPidControlRaw declarations. Implementation in IPidControlRawImpl.cpp ---------
325
326#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
327 return_t getAvailablePidsRaw(int j, std::vector<yarp::dev::PidControlTypeEnum> & avail) override
328 { return ret_not_ok; }
329#endif
330
331 return_t setPidRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, const yarp::dev::Pid & pid) override
332 { return ret_not_ok; }
333
334 return_t setPidsRaw(const yarp::dev::PidControlTypeEnum & pidtype, const yarp::dev::Pid * pids) override
335 { return setPidRaw(pidtype, 0, pids[0]); }
336
337 return_t setPidReferenceRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double ref) override
338 { return ret_not_ok; }
339
340 return_t setPidReferencesRaw(const yarp::dev::PidControlTypeEnum & pidtype, const double * refs) override
341 { return setPidReferenceRaw(pidtype, 0, refs[0]); }
342
343 return_t setPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double limit) override
344 { return ret_not_ok; }
345
346 return_t setPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum & pidtype, const double * limits) override
347 { return setPidErrorLimitRaw(pidtype, 0, limits[0]); }
348
349 return_t getPidErrorRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * err) override
350 { return ret_not_ok; }
351
352 return_t getPidErrorsRaw(const yarp::dev::PidControlTypeEnum & pidtype, double * errs) override
353 { return getPidErrorRaw(pidtype, 0, &errs[0]); }
354
355 return_t getPidOutputRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * out) override
356 { return ret_not_ok; }
357
358 return_t getPidOutputsRaw(const yarp::dev::PidControlTypeEnum & pidtype, double * outs) override
359 { return getPidOutputRaw(pidtype, 0, &outs[0]); }
360
361 return_t getPidRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, yarp::dev::Pid * pid) override
362 { return ret_not_ok; }
363
364 return_t getPidsRaw(const yarp::dev::PidControlTypeEnum & pidtype, yarp::dev::Pid * pids) override
365 { return getPidRaw(pidtype, 0, &pids[0]); }
366
367#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
368 return_t getPidOffsetRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double & v) override
369 { return ret_not_ok; }
370
371 return_t getPidFeedforwardRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double & v) override
372 { return ret_not_ok; }
373
374 return_t getPidExtraInfoRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, yarp::dev::PidExtraInfo & info) override
375 { return ret_not_ok; }
376
377 return_t getPidExtraInfosRaw(const yarp::dev::PidControlTypeEnum & pidtype, std::vector<yarp::dev::PidExtraInfo> & info) override
378 { return ret_not_ok; }
379#endif
380
381 return_t getPidReferenceRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * ref) override
382 { return ret_not_ok; }
383
384 return_t getPidReferencesRaw(const yarp::dev::PidControlTypeEnum & pidtype, double * refs) override
385 { return getPidReferenceRaw(pidtype, 0, &refs[0]); }
386
387 return_t getPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * limit) override
388 { return ret_not_ok; }
389
390 return_t getPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum & pidtype, double * limits) override
391 { return getPidErrorLimitRaw(pidtype, 0, &limits[0]); }
392
393 return_t resetPidRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j) override
394 { return ret_not_ok; }
395
396 return_t disablePidRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j) override
397 { return ret_not_ok; }
398
399 return_t enablePidRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j) override
400 { return ret_not_ok; }
401
402 return_t setPidOffsetRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double v) override
403 { return ret_not_ok; }
404
405#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
406 return_t setPidFeedforwardRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double v) override
407 { return ret_not_ok; }
408
409 return_t isPidEnabledRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, bool & enabled) override
410 { return ret_not_ok; }
411#else
412 return_t isPidEnabledRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, bool * enabled) override
413 { return ret_not_ok; }
414#endif
415
416 // ------- IPositionControlRaw declarations. Implementation in IPositionControlRawImpl.cpp -------
417
418 using yarp::dev::IPositionControlRaw::positionMoveRaw;
419
420 return_t positionMoveRaw(const double * refs) override
421 { return positionMoveRaw(0, refs[0]); }
422
423 return_t positionMoveRaw(int n_joint, const int * joints, const double * refs) override
424 { return positionMoveRaw(joints[0], refs[0]); }
425
426 using yarp::dev::IPositionControlRaw::relativeMoveRaw;
427
428 return_t relativeMoveRaw(const double * deltas) override
429 { return relativeMoveRaw(0, deltas[0]); }
430
431 return_t relativeMoveRaw(int n_joint, const int * joints, const double * deltas) override
432 { return relativeMoveRaw(joints[0], deltas[0]); }
433
434 using yarp::dev::IPositionControlRaw::checkMotionDoneRaw;
435
436#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
437 return_t checkMotionDoneRaw(bool & flag) override
438 { return checkMotionDoneRaw(0, flag); }
439
440 return_t checkMotionDoneRaw(const std::vector<int> & joints, bool & flag) override
441 { return checkMotionDoneRaw(joints[0], flag); }
442
443 return_t setTrajSpeedsRaw(const double * spds) override
444 { return setTrajSpeedRaw(0, spds[0]); }
445
446 return_t setTrajSpeedsRaw(int n_joint, const int * joints, const double * spds) override
447 { return setTrajSpeedRaw(joints[0], spds[0]); }
448
449 using yarp::dev::IPositionControlRaw::setTrajAccelerationRaw;
450
451 return_t setTrajAccelerationsRaw(const double * accs) override
452 { return setTrajAccelerationRaw(0, accs[0]); }
453
454 return_t setTrajAccelerationsRaw(int n_joint, const int * joints, const double * accs) override
455 { return setTrajAccelerationRaw(joints[0], accs[0]); }
456
457 return_t getTrajSpeedsRaw(double * spds) override
458 { return getTrajSpeedRaw(0, &spds[0]); }
459
460 return_t getTrajSpeedsRaw(int n_joint, const int * joints, double * spds) override
461 { return getTrajSpeedRaw(joints[0], &spds[0]); }
462
463 using yarp::dev::IPositionControlRaw::getTrajAccelerationRaw;
464
465 return_t getTrajAccelerationsRaw(double * accs) override
466 { return getTrajAccelerationRaw(0, &accs[0]); }
467
468 return_t getTrajAccelerationsRaw(int n_joint, const int * joints, double * accs) override
469 { return getTrajAccelerationRaw(joints[0], &accs[0]); }
470#else
471 return_t checkMotionDoneRaw(bool * flag) override
472 { return checkMotionDoneRaw(0, flag); }
473
474 return_t checkMotionDoneRaw(int n_joint, const int * joints, bool * flag) override
475 { return checkMotionDoneRaw(joints[0], flag); }
476
477 return_t setRefSpeedsRaw(const double * spds) override
478 { return setRefSpeedRaw(0, spds[0]); }
479
480 return_t setRefSpeedsRaw(int n_joint, const int * joints, const double * spds) override
481 { return setRefSpeedRaw(joints[0], spds[0]); }
482
483 using yarp::dev::IPositionControlRaw::setRefAccelerationRaw;
484
485 return_t setRefAccelerationsRaw(const double * accs) override
486 { return setRefAccelerationRaw(0, accs[0]); }
487
488 return_t setRefAccelerationsRaw(int n_joint, const int * joints, const double * accs) override
489 { return setRefAccelerationRaw(joints[0], accs[0]); }
490
491 return_t getRefSpeedsRaw(double * spds) override
492 { return getRefSpeedRaw(0, &spds[0]); }
493
494 return_t getRefSpeedsRaw(int n_joint, const int * joints, double * spds) override
495 { return getRefSpeedRaw(joints[0], &spds[0]); }
496
497 using yarp::dev::IPositionControlRaw::getRefAccelerationRaw;
498
499 return_t getRefAccelerationsRaw(double * accs) override
500 { return getRefAccelerationRaw(0, &accs[0]); }
501
502 return_t getRefAccelerationsRaw(int n_joint, const int * joints, double * accs) override
503 { return getRefAccelerationRaw(joints[0], &accs[0]); }
504#endif
505
506 using yarp::dev::IPositionControlRaw::stopRaw;
507
508 return_t stopRaw() override
509 { return stopRaw(0); }
510
511 return_t stopRaw(int n_joint, const int * joints) override
512 { return stopRaw(joints[0]); }
513
514 return_t getTargetPositionsRaw(double * refs) override
515 { return getTargetPositionRaw(0, &refs[0]); }
516
517 return_t getTargetPositionsRaw(int n_joint, const int * joints, double * refs) override
518 { return getTargetPositionRaw(joints[0], &refs[0]); }
519
520 // ------- IPositionDirectRaw declarations. Implementation in IPositionDirectRawImpl.cpp -------
521
522 return_t setPositionsRaw(const double * refs) override
523 { return setPositionRaw(0, refs[0]); }
524
525 return_t setPositionsRaw(int n_joint, const int * joints, const double * refs) override
526 { return setPositionRaw(joints[0], refs[0]); }
527
528 return_t getRefPositionsRaw(double * refs) override
529 { return getRefPositionRaw(0, &refs[0]); }
530
531 return_t getRefPositionsRaw(int n_joint, const int * joints, double * refs) override
532 { return getRefPositionRaw(joints[0], &refs[0]); }
533
534 // -------- ITorqueControlRaw declarations. Implementation in ITorqueControlRawImpl.cpp --------
535
536 return_t getRefTorqueRaw(int j, double * t) override;
537
538 return_t getRefTorquesRaw(double * t) override
539 { return getRefTorqueRaw(0, &t[0]); }
540
541 return_t setRefTorqueRaw(int j, double t) override;
542
543 return_t setRefTorquesRaw(int n_joint, const int * joints, const double * t) override
544 { return setRefTorqueRaw(joints[0], t[0]); }
545
546 return_t setRefTorquesRaw(const double * t) override
547 { return setRefTorqueRaw(0, t[0]); }
548
549 return_t getTorqueRaw(int j, double * t) override;
550
551 return_t getTorquesRaw(double * t) override
552 { return getTorqueRaw(0, &t[0]); }
553
554 return_t getTorqueRangeRaw(int j, double * min, double * max) override;
555
556 return_t getTorqueRangesRaw(double * min, double * max) override
557 { return getTorqueRangeRaw(0, &min[0], &max[0]); }
558
559 return_t getMotorTorqueParamsRaw(int j, yarp::dev::MotorTorqueParameters * params) override;
560 return_t setMotorTorqueParamsRaw(int j, const yarp::dev::MotorTorqueParameters params) override;
561
562 // --------- IVelocityControlRaw declarations. Implementation in IVelocityControlRawImpl.cpp ---------
563
564 using yarp::dev::IVelocityControlRaw::velocityMoveRaw;
565
566 return_t velocityMoveRaw(const double * sp) override
567 { return velocityMoveRaw(0, sp[0]); }
568
569 return_t velocityMoveRaw(int n_joint, const int * joints, const double * spds) override
570 { return velocityMoveRaw(joints[0], spds[0]); }
571
572#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
573 return_t getTargetVelocitiesRaw(double * vels) override
574 { return getTargetVelocityRaw(0, &vels[0]); }
575
576 return_t getTargetVelocitiesRaw(int n_joint, const int * joints, double * vels) override
577 { return getTargetVelocityRaw(joints[0], &vels[0]); }
578#else
579 return_t getRefVelocitiesRaw(double * vels) override
580 { return getRefVelocityRaw(0, &vels[0]); }
581
582 return_t getRefVelocitiesRaw(int n_joint, const int * joints, double * vels) override
583 { return getRefVelocityRaw(joints[0], &vels[0]); }
584#endif
585
586protected:
587
588 enum report_level { NONE, INFO, WARN, FAULT };
589 enum limit_switch { POSITIVE, NEGATIVE, INACTIVE };
590
591 struct report_storage
592 {
593 const char * reg;
594 const std::bitset<16> & actual;
595 const std::bitset<16> & stored;
596 };
597
598 bool reportBitToggle(report_storage report, int level, std::size_t pos, const char * msgSet, const char * msgReset = nullptr);
599
600 virtual void interpretModesOfOperation(std::int8_t modesOfOperation) = 0;
601 virtual void interpretIpStatus(std::uint16_t ipStatus) {}
602 virtual void onPositionLimitTriggered() = 0;
603 virtual void reset() = 0;
604
606 bool awaitControlMode(yarp::conf::vocab32_t mode);
607
609 double degreesToInternalUnits(double value, int derivativeOrder = 0) const;
610
612 double internalUnitsToDegrees(double value, int derivativeOrder = 0) const;
613
615 std::int16_t currentToInternalUnits(double value) const;
616
618 double internalUnitsToCurrent(std::int16_t value) const;
619
621 double internalUnitsToPeakCurrent(std::int16_t value) const;
622
624 double currentToTorque(double current) const;
625
627 double torqueToCurrent(double torque) const;
628
629 CanOpenNode * can {nullptr};
630 CommandBuffer commandBuffer;
631
632 std::unique_ptr<StateObserver> controlModeObserverPtr {new StateObserver(1.0)}; // arbitrary 1 second wait
633 std::unique_ptr<EncoderRead> lastEncoderRead {nullptr};
634
635 // read/write, no concurrent access
636
637 std::bitset<16> msr;
638 std::bitset<16> mer;
639 std::bitset<16> der;
640 std::bitset<16> der2;
641 std::bitset<16> cer;
642
643 std::int8_t modesOfOperation {0};
644
645 DriveState driveState {DriveState::NOT_READY_TO_SWITCH_ON};
646
647 bool configuredOnce {false};
648
649 // read/write with atomic access
650
651 std::atomic<std::int16_t> lastCurrentRead {0};
652
653 std::atomic<yarp::conf::vocab32_t> actualControlMode {0};
654 std::atomic<yarp::conf::vocab32_t> requestedcontrolMode {0};
655
656 std::atomic<double> tr {0.0};
657 std::atomic<double> k {0.0};
658 std::atomic<int> encoderPulses {0};
659
660 std::atomic<double> maxVel {0.0};
661 std::atomic<double> min {0.0};
662 std::atomic<double> max {0.0};
663 std::atomic<double> refSpeed {0.0};
664 std::atomic<double> refAcceleration {0.0};
665
666 std::atomic<double> lastHeartbeat {0.0};
667 std::atomic<std::uint8_t> lastNmtState {0};
668 std::atomic<std::uint16_t> lastFaultCode {0};
669 std::atomic<const char *> lastFaultMessage;
670
671 std::atomic<limit_switch> limitSwitchState {INACTIVE};
672
673 // read only after initial configuration, conceptually immutable
674
675 yarp::conf::vocab32_t initialControlMode {0};
676 yarp::conf::vocab32_t jointType {0};
677
678 PdoConfiguration tpdo1Conf;
679 PdoConfiguration tpdo2Conf;
680 PdoConfiguration tpdo3Conf;
681
682 const TechnosoftIpos_ParamsParser & params;
683
684private:
685
687 static bool validateInitialState(const TechnosoftIpos_ParamsParser & params, const std::string & id);
688
689 void interpretMsr(std::uint16_t msr);
690 void interpretMer(std::uint16_t mer);
691 void interpretDer(std::uint16_t der);
692 void interpretDer2(std::uint16_t der2);
693 void interpretCer(std::uint16_t cer);
694 void interpretStatusword(std::uint16_t statusword);
695
696 void handleTpdo1(std::uint16_t statusword, std::uint16_t msr, std::int8_t modesOfOperation);
697 void handleTpdo2(std::uint16_t mer, std::uint16_t der);
698 void handleTpdo3(std::int32_t position, std::int16_t current);
699 void handleEmcy(EmcyConsumer::code_t code, std::uint8_t reg, const std::uint8_t * msef);
700 void handleNmt(NmtState state);
701
702 bool monitorWorker(const yarp::os::YarpTimerEvent & event);
703
704 bool setPosLimitRaw(double limit, bool isMin);
705 bool getPosLimitRaw(double * limit, bool isMin);
706
707 yarp::dev::PolyDriver externalEncoderDevice;
708 yarp::dev::IEncodersTimedRaw * iEncodersTimedRawExternal {nullptr};
709 roboticslab::ICanBusSharer * iExternalEncoderCanBusSharer {nullptr};
710
711 yarp::os::Timer * monitorThread {nullptr};
712 roboticslab::ICanSenderDelegate * sender {nullptr};
713};
714
715} // namespace roboticslab
716
717#endif // __TECHNOSOFT_IPOS_BASE_HPP__
TechnosoftIpos_ParamsParser
Definition TechnosoftIpos_ParamsParser.h:90
roboticslab::CanOpenNode
Wrapper for CAN protocol handles with handy accessors.
Definition CanOpenNode.hpp:35
roboticslab::EmcyConsumer::code_t
std::pair< std::uint16_t, std::string > code_t
Emergency error code.
Definition EmcyConsumer.hpp:35
roboticslab::ICanBusSharer
Abstract base for a CAN bus sharer.
Definition ICanBusSharer.hpp:25
roboticslab::ICanSenderDelegate
Implementation-agnostic consumer for TX CAN transfers.
Definition ICanSenderDelegate.hpp:22
roboticslab::TechnosoftIposBase
Base class for all proxied TechnosoftIpos implementations.
Definition TechnosoftIposBase.hpp:79
roboticslab::TechnosoftIposBase::getId
unsigned int getId() override
Retrieve CAN node ID.
Definition ICanBusSharerImpl.cpp:39
roboticslab::TechnosoftIposBase::internalUnitsToDegrees
double internalUnitsToDegrees(double value, int derivativeOrder=0) const
Convert position, speed or acceleration to degrees.
Definition TechnosoftIposBase.cpp:665
roboticslab::TechnosoftIposBase::currentToInternalUnits
std::int16_t currentToInternalUnits(double value) const
Convert current to internal units.
Definition TechnosoftIposBase.cpp:672
roboticslab::TechnosoftIposBase::initialize
bool initialize() override
Perform CAN node initialization.
Definition ICanBusSharerImpl.cpp:67
roboticslab::TechnosoftIposBase::getAdditionalIds
std::vector< unsigned int > getAdditionalIds() override
Retrieve more associated CAN node IDs, if any.
Definition ICanBusSharerImpl.cpp:46
roboticslab::TechnosoftIposBase::currentToTorque
double currentToTorque(double current) const
Convert current (amperes) to torque (Nm).
Definition TechnosoftIposBase.cpp:693
roboticslab::TechnosoftIposBase::internalUnitsToPeakCurrent
double internalUnitsToPeakCurrent(std::int16_t value) const
Apply internal iPOS conversion to express drive peak current in amperes.
Definition TechnosoftIposBase.cpp:686
roboticslab::TechnosoftIposBase::notifyMessage
bool notifyMessage(const can_message &message) override
Notify observers that a new CAN message has arrived.
Definition ICanBusSharerImpl.cpp:186
roboticslab::TechnosoftIposBase::registerSender
bool registerSender(ICanSenderDelegate *sender) override
Pass a handle to a CAN sender delegate instance.
Definition ICanBusSharerImpl.cpp:58
roboticslab::TechnosoftIposBase::internalUnitsToCurrent
double internalUnitsToCurrent(std::int16_t value) const
Convert current to amperes.
Definition TechnosoftIposBase.cpp:679
roboticslab::TechnosoftIposBase::finalize
bool finalize() override
Finalize CAN node communications.
Definition ICanBusSharerImpl.cpp:144
roboticslab::TechnosoftIposBase::validateInitialState
static bool validateInitialState(const TechnosoftIpos_ParamsParser &params, const std::string &id)
Make sure stored variables actually make sense.
Definition TechnosoftIposBase.cpp:505
roboticslab::TechnosoftIposBase::torqueToCurrent
double torqueToCurrent(double torque) const
Convert torque (Nm) to current (amperes).
Definition TechnosoftIposBase.cpp:700
roboticslab::TechnosoftIposBase::degreesToInternalUnits
double degreesToInternalUnits(double value, int derivativeOrder=0) const
Convert position, speed or acceleration to internal units.
Definition TechnosoftIposBase.cpp:658
roboticslab::TechnosoftIposBase::awaitControlMode
bool awaitControlMode(yarp::conf::vocab32_t mode)
Wait with timeout for requested control mode change.
Definition TechnosoftIposBase.cpp:651
roboticslab::NmtState
NmtState
NMT state machine.
Definition NmtProtocol.hpp:33
roboticslab::DriveState
DriveState
CiA 402 drive states (associated to statusword).
Definition DriveStatusMachine.hpp:22
roboticslab::DriveState::NOT_READY_TO_SWITCH_ON
@ NOT_READY_TO_SWITCH_ON
Not ready to switch on state.
roboticslab
The main, catch-all namespace for Robotics Lab UC3M.
Definition groups.dox:6
roboticslab::TechnosoftIposBase::report_storage
Definition TechnosoftIposBase.hpp:592
roboticslab::can_message
Proxy CAN message structure.
Definition CanMessage.hpp:20
Generated by doxygen 1.9.8