YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FakeLaserWithMotor_motors.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#define _USE_MATH_DEFINES
7
9#include <yarp/os/Time.h>
10#include <yarp/os/Log.h>
11#include <yarp/os/LogStream.h>
14#include <yarp/math/Vec2D.h>
15#include <iostream>
16#include <limits>
17#include <cstring>
18#include <cstdlib>
19#include <cmath>
20
21//#define LASER_DEBUG
22#ifndef DEG2RAD
23#define DEG2RAD M_PI/180.0
24#endif
25
26YARP_LOG_COMPONENT(FAKE_LASER_MOTORS, "yarp.devices.fakeLaserWithMotor.motors")
27
30
32 std::enable_if_t<std::is_trivial<T>::value, int> = 0>
33 T* allocAndCheck(int size)
34{
35 T* t = new T[size];
36 yAssert(t != 0);
37 memset(t, 0, sizeof(T) * size);
38 return t;
39}
40
41template <class T,
42 std::enable_if_t<!std::is_trivial<T>::value, int> = 0>
43 T* allocAndCheck(int size)
44{
45 T* t = new T[size];
46 yAssert(t != 0);
47 return t;
48}
49
50template <class T>
51void checkAndDestroy(T*& p) {
52 if (p != 0) {
53 delete[] p;
54 p = 0;
55 }
56}
57
58
59//Motor interfaces
61{
62 ReturnValue ret = getEncodersRaw(encs);
63 m_mutex.lock();
64 for (int i = 0; i < m_njoints; i++) {
66 }
67 m_mutex.unlock();
68 return ret;
69}
70
71ReturnValue FakeLaserWithMotor::getEncoderTimedRaw(int j, double* encs, double* stamp)
72{
73 ReturnValue ret = getEncoderRaw(j, encs);
74 m_mutex.lock();
75 *stamp = m_timestamp.getTime();
76 m_mutex.unlock();
77
78 return ret;
79}
80
86
101
103{
105 for (int j = 0, index = 0; j < m_njoints; j++, index++)
106 {
107 ret &= positionMoveRaw(j, refs[index]);
108 }
109 return ret;
110}
111
113{
115 auto ret = getControlModeRaw(j, mode);
120 {
121 yCError(FAKE_LASER_MOTORS) << "relativeMoveRaw: skipping command because joint " << j << " is not in VOCAB_CM_POSITION mode";
122 }
123 _posCtrl_references[j] += delta;
124 return ReturnValue_ok;
125}
126
128{
130 for (int j = 0, index = 0; j < m_njoints; j++, index++)
131 {
132 ret &= relativeMoveRaw(j, deltas[index]);
133 }
134 return ret;
135}
136
137
139{
140 flag = false;
141 return ReturnValue_ok;
142}
143
145{
147 bool val, tot_res = true;
148
149 for (int j = 0, index = 0; j < m_njoints; j++, index++)
150 {
151 ret &= checkMotionDoneRaw(j, val);
152 tot_res &= val;
153 }
154 flag = tot_res;
155 return ret;
156}
157
159{
160 // Velocity is expressed in iDegrees/s
161 // save internally the new value of speed; it'll be used in the positionMove
162 int index = j;
163 _ref_speeds[index] = sp;
164 return ReturnValue_ok;
165}
166
168{
169 // Velocity is expressed in iDegrees/s
170 // save internally the new value of speed; it'll be used in the positionMove
171 for (int j = 0, index = 0; j < m_njoints; j++, index++)
172 {
173 _ref_speeds[index] = spds[index];
174 }
175 return ReturnValue_ok;
176}
177
179{
180 // Acceleration is expressed in iDegrees/s^2
181 // save internally the new value of the acceleration; it'll be used in the velocityMove command
182
183 if (acc > 1e6)
184 {
185 _ref_accs[j] = 1e6;
186 }
187 else if (acc < -1e6)
188 {
189 _ref_accs[j] = -1e6;
190 }
191 else
192 {
193 _ref_accs[j] = acc;
194 }
195
196 return ReturnValue_ok;
197}
198
200{
201 // Acceleration is expressed in iDegrees/s^2
202 // save internally the new value of the acceleration; it'll be used in the velocityMove command
203 for (int j = 0, index = 0; j < m_njoints; j++, index++)
204 {
205 if (accs[j] > 1e6)
206 {
207 _ref_accs[index] = 1e6;
208 }
209 else if (accs[j] < -1e6)
210 {
211 _ref_accs[index] = -1e6;
212 }
213 else
214 {
215 _ref_accs[index] = accs[j];
216 }
217 }
218 return ReturnValue_ok;
219}
220
222{
223 *spd = _ref_speeds[j];
224 return ReturnValue_ok;
225}
226
228{
229 memcpy(spds, _ref_speeds, sizeof(double) * m_njoints);
230 return ReturnValue_ok;
231}
232
234{
235 *acc = _ref_accs[j];
236 return ReturnValue_ok;
237}
238
240{
241 memcpy(accs, _ref_accs, sizeof(double) * m_njoints);
242 return ReturnValue_ok;
243}
244
249
251{
253 for (int j = 0; j < m_njoints; j++)
254 {
255 ret &= stopRaw(j);
256 }
257 return ret;
258}
260
261ReturnValue FakeLaserWithMotor::getAvailableControlModesRaw(int j, std::vector<yarp::dev::SelectableControlModeEnum>& avail)
262{
263 avail = _availableControlModes[j];
264 return ReturnValue_ok;
265}
266
267
273
274// IControl Mode 2
275ReturnValue FakeLaserWithMotor::getControlModesRaw(std::vector<yarp::dev::ControlModeEnum>& mode)
276{
278 for (int j = 0; j < m_njoints; j++)
279 {
280 ret = ret && getControlModeRaw(j, mode[j]);
281 }
282 return ret;
283}
284
285ReturnValue FakeLaserWithMotor::getControlModesRaw(const std::vector<int>& joints, std::vector<yarp::dev::ControlModeEnum>& modes)
286{
288 for (int j = 0; j < joints.size(); j++)
289 {
290 ret = ret && getControlModeRaw(joints[j], modes[j]);
291 }
292 return ret;
293}
294
295
296
309
310
311ReturnValue FakeLaserWithMotor::setControlModesRaw(const std::vector<int>& joints, const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
312{
314 for (int i = 0; i < joints.size(); i++)
315 {
316 ret &= setControlModeRaw(joints[i], modes[i]);
317 }
318 return ret;
319}
320
321ReturnValue FakeLaserWithMotor::setControlModesRaw(const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
322{
324 for (int i = 0; i < m_njoints; i++)
325 {
326 ret &= setControlModeRaw(i, modes[i]);
327 }
328 return ret;
329}
330
332{
335 _axisName = new std::string[nj];
336 _jointType = new JointTypeEnum[nj];
337
338 // Reserve space for data stored locally. values are initialized to 0
344
345 for (int i = 0; i < nj; ++i)
346 {
357 }
358
359 //resizeBuffers();
360
361 return true;
362}
363
377
378
383
388
393
398
400{
402
403 *value = _encoders[j];
404
405 return ret;
406}
407
409{
411 for (int j = 0; j < m_njoints; j++)
412 {
413 ReturnValue ok = getEncoderRaw(j, &encs[j]);
414 ret = ret && ok;
415
416 }
417 return ret;
418}
419
421{
422 // To avoid returning uninitialized memory, we set the encoder speed to 0
423 *sp = 0.0;
424 return ReturnValue_ok;
425}
426
428{
430 for (int j = 0; j < m_njoints; j++)
431 {
432 ret &= getEncoderSpeedRaw(j, &spds[j]);
433 }
434 return ret;
435}
436
438{
439 // To avoid returning uninitialized memory, we set the encoder acc to 0
440 *acc = 0.0;
441
442 return ReturnValue_ok;
443}
444
446{
448 for (int j = 0; j < m_njoints; j++)
449 {
450 ret &= getEncoderAccelerationRaw(j, &accs[j]);
451 }
452 return ret;
453}
454
455ReturnValue FakeLaserWithMotor::setTrajAccelerationsRaw(const int n_joint, const int* joints, const double* accs)
456{
458 for (int j = 0; j < n_joint; j++)
459 {
460 ret = ret && setTrajAccelerationRaw(joints[j], accs[j]);
461 }
462 return ret;
463}
464
465ReturnValue FakeLaserWithMotor::getTrajSpeedsRaw(const int n_joint, const int* joints, double* spds)
466{
468 for (int j = 0; j < n_joint; j++)
469 {
470 ret = ret && getTrajSpeedRaw(joints[j], &spds[j]);
471 }
472 return ret;
473}
474
475ReturnValue FakeLaserWithMotor::getTrajAccelerationsRaw(const int n_joint, const int* joints, double* accs)
476{
478 for (int j = 0; j < n_joint; j++)
479 {
480 ret = ret && getTrajAccelerationRaw(joints[j], &accs[j]);
481 }
482 return ret;
483}
484
486{
488 for (int j = 0; j < n_joint; j++)
489 {
490 ret = ret && stopRaw(joints[j]);
491 }
492 return ret;
493}
494
495ReturnValue FakeLaserWithMotor::positionMoveRaw(const int n_joint, const int* joints, const double* refs)
496{
497 for (int j = 0; j < n_joint; j++)
498 {
499 yCDebug(FAKE_LASER_MOTORS, "j: %d; ref %f;\n", joints[j], refs[j]); fflush(stdout);
500 }
501
503 for (int j = 0; j < n_joint; j++)
504 {
505 ret = ret && positionMoveRaw(joints[j], refs[j]);
506 }
507 return ret;
508}
509
510ReturnValue FakeLaserWithMotor::relativeMoveRaw(const int n_joint, const int* joints, const double* deltas)
511{
513 for (int j = 0; j < n_joint; j++)
514 {
515 ret = ret && relativeMoveRaw(joints[j], deltas[j]);
516 }
517 return ret;
518}
519
520ReturnValue FakeLaserWithMotor::checkMotionDoneRaw(const std::vector<int>& joints, bool& flag)
521{
523 bool val = true;
524 bool tot_val = true;
525
526 for (int j = 0; j < joints.size(); j++)
527 {
528 ret = ret && checkMotionDoneRaw(joints[j], val);
529 tot_val &= val;
530 }
531 flag = tot_val;
532 return ret;
533}
534
535ReturnValue FakeLaserWithMotor::setTrajSpeedsRaw(const int n_joint, const int* joints, const double* spds)
536{
538 for (int j = 0; j < n_joint; j++)
539 {
540 ret = ret && setTrajSpeedRaw(joints[j], spds[j]);
541 }
542 return ret;
543}
544
546{
548 auto ret = getControlModeRaw(axis, mode);
552 {
553 yCWarning(FAKE_LASER_MOTORS) << "getTargetPosition: Joint " << axis << " is not in POSITION mode, therefore the value returned by " <<
554 "this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
555 }
556 *ref = _posCtrl_references[axis];
557 return ReturnValue_ok;
558}
559
561{
563 for (int i = 0; i < m_njoints; i++) {
564 ret &= getTargetPositionRaw(i, &refs[i]);
565 }
566 return ret;
567}
568
570{
572 for (int i = 0; i < nj; i++)
573 {
574 ret &= getTargetPositionRaw(jnts[i], &refs[i]);
575 }
576 return ret;
577}
578
580{
582 auto ret = getControlModeRaw(j, mode);
587 {
588 yCError(FAKE_LASER_MOTORS) << "velocityMoveRaw: skipping command because board " << " joint " << j << " is not in VOCAB_CM_VELOCITY mode";
589 }
590 _command_speeds[j] = sp;
591 //last_velocity_command[j] = yarp::os::Time::now();
592 return ReturnValue_ok;
593}
594
596{
598 for (int i = 0; i < m_njoints; i++) {
599 ret &= velocityMoveRaw(i, sp[i]);
600 }
601 return ret;
602}
603ReturnValue FakeLaserWithMotor::velocityMoveRaw(const int n_joint, const int* joints, const double* spds)
604{
606 for (int i = 0; i < n_joint; i++)
607 {
608 ret &= velocityMoveRaw(joints[i], spds[i]);
609 }
610 return ret;
611}
612
614{
615 *ref = _command_speeds[axis];
616 return ReturnValue_ok;
617}
618
620{
622 for (int i = 0; i < m_njoints; i++)
623 {
624 ret &= getTargetVelocityRaw(i, &refs[i]);
625 }
626 return ret;
627}
628
630{
632 for (int i = 0; i < nj; i++)
633 {
634 ret &= getTargetVelocityRaw(jnts[i], &refs[i]);
635 }
636 return ret;
637}
638
639
641{
642 name = _axisName[j];
643 return ReturnValue_ok;
644}
645
void checkAndDestroy(T *&p)
FeatureMode mode
void checkAndDestroy(T *&p)
const yarp::os::LogComponent & FAKE_LASER_MOTORS()
T * allocAndCheck(int size)
constexpr yarp::conf::vocab32_t VOCAB_CM_IDLE
#define yAssert(x)
Definition Log.h:388
#define ReturnValue_ok
Definition ReturnValue.h:82
#define YARP_METHOD_NOT_YET_IMPLEMENTED()
yarp::dev::ReturnValue getTrajSpeedRaw(int j, double *ref) override
Get reference speed for a joint.
yarp::dev::ReturnValue setTrajAccelerationRaw(int j, double acc) override
Set reference acceleration for a joint.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue getEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of an axis.
yarp::dev::ReturnValue positionMoveRaw(int j, double ref) override
Set new reference point for a single axis.
yarp::dev::ReturnValue setTrajAccelerationsRaw(const double *accs) override
Set reference acceleration on all joints.
yarp::dev::ReturnValue setTrajSpeedRaw(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
yarp::dev::ReturnValue getTrajAccelerationRaw(int j, double *acc) override
Get reference acceleration for a joint.
yarp::dev::ReturnValue getTargetVelocitiesRaw(double *refs) override
Get the last reference speed set by velocityMove for all joints.
yarp::dev::ReturnValue getControlModeRaw(int j, yarp::dev::ControlModeEnum &mode) override
yarp::dev::ReturnValue checkMotionDoneRaw(bool &flag) override
Check if the current trajectory all joints is terminated.
yarp::dev::ReturnValue getEncoderTimedRaw(int j, double *encs, double *stamp) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getTrajSpeedsRaw(double *spds) override
Get reference speed of all joints.
yarp::dev::ReturnValue relativeMoveRaw(int j, double delta) override
Set relative position.
yarp::dev::ReturnValue setControlModeRaw(int j, yarp::dev::SelectableControlModeEnum mode) override
yarp::dev::ReturnValue getAxisNameRaw(int axis, std::string &name) override
yarp::dev::ReturnValue getTargetPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getTrajAccelerationsRaw(double *accs) override
Get reference acceleration of all joints.
yarp::dev::ReturnValue getEncodersRaw(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue getControlModesRaw(std::vector< yarp::dev::ControlModeEnum > &mode) override
yarp::dev::ReturnValue getEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
yarp::dev::ReturnValue velocityMoveRaw(int j, double sp) override
Start motion at a given speed, single joint.
yarp::dev::ReturnValue getTargetPositionsRaw(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue setEncoderRaw(int j, double val) override
Set the value of the encoder for a given joint.
yarp::dev::ReturnValue setControlModesRaw(const std::vector< int > &j, const std::vector< yarp::dev::SelectableControlModeEnum > &mode) override
yarp::dev::ReturnValue getTargetVelocityRaw(const int joint, double *ref) override
Get the last reference speed set by velocityMove for single joint.
yarp::dev::ReturnValue resetEncodersRaw() override
Reset encoders.
yarp::dev::JointTypeEnum * _jointType
yarp::dev::ReturnValue getAvailableControlModesRaw(int j, std::vector< yarp::dev::SelectableControlModeEnum > &avail) override
yarp::dev::ReturnValue setTrajSpeedsRaw(const double *spds) override
Set reference speed on all joints.
std::vector< yarp::dev::SelectableControlModeEnum > * _availableControlModes
yarp::dev::ReturnValue setEncodersRaw(const double *vals) override
Set the value of all encoders.
yarp::dev::ReturnValue getEncoderSpeedsRaw(double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue resetEncoderRaw(int j) override
Reset encoder, single joint.
yarp::dev::ReturnValue getEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue stopRaw() override
Stop motion, multiple joints.
yarp::dev::ReturnValue getEncoderRaw(int j, double *v) override
Read the value of an encoder.
yarp::dev::ReturnValue getEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all axes.
A mini-server for performing network communication in the background.
double getTime() const
Get the time stamp.
Definition Stamp.cpp:34
#define yCError(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
STL namespace.
SelectableControlModeEnum
The main, catch-all namespace for YARP.
Definition dirs.h:16