YARP
Yet Another Robot Platform
FakeBot.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms of the
7  * BSD-3-Clause license. See the accompanying LICENSE file for details.
8  */
9 
10 #include <yarp/dev/DeviceDriver.h>
11 
14 #include <yarp/sig/Vector.h>
15 #include <yarp/sig/Image.h>
16 #include <yarp/os/Time.h>
17 
18 
19 class FakeBot :
29  public yarp::os::Thread
30 {
31 private:
32  int njoints;
33  double m_x, m_y;
34  double m_dx, m_dy;
35  double m_tx, m_ty;
36  double m_tdx, m_tdy;
37  int m_w, m_h;
38  double xScale, yScale;
39  double noiseLevel;
40  yarp::sig::Vector pos, dpos, vel, speed, acc, loc, amp;
42  double lifetime;
43 
44  void init();
45 public:
46  FakeBot() :
47  njoints(2),
48  m_w(128),
49  m_h(128),
50  xScale(1),
51  yScale(1),
52  noiseLevel(0),
53  lifetime(-1)
54  {
55  pos.resize(njoints);
56  dpos.resize(njoints);
57  vel.resize(njoints);
58  speed.resize(njoints);
59  acc.resize(njoints);
60  loc.resize(njoints);
61  amp.resize(njoints);
62  for (int i=0; i<njoints; i++) {
63  pos[i] = 0;
64  dpos[i] = 0;
65  vel[i] = 0;
66  speed[i] = 0;
67  acc[i] = 0;
68  loc[i] = 0;
69  amp[i] = 1; // initially on - ok for simulator
70  }
71  init();
72  }
73 
74  bool open(yarp::os::Searchable& config) override;
75 
76  // IFrameGrabberImage
78 
79  int height() const override{
80  return m_h;
81  }
82 
83  int width() const override{
84  return m_w;
85  }
86 
87 
88 
89  // IPositionControl etc.
90 
91  bool getAxes(int *ax) override {
92  *ax = njoints;
93  return true;
94  }
95 
96  bool positionMove(int j, double ref) override {
97  if (j<njoints) {
98  pos[j] = ref;
99  }
100  return true;
101  }
102 
103 
104  bool positionMove(const double *refs) override {
105  for (int i=0; i<njoints; i++) {
106  pos[i] = refs[i];
107  }
108  return true;
109  }
110 
111 
112  bool relativeMove(int j, double delta) override {
113  if (j<njoints) {
114  dpos[j] = delta;
115  }
116  return true;
117  }
118 
119 
120  bool relativeMove(const double *deltas) override {
121  for (int i=0; i<njoints; i++) {
122  dpos[i] = deltas[i];
123  }
124  return true;
125  }
126 
127 
128  bool checkMotionDone(int j, bool *flag) override {
129  return true;
130  }
131 
132 
133  bool checkMotionDone(bool *flag) override {
134  return true;
135  }
136 
137 
138  bool setRefSpeed(int j, double sp) override {
139  if (j<njoints) {
140  speed[j] = sp;
141  }
142  return true;
143  }
144 
145 
146  bool setRefSpeeds(const double *spds) override {
147  for (int i=0; i<njoints; i++) {
148  speed[i] = spds[i];
149  }
150  return true;
151  }
152 
153 
154  bool setRefAcceleration(int j, double acc) override {
155  if (j<njoints) {
156  this->acc[j] = acc;
157  }
158  return true;
159  }
160 
161 
162  bool setRefAccelerations(const double *accs) override {
163  for (int i=0; i<njoints; i++) {
164  acc[i] = accs[i];
165  }
166  return true;
167  }
168 
169 
170  bool getRefSpeed(int j, double *ref) override {
171  if (j<njoints) {
172  (*ref) = speed[j];
173  }
174  return true;
175  }
176 
177 
178  bool getRefSpeeds(double *spds) override {
179  for (int i=0; i<njoints; i++) {
180  spds[i] = speed[i];
181  }
182  return true;
183  }
184 
185 
186  bool getRefAcceleration(int j, double *acc) override {
187  if (j<njoints) {
188  (*acc) = this->acc[j];
189  }
190  return true;
191  }
192 
193 
194  bool getRefAccelerations(double *accs) override {
195  for (int i=0; i<njoints; i++) {
196  accs[i] = acc[i];
197  }
198  return true;
199  }
200 
201 
202  bool stop(int j) override {
203  return true;
204  }
205 
206 
207  bool stop() override {
208  return true;
209  }
210 
211 
212  bool close() override {
213  return true;
214  }
215 
216  bool resetEncoder(int j) override {
217  if (j<njoints) {
218  pos[j] = 0;
219  dpos[j] = 0;
220  }
221  return true;
222  }
223 
224  bool resetEncoders() override {
225  for (int i=0; i<njoints; i++) {
226  pos[i] = 0;
227  }
228  return true;
229  }
230 
231  bool setEncoder(int j, double val) override {
232  if (j<njoints) {
233  pos[j] = val;
234  }
235  return true;
236  }
237 
238  bool setEncoders(const double *vals) override {
239  for (int i=0; i<njoints; i++) {
240  pos[i] = vals[i];
241  }
242  return true;
243  }
244 
245  bool getEncoder(int j, double *v) override {
246  if (j<njoints) {
247  (*v) = loc[j];
248  }
249 
250  return true;
251  }
252 
253  bool getEncoders(double *encs) override {
254  for (int i=0; i<njoints; i++) {
255  encs[i] = loc[i];
256  }
257  return true;
258  }
259 
260  bool getEncoderSpeed(int j, double *sp) override {
261  if (j<njoints) {
262  (*sp) = 0;
263  }
264  return true;
265  }
266 
267  bool getEncoderSpeeds(double *spds) override {
268  for (int i=0; i<njoints; i++) {
269  spds[i] = 0;
270  }
271  return true;
272  }
273 
274  bool getEncoderAcceleration(int j, double *spds) override {
275  if (j<njoints) {
276  (*spds) = 0;
277  }
278  return true;
279  }
280 
281  bool getEncoderAccelerations(double *accs) override {
282  for (int i=0; i<njoints; i++) {
283  accs[i] = 0;
284  }
285  return true;
286  }
287 
288  bool positionMove(const int n_joint, const int *joints, const double *refs) override { return false; }
289 
290  bool relativeMove(const int n_joint, const int *joints, const double *deltas) override { return false; }
291 
292  bool checkMotionDone(const int n_joint, const int *joints, bool *flags) override { return false; }
293 
294  bool setRefSpeeds(const int n_joint, const int *joints, const double *spds) override { return false; }
295 
296  bool setRefAccelerations(const int n_joint, const int *joints, const double *accs) override { return false; }
297 
298  bool getRefSpeeds(const int n_joint, const int *joints, double *spds) override { return false; }
299 
300  bool getRefAccelerations(const int n_joint, const int *joints, double *accs) override { return false; }
301 
302  bool stop(const int n_joint, const int *joints) override { return false; }
303 
304 
305  // IEncodersTimed
306  bool getEncodersTimed(double *encs, double *time) override
307  {
308  bool ret = getEncoders(encs);
309  double myTime = yarp::os::Time::now();
310 
311  for (int i=0; i<njoints; i++)
312  {
313  time[i] = myTime;
314  }
315  return ret;
316  }
317 
318  bool getEncoderTimed(int j, double *enc, double *time) override
319  {
320  bool ret = getEncoder(j, enc);
321  *time = yarp::os::Time::now();
322  return ret;
323  }
324 
325  bool velocityMove(int j, double sp) override {
326  if (j<njoints) {
327  vel[j] = sp;
328  }
329  return true;
330  }
331 
332  bool velocityMove(const double *sp) override {
333  for (int i=0; i<njoints; i++) {
334  vel[i] = sp[i];
335  }
336  return true;
337  }
338 
339  bool velocityMove(const int n_joint, const int *joints, const double *spds) override { return false; }
340 
341 
342 
343  bool enableAmp(int j) override {
344  if (j<njoints) {
345  amp[j] = 1;
346  }
347  return true;
348  }
349 
350  bool disableAmp(int j) override {
351  if (j<njoints) {
352  amp[j] = 0;
353  }
354  return true;
355  }
356 
357  bool getCurrent(int j, double *val) override {
358  if (j<njoints) {
359  val[j] = amp[j];
360  }
361  return true;
362  }
363 
364  bool getCurrents(double *vals) override {
365  for (int i=0; i<njoints; i++) {
366  vals[i] = amp[i];
367  }
368  return true;
369  }
370 
371  bool getMaxCurrent(int j, double* v) override {
372  *v = 0;
373  return true;
374  }
375 
376  bool setMaxCurrent(int j, double v) override {
377  return true;
378  }
379 
380  bool getAmpStatus(int *st) override {
381  *st = 0;
382  return true;
383  }
384 
385  bool getAmpStatus(int k, int *v) override
386  {
387  *v=0;
388  return true;
389  }
390 
391  bool calibrateAxisWithParams(int j, unsigned int iv, double v1, double v2, double v3) override
392  {
393  fprintf(stderr, "FakeBot: calibrating joint %d with parameters %u %lf %lf %lf\n", j, iv, v1, v2, v3);
394  return true;
395  }
396 
397  bool calibrationDone(int j) override
398  {
399  fprintf(stderr , "FakeBot: calibration done on joint %d.\n", j);
400  return true;
401  }
402 
403  bool getLimits(int axis, double *min, double *max) override
404  {
405  fprintf(stderr, "FakeBot: get limits\n");
406  *min=0;
407  *max=0;
408  return true;
409  }
410 
411  bool setLimits(int axis, double min, double max) override
412  {
413  fprintf(stderr, "FakeBot: set limits\n");
414  return true;
415  }
416 
417  bool setVelLimits(int axis, double min, double max) override { return false; }
418 
419  bool getVelLimits(int axis, double *min, double *max) override { return false; }
420 
421  void run() override;
422 };
423 
Interface for control devices, calibration commands.
bool relativeMove(int j, double delta) override
Set relative position.
Definition: FakeBot.h:112
Interface implemented by all device drivers.
Definition: DeviceDriver.h:37
bool getEncoder(int j, double *v) override
Read the value of an encoder.
Definition: FakeBot.h:245
FakeBot()
Definition: FakeBot.h:46
bool resetEncoder(int j) override
Reset encoder, single joint.
Definition: FakeBot.h:216
bool calibrateAxisWithParams(int j, unsigned int iv, double v1, double v2, double v3) override
Start calibration, this method is very often platform specific.
Definition: FakeBot.h:391
bool velocityMove(const double *sp) override
Start motion at a given speed, multiple joints.
Definition: FakeBot.h:332
bool getAmpStatus(int k, int *v) override
Definition: FakeBot.h:385
bool getCurrents(double *vals) override
Definition: FakeBot.h:364
define common interfaces to discover remote camera capabilities
bool getRefAccelerations(const int n_joint, const int *joints, double *accs) override
Get reference acceleration for a joint.
Definition: FakeBot.h:300
bool ret
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
Definition: FakeBot.cpp:85
Interface for a generic control board device implementing position control.
bool getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
Definition: FakeBot.h:267
A base class for nested structures that can be searched.
Definition: Searchable.h:68
bool setEncoders(const double *vals) override
Set the value of all encoders.
Definition: FakeBot.h:238
int width() const override
Return the width of each frame.
Definition: FakeBot.h:83
void resize(size_t size) override
Resize the vector.
Definition: Vector.h:270
bool calibrationDone(int j) override
Check if the calibration is terminated, on a particular joint.
Definition: FakeBot.h:397
bool getEncoderTimed(int j, double *enc, double *time) override
Read the instantaneous acceleration of all axes.
Definition: FakeBot.h:318
bool stop(const int n_joint, const int *joints) override
Stop motion for subset of joints.
Definition: FakeBot.h:302
bool getEncoders(double *encs) override
Read the position of all axes.
Definition: FakeBot.h:253
bool getEncodersTimed(double *encs, double *time) override
Read the instantaneous acceleration of all axes.
Definition: FakeBot.h:306
bool getRefAcceleration(int j, double *acc) override
Get reference acceleration for a joint.
Definition: FakeBot.h:186
bool getRefSpeed(int j, double *ref) override
Get reference speed for a joint.
Definition: FakeBot.h:170
Provides:
Definition: Vector.h:32
bool getLimits(int axis, double *min, double *max) override
Get the software limits for a particular axis.
Definition: FakeBot.h:403
bool getCurrent(int j, double *val) override
Definition: FakeBot.h:357
Read a YARP-format image from a device.
bool getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
Definition: FakeBot.h:281
bool setVelLimits(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...
Definition: FakeBot.h:417
bool getRefSpeeds(double *spds) override
Get reference speed of all joints.
Definition: FakeBot.h:178
Control board, extend encoder interface with timestamps.
bool setMaxCurrent(int j, double v) override
Definition: FakeBot.h:376
bool setRefAccelerations(const int n_joint, const int *joints, const double *accs) override
Set reference acceleration on all joints.
Definition: FakeBot.h:296
void run() override
Main body of the new thread.
Definition: FakeBot.cpp:173
bool getRefAccelerations(double *accs) override
Get reference acceleration of all joints.
Definition: FakeBot.h:194
bool relativeMove(const int n_joint, const int *joints, const double *deltas) override
Set relative position for a subset of joints.
Definition: FakeBot.h:290
bool getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
Definition: FakeBot.h:260
bool positionMove(const int n_joint, const int *joints, const double *refs) override
Set new reference point for a subset of joints.
Definition: FakeBot.h:288
bool checkMotionDone(bool *flag) override
Check if the current trajectory is terminated.
Definition: FakeBot.h:133
bool velocityMove(int j, double sp) override
Start motion at a given speed, single joint.
Definition: FakeBot.h:325
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
bool setRefSpeeds(const double *spds) override
Set reference speed on all joints.
Definition: FakeBot.h:146
bool stop() override
Stop motion, multiple joints.
Definition: FakeBot.h:207
bool setRefSpeeds(const int n_joint, const int *joints, const double *spds) override
Set reference speed on all joints.
Definition: FakeBot.h:294
A cheap and cheerful framework for human readable/writable forms of messages to devices.
Definition: DeviceDriver.h:132
bool checkMotionDone(const int n_joint, const int *joints, bool *flags) override
Check if the current trajectory is terminated.
Definition: FakeBot.h:292
bool getAmpStatus(int *st) override
Definition: FakeBot.h:380
int height() const override
Return the height of each frame.
Definition: FakeBot.h:79
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: FakeBot.h:138
bool getRefSpeeds(const int n_joint, const int *joints, double *spds) override
Get reference speed of all joints.
Definition: FakeBot.h:298
bool getEncoderAcceleration(int j, double *spds) override
Read the instantaneous acceleration of an axis.
Definition: FakeBot.h:274
bool setRefAcceleration(int j, double acc) override
Set reference acceleration for a joint.
Definition: FakeBot.h:154
bool setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
Definition: FakeBot.h:231
define control board standard interfaces
contains the definition of a Vector type
Interface for control devices, amplifier commands.
An abstraction for a thread of execution.
Definition: Thread.h:24
bool resetEncoders() override
Reset encoders.
Definition: FakeBot.h:224
bool positionMove(int j, double ref) override
Set new reference point for a single axis.
Definition: FakeBot.h:96
Interface for control boards implementing velocity control.
bool setRefAccelerations(const double *accs) override
Set reference acceleration on all joints.
Definition: FakeBot.h:162
bool enableAmp(int j) override
Enable the amplifier on a specific joint.
Definition: FakeBot.h:343
bool checkMotionDone(int j, bool *flag) override
Check if the current trajectory is terminated.
Definition: FakeBot.h:128
bool getImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &image) override
Get an rgb image from the frame grabber, if required demosaicking/color reconstruction is applied...
Definition: FakeBot.cpp:114
bool relativeMove(const double *deltas) override
Set relative position, all joints.
Definition: FakeBot.h:120
bool positionMove(const double *refs) override
Set new reference point for all axes.
Definition: FakeBot.h:104
bool getAxes(int *ax) override
Get the number of controlled axes.
Definition: FakeBot.h:91
bool getMaxCurrent(int j, double *v) override
Returns the maximum electric current allowed for a given motor.
Definition: FakeBot.h:371
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: FakeBot.h:339
bool close() override
Close the DeviceDriver.
Definition: FakeBot.h:212
bool disableAmp(int j) override
Disable the amplifier on a specific joint.
Definition: FakeBot.h:350
Interface for control devices, commands to get/set position and veloity limits.
bool setLimits(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...
Definition: FakeBot.h:411
bool stop(int j) override
Stop motion, single joint.
Definition: FakeBot.h:202
bool getVelLimits(int axis, double *min, double *max) override
Get the software speed limits for a particular axis.
Definition: FakeBot.h:419