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