YARP
Yet Another Robot Platform
FakeMotor.h
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4  * SPDX-License-Identifier: BSD-3-Clause
5  */
6 
7 #ifndef YARP_FAKEMOTOR_FAKEMOTOR_H
8 #define YARP_FAKEMOTOR_FAKEMOTOR_H
9 
10 #include <cstdio>
11 
13 #include <yarp/dev/Drivers.h>
14 #include <yarp/dev/PolyDriver.h>
15 
16 #include <yarp/os/Time.h>
17 #include <yarp/os/Log.h>
18 #include <yarp/os/LogComponent.h>
19 #include <yarp/sig/Vector.h>
20 
22 
23 
30 class FakeMotor :
31  public yarp::dev::DeprecatedDeviceDriver,
32  public yarp::dev::IPositionControl,
33  public yarp::dev::IEncodersTimed,
34  public yarp::dev::IVelocityControl
35 {
36 private:
37  int njoints{1};
39  yarp::sig::Vector speed;
42  double delay{0};
43  double last{-1};
44  bool posMode{true};
45 
46  void update();
47 
48 public:
49  FakeMotor() = default;
50  FakeMotor(const FakeMotor&) = delete;
51  FakeMotor(FakeMotor&&) = delete;
52  FakeMotor& operator=(const FakeMotor&) = delete;
54  ~FakeMotor() override = default;
55 
56  bool getAxes(int *ax) override
57  {
58  *ax = njoints;
59  yCInfo(FAKEMOTOR, "FakeMotor reporting %d axes present", *ax);
60  return true;
61  }
62 
63  bool open(yarp::os::Searchable& config) override
64  {
65  njoints = config.check("axes",yarp::os::Value(4),"number of axes to pretend to have").asInt32();
66  pos.resize(njoints);
67  speed.resize(njoints);
68  acc.resize(njoints);
69  vel.resize(njoints);
70  for (int i=0; i<njoints; i++) {
71  pos[i] = 0;
72  speed[i] = 0;
73  acc[i] = 0;
74  vel[i] = 0;
75  }
76 
77  delay=config.check("delay",yarp::os::Value(0), "delay in each call for debugging purpose, in ms").asInt32();
78  return true;
79  }
80 
81 
82  bool positionMove(int j, double ref) override
83  {
84  posMode = true;
85  if (j<njoints) {
86  pos[j] = ref;
87  }
88  return true;
89  }
90 
91 
92  bool positionMove(const double *refs) override
93  {
94  posMode = true;
95  for (int i=0; i<njoints; i++) {
96  pos[i] = refs[i];
97  }
98  return true;
99  }
100 
101 
102  bool relativeMove(int j, double delta) override
103  {
104  posMode = true;
105  if (j<njoints) {
106  pos[j] += delta;
107  }
108  return true;
109  }
110 
111 
112  bool relativeMove(const double *deltas) override
113  {
114  posMode = true;
115  for (int i=0; i<njoints; i++) {
116  pos[i] += deltas[i];
117  }
118  return true;
119  }
120 
121 
122  bool checkMotionDone(int j, bool *flag) override
123  {
125  return true;
126  }
127 
128 
129  bool checkMotionDone(bool *flag) override
130  {
131  return true;
132  }
133 
134 
135  bool setRefSpeed(int j, double sp) override
136  {
137  if (j<njoints) {
138  speed[j] = sp;
139  }
140  return true;
141  }
142 
143 
144  bool setRefSpeeds(const double *spds) override
145  {
146  for (int i=0; i<njoints; i++) {
147  speed[i] = spds[i];
148  }
149  return true;
150  }
151 
152 
153  bool setRefAcceleration(int j, double acc) override
154  {
155  if (j<njoints) {
156  this->acc[j] = acc;
157  }
158  return true;
159  }
160 
161 
162  bool setRefAccelerations(const double *accs) override
163  {
164  for (int i=0; i<njoints; i++) {
165  acc[i] = accs[i];
166  }
167  return true;
168  }
169 
170 
171  bool getRefSpeed(int j, double *ref) override
172  {
173  if (j<njoints) {
174  (*ref) = speed[j];
175  }
176  return true;
177  }
178 
179 
180  bool getRefSpeeds(double *spds) override
181  {
182  for (int i=0; i<njoints; i++) {
183  spds[i] = speed[i];
184  }
185  return true;
186  }
187 
188 
189  bool getRefAcceleration(int j, double *acc) override
190  {
191  if (j<njoints) {
192  (*acc) = this->acc[j];
193  }
194  return true;
195  }
196 
197 
198  bool getRefAccelerations(double *accs) override
199  {
200  for (int i=0; i<njoints; i++) {
201  accs[i] = acc[i];
202  }
203  return true;
204  }
205 
206 
207  bool stop(int j) override
208  {
209  return true;
210  }
211 
212 
213  bool stop() override
214  {
215  return true;
216  }
217 
218  bool positionMove(const int n_joint, const int *joints, const double *refs) override
219  {
220  return false;
221  }
222 
223  bool relativeMove(const int n_joint, const int *joints, const double *deltas) override
224  {
225  return false;
226  }
227 
228  bool checkMotionDone(const int n_joint, const int *joints, bool *flags) override
229  {
230  return false;
231  }
232 
233  bool setRefSpeeds(const int n_joint, const int *joints, const double *spds) override
234  {
235  return false;
236  }
237 
238  bool setRefAccelerations(const int n_joint, const int *joints, const double *accs) override
239  {
240  return false;
241  }
242 
243  bool getRefSpeeds(const int n_joint, const int *joints, double *spds) override
244  {
245  return false;
246  }
247 
248  bool getRefAccelerations(const int n_joint, const int *joints, double *accs) override
249  {
250  return false;
251  }
252 
253  bool stop(const int n_joint, const int *joints) override
254  {
255  return false;
256  }
257 
258  bool getTargetPosition(const int joint, double *ref) override
259  {
260  return false;
261  }
262 
263  bool getTargetPositions(double *refs) override
264  {
265  return false;
266  }
267 
268  bool getTargetPositions(const int n_joint, const int *joints, double *refs) override
269  {
270  return false;
271  }
272 
273  bool velocityMove(const int n_joint, const int *joints, const double *spds) override
274  {
275  return false;
276  }
277 
278  bool close() override
279  {
280  return true;
281  }
282 
283  bool resetEncoder(int j) override
284  {
285  if (j<njoints) {
286  pos[j] = 0;
287  }
288  return true;
289  }
290 
291  bool resetEncoders() override
292  {
293  for (int i=0; i<njoints; i++) {
294  pos[i] = 0;
295  }
296  last = yarp::os::Time::now();
297  return true;
298  }
299 
300  bool setEncoder(int j, double val) override
301  {
302  if (j<njoints) {
303  pos[j] = val;
304  }
305  last = yarp::os::Time::now(); // not quite right, but ok for test
306  return true;
307  }
308 
309  bool setEncoders(const double *vals) override
310  {
311  for (int i=0; i<njoints; i++) {
312  pos[i] = vals[i];
313  }
314  last = yarp::os::Time::now();
315  return true;
316  }
317 
318  bool getEncoder(int j, double *v) override
319  {
320  update();
321  if (j<njoints) {
322  (*v) = pos[j];
323  }
324  return true;
325  }
326 
327  bool getEncoders(double *encs) override
328  {
329  update();
330  for (int i=0; i<njoints; i++) {
331  encs[i] = pos[i];
332  }
333  return true;
334  }
335 
336 
337  bool getEncoderTimed(int j, double *encs, double *time) override
338  {
339  bool ret = getEncoder(j, encs);
340  *time = yarp::os::Time::now();
341  return ret;
342  }
343 
344  bool getEncodersTimed(double *encs, double *time) override
345  {
346  for (int i=0; i<njoints; i++)
347  {
348  getEncoderTimed(i, &encs[i], &time[i]);
349  }
350  return true;
351  }
352 
353  bool getEncoderSpeed(int j, double *sp) override
354  {
355  if (j<njoints) {
356  (*sp) = 0;
357  }
358  return true;
359  }
360 
361  bool getEncoderSpeeds(double *spds) override
362  {
363  for (int i=0; i<njoints; i++) {
364  spds[i] = 0;
365  }
366  return true;
367  }
368 
369  bool getEncoderAcceleration(int j, double *spds) override
370  {
371  if (j<njoints) {
372  (*spds) = 0;
373  }
374  return true;
375  }
376 
377  bool getEncoderAccelerations(double *accs) override
378  {
379  for (int i=0; i<njoints; i++) {
380  accs[i] = 0;
381  }
382  return true;
383  }
384 
385  bool velocityMove(int j, double sp) override
386  {
387  posMode = false;
388  if (j<njoints) {
389  vel[j] = sp;
390  }
391  return true;
392  }
393 
394  bool velocityMove(const double *sp) override
395  {
396  posMode = false;
397  for (int i=0; i<njoints; i++) {
398  vel[i] = sp[i];
399  }
400  return true;
401  }
402 
403 };
404 
409 class TestMotor : public FakeMotor
410 {
411 public:
412  bool open(yarp::os::Searchable& config) override;
413 };
414 
415 
416 #endif // YARP_FAKEMOTOR_FAKEMOTOR_H
define control board standard interfaces
const yarp::os::LogComponent & FAKEMOTOR()
Definition: FakeMotor.cpp:9
bool ret
contains the definition of a Vector type
fakeMotor deprecated: A fake motor control board for testing.
Definition: FakeMotor.h:35
bool checkMotionDone(bool *flag) override
Check if the current trajectory is terminated.
Definition: FakeMotor.h:129
FakeMotor(const FakeMotor &)=delete
bool setEncoders(const double *vals) override
Set the value of all encoders.
Definition: FakeMotor.h:309
bool setRefAccelerations(const double *accs) override
Set reference acceleration on all joints.
Definition: FakeMotor.h:162
bool setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
Definition: FakeMotor.h:300
bool positionMove(int j, double ref) override
Set new reference point for a single axis.
Definition: FakeMotor.h:82
bool getRefSpeeds(const int n_joint, const int *joints, double *spds) override
Get reference speed of all joints.
Definition: FakeMotor.h:243
bool setRefAccelerations(const int n_joint, const int *joints, const double *accs) override
Set reference acceleration on all joints.
Definition: FakeMotor.h:238
~FakeMotor() override=default
bool relativeMove(const int n_joint, const int *joints, const double *deltas) override
Set relative position for a subset of joints.
Definition: FakeMotor.h:223
bool stop(int j) override
Stop motion, single joint.
Definition: FakeMotor.h:207
bool velocityMove(const int n_joint, const int *joints, const double *spds) override
Start motion at a given speed for a subset of joints.
Definition: FakeMotor.h:273
bool getEncodersTimed(double *encs, double *time) override
Read the instantaneous acceleration of all axes.
Definition: FakeMotor.h:344
bool setRefSpeeds(const double *spds) override
Set reference speed on all joints.
Definition: FakeMotor.h:144
bool resetEncoders() override
Reset encoders.
Definition: FakeMotor.h:291
bool getRefAccelerations(double *accs) override
Get reference acceleration of all joints.
Definition: FakeMotor.h:198
bool getEncoder(int j, double *v) override
Read the value of an encoder.
Definition: FakeMotor.h:318
bool getRefSpeeds(double *spds) override
Get reference speed of all joints.
Definition: FakeMotor.h:180
bool getEncoderAcceleration(int j, double *spds) override
Read the instantaneous acceleration of an axis.
Definition: FakeMotor.h:369
bool setRefAcceleration(int j, double acc) override
Set reference acceleration for a joint.
Definition: FakeMotor.h:153
bool setRefSpeeds(const int n_joint, const int *joints, const double *spds) override
Set reference speed on all joints.
Definition: FakeMotor.h:233
bool getTargetPositions(double *refs) override
Get the last position reference for all axes.
Definition: FakeMotor.h:263
bool setRefSpeed(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
Definition: FakeMotor.h:135
bool getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
Definition: FakeMotor.h:353
bool relativeMove(int j, double delta) override
Set relative position.
Definition: FakeMotor.h:102
FakeMotor & operator=(const FakeMotor &)=delete
FakeMotor & operator=(FakeMotor &&)=delete
bool getRefAcceleration(int j, double *acc) override
Get reference acceleration for a joint.
Definition: FakeMotor.h:189
bool checkMotionDone(const int n_joint, const int *joints, bool *flags) override
Check if the current trajectory is terminated.
Definition: FakeMotor.h:228
bool checkMotionDone(int j, bool *flag) override
Check if the current trajectory is terminated.
Definition: FakeMotor.h:122
bool getAxes(int *ax) override
Get the number of controlled axes.
Definition: FakeMotor.h:56
bool stop() override
Stop motion, multiple joints.
Definition: FakeMotor.h:213
FakeMotor(FakeMotor &&)=delete
bool getTargetPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
Definition: FakeMotor.h:258
bool positionMove(const int n_joint, const int *joints, const double *refs) override
Set new reference point for a subset of joints.
Definition: FakeMotor.h:218
bool getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
Definition: FakeMotor.h:377
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
Definition: FakeMotor.h:63
FakeMotor()=default
bool velocityMove(int j, double sp) override
Start motion at a given speed, single joint.
Definition: FakeMotor.h:385
bool getRefAccelerations(const int n_joint, const int *joints, double *accs) override
Get reference acceleration for a joint.
Definition: FakeMotor.h:248
bool positionMove(const double *refs) override
Set new reference point for all axes.
Definition: FakeMotor.h:92
bool velocityMove(const double *sp) override
Start motion at a given speed, multiple joints.
Definition: FakeMotor.h:394
bool getEncoderTimed(int j, double *encs, double *time) override
Read the instantaneous acceleration of all axes.
Definition: FakeMotor.h:337
bool getTargetPositions(const int n_joint, const int *joints, double *refs) override
Get the last position reference for the specified group of axes.
Definition: FakeMotor.h:268
bool getRefSpeed(int j, double *ref) override
Get reference speed for a joint.
Definition: FakeMotor.h:171
bool stop(const int n_joint, const int *joints) override
Stop motion for subset of joints.
Definition: FakeMotor.h:253
bool resetEncoder(int j) override
Reset encoder, single joint.
Definition: FakeMotor.h:283
bool getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
Definition: FakeMotor.h:361
bool getEncoders(double *encs) override
Read the position of all axes.
Definition: FakeMotor.h:327
bool relativeMove(const double *deltas) override
Set relative position, all joints.
Definition: FakeMotor.h:112
bool close() override
Close the DeviceDriver.
Definition: FakeMotor.h:278
testMotor deprecated
Definition: FakeMotor.h:410
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
Definition: FakeMotor.cpp:29
A base class for nested structures that can be searched.
Definition: Searchable.h:63
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
A single value (typically within a Bottle).
Definition: Value.h:43
void resize(size_t size) override
Resize the vector.
Definition: Vector.h:220
#define yCInfo(component,...)
Definition: LogComponent.h:171
#define YARP_DECLARE_LOG_COMPONENT(name)
Definition: LogComponent.h:73
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
void delay(double seconds)
Wait for a certain number of seconds.
Definition: Time.cpp:111
The main, catch-all namespace for YARP.
Definition: dirs.h:16