YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoardRemappingEngine.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
9
10#include <yarp/os/Log.h>
11#include <yarp/os/LogStream.h>
12
13#include <algorithm>
14#include <iostream>
15#include <map>
16#include <mutex>
17#include <cassert>
18
19using namespace yarp::os;
20using namespace yarp::dev;
21using namespace yarp::sig;
22
23void ControlBoardRemappingEngine::setAxesNames(const std::vector<std::string>& axesNames)
24{
25 this->axesNames = axesNames;
26 setNrOfControlledAxes(axesNames.size());
27}
28
30{
31 controlledJoints = nrOfControlledAxes;
32 buffers.controlBoardModes.resize(nrOfControlledAxes,0);
33 buffers.dummyBuffer.resize(nrOfControlledAxes,0.0);
34}
35
37{
38 bool ok = attachAllUsingAxesNames(polylist);
39
40 //check if all devices are attached to the driver
41 bool ready=true;
42 for(unsigned int k=0; k<remappedControlBoards.getNrOfSubControlBoards(); k++)
43 {
44 if (!remappedControlBoards.subdevices[k].isAttached())
45 {
46 ready=false;
47 }
48 }
49
50 if (!ready)
51 {
52 yCError(CONTROLBOARDREMAPPER, "AttachAll failed, some subdevice was not found or its attach failed");
53 return false;
54 }
55
56 if( ok )
57 {
58 // configure the buffers, now that we have all the information about the attached devices
59 allJointsBuffers.configure(remappedControlBoards);
60 selectedJointsBuffers.configure(remappedControlBoards);
61 }
62
63 return ok;
64}
65
66// First we store a map between each axes name
67// in the passed PolyDriverList and the device in which they belong and their index
74
75
76bool ControlBoardRemappingEngine::attachAllUsingAxesNames(const PolyDriverList& polylist)
77{
78 std::map<std::string, axisLocation> axesLocationMap;
79
80 for(int p=0;p<polylist.size();p++)
81 {
82 // If there is a device with a specific device key, use it
83 // as a calibrator, otherwise rely on the subcontrolboards
84 // as usual
85 std::string deviceKey=polylist[p]->key;
86 if(deviceKey == "Calibrator" || deviceKey == "calibrator")
87 {
88 // Set the IRemoteCalibrator interface, the wrapper must point to the calibrator rdevice
90 polylist[p]->poly->view(calibrator);
91
93 continue;
94 }
95
96 // find if one of the desired axis is in this device
98 yarp::dev::IEncoders *iencs = nullptr;
99 polylist[p]->poly->view(iaxinfos);
100 polylist[p]->poly->view(iencs);
101
102 if( !iencs ||
103 !iaxinfos )
104 {
105 yCError(CONTROLBOARDREMAPPER) << "sub-device" << deviceKey << "does not implemented the required IAxisInfo or IEncoders interfaces";
106 return false;
107 }
108
109 size_t nrOfSubdeviceAxes;
110 bool ok = iencs->getAxes(nrOfSubdeviceAxes);
111
112 if( !ok )
113 {
114 yCError(CONTROLBOARDREMAPPER) << "sub-device" << deviceKey << "does not implemented the required getAxes method";
115 return false;
116 }
117
119 {
120 std::string axNameYARP;
121 ok = iaxinfos->getAxisName(axInSubDevice,axNameYARP);
122
123 std::string axName = axNameYARP;
124
125 if( !ok )
126 {
127 yCError(CONTROLBOARDREMAPPER) << "sub-device" << deviceKey << "does not implemented the required getAxisName method";
128 return false;
129 }
130
131 auto it = axesLocationMap.find(axName);
132 if( it != axesLocationMap.end() )
133 {
135 << "multiple axes have the same name" << axName
136 << "on on device " << polylist[p]->key << "with index" << axInSubDevice
137 <<"and another on device" << it->second.subDeviceKey << "with index" << it->second.indexInSubDevice;
138 return false;
139 }
140
142 newLocation.subDeviceKey = polylist[p]->key;
143 newLocation.indexInSubDevice = axInSubDevice;
144 newLocation.indexOfSubDeviceInPolyDriverList = p;
146 }
147 }
148
149 // We store the key of all the devices that we actually use in the remapped control device
150 std::vector<std::string> subControlBoardsKeys;
151 std::map<std::string, size_t> subControlBoardKey2IndexInPolyDriverList;
152 std::map<std::string, size_t> subControlBoardKey2IndexInRemappedControlBoards;
153
154
155 // Once we build the axis map, we build the mapping between the remapped axes and
156 // the couple subControlBoard, axis in subControlBoard
157 for(const auto& axesName : axesNames)
158 {
159 auto it = axesLocationMap.find(axesName);
160 if( it == axesLocationMap.end() )
161 {
163 << "axis " << axesName
164 << "specified in axesNames was not found in the axes of the controlboards passed to attachAll, attachAll failed.";
165 return false;
166 }
167
168 axisLocation loc = it->second;
169 std::string key = loc.subDeviceKey;
170
171 if(std::find(subControlBoardsKeys.begin(), subControlBoardsKeys.end(), key) == subControlBoardsKeys.end())
172 {
173 /* subControlBoardsKeys does not contain key */
175 subControlBoardsKeys.push_back(key);
177 }
178 }
179
180 assert(controlledJoints == (int) axesNames.size());
181
182 // We have now the number of controlboards to attach to
184 remappedControlBoards.lut.resize(controlledJoints);
185 remappedControlBoards.subdevices.resize(nrOfSubControlBoards);
186
187 // Open the controlboards
188 for(size_t ctrlBrd=0; ctrlBrd < nrOfSubControlBoards; ctrlBrd++)
189 {
192 tmpDevice->setVerbose(_verb);
194 bool ok = tmpDevice->attach(polylist[p]->poly,subControlBoardsKeys[ctrlBrd]);
195
196 if( !ok )
197 {
198 return false;
199 }
200 }
201
202
203 for(size_t l=0; l < axesNames.size(); l++)
204 {
205 axisLocation loc = axesLocationMap[axesNames[l]];
206 remappedControlBoards.lut[l].subControlBoardIndex = subControlBoardKey2IndexInRemappedControlBoards[loc.subDeviceKey];
207 remappedControlBoards.lut[l].axisIndexInSubControlBoard = (size_t)loc.indexInSubDevice;
208 }
209
210 return true;
211}
212
213
215{
216 //check if we already instantiated a subdevice previously
217 int devices=remappedControlBoards.getNrOfSubControlBoards();
218 for (int k = 0; k < devices; k++) {
219 remappedControlBoards.getSubControlBoard(k)->detach();
220 }
221
223 return true;
224}
225
226
230
231//
232// IPid Interface
233//
234
235ReturnValue ControlBoardRemappingEngine::getAvailablePids(int j, std::vector<yarp::dev::PidControlTypeEnum>& avail)
236{
237 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
238 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
239
240 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
241 if (!p) {
242 return ReturnValue::return_code::return_value_error_generic;
243 }
244
246
247 if (p->pid) {
248 ok = p->pid->getAvailablePids(off, avail);
249 }
250 else {
251 ok = ReturnValue::return_code::return_value_error_generic;
252 }
253
254 return ok;
255}
256
258{
259 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
260 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
261
262 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
263 if (!s)
264 {
265 return ReturnValue::return_code::return_value_error_generic;
266 }
267
268 if (s->pid)
269 {
270 return s->pid->setPid(pidtype, off, p);
271 }
272
273 return ReturnValue::return_code::return_value_error_generic;
274}
275
277{
279
280 for(int l=0;l<controlledJoints;l++)
281 {
282 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
283
284 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
285
286 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
287 if (!p)
288 {
289 return ReturnValue::return_code::return_value_error_generic;
290 }
291
292 if (p->pid)
293 {
294 ReturnValue ok = p->pid->setPid(pidtype, off, ps[l]);
295 ret=ret&&ok;
296 }
297 else
298 {
299 ret=ReturnValue::return_code::return_value_error_generic;
300 }
301 }
302 return ret;
303}
304
306{
307 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
308 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
309
310 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
311
312 if (!p)
313 {
314 return ReturnValue::return_code::return_value_error_generic;
315 }
316
317 if (p->pid)
318 {
319 return p->pid->setPidReference(pidtype, off, ref);
320 }
321
322 return ReturnValue::return_code::return_value_error_generic;
323}
324
326{
328
329 for(int l=0;l<controlledJoints;l++)
330 {
331 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
332
333 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
334
335 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
336 if (!p)
337 {
338 return ReturnValue::return_code::return_value_error_generic;
339 }
340
341 if (p->pid)
342 {
343 ReturnValue ok = p->pid->setPidReference(pidtype, off, refs[l]);
344 ret=ret&&ok;
345 }
346 else
347 {
348 ret=ReturnValue::return_code::return_value_error_generic;
349 }
350 }
351 return ret;
352}
353
355{
356 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
357 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
358
359 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
360 if (!p)
361 {
362 return ReturnValue::return_code::return_value_error_generic;
363 }
364
365 if (p->pid)
366 {
367 return p->pid->setPidErrorLimit(pidtype, off, limit);
368 }
369
370 return ReturnValue::return_code::return_value_error_generic;
371}
372
374{
376
377 for(int l=0;l<controlledJoints;l++)
378 {
379 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
380
381 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
382
383 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
384 if (!p)
385 {
386 return ReturnValue::return_code::return_value_error_generic;
387 }
388
389 if (p->pid)
390 {
391 ReturnValue ok = p->pid->setPidErrorLimit(pidtype, off, limits[l]);
392 ret=ret&&ok;
393 }
394 else
395 {
396 ret=ReturnValue::return_code::return_value_error_generic;
397 }
398 }
399 return ret;
400}
401
403{
404 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
405 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
406
407 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
408
409 if (!p)
410 {
411 return ReturnValue::return_code::return_value_error_generic;
412 }
413
414 if (p->pid)
415 {
416 return p->pid->getPidError(pidtype, off, err);
417 }
418
419 return ReturnValue::return_code::return_value_error_generic;
420}
421
423{
425
426 for(int l=0;l<controlledJoints;l++)
427 {
428 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
429 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
430
431 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
432 if (!p)
433 {
434 return ReturnValue::return_code::return_value_error_generic;
435 }
436
437 if (p->pid)
438 {
439 ReturnValue ok = p->pid->getPidError(pidtype, off, errs+l);
440 ret=ret&&ok;
441 }
442 else
443 {
444 ret=ReturnValue::return_code::return_value_error_generic;
445 }
446 }
447 return ret;
448}
449
451{
452 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
453 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
454
455 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
456 if (!p)
457 {
458 return ReturnValue::return_code::return_value_error_generic;
459 }
460
461 if (p->pid)
462 {
463 return p->pid->getPidOutput(pidtype, off, out);
464 }
465
466 return ReturnValue::return_code::return_value_error_generic;
467}
468
470{
472
473 for(int l=0;l<controlledJoints;l++)
474 {
475 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
476
477 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
478
479 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
480 if (!p)
481 {
482 return ReturnValue::return_code::return_value_error_generic;
483 }
484
485 if (p->pid)
486 {
487 ret=ret&&p->pid->getPidOutput(pidtype, off, outs+l);
488 }
489 else
490 {
491 ret=ReturnValue::return_code::return_value_error_generic;
492 }
493 }
494 return ret;
495}
496
498{
499 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
500 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
501
502 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
503 if (!p)
504 {
505 return ReturnValue::return_code::return_value_error_generic;
506 }
507
508 if (p->pid)
509 {
510 return p->pid->setPidOffset(pidtype, off, v);
511 }
512
513 return ReturnValue::return_code::return_value_error_generic;
514}
515
517{
518 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
519 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
520
521 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
522 if (!s)
523 {
524 return ReturnValue::return_code::return_value_error_generic;
525 }
526
527 if (s->pid)
528 {
529 return s->pid->getPidOffset(pidtype, off, v);
530 }
531
532 return ReturnValue::return_code::return_value_error_generic;
533}
534
536{
537 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
538 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
539
540 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
541 if (!p)
542 {
543 return ReturnValue::return_code::return_value_error_generic;
544 }
545
546 if (p->pid)
547 {
548 return p->pid->setPidFeedforward(pidtype, off, v);
549 }
550
551 return ReturnValue::return_code::return_value_error_generic;
552}
553
555{
556 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
557 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
558
559 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
560 if (!s)
561 {
562 return ReturnValue::return_code::return_value_error_generic;
563 }
564
565 if (s->pid)
566 {
567 return s->pid->getPidFeedforward(pidtype, off, v);
568 }
569
570 return ReturnValue::return_code::return_value_error_generic;
571}
572
574{
575 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
576 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
577
578 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
579 if (!s)
580 {
581 return ReturnValue::return_code::return_value_error_generic;
582 }
583
584 if (s->pid)
585 {
586 return s->pid->getPid(pidtype, off, p);
587 }
588
589 return ReturnValue::return_code::return_value_error_generic;
590}
591
593{
595
596 for(int l=0;l<controlledJoints;l++)
597 {
598 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
599 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
600
601 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
602 if (!p)
603 {
604 return ReturnValue::return_code::return_value_error_generic;
605 }
606
607 if (p->pid)
608 {
609 ReturnValue ok = p->pid->getPid(pidtype, off, pids+l);
610 ret = ok && ret;
611 }
612 else
613 {
614 ret=ReturnValue::return_code::return_value_error_generic;
615 }
616 }
617
618 return ret;
619}
620
622{
623 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
624 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
625
626 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
627 if (!p)
628 {
629 return ReturnValue::return_code::return_value_error_generic;
630 }
631
632 if (p->pid)
633 {
634 return p->pid->getPidReference(pidtype, off, ref);
635 }
636
637 return ReturnValue::return_code::return_value_error_generic;
638}
639
641{
643
644 for(int l=0;l<controlledJoints;l++)
645 {
646 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
647
648 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
649
650 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
651 if (!p)
652 {
653 return ReturnValue::return_code::return_value_error_generic;
654 }
655
656 if (p->pid)
657 {
658 ReturnValue ok = p->pid->getPidReference(pidtype, off, refs+l);
659 ret=ret && ok;
660 }
661 else
662 {
663 ret=ReturnValue::return_code::return_value_error_generic;
664 }
665 }
666 return ret;
667}
668
670{
671 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
672 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
673
674 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
675
676 if (!p)
677 {
678 return ReturnValue::return_code::return_value_error_generic;
679 }
680
681 if (p->pid)
682 {
683 return p->pid->getPidErrorLimit(pidtype, off, limit);
684 }
685 return ReturnValue::return_code::return_value_error_generic;
686}
687
689{
691
692 for(int l=0;l<controlledJoints;l++)
693 {
694 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
695
696 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
697
698 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
699
700 if (!p)
701 {
702 return ReturnValue::return_code::return_value_error_generic;
703 }
704
705 if (p->pid)
706 {
707 ReturnValue ok = p->pid->getPidErrorLimit(pidtype, off, limits+l);
708 ret=ret&&ok;
709 }
710 else
711 {
712 ret=ReturnValue::return_code::return_value_error_generic;
713 }
714 }
715 return ret;
716}
717
719{
720 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
721 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
722
723 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
724
725 if (!p)
726 {
727 return ReturnValue::return_code::return_value_error_generic;
728 }
729
730 if (p->pid)
731 {
732 return p->pid->resetPid(pidtype, off);
733 }
734
735 return ReturnValue::return_code::return_value_error_generic;
736}
737
739{
740 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
741 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
742
743 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
744
745 if (!p)
746 {
747 return ReturnValue::return_code::return_value_error_generic;
748 }
749
750 return p->pid->disablePid(pidtype, off);
751}
752
754{
755 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
756 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
757
758 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
759
760 if (!p)
761 {
762 return ReturnValue::return_code::return_value_error_generic;
763 }
764
765 if (p->pid)
766 {
767 return p->pid->enablePid(pidtype, off);
768 }
769
770 return ReturnValue::return_code::return_value_error_generic;
771}
772
774{
775 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
776 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
777
778 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
779
780 if (!p)
781 {
782 return ReturnValue::return_code::return_value_error_generic;
783 }
784
785 if (p->pid)
786 {
787 return p->pid->isPidEnabled(pidtype, off, enabled);
788 }
789
790 return ReturnValue::return_code::return_value_error_generic;
791}
792
794{
795 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
796 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
797
798 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
799
800 if (!p)
801 {
802 return ReturnValue::return_code::return_value_error_generic;
803 }
804
805 if (p->pid)
806 {
807 return p->pid->getPidExtraInfo(pidtype, off, units);
808 }
809
810 return ReturnValue::return_code::return_value_error_generic;
811}
812
813ReturnValue ControlBoardRemappingEngine::getPidExtraInfos(const PidControlTypeEnum& pidtype, std::vector<PidExtraInfo>& units)
814{
816
817 for(int l=0;l<controlledJoints;l++)
818 {
819 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
820 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
821
822 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
823 if (!p)
824 {
825 return ReturnValue::return_code::return_value_error_generic;
826 }
827
828 if (p->pid)
829 {
830 ReturnValue ok = p->pid->getPidExtraInfo(pidtype, off, units[l]);
831 ret = ok && ret;
832 }
833 else
834 {
835 ret=ReturnValue::return_code::return_value_error_generic;
836 }
837 }
838
839 return ret;
840}
841
842/* IPositionControl */
844{
845 ax=controlledJoints;
846 return ReturnValue_ok;
847}
848
850{
851 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
852 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
853
854 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
855
856 if (!p)
857 {
858 return ReturnValue::return_code::return_value_error_generic;
859 }
860
861 if (p->pos)
862 {
863 return p->pos->positionMove(off, ref);
864 }
865
866 return ReturnValue::return_code::return_value_error_generic;
867}
868
870{
872 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
873
874 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(refs,remappedControlBoards);
875
876 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
877 {
878 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
879
881 if (p->pos) {
882 ok = p->pos->positionMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
883 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
884 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
885 }
886 else {
887 ok = ReturnValue::return_code::return_value_error_generic;
888 }
889 ret = ret && ok;
890 }
891
892 return ret;
893}
894
895ReturnValue ControlBoardRemappingEngine::positionMove(const int n_joints, const int *joints, const double *refs)
896{
898 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
899
900 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(refs,n_joints,joints,remappedControlBoards);
901
902 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
903 {
904 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
905
907 if (p->pos) {
908 ok = p->pos->positionMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
909 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
910 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
911 }
912 else {
913 ok = ReturnValue::return_code::return_value_error_generic;
914 }
915 ret = ret && ok;
916 }
917
918 return ret;
919}
920
922{
923 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
924 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
925
926 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
927
928 if (!p)
929 {
930 return ReturnValue::return_code::return_value_error_generic;
931 }
932
933 if (p->pos)
934 {
935 ReturnValue ret = p->pos->getTargetPosition(off, ref);
936 return ret;
937 }
938
939 return ReturnValue::return_code::return_value_error_generic;
940}
941
943{
945 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
946
947 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
948 {
949 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
950
952
953 if( p->pos )
954 {
955 ok = p->pos->getTargetPositions(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
956 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
957 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
958 }
959 else
960 {
961 ok = ReturnValue::return_code::return_value_error_generic;
962 }
963
964 ret = ret && ok;
965 }
966
967 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(spds,remappedControlBoards);
968
969 return ret;
970}
971
972
973ReturnValue ControlBoardRemappingEngine::getTargetPositions(const int n_joints, const int *joints, double *targets)
974{
976 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
977
978 // Resize the input buffers
979 selectedJointsBuffers.resizeSubControlBoardBuffers(n_joints,joints,remappedControlBoards);
980
981 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
982 {
983 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
984
986
987 if( p->pos )
988 {
989 ok = p->pos->getTargetPositions(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
990 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
991 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
992 }
993 else
994 {
995 ok = ReturnValue::return_code::return_value_error_generic;
996 }
997
998 ret = ret && ok;
999 }
1000
1001 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(targets,n_joints,joints,remappedControlBoards);
1002
1003 return ret;
1004}
1005
1007{
1008 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1009 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1010
1011 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1012
1013 if (!p)
1014 {
1015 return ReturnValue::return_code::return_value_error_generic;
1016 }
1017
1018 if (p->pos)
1019 {
1020 return p->pos->relativeMove(off, delta);
1021 }
1022
1023 return ReturnValue::return_code::return_value_error_generic;
1024}
1025
1027{
1029 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1030
1031 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(deltas,remappedControlBoards);
1032
1033 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1034 {
1035 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1037 if (p->pos) {
1038 ok = p->pos->relativeMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1039 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1040 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1041 }
1042 else {
1043 ok = ReturnValue::return_code::return_value_error_generic;
1044 }
1045 ret = ret && ok;
1046 }
1047
1048 return ret;
1049}
1050
1051ReturnValue ControlBoardRemappingEngine::relativeMove(const int n_joints, const int *joints, const double *deltas)
1052{
1054 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1055
1056 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(deltas,n_joints,joints,remappedControlBoards);
1057
1058 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1059 {
1060 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1062 if (p->pos) {
1063 ok = p->pos->relativeMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1064 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1065 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1066 }
1067 else {
1068 ok = ReturnValue::return_code::return_value_error_generic;
1069 }
1070 ret = ret && ok;
1071 }
1072
1073 return ret;
1074}
1075
1077{
1078 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1079 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1080
1081 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1082
1083 if (!p)
1084 {
1085 return ReturnValue::return_code::return_value_error_generic;
1086 }
1087
1088 if (p->pos)
1089 {
1090 return p->pos->checkMotionDone(off, flag);
1091 }
1092
1093 return ReturnValue::return_code::return_value_error_generic;
1094}
1095
1097{
1099 flag=true;
1100
1101 for(int l=0;l<controlledJoints;l++)
1102 {
1103 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1104 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1105
1106 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1107
1108 if (!p)
1109 {
1110 return ReturnValue::return_code::return_value_error_generic;
1111 }
1112
1113 if (p->pos)
1114 {
1115 bool singleJointMotionDone =false;
1117 ret = ret && ok;
1118 flag = flag && singleJointMotionDone;
1119 }
1120 else
1121 {
1122 ret = ReturnValue::return_code::return_value_error_generic;
1123 }
1124 }
1125 return ret;
1126}
1127
1128ReturnValue ControlBoardRemappingEngine::checkMotionDone(const std::vector<int>& joints, bool& flag)
1129{
1131 flag=true;
1132
1133 for(int j=0;j<joints.size();j++)
1134 {
1135 int l = joints[j];
1136
1137 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1138 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1139
1140 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1141
1142 if (!p)
1143 {
1144 return ReturnValue::return_code::return_value_error_generic;
1145 }
1146
1147 if (p->pos)
1148 {
1149 bool singleJointMotionDone =false;
1151 ret = ret && ok;
1152 flag = flag && singleJointMotionDone;
1153 }
1154 else
1155 {
1156 ret = ReturnValue::return_code::return_value_error_generic;
1157 }
1158 }
1159
1160 return ret;
1161}
1162
1164{
1165 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1166 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1167
1168 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1169
1170 if (!p)
1171 {
1172 return ReturnValue::return_code::return_value_error_generic;
1173 }
1174
1175 if (p->pos)
1176 {
1177 return p->pos->setTrajSpeed(off, sp);
1178 }
1179
1180 return ReturnValue::return_code::return_value_error_generic;
1181}
1182
1184{
1186 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1187
1188 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(spds,remappedControlBoards);
1189
1190 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1191 {
1192 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1193
1195 if (p->pos) {
1196 ok = p->pos->setTrajSpeeds(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1197 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1198 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1199 }
1200 else {
1201 ok = ReturnValue::return_code::return_value_error_generic;
1202 }
1203 ret = ret && ok;
1204 }
1205
1206 return ret;
1207}
1208
1209ReturnValue ControlBoardRemappingEngine::setTrajSpeeds(const int n_joints, const int *joints, const double *spds)
1210{
1212 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1213
1214 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(spds,n_joints,joints,remappedControlBoards);
1215
1216 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1217 {
1218 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1219
1221 if (p->pos) {
1222 ok = p->pos->setTrajSpeeds(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1223 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1224 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1225 }
1226 else {
1227 ok = ReturnValue::return_code::return_value_error_generic;
1228 }
1229 ret = ret && ok;
1230 }
1231
1232 return ret;
1233}
1234
1236{
1237 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1238 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1239
1240 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1241
1242 if (!p)
1243 {
1244 return ReturnValue::return_code::return_value_error_generic;
1245 }
1246
1247 if (p->pos)
1248 {
1249 return p->pos->setTrajAcceleration(off, acc);
1250 }
1251
1252 return ReturnValue::return_code::return_value_error_generic;
1253}
1254
1256{
1258 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1259
1260 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(accs,remappedControlBoards);
1261
1262 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1263 {
1264 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1265
1267 if (p->pos) {
1269 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1270 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1271 }
1272 else {
1273 ok = ReturnValue::return_code::return_value_error_generic;
1274 }
1275 ret = ret && ok;
1276 }
1277
1278 return ret;
1279}
1280
1281ReturnValue ControlBoardRemappingEngine::setTrajAccelerations(const int n_joints, const int *joints, const double *accs)
1282{
1284 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1285
1286 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(accs,n_joints,joints,remappedControlBoards);
1287
1288 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1289 {
1290 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1291
1293 if (p->pos) {
1294 ok = p->pos->setTrajAccelerations(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1295 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1296 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1297 }
1298 else {
1299 ok = ReturnValue::return_code::return_value_error_generic;
1300 }
1301 ret = ret && ok;
1302 }
1303
1304 return ret;
1305}
1306
1308{
1309 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1310 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1311
1312 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1313
1314 if (!p)
1315 {
1316 return ReturnValue::return_code::return_value_error_generic;
1317 }
1318
1319 if (p->pos)
1320 {
1321 return p->pos->getTrajSpeed(off, ref);
1322 }
1323
1324 return ReturnValue::return_code::return_value_error_generic;
1325}
1326
1328{
1330 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1331
1332 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1333 {
1334 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1335
1337 if( p->pos )
1338 {
1339 ok = p->pos->getTrajSpeeds(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1340 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1341 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1342 }
1343 else
1344 {
1345 ok = ReturnValue::return_code::return_value_error_generic;
1346 }
1347
1348 ret = ret && ok;
1349 }
1350
1351 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(spds,remappedControlBoards);
1352
1353 return ret;
1354}
1355
1356
1357ReturnValue ControlBoardRemappingEngine::getTrajSpeeds(const int n_joints, const int *joints, double *spds)
1358{
1360 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1361
1362 // Resize the input buffers
1363 selectedJointsBuffers.resizeSubControlBoardBuffers(n_joints,joints,remappedControlBoards);
1364
1365 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1366 {
1367 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1368
1370
1371 if( p->pos )
1372 {
1373 ok = p->pos->getTrajSpeeds(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1374 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1375 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1376 }
1377 else
1378 {
1379 ok = ReturnValue::return_code::return_value_error_generic;
1380 }
1381
1382 ret = ret && ok;
1383 }
1384
1385 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(spds,n_joints,joints,remappedControlBoards);
1386
1387 return ret;
1388}
1389
1391{
1392 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1393 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1394
1395 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1396
1397 if (!p)
1398 {
1399 return ReturnValue::return_code::return_value_error_generic;
1400 }
1401
1402 if (p->pos)
1403 {
1404 return p->pos->getTrajAcceleration(off, acc);
1405 }
1406
1407 return ReturnValue::return_code::return_value_error_generic;
1408}
1409
1411{
1413 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1414
1415 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1416 {
1417 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1418
1420
1421 if( p->pos )
1422 {
1424 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1425 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1426 }
1427 else
1428 {
1429 ok = ReturnValue::return_code::return_value_error_generic;
1430 }
1431
1432 ret = ret && ok;
1433 }
1434
1435 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(accs,remappedControlBoards);
1436
1437 return ret;
1438}
1439
1440ReturnValue ControlBoardRemappingEngine::getTrajAccelerations(const int n_joints, const int *joints, double *accs)
1441{
1443 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1444
1445 // Resize the input buffers
1446 selectedJointsBuffers.resizeSubControlBoardBuffers(n_joints,joints,remappedControlBoards);
1447
1448 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1449 {
1450 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1451
1453 if( p->pos )
1454 {
1455 ok = p->pos->getTrajAccelerations(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1456 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1457 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1458 }
1459 else
1460 {
1461 ok = ReturnValue::return_code::return_value_error_generic;
1462 }
1463
1464 ret = ret && ok;
1465 }
1466
1467 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(accs,n_joints,joints,remappedControlBoards);
1468
1469 return ret;
1470}
1471
1473{
1474 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1475 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1476
1477 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1478
1479 if (!p)
1480 {
1481 return ReturnValue::return_code::return_value_error_generic;
1482 }
1483
1484 if (p->pos)
1485 {
1486 return p->pos->stop(off);
1487 }
1488
1489 return ReturnValue::return_code::return_value_error_generic;
1490}
1491
1493{
1495 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1496
1497 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1498 {
1499 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1500
1502 if (p->pos) {
1503 ok = p->pos->stop(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1504 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data());
1505 }
1506 else
1507 {
1508 ok = ReturnValue::return_code::return_value_error_generic;
1509 }
1510 ret = ret && ok;
1511 }
1512
1513 return ret;
1514}
1515
1516ReturnValue ControlBoardRemappingEngine::stop(const int n_joints, const int *joints)
1517{
1519 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1520 std::lock_guard<std::mutex> lock2(buffers.mutex);
1521
1522
1523 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(buffers.dummyBuffer.data(),n_joints,joints,remappedControlBoards);
1524
1525 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1526 {
1527 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1529 if (p->pos) {
1530 ok = p->pos->stop(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1531 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data());
1532 }
1533 else
1534 {
1535 ok = ReturnValue::return_code::return_value_error_generic;
1536 }
1537 ret = ret && ok;
1538 }
1539
1540 return ret;
1541}
1542
1543/* IJointFaultControl */
1544ReturnValue ControlBoardRemappingEngine::getLastJointFault(int j, int& fault, std::string& message)
1545{
1546 int off = (int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1547 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
1548
1549 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
1550
1551 if (!p)
1552 {
1553 return ReturnValue::return_code::return_value_error_generic;
1554 }
1555
1556 if (p->iFault)
1557 {
1558 return p->iFault->getLastJointFault(off, fault, message);
1559 }
1560
1561 return ReturnValue::return_code::return_value_error_generic;
1562}
1563
1564/* IVelocityControl */
1565
1567{
1568 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1569 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1570
1571 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1572
1573 if (!p)
1574 {
1575 return ReturnValue::return_code::return_value_error_generic;
1576 }
1577
1578 if (p->vel)
1579 {
1580 return p->vel->velocityMove(off, v);
1581 }
1582
1583 return ReturnValue::return_code::return_value_error_generic;
1584}
1585
1587{
1589 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1590
1591 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(v,remappedControlBoards);
1592
1593 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1594 {
1595 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1596
1598 if (p->vel) {
1599 ok = p->vel->velocityMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1600 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1601 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1602 }
1603 else {
1604 ok = ReturnValue::return_code::return_value_error_generic;
1605 }
1606 ret = ret && ok;
1607 }
1608
1609 return ret;
1610}
1611
1612/* IEncoders */
1614{
1615 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1616 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1617
1618 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1619 if (!p)
1620 {
1621 return ReturnValue::return_code::return_value_error_generic;
1622 }
1623
1624 if (p->iJntEnc)
1625 {
1626 return p->iJntEnc->resetEncoder(off);
1627 }
1628
1629 return ReturnValue::return_code::return_value_error_generic;
1630}
1631
1633{
1635
1636 for(int l=0;l<controlledJoints;l++)
1637 {
1638 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1639 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1640
1641 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1642 if (!p)
1643 {
1644 return ReturnValue::return_code::return_value_error_generic;
1645 }
1646
1647 if (p->iJntEnc)
1648 {
1649 ReturnValue ok = p->iJntEnc->resetEncoder(off);
1650 ret = ret && ok;
1651 }
1652 else
1653 {
1654 ret=ReturnValue::return_code::return_value_error_generic;
1655 }
1656 }
1657 return ret;
1658}
1659
1661{
1662 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1663 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1664
1665 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1666
1667 if (!p)
1668 {
1669 return ReturnValue::return_code::return_value_error_generic;
1670 }
1671
1672 if (p->iJntEnc)
1673 {
1674 return p->iJntEnc->setEncoder(off,val);
1675 }
1676
1677 return ReturnValue::return_code::return_value_error_generic;
1678}
1679
1681{
1683
1684 for(int l=0;l<controlledJoints;l++)
1685 {
1686 int off = (int) remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1687 size_t subIndex = remappedControlBoards.lut[l].subControlBoardIndex;
1688
1689 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
1690
1691 if (!p)
1692 {
1693 return ReturnValue::return_code::return_value_error_generic;
1694 }
1695
1696 if (p->iJntEnc)
1697 {
1698 ReturnValue ok = p->iJntEnc->setEncoder(off, vals[l]);
1699 ret = ret && ok;
1700 }
1701 else
1702 {
1703 ret = ReturnValue::return_code::return_value_error_generic;
1704 }
1705 }
1706 return ret;
1707}
1708
1710{
1711 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1712 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1713
1714 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1715
1716 if (!p)
1717 {
1718 return ReturnValue::return_code::return_value_error_generic;
1719 }
1720
1721 if (p->iJntEnc)
1722 {
1723 return p->iJntEnc->getEncoder(off, v);
1724 }
1725
1726 return ReturnValue::return_code::return_value_error_generic;
1727}
1728
1730{
1732
1733 for(int l=0;l<controlledJoints;l++)
1734 {
1735 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1736 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1737
1738 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1739
1740 if (!p)
1741 {
1742 return ReturnValue::return_code::return_value_error_generic;
1743 }
1744
1745 if (p->iJntEnc)
1746 {
1747 ReturnValue ok = p->iJntEnc->getEncoder(off, encs+l);
1748 ret = ret && ok;
1749 }
1750 else
1751 {
1752 ret = ReturnValue::return_code::return_value_error_generic;
1753 }
1754 }
1755 return ret;
1756}
1757
1759{
1761
1762 for(int l=0;l<controlledJoints;l++)
1763 {
1764 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1765 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1766
1767 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1768
1769 if (!p)
1770 {
1771 return ReturnValue::return_code::return_value_error_generic;
1772 }
1773
1774 if (p->iJntEnc)
1775 {
1776 ReturnValue ok = p->iJntEnc->getEncoderTimed(off, encs+l, t+l);
1777 ret = ret && ok;
1778 }
1779 else
1780 {
1781 ret = ReturnValue::return_code::return_value_error_generic;
1782 }
1783 }
1784 return ret;
1785}
1786
1788{
1789 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1790 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1791
1792 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1793
1794 if (!p)
1795 {
1796 return ReturnValue::return_code::return_value_error_generic;
1797 }
1798
1799 if (p->iJntEnc)
1800 {
1801 return p->iJntEnc->getEncoderTimed(off, v, t);
1802 }
1803
1804 return ReturnValue::return_code::return_value_error_generic;
1805}
1806
1808{
1809 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1810 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1811
1812 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1813
1814 if (!p)
1815 {
1816 return ReturnValue::return_code::return_value_error_generic;
1817 }
1818
1819 if (p->iJntEnc)
1820 {
1821 return p->iJntEnc->getEncoderSpeed(off, sp);
1822 }
1823
1824 return ReturnValue::return_code::return_value_error_generic;
1825}
1826
1828{
1830
1831 for(int l=0;l<controlledJoints;l++)
1832 {
1833 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1834 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1835
1836 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1837
1838 if (!p)
1839 {
1840 return ReturnValue::return_code::return_value_error_generic;
1841 }
1842
1843 if (p->iJntEnc)
1844 {
1845 ReturnValue ok = p->iJntEnc->getEncoderSpeed(off, spds+l);
1846 ret = ret && ok;
1847 }
1848 else
1849 {
1850 ret = ReturnValue::return_code::return_value_error_generic;
1851 }
1852 }
1853 return ret;
1854}
1855
1857{
1858 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1859 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1860
1861 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1862
1863 if (!p)
1864 {
1865 return ReturnValue::return_code::return_value_error_generic;
1866 }
1867
1868 if (p->iJntEnc)
1869 {
1870 return p->iJntEnc->getEncoderAcceleration(off,acc);
1871 }
1872
1873 return ReturnValue::return_code::return_value_error_generic;
1874}
1875
1877{
1879
1880 for(int l=0;l<controlledJoints;l++)
1881 {
1882 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1883 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1884
1885 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1886
1887 if (!p)
1888 {
1889 return ReturnValue::return_code::return_value_error_generic;
1890 }
1891
1892 if (p->iJntEnc)
1893 {
1894 ReturnValue ok = p->iJntEnc->getEncoderAcceleration(off, accs+l);
1895 ret = ret && ok;
1896 }
1897 else
1898 {
1899 ret = ReturnValue::return_code::return_value_error_generic;
1900 }
1901 }
1902 return ret;
1903}
1904
1905/* IMotor */
1907{
1908 *num=controlledJoints;
1909 return ReturnValue_ok;
1910}
1911
1913{
1914 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
1915 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
1916
1917 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1918
1919 if (!p)
1920 {
1921 return ReturnValue::return_code::return_value_error_generic;
1922 }
1923
1924 if (p->imotor)
1925 {
1926 return p->imotor->getTemperature(off, val);
1927 }
1928
1929 return ReturnValue::return_code::return_value_error_generic;
1930}
1931
1933{
1935 for(int l=0;l<controlledJoints;l++)
1936 {
1937 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1938 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1939
1940 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1941 if (!p)
1942 {
1943 return ReturnValue::return_code::return_value_error_generic;
1944 }
1945
1946 if (p->imotor)
1947 {
1948 ret=ret&&p->imotor->getTemperature(off, vals+l);
1949 }
1950 else
1951 {
1952 ret=ReturnValue::return_code::return_value_error_generic;
1953 }
1954 }
1955 return ret;
1956}
1957
1959{
1960 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
1961 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
1962
1963 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1964
1965 if (!p)
1966 {
1967 return ReturnValue::return_code::return_value_error_generic;
1968 }
1969
1970 if (p->imotor)
1971 {
1972 return p->imotor->getTemperatureLimit(off, val);
1973 }
1974
1975 return ReturnValue::return_code::return_value_error_generic;
1976}
1977
1979{
1980 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
1981 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
1982
1983 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1984
1985 if (!p)
1986 {
1987 return ReturnValue::return_code::return_value_error_generic;
1988 }
1989
1990 if (p->imotor)
1991 {
1992 return p->imotor->setTemperatureLimit(off,val);
1993 }
1994
1995 return ReturnValue::return_code::return_value_error_generic;
1996}
1997
1999{
2000 int off = (int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2001 size_t subIndex = remappedControlBoards.lut[m].subControlBoardIndex;
2002
2003 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2004
2005 if (!p)
2006 {
2007 return ReturnValue::return_code::return_value_error_generic;
2008 }
2009
2010 if (p->imotor)
2011 {
2012 return p->imotor->getGearboxRatio(off, val);
2013 }
2014
2015 return ReturnValue::return_code::return_value_error_generic;
2016}
2017
2019{
2020 int off = (int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2021 size_t subIndex = remappedControlBoards.lut[m].subControlBoardIndex;
2022
2023 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2024
2025 if (!p)
2026 {
2027 return ReturnValue::return_code::return_value_error_generic;
2028 }
2029
2030 if (p->imotor)
2031 {
2032 return p->imotor->setGearboxRatio(off, val);
2033 }
2034
2035 return ReturnValue::return_code::return_value_error_generic;
2036}
2037
2038/* IRemoteVariables */
2040{
2041 yCWarning(CONTROLBOARDREMAPPER, "getRemoteVariable is not properly implemented, use at your own risk.");
2042
2044
2045 for (unsigned int i = 0; i < remappedControlBoards.getNrOfSubControlBoards(); i++)
2046 {
2047 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(i);
2048
2049 if (!p)
2050 {
2051 return ReturnValue::return_code::return_value_error_generic;
2052 }
2053
2054 if (!p->iVar)
2055 {
2056 return ReturnValue::return_code::return_value_error_generic;
2057 }
2058
2061
2062 if (ok)
2063 {
2064 val.append(tmpval);
2065 }
2066
2067 b = b && ok;
2068 }
2069
2070 return b;
2071}
2072
2074{
2075 yCWarning(CONTROLBOARDREMAPPER, "setRemoteVariable is not properly implemented, use at your own risk.");
2076
2077 size_t bottle_size = val.size();
2078 size_t device_size = remappedControlBoards.getNrOfSubControlBoards();
2079 if (bottle_size != device_size)
2080 {
2081 yCError(CONTROLBOARDREMAPPER, "ControlBoardRemappingEngine::setRemoteVariable bottle_size != device_size failure");
2082 return ReturnValue::return_code::return_value_error_generic;
2083 }
2084
2086 for (unsigned int i = 0; i < device_size; i++)
2087 {
2088 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(i);
2089 if (!p)
2090 {
2091 yCError(CONTROLBOARDREMAPPER, "ControlBoardRemappingEngine::setRemoteVariable !p failure");
2092 ReturnValue::return_code::return_value_error_generic;
2093 }
2094 if (!p->iVar)
2095 {
2096 yCError(CONTROLBOARDREMAPPER, "ControlBoardRemappingEngine::setRemoteVariable !p->iVar failure");
2097 ReturnValue::return_code::return_value_error_generic;
2098 }
2099 Bottle* partial_val = val.get(i).asList();
2100 if (partial_val)
2101 {
2102 b &= p->iVar->setRemoteVariable(key, *partial_val);
2103 }
2104 else
2105 {
2106 yCError(CONTROLBOARDREMAPPER, "ControlBoardRemappingEngine::setRemoteVariable general failure");
2107 return ReturnValue::return_code::return_value_error_generic;
2108 }
2109 }
2110
2111 return b;
2112}
2113
2115{
2116 yCWarning(CONTROLBOARDREMAPPER, "getRemoteVariablesList is not properly implemented, use at your own risk.");
2117
2118 size_t subIndex = remappedControlBoards.lut[0].subControlBoardIndex;
2119 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2120
2121 if (!p)
2122 {
2123 return ReturnValue::return_code::return_value_error_generic;
2124 }
2125
2126 if (p->iVar)
2127 {
2128 return p->iVar->getRemoteVariablesList(listOfKeys);
2129 }
2130
2131 return ReturnValue::return_code::return_value_error_generic;
2132}
2133
2134/* IMotorEncoders */
2135
2137{
2138 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2139 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2140
2141 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2142
2143 if (!p)
2144 {
2145 return ReturnValue::return_code::return_value_error_generic;
2146 }
2147
2148 if (p->iMotEnc)
2149 {
2150 return p->iMotEnc->resetMotorEncoder(off);
2151 }
2152
2153 return ReturnValue::return_code::return_value_error_generic;
2154}
2155
2157{
2159
2160 for(int l=0;l<controlledJoints;l++)
2161 {
2162 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2163 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2164
2165 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2166 if (!p)
2167 {
2168 return ReturnValue::return_code::return_value_error_generic;
2169 }
2170
2171 if (p->iMotEnc)
2172 {
2174 ret= ret && ok;
2175 }
2176 else
2177 {
2178 ret=ReturnValue::return_code::return_value_error_generic;
2179 }
2180 }
2181
2182 return ret;
2183}
2184
2186{
2187 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2188 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2189
2190 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2191
2192 if (!p)
2193 {
2194 return ReturnValue::return_code::return_value_error_generic;
2195 }
2196
2197 if (p->iMotEnc)
2198 {
2199 return p->iMotEnc->setMotorEncoder(off,val);
2200 }
2201
2202 return ReturnValue::return_code::return_value_error_generic;
2203}
2204
2206{
2208
2209 for(int l=0;l<controlledJoints;l++)
2210 {
2211 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2212 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2213
2214 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2215
2216 if (!p)
2217 {
2218 return ReturnValue::return_code::return_value_error_generic;
2219 }
2220
2221 if (p->iMotEnc)
2222 {
2223 ReturnValue ok = p->iMotEnc->setMotorEncoder(off, vals[l]);
2224 ret = ret && ok;
2225 }
2226 else
2227 {
2228 ret=ReturnValue::return_code::return_value_error_generic;
2229 }
2230 }
2231
2232 return ret;
2233}
2234
2236{
2237 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2238 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2239
2240 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2241
2242 if (!p)
2243 {
2244 return ReturnValue::return_code::return_value_error_generic;
2245 }
2246
2247 if (p->iMotEnc)
2248 {
2249 return p->iMotEnc->setMotorEncoderCountsPerRevolution(off,cpr);
2250 }
2251
2252 return ReturnValue::return_code::return_value_error_generic;
2253}
2254
2256{
2257 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2258 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2259
2260 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2261
2262 if (!p)
2263 {
2264 return ReturnValue::return_code::return_value_error_generic;
2265 }
2266
2267 if (p->iMotEnc)
2268 {
2269 return p->iMotEnc->getMotorEncoderCountsPerRevolution(off, cpr);
2270 }
2271
2272 return ReturnValue::return_code::return_value_error_generic;
2273}
2274
2276{
2277 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2278 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2279
2280 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2281
2282 if (!p)
2283 {
2284 return ReturnValue::return_code::return_value_error_generic;
2285 }
2286
2287 if (p->iMotEnc)
2288 {
2289 return p->iMotEnc->getMotorEncoder(off, v);
2290 }
2291
2292 return ReturnValue::return_code::return_value_error_generic;
2293}
2294
2296{
2298
2299 for(int l=0;l<controlledJoints;l++)
2300 {
2301 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2302 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2303
2304 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2305 if (!p) {
2306 return ReturnValue::return_code::return_value_error_generic;
2307 }
2308
2309 if (p->iMotEnc)
2310 {
2311 ret=ret&&p->iMotEnc->getMotorEncoder(off, encs+l);
2312 } else {
2313 ret = ReturnValue::return_code::return_value_error_generic;
2314 }
2315 }
2316 return ret;
2317}
2318
2320{
2322
2323 for(int l=0;l<controlledJoints;l++)
2324 {
2325 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2326 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2327
2328 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2329
2330 if (!p)
2331 {
2332 return ReturnValue::return_code::return_value_error_generic;
2333 }
2334
2335 if (p->iMotEnc)
2336 {
2337 ReturnValue ok = p->iMotEnc->getMotorEncoderTimed(off, encs, t);
2338 ret = ret && ok;
2339 }
2340 else
2341 {
2342 ret = ReturnValue::return_code::return_value_error_generic;
2343 }
2344 }
2345 return ret;
2346}
2347
2349{
2350 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2351 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2352
2353 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2354
2355 if (!p)
2356 {
2357 return ReturnValue::return_code::return_value_error_generic;
2358 }
2359
2360 if (p->iMotEnc)
2361 {
2362 return p->iMotEnc->getMotorEncoderTimed(off, v, t);
2363 }
2364
2365 return ReturnValue::return_code::return_value_error_generic;
2366}
2367
2369{
2370 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2371 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2372
2373 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2374
2375 if (!p)
2376 {
2377 return ReturnValue::return_code::return_value_error_generic;
2378 }
2379
2380 if (p->iMotEnc)
2381 {
2382 return p->iMotEnc->getMotorEncoderSpeed(off, sp);
2383 }
2384
2385 return ReturnValue::return_code::return_value_error_generic;
2386}
2387
2389{
2391
2392 for(int l=0;l<controlledJoints;l++)
2393 {
2394 int off = (int) remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2395 size_t subIndex = remappedControlBoards.lut[l].subControlBoardIndex;
2396
2397 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2398
2399 if (!p)
2400 {
2401 return ReturnValue::return_code::return_value_error_generic;
2402 }
2403
2404 if (p->iMotEnc)
2405 {
2406 ReturnValue ok = p->iMotEnc->getMotorEncoderSpeed(off, spds + l);
2407 ret = ret && ok;
2408 }
2409 else
2410 {
2411 ret = ReturnValue::return_code::return_value_error_generic;
2412 }
2413 }
2414 return ret;
2415}
2416
2418{
2419 int off = (int) remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2420 size_t subIndex = remappedControlBoards.lut[m].subControlBoardIndex;
2421
2422 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2423
2424 if (!p)
2425 {
2426 return ReturnValue::return_code::return_value_error_generic;
2427 }
2428
2429 if (p->iMotEnc)
2430 {
2431 return p->iMotEnc->getMotorEncoderAcceleration(off,acc);
2432 }
2433 *acc=0.0;
2434 return ReturnValue::return_code::return_value_error_generic;
2435}
2436
2438{
2440
2441 for(int l=0;l<controlledJoints;l++)
2442 {
2443 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2444 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2445
2446 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2447
2448 if (!p)
2449 {
2450 return ReturnValue::return_code::return_value_error_generic;
2451 }
2452
2453 if (p->iMotEnc)
2454 {
2456 ret=ret && ok;
2457 }
2458 else
2459 {
2460 ret=ReturnValue::return_code::return_value_error_generic;
2461 }
2462 }
2463
2464 return ret;
2465}
2466
2467
2469{
2470 *num=controlledJoints;
2471 return ReturnValue_ok;
2472}
2473
2474/* IAmplifierControl */
2476{
2477 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2478 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2479
2480 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2481
2482 if (!p)
2483 {
2484 return ReturnValue::return_code::return_value_error_generic;
2485 }
2486
2487 if (p->amp)
2488 {
2489 return p->amp->enableAmp(off);
2490 }
2491
2492 return ReturnValue::return_code::return_value_error_generic;
2493}
2494
2499
2501{
2503
2504 for(int l=0;l<controlledJoints;l++)
2505 {
2506 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2507 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2508
2509 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2510
2511 if (!p)
2512 {
2513 return ReturnValue::return_code::return_value_error_generic;
2514 }
2515
2516 if (p->amp)
2517 {
2518 ReturnValue ok = p->amp->getAmpStatus(off, st+l);
2519 ret = ret && ok;
2520 }
2521 else
2522 {
2523 ret=ReturnValue::return_code::return_value_error_generic;
2524 }
2525 }
2526
2527 return ret;
2528}
2529
2531{
2532 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2533 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2534
2535 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2536
2537 if (!p)
2538 {
2539 return ReturnValue::return_code::return_value_error_generic;
2540 }
2541
2542 if (p->amp)
2543 {
2544 return p->amp->getAmpStatus(off,v);
2545 }
2546
2547 return ReturnValue::return_code::return_value_error_generic;
2548}
2549
2551{
2552 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2553 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2554
2555 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2556
2557 if (!p)
2558 {
2559 return ReturnValue::return_code::return_value_error_generic;
2560 }
2561
2562 if (p->amp)
2563 {
2564 return p->amp->setMaxCurrent(off,v);
2565 }
2566
2567 return ReturnValue::return_code::return_value_error_generic;
2568}
2569
2571{
2572 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2573 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2574
2575 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2576
2577 if (!p)
2578 {
2579 return ReturnValue::return_code::return_value_error_generic;
2580 }
2581
2582 if (p->amp)
2583 {
2584 return p->amp->getMaxCurrent(off,v);
2585 }
2586
2587 return ReturnValue::return_code::return_value_error_generic;
2588}
2589
2591{
2592 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2593 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2594
2595 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2596
2597 if(!p)
2598 {
2599 return ReturnValue::return_code::return_value_error_generic;
2600 }
2601
2602 if(!p->amp)
2603 {
2604 return ReturnValue::return_code::return_value_error_generic;
2605 }
2606
2607 return p->amp->getNominalCurrent(off, val);
2608}
2609
2611{
2612 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2613 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2614
2615 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2616
2617 if(!p)
2618 {
2619 return ReturnValue::return_code::return_value_error_generic;
2620 }
2621
2622 if(!p->amp)
2623 {
2624 return ReturnValue::return_code::return_value_error_generic;
2625 }
2626
2627 return p->amp->getPeakCurrent(off, val);
2628}
2629
2631{
2632 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2633 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2634
2635 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2636
2637 if (!p)
2638 {
2639 return ReturnValue::return_code::return_value_error_generic;
2640 }
2641
2642 if (!p->amp)
2643 {
2644 return ReturnValue::return_code::return_value_error_generic;
2645 }
2646
2647 return p->amp->setPeakCurrent(off, val);
2648}
2649
2651{
2652 int off = (int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2653 size_t subIndex = remappedControlBoards.lut[m].subControlBoardIndex;
2654
2655 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2656
2657 if (!p)
2658 {
2659 return ReturnValue::return_code::return_value_error_generic;
2660 }
2661
2662 if (!p->amp)
2663 {
2664 return ReturnValue::return_code::return_value_error_generic;
2665 }
2666
2667 return p->amp->setNominalCurrent(off, val);
2668}
2669
2671{
2672 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2673 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2674
2675 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2676
2677 if(!p)
2678 {
2679 return ReturnValue::return_code::return_value_error_generic;
2680 }
2681
2682 if(!p->amp)
2683 {
2684 return ReturnValue::return_code::return_value_error_generic;
2685 }
2686
2687 return p->amp->getPWM(off, val);
2688}
2690{
2691 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2692 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2693
2694 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2695
2696 if(!p)
2697 {
2698 return ReturnValue::return_code::return_value_error_generic;
2699 }
2700
2701 if(!p->amp)
2702 {
2703 return ReturnValue::return_code::return_value_error_generic;
2704 }
2705
2706 return p->amp->getPWMLimit(off, val);
2707}
2708
2710{
2711 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2712 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2713
2714 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2715
2716 if (!p)
2717 {
2718 return ReturnValue::return_code::return_value_error_generic;
2719 }
2720
2721 if (!p->amp)
2722 {
2723 return ReturnValue::return_code::return_value_error_generic;
2724 }
2725
2726 return p->amp->setPWMLimit(off, val);
2727}
2728
2730{
2731 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2732 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2733
2734 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2735
2736 if(!p)
2737 {
2738 return ReturnValue::return_code::return_value_error_generic;
2739 }
2740
2741 if(!p->amp)
2742 {
2743 return ReturnValue::return_code::return_value_error_generic;
2744 }
2745
2746 return p->amp->getPowerSupplyVoltage(off, val);
2747}
2748
2749
2750/* IControlLimits */
2751
2753{
2754 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2755 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2756
2757 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2758
2759 if (!p)
2760 {
2761 return ReturnValue::return_code::return_value_error_generic;
2762 }
2763
2764 if (p->lim)
2765 {
2766 return p->lim->setPosLimits(off,min, max);
2767 }
2768
2769 return ReturnValue::return_code::return_value_error_generic;
2770}
2771
2773{
2774 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2775 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2776
2777 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2778
2779 if (!p)
2780 {
2781 return ReturnValue::return_code::return_value_error_generic;
2782 }
2783
2784 if (p->lim)
2785 {
2786 return p->lim->getPosLimits(off,min, max);
2787 }
2788
2789 return ReturnValue::return_code::return_value_error_generic;
2790}
2791
2793{
2794 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2795 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2796
2797 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2798
2799 if (!p)
2800 {
2801 return ReturnValue::return_code::return_value_error_generic;
2802 }
2803
2804 if (!p->lim)
2805 {
2806 return ReturnValue::return_code::return_value_error_generic;
2807 }
2808
2809 return p->lim->setVelLimits(off,min, max);
2810}
2811
2813{
2814 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2815 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2816
2817 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2818
2819 if (!p)
2820 {
2821 return ReturnValue::return_code::return_value_error_generic;
2822 }
2823
2824 if(!p->lim)
2825 {
2826 return ReturnValue::return_code::return_value_error_generic;
2827 }
2828
2829 return p->lim->getVelLimits(off,min, max);
2830}
2831
2832/* IRemoteCalibrator */
2837
2842
2844{
2845 if(!getCalibratorDevice())
2846 {
2847 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2848 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
2849
2850 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(subIndex);
2851 if (!s)
2852 {
2853 return ReturnValue::return_code::return_value_error_generic;
2854 }
2855
2856 if (s->remcalib)
2857 {
2858 return s->remcalib->calibrateSingleJoint(off);
2859 }
2860
2861 return ReturnValue::return_code::return_value_error_generic;
2862 }
2863 else
2864 {
2865 return IRemoteCalibrator::getCalibratorDevice()->calibrateSingleJoint(j);
2866 }
2867}
2868
2870{
2871 if(!getCalibratorDevice())
2872 {
2873 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
2874 {
2875 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(ctrlBrd);
2876 if (!s)
2877 {
2878 return ReturnValue::return_code::return_value_error_generic;
2879 }
2880
2881 if (s->remcalib)
2882 {
2883 return s->remcalib->calibrateWholePart();
2884 }
2885 }
2886
2887 return ReturnValue::return_code::return_value_error_generic;
2888 }
2889 else
2890 {
2891 return IRemoteCalibrator::getCalibratorDevice()->calibrateWholePart();
2892 }
2893}
2894
2896{
2897 if(!getCalibratorDevice())
2898 {
2899 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2900 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
2901
2902 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(subIndex);
2903 if (!s)
2904 {
2905 return ReturnValue::return_code::return_value_error_generic;
2906 }
2907
2908 if (s->remcalib)
2909 {
2910 return s->remcalib->homingSingleJoint(off);
2911 }
2912
2913 return ReturnValue::return_code::return_value_error_generic;
2914 }
2915 else
2916 {
2917 return IRemoteCalibrator::getCalibratorDevice()->homingSingleJoint(j);
2918 }
2919}
2920
2922{
2923 if(!getCalibratorDevice())
2924 {
2926 for(int l=0;l<controlledJoints;l++)
2927 {
2928 ReturnValue ok = this->homingSingleJoint(l);
2929 ret = ret && ok;
2930 }
2931
2932 return ret;
2933 }
2934 else
2935 {
2936 return IRemoteCalibrator::getCalibratorDevice()->homingWholePart();
2937 }
2938}
2939
2941{
2942 if(!getCalibratorDevice())
2943 {
2944 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2945 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
2946
2947 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(subIndex);
2948 if (!s)
2949 {
2950 return ReturnValue::return_code::return_value_error_generic;
2951 }
2952
2953 if (s->remcalib)
2954 {
2955 return s->remcalib->parkSingleJoint(off,_wait);
2956 }
2957
2958 return ReturnValue::return_code::return_value_error_generic;
2959 }
2960 else
2961 {
2963 }
2964}
2965
2967{
2968 if(!getCalibratorDevice())
2969 {
2971
2972 for(int l=0; l<controlledJoints; l++)
2973 {
2974 bool _wait = false;
2975 ReturnValue ok = this->parkSingleJoint(l,_wait);
2976 ret = ret && ok;
2977 }
2978
2979 return ret;
2980 }
2981 else
2982 {
2984 }
2985}
2986
2988{
2989 if(!getCalibratorDevice())
2990 {
2991 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
2992 {
2993 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(ctrlBrd);
2994 if (!s)
2995 {
2996 return ReturnValue::return_code::return_value_error_generic;
2997 }
2998
2999 if (s->remcalib)
3000 {
3001 return s->remcalib->quitCalibrate();
3002 }
3003 }
3004
3005 return ReturnValue::return_code::return_value_error_generic;
3006 }
3007 else
3008 {
3009 return IRemoteCalibrator::getCalibratorDevice()->quitCalibrate();
3010 }
3011}
3012
3014{
3015 if(!getCalibratorDevice())
3016 {
3017 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3018 {
3019 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(ctrlBrd);
3020 if (!s)
3021 {
3022 return ReturnValue::return_code::return_value_error_generic;
3023 }
3024
3025 if (s->remcalib)
3026 {
3027 return s->remcalib->quitPark();
3028 }
3029 }
3030
3031 return ReturnValue::return_code::return_value_error_generic;
3032 }
3033 else
3034 {
3035 return IRemoteCalibrator::getCalibratorDevice()->quitPark();
3036 }
3037}
3038
3039
3040/* IControlCalibration */
3041ReturnValue ControlBoardRemappingEngine::calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3)
3042{
3043 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3044 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3045
3046 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
3047
3048 if (p && p->calib)
3049 {
3050 return p->calib->calibrateAxisWithParams(off, ui,v1,v2,v3);
3051 }
3052 return ReturnValue::return_code::return_value_error_generic;
3053}
3054
3056{
3057 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3058 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
3059
3060 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
3061
3062 if (p && p->calib)
3063 {
3064 return p->calib->setCalibrationParameters(off, params);
3065 }
3066
3067 return ReturnValue::return_code::return_value_error_generic;
3068}
3069
3071{
3072 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3073 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3074
3075 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3076
3077 if (!p)
3078 {
3079 return ReturnValue::return_code::return_value_error_generic;
3080 }
3081
3082 if (p->calib)
3083 {
3084 return p->calib->calibrationDone(off);
3085 }
3086
3087 return ReturnValue::return_code::return_value_error_generic;
3088}
3089
3091{
3092 yCError(CONTROLBOARDREMAPPER, "Calling abortPark -- not implemented");
3093 return ReturnValue::return_code::return_value_error_generic;
3094}
3095
3097{
3098 yCError(CONTROLBOARDREMAPPER, "Calling abortCalibration -- not implemented");
3099 return ReturnValue::return_code::return_value_error_generic;
3100}
3101
3102/* IAxisInfo */
3103
3105{
3106 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3107 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3108
3109 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3110
3111 if (!p)
3112 {
3113 return ReturnValue::return_code::return_value_error_generic;
3114 }
3115
3116 if (p->info)
3117 {
3118 return p->info->getAxisName(off, name);
3119 }
3120
3121 return ReturnValue::return_code::return_value_error_generic;
3122}
3123
3125{
3126 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3127 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
3128
3129 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
3130
3131 if (!p)
3132 {
3133 return ReturnValue::return_code::return_value_error_generic;
3134 }
3135
3136 if (p->info)
3137 {
3138 return p->info->getJointType(off, type);
3139 }
3140
3141 return ReturnValue::return_code::return_value_error_generic;
3142}
3143
3145{
3147
3148 for(int l=0;l<controlledJoints;l++)
3149 {
3150 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
3151 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
3152
3153 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3154
3155 if (!p)
3156 {
3157 return ReturnValue::return_code::return_value_error_generic;
3158 }
3159
3160 if (p->iTorque)
3161 {
3162 ReturnValue ok = p->iTorque->getRefTorque(off, refs+l);
3163 ret = ret && ok;
3164 }
3165 else
3166 {
3167 ret = ReturnValue::return_code::return_value_error_generic;
3168 }
3169 }
3170 return ret;
3171}
3172
3174{
3175 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3176 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3177
3178 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3179
3180 if (!p)
3181 {
3182 return ReturnValue::return_code::return_value_error_generic;
3183 }
3184
3185 if (p->iTorque)
3186 {
3187 return p->iTorque->getRefTorque(off, t);
3188 }
3189 return ReturnValue::return_code::return_value_error_generic;
3190}
3191
3193{
3195 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
3196
3197 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(t,remappedControlBoards);
3198
3199 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3200 {
3201 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3202
3203 ReturnValue ok;
3204
3205 if( p->iTorque )
3206 {
3207 ok = p->iTorque->setRefTorques(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3208 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3209 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3210 }
3211 else
3212 {
3213 ok = ReturnValue::return_code::return_value_error_generic;
3214 }
3215
3216 ret = ret && ok;
3217 }
3218
3219 return ret;
3220}
3221
3223{
3224 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3225 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
3226
3227 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
3228 if (!p)
3229 {
3230 return ReturnValue::return_code::return_value_error_generic;
3231 }
3232
3233 if (p->iTorque)
3234 {
3235 return p->iTorque->setRefTorque(off, t);
3236 }
3237 return ReturnValue::return_code::return_value_error_generic;
3238}
3239
3240ReturnValue ControlBoardRemappingEngine::setRefTorques(const int n_joints, const int *joints, const double *t)
3241{
3243 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
3244
3245 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(t,n_joints,joints,remappedControlBoards);
3246
3247 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3248 {
3249 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3250
3251 ReturnValue ok = p->iTorque->setRefTorques(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3252 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3253 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3254 ret = ret && ok;
3255 }
3256
3257 return ret;
3258}
3259
3261{
3262 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3263 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3264
3265 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3266
3267 if (!p)
3268 {
3269 return ReturnValue::return_code::return_value_error_generic;
3270 }
3271
3272 if (p->iTorque)
3273 {
3274 return p->iTorque->getMotorTorqueParams(off, params);
3275 }
3276
3277 return ReturnValue::return_code::return_value_error_generic;
3278}
3279
3281{
3282 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3283 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3284
3285 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3286
3287 if (!p)
3288 {
3289 return ReturnValue::return_code::return_value_error_generic;
3290 }
3291
3292 if (p->iTorque)
3293 {
3294 return p->iTorque->setMotorTorqueParams(off, params);
3295 }
3296
3297 return ReturnValue::return_code::return_value_error_generic;
3298}
3299
3301{
3302 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3303 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3304
3305 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3306
3307 if (!p)
3308 {
3309 return ReturnValue::return_code::return_value_error_generic;
3310 }
3311
3312 if (p->iImpedance)
3313 {
3314 return p->iImpedance->setImpedance(off, stiff, damp);
3315 }
3316
3317 return ReturnValue::return_code::return_value_error_generic;
3318}
3319
3321{
3322 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3323 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3324
3325 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3326
3327 if (!p)
3328 {
3329 return ReturnValue::return_code::return_value_error_generic;
3330 }
3331
3332 if (p->iImpedance)
3333 {
3334 return p->iImpedance->setImpedanceOffset(off, offset);
3335 }
3336
3337 return ReturnValue::return_code::return_value_error_generic;
3338}
3339
3341{
3342 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3343 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3344
3345 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3346
3347 if (!p)
3348 {
3349 return ReturnValue::return_code::return_value_error_generic;
3350 }
3351
3352 if (p->iTorque)
3353 {
3354 return p->iTorque->getTorque(off, t);
3355 }
3356
3357 return ReturnValue::return_code::return_value_error_generic;
3358}
3359
3361{
3363
3364 for(int l=0;l<controlledJoints;l++)
3365 {
3366 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
3367 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
3368
3369 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3370
3371 if (!p)
3372 {
3373 return ReturnValue::return_code::return_value_error_generic;
3374 }
3375
3376 if (p->iTorque)
3377 {
3378 ReturnValue ok = p->iTorque->getTorque(off, t+l);
3379 ret = ret && ok;
3380 }
3381 else
3382 {
3383 ret=ReturnValue::return_code::return_value_error_generic;
3384 }
3385 }
3386
3387 return ret;
3388 }
3389
3391{
3392 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3393 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3394
3395 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3396
3397 if (!p)
3398 {
3399 return ReturnValue::return_code::return_value_error_generic;
3400 }
3401
3402 if (p->iTorque)
3403 {
3404 return p->iTorque->getTorqueRange(off, min, max);
3405 }
3406
3407 return ReturnValue::return_code::return_value_error_generic;
3408}
3409
3411{
3413
3414 for(int l=0;l<controlledJoints;l++)
3415 {
3416 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
3417 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
3418
3419 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3420
3421 if (!p)
3422 {
3423 return ReturnValue::return_code::return_value_error_generic;
3424 }
3425
3426 if (p->iTorque)
3427 {
3428 ReturnValue ok = p->iTorque->getTorqueRange(off, min+l, max+l);
3429 ret = ret && ok;
3430 }
3431 else
3432 {
3433 ret=ReturnValue::return_code::return_value_error_generic;
3434 }
3435 }
3436 return ret;
3437 }
3438
3440{
3441 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3442 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3443
3444 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3445
3446 if (!p)
3447 {
3448 return ReturnValue::return_code::return_value_error_generic;
3449 }
3450
3451 if (p->iImpedance)
3452 {
3453 return p->iImpedance->getImpedance(off, stiff, damp);
3454 }
3455
3456 return ReturnValue::return_code::return_value_error_generic;
3457}
3458
3460{
3461 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3462 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3463
3464 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3465
3466 if (!p)
3467 {
3468 return ReturnValue::return_code::return_value_error_generic;
3469 }
3470
3471 if (p->iImpedance)
3472 {
3473 return p->iImpedance->getImpedanceOffset(off, offset);
3474 }
3475
3476 return ReturnValue::return_code::return_value_error_generic;
3477}
3478
3479ReturnValue ControlBoardRemappingEngine::getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
3480{
3481 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3482 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3483
3484 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3485
3486 if (!p)
3487 {
3488 return ReturnValue::return_code::return_value_error_generic;
3489 }
3490
3491 if (p->iImpedance)
3492 {
3493 return p->iImpedance->getCurrentImpedanceLimit(off, min_stiff, max_stiff, min_damp, max_damp);
3494 }
3495
3496 return ReturnValue::return_code::return_value_error_generic;
3497}
3498
3499ReturnValue ControlBoardRemappingEngine::getAvailableControlModes(int j, std::vector<yarp::dev::SelectableControlModeEnum>& avail)
3500{
3501 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3502 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3503
3504 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3505 if (!p) {
3506 return ReturnValue::return_code::return_value_error_generic;
3507 }
3508
3510
3511 if (p->iMode) {
3512 ok = p->iMode->getAvailableControlModes(off, avail);
3513 }
3514 else {
3515 ok = ReturnValue::return_code::return_value_error_generic;
3516 }
3517
3518 return ok;
3519}
3520
3522{
3523 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3524 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3525
3526 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3527 if (!p) {
3528 return ReturnValue::return_code::return_value_error_generic;
3529 }
3530
3532
3533 if (p->iMode) {
3534 ok = p->iMode->getControlMode(off, mode);
3535 }
3536 else {
3537 ok = ReturnValue::return_code::return_value_error_generic;
3538 }
3539
3540 return ok;
3541
3542}
3543
3544ReturnValue ControlBoardRemappingEngine::getControlModes(std::vector<yarp::dev::ControlModeEnum>& modes)
3545{
3547 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
3548
3549 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3550 {
3551 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3552
3553 ReturnValue ok;
3554
3555 if( p->iMode )
3556 {
3557 ok = p->iMode->getControlModes( allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
3559 }
3560 else
3561 {
3562 ok = ReturnValue::return_code::return_value_error_generic;
3563 }
3564
3565 ret = ret && ok;
3566 }
3567
3568 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(modes.data(),remappedControlBoards);
3569
3570 return ret;
3571}
3572
3573// IControlMode interface
3574ReturnValue ControlBoardRemappingEngine::getControlModes(const std::vector<int>& joints, std::vector<yarp::dev::ControlModeEnum>& modes)
3575{
3577 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
3578
3579 // Resize the input buffers
3580 selectedJointsBuffers.resizeSubControlBoardBuffers(joints.size(),joints.data(),remappedControlBoards);
3581
3582 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3583 {
3584 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3585
3586 ReturnValue ok;
3587
3588 if( p->iMode )
3589 {
3590 ok = p->iMode->getControlModes( selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
3592 }
3593 else
3594 {
3595 ok = ReturnValue::return_code::return_value_error_generic;
3596 }
3597
3598 ret = ret && ok;
3599 }
3600
3601 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(modes.data(),joints.size(),joints.data(),remappedControlBoards);
3602
3603 return ret;
3604}
3605
3607{
3609
3610 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3611 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3612
3613 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3614
3615 if (!p)
3616 {
3617 return ReturnValue::return_code::return_value_error_generic;
3618 }
3619
3620 if (p->iMode)
3621 {
3622 ret = p->iMode->setControlMode(off, mode);
3623 }
3624 else
3625 {
3626 ret = ReturnValue::return_code::return_value_error_generic;
3627 }
3628
3629 return ret;
3630}
3631
3632ReturnValue ControlBoardRemappingEngine::setControlModes(const std::vector<int>& joints, const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
3633{
3635 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
3636
3637 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(modes.data(),joints.size(),joints.data(),remappedControlBoards);
3638
3639 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3640 {
3641 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3642
3644 if (p->iMode) {
3645 ok = p->iMode->setControlModes( selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
3647 }
3648 else {
3649 ok = ReturnValue::return_code::return_value_error_generic;
3650 }
3651 ret = ret && ok;
3652 }
3653
3654 return ret;
3655}
3656
3657ReturnValue ControlBoardRemappingEngine::setControlModes(const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
3658{
3660 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
3661
3662 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(modes.data(),remappedControlBoards);
3663
3664 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3665 {
3666 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3667
3669 if (p->iMode) {
3670 ok = p->iMode->setControlModes(allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
3672 }
3673 else {
3674 ok = ReturnValue::return_code::return_value_error_generic;
3675 }
3676 ret = ret && ok;
3677 }
3678
3679 return ret;
3680}
3681
3683{
3684 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3685 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3686
3687 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3688
3689 if (!p)
3690 {
3691 return ReturnValue::return_code::return_value_error_generic;
3692 }
3693
3694 if (p->posDir)
3695 {
3696 return p->posDir->setPosition(off, ref);
3697 }
3698
3699 return ReturnValue::return_code::return_value_error_generic;
3700}
3701
3702ReturnValue ControlBoardRemappingEngine::setPositions(const int n_joints, const int *joints, const double *dpos)
3703{
3705 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
3706
3707 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(dpos,n_joints,joints,remappedControlBoards);
3708
3709 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3710 {
3711 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3712
3714 if (p->posDir) {
3715 ok = p->posDir->setPositions(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3716 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3717 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3718 }
3719 else {
3720 ok = ReturnValue::return_code::return_value_error_generic;
3721 }
3722 ret = ret && ok;
3723 }
3724
3725 return ret;
3726}
3727
3729{
3731 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
3732
3733 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(refs,remappedControlBoards);
3734
3735 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3736 {
3737 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3738
3740 if (p->posDir) {
3741 ok = p->posDir->setPositions(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3742 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3743 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3744 }
3745 else {
3746 ok = ReturnValue::return_code::return_value_error_generic;
3747 }
3748 ret = ret && ok;
3749 }
3750
3751 return ret;
3752}
3753
3755{
3756 double averageTimestamp = 0.0;
3757 int collectedTimestamps = 0;
3758
3759 for(int l=0;l<controlledJoints;l++)
3760 {
3761 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
3762
3763 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3764
3765 if (!p)
3766 {
3767 return Stamp();
3768 }
3769
3770 if(p->iTimed)
3771 {
3774 }
3775 }
3776
3777
3778 std::lock_guard<std::mutex> lock(buffers.mutex);
3779
3780 if( collectedTimestamps > 0 )
3781 {
3783 }
3784 else
3785 {
3786 buffers.stamp.update();
3787 }
3788
3789 return buffers.stamp;
3790}
3791
3793{
3794 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3795 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3796
3797 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3798
3799 if (!p)
3800 {
3801 return ReturnValue::return_code::return_value_error_generic;
3802 }
3803
3804 if (p->posDir)
3805 {
3806 ReturnValue ret = p->posDir->getRefPosition(off, ref);
3807 return ret;
3808 }
3809
3810 return ReturnValue::return_code::return_value_error_generic;
3811}
3812
3814{
3816 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
3817
3818 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3819 {
3820 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3821
3823
3824 if( p->posDir )
3825 {
3826 ok = p->posDir->getRefPositions(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3827 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3828 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3829 }
3830 else
3831 {
3832 ok = ReturnValue::return_code::return_value_error_generic;
3833 }
3834
3835 ret = ret && ok;
3836 }
3837
3838 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(spds,remappedControlBoards);
3839
3840 return ret;
3841}
3842
3843
3844ReturnValue ControlBoardRemappingEngine::getRefPositions(const int n_joints, const int *joints, double *targets)
3845{
3847 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
3848
3849 // Resize the input buffers
3850 selectedJointsBuffers.resizeSubControlBoardBuffers(n_joints,joints,remappedControlBoards);
3851
3852 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3853 {
3854 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3855
3857
3858 if( p->posDir )
3859 {
3860 ok = p->posDir->getRefPositions(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3861 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3862 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3863 }
3864 else
3865 {
3866 ok = ReturnValue::return_code::return_value_error_generic;
3867 }
3868
3869 ret = ret && ok;
3870 }
3871
3872 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(targets,n_joints,joints,remappedControlBoards);
3873
3874 return ret;
3875}
3876
3877
3878
3879// IVelocityControl interface
3880ReturnValue ControlBoardRemappingEngine::velocityMove(const int n_joints, const int *joints, const double *spds)
3881{
3883 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
3884
3885 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(spds,n_joints,joints,remappedControlBoards);
3886
3887 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3888 {
3889 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3890
3892 if (p->vel) {
3893 ok = p->vel->velocityMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3894 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3895 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3896 }
3897 else
3898 {
3899 ok = ReturnValue::return_code::return_value_error_generic;
3900 }
3901 ret = ret && ok;
3902 }
3903
3904 return ret;
3905}
3906
3908{
3909 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3910 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3911
3912 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3913
3914 if (!p)
3915 {
3916 return ReturnValue::return_code::return_value_error_generic;
3917 }
3918
3919 if (p->vel)
3920 {
3921 ReturnValue ret = p->vel->getTargetVelocity(off, vel);
3922 return ret;
3923 }
3924
3925 return ReturnValue::return_code::return_value_error_generic;
3926}
3927
3928
3930{
3932 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
3933
3934 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3935 {
3936 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3937
3939
3940 if( p->vel )
3941 {
3942 ok = p->vel->getTargetVelocities(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3943 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3944 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3945 }
3946 else
3947 {
3948 ok = ReturnValue::return_code::return_value_error_generic;
3949 }
3950
3951 ret = ret && ok;
3952 }
3953
3954 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(vels,remappedControlBoards);
3955
3956 return ret;
3957}
3958
3959ReturnValue ControlBoardRemappingEngine::getTargetVelocities(const int n_joints, const int* joints, double* vels)
3960{
3962 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
3963
3964 // Resize the input buffers
3965 selectedJointsBuffers.resizeSubControlBoardBuffers(n_joints,joints,remappedControlBoards);
3966
3967 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3968 {
3969 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3970
3972
3973 if( p->vel )
3974 {
3975 ok = p->vel->getTargetVelocities(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3976 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3977 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3978 }
3979 else
3980 {
3981 ok = ReturnValue::return_code::return_value_error_generic;
3982 }
3983
3984 ret = ret && ok;
3985 }
3986
3987 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(vels,n_joints,joints,remappedControlBoards);
3988
3989 return ret;
3990}
3991
3993{
3994 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3995 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3996
3997 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
3998
3999 if (!s)
4000 {
4001 return ReturnValue::return_code::return_value_error_generic;
4002 }
4003
4004 if (s->iInteract)
4005 {
4006 return s->iInteract->getInteractionMode(off, mode);
4007 }
4008
4009 return ReturnValue::return_code::return_value_error_generic;
4010}
4011
4012ReturnValue ControlBoardRemappingEngine::getInteractionModes(const std::vector<int>& joints, std::vector<yarp::dev::InteractionModeEnum>& modes)
4013{
4015 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
4016
4017 // Resize the input buffers
4018 selectedJointsBuffers.resizeSubControlBoardBuffers(joints.size(),joints.data(),remappedControlBoards);
4019
4020 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4021 {
4022 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4023
4025
4026 if( p->iMode )
4027 {
4028 ok = p->iInteract->getInteractionModes(selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
4030 }
4031 else
4032 {
4033 ok = ReturnValue::return_code::return_value_error_generic;
4034 }
4035
4036 ret = ret && ok;
4037 }
4038
4039 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(modes.data(), joints.size(), joints.data(), remappedControlBoards);
4040
4041 return ret;
4042}
4043
4044ReturnValue ControlBoardRemappingEngine::getInteractionModes(std::vector<yarp::dev::InteractionModeEnum>& modes)
4045{
4047 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4048
4049 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4050 {
4051 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4052
4054
4055 if( p->iMode )
4056 {
4059 }
4060 else
4061 {
4062 ok = ReturnValue::return_code::return_value_error_generic;
4063 }
4064
4065 ret = ret && ok;
4066 }
4067
4068 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(modes.data(), remappedControlBoards);
4069
4070 return ret;
4071}
4072
4074{
4075 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4076 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
4077
4078 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
4079
4080 if (!s)
4081 {
4082 return ReturnValue::return_code::return_value_error_generic;
4083 }
4084
4085 if (s->iInteract)
4086 {
4087 return s->iInteract->setInteractionMode(off, mode);
4088 }
4089
4090 return ReturnValue::return_code::return_value_error_generic;
4091}
4092
4093ReturnValue ControlBoardRemappingEngine::setInteractionModes(const std::vector<int>& joints, const std::vector<yarp::dev::InteractionModeEnum>& modes)
4094{
4096 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
4097
4098 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(modes.data(), joints.size(), joints.data(), remappedControlBoards);
4099
4100 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4101 {
4102 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4103
4106 ret = ret && ok;
4107 }
4108
4109 return ret;
4110}
4111
4112ReturnValue ControlBoardRemappingEngine::setInteractionModes(const std::vector<yarp::dev::InteractionModeEnum>& modes)
4113{
4115 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4116
4117 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(modes.data(),remappedControlBoards);
4118
4119 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4120 {
4121 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4122
4124 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
4126 ret = ret && ok;
4127 }
4128
4129 return ret;
4130}
4131
4133{
4134 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4135
4136 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4137
4138 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4139
4140 if (p && p->iPwm)
4141 {
4142 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4143 ret = p->iPwm->setRefDutyCycle(off, ref);
4144 }
4145
4146 return ret;
4147}
4148
4150{
4152
4153 for(int l=0;l<controlledJoints;l++)
4154 {
4155 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4156 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4157
4158 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4159
4160 if (!p)
4161 {
4162 return ReturnValue::return_code::return_value_error_generic;
4163 }
4164
4165 if (p->iPwm)
4166 {
4167 ReturnValue ok = p->iPwm->setRefDutyCycle(off, refs[l]);
4168 ret = ret && ok;
4169 }
4170 else
4171 {
4172 ret=ReturnValue::return_code::return_value_error_generic;
4173 }
4174 }
4175
4176 return ret;
4177}
4178
4180{
4181 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;;
4182
4183 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4184
4185 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4186
4187 if (p && p->iPwm)
4188 {
4189 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4190 ret = p->iPwm->getRefDutyCycle(off, ref);
4191 }
4192
4193 return ret;
4194}
4195
4197{
4199
4200 for(int l=0;l<controlledJoints;l++)
4201 {
4202 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4203 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4204
4205 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4206
4207 if (!p)
4208 {
4209 return ReturnValue::return_code::return_value_error_generic;
4210 }
4211
4212 if (p->iPwm)
4213 {
4214 ReturnValue ok = p->iPwm->getRefDutyCycle(off, refs+l);
4215 ret = ret && ok;
4216 }
4217 else
4218 {
4219 ret=ReturnValue::return_code::return_value_error_generic;
4220 }
4221 }
4222
4223 return ret;
4224}
4225
4227{
4228 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4229
4230 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4231
4232 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4233
4234 if (p && p->iPwm)
4235 {
4236 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4237 ret = p->iPwm->getDutyCycle(off, val);
4238 }
4239
4240 return ret;
4241}
4242
4244{
4246
4247 for(int l=0;l<controlledJoints;l++)
4248 {
4249 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4250 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4251
4252 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4253
4254 if (!p)
4255 {
4256 return ReturnValue::return_code::return_value_error_generic;
4257 }
4258
4259 if (p->iPwm)
4260 {
4261 ReturnValue ok = p->iPwm->getDutyCycle(off, vals+l);
4262 ret = ret && ok;
4263 }
4264 else
4265 {
4266 ret=ReturnValue::return_code::return_value_error_generic;
4267 }
4268 }
4269
4270 return ret;
4271}
4272
4274{
4275 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4276
4277 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
4278
4279 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4280
4281 if (p && p->iCurr)
4282 {
4283 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4284 ret = p->iCurr->getCurrent(off, val);
4285 }
4286
4287 return ret;
4288}
4289
4291{
4293
4294 for(int l=0;l<controlledJoints;l++)
4295 {
4296 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4297 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4298
4299 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4300
4301 if (!p)
4302 {
4303 return ReturnValue::return_code::return_value_error_generic;
4304 }
4305
4306 if (p->iCurr)
4307 {
4308 ReturnValue ok = p->iCurr->getCurrent(off, vals+l);
4309 ret = ret && ok;
4310 }
4311 else
4312 {
4313 ret=ReturnValue::return_code::return_value_error_generic;
4314 }
4315 }
4316
4317 return ret;
4318}
4319
4321{
4322 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4323
4324 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4325
4326 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4327
4328 if (p && p->iCurr)
4329 {
4330 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4331 ret = p->iCurr->getCurrentRange(off, min, max);
4332 }
4333
4334 return ret;
4335}
4336
4338{
4340
4341 for(int l=0;l<controlledJoints;l++)
4342 {
4343 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4344 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4345
4346 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4347
4348 if (!p)
4349 {
4350 return ReturnValue::return_code::return_value_error_generic;
4351 }
4352
4353 if (p->iCurr)
4354 {
4355 ReturnValue ok = p->iCurr->getCurrentRange(off, min+l, max+l);
4356 ret = ret && ok;
4357 }
4358 else
4359 {
4360 ret=ReturnValue::return_code::return_value_error_generic;
4361 }
4362 }
4363
4364 return ret;
4365}
4366
4368{
4369 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4370
4371 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4372
4373 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4374
4375 if (p && p->iCurr)
4376 {
4377 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4378 ret = p->iCurr->setRefCurrent(off, curr);
4379 }
4380
4381 return ret;
4382}
4383
4385{
4387 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
4388
4389 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(currs,n_motor,motors,remappedControlBoards);
4390
4391 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4392 {
4393 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4394
4395 if (!(p && p->iCurr))
4396 {
4397 ret = ReturnValue::return_code::return_value_error_generic;
4398 break;
4399 }
4400
4401 ReturnValue ok = p->iCurr->setRefCurrents(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
4402 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
4403 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
4404 ret = ret && ok;
4405 }
4406
4407 return ret;
4408}
4409
4411{
4413 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4414
4415 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(currs,remappedControlBoards);
4416
4417 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4418 {
4419 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4420
4422 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
4423 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
4424 ret = ret && ok;
4425 }
4426
4427 return ret;
4428}
4429
4431{
4432 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4433
4434 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4435
4436 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4437
4438 if (p && p->iCurr)
4439 {
4440 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4441 ret = p->iCurr->getRefCurrent(off, curr);
4442 }
4443
4444 return ret;
4445}
4446
4448{
4450
4451 for(int l=0;l<controlledJoints;l++)
4452 {
4453 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4454 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4455
4456 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4457
4458 if (!p)
4459 {
4460 return ReturnValue::return_code::return_value_error_generic;
4461 }
4462
4463 if (p->iCurr)
4464 {
4465 ReturnValue ok = p->iCurr->getRefCurrent(off, currs+l);
4466 ret = ret && ok;
4467 }
4468 else
4469 {
4470 ret=ReturnValue::return_code::return_value_error_generic;
4471 }
4472 }
4473
4474 return ret;
4475}
4476
4477// IJointBraked interface
4478
4479
4481{
4482 ReturnValue ret = ReturnValue::return_code::return_value_error_not_ready;
4483
4484 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
4485 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4486
4487 if (p && p->iBrake) {
4488 int off = (int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4489 ret = p->iBrake->isJointBraked(off, braked);
4490 }
4491
4492 return ret;
4493}
4494
4496{
4497 ReturnValue ret = ReturnValue::return_code::return_value_error_not_ready;
4498
4499 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
4500 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4501
4502 if (p && p->iBrake) {
4503 int off = (int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4504 ret = p->iBrake->setManualBrakeActive(off, active);
4505 }
4506
4507 return ret;
4508}
4509
4511{
4512 ReturnValue ret = ReturnValue::return_code::return_value_error_not_ready;
4513
4514 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
4515 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4516
4517 if (p && p->iBrake) {
4518 int off = (int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4519 ret = p->iBrake->setAutoBrakeEnabled(off, enabled);
4520 }
4521
4522 return ret;
4523}
4524
4526{
4527 ReturnValue ret = ReturnValue::return_code::return_value_error_not_ready;
4528
4529 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
4530 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4531
4532 if (p && p->iBrake) {
4533 int off = (int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4534 ret = p->iBrake->getAutoBrakeEnabled(off, enabled);
4535 }
4536
4537 return ret;
4538}
4539
4540// IVelocityDirect interface
4541
4543{
4544 int off = static_cast<int>(remappedControlBoards.lut[jnt].axisIndexInSubControlBoard);
4545 size_t subIndex = remappedControlBoards.lut[jnt].subControlBoardIndex;
4546 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4547
4548 if (!p || !p->iVelDir)
4549 {
4550 return ReturnValue::return_code::return_value_error_generic;
4551 }
4552 return p->iVelDir->setRefVelocity(off, vel);
4553}
4554
4556{
4557 if (vels.size() != static_cast<size_t>(controlledJoints))
4558 {
4559 return ReturnValue::return_code::return_value_error_method_failed;
4560 }
4562 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4563 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(vels.data(), remappedControlBoards);
4564
4565 for (size_t ctrlBrd = 0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4566 {
4567 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(ctrlBrd);
4568 if (!p || !p->iVelDir)
4569 {
4570 ret = ReturnValue::return_code::return_value_error_generic;
4571 continue;
4572 }
4573 std::vector<double> subVels = allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd];
4574 ret = ret && p->iVelDir->setRefVelocity(subVels);
4575 }
4576 return ret;
4577}
4578
4579yarp::dev::ReturnValue ControlBoardRemappingEngine::setRefVelocity(const std::vector<int>& jnts, const std::vector<double>& vels)
4580{
4581 if (jnts.size() != vels.size())
4582 {
4583 return ReturnValue::return_code::return_value_error_method_failed;
4584 }
4586 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
4587 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(vels.data(), static_cast<int>(jnts.size()), jnts.data(), remappedControlBoards);
4588
4589 for (size_t ctrlBrd = 0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4590 {
4591 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(ctrlBrd);
4592 if (!p || !p->iVelDir)
4593 {
4594 ret = ReturnValue::return_code::return_value_error_generic;
4595 continue;
4596 }
4597 std::vector<int> subJnts = selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd];
4598 std::vector<double> subVels = selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd];
4599 ret = ret && p->iVelDir->setRefVelocity(subJnts, subVels);
4600 }
4601 return ret;
4602}
4603
4605{
4606 int off = static_cast<int>(remappedControlBoards.lut[jnt].axisIndexInSubControlBoard);
4607 size_t subIndex = remappedControlBoards.lut[jnt].subControlBoardIndex;
4608 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4609
4610 if (!p || !p->iVelDir)
4611 {
4612 return ReturnValue::return_code::return_value_error_generic;
4613 }
4614 return p->iVelDir->getRefVelocity(off, vel);
4615}
4616
4618{
4619 vels.resize(controlledJoints);
4621 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4622
4623 for (int l = 0; l < controlledJoints; l++)
4624 {
4625 int off = static_cast<int>(remappedControlBoards.lut[l].axisIndexInSubControlBoard);
4626 size_t subIndex = remappedControlBoards.lut[l].subControlBoardIndex;
4627 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4628 if (!p || !p->iVelDir)
4629 {
4630 ret = ReturnValue::return_code::return_value_error_generic;
4631 continue;
4632 }
4633 ret = ret && p->iVelDir->getRefVelocity(off, vels[l]);
4634 }
4635 return ret;
4636}
4637
4639{
4640 vels.resize(jnts.size());
4642
4643 for (size_t i = 0; i < jnts.size(); ++i)
4644 {
4645 int j = jnts[i];
4646 int off = static_cast<int>(remappedControlBoards.lut[j].axisIndexInSubControlBoard);
4647 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
4648 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4649 if (!p || !p->iVelDir)
4650 {
4651 ret = ReturnValue::return_code::return_value_error_generic;
4652 continue;
4653 }
4654 ret = ret && p->iVelDir->getRefVelocity(off, vels[i]);
4655 }
4656 return ret;
4657}
const yarp::os::LogComponent & CONTROLBOARDREMAPPER()
FeatureMode mode
#define ReturnValue_ok
Definition ReturnValue.h:82
bool configure(const RemappedControlBoards &remappedControlBoards)
Resize the buffers using the information in the RemappedControlBoards.
std::vector< std::vector< double > > m_bufferForSubControlBoard
std::vector< std::vector< yarp::dev::SelectableControlModeEnum > > m_bufferForSubControlBoardSelectableControlModesEnum
std::vector< std::vector< yarp::dev::InteractionModeEnum > > m_bufferForSubControlBoardInteractionModes
std::vector< std::vector< yarp::dev::ControlModeEnum > > m_bufferForSubControlBoardControlModesEnum
void fillSubControlBoardBuffersFromArbitraryJointVector(const double *arbitraryVec, const int n_joints, const int *joints, const RemappedControlBoards &remappedControlBoards)
Fill buffers for the SubControlBoard from a vector of joints of the RemappedControlBoards.
std::vector< std::vector< int > > m_jointsInSubControlBoard
void resizeSubControlBoardBuffers(const int n_joints, const int *joints, const RemappedControlBoards &remappedControlBoards)
Resize buffers to have the dimension of specified by the method (used for multi joint methods that re...
void fillArbitraryJointVectorFromSubControlBoardBuffers(double *arbitraryVec, const int n_joints, const int *joints, const RemappedControlBoards &remappedControlBoards)
Fill a vector of joints of the ControlBoardRemapper from the buffers of the SubControlBoard .
std::mutex mutex
Mutex to grab to use this class.
yarp::dev::ReturnValue getPidErrorLimit(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *limit) override
Get the error limit for the controller on a specific joint.
yarp::dev::ReturnValue positionMove(int j, double ref) override
Set new reference point for a single axis.
yarp::dev::ReturnValue getImpedanceOffset(int j, double *offset) override
Get current force Offset for a specific joint.
yarp::dev::ReturnValue resetEncoders() override
Reset encoders.
yarp::dev::ReturnValue getRemoteVariablesList(yarp::os::Bottle *listOfKeys) override
yarp::dev::ReturnValue getTrajAcceleration(int j, double *acc) override
Get reference acceleration for a joint.
yarp::dev::ReturnValue getTemperature(int m, double *val) override
Get temperature of a motor.
yarp::dev::ReturnValue getPid(const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::Pid *p) override
Get current PID value for a specific joint.
yarp::dev::ReturnValue setControlModes(const std::vector< int > &joints, const std::vector< yarp::dev::SelectableControlModeEnum > &modes) override
Set the current control mode for a subset of axes.
yarp::dev::ReturnValue setPidErrorLimits(const yarp::dev::PidControlTypeEnum &pidtype, const double *limits) override
Get the error limit for the controller on all joints.
yarp::dev::ReturnValue setPidFeedforward(const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override
Set the feedforward value for the specified PID controller.
yarp::dev::ReturnValue calibrateWholePart() override
calibrateWholePart: call the procedure for calibrating the whole device
yarp::dev::ReturnValue getMotorEncoders(double *encs) override
Read the position of all motor encoders.
yarp::dev::IRemoteCalibrator * getCalibratorDevice() override
getCalibratorDevice: return the pointer stored with the setCalibratorDevice
yarp::dev::ReturnValue getPidOutput(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
yarp::dev::ReturnValue getPWM(int m, double *val) override
yarp::dev::ReturnValue setMaxCurrent(int j, double v) override
yarp::dev::ReturnValue getRefDutyCycles(double *refs) override
Gets the last reference sent using the setRefDutyCycles function.
yarp::dev::ReturnValue setPidErrorLimit(const yarp::dev::PidControlTypeEnum &pidtype, int j, double limit) override
Set the error limit for the controller on a specific joint.
yarp::dev::ReturnValue getEncoderTimed(int j, double *v, double *t) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getControlMode(int j, yarp::dev::ControlModeEnum &mode) override
Get the current control mode.
yarp::dev::ReturnValue setCalibrationParameters(int j, const yarp::dev::CalibrationParameters &params) override
Start calibration, this method is very often platform specific.
yarp::dev::ReturnValue getMotorEncoderSpeed(int m, double *sp) override
Read the instantaneous speed of a motor encoder.
yarp::dev::ReturnValue getPosLimits(int j, double *min, double *max) override
Get the software limits for a particular axis.
yarp::dev::ReturnValue getAvailableControlModes(int j, std::vector< yarp::dev::SelectableControlModeEnum > &avail) override
Return a list of available control mode for the given joint.
yarp::dev::ReturnValue setRefDutyCycle(int m, double ref) override
Sets the reference dutycycle to a single motor.
yarp::dev::ReturnValue setNominalCurrent(int m, const double val) override
yarp::dev::ReturnValue getTemperatures(double *vals) override
Get temperature of all the motors.
yarp::dev::ReturnValue setTrajSpeeds(const double *spds) override
Set reference speed on all joints.
yarp::dev::ReturnValue getNominalCurrent(int m, double *val) override
yarp::dev::ReturnValue getCurrent(int m, double *curr) override
Get the instantaneous current measurement for a single motor.
yarp::dev::ReturnValue getAmpStatus(int *st) override
yarp::dev::ReturnValue getPidExtraInfo(const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::PidExtraInfo &units) override
Get PID extra info and measurement units for a specific joint.
yarp::dev::ReturnValue setPeakCurrent(int m, const double val) override
yarp::dev::ReturnValue getCurrentRanges(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
yarp::dev::ReturnValue getPids(const yarp::dev::PidControlTypeEnum &pidtype, yarp::dev::Pid *pids) override
Get current PID value for a specific joint.
yarp::dev::ReturnValue disableAmp(int j) override
Disable the amplifier on a specific joint.
yarp::dev::ReturnValue getNumberOfMotorEncoders(int *num) override
Get the number of available motor encoders.
yarp::dev::ReturnValue setControlMode(int j, yarp::dev::SelectableControlModeEnum mode) override
Set the current control mode.
yarp::dev::ReturnValue getEncoderAcceleration(int j, double *acc) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue setPositions(const int n_joints, const int *joints, const double *dpos) override
Set new reference point for all axes.
yarp::dev::ReturnValue getRefCurrents(double *currs) override
Get the reference value of the currents for all motors.
yarp::dev::ReturnValue setAutoBrakeEnabled(int j, bool enabled) override
Enables/Disables the automatic joint brake.
yarp::dev::ReturnValue getTemperatureLimit(int m, double *val) override
Retrieves the current temperature limit for a specific motor.
yarp::dev::ReturnValue getRefPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
void setAxesNames(const std::vector< std::string > &axesNames)
yarp::dev::ReturnValue getLastJointFault(int j, int &fault, std::string &message) override
yarp::dev::ReturnValue getTorque(int j, double *t) override
Get the value of the torque on a given joint (this is the feedback if you have a torque sensor).
yarp::dev::ReturnValue setPids(const yarp::dev::PidControlTypeEnum &pidtype, const yarp::dev::Pid *ps) override
Set new PID value on multiple axes.
yarp::dev::ReturnValue setInteractionModes(const std::vector< int > &joints, const std::vector< yarp::dev::InteractionModeEnum > &modes) override
Set the interaction mode of the robot for a set of joints, values can be stiff or compliant.
yarp::dev::ReturnValue abortPark() override
yarp::dev::ReturnValue getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
Get the current impedance limits for a specific joint.
yarp::dev::ReturnValue getRefTorque(int j, double *t) override
Get the reference value of the torque for a given joint.
yarp::dev::ReturnValue getTorqueRanges(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
yarp::dev::ReturnValue isPidEnabled(const yarp::dev::PidControlTypeEnum &pidtype, int j, bool &enabled) override
Get the current status (enabled/disabled) of the PID.
yarp::dev::ReturnValue homingSingleJoint(int j) override
homingSingleJoint: call the homing procedure for a single joint
yarp::dev::ReturnValue enableAmp(int j) override
Enable the amplifier on a specific joint.
yarp::dev::ReturnValue resetPid(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Reset the controller of a given joint, usually sets the current status of the joint as the reference ...
yarp::dev::ReturnValue getMotorEncoder(int m, double *v) override
Read the value of a motor encoder.
yarp::dev::ReturnValue getTargetVelocities(double *vels) override
Get the last reference speed set by velocityMove for all joints.
yarp::dev::ReturnValue parkSingleJoint(int j, bool _wait=true) override
parkSingleJoint(): start the parking procedure for the single joint
yarp::dev::ReturnValue getTrajSpeed(int j, double *ref) override
Get reference speed for a joint.
yarp::dev::ReturnValue getTargetVelocity(const int joint, double *vel) override
Get the last reference speed set by velocityMove for single joint.
yarp::dev::ReturnValue setEncoders(const double *vals) override
Set the value of all encoders.
yarp::dev::ReturnValue getInteractionMode(int j, yarp::dev::InteractionModeEnum &mode) override
Get the current interaction mode of the robot, values can be stiff or compliant.
yarp::dev::ReturnValue setVelLimits(int j, double min, double max) override
Set the software speed limits for a particular axis, the behavior of the control card when these limi...
yarp::dev::ReturnValue getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
yarp::dev::ReturnValue getRefCurrent(int m, double *curr) override
Get the reference value of the current for a single motor.
yarp::dev::ReturnValue getRemoteVariable(std::string key, yarp::os::Bottle &val) override
yarp::dev::ReturnValue getAutoBrakeEnabled(int j, bool &enabled) const override
checks if the automatic joint brake mode is enabled or disabled.
yarp::dev::ReturnValue setPWMLimit(int m, const double val) override
yarp::dev::ReturnValue calibrationDone(int j) override
Check if the calibration is terminated, on a particular joint.
yarp::dev::ReturnValue parkWholePart() override
parkWholePart: start the parking procedure for the whole part
yarp::dev::ReturnValue getPidError(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
yarp::dev::ReturnValue getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters *params) override
Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
yarp::dev::ReturnValue getRefTorques(double *refs) override
Get the reference value of the torque for all joints.
yarp::dev::ReturnValue isJointBraked(int j, bool &braked) const override
Check is the joint brake is currently active.
yarp::dev::ReturnValue getJointType(int j, yarp::dev::JointTypeEnum &type) override
yarp::dev::ReturnValue getTargetPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getTorqueRange(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
yarp::dev::ReturnValue setPosLimits(int j, double min, double max) override
Set the software limits for a particular axis, the behavior of the control card when these limits are...
yarp::dev::ReturnValue getInteractionModes(const std::vector< int > &joints, std::vector< yarp::dev::InteractionModeEnum > &modes) override
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
yarp::dev::ReturnValue getPidReference(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *ref) override
Get the current reference of the PID controller for a specific joint.
yarp::dev::ReturnValue getPeakCurrent(int m, double *val) override
yarp::dev::ReturnValue getRefPositions(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue setManualBrakeActive(int j, bool active) override
Enables/Disables manually the joint brake.
yarp::dev::ReturnValue setPidReferences(const yarp::dev::PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
yarp::dev::ReturnValue setGearboxRatio(int m, const double val) override
Set the gearbox ratio for a specific motor.
yarp::dev::ReturnValue getAxisName(int j, std::string &name) override
yarp::dev::ReturnValue setPid(const yarp::dev::PidControlTypeEnum &pidtype, int j, const yarp::dev::Pid &p) override
Set new PID value for a joint axis.
yarp::dev::ReturnValue setTrajAccelerations(const double *accs) override
Set reference acceleration on all joints.
yarp::dev::ReturnValue getPidErrors(const yarp::dev::PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
yarp::dev::ReturnValue getGearboxRatio(int m, double *val) override
Get the gearbox ratio for a specific motor.
yarp::dev::ReturnValue getRefVelocity(const int jnt, double &vel) override
Get the last reference velocity set by setRefVelocity() for a single joint.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue getTorques(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
yarp::dev::ReturnValue enablePid(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Enable the PID computation for a joint.
yarp::dev::ReturnValue getMotorEncoderCountsPerRevolution(int m, double *cpr) override
Gets number of counts per revolution for motor encoder m.
yarp::dev::ReturnValue setMotorEncoder(int m, const double val) override
Set the value of the motor encoder for a given motor.
yarp::dev::ReturnValue quitPark() override
quitPark: interrupt the park procedure
yarp::dev::ReturnValue getMotorEncoderTimed(int m, double *v, double *t) override
Read the instantaneous position of a motor encoder.
yarp::dev::ReturnValue getImpedance(int j, double *stiff, double *damp) override
Get current impedance gains (stiffness,damping,offset) for a specific joint.
yarp::dev::ReturnValue getEncodersTimed(double *encs, double *t) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue setPidReference(const yarp::dev::PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
yarp::dev::ReturnValue setRefTorque(int j, double t) override
Set the reference value of the torque for a given joint.
yarp::dev::ReturnValue calibrateSingleJoint(int j) override
calibrateSingleJoint: call the calibration procedure for the single joint
yarp::dev::ReturnValue getPWMLimit(int m, double *val) override
yarp::dev::ReturnValue stop() override
Stop motion, multiple joints.
yarp::dev::ReturnValue getPidExtraInfos(const yarp::dev::PidControlTypeEnum &pidtype, std::vector< yarp::dev::PidExtraInfo > &units) override
Get PID extra info and measurement units for a specific joint.
yarp::dev::ReturnValue homingWholePart() override
homingWholePart: call the homing procedure for a the whole part/device
yarp::dev::ReturnValue resetMotorEncoders() override
Reset motor encoders.
yarp::dev::ReturnValue setMotorEncoderCountsPerRevolution(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
yarp::dev::ReturnValue setRefVelocity(int jnt, double vel) override
Set the velocity of single joint.
yarp::dev::ReturnValue getDutyCycle(int m, double *val) override
Gets the current dutycycle of the output of the amplifier (i.e.
yarp::dev::ReturnValue getMotorEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all motor encoders.
yarp::dev::ReturnValue calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) override
Start calibration, this method is very often platform specific.
yarp::dev::ReturnValue setRefTorques(const double *t) override
Set the reference value of the torque for all joints.
yarp::dev::ReturnValue getControlModes(const std::vector< int > &joints, std::vector< yarp::dev::ControlModeEnum > &modes) override
Get the current control mode for a subset of axes.
yarp::dev::ReturnValue getPidFeedforward(const yarp::dev::PidControlTypeEnum &pidtype, int j, double &v) override
Get the currently set feedforward value for the specified PID controller.
yarp::dev::ReturnValue getCurrents(double *currs) override
Get the instantaneous current measurement for all motors.
yarp::dev::ReturnValue disablePid(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Disable the PID computation for a joint.
yarp::dev::ReturnValue isCalibratorDevicePresent(bool *isCalib) override
isCalibratorDevicePresent: check if a calibrator device has been set
yarp::dev::ReturnValue getPidErrorLimits(const yarp::dev::PidControlTypeEnum &pidtype, double *limits) override
Get the error limit for all controllers.
yarp::dev::ReturnValue resetMotorEncoder(int m) override
Reset motor encoder, single motor.
yarp::dev::ReturnValue getMaxCurrent(int j, double *v) override
Returns the maximum electric current allowed for a given motor.
yarp::dev::ReturnValue quitCalibrate() override
quitCalibrate: interrupt the calibration procedure
yarp::dev::ReturnValue setRemoteVariable(std::string key, const yarp::os::Bottle &val) override
yarp::dev::ReturnValue getTrajAccelerations(double *accs) override
Get reference acceleration of all joints.
yarp::dev::ReturnValue setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params) override
Set a subset of motor parameters (bemf, ktau etc) useful for torque control.
bool attachAllDevices(const yarp::dev::PolyDriverList &l)
yarp::dev::ReturnValue getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
yarp::dev::ReturnValue setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
yarp::dev::ReturnValue getPidOutputs(const yarp::dev::PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
yarp::dev::ReturnValue getPidOffset(const yarp::dev::PidControlTypeEnum &pidtype, int j, double &v) override
Get the currently set offset value for the specified PID controller.
yarp::dev::ReturnValue getMotorEncodersTimed(double *encs, double *t) override
Read the instantaneous position of all motor encoders.
void setNrOfControlledAxes(const size_t nrOfControlledAxes)
Set the number of controlled axes, resizing appropriately all the necessary buffers.
yarp::dev::ReturnValue getRefDutyCycle(int m, double *ref) override
Gets the last reference sent using the setRefDutyCycle function.
yarp::os::Stamp getLastInputStamp() override
Return the time stamp relative to the last acquisition.
yarp::dev::ReturnValue getPidReferences(const yarp::dev::PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all PID controllers.
yarp::dev::ReturnValue setImpedanceOffset(int j, double offset) override
Set current force Offset for a specific joint.
yarp::dev::ReturnValue setTemperatureLimit(int m, const double val) override
Set the temperature limit for a specific motor.
yarp::dev::ReturnValue abortCalibration() override
yarp::dev::ReturnValue getMotorEncoderSpeeds(double *spds) override
Read the instantaneous speed of all motor encoders.
yarp::dev::ReturnValue getCurrentRange(int m, double *min, double *max) override
Get the full scale of the current measurement for a given motor (e.g.
yarp::dev::ReturnValue setImpedance(int j, double stiff, double damp) override
Set current impedance gains (stiffness,damping) for a specific joint.
yarp::dev::ReturnValue getTrajSpeeds(double *spds) override
Get reference speed of all joints.
yarp::dev::ReturnValue getTargetPositions(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue getPowerSupplyVoltage(int m, double *val) override
yarp::dev::ReturnValue getEncoders(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue getNumberOfMotors(int *num) override
Retrieves the number of controlled motors from the current physical interface.
yarp::dev::ReturnValue setTrajAcceleration(int j, double acc) override
Set reference acceleration for a joint.
yarp::dev::ReturnValue getMotorEncoderAcceleration(int m, double *acc) override
Read the instantaneous acceleration of a motor encoder.
yarp::dev::ReturnValue getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue setRefCurrents(const double *currs) override
Set the reference value of the currents for all motors.
yarp::dev::ReturnValue setPosition(int j, double ref) override
Set new position for a single axis.
yarp::dev::ReturnValue velocityMove(int j, double v) override
Start motion at a given speed, single joint.
yarp::dev::ReturnValue checkMotionDone(int j, bool &flag) override
Check if the current trajectory is terminated.
yarp::dev::ReturnValue resetEncoder(int j) override
Reset encoder, single joint.
yarp::dev::ReturnValue getAvailablePids(int j, std::vector< yarp::dev::PidControlTypeEnum > &avail) override
ControlBoard methods.
yarp::dev::ReturnValue setRefDutyCycles(const double *refs) override
Sets the reference dutycycle for all the motors.
yarp::dev::ReturnValue getVelLimits(int j, double *min, double *max) override
Get the software speed limits for a particular axis.
yarp::dev::ReturnValue getEncoder(int j, double *v) override
Read the value of an encoder.
yarp::dev::ReturnValue setRefCurrent(int m, double curr) override
Set the reference value of the current for a single motor.
yarp::dev::ReturnValue setTrajSpeed(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 relativeMove(int j, double delta) override
Set relative position.
yarp::dev::ReturnValue setInteractionMode(int j, yarp::dev::InteractionModeEnum mode) override
Set the interaction mode of the robot, values can be stiff or compliant.
yarp::dev::ReturnValue setMotorEncoders(const double *vals) override
Set the value of all motor encoders.
yarp::dev::ReturnValue setPidOffset(const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override
Set an offset value for a given controller.
yarp::dev::ReturnValue getDutyCycles(double *vals) override
Gets the current dutycycle of the output of the amplifier (i.e.
bool configure(const RemappedControlBoards &remappedControlBoards)
Resize the buffers using the information in the RemappedControlBoards.
std::mutex mutex
Mutex to grab to use this class.
std::vector< std::vector< int > > m_jointsInSubControlBoard
void fillCompleteJointVectorFromSubControlBoardBuffers(double *full, const RemappedControlBoards &remappedControlBoards)
Fill a vector of joints of the ControlBoardRemapper from the buffers of the SubControlBoard .
void fillSubControlBoardBuffersFromCompleteJointVector(const double *full, const RemappedControlBoards &remappedControlBoards)
Fill buffers for the SubControlBoard from a vector of joints of the RemappedControlBoards.
std::vector< std::vector< double > > m_bufferForSubControlBoard
std::vector< std::vector< yarp::dev::ControlModeEnum > > m_bufferForSubControlBoardControlModesEnum
std::vector< std::vector< yarp::dev::SelectableControlModeEnum > > m_bufferForSubControlBoardSelectableControlModesEnum
std::vector< std::vector< yarp::dev::InteractionModeEnum > > m_bufferForSubControlBoardInteractionModes
std::vector< RemappedSubControlBoard > subdevices
Vector of dimension getNrOfSubControlBoards .
std::vector< RemappedAxis > lut
Vector of dimension getNrOfRemappedAxes .
RemappedSubControlBoard * getSubControlBoard(size_t i)
Given a controlboard index between 0 and getNrOfSubControlBoards()-1, return the relative SubControlB...
yarp::dev::IPositionControl * pos
yarp::dev::IImpedanceControl * iImpedance
yarp::dev::IAmplifierControl * amp
yarp::dev::IEncodersTimed * iJntEnc
yarp::dev::IInteractionMode * iInteract
yarp::dev::IRemoteVariables * iVar
yarp::dev::ICurrentControl * iCurr
yarp::dev::ITorqueControl * iTorque
yarp::dev::IVelocityControl * vel
yarp::dev::IControlMode * iMode
yarp::dev::IJointFault * iFault
yarp::dev::IVelocityDirect * iVelDir
yarp::dev::IPositionDirect * posDir
yarp::dev::IRemoteCalibrator * remcalib
yarp::dev::IControlLimits * lim
yarp::dev::IJointBrake * iBrake
yarp::dev::IControlCalibration * calib
yarp::dev::IMotorEncoders * iMotEnc
yarp::dev::IPreciselyTimed * iTimed
virtual yarp::dev::ReturnValue setPWMLimit(int j, const double val)=0
virtual yarp::dev::ReturnValue setNominalCurrent(int m, const double val)=0
virtual yarp::dev::ReturnValue getPeakCurrent(int m, double *val)=0
virtual yarp::dev::ReturnValue getPWMLimit(int j, double *val)=0
virtual yarp::dev::ReturnValue setPeakCurrent(int m, const double val)=0
virtual yarp::dev::ReturnValue getMaxCurrent(int j, double *v)=0
Returns the maximum electric current allowed for a given motor.
virtual yarp::dev::ReturnValue getPWM(int j, double *val)=0
virtual yarp::dev::ReturnValue getAmpStatus(int *st)=0
virtual yarp::dev::ReturnValue setMaxCurrent(int j, double v)=0
virtual yarp::dev::ReturnValue getNominalCurrent(int m, double *val)=0
virtual yarp::dev::ReturnValue enableAmp(int j)=0
Enable the amplifier on a specific joint.
virtual yarp::dev::ReturnValue getPowerSupplyVoltage(int j, double *val)=0
Interface for getting information about specific axes, if available.
Definition IAxisInfo.h:37
virtual yarp::dev::ReturnValue getJointType(int axis, yarp::dev::JointTypeEnum &type)
Definition IAxisInfo.h:64
virtual yarp::dev::ReturnValue getAxisName(int axis, std::string &name)=0
virtual yarp::dev::ReturnValue calibrationDone(int j)=0
Check if the calibration is terminated, on a particular joint.
virtual yarp::dev::ReturnValue calibrateAxisWithParams(int axis, unsigned int type, double p1, double p2, double p3)=0
Start calibration, this method is very often platform specific.
virtual yarp::dev::ReturnValue setCalibrationParameters(int axis, const CalibrationParameters &params)=0
Start calibration, this method is very often platform specific.
virtual yarp::dev::ReturnValue setPosLimits(int axis, double min, double max)=0
Set the software limits for a particular axis, the behavior of the control card when these limits are...
virtual yarp::dev::ReturnValue getVelLimits(int axis, double *min, double *max)=0
Get the software speed limits for a particular axis.
virtual yarp::dev::ReturnValue getPosLimits(int axis, double *min, double *max)=0
Get the software limits for a particular axis.
virtual yarp::dev::ReturnValue setVelLimits(int axis, double min, double max)=0
Set the software speed limits for a particular axis, the behavior of the control card when these limi...
virtual yarp::dev::ReturnValue getControlMode(int j, yarp::dev::ControlModeEnum &mode)=0
Get the current control mode.
virtual yarp::dev::ReturnValue getControlModes(std::vector< yarp::dev::ControlModeEnum > &mode)=0
Get the current control mode (multiple joints).
virtual yarp::dev::ReturnValue setControlModes(const std::vector< int > &joints, const std::vector< yarp::dev::SelectableControlModeEnum > &mode)=0
Set the current control mode for a subset of axes.
virtual yarp::dev::ReturnValue setControlMode(int j, yarp::dev::SelectableControlModeEnum mode)=0
Set the current control mode.
virtual yarp::dev::ReturnValue getAvailableControlModes(int j, std::vector< yarp::dev::SelectableControlModeEnum > &avail)=0
Return a list of available control mode for the given joint.
virtual yarp::dev::ReturnValue setRefCurrent(int m, double curr)=0
Set the reference value of the current for a single motor.
virtual yarp::dev::ReturnValue setRefCurrents(const double *currs)=0
Set the reference value of the currents for all motors.
virtual yarp::dev::ReturnValue getCurrent(int m, double *curr)=0
Get the instantaneous current measurement for a single motor.
virtual yarp::dev::ReturnValue getCurrentRange(int m, double *min, double *max)=0
Get the full scale of the current measurement for a given motor (e.g.
virtual yarp::dev::ReturnValue getRefCurrent(int m, double *curr)=0
Get the reference value of the current for a single motor.
virtual yarp::dev::ReturnValue getEncoderTimed(int j, double *encs, double *time)=0
Read the instantaneous acceleration of all axes.
Control board, encoder interface.
Definition IEncoders.h:117
virtual yarp::dev::ReturnValue getEncoderAcceleration(int j, double *spds)=0
Read the instantaneous acceleration of an axis.
virtual yarp::dev::ReturnValue getEncoderSpeed(int j, double *sp)=0
Read the istantaneous speed of an axis.
virtual yarp::dev::ReturnValue getEncoder(int j, double *v)=0
Read the value of an encoder.
virtual yarp::dev::ReturnValue resetEncoder(int j)=0
Reset encoder, single joint.
virtual yarp::dev::ReturnValue setEncoder(int j, double val)=0
Set the value of the encoder for a given joint.
virtual yarp::dev::ReturnValue getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)=0
Get the current impedance limits for a specific joint.
virtual yarp::dev::ReturnValue getImpedance(int j, double *stiffness, double *damping)=0
Get current impedance gains (stiffness,damping,offset) for a specific joint.
virtual yarp::dev::ReturnValue getImpedanceOffset(int j, double *offset)=0
Get current force Offset for a specific joint.
virtual yarp::dev::ReturnValue setImpedanceOffset(int j, double offset)=0
Set current force Offset for a specific joint.
virtual yarp::dev::ReturnValue setImpedance(int j, double stiffness, double damping)=0
Set current impedance gains (stiffness,damping) for a specific joint.
virtual yarp::dev::ReturnValue setInteractionModes(const std::vector< int > &joints, const std::vector< yarp::dev::InteractionModeEnum > &modes)=0
Set the interaction mode of the robot for a set of joints, values can be stiff or compliant.
virtual yarp::dev::ReturnValue getInteractionMode(int axis, yarp::dev::InteractionModeEnum &mode)=0
Get the current interaction mode of the robot, values can be stiff or compliant.
virtual yarp::dev::ReturnValue setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode)=0
Set the interaction mode of the robot, values can be stiff or compliant.
virtual yarp::dev::ReturnValue getInteractionModes(const std::vector< int > &joints, std::vector< yarp::dev::InteractionModeEnum > &modes)=0
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
virtual yarp::dev::ReturnValue setAutoBrakeEnabled(int j, bool enabled)=0
Enables/Disables the automatic joint brake.
virtual yarp::dev::ReturnValue isJointBraked(int j, bool &braked) const =0
Check is the joint brake is currently active.
virtual yarp::dev::ReturnValue getAutoBrakeEnabled(int j, bool &enabled) const =0
checks if the automatic joint brake mode is enabled or disabled.
virtual yarp::dev::ReturnValue setManualBrakeActive(int j, bool active)=0
Enables/Disables manually the joint brake.
virtual yarp::dev::ReturnValue getLastJointFault(int j, int &fault, std::string &message)=0
virtual yarp::dev::ReturnValue getMotorEncoderSpeed(int m, double *sp)=0
Read the instantaneous speed of a motor encoder.
virtual yarp::dev::ReturnValue getMotorEncoderAcceleration(int m, double *acc)=0
Read the instantaneous acceleration of a motor encoder.
virtual yarp::dev::ReturnValue setMotorEncoder(int m, const double val)=0
Set the value of the motor encoder for a given motor.
virtual yarp::dev::ReturnValue getMotorEncoderCountsPerRevolution(int m, double *cpr)=0
Gets number of counts per revolution for motor encoder m.
virtual yarp::dev::ReturnValue resetMotorEncoder(int m)=0
Reset motor encoder, single motor.
virtual yarp::dev::ReturnValue setMotorEncoderCountsPerRevolution(int m, const double cpr)=0
Sets number of counts per revolution for motor encoder m.
virtual yarp::dev::ReturnValue getMotorEncoder(int m, double *v)=0
Read the value of a motor encoder.
virtual yarp::dev::ReturnValue getMotorEncoderTimed(int m, double *encs, double *time)=0
Read the instantaneous position of a motor encoder.
virtual yarp::dev::ReturnValue getTemperatureLimit(int m, double *temp)=0
Retrieves the current temperature limit for a specific motor.
virtual yarp::dev::ReturnValue setGearboxRatio(int m, const double val)=0
Set the gearbox ratio for a specific motor.
virtual yarp::dev::ReturnValue getTemperature(int m, double *val)=0
Get temperature of a motor.
virtual yarp::dev::ReturnValue getGearboxRatio(int m, double *val)=0
Get the gearbox ratio for a specific motor.
virtual yarp::dev::ReturnValue setTemperatureLimit(int m, const double temp)=0
Set the temperature limit for a specific motor.
virtual yarp::dev::ReturnValue getDutyCycle(int m, double *val)=0
Gets the current dutycycle of the output of the amplifier (i.e.
virtual yarp::dev::ReturnValue setRefDutyCycle(int m, double ref)=0
Sets the reference dutycycle to a single motor.
virtual yarp::dev::ReturnValue getRefDutyCycle(int m, double *ref)=0
Gets the last reference sent using the setRefDutyCycle function.
virtual yarp::dev::ReturnValue disablePid(const PidControlTypeEnum &pidtype, int j)=0
Disable the PID computation for a joint.
virtual yarp::dev::ReturnValue getPidErrorLimit(const PidControlTypeEnum &pidtype, int j, double *limit)=0
Get the error limit for the controller on a specific joint.
virtual yarp::dev::ReturnValue getPidOutput(const PidControlTypeEnum &pidtype, int j, double *out)=0
Get the output of the controller (e.g.
virtual yarp::dev::ReturnValue getPidFeedforward(const PidControlTypeEnum &pidtype, int j, double &v)=0
Get the currently set feedforward value for the specified PID controller.
virtual yarp::dev::ReturnValue getPidOffset(const PidControlTypeEnum &pidtype, int j, double &v)=0
Get the currently set offset value for the specified PID controller.
virtual yarp::dev::ReturnValue isPidEnabled(const PidControlTypeEnum &pidtype, int j, bool &enabled)=0
Get the current status (enabled/disabled) of the PID.
virtual yarp::dev::ReturnValue resetPid(const PidControlTypeEnum &pidtype, int j)=0
Reset the controller of a given joint, usually sets the current status of the joint as the reference ...
virtual yarp::dev::ReturnValue getAvailablePids(int j, std::vector< PidControlTypeEnum > &avail)=0
Return a list of available pid types mode for the given joint.
virtual yarp::dev::ReturnValue getPidExtraInfo(const PidControlTypeEnum &pidtype, int j, yarp::dev::PidExtraInfo &info)=0
Get PID extra info and measurement units for a specific joint.
virtual yarp::dev::ReturnValue setPidReference(const PidControlTypeEnum &pidtype, int j, double ref)=0
Set the controller reference for a given axis.
virtual yarp::dev::ReturnValue setPidErrorLimit(const PidControlTypeEnum &pidtype, int j, double limit)=0
Set the error limit for the controller on a specific joint.
virtual yarp::dev::ReturnValue setPid(const PidControlTypeEnum &pidtype, int j, const Pid &pid)=0
Set new PID value for a joint axis.
virtual yarp::dev::ReturnValue getPidError(const PidControlTypeEnum &pidtype, int j, double *err)=0
Get the current error for a joint.
virtual yarp::dev::ReturnValue setPidFeedforward(const PidControlTypeEnum &pidtype, int j, double v)=0
Set the feedforward value for the specified PID controller.
virtual yarp::dev::ReturnValue enablePid(const PidControlTypeEnum &pidtype, int j)=0
Enable the PID computation for a joint.
virtual yarp::dev::ReturnValue getPidReference(const PidControlTypeEnum &pidtype, int j, double *ref)=0
Get the current reference of the PID controller for a specific joint.
virtual yarp::dev::ReturnValue getPid(const PidControlTypeEnum &pidtype, int j, Pid *pid)=0
Get current PID value for a specific joint.
virtual yarp::dev::ReturnValue setPidOffset(const PidControlTypeEnum &pidtype, int j, double v)=0
Set an offset value for a given controller.
virtual yarp::dev::ReturnValue getTargetPosition(const int joint, double *ref)=0
Get the last position reference for the specified axis.
virtual yarp::dev::ReturnValue setTrajSpeed(int j, double sp)=0
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
virtual yarp::dev::ReturnValue getTargetPositions(double *refs)=0
Get the last position reference for all axes.
virtual yarp::dev::ReturnValue setTrajAcceleration(int j, double acc)=0
Set reference acceleration for a joint.
virtual yarp::dev::ReturnValue relativeMove(int j, double delta)=0
Set relative position.
virtual yarp::dev::ReturnValue getTrajSpeeds(double *spds)=0
Get reference speed of all joints.
virtual yarp::dev::ReturnValue setTrajSpeeds(const double *spds)=0
Set reference speed on all joints.
virtual yarp::dev::ReturnValue positionMove(int j, double ref)=0
Set new reference point for a single axis.
virtual yarp::dev::ReturnValue getTrajAccelerations(double *accs)=0
Get reference acceleration of all joints.
virtual yarp::dev::ReturnValue checkMotionDone(int j, bool &flag)=0
Check if the current trajectory is terminated.
virtual yarp::dev::ReturnValue getTrajSpeed(int j, double *ref)=0
Get reference speed for a joint.
virtual yarp::dev::ReturnValue setTrajAccelerations(const double *accs)=0
Set reference acceleration on all joints.
virtual yarp::dev::ReturnValue stop(int j)=0
Stop motion, single joint.
virtual yarp::dev::ReturnValue getTrajAcceleration(int j, double *acc)=0
Get reference acceleration for a joint.
virtual yarp::dev::ReturnValue getRefPosition(const int joint, double *ref)=0
Get the last position reference for the specified axis.
virtual yarp::dev::ReturnValue setPositions(const int n_joint, const int *joints, const double *refs)=0
Set new reference point for all axes.
virtual yarp::dev::ReturnValue setPosition(int j, double ref)=0
Set new position for a single axis.
virtual yarp::dev::ReturnValue getRefPositions(double *refs)=0
Get the last position reference for all axes.
virtual yarp::os::Stamp getLastInputStamp()=0
Return the time stamp relative to the last acquisition.
IRemoteCalibrator interface is meant to remotize the access of the calibration device in order to all...
virtual yarp::dev::ReturnValue calibrateWholePart()=0
calibrateWholePart: call the procedure for calibrating the whole device
virtual yarp::dev::ReturnValue releaseCalibratorDevice()
releaseCalibratorDevice: reset the internal pointer to NULL when stop using the calibrator
virtual yarp::dev::ReturnValue parkWholePart()=0
parkWholePart: start the parking procedure for the whole part
virtual yarp::dev::ReturnValue calibrateSingleJoint(int j)=0
calibrateSingleJoint: call the calibration procedure for the single joint
virtual yarp::dev::ReturnValue isCalibratorDevicePresent(bool *isCalib)
isCalibratorDevicePresent: check if a calibrator device has been set
virtual yarp::dev::ReturnValue homingSingleJoint(int j)=0
homingSingleJoint: call the homing procedure for a single joint
virtual yarp::dev::ReturnValue parkSingleJoint(int j, bool _wait=true)=0
parkSingleJoint(): start the parking procedure for the single joint
virtual yarp::dev::IRemoteCalibrator * getCalibratorDevice()
getCalibratorDevice: return the pointer stored with the setCalibratorDevice
virtual yarp::dev::ReturnValue quitCalibrate()=0
quitCalibrate: interrupt the calibration procedure
virtual yarp::dev::ReturnValue quitPark()=0
quitPark: interrupt the park procedure
virtual yarp::dev::ReturnValue setCalibratorDevice(yarp::dev::IRemoteCalibrator *dev)
setCalibratorDevice: store the pointer to the calibrator device.
virtual yarp::dev::ReturnValue getRemoteVariablesList(yarp::os::Bottle *listOfKeys)=0
virtual yarp::dev::ReturnValue getRemoteVariable(std::string key, yarp::os::Bottle &val)=0
virtual yarp::dev::ReturnValue setRemoteVariable(std::string key, const yarp::os::Bottle &val)=0
virtual yarp::dev::ReturnValue getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters *params)=0
Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
virtual yarp::dev::ReturnValue getTorqueRange(int j, double *min, double *max)=0
Get the full scale of the torque sensor of a given joint.
virtual yarp::dev::ReturnValue getTorque(int j, double *t)=0
Get the value of the torque on a given joint (this is the feedback if you have a torque sensor).
virtual yarp::dev::ReturnValue setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params)=0
Set a subset of motor parameters (bemf, ktau etc) useful for torque control.
virtual yarp::dev::ReturnValue getRefTorque(int j, double *t)=0
Get the reference value of the torque for a given joint.
virtual yarp::dev::ReturnValue setRefTorque(int j, double t)=0
Set the reference value of the torque for a given joint.
virtual yarp::dev::ReturnValue setRefTorques(const double *t)=0
Set the reference value of the torque for all joints.
virtual yarp::dev::ReturnValue getTargetVelocities(double *vels)=0
Get the last reference speed set by velocityMove for all joints.
virtual yarp::dev::ReturnValue getTargetVelocity(const int joint, double *vel)=0
Get the last reference speed set by velocityMove for single joint.
virtual yarp::dev::ReturnValue velocityMove(int j, double sp)=0
Start motion at a given speed, single joint.
virtual yarp::dev::ReturnValue getRefVelocity(const int jnt, double &vel)=0
Get the last reference velocity set by setRefVelocity() for a single joint.
virtual yarp::dev::ReturnValue setRefVelocity(int jnt, double vel)=0
Set the velocity of single joint.
Contains the parameters for a PID.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
void append(const Bottle &alt)
Append the content of the given bottle to the current list.
Definition Bottle.cpp:354
size_type size() const
Gets the number of elements in the bottle.
Definition Bottle.cpp:252
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:247
A mini-server for performing network communication in the background.
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:18
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:49
virtual Bottle * asList() const
Get list value.
Definition Value.cpp:246
#define yCError(component,...)
#define yCWarning(component,...)
For streams capable of holding different kinds of content, check what they actually have.
SelectableControlModeEnum
PidControlTypeEnum
Definition PidEnums.h:15
An interface to the operating system, including Port based communication.