YARP
Yet Another Robot Platform
ControlBoardWrapperPositionControl.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
12{
13 size_t off;
14 try {
15 off = device.lut.at(j).offset;
16 } catch (...) {
17 yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
18 return false;
19 }
20 size_t subIndex = device.lut[j].deviceEntry;
21
22 SubDevice* p = device.getSubdevice(subIndex);
23 if (!p) {
24 return false;
25 }
26
27 if (p->pos) {
28 return p->pos->positionMove(static_cast<int>(off + p->base), ref);
29 }
30
31 return false;
32}
33
34
36{
37 bool ret = true;
38 int j_wrap = 0; // index of the wrapper joint
39
40 int nDev = device.subdevices.size();
41 for (int subDev_idx = 0; subDev_idx < nDev; subDev_idx++) {
42 size_t subIndex = device.lut[j_wrap].deviceEntry;
43 SubDevice* p = device.getSubdevice(subIndex);
44
45 if (!p) {
46 return false;
47 }
48
49 int wrapped_joints = static_cast<int>((p->top - p->base) + 1);
50 int* joints = new int[wrapped_joints];
51
52 if (p->pos) {
53 // versione comandi su subset di giunti
54 for (int j_dev = 0; j_dev < wrapped_joints; j_dev++) {
55 joints[j_dev] = static_cast<int>(p->base + j_dev); // for all joints is equivalent to add offset term
56 }
57
58 ret = ret && p->pos->positionMove(wrapped_joints, joints, &refs[j_wrap]);
59 j_wrap += wrapped_joints;
60 } else {
61 ret = false;
62 }
63
64 if (joints != nullptr) {
65 delete[] joints;
66 joints = nullptr;
67 }
68 }
69
70 return ret;
71}
72
73
74bool ControlBoardWrapperPositionControl::positionMove(const int n_joints, const int* joints, const double* refs)
75{
76 bool ret = true;
77
78 rpcDataMutex.lock();
79 //Reset subdev_jointsVectorLen vector
80 memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
81
82 // Create a map of joints for each subDevice
83 size_t subIndex = 0;
84 for (int j = 0; j < n_joints; j++) {
85 subIndex = device.lut[joints[j]].deviceEntry;
87 static_cast<int>(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base);
88 rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = refs[j];
90 }
91
92 for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
93 if (rpcData.subdevices_p[subIndex]->pos) {
95 } else {
96 ret = false;
97 }
98 }
99 rpcDataMutex.unlock();
100 return ret;
101}
102
103
105{
106 size_t off;
107 try {
108 off = device.lut.at(j).offset;
109 } catch (...) {
110 yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
111 return false;
112 }
113 size_t subIndex = device.lut[j].deviceEntry;
114
115 SubDevice* p = device.getSubdevice(subIndex);
116
117 if (!p) {
118 return false;
119 }
120
121 if (p->pos) {
122 bool ret = p->pos->getTargetPosition(static_cast<int>(off + p->base), ref);
123 return ret;
124 }
125 *ref = 0;
126 return false;
127}
128
129
131{
132 auto* targets = new double[device.maxNumOfJointsInDevices];
133 bool ret = true;
134 for (size_t d = 0; d < device.subdevices.size(); d++) {
136 if (!p) {
137 ret = false;
138 break;
139 }
140
141 if ((p->pos) && (ret = p->pos->getTargetPositions(targets))) {
142 for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) {
143 spds[juser] = targets[jdevice];
144 }
145 } else {
146 printError("getTargetPositions", p->id, ret);
147 ret = false;
148 break;
149 }
150 }
151
152 delete[] targets;
153 return ret;
154}
155
156
157bool ControlBoardWrapperPositionControl::getTargetPositions(const int n_joints, const int* joints, double* targets)
158{
159 bool ret = true;
160
161 rpcDataMutex.lock();
162 //Reset subdev_jointsVectorLen vector
163 memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
164
165 // Create a map of joints for each subDevice
166 size_t subIndex = 0;
167 for (int j = 0; j < n_joints; j++) {
168 subIndex = device.lut[joints[j]].deviceEntry;
170 static_cast<int>(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base);
172 }
173
174 for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
175 if (rpcData.subdevices_p[subIndex]->pos) {
177 }
178 }
179
180 if (ret) {
181 // ReMix values by user expectations
182 for (size_t i = 0; i < rpcData.deviceNum; i++) {
183 rpcData.subdev_jointsVectorLen[i] = 0; // reset tmp index
184 }
185
186 // fill the output vector
187 for (int j = 0; j < n_joints; j++) {
188 subIndex = device.lut[joints[j]].deviceEntry;
189 targets[j] = rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]];
191 }
192 } else {
193 for (int j = 0; j < n_joints; j++) {
194 targets[j] = 0;
195 }
196 }
197 rpcDataMutex.unlock();
198 return ret;
199}
200
201
203{
204 size_t off;
205 try {
206 off = device.lut.at(j).offset;
207 } catch (...) {
208 yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
209 return false;
210 }
211 size_t subIndex = device.lut[j].deviceEntry;
212
213 SubDevice* p = device.getSubdevice(subIndex);
214 if (!p) {
215 return false;
216 }
217
218 if (p->pos) {
219 return p->pos->relativeMove(static_cast<int>(off + p->base), delta);
220 }
221
222 return false;
223}
224
225
227{
228 bool ret = true;
229
230 for (size_t l = 0; l < controlledJoints; l++) {
231 int off = device.lut[l].offset;
232 size_t subIndex = device.lut[l].deviceEntry;
233
234 SubDevice* p = device.getSubdevice(subIndex);
235 if (!p) {
236 return false;
237 }
238
239 if (p->pos) {
240 ret = ret && p->pos->relativeMove(static_cast<int>(off + p->base), deltas[l]);
241 } else {
242 ret = false;
243 }
244 }
245 return ret;
246}
247
248
249bool ControlBoardWrapperPositionControl::relativeMove(const int n_joints, const int* joints, const double* deltas)
250{
251 bool ret = true;
252
253 rpcDataMutex.lock();
254 //Reset subdev_jointsVectorLen vector
255 memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
256
257 // Create a map of joints for each subDevice
258 size_t subIndex = 0;
259 for (int j = 0; j < n_joints; j++) {
260 subIndex = device.lut[joints[j]].deviceEntry;
262 static_cast<int>(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base);
263 rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = deltas[j];
265 }
266
267 for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
268 if (rpcData.subdevices_p[subIndex]->pos) {
270 } else {
271 ret = false;
272 }
273 }
274 rpcDataMutex.unlock();
275 return ret;
276}
277
278
280{
281 size_t off;
282 try {
283 off = device.lut.at(j).offset;
284 } catch (...) {
285 yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
286 return false;
287 }
288 size_t subIndex = device.lut[j].deviceEntry;
289
290 SubDevice* p = device.getSubdevice(subIndex);
291
292 if (!p) {
293 return false;
294 }
295
296 if (p->pos) {
297 return p->pos->checkMotionDone(static_cast<int>(off + p->base), flag);
298 }
299
300 return false;
301}
302
303
305{
306 bool ret = true;
307
308 rpcDataMutex.lock();
309 //Reset subdev_jointsVectorLen vector
310 memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
311
312 // Create a map of joints for each subDevice
313 // In this case the "all joint version" of checkMotionDone(bool *flag) cannot be
314 // called because the return value is an 'and' of all joints.
315 // Therefore only the corret joints must be evaluated.
316
317 size_t subIndex = 0;
318 for (size_t j = 0; j < controlledJoints; j++) {
319 subIndex = device.lut[j].deviceEntry;
321 static_cast<int>(device.lut[j].offset + rpcData.subdevices_p[subIndex]->base);
323 }
324
325 bool tmp_subdeviceDone = true;
326 bool tmp_deviceDone = true;
327
328 // for each subdevice wrapped call checkmotiondone only on interested joints
329 for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
330 if (rpcData.subdevices_p[subIndex]->pos) {
331 ret = ret && rpcData.subdevices_p[subIndex]->pos->checkMotionDone(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], &tmp_subdeviceDone);
332 tmp_deviceDone &= tmp_subdeviceDone;
333 }
334 }
335 rpcDataMutex.unlock();
336
337 // return a single value to the caller
338 *flag = tmp_deviceDone;
339 return ret;
340}
341
342
343bool ControlBoardWrapperPositionControl::checkMotionDone(const int n_joints, const int* joints, bool* flags)
344{
345 bool ret = true;
346 bool tmp = true;
347 bool XFlags = true;
348
349 rpcDataMutex.lock();
350 //Reset subdev_jointsVectorLen vector
351 memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
352
353 // Create a map of joints for each subDevice
354 size_t subIndex = 0;
355 for (int j = 0; j < n_joints; j++) {
356 subIndex = device.lut[joints[j]].deviceEntry;
358 static_cast<int>(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base);
360 }
361
362 for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
363 if (rpcData.subdevices_p[subIndex]->pos) {
364 ret = ret && rpcData.subdevices_p[subIndex]->pos->checkMotionDone(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], &XFlags);
365 tmp = tmp && XFlags;
366 } else {
367 ret = false;
368 }
369 }
370 if (ret) {
371 *flags = tmp;
372 } else {
373 *flags = false;
374 }
375 rpcDataMutex.unlock();
376 return ret;
377}
378
379
381{
382 size_t off;
383 try {
384 off = device.lut.at(j).offset;
385 } catch (...) {
386 yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
387 return false;
388 }
389 size_t subIndex = device.lut[j].deviceEntry;
390
391 SubDevice* p = device.getSubdevice(subIndex);
392 if (!p) {
393 return false;
394 }
395
396 if (p->pos) {
397 return p->pos->setRefSpeed(static_cast<int>(off + p->base), sp);
398 }
399 return false;
400}
401
402
404{
405 bool ret = true;
406 int j_wrap = 0; // index of the wrapper joint
407
408 for (size_t subDev_idx = 0; subDev_idx < device.subdevices.size(); subDev_idx++) {
409 SubDevice* p = device.getSubdevice(subDev_idx);
410
411 if (!p) {
412 return false;
413 }
414
415 int wrapped_joints = static_cast<int>((p->top - p->base) + 1);
416 int* joints = new int[wrapped_joints];
417
418 if (p->pos) {
419 // verione comandi su subset di giunti
420 for (int j_dev = 0; j_dev < wrapped_joints; j_dev++) {
421 joints[j_dev] = static_cast<int>(p->base + j_dev);
422 }
423
424 ret = ret && p->pos->setRefSpeeds(wrapped_joints, joints, &spds[j_wrap]);
425 j_wrap += wrapped_joints;
426 } else {
427 ret = false;
428 }
429
430 if (joints != nullptr) {
431 delete[] joints;
432 joints = nullptr;
433 }
434 }
435
436 return ret;
437}
438
439
440bool ControlBoardWrapperPositionControl::setRefSpeeds(const int n_joints, const int* joints, const double* spds)
441{
442 bool ret = true;
443
444 rpcDataMutex.lock();
445 //Reset subdev_jointsVectorLen vector
446 memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
447
448 // Create a map of joints for each subDevice
449 size_t subIndex = 0;
450 for (int j = 0; j < n_joints; j++) {
451 subIndex = device.lut[joints[j]].deviceEntry;
453 static_cast<int>(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base);
454 rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = spds[j];
456 }
457
458 for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
459 if (rpcData.subdevices_p[subIndex]->pos) {
461 } else {
462 ret = false;
463 }
464 }
465 rpcDataMutex.unlock();
466 return ret;
467}
468
469
471{
472 size_t off;
473 try {
474 off = device.lut.at(j).offset;
475 } catch (...) {
476 yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
477 return false;
478 }
479 size_t subIndex = device.lut[j].deviceEntry;
480
481 SubDevice* p = device.getSubdevice(subIndex);
482
483 if (!p) {
484 return false;
485 }
486
487 if (p->pos) {
488 return p->pos->getRefSpeed(static_cast<int>(off + p->base), ref);
489 }
490 *ref = 0;
491 return false;
492}
493
494
496{
497 auto* references = new double[device.maxNumOfJointsInDevices];
498 bool ret = true;
499 for (size_t d = 0; d < device.subdevices.size(); d++) {
501 if (!p) {
502 ret = false;
503 break;
504 }
505
506 if ((p->pos) && (ret = p->pos->getRefSpeeds(references))) {
507 for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) {
508 spds[juser] = references[jdevice];
509 }
510 } else {
511 printError("getRefSpeeds", p->id, ret);
512 ret = false;
513 break;
514 }
515 }
516
517 delete[] references;
518 return ret;
519}
520
521
522bool ControlBoardWrapperPositionControl::getRefSpeeds(const int n_joints, const int* joints, double* spds)
523{
524 bool ret = true;
525
526 rpcDataMutex.lock();
527 //Reset subdev_jointsVectorLen vector
528 memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum);
529
530 // Create a map of joints for each subDevice
531 size_t subIndex = 0;
532 for (int j = 0; j < n_joints; j++) {
533 subIndex = device.lut[joints[j]].deviceEntry;
535 static_cast<int>(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base);
537 }
538
539 for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) {
540 if (rpcData.subdevices_p[subIndex]->pos) {
542 } else {
543 ret = false;
544 }
545 }
546
547 if (ret) {
548 // ReMix values by user expectations
549 for (size_t i = 0; i < rpcData.deviceNum; i++) {
550 rpcData.subdev_jointsVectorLen[i] = 0; // reset tmp index
551 }
552
553 // fill the output vector
554 for (int j = 0; j < n_joints; j++) {
555 subIndex = device.lut[joints[j]].deviceEntry;
556 spds[j] = rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]];
558 }
559 } else {
560 for (int j = 0; j < n_joints; j++) {
561 spds[j] = 0;
562 }
563 }
564 rpcDataMutex.unlock();
565 return ret;
566}
const yarp::os::LogComponent & CONTROLBOARD()
bool ret
void printError(const std::string &func_name, const std::string &info, bool result)
bool getTargetPositions(double *refs) override
Get the last position reference for all axes.
bool getRefSpeeds(double *spds) override
Get reference speed of all joints.
bool getRefSpeed(int j, double *ref) override
Get reference speed for a joint.
bool getTargetPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
bool checkMotionDone(int j, bool *flag) override
Check if the current trajectory is terminated.
bool positionMove(int j, double ref) override
Set new reference point for a single axis.
bool relativeMove(int j, double delta) override
Set relative position.
bool setRefSpeeds(const double *spds) override
Set reference speed on all joints.
bool setRefSpeed(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
int * subdev_jointsVectorLen
SubDevice ** subdevices_p
double ** values
size_t base
Definition: SubDevice.h:55
std::string id
Definition: SubDevice.h:54
yarp::dev::IPositionControl * pos
Definition: SubDevice.h:68
size_t top
Definition: SubDevice.h:56
size_t wbase
Definition: SubDevice.h:57
std::vector< DevicesLutEntry > lut
Definition: SubDevice.h:122
size_t maxNumOfJointsInDevices
Definition: SubDevice.h:123
SubDeviceVector subdevices
Definition: SubDevice.h:121
SubDevice * getSubdevice(size_t i)
Definition: SubDevice.h:125
virtual bool setRefSpeed(int j, double sp)=0
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
virtual bool relativeMove(int j, double delta)=0
Set relative position.
virtual bool getTargetPosition(const int joint, double *ref)
Get the last position reference for the specified axis.
virtual bool setRefSpeeds(const double *spds)=0
Set reference speed on all joints.
virtual bool checkMotionDone(int j, bool *flag)=0
Check if the current trajectory is terminated.
virtual bool getRefSpeeds(double *spds)=0
Get reference speed of all joints.
virtual bool getRefSpeed(int j, double *ref)=0
Get reference speed for a joint.
virtual bool positionMove(int j, double ref)=0
Set new reference point for a single axis.
virtual bool getTargetPositions(double *refs)
Get the last position reference for all axes.
#define yCError(component,...)
Definition: LogComponent.h:213