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