YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoardRemapper.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
24{
25 return detachAll();
26}
27
29{
30 Property prop;
31 prop.fromString(config.toString());
32
33 _verb = (prop.check("verbose","if present, give detailed output"));
34 if (_verb)
35 {
36 yCInfo(CONTROLBOARDREMAPPER, "running with verbose output");
37 }
38
39 if(!parseOptions(prop))
40 {
41 return false;
42 }
43
44 return true;
45}
46
47bool ControlBoardRemapper::parseOptions(Property& prop)
48{
49 bool ok = true;
50
51 usingAxesNamesForAttachAll = prop.check("axesNames", "list of networks merged by this wrapper");
52 usingNetworksForAttachAll = prop.check("networks", "list of networks merged by this wrapper");
53
54
55 if( usingAxesNamesForAttachAll &&
56 usingNetworksForAttachAll )
57 {
58 yCError(CONTROLBOARDREMAPPER) << "Both axesNames and networks option present, this is not supported.";
59 return false;
60 }
61
62 if( !usingAxesNamesForAttachAll &&
63 !usingNetworksForAttachAll )
64 {
65 yCError(CONTROLBOARDREMAPPER) << "axesNames option not found";
66 return false;
67 }
68
69 if( usingAxesNamesForAttachAll )
70 {
71 ok = parseAxesNames(prop);
72 }
73
74 if( usingNetworksForAttachAll )
75 {
76 ok = parseNetworks(prop);
77 }
78
79 return ok;
80}
81
82bool ControlBoardRemapper::parseAxesNames(const Property& prop)
83{
84 Bottle *propAxesNames=prop.find("axesNames").asList();
85 if(propAxesNames==nullptr)
86 {
87 yCError(CONTROLBOARDREMAPPER) << "Parsing parameters: \"axesNames\" should be followed by a list";
88 return false;
89 }
90
91 axesNames.resize(propAxesNames->size());
92 for(size_t ax=0; ax < propAxesNames->size(); ax++)
93 {
94 axesNames[ax] = propAxesNames->get(ax).asString();
95 }
96
97 this->setNrOfControlledAxes(axesNames.size());
98
99 return true;
100}
101
102bool ControlBoardRemapper::parseNetworks(const Property& prop)
103{
104 Bottle *nets=prop.find("networks").asList();
105 if(nets==nullptr)
106 {
107 yCError(CONTROLBOARDREMAPPER) << "Parsing parameters: \"networks\" should be followed by a list";
108 return false;
109 }
110
111 if (!prop.check("joints", "number of joints of the part"))
112 {
113 yCError(CONTROLBOARDREMAPPER) << "joints options not found when reading networks option";
114 return false;
115 }
116
117 this->setNrOfControlledAxes((size_t)prop.find("joints").asInt32());
118
119 int nsubdevices=nets->size();
120 remappedControlBoards.lut.resize(controlledJoints);
121 remappedControlBoards.subdevices.resize(nsubdevices);
122
123 // configure the devices
124 for(size_t k=0;k<nets->size();k++)
125 {
126 Bottle parameters;
127 int wBase;
128 int wTop;
129 int base;
130 int top;
131
132 parameters=prop.findGroup(nets->get(k).asString());
133
134 if(parameters.size()==2)
135 {
136 Bottle *bot=parameters.get(1).asList();
138 if(bot==nullptr)
139 {
140 // probably data are not passed in the correct way, try to read them as a string.
141 std::string bString(parameters.get(1).asString());
142 tmpBot.fromString(bString);
143
144 if(tmpBot.size() != 4)
145 {
147 << "Check network parameters in part description"
148 << "--> I was expecting" << nets->get(k).asString() << "followed by a list of four integers in parenthesis"
149 << "Got:" << parameters.toString();
150 return false;
151 }
152 else
153 {
154 bot = &tmpBot;
155 }
156 }
157
158 // If I came here, bot is correct
159 wBase=bot->get(0).asInt32();
160 wTop=bot->get(1).asInt32();
161 base=bot->get(2).asInt32();
162 top=bot->get(3).asInt32();
163 }
164 else if (parameters.size()==5)
165 {
166 // yCError(CONTROLBOARDREMAPPER) << "Parameter networks use deprecated syntax";
167 wBase=parameters.get(1).asInt32();
168 wTop=parameters.get(2).asInt32();
169 base=parameters.get(3).asInt32();
170 top=parameters.get(4).asInt32();
171 }
172 else
173 {
175 << "Check network parameters in part description"
176 << "--> I was expecting" << nets->get(k).asString() << "followed by a list of four integers in parenthesis"
177 << "Got:" << parameters.toString();
178 return false;
179 }
180
181 RemappedSubControlBoard *tmpDevice=remappedControlBoards.getSubControlBoard((size_t)k);
182 tmpDevice->setVerbose(_verb);
183
184 if( (wTop-wBase) != (top-base) )
185 {
187 << "Check network parameters in network " << nets->get(k).asString().c_str()
188 << "I was expecting a well form quadruple of numbers, got instead: "<< parameters.toString().c_str();
189 }
190
191 tmpDevice->id = nets->get(k).asString();
192
193 for(int j=wBase;j<=wTop;j++)
194 {
195 int off = j-wBase;
196 remappedControlBoards.lut[j].subControlBoardIndex=k;
197 remappedControlBoards.lut[j].axisIndexInSubControlBoard=base+off;
198 }
199 }
200
201 return true;
202}
203
204void ControlBoardRemapper::setNrOfControlledAxes(const size_t nrOfControlledAxes)
205{
206 controlledJoints = nrOfControlledAxes;
207 buffers.controlBoardModes.resize(nrOfControlledAxes,0);
208 buffers.dummyBuffer.resize(nrOfControlledAxes,0.0);
209}
210
211
212bool ControlBoardRemapper::updateAxesName()
213{
214 bool ret = true;
215 axesNames.resize(controlledJoints);
216
217 for(int l=0; l < controlledJoints; l++)
218 {
219 std::string axNameYARP;
220 bool ok = this->getAxisName(l,axNameYARP);
221 if( ok )
222 {
223 axesNames[l] = axNameYARP;
224 }
225
226 ret = ret && ok;
227 }
228
229 return ret;
230}
231
233{
234 // For both cases, now configure everything that need
235 // all the attribute to be correctly configured
236 bool ok = false;
237
238 if( usingAxesNamesForAttachAll )
239 {
240 ok = attachAllUsingAxesNames(polylist);
241 }
242
243 if( usingNetworksForAttachAll )
244 {
245 ok = attachAllUsingNetworks(polylist);
246 }
247
248 //check if all devices are attached to the driver
249 bool ready=true;
250 for(unsigned int k=0; k<remappedControlBoards.getNrOfSubControlBoards(); k++)
251 {
252 if (!remappedControlBoards.subdevices[k].isAttached())
253 {
254 ready=false;
255 }
256 }
257
258 if (!ready)
259 {
260 yCError(CONTROLBOARDREMAPPER, "AttachAll failed, some subdevice was not found or its attach failed");
261 return false;
262 }
263
264 if( ok )
265 {
266 configureBuffers();
267 }
268
269 return ok;
270}
271
272// First we store a map between each axes name
273// in the passed PolyDriverList and the device in which they belong and their index
280
281
282bool ControlBoardRemapper::attachAllUsingAxesNames(const PolyDriverList& polylist)
283{
284 std::map<std::string, axisLocation> axesLocationMap;
285
286 for(int p=0;p<polylist.size();p++)
287 {
288 // If there is a device with a specific device key, use it
289 // as a calibrator, otherwise rely on the subcontrolboards
290 // as usual
291 std::string deviceKey=polylist[p]->key;
292 if(deviceKey == "Calibrator" || deviceKey == "calibrator")
293 {
294 // Set the IRemoteCalibrator interface, the wrapper must point to the calibrator rdevice
296 polylist[p]->poly->view(calibrator);
297
299 continue;
300 }
301
302 // find if one of the desired axis is in this device
304 yarp::dev::IEncoders *iencs = nullptr;
305 polylist[p]->poly->view(iaxinfos);
306 polylist[p]->poly->view(iencs);
307
308 if( !iencs ||
309 !iaxinfos )
310 {
311 yCError(CONTROLBOARDREMAPPER) << "sub-device" << deviceKey << "does not implemented the required IAxisInfo or IEncoders interfaces";
312 return false;
313 }
314
315 size_t nrOfSubdeviceAxes;
316 bool ok = iencs->getAxes(nrOfSubdeviceAxes);
317
318 if( !ok )
319 {
320 yCError(CONTROLBOARDREMAPPER) << "sub-device" << deviceKey << "does not implemented the required getAxes method";
321 return false;
322 }
323
325 {
326 std::string axNameYARP;
327 ok = iaxinfos->getAxisName(axInSubDevice,axNameYARP);
328
329 std::string axName = axNameYARP;
330
331 if( !ok )
332 {
333 yCError(CONTROLBOARDREMAPPER) << "sub-device" << deviceKey << "does not implemented the required getAxisName method";
334 return false;
335 }
336
337 auto it = axesLocationMap.find(axName);
338 if( it != axesLocationMap.end() )
339 {
341 << "multiple axes have the same name" << axName
342 << "on on device " << polylist[p]->key << "with index" << axInSubDevice
343 <<"and another on device" << it->second.subDeviceKey << "with index" << it->second.indexInSubDevice;
344 return false;
345 }
346
348 newLocation.subDeviceKey = polylist[p]->key;
349 newLocation.indexInSubDevice = axInSubDevice;
350 newLocation.indexOfSubDeviceInPolyDriverList = p;
352 }
353 }
354
355 // We store the key of all the devices that we actually use in the remapped control device
356 std::vector<std::string> subControlBoardsKeys;
357 std::map<std::string, size_t> subControlBoardKey2IndexInPolyDriverList;
358 std::map<std::string, size_t> subControlBoardKey2IndexInRemappedControlBoards;
359
360
361 // Once we build the axis map, we build the mapping between the remapped axes and
362 // the couple subControlBoard, axis in subControlBoard
363 for(const auto& axesName : axesNames)
364 {
365 auto it = axesLocationMap.find(axesName);
366 if( it == axesLocationMap.end() )
367 {
369 << "axis " << axesName
370 << "specified in axesNames was not found in the axes of the controlboards passed to attachAll, attachAll failed.";
371 return false;
372 }
373
374 axisLocation loc = it->second;
375 std::string key = loc.subDeviceKey;
376
377 if(std::find(subControlBoardsKeys.begin(), subControlBoardsKeys.end(), key) == subControlBoardsKeys.end())
378 {
379 /* subControlBoardsKeys does not contain key */
381 subControlBoardsKeys.push_back(key);
383 }
384 }
385
386 assert(controlledJoints == (int) axesNames.size());
387
388 // We have now the number of controlboards to attach to
390 remappedControlBoards.lut.resize(controlledJoints);
391 remappedControlBoards.subdevices.resize(nrOfSubControlBoards);
392
393 // Open the controlboards
394 for(size_t ctrlBrd=0; ctrlBrd < nrOfSubControlBoards; ctrlBrd++)
395 {
398 tmpDevice->setVerbose(_verb);
400 bool ok = tmpDevice->attach(polylist[p]->poly,subControlBoardsKeys[ctrlBrd]);
401
402 if( !ok )
403 {
404 return false;
405 }
406 }
407
408
409 for(size_t l=0; l < axesNames.size(); l++)
410 {
411 axisLocation loc = axesLocationMap[axesNames[l]];
412 remappedControlBoards.lut[l].subControlBoardIndex = subControlBoardKey2IndexInRemappedControlBoards[loc.subDeviceKey];
413 remappedControlBoards.lut[l].axisIndexInSubControlBoard = (size_t)loc.indexInSubDevice;
414 }
415
416 return true;
417}
418
419
420bool ControlBoardRemapper::attachAllUsingNetworks(const PolyDriverList &polylist)
421{
422 for(int p=0;p<polylist.size();p++)
423 {
424 // look if we have to attach to a calibrator
425 std::string subDeviceKey = polylist[p]->key;
426 if(subDeviceKey == "Calibrator" || subDeviceKey == "calibrator")
427 {
428 // Set the IRemoteCalibrator interface, the wrapper must point to the calibrator rdevice
430 polylist[p]->poly->view(calibrator);
431
433 continue;
434 }
435
436 // find appropriate entry in list of subdevices and attach
437 unsigned int k=0;
439 for(k=0; k<remappedControlBoards.getNrOfSubControlBoards(); k++)
440 {
441 if (remappedControlBoards.subdevices[k].id==subDeviceKey)
442 {
443 if (!remappedControlBoards.subdevices[k].attach(polylist[p]->poly, subDeviceKey))
444 {
445 yCError(CONTROLBOARDREMAPPER, "attach to subdevice %s failed", subDeviceKey.c_str());
446 return false;
447 }
449 }
450 }
452 {
453 yCError(CONTROLBOARDREMAPPER, "subdevice %s not found in the list of subdevices", subDeviceKey.c_str());
454 std::string list_of_valid;
455 for (auto it = remappedControlBoards.subdevices.begin(); it != remappedControlBoards.subdevices.end(); ++it)
456 {
457 list_of_valid += it->id + " ";
458 }
459 yCError(CONTROLBOARDREMAPPER, "Valid options are: %s", list_of_valid.c_str());
460 return false;
461 }
462 }
463
464 bool ok = updateAxesName();
465
466 if( !ok )
467 {
468 yCWarning(CONTROLBOARDREMAPPER) << "unable to update axesNames";
469 }
470
471 return true;
472}
473
474
476{
477 //check if we already instantiated a subdevice previously
478 int devices=remappedControlBoards.getNrOfSubControlBoards();
479 for (int k = 0; k < devices; k++) {
480 remappedControlBoards.getSubControlBoard(k)->detach();
481 }
482
484 return true;
485}
486
487void ControlBoardRemapper::configureBuffers()
488{
489 allJointsBuffers.configure(remappedControlBoards);
490 selectedJointsBuffers.configure(remappedControlBoards);
491}
492
493
497
498//
499// IPid Interface
500//
501
502ReturnValue ControlBoardRemapper::getAvailablePids(int j, std::vector<yarp::dev::PidControlTypeEnum>& avail)
503{
504 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
505 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
506
507 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
508 if (!p) {
509 return ReturnValue::return_code::return_value_error_generic;
510 }
511
513
514 if (p->pid) {
515 ok = p->pid->getAvailablePids(off, avail);
516 }
517 else {
518 ok = ReturnValue::return_code::return_value_error_generic;
519 }
520
521 return ok;
522}
523
525{
526 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
527 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
528
529 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
530 if (!s)
531 {
532 return ReturnValue::return_code::return_value_error_generic;
533 }
534
535 if (s->pid)
536 {
537 return s->pid->setPid(pidtype, off, p);
538 }
539
540 return ReturnValue::return_code::return_value_error_generic;
541}
542
544{
546
547 for(int l=0;l<controlledJoints;l++)
548 {
549 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
550
551 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
552
553 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
554 if (!p)
555 {
556 return ReturnValue::return_code::return_value_error_generic;
557 }
558
559 if (p->pid)
560 {
561 ReturnValue ok = p->pid->setPid(pidtype, off, ps[l]);
562 ret=ret&&ok;
563 }
564 else
565 {
566 ret=ReturnValue::return_code::return_value_error_generic;
567 }
568 }
569 return ret;
570}
571
573{
574 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
575 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
576
577 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
578
579 if (!p)
580 {
581 return ReturnValue::return_code::return_value_error_generic;
582 }
583
584 if (p->pid)
585 {
586 return p->pid->setPidReference(pidtype, off, ref);
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
600 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
601
602 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
603 if (!p)
604 {
605 return ReturnValue::return_code::return_value_error_generic;
606 }
607
608 if (p->pid)
609 {
610 ReturnValue ok = p->pid->setPidReference(pidtype, off, refs[l]);
611 ret=ret&&ok;
612 }
613 else
614 {
615 ret=ReturnValue::return_code::return_value_error_generic;
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->setPidErrorLimit(pidtype, off, limit);
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->setPidErrorLimit(pidtype, off, limits[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->getPidError(pidtype, off, err);
684 }
685
686 return ReturnValue::return_code::return_value_error_generic;
687}
688
690{
692
693 for(int l=0;l<controlledJoints;l++)
694 {
695 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
696 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
697
698 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
699 if (!p)
700 {
701 return ReturnValue::return_code::return_value_error_generic;
702 }
703
704 if (p->pid)
705 {
706 ReturnValue ok = p->pid->getPidError(pidtype, off, errs+l);
707 ret=ret&&ok;
708 }
709 else
710 {
711 ret=ReturnValue::return_code::return_value_error_generic;
712 }
713 }
714 return ret;
715}
716
718{
719 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
720 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
721
722 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
723 if (!p)
724 {
725 return ReturnValue::return_code::return_value_error_generic;
726 }
727
728 if (p->pid)
729 {
730 return p->pid->getPidOutput(pidtype, off, out);
731 }
732
733 return ReturnValue::return_code::return_value_error_generic;
734}
735
737{
739
740 for(int l=0;l<controlledJoints;l++)
741 {
742 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
743
744 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
745
746 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
747 if (!p)
748 {
749 return ReturnValue::return_code::return_value_error_generic;
750 }
751
752 if (p->pid)
753 {
754 ret=ret&&p->pid->getPidOutput(pidtype, off, outs+l);
755 }
756 else
757 {
758 ret=ReturnValue::return_code::return_value_error_generic;
759 }
760 }
761 return ret;
762}
763
765{
766 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
767 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
768
769 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
770 if (!p)
771 {
772 return ReturnValue::return_code::return_value_error_generic;
773 }
774
775 if (p->pid)
776 {
777 return p->pid->setPidOffset(pidtype, off, v);
778 }
779
780 return ReturnValue::return_code::return_value_error_generic;
781}
782
784{
785 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
786 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
787
788 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
789 if (!s)
790 {
791 return ReturnValue::return_code::return_value_error_generic;
792 }
793
794 if (s->pid)
795 {
796 return s->pid->getPidOffset(pidtype, off, v);
797 }
798
799 return ReturnValue::return_code::return_value_error_generic;
800}
801
803{
804 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
805 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
806
807 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
808 if (!p)
809 {
810 return ReturnValue::return_code::return_value_error_generic;
811 }
812
813 if (p->pid)
814 {
815 return p->pid->setPidFeedforward(pidtype, off, v);
816 }
817
818 return ReturnValue::return_code::return_value_error_generic;
819}
820
822{
823 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
824 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
825
826 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
827 if (!s)
828 {
829 return ReturnValue::return_code::return_value_error_generic;
830 }
831
832 if (s->pid)
833 {
834 return s->pid->getPidFeedforward(pidtype, off, v);
835 }
836
837 return ReturnValue::return_code::return_value_error_generic;
838}
839
841{
842 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
843 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
844
845 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
846 if (!s)
847 {
848 return ReturnValue::return_code::return_value_error_generic;
849 }
850
851 if (s->pid)
852 {
853 return s->pid->getPid(pidtype, off, p);
854 }
855
856 return ReturnValue::return_code::return_value_error_generic;
857}
858
860{
862
863 for(int l=0;l<controlledJoints;l++)
864 {
865 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
866 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
867
868 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
869 if (!p)
870 {
871 return ReturnValue::return_code::return_value_error_generic;
872 }
873
874 if (p->pid)
875 {
876 ReturnValue ok = p->pid->getPid(pidtype, off, pids+l);
877 ret = ok && ret;
878 }
879 else
880 {
881 ret=ReturnValue::return_code::return_value_error_generic;
882 }
883 }
884
885 return ret;
886}
887
889{
890 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
891 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
892
893 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
894 if (!p)
895 {
896 return ReturnValue::return_code::return_value_error_generic;
897 }
898
899 if (p->pid)
900 {
901 return p->pid->getPidReference(pidtype, off, ref);
902 }
903
904 return ReturnValue::return_code::return_value_error_generic;
905}
906
908{
910
911 for(int l=0;l<controlledJoints;l++)
912 {
913 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
914
915 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
916
917 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
918 if (!p)
919 {
920 return ReturnValue::return_code::return_value_error_generic;
921 }
922
923 if (p->pid)
924 {
925 ReturnValue ok = p->pid->getPidReference(pidtype, off, refs+l);
926 ret=ret && ok;
927 }
928 else
929 {
930 ret=ReturnValue::return_code::return_value_error_generic;
931 }
932 }
933 return ret;
934}
935
937{
938 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
939 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
940
941 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
942
943 if (!p)
944 {
945 return ReturnValue::return_code::return_value_error_generic;
946 }
947
948 if (p->pid)
949 {
950 return p->pid->getPidErrorLimit(pidtype, off, limit);
951 }
952 return ReturnValue::return_code::return_value_error_generic;
953}
954
956{
958
959 for(int l=0;l<controlledJoints;l++)
960 {
961 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
962
963 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
964
965 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
966
967 if (!p)
968 {
969 return ReturnValue::return_code::return_value_error_generic;
970 }
971
972 if (p->pid)
973 {
974 ReturnValue ok = p->pid->getPidErrorLimit(pidtype, off, limits+l);
975 ret=ret&&ok;
976 }
977 else
978 {
979 ret=ReturnValue::return_code::return_value_error_generic;
980 }
981 }
982 return ret;
983}
984
986{
987 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
988 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
989
990 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
991
992 if (!p)
993 {
994 return ReturnValue::return_code::return_value_error_generic;
995 }
996
997 if (p->pid)
998 {
999 return p->pid->resetPid(pidtype, off);
1000 }
1001
1002 return ReturnValue::return_code::return_value_error_generic;
1003}
1004
1006{
1007 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1008 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1009
1010 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1011
1012 if (!p)
1013 {
1014 return ReturnValue::return_code::return_value_error_generic;
1015 }
1016
1017 return p->pid->disablePid(pidtype, off);
1018}
1019
1021{
1022 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1023 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1024
1025 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1026
1027 if (!p)
1028 {
1029 return ReturnValue::return_code::return_value_error_generic;
1030 }
1031
1032 if (p->pid)
1033 {
1034 return p->pid->enablePid(pidtype, off);
1035 }
1036
1037 return ReturnValue::return_code::return_value_error_generic;
1038}
1039
1041{
1042 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1043 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1044
1045 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1046
1047 if (!p)
1048 {
1049 return ReturnValue::return_code::return_value_error_generic;
1050 }
1051
1052 if (p->pid)
1053 {
1054 return p->pid->isPidEnabled(pidtype, off, enabled);
1055 }
1056
1057 return ReturnValue::return_code::return_value_error_generic;
1058}
1059
1061{
1062 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1063 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1064
1065 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1066
1067 if (!p)
1068 {
1069 return ReturnValue::return_code::return_value_error_generic;
1070 }
1071
1072 if (p->pid)
1073 {
1074 return p->pid->getPidExtraInfo(pidtype, off, units);
1075 }
1076
1077 return ReturnValue::return_code::return_value_error_generic;
1078}
1079
1080ReturnValue ControlBoardRemapper::getPidExtraInfos(const PidControlTypeEnum& pidtype, std::vector<PidExtraInfo>& units)
1081{
1083
1084 for(int l=0;l<controlledJoints;l++)
1085 {
1086 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1087 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1088
1089 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1090 if (!p)
1091 {
1092 return ReturnValue::return_code::return_value_error_generic;
1093 }
1094
1095 if (p->pid)
1096 {
1097 ReturnValue ok = p->pid->getPidExtraInfo(pidtype, off, units[l]);
1098 ret = ok && ret;
1099 }
1100 else
1101 {
1102 ret=ReturnValue::return_code::return_value_error_generic;
1103 }
1104 }
1105
1106 return ret;
1107}
1108
1109/* IPositionControl */
1111{
1112 ax=controlledJoints;
1113 return ReturnValue_ok;
1114}
1115
1117{
1118 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1119 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1120
1121 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1122
1123 if (!p)
1124 {
1125 return ReturnValue::return_code::return_value_error_generic;
1126 }
1127
1128 if (p->pos)
1129 {
1130 return p->pos->positionMove(off, ref);
1131 }
1132
1133 return ReturnValue::return_code::return_value_error_generic;
1134}
1135
1137{
1139 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1140
1141 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(refs,remappedControlBoards);
1142
1143 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1144 {
1145 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1146
1148 if (p->pos) {
1149 ok = p->pos->positionMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1150 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1151 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1152 }
1153 else {
1154 ok = ReturnValue::return_code::return_value_error_generic;
1155 }
1156 ret = ret && ok;
1157 }
1158
1159 return ret;
1160}
1161
1162ReturnValue ControlBoardRemapper::positionMove(const int n_joints, const int *joints, const double *refs)
1163{
1165 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1166
1167 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(refs,n_joints,joints,remappedControlBoards);
1168
1169 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1170 {
1171 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1172
1174 if (p->pos) {
1175 ok = p->pos->positionMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1176 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1177 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1178 }
1179 else {
1180 ok = ReturnValue::return_code::return_value_error_generic;
1181 }
1182 ret = ret && ok;
1183 }
1184
1185 return ret;
1186}
1187
1189{
1190 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1191 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1192
1193 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1194
1195 if (!p)
1196 {
1197 return ReturnValue::return_code::return_value_error_generic;
1198 }
1199
1200 if (p->pos)
1201 {
1202 ReturnValue ret = p->pos->getTargetPosition(off, ref);
1203 return ret;
1204 }
1205
1206 return ReturnValue::return_code::return_value_error_generic;
1207}
1208
1210{
1212 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1213
1214 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1215 {
1216 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1217
1219
1220 if( p->pos )
1221 {
1222 ok = p->pos->getTargetPositions(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1223 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1224 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1225 }
1226 else
1227 {
1228 ok = ReturnValue::return_code::return_value_error_generic;
1229 }
1230
1231 ret = ret && ok;
1232 }
1233
1234 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(spds,remappedControlBoards);
1235
1236 return ret;
1237}
1238
1239
1240ReturnValue ControlBoardRemapper::getTargetPositions(const int n_joints, const int *joints, double *targets)
1241{
1243 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1244
1245 // Resize the input buffers
1246 selectedJointsBuffers.resizeSubControlBoardBuffers(n_joints,joints,remappedControlBoards);
1247
1248 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1249 {
1250 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1251
1253
1254 if( p->pos )
1255 {
1256 ok = p->pos->getTargetPositions(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1257 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1258 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1259 }
1260 else
1261 {
1262 ok = ReturnValue::return_code::return_value_error_generic;
1263 }
1264
1265 ret = ret && ok;
1266 }
1267
1268 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(targets,n_joints,joints,remappedControlBoards);
1269
1270 return ret;
1271}
1272
1274{
1275 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1276 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1277
1278 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1279
1280 if (!p)
1281 {
1282 return ReturnValue::return_code::return_value_error_generic;
1283 }
1284
1285 if (p->pos)
1286 {
1287 return p->pos->relativeMove(off, delta);
1288 }
1289
1290 return ReturnValue::return_code::return_value_error_generic;
1291}
1292
1294{
1296 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1297
1298 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(deltas,remappedControlBoards);
1299
1300 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1301 {
1302 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1304 if (p->pos) {
1305 ok = p->pos->relativeMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1306 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1307 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1308 }
1309 else {
1310 ok = ReturnValue::return_code::return_value_error_generic;
1311 }
1312 ret = ret && ok;
1313 }
1314
1315 return ret;
1316}
1317
1318ReturnValue ControlBoardRemapper::relativeMove(const int n_joints, const int *joints, const double *deltas)
1319{
1321 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1322
1323 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(deltas,n_joints,joints,remappedControlBoards);
1324
1325 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1326 {
1327 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1329 if (p->pos) {
1330 ok = p->pos->relativeMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1331 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1332 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1333 }
1334 else {
1335 ok = ReturnValue::return_code::return_value_error_generic;
1336 }
1337 ret = ret && ok;
1338 }
1339
1340 return ret;
1341}
1342
1344{
1345 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1346 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1347
1348 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1349
1350 if (!p)
1351 {
1352 return ReturnValue::return_code::return_value_error_generic;
1353 }
1354
1355 if (p->pos)
1356 {
1357 return p->pos->checkMotionDone(off, flag);
1358 }
1359
1360 return ReturnValue::return_code::return_value_error_generic;
1361}
1362
1364{
1366 flag=true;
1367
1368 for(int l=0;l<controlledJoints;l++)
1369 {
1370 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1371 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1372
1373 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1374
1375 if (!p)
1376 {
1377 return ReturnValue::return_code::return_value_error_generic;
1378 }
1379
1380 if (p->pos)
1381 {
1382 bool singleJointMotionDone =false;
1384 ret = ret && ok;
1385 flag = flag && singleJointMotionDone;
1386 }
1387 else
1388 {
1389 ret = ReturnValue::return_code::return_value_error_generic;
1390 }
1391 }
1392 return ret;
1393}
1394
1395ReturnValue ControlBoardRemapper::checkMotionDone(const std::vector<int>& joints, bool& flag)
1396{
1398 flag=true;
1399
1400 for(int j=0;j<joints.size();j++)
1401 {
1402 int l = joints[j];
1403
1404 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1405 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1406
1407 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1408
1409 if (!p)
1410 {
1411 return ReturnValue::return_code::return_value_error_generic;
1412 }
1413
1414 if (p->pos)
1415 {
1416 bool singleJointMotionDone =false;
1418 ret = ret && ok;
1419 flag = flag && singleJointMotionDone;
1420 }
1421 else
1422 {
1423 ret = ReturnValue::return_code::return_value_error_generic;
1424 }
1425 }
1426
1427 return ret;
1428}
1429
1431{
1432 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1433 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1434
1435 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1436
1437 if (!p)
1438 {
1439 return ReturnValue::return_code::return_value_error_generic;
1440 }
1441
1442 if (p->pos)
1443 {
1444 return p->pos->setTrajSpeed(off, sp);
1445 }
1446
1447 return ReturnValue::return_code::return_value_error_generic;
1448}
1449
1451{
1453 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1454
1455 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(spds,remappedControlBoards);
1456
1457 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1458 {
1459 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1460
1462 if (p->pos) {
1463 ok = p->pos->setTrajSpeeds(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1464 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1465 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1466 }
1467 else {
1468 ok = ReturnValue::return_code::return_value_error_generic;
1469 }
1470 ret = ret && ok;
1471 }
1472
1473 return ret;
1474}
1475
1476ReturnValue ControlBoardRemapper::setTrajSpeeds(const int n_joints, const int *joints, const double *spds)
1477{
1479 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1480
1481 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(spds,n_joints,joints,remappedControlBoards);
1482
1483 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1484 {
1485 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1486
1488 if (p->pos) {
1489 ok = p->pos->setTrajSpeeds(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1490 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1491 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1492 }
1493 else {
1494 ok = ReturnValue::return_code::return_value_error_generic;
1495 }
1496 ret = ret && ok;
1497 }
1498
1499 return ret;
1500}
1501
1503{
1504 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1505 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1506
1507 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1508
1509 if (!p)
1510 {
1511 return ReturnValue::return_code::return_value_error_generic;
1512 }
1513
1514 if (p->pos)
1515 {
1516 return p->pos->setTrajAcceleration(off, acc);
1517 }
1518
1519 return ReturnValue::return_code::return_value_error_generic;
1520}
1521
1523{
1525 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1526
1527 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(accs,remappedControlBoards);
1528
1529 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1530 {
1531 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1532
1534 if (p->pos) {
1536 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1537 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1538 }
1539 else {
1540 ok = ReturnValue::return_code::return_value_error_generic;
1541 }
1542 ret = ret && ok;
1543 }
1544
1545 return ret;
1546}
1547
1548ReturnValue ControlBoardRemapper::setTrajAccelerations(const int n_joints, const int *joints, const double *accs)
1549{
1551 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1552
1553 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(accs,n_joints,joints,remappedControlBoards);
1554
1555 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1556 {
1557 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1558
1560 if (p->pos) {
1561 ok = p->pos->setTrajAccelerations(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1562 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1563 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1564 }
1565 else {
1566 ok = ReturnValue::return_code::return_value_error_generic;
1567 }
1568 ret = ret && ok;
1569 }
1570
1571 return ret;
1572}
1573
1575{
1576 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1577 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1578
1579 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1580
1581 if (!p)
1582 {
1583 return ReturnValue::return_code::return_value_error_generic;
1584 }
1585
1586 if (p->pos)
1587 {
1588 return p->pos->getTrajSpeed(off, ref);
1589 }
1590
1591 return ReturnValue::return_code::return_value_error_generic;
1592}
1593
1595{
1597 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1598
1599 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1600 {
1601 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1602
1604 if( p->pos )
1605 {
1606 ok = p->pos->getTrajSpeeds(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1607 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1608 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1609 }
1610 else
1611 {
1612 ok = ReturnValue::return_code::return_value_error_generic;
1613 }
1614
1615 ret = ret && ok;
1616 }
1617
1618 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(spds,remappedControlBoards);
1619
1620 return ret;
1621}
1622
1623
1624ReturnValue ControlBoardRemapper::getTrajSpeeds(const int n_joints, const int *joints, double *spds)
1625{
1627 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1628
1629 // Resize the input buffers
1630 selectedJointsBuffers.resizeSubControlBoardBuffers(n_joints,joints,remappedControlBoards);
1631
1632 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1633 {
1634 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1635
1637
1638 if( p->pos )
1639 {
1640 ok = p->pos->getTrajSpeeds(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1641 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1642 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1643 }
1644 else
1645 {
1646 ok = ReturnValue::return_code::return_value_error_generic;
1647 }
1648
1649 ret = ret && ok;
1650 }
1651
1652 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(spds,n_joints,joints,remappedControlBoards);
1653
1654 return ret;
1655}
1656
1658{
1659 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1660 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1661
1662 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1663
1664 if (!p)
1665 {
1666 return ReturnValue::return_code::return_value_error_generic;
1667 }
1668
1669 if (p->pos)
1670 {
1671 return p->pos->getTrajAcceleration(off, acc);
1672 }
1673
1674 return ReturnValue::return_code::return_value_error_generic;
1675}
1676
1678{
1680 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1681
1682 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1683 {
1684 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1685
1687
1688 if( p->pos )
1689 {
1691 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1692 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1693 }
1694 else
1695 {
1696 ok = ReturnValue::return_code::return_value_error_generic;
1697 }
1698
1699 ret = ret && ok;
1700 }
1701
1702 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(accs,remappedControlBoards);
1703
1704 return ret;
1705}
1706
1707ReturnValue ControlBoardRemapper::getTrajAccelerations(const int n_joints, const int *joints, double *accs)
1708{
1710 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1711
1712 // Resize the input buffers
1713 selectedJointsBuffers.resizeSubControlBoardBuffers(n_joints,joints,remappedControlBoards);
1714
1715 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1716 {
1717 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1718
1720 if( p->pos )
1721 {
1722 ok = p->pos->getTrajAccelerations(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1723 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1724 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1725 }
1726 else
1727 {
1728 ok = ReturnValue::return_code::return_value_error_generic;
1729 }
1730
1731 ret = ret && ok;
1732 }
1733
1734 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(accs,n_joints,joints,remappedControlBoards);
1735
1736 return ret;
1737}
1738
1740{
1741 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1742 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1743
1744 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1745
1746 if (!p)
1747 {
1748 return ReturnValue::return_code::return_value_error_generic;
1749 }
1750
1751 if (p->pos)
1752 {
1753 return p->pos->stop(off);
1754 }
1755
1756 return ReturnValue::return_code::return_value_error_generic;
1757}
1758
1760{
1762 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1763
1764 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1765 {
1766 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1767
1769 if (p->pos) {
1770 ok = p->pos->stop(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1771 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data());
1772 }
1773 else
1774 {
1775 ok = ReturnValue::return_code::return_value_error_generic;
1776 }
1777 ret = ret && ok;
1778 }
1779
1780 return ret;
1781}
1782
1783ReturnValue ControlBoardRemapper::stop(const int n_joints, const int *joints)
1784{
1786 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
1787 std::lock_guard<std::mutex> lock2(buffers.mutex);
1788
1789
1790 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(buffers.dummyBuffer.data(),n_joints,joints,remappedControlBoards);
1791
1792 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1793 {
1794 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1796 if (p->pos) {
1797 ok = p->pos->stop(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1798 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data());
1799 }
1800 else
1801 {
1802 ok = ReturnValue::return_code::return_value_error_generic;
1803 }
1804 ret = ret && ok;
1805 }
1806
1807 return ret;
1808}
1809
1810/* IJointFaultControl */
1811ReturnValue ControlBoardRemapper::getLastJointFault(int j, int& fault, std::string& message)
1812{
1813 int off = (int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1814 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
1815
1816 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
1817
1818 if (!p)
1819 {
1820 return ReturnValue::return_code::return_value_error_generic;
1821 }
1822
1823 if (p->iFault)
1824 {
1825 return p->iFault->getLastJointFault(off, fault, message);
1826 }
1827
1828 return ReturnValue::return_code::return_value_error_generic;
1829}
1830
1831/* IVelocityControl */
1832
1834{
1835 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1836 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1837
1838 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1839
1840 if (!p)
1841 {
1842 return ReturnValue::return_code::return_value_error_generic;
1843 }
1844
1845 if (p->vel)
1846 {
1847 return p->vel->velocityMove(off, v);
1848 }
1849
1850 return ReturnValue::return_code::return_value_error_generic;
1851}
1852
1854{
1856 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
1857
1858 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(v,remappedControlBoards);
1859
1860 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
1861 {
1862 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
1863
1865 if (p->vel) {
1866 ok = p->vel->velocityMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
1867 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
1868 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
1869 }
1870 else {
1871 ok = ReturnValue::return_code::return_value_error_generic;
1872 }
1873 ret = ret && ok;
1874 }
1875
1876 return ret;
1877}
1878
1879/* IEncoders */
1881{
1882 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1883 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1884
1885 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1886 if (!p)
1887 {
1888 return ReturnValue::return_code::return_value_error_generic;
1889 }
1890
1891 if (p->iJntEnc)
1892 {
1893 return p->iJntEnc->resetEncoder(off);
1894 }
1895
1896 return ReturnValue::return_code::return_value_error_generic;
1897}
1898
1900{
1902
1903 for(int l=0;l<controlledJoints;l++)
1904 {
1905 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1906 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
1907
1908 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1909 if (!p)
1910 {
1911 return ReturnValue::return_code::return_value_error_generic;
1912 }
1913
1914 if (p->iJntEnc)
1915 {
1916 ReturnValue ok = p->iJntEnc->resetEncoder(off);
1917 ret = ret && ok;
1918 }
1919 else
1920 {
1921 ret=ReturnValue::return_code::return_value_error_generic;
1922 }
1923 }
1924 return ret;
1925}
1926
1928{
1929 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1930 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1931
1932 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1933
1934 if (!p)
1935 {
1936 return ReturnValue::return_code::return_value_error_generic;
1937 }
1938
1939 if (p->iJntEnc)
1940 {
1941 return p->iJntEnc->setEncoder(off,val);
1942 }
1943
1944 return ReturnValue::return_code::return_value_error_generic;
1945}
1946
1948{
1950
1951 for(int l=0;l<controlledJoints;l++)
1952 {
1953 int off = (int) remappedControlBoards.lut[l].axisIndexInSubControlBoard;
1954 size_t subIndex = remappedControlBoards.lut[l].subControlBoardIndex;
1955
1956 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
1957
1958 if (!p)
1959 {
1960 return ReturnValue::return_code::return_value_error_generic;
1961 }
1962
1963 if (p->iJntEnc)
1964 {
1965 ReturnValue ok = p->iJntEnc->setEncoder(off, vals[l]);
1966 ret = ret && ok;
1967 }
1968 else
1969 {
1970 ret = ReturnValue::return_code::return_value_error_generic;
1971 }
1972 }
1973 return ret;
1974}
1975
1977{
1978 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
1979 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
1980
1981 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
1982
1983 if (!p)
1984 {
1985 return ReturnValue::return_code::return_value_error_generic;
1986 }
1987
1988 if (p->iJntEnc)
1989 {
1990 return p->iJntEnc->getEncoder(off, v);
1991 }
1992
1993 return ReturnValue::return_code::return_value_error_generic;
1994}
1995
1997{
1999
2000 for(int l=0;l<controlledJoints;l++)
2001 {
2002 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2003 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2004
2005 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2006
2007 if (!p)
2008 {
2009 return ReturnValue::return_code::return_value_error_generic;
2010 }
2011
2012 if (p->iJntEnc)
2013 {
2014 ReturnValue ok = p->iJntEnc->getEncoder(off, encs+l);
2015 ret = ret && ok;
2016 }
2017 else
2018 {
2019 ret = ReturnValue::return_code::return_value_error_generic;
2020 }
2021 }
2022 return ret;
2023}
2024
2026{
2028
2029 for(int l=0;l<controlledJoints;l++)
2030 {
2031 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2032 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2033
2034 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2035
2036 if (!p)
2037 {
2038 return ReturnValue::return_code::return_value_error_generic;
2039 }
2040
2041 if (p->iJntEnc)
2042 {
2043 ReturnValue ok = p->iJntEnc->getEncoderTimed(off, encs+l, t+l);
2044 ret = ret && ok;
2045 }
2046 else
2047 {
2048 ret = ReturnValue::return_code::return_value_error_generic;
2049 }
2050 }
2051 return ret;
2052}
2053
2055{
2056 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2057 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2058
2059 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2060
2061 if (!p)
2062 {
2063 return ReturnValue::return_code::return_value_error_generic;
2064 }
2065
2066 if (p->iJntEnc)
2067 {
2068 return p->iJntEnc->getEncoderTimed(off, v, t);
2069 }
2070
2071 return ReturnValue::return_code::return_value_error_generic;
2072}
2073
2075{
2076 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2077 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2078
2079 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2080
2081 if (!p)
2082 {
2083 return ReturnValue::return_code::return_value_error_generic;
2084 }
2085
2086 if (p->iJntEnc)
2087 {
2088 return p->iJntEnc->getEncoderSpeed(off, sp);
2089 }
2090
2091 return ReturnValue::return_code::return_value_error_generic;
2092}
2093
2095{
2097
2098 for(int l=0;l<controlledJoints;l++)
2099 {
2100 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2101 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2102
2103 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2104
2105 if (!p)
2106 {
2107 return ReturnValue::return_code::return_value_error_generic;
2108 }
2109
2110 if (p->iJntEnc)
2111 {
2112 ReturnValue ok = p->iJntEnc->getEncoderSpeed(off, spds+l);
2113 ret = ret && ok;
2114 }
2115 else
2116 {
2117 ret = ReturnValue::return_code::return_value_error_generic;
2118 }
2119 }
2120 return ret;
2121}
2122
2124{
2125 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2126 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2127
2128 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2129
2130 if (!p)
2131 {
2132 return ReturnValue::return_code::return_value_error_generic;
2133 }
2134
2135 if (p->iJntEnc)
2136 {
2137 return p->iJntEnc->getEncoderAcceleration(off,acc);
2138 }
2139
2140 return ReturnValue::return_code::return_value_error_generic;
2141}
2142
2144{
2146
2147 for(int l=0;l<controlledJoints;l++)
2148 {
2149 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2150 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2151
2152 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2153
2154 if (!p)
2155 {
2156 return ReturnValue::return_code::return_value_error_generic;
2157 }
2158
2159 if (p->iJntEnc)
2160 {
2161 ReturnValue ok = p->iJntEnc->getEncoderAcceleration(off, accs+l);
2162 ret = ret && ok;
2163 }
2164 else
2165 {
2166 ret = ReturnValue::return_code::return_value_error_generic;
2167 }
2168 }
2169 return ret;
2170}
2171
2172/* IMotor */
2174{
2175 *num=controlledJoints;
2176 return ReturnValue_ok;
2177}
2178
2180{
2181 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2182 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2183
2184 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2185
2186 if (!p)
2187 {
2188 return ReturnValue::return_code::return_value_error_generic;
2189 }
2190
2191 if (p->imotor)
2192 {
2193 return p->imotor->getTemperature(off, val);
2194 }
2195
2196 return ReturnValue::return_code::return_value_error_generic;
2197}
2198
2200{
2202 for(int l=0;l<controlledJoints;l++)
2203 {
2204 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2205 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2206
2207 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2208 if (!p)
2209 {
2210 return ReturnValue::return_code::return_value_error_generic;
2211 }
2212
2213 if (p->imotor)
2214 {
2215 ret=ret&&p->imotor->getTemperature(off, vals+l);
2216 }
2217 else
2218 {
2219 ret=ReturnValue::return_code::return_value_error_generic;
2220 }
2221 }
2222 return ret;
2223}
2224
2226{
2227 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2228 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2229
2230 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2231
2232 if (!p)
2233 {
2234 return ReturnValue::return_code::return_value_error_generic;
2235 }
2236
2237 if (p->imotor)
2238 {
2239 return p->imotor->getTemperatureLimit(off, val);
2240 }
2241
2242 return ReturnValue::return_code::return_value_error_generic;
2243}
2244
2246{
2247 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2248 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2249
2250 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2251
2252 if (!p)
2253 {
2254 return ReturnValue::return_code::return_value_error_generic;
2255 }
2256
2257 if (p->imotor)
2258 {
2259 return p->imotor->setTemperatureLimit(off,val);
2260 }
2261
2262 return ReturnValue::return_code::return_value_error_generic;
2263}
2264
2266{
2267 int off = (int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2268 size_t subIndex = remappedControlBoards.lut[m].subControlBoardIndex;
2269
2270 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2271
2272 if (!p)
2273 {
2274 return ReturnValue::return_code::return_value_error_generic;
2275 }
2276
2277 if (p->imotor)
2278 {
2279 return p->imotor->getGearboxRatio(off, val);
2280 }
2281
2282 return ReturnValue::return_code::return_value_error_generic;
2283}
2284
2286{
2287 int off = (int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2288 size_t subIndex = remappedControlBoards.lut[m].subControlBoardIndex;
2289
2290 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2291
2292 if (!p)
2293 {
2294 return ReturnValue::return_code::return_value_error_generic;
2295 }
2296
2297 if (p->imotor)
2298 {
2299 return p->imotor->setGearboxRatio(off, val);
2300 }
2301
2302 return ReturnValue::return_code::return_value_error_generic;
2303}
2304
2305/* IRemoteVariables */
2307{
2308 yCWarning(CONTROLBOARDREMAPPER, "getRemoteVariable is not properly implemented, use at your own risk.");
2309
2311
2312 for (unsigned int i = 0; i < remappedControlBoards.getNrOfSubControlBoards(); i++)
2313 {
2314 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(i);
2315
2316 if (!p)
2317 {
2318 return ReturnValue::return_code::return_value_error_generic;
2319 }
2320
2321 if (!p->iVar)
2322 {
2323 return ReturnValue::return_code::return_value_error_generic;
2324 }
2325
2328
2329 if (ok)
2330 {
2331 val.append(tmpval);
2332 }
2333
2334 b = b && ok;
2335 }
2336
2337 return b;
2338}
2339
2341{
2342 yCWarning(CONTROLBOARDREMAPPER, "setRemoteVariable is not properly implemented, use at your own risk.");
2343
2344 size_t bottle_size = val.size();
2345 size_t device_size = remappedControlBoards.getNrOfSubControlBoards();
2346 if (bottle_size != device_size)
2347 {
2348 yCError(CONTROLBOARDREMAPPER, "ControlBoardRemapper::setRemoteVariable bottle_size != device_size failure");
2349 return ReturnValue::return_code::return_value_error_generic;
2350 }
2351
2353 for (unsigned int i = 0; i < device_size; i++)
2354 {
2355 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(i);
2356 if (!p)
2357 {
2358 yCError(CONTROLBOARDREMAPPER, "ControlBoardRemapper::setRemoteVariable !p failure");
2359 ReturnValue::return_code::return_value_error_generic;
2360 }
2361 if (!p->iVar)
2362 {
2363 yCError(CONTROLBOARDREMAPPER, "ControlBoardRemapper::setRemoteVariable !p->iVar failure");
2364 ReturnValue::return_code::return_value_error_generic;
2365 }
2366 Bottle* partial_val = val.get(i).asList();
2367 if (partial_val)
2368 {
2369 b &= p->iVar->setRemoteVariable(key, *partial_val);
2370 }
2371 else
2372 {
2373 yCError(CONTROLBOARDREMAPPER, "ControlBoardRemapper::setRemoteVariable general failure");
2374 return ReturnValue::return_code::return_value_error_generic;
2375 }
2376 }
2377
2378 return b;
2379}
2380
2382{
2383 yCWarning(CONTROLBOARDREMAPPER, "getRemoteVariablesList is not properly implemented, use at your own risk.");
2384
2385 size_t subIndex = remappedControlBoards.lut[0].subControlBoardIndex;
2386 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2387
2388 if (!p)
2389 {
2390 return ReturnValue::return_code::return_value_error_generic;
2391 }
2392
2393 if (p->iVar)
2394 {
2395 return p->iVar->getRemoteVariablesList(listOfKeys);
2396 }
2397
2398 return ReturnValue::return_code::return_value_error_generic;
2399}
2400
2401/* IMotorEncoders */
2402
2404{
2405 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2406 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2407
2408 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2409
2410 if (!p)
2411 {
2412 return ReturnValue::return_code::return_value_error_generic;
2413 }
2414
2415 if (p->iMotEnc)
2416 {
2417 return p->iMotEnc->resetMotorEncoder(off);
2418 }
2419
2420 return ReturnValue::return_code::return_value_error_generic;
2421}
2422
2424{
2426
2427 for(int l=0;l<controlledJoints;l++)
2428 {
2429 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2430 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2431
2432 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2433 if (!p)
2434 {
2435 return ReturnValue::return_code::return_value_error_generic;
2436 }
2437
2438 if (p->iMotEnc)
2439 {
2441 ret= ret && ok;
2442 }
2443 else
2444 {
2445 ret=ReturnValue::return_code::return_value_error_generic;
2446 }
2447 }
2448
2449 return ret;
2450}
2451
2453{
2454 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2455 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2456
2457 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2458
2459 if (!p)
2460 {
2461 return ReturnValue::return_code::return_value_error_generic;
2462 }
2463
2464 if (p->iMotEnc)
2465 {
2466 return p->iMotEnc->setMotorEncoder(off,val);
2467 }
2468
2469 return ReturnValue::return_code::return_value_error_generic;
2470}
2471
2473{
2475
2476 for(int l=0;l<controlledJoints;l++)
2477 {
2478 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2479 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2480
2481 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2482
2483 if (!p)
2484 {
2485 return ReturnValue::return_code::return_value_error_generic;
2486 }
2487
2488 if (p->iMotEnc)
2489 {
2490 ReturnValue ok = p->iMotEnc->setMotorEncoder(off, vals[l]);
2491 ret = ret && ok;
2492 }
2493 else
2494 {
2495 ret=ReturnValue::return_code::return_value_error_generic;
2496 }
2497 }
2498
2499 return ret;
2500}
2501
2503{
2504 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2505 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2506
2507 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2508
2509 if (!p)
2510 {
2511 return ReturnValue::return_code::return_value_error_generic;
2512 }
2513
2514 if (p->iMotEnc)
2515 {
2516 return p->iMotEnc->setMotorEncoderCountsPerRevolution(off,cpr);
2517 }
2518
2519 return ReturnValue::return_code::return_value_error_generic;
2520}
2521
2523{
2524 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2525 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2526
2527 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2528
2529 if (!p)
2530 {
2531 return ReturnValue::return_code::return_value_error_generic;
2532 }
2533
2534 if (p->iMotEnc)
2535 {
2536 return p->iMotEnc->getMotorEncoderCountsPerRevolution(off, cpr);
2537 }
2538
2539 return ReturnValue::return_code::return_value_error_generic;
2540}
2541
2543{
2544 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2545 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2546
2547 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2548
2549 if (!p)
2550 {
2551 return ReturnValue::return_code::return_value_error_generic;
2552 }
2553
2554 if (p->iMotEnc)
2555 {
2556 return p->iMotEnc->getMotorEncoder(off, v);
2557 }
2558
2559 return ReturnValue::return_code::return_value_error_generic;
2560}
2561
2563{
2565
2566 for(int l=0;l<controlledJoints;l++)
2567 {
2568 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2569 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2570
2571 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2572 if (!p) {
2573 return ReturnValue::return_code::return_value_error_generic;
2574 }
2575
2576 if (p->iMotEnc)
2577 {
2578 ret=ret&&p->iMotEnc->getMotorEncoder(off, encs+l);
2579 } else {
2580 ret = ReturnValue::return_code::return_value_error_generic;
2581 }
2582 }
2583 return ret;
2584}
2585
2587{
2589
2590 for(int l=0;l<controlledJoints;l++)
2591 {
2592 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2593 size_t subIndex=remappedControlBoards.lut[l].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->iMotEnc)
2603 {
2604 ReturnValue ok = p->iMotEnc->getMotorEncoderTimed(off, encs, t);
2605 ret = ret && ok;
2606 }
2607 else
2608 {
2609 ret = ReturnValue::return_code::return_value_error_generic;
2610 }
2611 }
2612 return ret;
2613}
2614
2616{
2617 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2618 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2619
2620 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2621
2622 if (!p)
2623 {
2624 return ReturnValue::return_code::return_value_error_generic;
2625 }
2626
2627 if (p->iMotEnc)
2628 {
2629 return p->iMotEnc->getMotorEncoderTimed(off, v, t);
2630 }
2631
2632 return ReturnValue::return_code::return_value_error_generic;
2633}
2634
2636{
2637 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2638 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2639
2640 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2641
2642 if (!p)
2643 {
2644 return ReturnValue::return_code::return_value_error_generic;
2645 }
2646
2647 if (p->iMotEnc)
2648 {
2649 return p->iMotEnc->getMotorEncoderSpeed(off, sp);
2650 }
2651
2652 return ReturnValue::return_code::return_value_error_generic;
2653}
2654
2656{
2658
2659 for(int l=0;l<controlledJoints;l++)
2660 {
2661 int off = (int) remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2662 size_t subIndex = remappedControlBoards.lut[l].subControlBoardIndex;
2663
2664 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2665
2666 if (!p)
2667 {
2668 return ReturnValue::return_code::return_value_error_generic;
2669 }
2670
2671 if (p->iMotEnc)
2672 {
2673 ReturnValue ok = p->iMotEnc->getMotorEncoderSpeed(off, spds + l);
2674 ret = ret && ok;
2675 }
2676 else
2677 {
2678 ret = ReturnValue::return_code::return_value_error_generic;
2679 }
2680 }
2681 return ret;
2682}
2683
2685{
2686 int off = (int) remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2687 size_t subIndex = remappedControlBoards.lut[m].subControlBoardIndex;
2688
2689 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2690
2691 if (!p)
2692 {
2693 return ReturnValue::return_code::return_value_error_generic;
2694 }
2695
2696 if (p->iMotEnc)
2697 {
2698 return p->iMotEnc->getMotorEncoderAcceleration(off,acc);
2699 }
2700 *acc=0.0;
2701 return ReturnValue::return_code::return_value_error_generic;
2702}
2703
2705{
2707
2708 for(int l=0;l<controlledJoints;l++)
2709 {
2710 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2711 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2712
2713 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2714
2715 if (!p)
2716 {
2717 return ReturnValue::return_code::return_value_error_generic;
2718 }
2719
2720 if (p->iMotEnc)
2721 {
2723 ret=ret && ok;
2724 }
2725 else
2726 {
2727 ret=ReturnValue::return_code::return_value_error_generic;
2728 }
2729 }
2730
2731 return ret;
2732}
2733
2734
2736{
2737 *num=controlledJoints;
2738 return ReturnValue_ok;
2739}
2740
2741/* IAmplifierControl */
2743{
2744 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2745 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2746
2747 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2748
2749 if (!p)
2750 {
2751 return ReturnValue::return_code::return_value_error_generic;
2752 }
2753
2754 if (p->amp)
2755 {
2756 return p->amp->enableAmp(off);
2757 }
2758
2759 return ReturnValue::return_code::return_value_error_generic;
2760}
2761
2766
2768{
2770
2771 for(int l=0;l<controlledJoints;l++)
2772 {
2773 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
2774 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
2775
2776 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2777
2778 if (!p)
2779 {
2780 return ReturnValue::return_code::return_value_error_generic;
2781 }
2782
2783 if (p->amp)
2784 {
2785 ReturnValue ok = p->amp->getAmpStatus(off, st+l);
2786 ret = ret && ok;
2787 }
2788 else
2789 {
2790 ret=ReturnValue::return_code::return_value_error_generic;
2791 }
2792 }
2793
2794 return ret;
2795}
2796
2798{
2799 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2800 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2801
2802 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2803
2804 if (!p)
2805 {
2806 return ReturnValue::return_code::return_value_error_generic;
2807 }
2808
2809 if (p->amp)
2810 {
2811 return p->amp->getAmpStatus(off,v);
2812 }
2813
2814 return ReturnValue::return_code::return_value_error_generic;
2815}
2816
2818{
2819 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2820 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2821
2822 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2823
2824 if (!p)
2825 {
2826 return ReturnValue::return_code::return_value_error_generic;
2827 }
2828
2829 if (p->amp)
2830 {
2831 return p->amp->setMaxCurrent(off,v);
2832 }
2833
2834 return ReturnValue::return_code::return_value_error_generic;
2835}
2836
2838{
2839 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
2840 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
2841
2842 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2843
2844 if (!p)
2845 {
2846 return ReturnValue::return_code::return_value_error_generic;
2847 }
2848
2849 if (p->amp)
2850 {
2851 return p->amp->getMaxCurrent(off,v);
2852 }
2853
2854 return ReturnValue::return_code::return_value_error_generic;
2855}
2856
2858{
2859 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2860 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2861
2862 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2863
2864 if(!p)
2865 {
2866 return ReturnValue::return_code::return_value_error_generic;
2867 }
2868
2869 if(!p->amp)
2870 {
2871 return ReturnValue::return_code::return_value_error_generic;
2872 }
2873
2874 return p->amp->getNominalCurrent(off, val);
2875}
2876
2878{
2879 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2880 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2881
2882 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2883
2884 if(!p)
2885 {
2886 return ReturnValue::return_code::return_value_error_generic;
2887 }
2888
2889 if(!p->amp)
2890 {
2891 return ReturnValue::return_code::return_value_error_generic;
2892 }
2893
2894 return p->amp->getPeakCurrent(off, val);
2895}
2896
2898{
2899 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2900 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2901
2902 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2903
2904 if (!p)
2905 {
2906 return ReturnValue::return_code::return_value_error_generic;
2907 }
2908
2909 if (!p->amp)
2910 {
2911 return ReturnValue::return_code::return_value_error_generic;
2912 }
2913
2914 return p->amp->setPeakCurrent(off, val);
2915}
2916
2918{
2919 int off = (int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2920 size_t subIndex = remappedControlBoards.lut[m].subControlBoardIndex;
2921
2922 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
2923
2924 if (!p)
2925 {
2926 return ReturnValue::return_code::return_value_error_generic;
2927 }
2928
2929 if (!p->amp)
2930 {
2931 return ReturnValue::return_code::return_value_error_generic;
2932 }
2933
2934 return p->amp->setNominalCurrent(off, val);
2935}
2936
2938{
2939 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2940 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2941
2942 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2943
2944 if(!p)
2945 {
2946 return ReturnValue::return_code::return_value_error_generic;
2947 }
2948
2949 if(!p->amp)
2950 {
2951 return ReturnValue::return_code::return_value_error_generic;
2952 }
2953
2954 return p->amp->getPWM(off, val);
2955}
2957{
2958 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2959 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2960
2961 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2962
2963 if(!p)
2964 {
2965 return ReturnValue::return_code::return_value_error_generic;
2966 }
2967
2968 if(!p->amp)
2969 {
2970 return ReturnValue::return_code::return_value_error_generic;
2971 }
2972
2973 return p->amp->getPWMLimit(off, val);
2974}
2975
2977{
2978 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2979 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
2980
2981 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
2982
2983 if (!p)
2984 {
2985 return ReturnValue::return_code::return_value_error_generic;
2986 }
2987
2988 if (!p->amp)
2989 {
2990 return ReturnValue::return_code::return_value_error_generic;
2991 }
2992
2993 return p->amp->setPWMLimit(off, val);
2994}
2995
2997{
2998 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
2999 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
3000
3001 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3002
3003 if(!p)
3004 {
3005 return ReturnValue::return_code::return_value_error_generic;
3006 }
3007
3008 if(!p->amp)
3009 {
3010 return ReturnValue::return_code::return_value_error_generic;
3011 }
3012
3013 return p->amp->getPowerSupplyVoltage(off, val);
3014}
3015
3016
3017/* IControlLimits */
3018
3020{
3021 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3022 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3023
3024 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3025
3026 if (!p)
3027 {
3028 return ReturnValue::return_code::return_value_error_generic;
3029 }
3030
3031 if (p->lim)
3032 {
3033 return p->lim->setPosLimits(off,min, max);
3034 }
3035
3036 return ReturnValue::return_code::return_value_error_generic;
3037}
3038
3039ReturnValue ControlBoardRemapper::getPosLimits(int j, double *min, double *max)
3040{
3041 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3042 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3043
3044 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3045
3046 if (!p)
3047 {
3048 return ReturnValue::return_code::return_value_error_generic;
3049 }
3050
3051 if (p->lim)
3052 {
3053 return p->lim->getPosLimits(off,min, max);
3054 }
3055
3056 return ReturnValue::return_code::return_value_error_generic;
3057}
3058
3060{
3061 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3062 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3063
3064 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3065
3066 if (!p)
3067 {
3068 return ReturnValue::return_code::return_value_error_generic;
3069 }
3070
3071 if (!p->lim)
3072 {
3073 return ReturnValue::return_code::return_value_error_generic;
3074 }
3075
3076 return p->lim->setVelLimits(off,min, max);
3077}
3078
3079ReturnValue ControlBoardRemapper::getVelLimits(int j, double *min, double *max)
3080{
3081 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3082 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3083
3084 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3085
3086 if (!p)
3087 {
3088 return ReturnValue::return_code::return_value_error_generic;
3089 }
3090
3091 if(!p->lim)
3092 {
3093 return ReturnValue::return_code::return_value_error_generic;
3094 }
3095
3096 return p->lim->getVelLimits(off,min, max);
3097}
3098
3099/* IRemoteCalibrator */
3104
3109
3111{
3112 if(!getCalibratorDevice())
3113 {
3114 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3115 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
3116
3117 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(subIndex);
3118 if (!s)
3119 {
3120 return ReturnValue::return_code::return_value_error_generic;
3121 }
3122
3123 if (s->remcalib)
3124 {
3125 return s->remcalib->calibrateSingleJoint(off);
3126 }
3127
3128 return ReturnValue::return_code::return_value_error_generic;
3129 }
3130 else
3131 {
3132 return IRemoteCalibrator::getCalibratorDevice()->calibrateSingleJoint(j);
3133 }
3134}
3135
3137{
3138 if(!getCalibratorDevice())
3139 {
3140 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3141 {
3142 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(ctrlBrd);
3143 if (!s)
3144 {
3145 return ReturnValue::return_code::return_value_error_generic;
3146 }
3147
3148 if (s->remcalib)
3149 {
3150 return s->remcalib->calibrateWholePart();
3151 }
3152 }
3153
3154 return ReturnValue::return_code::return_value_error_generic;
3155 }
3156 else
3157 {
3158 return IRemoteCalibrator::getCalibratorDevice()->calibrateWholePart();
3159 }
3160}
3161
3163{
3164 if(!getCalibratorDevice())
3165 {
3166 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3167 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
3168
3169 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(subIndex);
3170 if (!s)
3171 {
3172 return ReturnValue::return_code::return_value_error_generic;
3173 }
3174
3175 if (s->remcalib)
3176 {
3177 return s->remcalib->homingSingleJoint(off);
3178 }
3179
3180 return ReturnValue::return_code::return_value_error_generic;
3181 }
3182 else
3183 {
3184 return IRemoteCalibrator::getCalibratorDevice()->homingSingleJoint(j);
3185 }
3186}
3187
3189{
3190 if(!getCalibratorDevice())
3191 {
3193 for(int l=0;l<controlledJoints;l++)
3194 {
3195 ReturnValue ok = this->homingSingleJoint(l);
3196 ret = ret && ok;
3197 }
3198
3199 return ret;
3200 }
3201 else
3202 {
3203 return IRemoteCalibrator::getCalibratorDevice()->homingWholePart();
3204 }
3205}
3206
3208{
3209 if(!getCalibratorDevice())
3210 {
3211 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3212 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
3213
3214 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(subIndex);
3215 if (!s)
3216 {
3217 return ReturnValue::return_code::return_value_error_generic;
3218 }
3219
3220 if (s->remcalib)
3221 {
3222 return s->remcalib->parkSingleJoint(off,_wait);
3223 }
3224
3225 return ReturnValue::return_code::return_value_error_generic;
3226 }
3227 else
3228 {
3230 }
3231}
3232
3234{
3235 if(!getCalibratorDevice())
3236 {
3238
3239 for(int l=0; l<controlledJoints; l++)
3240 {
3241 bool _wait = false;
3242 ReturnValue ok = this->parkSingleJoint(l,_wait);
3243 ret = ret && ok;
3244 }
3245
3246 return ret;
3247 }
3248 else
3249 {
3251 }
3252}
3253
3255{
3256 if(!getCalibratorDevice())
3257 {
3258 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3259 {
3260 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(ctrlBrd);
3261 if (!s)
3262 {
3263 return ReturnValue::return_code::return_value_error_generic;
3264 }
3265
3266 if (s->remcalib)
3267 {
3268 return s->remcalib->quitCalibrate();
3269 }
3270 }
3271
3272 return ReturnValue::return_code::return_value_error_generic;
3273 }
3274 else
3275 {
3276 return IRemoteCalibrator::getCalibratorDevice()->quitCalibrate();
3277 }
3278}
3279
3281{
3282 if(!getCalibratorDevice())
3283 {
3284 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3285 {
3286 RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(ctrlBrd);
3287 if (!s)
3288 {
3289 return ReturnValue::return_code::return_value_error_generic;
3290 }
3291
3292 if (s->remcalib)
3293 {
3294 return s->remcalib->quitPark();
3295 }
3296 }
3297
3298 return ReturnValue::return_code::return_value_error_generic;
3299 }
3300 else
3301 {
3302 return IRemoteCalibrator::getCalibratorDevice()->quitPark();
3303 }
3304}
3305
3306
3307/* IControlCalibration */
3308ReturnValue ControlBoardRemapper::calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3)
3309{
3310 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3311 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3312
3313 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
3314
3315 if (p && p->calib)
3316 {
3317 return p->calib->calibrateAxisWithParams(off, ui,v1,v2,v3);
3318 }
3319 return ReturnValue::return_code::return_value_error_generic;
3320}
3321
3323{
3324 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3325 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
3326
3327 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
3328
3329 if (p && p->calib)
3330 {
3331 return p->calib->setCalibrationParameters(off, params);
3332 }
3333
3334 return ReturnValue::return_code::return_value_error_generic;
3335}
3336
3338{
3339 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3340 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3341
3342 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3343
3344 if (!p)
3345 {
3346 return ReturnValue::return_code::return_value_error_generic;
3347 }
3348
3349 if (p->calib)
3350 {
3351 return p->calib->calibrationDone(off);
3352 }
3353
3354 return ReturnValue::return_code::return_value_error_generic;
3355}
3356
3358{
3359 yCError(CONTROLBOARDREMAPPER, "Calling abortPark -- not implemented");
3360 return ReturnValue::return_code::return_value_error_generic;
3361}
3362
3364{
3365 yCError(CONTROLBOARDREMAPPER, "Calling abortCalibration -- not implemented");
3366 return ReturnValue::return_code::return_value_error_generic;
3367}
3368
3369/* IAxisInfo */
3370
3372{
3373 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3374 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3375
3376 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3377
3378 if (!p)
3379 {
3380 return ReturnValue::return_code::return_value_error_generic;
3381 }
3382
3383 if (p->info)
3384 {
3385 return p->info->getAxisName(off, name);
3386 }
3387
3388 return ReturnValue::return_code::return_value_error_generic;
3389}
3390
3392{
3393 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3394 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
3395
3396 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
3397
3398 if (!p)
3399 {
3400 return ReturnValue::return_code::return_value_error_generic;
3401 }
3402
3403 if (p->info)
3404 {
3405 return p->info->getJointType(off, type);
3406 }
3407
3408 return ReturnValue::return_code::return_value_error_generic;
3409}
3410
3412{
3414
3415 for(int l=0;l<controlledJoints;l++)
3416 {
3417 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
3418 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
3419
3420 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3421
3422 if (!p)
3423 {
3424 return ReturnValue::return_code::return_value_error_generic;
3425 }
3426
3427 if (p->iTorque)
3428 {
3429 ReturnValue ok = p->iTorque->getRefTorque(off, refs+l);
3430 ret = ret && ok;
3431 }
3432 else
3433 {
3434 ret = ReturnValue::return_code::return_value_error_generic;
3435 }
3436 }
3437 return ret;
3438}
3439
3441{
3442 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3443 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3444
3445 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3446
3447 if (!p)
3448 {
3449 return ReturnValue::return_code::return_value_error_generic;
3450 }
3451
3452 if (p->iTorque)
3453 {
3454 return p->iTorque->getRefTorque(off, t);
3455 }
3456 return ReturnValue::return_code::return_value_error_generic;
3457}
3458
3460{
3462 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
3463
3464 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(t,remappedControlBoards);
3465
3466 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3467 {
3468 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3469
3470 ReturnValue ok;
3471
3472 if( p->iTorque )
3473 {
3474 ok = p->iTorque->setRefTorques(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3475 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3476 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3477 }
3478 else
3479 {
3480 ok = ReturnValue::return_code::return_value_error_generic;
3481 }
3482
3483 ret = ret && ok;
3484 }
3485
3486 return ret;
3487}
3488
3490{
3491 int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3492 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
3493
3494 RemappedSubControlBoard *p = remappedControlBoards.getSubControlBoard(subIndex);
3495 if (!p)
3496 {
3497 return ReturnValue::return_code::return_value_error_generic;
3498 }
3499
3500 if (p->iTorque)
3501 {
3502 return p->iTorque->setRefTorque(off, t);
3503 }
3504 return ReturnValue::return_code::return_value_error_generic;
3505}
3506
3507ReturnValue ControlBoardRemapper::setRefTorques(const int n_joints, const int *joints, const double *t)
3508{
3510 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
3511
3512 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(t,n_joints,joints,remappedControlBoards);
3513
3514 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3515 {
3516 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3517
3518 ReturnValue ok = p->iTorque->setRefTorques(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3519 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3520 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3521 ret = ret && ok;
3522 }
3523
3524 return ret;
3525}
3526
3528{
3529 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3530 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3531
3532 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3533
3534 if (!p)
3535 {
3536 return ReturnValue::return_code::return_value_error_generic;
3537 }
3538
3539 if (p->iTorque)
3540 {
3541 return p->iTorque->getMotorTorqueParams(off, params);
3542 }
3543
3544 return ReturnValue::return_code::return_value_error_generic;
3545}
3546
3548{
3549 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3550 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3551
3552 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3553
3554 if (!p)
3555 {
3556 return ReturnValue::return_code::return_value_error_generic;
3557 }
3558
3559 if (p->iTorque)
3560 {
3561 return p->iTorque->setMotorTorqueParams(off, params);
3562 }
3563
3564 return ReturnValue::return_code::return_value_error_generic;
3565}
3566
3568{
3569 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3570 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3571
3572 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3573
3574 if (!p)
3575 {
3576 return ReturnValue::return_code::return_value_error_generic;
3577 }
3578
3579 if (p->iImpedance)
3580 {
3581 return p->iImpedance->setImpedance(off, stiff, damp);
3582 }
3583
3584 return ReturnValue::return_code::return_value_error_generic;
3585}
3586
3588{
3589 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3590 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3591
3592 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3593
3594 if (!p)
3595 {
3596 return ReturnValue::return_code::return_value_error_generic;
3597 }
3598
3599 if (p->iImpedance)
3600 {
3601 return p->iImpedance->setImpedanceOffset(off, offset);
3602 }
3603
3604 return ReturnValue::return_code::return_value_error_generic;
3605}
3606
3608{
3609 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3610 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3611
3612 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3613
3614 if (!p)
3615 {
3616 return ReturnValue::return_code::return_value_error_generic;
3617 }
3618
3619 if (p->iTorque)
3620 {
3621 return p->iTorque->getTorque(off, t);
3622 }
3623
3624 return ReturnValue::return_code::return_value_error_generic;
3625}
3626
3628{
3630
3631 for(int l=0;l<controlledJoints;l++)
3632 {
3633 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
3634 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
3635
3636 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3637
3638 if (!p)
3639 {
3640 return ReturnValue::return_code::return_value_error_generic;
3641 }
3642
3643 if (p->iTorque)
3644 {
3645 ReturnValue ok = p->iTorque->getTorque(off, t+l);
3646 ret = ret && ok;
3647 }
3648 else
3649 {
3650 ret=ReturnValue::return_code::return_value_error_generic;
3651 }
3652 }
3653
3654 return ret;
3655 }
3656
3658{
3659 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3660 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3661
3662 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3663
3664 if (!p)
3665 {
3666 return ReturnValue::return_code::return_value_error_generic;
3667 }
3668
3669 if (p->iTorque)
3670 {
3671 return p->iTorque->getTorqueRange(off, min, max);
3672 }
3673
3674 return ReturnValue::return_code::return_value_error_generic;
3675}
3676
3678{
3680
3681 for(int l=0;l<controlledJoints;l++)
3682 {
3683 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
3684 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
3685
3686 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3687
3688 if (!p)
3689 {
3690 return ReturnValue::return_code::return_value_error_generic;
3691 }
3692
3693 if (p->iTorque)
3694 {
3695 ReturnValue ok = p->iTorque->getTorqueRange(off, min+l, max+l);
3696 ret = ret && ok;
3697 }
3698 else
3699 {
3700 ret=ReturnValue::return_code::return_value_error_generic;
3701 }
3702 }
3703 return ret;
3704 }
3705
3707{
3708 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3709 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3710
3711 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3712
3713 if (!p)
3714 {
3715 return ReturnValue::return_code::return_value_error_generic;
3716 }
3717
3718 if (p->iImpedance)
3719 {
3720 return p->iImpedance->getImpedance(off, stiff, damp);
3721 }
3722
3723 return ReturnValue::return_code::return_value_error_generic;
3724}
3725
3727{
3728 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3729 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3730
3731 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3732
3733 if (!p)
3734 {
3735 return ReturnValue::return_code::return_value_error_generic;
3736 }
3737
3738 if (p->iImpedance)
3739 {
3740 return p->iImpedance->getImpedanceOffset(off, offset);
3741 }
3742
3743 return ReturnValue::return_code::return_value_error_generic;
3744}
3745
3746ReturnValue ControlBoardRemapper::getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
3747{
3748 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3749 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3750
3751 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3752
3753 if (!p)
3754 {
3755 return ReturnValue::return_code::return_value_error_generic;
3756 }
3757
3758 if (p->iImpedance)
3759 {
3760 return p->iImpedance->getCurrentImpedanceLimit(off, min_stiff, max_stiff, min_damp, max_damp);
3761 }
3762
3763 return ReturnValue::return_code::return_value_error_generic;
3764}
3765
3766ReturnValue ControlBoardRemapper::getAvailableControlModes(int j, std::vector<yarp::dev::SelectableControlModeEnum>& avail)
3767{
3768 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3769 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3770
3771 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3772 if (!p) {
3773 return ReturnValue::return_code::return_value_error_generic;
3774 }
3775
3777
3778 if (p->iMode) {
3779 ok = p->iMode->getAvailableControlModes(off, avail);
3780 }
3781 else {
3782 ok = ReturnValue::return_code::return_value_error_generic;
3783 }
3784
3785 return ok;
3786}
3787
3789{
3790 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3791 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3792
3793 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3794 if (!p) {
3795 return ReturnValue::return_code::return_value_error_generic;
3796 }
3797
3799
3800 if (p->iMode) {
3801 ok = p->iMode->getControlMode(off, mode);
3802 }
3803 else {
3804 ok = ReturnValue::return_code::return_value_error_generic;
3805 }
3806
3807 return ok;
3808
3809}
3810
3811ReturnValue ControlBoardRemapper::getControlModes(std::vector<yarp::dev::ControlModeEnum>& modes)
3812{
3814 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
3815
3816 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3817 {
3818 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3819
3820 ReturnValue ok;
3821
3822 if( p->iMode )
3823 {
3824 ok = p->iMode->getControlModes( allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
3826 }
3827 else
3828 {
3829 ok = ReturnValue::return_code::return_value_error_generic;
3830 }
3831
3832 ret = ret && ok;
3833 }
3834
3835 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(modes.data(),remappedControlBoards);
3836
3837 return ret;
3838}
3839
3840// IControlMode interface
3841ReturnValue ControlBoardRemapper::getControlModes(const std::vector<int>& joints, std::vector<yarp::dev::ControlModeEnum>& modes)
3842{
3844 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
3845
3846 // Resize the input buffers
3847 selectedJointsBuffers.resizeSubControlBoardBuffers(joints.size(),joints.data(),remappedControlBoards);
3848
3849 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3850 {
3851 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3852
3853 ReturnValue ok;
3854
3855 if( p->iMode )
3856 {
3857 ok = p->iMode->getControlModes( selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
3859 }
3860 else
3861 {
3862 ok = ReturnValue::return_code::return_value_error_generic;
3863 }
3864
3865 ret = ret && ok;
3866 }
3867
3868 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(modes.data(),joints.size(),joints.data(),remappedControlBoards);
3869
3870 return ret;
3871}
3872
3874{
3876
3877 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3878 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3879
3880 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3881
3882 if (!p)
3883 {
3884 return ReturnValue::return_code::return_value_error_generic;
3885 }
3886
3887 if (p->iMode)
3888 {
3889 ret = p->iMode->setControlMode(off, mode);
3890 }
3891 else
3892 {
3893 ret = ReturnValue::return_code::return_value_error_generic;
3894 }
3895
3896 return ret;
3897}
3898
3899ReturnValue ControlBoardRemapper::setControlModes(const std::vector<int>& joints, const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
3900{
3902 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
3903
3904 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(modes.data(),joints.size(),joints.data(),remappedControlBoards);
3905
3906 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3907 {
3908 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3909
3911 if (p->iMode) {
3912 ok = p->iMode->setControlModes( selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
3914 }
3915 else {
3916 ok = ReturnValue::return_code::return_value_error_generic;
3917 }
3918 ret = ret && ok;
3919 }
3920
3921 return ret;
3922}
3923
3924ReturnValue ControlBoardRemapper::setControlModes(const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
3925{
3927 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
3928
3929 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(modes.data(),remappedControlBoards);
3930
3931 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3932 {
3933 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3934
3936 if (p->iMode) {
3937 ok = p->iMode->setControlModes(allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
3939 }
3940 else {
3941 ok = ReturnValue::return_code::return_value_error_generic;
3942 }
3943 ret = ret && ok;
3944 }
3945
3946 return ret;
3947}
3948
3950{
3951 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
3952 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
3953
3954 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
3955
3956 if (!p)
3957 {
3958 return ReturnValue::return_code::return_value_error_generic;
3959 }
3960
3961 if (p->posDir)
3962 {
3963 return p->posDir->setPosition(off, ref);
3964 }
3965
3966 return ReturnValue::return_code::return_value_error_generic;
3967}
3968
3969ReturnValue ControlBoardRemapper::setPositions(const int n_joints, const int *joints, const double *dpos)
3970{
3972 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
3973
3974 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(dpos,n_joints,joints,remappedControlBoards);
3975
3976 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
3977 {
3978 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
3979
3981 if (p->posDir) {
3982 ok = p->posDir->setPositions(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
3983 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
3984 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
3985 }
3986 else {
3987 ok = ReturnValue::return_code::return_value_error_generic;
3988 }
3989 ret = ret && ok;
3990 }
3991
3992 return ret;
3993}
3994
3996{
3998 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
3999
4000 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(refs,remappedControlBoards);
4001
4002 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4003 {
4004 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4005
4007 if (p->posDir) {
4008 ok = p->posDir->setPositions(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
4009 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
4010 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
4011 }
4012 else {
4013 ok = ReturnValue::return_code::return_value_error_generic;
4014 }
4015 ret = ret && ok;
4016 }
4017
4018 return ret;
4019}
4020
4022{
4023 double averageTimestamp = 0.0;
4024 int collectedTimestamps = 0;
4025
4026 for(int l=0;l<controlledJoints;l++)
4027 {
4028 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4029
4030 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4031
4032 if (!p)
4033 {
4034 return Stamp();
4035 }
4036
4037 if(p->iTimed)
4038 {
4041 }
4042 }
4043
4044
4045 std::lock_guard<std::mutex> lock(buffers.mutex);
4046
4047 if( collectedTimestamps > 0 )
4048 {
4050 }
4051 else
4052 {
4053 buffers.stamp.update();
4054 }
4055
4056 return buffers.stamp;
4057}
4058
4060{
4061 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4062 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
4063
4064 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4065
4066 if (!p)
4067 {
4068 return ReturnValue::return_code::return_value_error_generic;
4069 }
4070
4071 if (p->posDir)
4072 {
4073 ReturnValue ret = p->posDir->getRefPosition(off, ref);
4074 return ret;
4075 }
4076
4077 return ReturnValue::return_code::return_value_error_generic;
4078}
4079
4081{
4083 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4084
4085 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4086 {
4087 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4088
4090
4091 if( p->posDir )
4092 {
4093 ok = p->posDir->getRefPositions(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
4094 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
4095 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
4096 }
4097 else
4098 {
4099 ok = ReturnValue::return_code::return_value_error_generic;
4100 }
4101
4102 ret = ret && ok;
4103 }
4104
4105 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(spds,remappedControlBoards);
4106
4107 return ret;
4108}
4109
4110
4111ReturnValue ControlBoardRemapper::getRefPositions(const int n_joints, const int *joints, double *targets)
4112{
4114 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
4115
4116 // Resize the input buffers
4117 selectedJointsBuffers.resizeSubControlBoardBuffers(n_joints,joints,remappedControlBoards);
4118
4119 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4120 {
4121 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4122
4124
4125 if( p->posDir )
4126 {
4127 ok = p->posDir->getRefPositions(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
4128 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
4129 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
4130 }
4131 else
4132 {
4133 ok = ReturnValue::return_code::return_value_error_generic;
4134 }
4135
4136 ret = ret && ok;
4137 }
4138
4139 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(targets,n_joints,joints,remappedControlBoards);
4140
4141 return ret;
4142}
4143
4144
4145
4146// IVelocityControl interface
4147ReturnValue ControlBoardRemapper::velocityMove(const int n_joints, const int *joints, const double *spds)
4148{
4150 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
4151
4152 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(spds,n_joints,joints,remappedControlBoards);
4153
4154 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4155 {
4156 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4157
4159 if (p->vel) {
4160 ok = p->vel->velocityMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
4161 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
4162 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
4163 }
4164 else
4165 {
4166 ok = ReturnValue::return_code::return_value_error_generic;
4167 }
4168 ret = ret && ok;
4169 }
4170
4171 return ret;
4172}
4173
4175{
4176 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4177 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
4178
4179 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4180
4181 if (!p)
4182 {
4183 return ReturnValue::return_code::return_value_error_generic;
4184 }
4185
4186 if (p->vel)
4187 {
4188 ReturnValue ret = p->vel->getTargetVelocity(off, vel);
4189 return ret;
4190 }
4191
4192 return ReturnValue::return_code::return_value_error_generic;
4193}
4194
4195
4197{
4199 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4200
4201 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4202 {
4203 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4204
4206
4207 if( p->vel )
4208 {
4209 ok = p->vel->getTargetVelocities(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
4210 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
4211 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
4212 }
4213 else
4214 {
4215 ok = ReturnValue::return_code::return_value_error_generic;
4216 }
4217
4218 ret = ret && ok;
4219 }
4220
4221 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(vels,remappedControlBoards);
4222
4223 return ret;
4224}
4225
4226ReturnValue ControlBoardRemapper::getTargetVelocities(const int n_joints, const int* joints, double* vels)
4227{
4229 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
4230
4231 // Resize the input buffers
4232 selectedJointsBuffers.resizeSubControlBoardBuffers(n_joints,joints,remappedControlBoards);
4233
4234 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4235 {
4236 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4237
4239
4240 if( p->vel )
4241 {
4242 ok = p->vel->getTargetVelocities(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
4243 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
4244 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
4245 }
4246 else
4247 {
4248 ok = ReturnValue::return_code::return_value_error_generic;
4249 }
4250
4251 ret = ret && ok;
4252 }
4253
4254 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(vels,n_joints,joints,remappedControlBoards);
4255
4256 return ret;
4257}
4258
4260{
4261 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4262 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
4263
4264 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
4265
4266 if (!s)
4267 {
4268 return ReturnValue::return_code::return_value_error_generic;
4269 }
4270
4271 if (s->iInteract)
4272 {
4273 return s->iInteract->getInteractionMode(off, mode);
4274 }
4275
4276 return ReturnValue::return_code::return_value_error_generic;
4277}
4278
4279ReturnValue ControlBoardRemapper::getInteractionModes(const std::vector<int>& joints, std::vector<yarp::dev::InteractionModeEnum>& modes)
4280{
4282 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
4283
4284 // Resize the input buffers
4285 selectedJointsBuffers.resizeSubControlBoardBuffers(joints.size(),joints.data(),remappedControlBoards);
4286
4287 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4288 {
4289 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4290
4292
4293 if( p->iMode )
4294 {
4295 ok = p->iInteract->getInteractionModes(selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
4297 }
4298 else
4299 {
4300 ok = ReturnValue::return_code::return_value_error_generic;
4301 }
4302
4303 ret = ret && ok;
4304 }
4305
4306 selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(modes.data(), joints.size(), joints.data(), remappedControlBoards);
4307
4308 return ret;
4309}
4310
4311ReturnValue ControlBoardRemapper::getInteractionModes(std::vector<yarp::dev::InteractionModeEnum>& modes)
4312{
4314 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4315
4316 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4317 {
4318 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4319
4321
4322 if( p->iMode )
4323 {
4326 }
4327 else
4328 {
4329 ok = ReturnValue::return_code::return_value_error_generic;
4330 }
4331
4332 ret = ret && ok;
4333 }
4334
4335 allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(modes.data(), remappedControlBoards);
4336
4337 return ret;
4338}
4339
4341{
4342 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4343 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
4344
4345 RemappedSubControlBoard *s=remappedControlBoards.getSubControlBoard(subIndex);
4346
4347 if (!s)
4348 {
4349 return ReturnValue::return_code::return_value_error_generic;
4350 }
4351
4352 if (s->iInteract)
4353 {
4354 return s->iInteract->setInteractionMode(off, mode);
4355 }
4356
4357 return ReturnValue::return_code::return_value_error_generic;
4358}
4359
4360ReturnValue ControlBoardRemapper::setInteractionModes(const std::vector<int>& joints, const std::vector<yarp::dev::InteractionModeEnum>& modes)
4361{
4363 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
4364
4365 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(modes.data(), joints.size(), joints.data(), remappedControlBoards);
4366
4367 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4368 {
4369 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4370
4373 ret = ret && ok;
4374 }
4375
4376 return ret;
4377}
4378
4379ReturnValue ControlBoardRemapper::setInteractionModes(const std::vector<yarp::dev::InteractionModeEnum>& modes)
4380{
4382 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4383
4384 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(modes.data(),remappedControlBoards);
4385
4386 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4387 {
4388 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4389
4391 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
4393 ret = ret && ok;
4394 }
4395
4396 return ret;
4397}
4398
4400{
4401 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4402
4403 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4404
4405 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4406
4407 if (p && p->iPwm)
4408 {
4409 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4410 ret = p->iPwm->setRefDutyCycle(off, ref);
4411 }
4412
4413 return ret;
4414}
4415
4417{
4419
4420 for(int l=0;l<controlledJoints;l++)
4421 {
4422 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4423 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4424
4425 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4426
4427 if (!p)
4428 {
4429 return ReturnValue::return_code::return_value_error_generic;
4430 }
4431
4432 if (p->iPwm)
4433 {
4434 ReturnValue ok = p->iPwm->setRefDutyCycle(off, refs[l]);
4435 ret = ret && ok;
4436 }
4437 else
4438 {
4439 ret=ReturnValue::return_code::return_value_error_generic;
4440 }
4441 }
4442
4443 return ret;
4444}
4445
4447{
4448 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;;
4449
4450 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4451
4452 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4453
4454 if (p && p->iPwm)
4455 {
4456 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4457 ret = p->iPwm->getRefDutyCycle(off, ref);
4458 }
4459
4460 return ret;
4461}
4462
4464{
4466
4467 for(int l=0;l<controlledJoints;l++)
4468 {
4469 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4470 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4471
4472 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4473
4474 if (!p)
4475 {
4476 return ReturnValue::return_code::return_value_error_generic;
4477 }
4478
4479 if (p->iPwm)
4480 {
4481 ReturnValue ok = p->iPwm->getRefDutyCycle(off, refs+l);
4482 ret = ret && ok;
4483 }
4484 else
4485 {
4486 ret=ReturnValue::return_code::return_value_error_generic;
4487 }
4488 }
4489
4490 return ret;
4491}
4492
4494{
4495 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4496
4497 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4498
4499 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4500
4501 if (p && p->iPwm)
4502 {
4503 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4504 ret = p->iPwm->getDutyCycle(off, val);
4505 }
4506
4507 return ret;
4508}
4509
4511{
4513
4514 for(int l=0;l<controlledJoints;l++)
4515 {
4516 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4517 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4518
4519 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4520
4521 if (!p)
4522 {
4523 return ReturnValue::return_code::return_value_error_generic;
4524 }
4525
4526 if (p->iPwm)
4527 {
4528 ReturnValue ok = p->iPwm->getDutyCycle(off, vals+l);
4529 ret = ret && ok;
4530 }
4531 else
4532 {
4533 ret=ReturnValue::return_code::return_value_error_generic;
4534 }
4535 }
4536
4537 return ret;
4538}
4539
4541{
4542 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4543
4544 size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
4545
4546 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4547
4548 if (p && p->iCurr)
4549 {
4550 int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4551 ret = p->iCurr->getCurrent(off, val);
4552 }
4553
4554 return ret;
4555}
4556
4558{
4560
4561 for(int l=0;l<controlledJoints;l++)
4562 {
4563 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4564 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4565
4566 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4567
4568 if (!p)
4569 {
4570 return ReturnValue::return_code::return_value_error_generic;
4571 }
4572
4573 if (p->iCurr)
4574 {
4575 ReturnValue ok = p->iCurr->getCurrent(off, vals+l);
4576 ret = ret && ok;
4577 }
4578 else
4579 {
4580 ret=ReturnValue::return_code::return_value_error_generic;
4581 }
4582 }
4583
4584 return ret;
4585}
4586
4588{
4589 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4590
4591 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4592
4593 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4594
4595 if (p && p->iCurr)
4596 {
4597 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4598 ret = p->iCurr->getCurrentRange(off, min, max);
4599 }
4600
4601 return ret;
4602}
4603
4605{
4607
4608 for(int l=0;l<controlledJoints;l++)
4609 {
4610 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4611 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4612
4613 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4614
4615 if (!p)
4616 {
4617 return ReturnValue::return_code::return_value_error_generic;
4618 }
4619
4620 if (p->iCurr)
4621 {
4622 ReturnValue ok = p->iCurr->getCurrentRange(off, min+l, max+l);
4623 ret = ret && ok;
4624 }
4625 else
4626 {
4627 ret=ReturnValue::return_code::return_value_error_generic;
4628 }
4629 }
4630
4631 return ret;
4632}
4633
4635{
4636 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4637
4638 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4639
4640 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4641
4642 if (p && p->iCurr)
4643 {
4644 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4645 ret = p->iCurr->setRefCurrent(off, curr);
4646 }
4647
4648 return ret;
4649}
4650
4651ReturnValue ControlBoardRemapper::setRefCurrents(const int n_motor, const int* motors, const double* currs)
4652{
4654 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
4655
4656 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(currs,n_motor,motors,remappedControlBoards);
4657
4658 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4659 {
4660 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4661
4662 if (!(p && p->iCurr))
4663 {
4664 ret = ReturnValue::return_code::return_value_error_generic;
4665 break;
4666 }
4667
4668 ReturnValue ok = p->iCurr->setRefCurrents(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
4669 selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
4670 selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
4671 ret = ret && ok;
4672 }
4673
4674 return ret;
4675}
4676
4678{
4680 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4681
4682 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(currs,remappedControlBoards);
4683
4684 for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4685 {
4686 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
4687
4689 allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
4690 allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
4691 ret = ret && ok;
4692 }
4693
4694 return ret;
4695}
4696
4698{
4699 ReturnValue ret = ReturnValue::return_code::return_value_error_generic;
4700
4701 size_t subIndex=remappedControlBoards.lut[m].subControlBoardIndex;
4702
4703 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4704
4705 if (p && p->iCurr)
4706 {
4707 int off=(int)remappedControlBoards.lut[m].axisIndexInSubControlBoard;
4708 ret = p->iCurr->getRefCurrent(off, curr);
4709 }
4710
4711 return ret;
4712}
4713
4715{
4717
4718 for(int l=0;l<controlledJoints;l++)
4719 {
4720 int off=(int)remappedControlBoards.lut[l].axisIndexInSubControlBoard;
4721 size_t subIndex=remappedControlBoards.lut[l].subControlBoardIndex;
4722
4723 RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
4724
4725 if (!p)
4726 {
4727 return ReturnValue::return_code::return_value_error_generic;
4728 }
4729
4730 if (p->iCurr)
4731 {
4732 ReturnValue ok = p->iCurr->getRefCurrent(off, currs+l);
4733 ret = ret && ok;
4734 }
4735 else
4736 {
4737 ret=ReturnValue::return_code::return_value_error_generic;
4738 }
4739 }
4740
4741 return ret;
4742}
4743
4744// IJointBraked interface
4745
4746
4748{
4749 ReturnValue ret = ReturnValue::return_code::return_value_error_not_ready;
4750
4751 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
4752 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4753
4754 if (p && p->iBrake) {
4755 int off = (int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4756 ret = p->iBrake->isJointBraked(off, braked);
4757 }
4758
4759 return ret;
4760}
4761
4763{
4764 ReturnValue ret = ReturnValue::return_code::return_value_error_not_ready;
4765
4766 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
4767 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4768
4769 if (p && p->iBrake) {
4770 int off = (int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4771 ret = p->iBrake->setManualBrakeActive(off, active);
4772 }
4773
4774 return ret;
4775}
4776
4778{
4779 ReturnValue ret = ReturnValue::return_code::return_value_error_not_ready;
4780
4781 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
4782 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4783
4784 if (p && p->iBrake) {
4785 int off = (int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4786 ret = p->iBrake->setAutoBrakeEnabled(off, enabled);
4787 }
4788
4789 return ret;
4790}
4791
4793{
4794 ReturnValue ret = ReturnValue::return_code::return_value_error_not_ready;
4795
4796 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
4797 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4798
4799 if (p && p->iBrake) {
4800 int off = (int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
4801 ret = p->iBrake->getAutoBrakeEnabled(off, enabled);
4802 }
4803
4804 return ret;
4805}
4806
4807// IVelocityDirect interface
4808
4810{
4811 int off = static_cast<int>(remappedControlBoards.lut[jnt].axisIndexInSubControlBoard);
4812 size_t subIndex = remappedControlBoards.lut[jnt].subControlBoardIndex;
4813 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4814
4815 if (!p || !p->iVelDir)
4816 {
4817 return ReturnValue::return_code::return_value_error_generic;
4818 }
4819 return p->iVelDir->setRefVelocity(off, vel);
4820}
4821
4823{
4824 if (vels.size() != static_cast<size_t>(controlledJoints))
4825 {
4826 return ReturnValue::return_code::return_value_error_method_failed;
4827 }
4829 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4830 allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(vels.data(), remappedControlBoards);
4831
4832 for (size_t ctrlBrd = 0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4833 {
4834 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(ctrlBrd);
4835 if (!p || !p->iVelDir)
4836 {
4837 ret = ReturnValue::return_code::return_value_error_generic;
4838 continue;
4839 }
4840 std::vector<double> subVels = allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd];
4841 ret = ret && p->iVelDir->setRefVelocity(subVels);
4842 }
4843 return ret;
4844}
4845
4846yarp::dev::ReturnValue ControlBoardRemapper::setRefVelocity(const std::vector<int>& jnts, const std::vector<double>& vels)
4847{
4848 if (jnts.size() != vels.size())
4849 {
4850 return ReturnValue::return_code::return_value_error_method_failed;
4851 }
4853 std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
4854 selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(vels.data(), static_cast<int>(jnts.size()), jnts.data(), remappedControlBoards);
4855
4856 for (size_t ctrlBrd = 0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
4857 {
4858 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(ctrlBrd);
4859 if (!p || !p->iVelDir)
4860 {
4861 ret = ReturnValue::return_code::return_value_error_generic;
4862 continue;
4863 }
4864 std::vector<int> subJnts = selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd];
4865 std::vector<double> subVels = selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd];
4866 ret = ret && p->iVelDir->setRefVelocity(subJnts, subVels);
4867 }
4868 return ret;
4869}
4870
4872{
4873 int off = static_cast<int>(remappedControlBoards.lut[jnt].axisIndexInSubControlBoard);
4874 size_t subIndex = remappedControlBoards.lut[jnt].subControlBoardIndex;
4875 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4876
4877 if (!p || !p->iVelDir)
4878 {
4879 return ReturnValue::return_code::return_value_error_generic;
4880 }
4881 return p->iVelDir->getRefVelocity(off, vel);
4882}
4883
4885{
4886 vels.resize(controlledJoints);
4888 std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
4889
4890 for (int l = 0; l < controlledJoints; l++)
4891 {
4892 int off = static_cast<int>(remappedControlBoards.lut[l].axisIndexInSubControlBoard);
4893 size_t subIndex = remappedControlBoards.lut[l].subControlBoardIndex;
4894 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4895 if (!p || !p->iVelDir)
4896 {
4897 ret = ReturnValue::return_code::return_value_error_generic;
4898 continue;
4899 }
4900 ret = ret && p->iVelDir->getRefVelocity(off, vels[l]);
4901 }
4902 return ret;
4903}
4904
4905yarp::dev::ReturnValue ControlBoardRemapper::getRefVelocity(const std::vector<int>& jnts, std::vector<double>& vels)
4906{
4907 vels.resize(jnts.size());
4909
4910 for (size_t i = 0; i < jnts.size(); ++i)
4911 {
4912 int j = jnts[i];
4913 int off = static_cast<int>(remappedControlBoards.lut[j].axisIndexInSubControlBoard);
4914 size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
4915 RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
4916 if (!p || !p->iVelDir)
4917 {
4918 ret = ReturnValue::return_code::return_value_error_generic;
4919 continue;
4920 }
4921 ret = ret && p->iVelDir->getRefVelocity(off, vels[i]);
4922 }
4923 return ret;
4924}
const yarp::os::LogComponent & CONTROLBOARDREMAPPER()
#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 getPidReferences(const yarp::dev::PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all PID controllers.
yarp::dev::ReturnValue getTargetVelocities(double *vels) override
Get the last reference speed set by velocityMove for all joints.
yarp::dev::ReturnValue getTemperatures(double *vals) override
Get temperature of all the motors.
yarp::dev::ReturnValue homingSingleJoint(int j) override
homingSingleJoint: call the homing procedure for a single joint
yarp::dev::ReturnValue isCalibratorDevicePresent(bool *isCalib) override
isCalibratorDevicePresent: check if a calibrator device has been set
yarp::dev::ReturnValue getTargetPositions(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue quitCalibrate() override
quitCalibrate: interrupt the calibration procedure
yarp::dev::ReturnValue getPWM(int m, double *val) override
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 setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
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 getTrajSpeed(int j, double *ref) override
Get reference speed for a joint.
yarp::dev::ReturnValue homingWholePart() override
homingWholePart: call the homing procedure for a the whole part/device
yarp::dev::ReturnValue setTemperatureLimit(int m, const double val) override
Set the temperature limit for a specific motor.
yarp::dev::ReturnValue stop() override
Stop motion, multiple joints.
yarp::dev::ReturnValue setPosition(int j, double ref) override
Set new position for a single axis.
yarp::dev::ReturnValue checkMotionDone(int j, bool &flag) override
Check if the current trajectory is terminated.
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 setRefTorque(int j, double t) override
Set the reference value of the torque for a given joint.
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 getMotorEncoderAcceleration(int m, double *acc) override
Read the instantaneous acceleration of a motor encoder.
yarp::dev::ReturnValue getMotorEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all motor encoders.
bool close() override
Close the device driver by deallocating all resources and closing ports.
yarp::dev::ReturnValue getEncodersTimed(double *encs, double *t) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getPWMLimit(int m, double *val) override
yarp::dev::ReturnValue getNominalCurrent(int m, double *val) override
yarp::dev::ReturnValue getAmpStatus(int *st) override
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 getNumberOfMotorEncoders(int *num) override
Get the number of available motor encoders.
yarp::dev::ReturnValue getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
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 setPeakCurrent(int m, const double val) override
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 quitPark() override
quitPark: interrupt the park procedure
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 setPidReference(const yarp::dev::PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
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 getPidOutput(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
yarp::dev::ReturnValue setRefCurrent(int m, double curr) override
Set the reference value of the current for a single motor.
yarp::dev::ReturnValue getGearboxRatio(int m, double *val) override
Get the gearbox ratio for a specific motor.
yarp::dev::ReturnValue positionMove(int j, double ref) override
Set new reference point for a single axis.
yarp::dev::ReturnValue getAvailablePids(int j, std::vector< yarp::dev::PidControlTypeEnum > &avail) override
ControlBoard methods.
yarp::dev::ReturnValue getRemoteVariable(std::string key, yarp::os::Bottle &val) override
yarp::dev::ReturnValue getRefDutyCycles(double *refs) override
Gets the last reference sent using the setRefDutyCycles function.
yarp::dev::ReturnValue velocityMove(int j, double v) override
Start motion at a given speed, single joint.
yarp::dev::ReturnValue getEncoders(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue setMotorEncoder(int m, const double val) override
Set the value of the motor encoder for a given motor.
yarp::dev::ReturnValue getNumberOfMotors(int *num) override
Retrieves the number of controlled motors from the current physical interface.
yarp::dev::ReturnValue getMotorEncoder(int m, double *v) override
Read the value of a motor encoder.
yarp::dev::ReturnValue parkWholePart() override
parkWholePart: start the parking procedure for the whole part
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 abortCalibration() override
yarp::dev::ReturnValue abortPark() override
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 disablePid(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Disable the PID computation for a joint.
yarp::dev::ReturnValue getRefDutyCycle(int m, double *ref) override
Gets the last reference sent using the setRefDutyCycle function.
yarp::dev::ReturnValue getPidError(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
yarp::dev::ReturnValue getPidErrorLimits(const yarp::dev::PidControlTypeEnum &pidtype, double *limits) override
Get the error limit for all controllers.
yarp::dev::ReturnValue setRefCurrents(const double *currs) override
Set the reference value of the currents for all motors.
yarp::dev::ReturnValue getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
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 calibrationDone(int j) override
Check if the calibration is terminated, on a particular joint.
yarp::dev::ReturnValue getMotorEncoderSpeeds(double *spds) override
Read the instantaneous speed of all motor encoders.
yarp::dev::ReturnValue getAxisName(int j, std::string &name) override
yarp::dev::ReturnValue setCalibrationParameters(int j, const yarp::dev::CalibrationParameters &params) override
Start calibration, this method is very often platform specific.
yarp::dev::ReturnValue resetMotorEncoders() override
Reset motor encoders.
yarp::dev::ReturnValue getJointType(int j, yarp::dev::JointTypeEnum &type) override
yarp::dev::ReturnValue getTorqueRanges(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
yarp::dev::ReturnValue getCurrents(double *currs) override
Get the instantaneous current measurement for all motors.
yarp::dev::ReturnValue enablePid(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Enable the PID computation for a joint.
yarp::dev::ReturnValue setImpedanceOffset(int j, double offset) override
Set current force Offset for a specific joint.
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 setPid(const yarp::dev::PidControlTypeEnum &pidtype, int j, const yarp::dev::Pid &p) override
Set new PID value for a joint axis.
bool open(yarp::os::Searchable &prop) override
Open the device driver.
yarp::dev::ReturnValue setTrajAcceleration(int j, double acc) override
Set reference acceleration for a joint.
yarp::dev::IRemoteCalibrator * getCalibratorDevice() override
getCalibratorDevice: return the pointer stored with the setCalibratorDevice
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 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 getMotorEncodersTimed(double *encs, double *t) override
Read the instantaneous position of all motor encoders.
yarp::dev::ReturnValue enableAmp(int j) override
Enable the amplifier on a specific joint.
yarp::dev::ReturnValue getPidErrors(const yarp::dev::PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
yarp::dev::ReturnValue setNominalCurrent(int m, const double val) override
yarp::dev::ReturnValue setControlMode(int j, yarp::dev::SelectableControlModeEnum mode) override
Set the current control mode.
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 isJointBraked(int j, bool &braked) const override
Check is the joint brake is currently active.
yarp::dev::ReturnValue calibrateSingleJoint(int j) override
calibrateSingleJoint: call the calibration procedure for the single joint
yarp::dev::ReturnValue getPowerSupplyVoltage(int m, double *val) override
yarp::dev::ReturnValue getTrajAccelerations(double *accs) override
Get reference acceleration of all joints.
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 setImpedance(int j, double stiff, double damp) override
Set current impedance gains (stiffness,damping) for a specific joint.
bool detachAll() override
Detach the object (you must have first called attach).
yarp::dev::ReturnValue getControlMode(int j, yarp::dev::ControlModeEnum &mode) override
Get the current control mode.
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.
yarp::dev::ReturnValue relativeMove(int j, double delta) override
Set relative position.
yarp::dev::ReturnValue getVelLimits(int j, double *min, double *max) override
Get the software speed limits for a particular axis.
yarp::dev::ReturnValue resetEncoders() override
Reset encoders.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue resetMotorEncoder(int m) override
Reset motor encoder, single motor.
yarp::dev::ReturnValue getTargetPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getRefCurrents(double *currs) override
Get the reference value of the currents for all motors.
yarp::dev::ReturnValue getMotorEncoderSpeed(int m, double *sp) override
Read the instantaneous speed of a motor encoder.
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 getImpedance(int j, double *stiff, double *damp) override
Get current impedance gains (stiffness,damping,offset) for a specific joint.
yarp::dev::ReturnValue getEncoderAcceleration(int j, double *acc) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue setRefTorques(const double *t) override
Set the reference value of the torque for all joints.
yarp::dev::ReturnValue setPWMLimit(int m, const double val) override
yarp::os::Stamp getLastInputStamp() override
Return the time stamp relative to the last acquisition.
yarp::dev::ReturnValue getEncoder(int j, double *v) override
Read the value of an encoder.
yarp::dev::ReturnValue setGearboxRatio(int m, const double val) override
Set the gearbox ratio for a specific motor.
yarp::dev::ReturnValue getDutyCycles(double *vals) override
Gets the current dutycycle of the output of the amplifier (i.e.
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 setTrajAccelerations(const double *accs) override
Set reference acceleration on all joints.
yarp::dev::ReturnValue parkSingleJoint(int j, bool _wait=true) override
parkSingleJoint(): start the parking procedure for the single joint
yarp::dev::ReturnValue setMotorEncoders(const double *vals) override
Set the value of all motor encoders.
yarp::dev::ReturnValue getTemperatureLimit(int m, double *val) override
Retrieves the current temperature limit for a specific motor.
yarp::dev::ReturnValue getCurrent(int m, double *curr) override
Get the instantaneous current measurement for a single motor.
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 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.
bool attachAll(const yarp::dev::PolyDriverList &l) override
Attach to a list of objects.
yarp::dev::ReturnValue disableAmp(int j) override
Disable the amplifier on a specific joint.
yarp::dev::ReturnValue getMotorEncoderCountsPerRevolution(int m, double *cpr) override
Gets number of counts per revolution for motor encoder m.
yarp::dev::ReturnValue resetEncoder(int j) override
Reset encoder, single joint.
yarp::dev::ReturnValue getAutoBrakeEnabled(int j, bool &enabled) const override
checks if the automatic joint brake mode is enabled or disabled.
yarp::dev::ReturnValue getPeakCurrent(int m, double *val) override
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 getTorqueRange(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
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 getCurrentRanges(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
yarp::dev::ReturnValue setAutoBrakeEnabled(int j, bool enabled) override
Enables/Disables the automatic joint brake.
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 getMotorEncoderTimed(int m, double *v, double *t) override
Read the instantaneous position of a motor encoder.
yarp::dev::ReturnValue setRefDutyCycles(const double *refs) override
Sets the reference dutycycle for all the motors.
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 setPids(const yarp::dev::PidControlTypeEnum &pidtype, const yarp::dev::Pid *ps) override
Set new PID value on multiple axes.
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 getTemperature(int m, double *val) override
Get temperature of a motor.
yarp::dev::ReturnValue getRefPositions(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue setRemoteVariable(std::string key, const yarp::os::Bottle &val) override
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 setMaxCurrent(int j, double v) override
yarp::dev::ReturnValue getMotorEncoders(double *encs) override
Read the position of all motor encoders.
yarp::dev::ReturnValue getMaxCurrent(int j, double *v) override
Returns the maximum electric current allowed for a given motor.
yarp::dev::ReturnValue getEncoderTimed(int j, double *v, double *t) override
Read the instantaneous acceleration of all 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 getRefPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getPidOutputs(const yarp::dev::PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
yarp::dev::ReturnValue getLastJointFault(int j, int &fault, std::string &message) override
yarp::dev::ReturnValue getDutyCycle(int m, double *val) override
Gets the current dutycycle of the output of the amplifier (i.e.
yarp::dev::ReturnValue setEncoders(const double *vals) override
Set the value of all encoders.
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 getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
yarp::dev::ReturnValue setTrajSpeeds(const double *spds) override
Set reference speed on all joints.
yarp::dev::ReturnValue getRemoteVariablesList(yarp::os::Bottle *listOfKeys) override
yarp::dev::ReturnValue setPidReferences(const yarp::dev::PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
yarp::dev::ReturnValue setRefDutyCycle(int m, double ref) override
Sets the reference dutycycle to a single motor.
yarp::dev::ReturnValue setRefVelocity(int jnt, double vel) override
Set the velocity of single 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 setMotorEncoderCountsPerRevolution(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
yarp::dev::ReturnValue getTrajSpeeds(double *spds) override
Get reference speed of all joints.
yarp::dev::ReturnValue calibrateWholePart() override
calibrateWholePart: call the procedure for calibrating the whole device
yarp::dev::ReturnValue getRefTorque(int j, double *t) override
Get the reference value of the torque for a given joint.
yarp::dev::ReturnValue getRefCurrent(int m, double *curr) override
Get the reference value of the current for a single motor.
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 getPosLimits(int j, double *min, double *max) override
Get the software limits for a particular axis.
yarp::dev::ReturnValue getTrajAcceleration(int j, double *acc) override
Get reference acceleration for a joint.
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 getRefTorques(double *refs) override
Get the reference value of the torque for all joints.
yarp::dev::ReturnValue setManualBrakeActive(int j, bool active) override
Enables/Disables manually the joint brake.
yarp::dev::ReturnValue getImpedanceOffset(int j, double *offset) override
Get current force Offset for a specific joint.
yarp::dev::ReturnValue getTargetVelocity(const int joint, double *vel) override
Get the last reference speed set by velocityMove for single joint.
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:65
void append(const Bottle &alt)
Append the content of the given bottle to the current list.
Definition Bottle.cpp:359
size_type size() const
Gets the number of elements in the bottle.
Definition Bottle.cpp:257
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:252
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition Bottle.cpp:217
A mini-server for performing network communication in the background.
A class for storing options and configuration information.
Definition Property.h:33
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Bottle & findGroup(const std::string &key) const override
Gets a list corresponding to a given keyword.
A base class for nested structures that can be searched.
Definition Searchable.h:31
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition Value.cpp:210
virtual Bottle * asList() const
Get list value.
Definition Value.cpp:252
virtual std::string asString() const
Get string value.
Definition Value.cpp:246
#define yCInfo(component,...)
#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.
Definition OSTestMsg.cpp:13
size_t indexOfSubDeviceInPolyDriverList