yarp-devices
FakeJoint.hpp
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 #ifndef __FAKE_JOINT_HPP__
4 #define __FAKE_JOINT_HPP__
5 
6 #include <string>
7 
8 #include <yarp/os/SystemClock.h>
9 #include <yarp/dev/ControlBoardInterfaces.h>
10 
11 #include "ICanBusSharer.hpp"
12 
13 constexpr auto JOINT_TYPE = yarp::dev::VOCAB_JOINTTYPE_REVOLUTE; // yarpmotorgui can't handle VOCAB_JOINTTYPE_UNKNOWN
14 
15 namespace roboticslab
16 {
17 
28 class FakeJoint : public yarp::dev::DeviceDriver,
29  public yarp::dev::IAmplifierControlRaw,
30  public yarp::dev::IAxisInfoRaw,
31  public yarp::dev::IControlCalibrationRaw,
32  public yarp::dev::IControlLimitsRaw,
33  public yarp::dev::IControlModeRaw,
34  public yarp::dev::ICurrentControlRaw,
35  public yarp::dev::IEncodersTimedRaw,
36  public yarp::dev::IImpedanceControlRaw,
37  public yarp::dev::IInteractionModeRaw,
38  public yarp::dev::IMotorRaw,
39  public yarp::dev::IMotorEncodersRaw,
40  public yarp::dev::IPidControlRaw,
41  public yarp::dev::IPositionControlRaw,
42  public yarp::dev::IPositionDirectRaw,
43  public yarp::dev::IPWMControlRaw,
44  public yarp::dev::IRemoteVariablesRaw,
45  public yarp::dev::ITorqueControlRaw,
46  public yarp::dev::IVelocityControlRaw,
47  public ICanBusSharer
48 {
49 public:
50  // --------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp ---------
51 
52  bool open(yarp::os::Searchable & config) override;
53 
54  bool close() override
55  { return true; }
56 
57  // --------- ICanBusSharer declarations ---------
58 
59  unsigned int getId() override
60  { return 0; }
61  bool notifyMessage(const can_message & message) override
62  { return true; }
63  bool initialize() override
64  { return true; }
65  bool finalize() override
66  { return true; }
67  bool registerSender(ICanSenderDelegate * sender) override
68  { return true; }
69  bool synchronize(double timestamp) override
70  { return true; }
71 
72  // --------- IAmplifierControlRaw declarations ---------
73 
74  bool enableAmpRaw(int j) override
75  { return true; }
76  bool disableAmpRaw(int j) override
77  { return true; }
78  bool getAmpStatusRaw(int j, int * v) override
79  { *v = 0; return true; }
80  bool getAmpStatusRaw(int * st) override
81  { *st = 0; return true; }
82  //bool getCurrentRaw(int j, double * val) override;
83  //bool getCurrentsRaw(double * vals) override;
84  bool getMaxCurrentRaw(int j, double * v) override
85  { *v = 0.0; return true; }
86  bool setMaxCurrentRaw(int j, double v) override
87  { return true; }
88  bool getNominalCurrentRaw(int m, double * val) override
89  { *val = 0.0; return true; }
90  bool setNominalCurrentRaw(int m, double val) override
91  { return true; }
92  bool getPeakCurrentRaw(int m, double * val) override
93  { *val = 0.0; return true; }
94  bool setPeakCurrentRaw(int m, double val) override
95  { return true; }
96  bool getPWMRaw(int j, double * val) override
97  { *val = 0.0; return true; }
98  bool getPWMLimitRaw(int j, double * val) override
99  { *val = 0.0; return true; }
100  bool setPWMLimitRaw(int j, double val) override
101  { return true; }
102  bool getPowerSupplyVoltageRaw(int j, double * val) override
103  { *val = 0.0; return true; }
104 
105  // --------- IAxisInfoRaw declarations ---------
106 
107  bool getAxisNameRaw(int axis, std::string & name) override
108  { name = jointName; return true; }
109  bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum & type) override
110  { type = JOINT_TYPE; return true; }
111 
112  // --------- IControlCalibrationRaw declarations ---------
113 
114  bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
115  { return true; }
116  bool setCalibrationParametersRaw(int axis, const yarp::dev::CalibrationParameters & params) override
117  { return true; }
118  bool calibrationDoneRaw(int j) override
119  { return true; }
120 
121  // --------- IControlLimitsRaw declarations ---------
122 
123  bool setLimitsRaw(int axis, double min, double max) override
124  { return true; }
125  bool getLimitsRaw(int axis, double * min, double * max) override
126  { *min = *max = 0.0; return true; }
127  bool setVelLimitsRaw(int axis, double min, double max) override
128  { return true; }
129  bool getVelLimitsRaw(int axis, double * min, double * max) override
130  { *min = *max = 0.0; return true; }
131 
132  // --------- IControlModeRaw declarations ---------
133 
134  bool getControlModeRaw(int j, int * mode) override
135  { *mode = controlMode; return true; }
136  bool getControlModesRaw(int * modes) override
137  { return getControlModeRaw(0, &modes[0]); }
138  bool getControlModesRaw(int n_joint, const int * joints, int * modes) override
139  { return getControlModeRaw(0, &modes[0]); }
140  bool setControlModeRaw(int j, int mode) override
141  { controlMode = mode; return true; }
142  bool setControlModesRaw(int * modes) override
143  { return setControlModeRaw(0, modes[0]); }
144  bool setControlModesRaw(int n_joint, const int * joints, int * modes) override
145  { return setControlModeRaw(0, modes[0]); }
146 
147  // --------- ICurrentControlRaw declarations ---------
148 
149  //bool getNumberOfMotorsRaw(int * ax) override;
150  bool getCurrentRaw(int m, double * curr) override
151  { *curr = 0.0; return true; }
152  bool getCurrentsRaw(double * currs) override
153  { return getCurrentRaw(0, &currs[0]); }
154  bool getCurrentRangeRaw(int m, double * min, double * max) override
155  { *min = *max = 0.0; return true; }
156  bool getCurrentRangesRaw(double * mins, double * maxs) override
157  { return getCurrentRangeRaw(0, &mins[0], &maxs[0]); }
158  bool setRefCurrentRaw(int m, double curr) override
159  { return true; }
160  bool setRefCurrentsRaw(const double * currs) override
161  { return true; }
162  bool setRefCurrentsRaw(int n_motor, const int * motors, const double * currs) override
163  { return true; }
164  bool getRefCurrentRaw(int m, double * curr) override
165  { *curr = 0.0; return true; }
166  bool getRefCurrentsRaw(double * currs) override
167  { return getRefCurrentRaw(0, &currs[0]); }
168 
169  // ---------- IEncodersRaw declarations ----------.
170 
171  bool getAxes(int * ax) override
172  { *ax = 1; return true; }
173  bool resetEncoderRaw(int j) override
174  { return true; }
175  bool resetEncodersRaw() override
176  { return true; }
177  bool setEncoderRaw(int j, double val) override
178  { return true; }
179  bool setEncodersRaw(const double * vals) override
180  { return true; }
181  bool getEncoderRaw(int j, double * v) override
182  { *v = 0.0; return true; }
183  bool getEncodersRaw(double * encs) override
184  { return getEncoderRaw(0, &encs[0]); }
185  bool getEncoderSpeedRaw(int j, double * spd) override
186  { *spd = 0.0; return true; }
187  bool getEncoderSpeedsRaw(double * spds) override
188  { return getEncoderRaw(0, &spds[0]); }
189  bool getEncoderAccelerationRaw(int j, double * acc) override
190  { *acc = 0.0; return true; }
191  bool getEncoderAccelerationsRaw(double * accs) override
192  { return getEncoderAccelerationRaw(0, &accs[0]); }
193 
194  // ---------- IEncodersTimedRaw declarations ----------
195 
196  bool getEncoderTimedRaw(int j, double * enc, double * time) override
197  { *enc = 0.0; *time = yarp::os::SystemClock::nowSystem(); return true; }
198  bool getEncodersTimedRaw(double * encs, double * times) override
199  { return getEncoderTimedRaw(0, &encs[0], &times[0]); }
200 
201  // --------- IImpedanceControlRaw declarations ---------
202 
203  //bool getAxes(int * ax) override;
204  bool getImpedanceRaw(int j, double * stiffness, double * damping) override
205  { *stiffness = *damping = 0.0; return true; }
206  bool setImpedanceRaw(int j, double stiffness, double damping) override
207  { return true; }
208  bool setImpedanceOffsetRaw(int j, double offset) override
209  { return true; }
210  bool getImpedanceOffsetRaw(int j, double * offset) override
211  { *offset = 0.0; return true; }
212  bool getCurrentImpedanceLimitRaw(int j, double * min_stiff, double * max_stiff, double * min_damp, double * max_damp) override
213  { *min_stiff = *max_stiff = *min_damp = *max_damp = 0.0; return true; }
214 
215  // ------- IInteractionModeRaw declarations -------
216 
217  bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum * mode) override
218  { *mode = interactionMode; return true; }
219  bool getInteractionModesRaw(yarp::dev::InteractionModeEnum * modes) override
220  { return getInteractionModeRaw(0, &modes[0]); }
221  bool getInteractionModesRaw(int n_joints, int * joints, yarp::dev::InteractionModeEnum * modes) override
222  { return getInteractionModeRaw(0, &modes[0]); }
223  bool setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode) override
224  { interactionMode = mode; return true; }
225  bool setInteractionModesRaw(yarp::dev::InteractionModeEnum * modes) override
226  { return true; }
227  bool setInteractionModesRaw(int n_joints, int * joints, yarp::dev::InteractionModeEnum * modes) override
228  { return true; }
229 
230  // --------- IMotorRaw declarations ---------
231 
232  bool getNumberOfMotorsRaw(int * num) override
233  { return getAxes(num); }
234  bool getTemperatureRaw(int m, double * val) override
235  { *val = 0.0; return true; }
236  bool getTemperaturesRaw(double * vals) override
237  { return getTemperatureRaw(0, &vals[0]); }
238  bool getTemperatureLimitRaw(int m, double * temp) override
239  { *temp = 0.0; return true; }
240  bool setTemperatureLimitRaw(int m, double temp) override
241  { return true; }
242  bool getGearboxRatioRaw(int m, double * val) override
243  { *val = 0.0; return true; }
244  bool setGearboxRatioRaw(int m, double val) override
245  { return true; }
246 
247  // --------- IMotorEncodersRaw declarations ---------
248 
249  bool getNumberOfMotorEncodersRaw(int * num) override
250  { return getAxes(num); }
251  bool resetMotorEncoderRaw(int m) override
252  { return true; }
253  bool resetMotorEncodersRaw() override
254  { return true; }
255  bool setMotorEncoderCountsPerRevolutionRaw(int m, double cpr) override
256  { return true; }
257  bool getMotorEncoderCountsPerRevolutionRaw(int m, double * cpr) override
258  { *cpr = 0.0; return true; }
259  bool setMotorEncoderRaw(int m, double val) override
260  { return true; }
261  bool setMotorEncodersRaw(const double * vals) override
262  { return true; }
263  bool getMotorEncoderRaw(int m, double * v) override
264  { *v = 0.0; return true; }
265  bool getMotorEncodersRaw(double * encs) override
266  { return getMotorEncoderRaw(0, &encs[0]); }
267  bool getMotorEncoderTimedRaw(int m, double * enc, double * stamp) override
268  { *enc = 0.0; *stamp = yarp::os::SystemClock::nowSystem(); return true; }
269  bool getMotorEncodersTimedRaw(double * encs, double * stamps) override
270  { return getMotorEncoderTimedRaw(0, &encs[0], &stamps[0]); }
271  bool getMotorEncoderSpeedRaw(int m, double * sp) override
272  { *sp = 0.0; return true; }
273  bool getMotorEncoderSpeedsRaw(double *spds) override
274  { return getMotorEncoderSpeedRaw(0, &spds[0]); }
275  bool getMotorEncoderAccelerationRaw(int m, double * acc) override
276  { *acc = 0.0; return true; }
277  bool getMotorEncoderAccelerationsRaw(double * accs) override
278  { return getMotorEncoderAccelerationRaw(0, &accs[0]); }
279 
280  // --------- IPidControlRaw declarations ---------
281 
282  bool setPidRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, const yarp::dev::Pid & pid) override
283  { return true; }
284  bool setPidsRaw(const yarp::dev::PidControlTypeEnum & pidtype, const yarp::dev::Pid * pids) override
285  { return true; }
286  bool setPidReferenceRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double ref) override
287  { return true; }
288  bool setPidReferencesRaw(const yarp::dev::PidControlTypeEnum & pidtype, const double * refs) override
289  { return true; }
290  bool setPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double limit) override
291  { return true; }
292  bool setPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum & pidtype, const double * limits) override
293  { return true; }
294  bool getPidErrorRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * err) override
295  { *err = 0.0; return true; }
296  bool getPidErrorsRaw(const yarp::dev::PidControlTypeEnum & pidtype, double * errs) override
297  { return getPidErrorRaw(pidtype, 0, &errs[0]); }
298  bool getPidOutputRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * out) override
299  { *out = 0.0; return true; }
300  bool getPidOutputsRaw(const yarp::dev::PidControlTypeEnum & pidtype, double * outs) override
301  { return getPidOutputRaw(pidtype, 0, &outs[0]); }
302  bool getPidRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, yarp::dev::Pid * pid) override
303  { return true; }
304  bool getPidsRaw(const yarp::dev::PidControlTypeEnum & pidtype, yarp::dev::Pid * pids) override
305  { return true; }
306  bool getPidReferenceRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * ref) override
307  { *ref = 0.0; return true; }
308  bool getPidReferencesRaw(const yarp::dev::PidControlTypeEnum & pidtype, double * refs) override
309  { return getPidReferenceRaw(pidtype, 0, &refs[0]); }
310  bool getPidErrorLimitRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * limit) override
311  { *limit = 0.0; return true; }
312  bool getPidErrorLimitsRaw(const yarp::dev::PidControlTypeEnum & pidtype, double * limits) override
313  { return getPidErrorLimitRaw(pidtype, 0, &limits[0]); }
314  bool resetPidRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j) override
315  { return true; }
316  bool disablePidRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j) override
317  { return true; }
318  bool enablePidRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j) override
319  { return true; }
320  bool setPidOffsetRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, double v) override
321  { return true; }
322  bool isPidEnabledRaw(const yarp::dev::PidControlTypeEnum & pidtype, int j, bool * enabled) override
323  { *enabled = true; return true; }
324 
325  // ------- IPositionControlRaw declarations -------
326 
327  //bool getAxesRaw(int * ax) override;
328  bool positionMoveRaw(int j, double ref) override
329  { return true; }
330  bool positionMoveRaw(const double * refs) override
331  { return true; }
332  bool positionMoveRaw(int n_joint, const int * joints, const double * refs) override
333  { return true; }
334  bool relativeMoveRaw(int j, double delta) override
335  { return true; }
336  bool relativeMoveRaw(const double * deltas) override
337  { return true; }
338  bool relativeMoveRaw(int n_joint, const int * joints, const double * deltas) override
339  { return true; }
340  bool checkMotionDoneRaw(int j, bool * flag) override
341  { *flag = true; return true; }
342  bool checkMotionDoneRaw(bool * flag) override
343  { return checkMotionDoneRaw(0, flag); }
344  bool checkMotionDoneRaw(int n_joint, const int * joints, bool * flag) override
345  { return checkMotionDoneRaw(0, flag); }
346  bool setRefSpeedRaw(int j, double sp) override
347  { return true; }
348  bool setRefSpeedsRaw(const double * spds) override
349  { return true; }
350  bool setRefSpeedsRaw(int n_joint, const int * joints, const double * spds) override
351  { return true; }
352  bool setRefAccelerationRaw(int j, double acc) override
353  { return true; }
354  bool setRefAccelerationsRaw(const double * accs) override
355  { return true; }
356  bool setRefAccelerationsRaw(int n_joint, const int * joints, const double * accs) override
357  { return true; }
358  bool getRefSpeedRaw(int j, double * spd) override
359  { *spd = 0.0; return true; }
360  bool getRefSpeedsRaw(double * spds) override
361  { return getRefSpeedRaw(0, &spds[0]); }
362  bool getRefSpeedsRaw(int n_joint, const int * joints, double * spds) override
363  { return getRefSpeedRaw(0, &spds[0]); }
364  bool getRefAccelerationRaw(int j, double * acc) override
365  { *acc = 0.0; return true; }
366  bool getRefAccelerationsRaw(double * accs) override
367  { return getRefAccelerationRaw(0, &accs[0]); }
368  bool getRefAccelerationsRaw(int n_joint, const int * joints, double * accs) override
369  { return getRefAccelerationRaw(0, &accs[0]); }
370  bool stopRaw(int j) override
371  { return true; }
372  bool stopRaw() override
373  { return true; }
374  bool stopRaw(int n_joint, const int *joints) override
375  { return true; }
376  bool getTargetPositionRaw(int joint, double * ref) override
377  { *ref = 0.0; return true; }
378  bool getTargetPositionsRaw(double * refs) override
379  { return getTargetPositionRaw(0, &refs[0]); }
380  bool getTargetPositionsRaw(int n_joint, const int * joints, double * refs) override
381  { return getTargetPositionRaw(0, &refs[0]); }
382 
383  // ------- IPositionDirectRaw declarations -------
384 
385  //bool getAxesRaw(int * ax) override;
386  bool setPositionRaw(int j, double ref) override
387  { return true; }
388  bool setPositionsRaw(const double * refs) override
389  { return true; }
390  bool setPositionsRaw(int n_joint, const int * joints, const double * refs) override
391  { return true; }
392  bool getRefPositionRaw(int joint, double * ref) override
393  { *ref = 0.0; return true; }
394  bool getRefPositionsRaw(double * refs) override
395  { return getRefPositionRaw(0, &refs[0]); }
396  bool getRefPositionsRaw(int n_joint, const int * joints, double * refs) override
397  { return getRefPositionRaw(0, &refs[0]); }
398 
399  // --------- IPWMControl declarations ---------
400 
401  //bool getNumberOfMotorsRaw(int * number) override;
402  bool setRefDutyCycleRaw(int m, double ref) override
403  { return true; }
404  bool setRefDutyCyclesRaw(const double * refs) override
405  { return true; }
406  bool getRefDutyCycleRaw(int m, double * ref) override
407  { *ref = 0.0; return true; }
408  bool getRefDutyCyclesRaw(double * refs) override
409  { return getRefDutyCycleRaw(0, &refs[0]); }
410  bool getDutyCycleRaw(int m, double * val) override
411  { *val = 0.0; return true; }
412  bool getDutyCyclesRaw(double * vals) override
413  { return getDutyCycleRaw(0, &vals[0]); }
414 
415  // ------- IRemoteVariablesRaw declarations -------
416 
417  bool getRemoteVariableRaw(std::string key, yarp::os::Bottle & val) override
418  { return true; }
419  bool setRemoteVariableRaw(std::string key, const yarp::os::Bottle & val) override
420  { return true; }
421  bool getRemoteVariablesListRaw(yarp::os::Bottle * listOfKeys) override
422  { return true; }
423 
424  // -------- ITorqueControlRaw declarations --------
425 
426  //bool getAxesRaw(int * ax) override;
427  bool getRefTorqueRaw(int j, double * t) override
428  { *t = 0.0; return true; }
429  bool getRefTorquesRaw(double * t) override
430  { return getRefTorqueRaw(0, &t[0]); }
431  bool setRefTorqueRaw(int j, double t) override
432  { return true; }
433  bool setRefTorquesRaw(const double * t) override
434  { return true; }
435  bool setRefTorquesRaw(int n_joint, const int * joints, const double * t) override
436  { return true; }
437  bool getMotorTorqueParamsRaw(int j, yarp::dev::MotorTorqueParameters * params) override
438  { return true; }
439  bool setMotorTorqueParamsRaw(int j, const yarp::dev::MotorTorqueParameters params) override
440  { return true; }
441  bool getTorqueRaw(int j, double * t) override
442  { *t = 0.0; return true; }
443  bool getTorquesRaw(double * t) override
444  { return getTorqueRaw(0, &t[0]); }
445  bool getTorqueRangeRaw(int j, double * min, double * max) override
446  { *min = *max = 0.0; return true; }
447  bool getTorqueRangesRaw(double * mins, double * maxs) override
448  { return getTorqueRangeRaw(0, &mins[0], &maxs[0]); }
449 
450  // --------- IVelocityControl declarations ---------
451 
452  //bool getAxesRaw(int * ax) override;
453  bool velocityMoveRaw(int j, double spd) override
454  { return true; }
455  bool velocityMoveRaw(const double * spds) override
456  { return true; }
457  bool velocityMoveRaw(int n_joint, const int * joints, const double * spds) override
458  { return true; }
459  bool getRefVelocityRaw(int joint, double * vel) override
460  { *vel = 0.0; return true; }
461  bool getRefVelocitiesRaw(double * vels) override
462  { return getRefVelocityRaw(0, &vels[0]); }
463  bool getRefVelocitiesRaw(int n_joint, const int * joints, double * vels) override
464  { return getRefVelocityRaw(0, &vels[0]); }
465  //bool setRefAccelerationRaw(int j, double acc) override;
466  //bool setRefAccelerationsRaw(const double * accs) override;
467  //bool setRefAccelerationsRaw(int n_joint, const int * joints, const double * accs) override;
468  //bool getRefAccelerationRaw(int j, double * acc) override;
469  //bool getRefAccelerationsRaw(double * accs) override;
470  //bool getRefAccelerationsRaw(int n_joint, const int * joints, double * accs) override;
471  //bool stopRaw(int j) override;
472  //bool stopRaw() override;
473  //bool stopRaw(int n_joint, const int *joints) override;
474 
475 private:
476  std::string jointName;
477  int controlMode {VOCAB_CM_CONFIGURED};
478  yarp::dev::InteractionModeEnum interactionMode {yarp::dev::VOCAB_IM_UNKNOWN};
479 };
480 
481 } // namespace roboticslab
482 
483 #endif // __FAKE_JOINT_HPP__
Implementation for a fake joint (instant movement) as a single CAN bus joint (control board raw inter...
Definition: FakeJoint.hpp:48
bool notifyMessage(const can_message &message) override
Notify observers that a new CAN message has arrived.
Definition: FakeJoint.hpp:61
unsigned int getId() override
Retrieve CAN node ID.
Definition: FakeJoint.hpp:59
bool synchronize(double timestamp) override
Perform synchronized action on CAN master's request.
Definition: FakeJoint.hpp:69
bool initialize() override
Perform CAN node initialization.
Definition: FakeJoint.hpp:63
bool finalize() override
Finalize CAN node communications.
Definition: FakeJoint.hpp:65
bool registerSender(ICanSenderDelegate *sender) override
Pass a handle to a CAN sender delegate instance.
Definition: FakeJoint.hpp:67
Abstract base for a CAN bus sharer.
Definition: ICanBusSharer.hpp:25
Implementation-agnostic consumer for TX CAN transfers.
Definition: ICanSenderDelegate.hpp:22
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
Proxy CAN message structure.
Definition: CanMessage.hpp:20