YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FakeMotionControlMicro.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
7
9
10#include <yarp/os/Bottle.h>
11#include <yarp/os/Time.h>
13#include <yarp/os/LogStream.h>
14#include <yarp/os/NetType.h>
15#include <yarp/dev/Drivers.h>
17
18#include <sstream>
19#include <cstring>
20
21using namespace yarp::dev;
22using namespace yarp::os;
23using namespace yarp::os::impl;
24
25// macros
26#define NEW_JSTATUS_STRUCT 1
27#define VELOCITY_WATCHDOG 0.1
28#define OPENLOOP_WATCHDOG 0.1
29#define PWM_CONSTANT 0.1
30
31namespace {
32YARP_LOG_COMPONENT(FAKEMOTIONCONTROLMICRO, "yarp.device.fakeMotionControlMicro")
33}
34
36{
37 std::lock_guard lock(_mutex);
38
39 for (int i=0;i <_njoints ;i++)
40 {
41 }
42 prev_time = yarp::os::Time::now();
43}
44
45static inline bool NOT_YET_IMPLEMENTED(const char *txt)
46{
47 yCError(FAKEMOTIONCONTROLMICRO) << txt << "is not yet implemented";
48 return true;
49}
50
51static inline bool DEPRECATED(const char *txt)
52{
53 yCError(FAKEMOTIONCONTROLMICRO) << txt << "has been deprecated";
54 return true;
55}
56
57
58// replace with to_string as soon as C++11 is required by YARP
63template<typename T>
64std::string toString(const T& value)
65{
66 std::ostringstream oss;
67 oss << value;
68 return oss.str();
69}
70
71//generic function that check is key1 is present in input bottle and that the result has size elements
72// return true/false
73bool FakeMotionControlMicro::extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
74{
75 size++;
76 Bottle &tmp=input.findGroup(key1, txt);
77
78 if (tmp.isNull())
79 {
80 yCError(FAKEMOTIONCONTROLMICRO) << key1.c_str() << "parameter not found";
81 return false;
82 }
83
84 if(tmp.size()!=(size_t) size)
85 {
86 yCError(FAKEMOTIONCONTROLMICRO) << key1.c_str() << "incorrect number of entries";
87 return false;
88 }
89
90 out=tmp;
91 return true;
92}
93
95{
96 pos.resize(_njoints);
97 dpos.resize(_njoints);
98 vel.resize(_njoints);
99 speed.resize(_njoints);
100 acc.resize(_njoints);
101 loc.resize(_njoints);
102 amp.resize(_njoints);
103
104 pos.zero();
105 dpos.zero();
106 vel.zero();
107 speed.zero();
108 acc.zero();
109 loc.zero();
110 amp.zero();
111}
112
114{
115 _axisMap = allocAndCheck<int>(nj);
116 _controlModes = allocAndCheck<int>(nj);
117 _angleToEncoder = allocAndCheck<double>(nj);
118 _encodersStamp = allocAndCheck<double>(nj);
119 _hwfault_code = allocAndCheck<int>(nj);
120 _hwfault_message = allocAndCheck<std::string>(nj);
121
122 _axisName = new std::string[nj];
123 _jointType = new JointTypeEnum[nj];
124
126
127 return true;
128}
129
130bool FakeMotionControlMicro::dealloc()
131{
132 checkAndDestroy(_axisMap);
133 checkAndDestroy(_controlModes);
134 checkAndDestroy(_angleToEncoder);
135 checkAndDestroy(_encodersStamp);
136
137 checkAndDestroy(_axisName);
138 checkAndDestroy(_jointType);
139
140 checkAndDestroy(_hwfault_code);
141 checkAndDestroy(_hwfault_message);
142
143 return true;
144}
145
147 PeriodicThread(0.01),
152 _mutex(),
153 _njoints (0),
154 _axisMap (nullptr),
155 _angleToEncoder(nullptr),
156 _encodersStamp (nullptr),
157 _axisName (nullptr),
158 _jointType (nullptr),
159 useRawEncoderData (false),
160 prev_time (0.0),
161 opened (false),
162 verbose (VERY_VERBOSE)
163{
165 std::string tmp = yarp::conf::environment::get_string("VERBOSE_STICA");
166 verbosewhenok = (tmp != "") ? (bool)yarp::conf::numeric::from_string<int>(tmp) :
167 false;
168}
169
175
177{
178 return opened;
179}
180
182{
184 for(int i=0; i<_njoints; i++)
185 {
186 _controlModes[i] = VOCAB_CM_POSITION;
187 }
188 prev_time = yarp::os::Time::now();
189 return true;
190}
191
195
197{
198 if (!this->parseParams(config)) {return false;}
199
200 std::string str;
201
202 //
203 // Read Configuration params from file
204 //
205 _njoints = m_GENERAL_Joints;
206
207 if(!alloc(_njoints))
208 {
209 yCError(FAKEMOTIONCONTROLMICRO) << "Malloc failed";
210 return false;
211 }
212
213 if(!fromConfig(config))
214 {
215 yCError(FAKEMOTIONCONTROLMICRO) << "Missing parameters in config file";
216 return false;
217 }
218
219 // INIT ALL INTERFACES
220 yarp::sig::Vector tmpZeros; tmpZeros.resize (_njoints, 0.0);
221 yarp::sig::Vector tmpOnes; tmpOnes.resize (_njoints, 1.0);
222 ControlBoardHelper cb(_njoints, _axisMap, _angleToEncoder, nullptr, nullptr, nullptr, nullptr);
224 ImplementEncodersTimed::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
225 ImplementMotorEncoders::initialize(_njoints, _axisMap, _angleToEncoder, nullptr);
226 ImplementAxisInfo::initialize(_njoints, _axisMap);
227 ImplementJointFault::initialize(_njoints, _axisMap);
228
229 //start the rateThread
230 bool init = this->start();
231 if(!init)
232 {
233 yCError(FAKEMOTIONCONTROLMICRO) << "open() has an error in call of FakeMotionControlMicro::init() for board" ;
234 return false;
235 }
236 else
237 {
238 if(verbosewhenok)
239 {
240 yCDebug(FAKEMOTIONCONTROLMICRO) << "init() has successfully initialized board ";
241 }
242 }
243 opened = true;
244
245 return true;
246}
247
249{
250 size_t i;
251
252 // AxisMap
253 if (!m_GENERAL_AxisMap.empty())
254 {
255 for (i = 0; i < m_GENERAL_AxisMap.size(); i++) {
256 _axisMap[i] = m_GENERAL_AxisMap[i];
257 }
258 }
259 else
260 {
261 for (i = 0; i < _njoints; i++) {
262 _axisMap[i] = i;
263 }
264 }
265
266 // AxisName
267 if (!m_GENERAL_AxisName.empty())
268 {
269 for (i = 0; i < m_GENERAL_AxisName.size(); i++) {
270 _axisName[_axisMap[i]] = m_GENERAL_AxisName[i];
271 }
272 }
273 else
274 {
275 for (i = 0; i < _njoints; i++) {
276 _axisName[_axisMap[i]] = "joint" + toString(i);
277 }
278 }
279
280 // Axis Type
281 if (!m_GENERAL_AxisType.empty())
282 {
283 //beware: axis type has to be remapped here because they are not set using the toHw() helper function
284 for (i = 0; i < m_GENERAL_AxisType.size(); i++)
285 {
286 std::string typeString = m_GENERAL_AxisType[i];
287 if (typeString == "revolute") {
288 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_REVOLUTE;
289 }
290 else if (typeString == "prismatic") {
291 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_PRISMATIC;
292 }
293 else {
294 yCError(FAKEMOTIONCONTROLMICRO, "Unknown AxisType value %s!", typeString.c_str());
295 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_UNKNOWN;
296 return false;
297 }
298 }
299 }
300 else
301 {
302 yCInfo(FAKEMOTIONCONTROLMICRO) << "Using default AxisType (revolute)";
303 for (i = 0; i < _njoints; i++) {
304 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_REVOLUTE;
305 }
306 }
307
308
309
310
311
312
313// double tmp_A2E;
314 // Encoder scales
315 if(!m_GENERAL_Encoder.empty())
316 {
317 for (i = 0; i < m_GENERAL_Encoder.size(); i++) {
318 _angleToEncoder[i] = m_GENERAL_Encoder[i]; }
319 }
320 else
321 {
322 yCInfo(FAKEMOTIONCONTROLMICRO) << "Using default Encoder=1";
323 for (i = 0; i < _njoints; i++) {
324 _angleToEncoder[i] = 1; }
325 }
326
327 return true;
328}
329
330
332{
333 std::lock_guard lock(_mutex);
334
336
337 yCTrace(FAKEMOTIONCONTROLMICRO) << " close()";
338
342
343// cleanup();
344
345 return true;
346}
347
348void FakeMotionControlMicro::cleanup()
349{
350
351}
352
354
356{
357 return ReturnValue_ok;
358}
359
361{
362 return ReturnValue_ok;
363}
364
369
374
376{
378
379 // To simulate a real controlboard, we assume that the joint
380 // encoders is exactly the last set by setPosition(s) or positionMove
381 *value = pos[j];
382
383 return ret;
384}
385
387{
389 for(int j=0; j< _njoints; j++)
390 {
391 ReturnValue ok = getEncoderRaw(j, &encs[j]);
392 ret = ret && ok;
393
394 }
395 return ret;
396}
397
399{
400 // To avoid returning uninitialized memory, we set the encoder speed to 0
401 *sp = 0.0;
402 return ReturnValue_ok;
403}
404
406{
408 for(int j=0; j< _njoints; j++)
409 {
410 ret &= getEncoderSpeedRaw(j, &spds[j]);
411 }
412 return ret;
413}
414
416{
417 // To avoid returning uninitialized memory, we set the encoder acc to 0
418 *acc = 0.0;
419
420 return ReturnValue_ok;
421}
422
424{
426 for(int j=0; j< _njoints; j++)
427 {
428 ret &= getEncoderAccelerationRaw(j, &accs[j]);
429 }
430 return ret;
431}
432
434
436{
437 ReturnValue ret = getEncodersRaw(encs);
438 _mutex.lock();
439 for (int i = 0; i < _njoints; i++) {
440 stamps[i] = _encodersStamp[i];
441 }
442 _mutex.unlock();
443 return ret;
444}
445
447{
448 ReturnValue ret = getEncoderRaw(j, encs);
449 _mutex.lock();
450 *stamp = _encodersStamp[j];
451 _mutex.unlock();
452
453 return ret;
454}
455
457
459{
460 *num=_njoints;
461 return ReturnValue_ok;
462}
463
468
473
478
483
488
493
495{
496 *value = pos[m]*10;
497 return ReturnValue_ok;
498}
499
501{
503 for(int j=0; j< _njoints; j++)
504 {
505 ret &= getMotorEncoderRaw(j, &encs[j]);
506
507 }
508 return ret;
509}
510
512{
513 *sp = 0.0;
514 return ReturnValue_ok;
515}
516
518{
520 for(int j=0; j< _njoints; j++)
521 {
522 ret &= getMotorEncoderSpeedRaw(j, &spds[j]);
523 }
524 return ret;
525}
526
528{
529 *acc = 0.0;
530 return ReturnValue_ok;
531}
532
534{
536 for(int j=0; j< _njoints; j++)
537 {
538 ret &= getMotorEncoderAccelerationRaw(j, &accs[j]);
539 }
540 return ret;
541}
542
544{
546 _mutex.lock();
547 for (int i = 0; i < _njoints; i++) {
548 stamps[i] = _encodersStamp[i];
549 }
550 _mutex.unlock();
551
552 return ret;
553}
554
556{
557 ReturnValue ret = getMotorEncoderRaw(m, encs);
558 _mutex.lock();
559 *stamp = _encodersStamp[m];
560 _mutex.unlock();
561
562 return ret;
563}
565
566
568{
569 name = "ERROR";
570 name = _axisName[j];
571 return ReturnValue_ok;
572}
573
575{
576 type = _jointType[j];
577 return ReturnValue_ok;
578}
579
580ReturnValue FakeMotionControlMicro::getLastJointFaultRaw(int j, int& fault, std::string& message)
581{
582 _mutex.lock();
583 fault = _hwfault_code[j];
584 message = _hwfault_message[j];
585 _mutex.unlock();
586 return ReturnValue_ok;
587}
588
590{
591 ax = _njoints;
592 return ReturnValue_ok;
593}
594// eof
void checkAndDestroy(T *&p)
std::string toString(const T &value)
convert an arbitrary type to string.
static bool NOT_YET_IMPLEMENTED(const char *txt)
static bool DEPRECATED(const char *txt)
std::string toString(const T &value)
convert an arbitrary type to string.
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION
#define ReturnValue_ok
Definition ReturnValue.h:82
#define YARP_METHOD_NOT_YET_IMPLEMENTED()
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
void threadRelease() override
Release method.
yarp::dev::ReturnValue getMotorEncoderSpeedsRaw(double *spds) override
Read the instantaneous speed of all motor encoders.
yarp::dev::ReturnValue setMotorEncoderRaw(int j, double val) override
Set the value of the motor encoder for a given motor.
yarp::dev::ReturnValue resetEncoderRaw(int j) override
Reset encoder, single joint.
yarp::dev::ReturnValue getMotorEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all motor encoders.
bool alloc(int njoints)
Allocated buffers.
yarp::dev::ReturnValue getMotorEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous position of all motor encoders.
bool fromConfig(yarp::os::Searchable &config)
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue getMotorEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of a motor encoder.
yarp::dev::ReturnValue getEncoderSpeedsRaw(double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue getEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getMotorEncodersRaw(double *encs) override
Read the position of all motor encoders.
yarp::dev::ReturnValue getNumberOfMotorEncodersRaw(int *num) override
Get the number of available motor encoders.
yarp::dev::ReturnValue getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue resetMotorEncoderRaw(int j) override
Reset motor encoder, single motor.
yarp::dev::ReturnValue setMotorEncodersRaw(const double *vals) override
Set the value of all motor encoders.
yarp::dev::ReturnValue getAxisNameRaw(int axis, std::string &name) override
void run() override
Loop function.
yarp::dev::ReturnValue getMotorEncoderRaw(int j, double *v) override
Read the value of a motor encoder.
yarp::dev::ReturnValue setEncodersRaw(const double *vals) override
Set the value of all encoders.
yarp::dev::ReturnValue resetMotorEncodersRaw() override
Reset motor encoders.
void resizeBuffers()
Resize previously allocated buffers.
bool open(yarp::os::Searchable &par) override
Open the DeviceDriver.
yarp::dev::ReturnValue getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
Read the instantaneous position of a motor encoder.
bool threadInit() override
Initialization method.
yarp::dev::ReturnValue getMotorEncoderCountsPerRevolutionRaw(int m, double *v) override
Gets number of counts per revolution for motor encoder m.
yarp::dev::ReturnValue getEncodersRaw(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue getMotorEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of a motor encoder.
yarp::dev::ReturnValue getEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getEncoderTimedRaw(int j, double *encs, double *stamp) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue getEncoderRaw(int j, double *v) override
Read the value of an encoder.
yarp::dev::ReturnValue getEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of an axis.
yarp::dev::ReturnValue setEncoderRaw(int j, double val) override
Set the value of the encoder for a given joint.
yarp::dev::ReturnValue getLastJointFaultRaw(int j, int &fault, std::string &message) override
yarp::dev::ReturnValue resetEncodersRaw() override
Reset encoders.
yarp::dev::ReturnValue setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
Bottle & findGroup(const std::string &key) const override
Gets a list corresponding to a given keyword.
Definition Bottle.cpp:294
A mini-server for performing network communication in the background.
An abstraction for a periodic thread.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A base class for nested structures that can be searched.
Definition Searchable.h:31
void zero()
Set to zero the elements of the vector.
Definition VectorBase.h:296
void resize(size_t size)
Resize the vector.
Definition VectorBase.h:106
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
std::string get_string(const std::string &key, bool *found=nullptr)
Read a string from an environment variable.
Definition environment.h:65
For streams capable of holding different kinds of content, check what they actually have.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
The components from which ports and connections are built.
An interface to the operating system, including Port based communication.