YARP
Yet Another Robot Platform
RemoteControlBoard.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
10
11#include <cstring>
12
15#include <yarp/os/Time.h>
16#include <yarp/os/Network.h>
18#include <yarp/os/Vocab.h>
19#include <yarp/os/Stamp.h>
20#include <yarp/os/LogStream.h>
21#include <yarp/os/QosStyle.h>
22
23
25#include <yarp/dev/PolyDriver.h>
29
30#include <mutex>
31
32
33using namespace yarp::os;
34using namespace yarp::dev;
35using namespace yarp::sig;
36
37namespace {
38
39constexpr int PROTOCOL_VERSION_MAJOR = 1;
40constexpr int PROTOCOL_VERSION_MINOR = 9;
41constexpr int PROTOCOL_VERSION_TWEAK = 0;
42
43constexpr double DIAGNOSTIC_THREAD_PERIOD = 1.000;
44
45inline bool getTimeStamp(Bottle &bot, Stamp &st)
46{
47 if (bot.get(3).asVocab32()==VOCAB_TIMESTAMP)
48 {
49 //yup! we have a timestamp
50 int fr=bot.get(4).asInt32();
51 double ts=bot.get(5).asFloat64();
52 st=Stamp(fr,ts);
53 return true;
54 }
55 return false;
56}
57
58} // namespace
59
60
62 public PeriodicThread
63{
64 StateExtendedInputPort *owner{nullptr};
65 std::string ownerName;
66
67public:
68 using PeriodicThread::PeriodicThread;
69
71 {
72 owner = o;
73 ownerName = owner->getName();
74 }
75
76 void run() override
77 {
78 if (owner != nullptr)
79 {
80 if (owner->getIterations() > 100)
81 {
82 int it;
83 double av;
84 double max;
85 double min;
86 owner->getEstFrequency(it, av, min, max);
87 owner->resetStat();
89 "%s: %d msgs av:%.2lf min:%.2lf max:%.2lf [ms]",
90 ownerName.c_str(),
91 it,
92 av,
93 min,
94 max);
95 }
96
97 }
98 }
99};
100
101
103{
104 if (!njIsKnown) {
105 int axes = 0;
106 bool ok = get1V1I(VOCAB_AXES, axes);
107 if (axes >= 0 && ok) {
108 nj = axes;
109 njIsKnown = true;
110 }
111 }
112 return njIsKnown;
113}
114
115
117{
118 bool error=false;
119 // verify protocol
120 Bottle cmd, reply;
123 rpc_p.write(cmd, reply);
124
125 // check size and format of messages, expected [prot] int int int [ok]
126 if (reply.size() != 5) {
127 error = true;
128 }
129
130 if (reply.get(0).asVocab32() != VOCAB_PROTOCOL_VERSION) {
131 error = true;
132 }
133
134 if (!error)
135 {
136 protocolVersion.major=reply.get(1).asInt32();
137 protocolVersion.minor=reply.get(2).asInt32();
138 protocolVersion.tweak=reply.get(3).asInt32();
139
140 //verify protocol
142 error = true;
143 }
144
146 error = true;
147 }
148 }
149
150 if (!error) {
151 return true;
152 }
153
154 // protocol did not match
156 "Expecting protocol %d %d %d, but the device we are connecting to has protocol version %d %d %d",
163
164 bool ret;
165 if (ignore)
166 {
167 yCWarning(REMOTECONTROLBOARD, "Ignoring error but please update YARP or the remotecontrolboard implementation");
168 ret = true;
169 }
170 else
171 {
172 yCError(REMOTECONTROLBOARD, "Please update YARP or the remotecontrolboard implementation");
173 ret = false;
174 }
175
176 return ret;
177}
178
180{
181 remote = config.find("remote").asString();
182 local = config.find("local").asString();
183
184 if (config.check("timeout"))
185 {
187 }
188 // check the Qos perefernces if available (local and remote)
189 yarp::os::QosStyle localQos;
190 if (config.check("local_qos")) {
191 Bottle& qos = config.findGroup("local_qos");
192 if (qos.check("thread_priority")) {
193 localQos.setThreadPriority(qos.find("thread_priority").asInt32());
194 }
195 if (qos.check("thread_policy")) {
196 localQos.setThreadPolicy(qos.find("thread_policy").asInt32());
197 }
198 if (qos.check("packet_priority")) {
199 localQos.setPacketPriority(qos.find("packet_priority").asString());
200 }
201 }
202
203 yarp::os::QosStyle remoteQos;
204 if (config.check("remote_qos")) {
205 Bottle& qos = config.findGroup("remote_qos");
206 if (qos.check("thread_priority")) {
207 remoteQos.setThreadPriority(qos.find("thread_priority").asInt32());
208 }
209 if (qos.check("thread_policy")) {
210 remoteQos.setThreadPolicy(qos.find("thread_policy").asInt32());
211 }
212 if (qos.check("packet_priority")) {
213 remoteQos.setPacketPriority(qos.find("packet_priority").asString());
214 }
215 }
216
217 bool writeStrict_isFound = config.check("writeStrict");
218 if(writeStrict_isFound)
219 {
220 Value &gotStrictVal = config.find("writeStrict");
221 if(gotStrictVal.asString() == "on")
222 {
225 yCInfo(REMOTECONTROLBOARD, "RemoteControlBoard is ENABLING the writeStrict option for all commands");
226 }
227 else if(gotStrictVal.asString() == "off")
228 {
231 yCInfo(REMOTECONTROLBOARD, "RemoteControlBoard is DISABLING the writeStrict opition for all commands");
232 } else {
233 yCError(REMOTECONTROLBOARD, "Found writeStrict opition with wrong value. Accepted options are 'on' or 'off'");
234 }
235 }
236
237 if (local=="") {
238 yCError(REMOTECONTROLBOARD, "Problem connecting to remote controlboard, 'local' port prefix not given");
239 return false;
240 }
241
242 if (remote=="") {
243 yCError(REMOTECONTROLBOARD, "Problem connecting to remote controlboard, 'remote' port name not given");
244 return false;
245 }
246
247 std::string carrier =
248 config.check("carrier",
249 Value("udp"),
250 "default carrier for streaming robot state").asString();
251
252 bool portProblem = false;
253 if (local != "") {
254 std::string s1 = local;
255 s1 += "/rpc:o";
256 if (!rpc_p.open(s1)) { portProblem = true; }
257 s1 = local;
258 s1 += "/command:o";
259 if (!command_p.open(s1)) { portProblem = true; }
260 s1 = local;
261 s1 += "/stateExt:i";
262 if (!extendedIntputStatePort.open(s1)) { portProblem = true; }
263 if (!portProblem)
264 {
266 }
267 }
268
269 bool connectionProblem = false;
270 if (remote != "" && !portProblem)
271 {
272 std::string s1 = remote;
273 s1 += "/rpc:i";
274 std::string s2 = local;
275 s2 += "/rpc:o";
276 bool ok = false;
277 // RPC port needs to be tcp, therefore no carrier option is added here
278 // ok=Network::connect(s2.c_str(), s1.c_str()); //This doesn't take into consideration possible YARP_PORT_PREFIX on local ports
279 // ok=Network::connect(rpc_p.getName(), s1.c_str()); //This should work also with YARP_PORT_PREFIX because getting back the name of the port will return the modified name
280 ok=rpc_p.addOutput(s1); //This works because we are manipulating only remote side and let yarp handle the local side
281 if (!ok) {
282 yCError(REMOTECONTROLBOARD, "Problem connecting to %s, is the remote device available?", s1.c_str());
283 connectionProblem = true;
284 }
285
286 s1 = remote;
287 s1 += "/command:i";
288 s2 = local;
289 s2 += "/command:o";
290 //ok = Network::connect(s2.c_str(), s1.c_str(), carrier);
291 // ok=Network::connect(command_p.getName(), s1.c_str(), carrier); //doesn't take into consideration possible YARP_PORT_PREFIX on local ports
292 ok = command_p.addOutput(s1, carrier);
293 if (!ok) {
294 yCError(REMOTECONTROLBOARD, "Problem connecting to %s, is the remote device available?", s1.c_str());
295 connectionProblem = true;
296 }
297 // set the QoS preferences for the 'command' port
298 if (config.check("local_qos") || config.check("remote_qos")) {
299 NetworkBase::setConnectionQos(command_p.getName(), s1, localQos, remoteQos, false);
300 }
301
302 s1 = remote;
303 s1 += "/stateExt:o";
304 s2 = local;
305 s2 += "/stateExt:i";
306 // not checking return value for now since it is wip (different machines can have different compilation flags
307 ok = Network::connect(s1, extendedIntputStatePort.getName(), carrier);
308 if (ok)
309 {
310 // set the QoS preferences for the 'state' port
311 if (config.check("local_qos") || config.check("remote_qos")) {
312 NetworkBase::setConnectionQos(s1, extendedIntputStatePort.getName(), remoteQos, localQos, false);
313 }
314 }
315 else
316 {
317 yCError(REMOTECONTROLBOARD, "Problem connecting to %s, is the remote device available?", s1.c_str());
318 connectionProblem = true;
319 }
320 }
321
322 if (connectionProblem||portProblem) {
323
324 rpc_p.close();
327 return false;
328 }
329
330 state_buffer.setStrict(false);
332
333 if (!checkProtocolVersion(config.check("ignoreProtocolCheck")))
334 {
335 yCError(REMOTECONTROLBOARD) << "checkProtocolVersion failed";
336 command_buffer.detach();
337 rpc_p.close();
340 return false;
341 }
342
343 if (!isLive()) {
344 if (remote!="") {
345 yCError(REMOTECONTROLBOARD, "Problems with obtaining the number of controlled axes");
346 command_buffer.detach();
347 rpc_p.close();
350 return false;
351 }
352 }
353
354 if (config.check("diagnostic"))
355 {
356 diagnosticThread = new DiagnosticThread(DIAGNOSTIC_THREAD_PERIOD);
359 } else {
360 diagnosticThread = nullptr;
361 }
362
363 // allocate memory for helper struct
364 // single joint
376
377 // whole part (safe here because we already got the nj
389 return true;
390}
391
393{
394 if (diagnosticThread!=nullptr)
395 {
397 delete diagnosticThread;
398 }
399
403
404 rpc_p.close();
407 return true;
408}
409
410// BEGIN Helpers functions
411
413{
414 Bottle cmd, response;
415 cmd.addVocab32(v);
416 bool ok=rpc_p.write(cmd, response);
417 if (CHECK_FAIL(ok, response)) {
418 return true;
419 }
420 return false;
421}
422
423bool RemoteControlBoard::send2V(int v1, int v2)
424{
425 Bottle cmd, response;
426 cmd.addVocab32(v1);
427 cmd.addVocab32(v2);
428 bool ok=rpc_p.write(cmd, response);
429 if (CHECK_FAIL(ok, response)) {
430 return true;
431 }
432 return false;
433}
434
435bool RemoteControlBoard::send2V1I(int v1, int v2, int axis)
436{
437 Bottle cmd, response;
438 cmd.addVocab32(v1);
439 cmd.addVocab32(v2);
440 cmd.addInt32(axis);
441 bool ok=rpc_p.write(cmd, response);
442 if (CHECK_FAIL(ok, response)) {
443 return true;
444 }
445 return false;
446}
447
448bool RemoteControlBoard::send1V1I(int v, int axis)
449{
450 Bottle cmd, response;
451 cmd.addVocab32(v);
452 cmd.addInt32(axis);
453 bool ok=rpc_p.write(cmd, response);
454 if (CHECK_FAIL(ok, response)) {
455 return true;
456 }
457 return false;
458}
459
460bool RemoteControlBoard::send3V1I(int v1, int v2, int v3, int j)
461{
462 Bottle cmd, response;
463 cmd.addVocab32(v1);
464 cmd.addVocab32(v2);
465 cmd.addVocab32(v3);
466 cmd.addInt32(j);
467 bool ok=rpc_p.write(cmd, response);
468 if (CHECK_FAIL(ok, response)) {
469 return true;
470 }
471 return false;
472}
473
475{
476 Bottle cmd, response;
478 cmd.addVocab32(code);
479
480 bool ok = rpc_p.write(cmd, response);
481 return CHECK_FAIL(ok, response);
482}
483
484bool RemoteControlBoard::set1V2D(int code, double v)
485{
486 Bottle cmd, response;
488 cmd.addVocab32(code);
489 cmd.addFloat64(v);
490
491 bool ok = rpc_p.write(cmd, response);
492
493 return CHECK_FAIL(ok, response);
494}
495
496bool RemoteControlBoard::set1V1I(int code, int v)
497{
498 Bottle cmd, response;
500 cmd.addVocab32(code);
501 cmd.addInt32(v);
502
503 bool ok = rpc_p.write(cmd, response);
504
505 return CHECK_FAIL(ok, response);
506}
507
508bool RemoteControlBoard::get1V1D(int code, double& v) const
509{
510 Bottle cmd;
511 Bottle response;
513 cmd.addVocab32(code);
514
515 bool ok = rpc_p.write(cmd, response);
516
517 if (CHECK_FAIL(ok, response)) {
518 // response should be [cmd] [name] value
519 v = response.get(2).asFloat64();
520
521 getTimeStamp(response, lastStamp);
522 return true;
523 }
524
525 return false;
526}
527
528bool RemoteControlBoard::get1V1I(int code, int& v) const
529{
530 Bottle cmd;
531 Bottle response;
533 cmd.addVocab32(code);
534
535 bool ok = rpc_p.write(cmd, response);
536
537 if (CHECK_FAIL(ok, response)) {
538 // response should be [cmd] [name] value
539 v = response.get(2).asInt32();
540
541 getTimeStamp(response, lastStamp);
542 return true;
543 }
544
545 return false;
546}
547
548bool RemoteControlBoard::set1V1I1D(int code, int j, double val)
549{
550 Bottle cmd, response;
552 cmd.addVocab32(code);
553 cmd.addInt32(j);
554 cmd.addFloat64(val);
555 bool ok = rpc_p.write(cmd, response);
556 return CHECK_FAIL(ok, response);
557}
558
559bool RemoteControlBoard::set1V1I2D(int code, int j, double val1, double val2)
560{
561 Bottle cmd, response;
563 cmd.addVocab32(code);
564 cmd.addInt32(j);
565 cmd.addFloat64(val1);
566 cmd.addFloat64(val2);
567
568 bool ok = rpc_p.write(cmd, response);
569 return CHECK_FAIL(ok, response);
570}
571
572bool RemoteControlBoard::set1VDA(int v, const double *val)
573{
574 if (!isLive()) {
575 return false;
576 }
577 Bottle cmd, response;
579 cmd.addVocab32(v);
580 Bottle& l = cmd.addList();
581 for (size_t i = 0; i < nj; i++) {
582 l.addFloat64(val[i]);
583 }
584 bool ok = rpc_p.write(cmd, response);
585 return CHECK_FAIL(ok, response);
586}
587
588bool RemoteControlBoard::set2V1DA(int v1, int v2, const double *val)
589{
590 if (!isLive()) {
591 return false;
592 }
593 Bottle cmd, response;
595 cmd.addVocab32(v1);
596 cmd.addVocab32(v2);
597 Bottle& l = cmd.addList();
598 for (size_t i = 0; i < nj; i++) {
599 l.addFloat64(val[i]);
600 }
601 bool ok = rpc_p.write(cmd, response);
602 return CHECK_FAIL(ok, response);
603}
604
605bool RemoteControlBoard::set2V2DA(int v1, int v2, const double *val1, const double *val2)
606{
607 if (!isLive()) {
608 return false;
609 }
610 Bottle cmd, response;
612 cmd.addVocab32(v1);
613 cmd.addVocab32(v2);
614 Bottle& l1 = cmd.addList();
615 for (size_t i = 0; i < nj; i++) {
616 l1.addFloat64(val1[i]);
617 }
618 Bottle& l2 = cmd.addList();
619 for (size_t i = 0; i < nj; i++) {
620 l2.addFloat64(val2[i]);
621 }
622 bool ok = rpc_p.write(cmd, response);
623 return CHECK_FAIL(ok, response);
624}
625
626bool RemoteControlBoard::set1V1I1IA1DA(int v, const int len, const int *val1, const double *val2)
627{
628 if (!isLive()) {
629 return false;
630 }
631 Bottle cmd, response;
633 cmd.addVocab32(v);
634 cmd.addInt32(len);
635 int i;
636 Bottle& l1 = cmd.addList();
637 for (i = 0; i < len; i++) {
638 l1.addInt32(val1[i]);
639 }
640 Bottle& l2 = cmd.addList();
641 for (i = 0; i < len; i++) {
642 l2.addFloat64(val2[i]);
643 }
644 bool ok = rpc_p.write(cmd, response);
645 return CHECK_FAIL(ok, response);
646}
647
648bool RemoteControlBoard::set2V1I1D(int v1, int v2, int axis, double val)
649{
650 Bottle cmd, response;
652 cmd.addVocab32(v1);
653 cmd.addVocab32(v2);
654 cmd.addInt32(axis);
655 cmd.addFloat64(val);
656 bool ok = rpc_p.write(cmd, response);
657 return CHECK_FAIL(ok, response);
658}
659
660bool RemoteControlBoard::setValWithPidType(int voc, PidControlTypeEnum type, int axis, double val)
661{
662 if (!isLive()) {
663 return false;
664 }
665 Bottle cmd, response;
668 cmd.addVocab32(voc);
669 cmd.addVocab32(type);
670 cmd.addInt32(axis);
671 cmd.addFloat64(val);
672 bool ok = rpc_p.write(cmd, response);
673 return CHECK_FAIL(ok, response);
674}
675
676bool RemoteControlBoard::setValWithPidType(int voc, PidControlTypeEnum type, const double* val_arr)
677{
678 if (!isLive()) {
679 return false;
680 }
681 Bottle cmd, response;
684 cmd.addVocab32(voc);
685 cmd.addVocab32(type);
686 Bottle& l = cmd.addList();
687 for (size_t i = 0; i < nj; i++) {
688 l.addFloat64(val_arr[i]);
689 }
690 bool ok = rpc_p.write(cmd, response);
691 return CHECK_FAIL(ok, response);
692}
693
694bool RemoteControlBoard::getValWithPidType(int voc, PidControlTypeEnum type, int j, double *val)
695{
696 Bottle cmd, response;
699 cmd.addVocab32(voc);
700 cmd.addVocab32(type);
701 cmd.addInt32(j);
702 bool ok = rpc_p.write(cmd, response);
703
704 if (CHECK_FAIL(ok, response))
705 {
706 *val = response.get(2).asFloat64();
707 getTimeStamp(response, lastStamp);
708 return true;
709 }
710 return false;
711}
712
714{
715 if (!isLive()) {
716 return false;
717 }
718 Bottle cmd, response;
721 cmd.addVocab32(voc);
722 cmd.addVocab32(type);
723 bool ok = rpc_p.write(cmd, response);
724 if (CHECK_FAIL(ok, response))
725 {
726 Bottle* lp = response.get(2).asList();
727 if (lp == nullptr) {
728 return false;
729 }
730 Bottle& l = *lp;
732 for (size_t i = 0; i < nj; i++) {
733 val[i] = l.get(i).asFloat64();
734 }
735 getTimeStamp(response, lastStamp);
736 return true;
737 }
738 return false;
739}
740
741bool RemoteControlBoard::set2V1I(int v1, int v2, int axis)
742{
743 Bottle cmd, response;
745 cmd.addVocab32(v1);
746 cmd.addVocab32(v2);
747 cmd.addInt32(axis);
748 bool ok = rpc_p.write(cmd, response);
749 return CHECK_FAIL(ok, response);
750}
751
752bool RemoteControlBoard::get1V1I1D(int v, int j, double *val)
753{
754 Bottle cmd, response;
756 cmd.addVocab32(v);
757 cmd.addInt32(j);
758 bool ok = rpc_p.write(cmd, response);
759
760 if (CHECK_FAIL(ok, response)) {
761 // ok
762 *val = response.get(2).asFloat64();
763
764 getTimeStamp(response, lastStamp);
765 return true;
766 }
767 return false;
768}
769
770bool RemoteControlBoard::get1V1I1I(int v, int j, int *val)
771{
772 Bottle cmd, response;
774 cmd.addVocab32(v);
775 cmd.addInt32(j);
776 bool ok = rpc_p.write(cmd, response);
777 if (CHECK_FAIL(ok, response)) {
778 // ok
779 *val = response.get(2).asInt32();
780
781 getTimeStamp(response, lastStamp);
782 return true;
783 }
784 return false;
785}
786
787bool RemoteControlBoard::get2V1I1D(int v1, int v2, int j, double *val)
788{
789 Bottle cmd, response;
791 cmd.addVocab32(v1);
792 cmd.addVocab32(v2);
793 cmd.addInt32(j);
794 bool ok = rpc_p.write(cmd, response);
795
796 if (CHECK_FAIL(ok, response)) {
797 // ok
798 *val = response.get(2).asFloat64();
799
800 getTimeStamp(response, lastStamp);
801 return true;
802 }
803 return false;
804}
805
806bool RemoteControlBoard::get2V1I2D(int v1, int v2, int j, double *val1, double *val2)
807{
808 Bottle cmd, response;
810 cmd.addVocab32(v1);
811 cmd.addVocab32(v2);
812 cmd.addInt32(j);
813 bool ok = rpc_p.write(cmd, response);
814 if (CHECK_FAIL(ok, response)) {
815 // ok
816 *val1 = response.get(2).asFloat64();
817 *val2 = response.get(3).asFloat64();
818
819 getTimeStamp(response, lastStamp);
820 return true;
821 }
822 return false;
823}
824
825bool RemoteControlBoard::get1V1I2D(int code, int axis, double *v1, double *v2)
826{
827 Bottle cmd, response;
829 cmd.addVocab32(code);
830 cmd.addInt32(axis);
831
832 bool ok = rpc_p.write(cmd, response);
833
834 if (CHECK_FAIL(ok, response)) {
835 *v1 = response.get(2).asFloat64();
836 *v2 = response.get(3).asFloat64();
837 return true;
838 }
839 return false;
840}
841
842bool RemoteControlBoard::get1V1I1B(int v, int j, bool &val)
843{
844 Bottle cmd, response;
846 cmd.addVocab32(v);
847 cmd.addInt32(j);
848 bool ok = rpc_p.write(cmd, response);
849 if (CHECK_FAIL(ok, response)) {
850 val = (response.get(2).asInt32()!=0);
851 getTimeStamp(response, lastStamp);
852 return true;
853 }
854 return false;
855}
856
857bool RemoteControlBoard::get1V1I1IA1B(int v, const int len, const int *val1, bool &retVal)
858{
859 Bottle cmd, response;
861 cmd.addVocab32(v);
862 cmd.addInt32(len);
863 Bottle& l1 = cmd.addList();
864 for (int i = 0; i < len; i++) {
865 l1.addInt32(val1[i]);
866 }
867
868 bool ok = rpc_p.write(cmd, response);
869
870 if (CHECK_FAIL(ok, response)) {
871 retVal = (response.get(2).asInt32()!=0);
872 getTimeStamp(response, lastStamp);
873 return true;
874 }
875 return false;
876}
877
878bool RemoteControlBoard::get2V1I1IA1DA(int v1, int v2, const int n_joints, const int *joints, double *retVals, std::string functionName)
879{
880 Bottle cmd, response;
881 if (!isLive()) {
882 return false;
883 }
884
886 cmd.addVocab32(v1);
887 cmd.addVocab32(v2);
888 cmd.addInt32(n_joints);
889
890 Bottle& l1 = cmd.addList();
891 for (int i = 0; i < n_joints; i++) {
892 l1.addInt32(joints[i]);
893 }
894
895 bool ok = rpc_p.write(cmd, response);
896
897 if (CHECK_FAIL(ok, response))
898 {
899 int i;
900 Bottle& list = *(response.get(0).asList());
901 yCAssert(REMOTECONTROLBOARD, list.size() >= (size_t) n_joints)
902
903 if (list.size() != (size_t )n_joints)
904 {
906 "%s length of response does not match: expected %d, received %zu\n ",
907 functionName.c_str(),
908 n_joints ,
909 list.size() );
910 return false;
911 }
912 else
913 {
914 for (i = 0; i < n_joints; i++)
915 {
916 retVals[i] = (double) list.get(i).asFloat64();
917 }
918 return true;
919 }
920 }
921 return false;
922}
923
924bool RemoteControlBoard::get1V1B(int v, bool &val)
925{
926 Bottle cmd, response;
928 cmd.addVocab32(v);
929 bool ok = rpc_p.write(cmd, response);
930 if (CHECK_FAIL(ok, response)) {
931 val = (response.get(2).asInt32()!=0);
932 getTimeStamp(response, lastStamp);
933 return true;
934 }
935 return false;
936}
937
938bool RemoteControlBoard::get1VIA(int v, int *val)
939{
940 if (!isLive()) {
941 return false;
942 }
943 Bottle cmd, response;
945 cmd.addVocab32(v);
946 bool ok = rpc_p.write(cmd, response);
947 if (CHECK_FAIL(ok, response)) {
948 Bottle* lp = response.get(2).asList();
949 if (lp == nullptr) {
950 return false;
951 }
952 Bottle& l = *lp;
954 for (size_t i = 0; i < nj; i++) {
955 val[i] = l.get(i).asInt32();
956 }
957
958 getTimeStamp(response, lastStamp);
959
960 return true;
961 }
962 return false;
963}
964
965bool RemoteControlBoard::get1VDA(int v, double *val)
966{
967 if (!isLive()) {
968 return false;
969 }
970 Bottle cmd, response;
972 cmd.addVocab32(v);
973 bool ok = rpc_p.write(cmd, response);
974 if (CHECK_FAIL(ok, response)) {
975 Bottle* lp = response.get(2).asList();
976 if (lp == nullptr) {
977 return false;
978 }
979 Bottle& l = *lp;
981 for (size_t i = 0; i < nj; i++) {
982 val[i] = l.get(i).asFloat64();
983 }
984
985 getTimeStamp(response, lastStamp);
986
987 return true;
988 }
989 return false;
990}
991
992bool RemoteControlBoard::get1V1DA(int v1, double *val)
993{
994 if (!isLive()) {
995 return false;
996 }
997 Bottle cmd, response;
999 cmd.addVocab32(v1);
1000 bool ok = rpc_p.write(cmd, response);
1001
1002 if (CHECK_FAIL(ok, response)) {
1003 Bottle* lp = response.get(2).asList();
1004 if (lp == nullptr) {
1005 return false;
1006 }
1007 Bottle& l = *lp;
1009 for (size_t i = 0; i < nj; i++) {
1010 val[i] = l.get(i).asFloat64();
1011 }
1012
1013 getTimeStamp(response, lastStamp);
1014 return true;
1015 }
1016 return false;
1017}
1018
1019bool RemoteControlBoard::get2V1DA(int v1, int v2, double *val)
1020{
1021 if (!isLive()) {
1022 return false;
1023 }
1024 Bottle cmd, response;
1025 cmd.addVocab32(VOCAB_GET);
1026 cmd.addVocab32(v1);
1027 cmd.addVocab32(v2);
1028 bool ok = rpc_p.write(cmd, response);
1029
1030 if (CHECK_FAIL(ok, response)) {
1031 Bottle* lp = response.get(2).asList();
1032 if (lp == nullptr) {
1033 return false;
1034 }
1035 Bottle& l = *lp;
1037 for (size_t i = 0; i < nj; i++) {
1038 val[i] = l.get(i).asFloat64();
1039 }
1040
1041 getTimeStamp(response, lastStamp);
1042 return true;
1043 }
1044 return false;
1045}
1046
1047bool RemoteControlBoard::get2V2DA(int v1, int v2, double *val1, double *val2)
1048{
1049 if (!isLive()) {
1050 return false;
1051 }
1052 Bottle cmd, response;
1053 cmd.addVocab32(VOCAB_GET);
1054 cmd.addVocab32(v1);
1055 cmd.addVocab32(v2);
1056 bool ok = rpc_p.write(cmd, response);
1057 if (CHECK_FAIL(ok, response)) {
1058 Bottle* lp1 = response.get(2).asList();
1059 if (lp1 == nullptr) {
1060 return false;
1061 }
1062 Bottle& l1 = *lp1;
1063 Bottle* lp2 = response.get(3).asList();
1064 if (lp2 == nullptr) {
1065 return false;
1066 }
1067 Bottle& l2 = *lp2;
1068
1069 size_t nj1 = l1.size();
1070 size_t nj2 = l2.size();
1071 // yCAssert(REMOTECONTROLBOARD, nj == nj1);
1072 // yCAssert(REMOTECONTROLBOARD, nj == nj2);
1073
1074 for (size_t i = 0; i < nj1; i++) {
1075 val1[i] = l1.get(i).asFloat64();
1076 }
1077 for (size_t i = 0; i < nj2; i++) {
1078 val2[i] = l2.get(i).asFloat64();
1079 }
1080
1081 getTimeStamp(response, lastStamp);
1082 return true;
1083 }
1084 return false;
1085}
1086
1087bool RemoteControlBoard::get1V1I1S(int code, int j, std::string &name)
1088{
1089 Bottle cmd, response;
1090 cmd.addVocab32(VOCAB_GET);
1091 cmd.addVocab32(code);
1092 cmd.addInt32(j);
1093 bool ok = rpc_p.write(cmd, response);
1094
1095 if (CHECK_FAIL(ok, response)) {
1096 name = response.get(2).asString();
1097 return true;
1098 }
1099 return false;
1100}
1101
1102
1103bool RemoteControlBoard::get1V1I1IA1DA(int v, const int len, const int *val1, double *val2)
1104{
1105 if (!isLive()) {
1106 return false;
1107 }
1108
1109 Bottle cmd, response;
1110 cmd.addVocab32(VOCAB_GET);
1111 cmd.addVocab32(v);
1112 cmd.addInt32(len);
1113 Bottle &l1 = cmd.addList();
1114 for (int i = 0; i < len; i++) {
1115 l1.addInt32(val1[i]);
1116 }
1117
1118 bool ok = rpc_p.write(cmd, response);
1119
1120 if (CHECK_FAIL(ok, response)) {
1121 Bottle* lp2 = response.get(2).asList();
1122 if (lp2 == nullptr) {
1123 return false;
1124 }
1125 Bottle& l2 = *lp2;
1126
1127 size_t nj2 = l2.size();
1128 if(nj2 != (unsigned)len)
1129 {
1130 yCError(REMOTECONTROLBOARD, "received an answer with an unexpected number of entries!");
1131 return false;
1132 }
1133 for (size_t i = 0; i < nj2; i++) {
1134 val2[i] = l2.get(i).asFloat64();
1135 }
1136
1137 getTimeStamp(response, lastStamp);
1138 return true;
1139 }
1140 return false;
1141}
1142
1143// END Helpers functions
1144
1146{
1147 return get1V1I(VOCAB_AXES, *ax);
1148}
1149
1150// BEGIN IPidControl
1151
1152bool RemoteControlBoard::setPid(const PidControlTypeEnum& pidtype, int j, const Pid &pid)
1153{
1154 Bottle cmd, response;
1155 cmd.addVocab32(VOCAB_SET);
1156 cmd.addVocab32(VOCAB_PID);
1157 cmd.addVocab32(VOCAB_PID);
1158 cmd.addVocab32(pidtype);
1159 cmd.addInt32(j);
1160 Bottle& l = cmd.addList();
1161 l.addFloat64(pid.kp);
1162 l.addFloat64(pid.kd);
1163 l.addFloat64(pid.ki);
1164 l.addFloat64(pid.max_int);
1165 l.addFloat64(pid.max_output);
1166 l.addFloat64(pid.offset);
1167 l.addFloat64(pid.scale);
1170 l.addFloat64(pid.kff);
1171 bool ok = rpc_p.write(cmd, response);
1172 return CHECK_FAIL(ok, response);
1173}
1174
1175bool RemoteControlBoard::setPids(const PidControlTypeEnum& pidtype, const Pid *pids)
1176{
1177 if (!isLive()) {
1178 return false;
1179 }
1180 Bottle cmd, response;
1181 cmd.addVocab32(VOCAB_SET);
1182 cmd.addVocab32(VOCAB_PID);
1184 cmd.addVocab32(pidtype);
1185 Bottle& l = cmd.addList();
1186 for (size_t i = 0; i < nj; i++) {
1187 Bottle& m = l.addList();
1188 m.addFloat64(pids[i].kp);
1189 m.addFloat64(pids[i].kd);
1190 m.addFloat64(pids[i].ki);
1191 m.addFloat64(pids[i].max_int);
1192 m.addFloat64(pids[i].max_output);
1193 m.addFloat64(pids[i].offset);
1194 m.addFloat64(pids[i].scale);
1195 m.addFloat64(pids[i].stiction_up_val);
1196 m.addFloat64(pids[i].stiction_down_val);
1197 m.addFloat64(pids[i].kff);
1198 }
1199
1200 bool ok = rpc_p.write(cmd, response);
1201 return CHECK_FAIL(ok, response);
1202}
1203
1204bool RemoteControlBoard::setPidReference(const PidControlTypeEnum& pidtype, int j, double ref)
1205{
1206 return setValWithPidType(VOCAB_REF, pidtype, j, ref);
1207}
1208
1209bool RemoteControlBoard::setPidReferences(const PidControlTypeEnum& pidtype, const double *refs)
1210{
1211 return setValWithPidType(VOCAB_REFS, pidtype, refs);
1212}
1213
1214bool RemoteControlBoard::setPidErrorLimit(const PidControlTypeEnum& pidtype, int j, double limit)
1215{
1216 return setValWithPidType(VOCAB_LIM, pidtype, j, limit);
1217}
1218
1219bool RemoteControlBoard::setPidErrorLimits(const PidControlTypeEnum& pidtype, const double *limits)
1220{
1221 return setValWithPidType(VOCAB_LIMS, pidtype, limits);
1222}
1223
1224bool RemoteControlBoard::getPidError(const PidControlTypeEnum& pidtype, int j, double *err)
1225{
1226 return getValWithPidType(VOCAB_ERR, pidtype, j, err);
1227}
1228
1230{
1231 return getValWithPidType(VOCAB_ERRS, pidtype, errs);
1232}
1233
1234bool RemoteControlBoard::getPid(const PidControlTypeEnum& pidtype, int j, Pid *pid)
1235{
1236 Bottle cmd, response;
1237 cmd.addVocab32(VOCAB_GET);
1238 cmd.addVocab32(VOCAB_PID);
1239 cmd.addVocab32(VOCAB_PID);
1240 cmd.addVocab32(pidtype);
1241 cmd.addInt32(j);
1242 bool ok = rpc_p.write(cmd, response);
1243 if (CHECK_FAIL(ok, response)) {
1244 Bottle* lp = response.get(2).asList();
1245 if (lp == nullptr) {
1246 return false;
1247 }
1248 Bottle& l = *lp;
1249 pid->kp = l.get(0).asFloat64();
1250 pid->kd = l.get(1).asFloat64();
1251 pid->ki = l.get(2).asFloat64();
1252 pid->max_int = l.get(3).asFloat64();
1253 pid->max_output = l.get(4).asFloat64();
1254 pid->offset = l.get(5).asFloat64();
1255 pid->scale = l.get(6).asFloat64();
1256 pid->stiction_up_val = l.get(7).asFloat64();
1257 pid->stiction_down_val = l.get(8).asFloat64();
1258 pid->kff = l.get(9).asFloat64();
1259 return true;
1260 }
1261 return false;
1262}
1263
1265{
1266 if (!isLive()) {
1267 return false;
1268 }
1269 Bottle cmd, response;
1270 cmd.addVocab32(VOCAB_GET);
1271 cmd.addVocab32(VOCAB_PID);
1273 cmd.addVocab32(pidtype);
1274 bool ok = rpc_p.write(cmd, response);
1275 if (CHECK_FAIL(ok, response))
1276 {
1277 Bottle* lp = response.get(2).asList();
1278 if (lp == nullptr) {
1279 return false;
1280 }
1281 Bottle& l = *lp;
1283 for (size_t i = 0; i < nj; i++)
1284 {
1285 Bottle* mp = l.get(i).asList();
1286 if (mp == nullptr) {
1287 return false;
1288 }
1289 pids[i].kp = mp->get(0).asFloat64();
1290 pids[i].kd = mp->get(1).asFloat64();
1291 pids[i].ki = mp->get(2).asFloat64();
1292 pids[i].max_int = mp->get(3).asFloat64();
1293 pids[i].max_output = mp->get(4).asFloat64();
1294 pids[i].offset = mp->get(5).asFloat64();
1295 pids[i].scale = mp->get(6).asFloat64();
1296 pids[i].stiction_up_val = mp->get(7).asFloat64();
1297 pids[i].stiction_down_val = mp->get(8).asFloat64();
1298 pids[i].kff = mp->get(9).asFloat64();
1299 }
1300 return true;
1301 }
1302 return false;
1303}
1304
1305bool RemoteControlBoard::getPidReference(const PidControlTypeEnum& pidtype, int j, double *ref)
1306{
1307 return getValWithPidType(VOCAB_REF, pidtype, j, ref);
1308}
1309
1311{
1312 return getValWithPidType(VOCAB_REFS, pidtype, refs);
1313}
1314
1315bool RemoteControlBoard::getPidErrorLimit(const PidControlTypeEnum& pidtype, int j, double *limit)
1316{
1317 return getValWithPidType(VOCAB_LIM, pidtype, j, limit);
1318}
1319
1321{
1322 return getValWithPidType(VOCAB_LIMS, pidtype, limits);
1323}
1324
1326{
1327 if (!isLive()) {
1328 return false;
1329 }
1330 Bottle cmd, response;
1331 cmd.addVocab32(VOCAB_SET);
1332 cmd.addVocab32(VOCAB_PID);
1334 cmd.addVocab32(pidtype);
1335 cmd.addInt32(j);
1336 bool ok = rpc_p.write(cmd, response);
1337 return CHECK_FAIL(ok, response);
1338}
1339
1341{
1342 if (!isLive()) {
1343 return false;
1344 }
1345 Bottle cmd, response;
1346 cmd.addVocab32(VOCAB_SET);
1347 cmd.addVocab32(VOCAB_PID);
1349 cmd.addVocab32(pidtype);
1350 cmd.addInt32(j);
1351 bool ok = rpc_p.write(cmd, response);
1352 return CHECK_FAIL(ok, response);
1353}
1354
1356{
1357 if (!isLive()) {
1358 return false;
1359 }
1360 Bottle cmd, response;
1361 cmd.addVocab32(VOCAB_SET);
1362 cmd.addVocab32(VOCAB_PID);
1364 cmd.addVocab32(pidtype);
1365 cmd.addInt32(j);
1366 bool ok = rpc_p.write(cmd, response);
1367 return CHECK_FAIL(ok, response);
1368}
1369
1370bool RemoteControlBoard::isPidEnabled(const PidControlTypeEnum& pidtype, int j, bool* enabled)
1371{
1372 Bottle cmd, response;
1373 cmd.addVocab32(VOCAB_GET);
1374 cmd.addVocab32(VOCAB_PID);
1376 cmd.addVocab32(pidtype);
1377 cmd.addInt32(j);
1378 bool ok = rpc_p.write(cmd, response);
1379 if (CHECK_FAIL(ok, response))
1380 {
1381 *enabled = response.get(2).asBool();
1382 return true;
1383 }
1384 return false;
1385}
1386
1387bool RemoteControlBoard::getPidOutput(const PidControlTypeEnum& pidtype, int j, double *out)
1388{
1389 return getValWithPidType(VOCAB_OUTPUT, pidtype, j, out);
1390}
1391
1393{
1394 return getValWithPidType(VOCAB_OUTPUTS, pidtype, outs);
1395}
1396
1397bool RemoteControlBoard::setPidOffset(const PidControlTypeEnum& pidtype, int j, double v)
1398{
1399 return setValWithPidType(VOCAB_OFFSET, pidtype, j, v);
1400}
1401
1402// END IPidControl
1403
1404// BEGIN IEncoder
1405
1407{
1408 return set1V1I(VOCAB_E_RESET, j);
1409}
1410
1412{
1413 return set1V(VOCAB_E_RESETS);
1414}
1415
1416bool RemoteControlBoard::setEncoder(int j, double val)
1417{
1418 return set1V1I1D(VOCAB_ENCODER, j, val);
1419}
1420
1421bool RemoteControlBoard::setEncoders(const double *vals)
1422{
1423 return set1VDA(VOCAB_ENCODERS, vals);
1424}
1425
1427{
1428 double localArrivalTime = 0.0;
1429
1430 extendedPortMutex.lock();
1431 bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER, v, lastStamp, localArrivalTime);
1432 extendedPortMutex.unlock();
1433 return ret;
1434}
1435
1436bool RemoteControlBoard::getEncoderTimed(int j, double *v, double *t)
1437{
1438 double localArrivalTime = 0.0;
1439
1440 extendedPortMutex.lock();
1441 bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER, v, lastStamp, localArrivalTime);
1442 *t=lastStamp.getTime();
1443 extendedPortMutex.unlock();
1444 return ret;
1445}
1446
1448{
1449 double localArrivalTime = 0.0;
1450
1451 extendedPortMutex.lock();
1452 bool ret = extendedIntputStatePort.getLastVector(VOCAB_ENCODERS, encs, lastStamp, localArrivalTime);
1453 extendedPortMutex.unlock();
1454
1455 return ret;
1456}
1457
1458bool RemoteControlBoard::getEncodersTimed(double *encs, double *ts)
1459{
1460 double localArrivalTime=0.0;
1461
1462 extendedPortMutex.lock();
1463 bool ret = extendedIntputStatePort.getLastVector(VOCAB_ENCODERS, encs, lastStamp, localArrivalTime);
1464 std::fill_n(ts, nj, lastStamp.getTime());
1465 extendedPortMutex.unlock();
1466 return ret;
1467}
1468
1470{
1471 double localArrivalTime=0.0;
1472
1473 extendedPortMutex.lock();
1474 bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER_SPEED, sp, lastStamp, localArrivalTime);
1475 extendedPortMutex.unlock();
1476 return ret;
1477}
1478
1480{
1481 double localArrivalTime=0.0;
1482
1483 extendedPortMutex.lock();
1485 extendedPortMutex.unlock();
1486 return ret;
1487}
1488
1490{
1491 double localArrivalTime=0.0;
1492 extendedPortMutex.lock();
1494 extendedPortMutex.unlock();
1495 return ret;
1496}
1497
1499{
1500 double localArrivalTime=0.0;
1501 extendedPortMutex.lock();
1503 extendedPortMutex.unlock();
1504 return ret;
1505}
1506
1507// END IEncoder
1508
1509// BEGIN IRemoteVariable
1510
1512{
1513 Bottle cmd, response;
1514 cmd.addVocab32(VOCAB_GET);
1517 cmd.addString(key);
1518 bool ok = rpc_p.write(cmd, response);
1519 if (CHECK_FAIL(ok, response))
1520 {
1521 val = *(response.get(2).asList());
1522 return true;
1523 }
1524 return false;
1525}
1526
1528{
1529 Bottle cmd, response;
1530 cmd.addVocab32(VOCAB_SET);
1533 cmd.addString(key);
1534 cmd.append(val);
1535 //std::string s = cmd.toString();
1536 bool ok = rpc_p.write(cmd, response);
1537
1538 return CHECK_FAIL(ok, response);
1539}
1540
1541
1543{
1544 Bottle cmd, response;
1545 cmd.addVocab32(VOCAB_GET);
1548 bool ok = rpc_p.write(cmd, response);
1549 //std::string s = response.toString();
1550 if (CHECK_FAIL(ok, response))
1551 {
1552 *listOfKeys = *(response.get(2).asList());
1553 //std::string s = listOfKeys->toString();
1554 return true;
1555 }
1556 return false;
1557}
1558
1559// END IRemoteVariable
1560
1561// BEGIN IMotor
1562
1564{
1565 return get1V1I(VOCAB_MOTORS_NUMBER, *num);
1566}
1567
1568bool RemoteControlBoard::getTemperature (int m, double* val)
1569{
1570 return get1V1I1D(VOCAB_TEMPERATURE, m, val);
1571}
1572
1574{
1575 return get1VDA(VOCAB_TEMPERATURES, vals);
1576}
1577
1579{
1580 return get1V1I1D(VOCAB_TEMPERATURE_LIMIT, m, val);
1581}
1582
1583bool RemoteControlBoard::setTemperatureLimit (int m, const double val)
1584{
1585 return set1V1I1D(VOCAB_TEMPERATURE_LIMIT, m, val);
1586}
1587
1589{
1590 return get1V1I1D(VOCAB_GEARBOX_RATIO, m, val);
1591}
1592
1593bool RemoteControlBoard::setGearboxRatio(int m, const double val)
1594{
1595 return set1V1I1D(VOCAB_GEARBOX_RATIO, m, val);
1596}
1597
1598// END IMotor
1599
1600// BEGIN IMotorEncoder
1601
1603{
1604 return set1V1I(VOCAB_MOTOR_E_RESET, j);
1605}
1606
1608{
1610}
1611
1612bool RemoteControlBoard::setMotorEncoder(int j, const double val)
1613{
1614 return set1V1I1D(VOCAB_MOTOR_ENCODER, j, val);
1615}
1616
1618{
1619 return set1V1I1D(VOCAB_MOTOR_CPR, m, cpr);
1620}
1621
1623{
1624 return get1V1I1D(VOCAB_MOTOR_CPR, m, cpr);
1625}
1626
1628{
1629 return set1VDA(VOCAB_MOTOR_ENCODERS, vals);
1630}
1631
1633{
1634 double localArrivalTime = 0.0;
1635
1636 extendedPortMutex.lock();
1638 extendedPortMutex.unlock();
1639 return ret;
1640}
1641
1642bool RemoteControlBoard::getMotorEncoderTimed(int j, double *v, double *t)
1643{
1644 double localArrivalTime = 0.0;
1645
1646 extendedPortMutex.lock();
1648 *t=lastStamp.getTime();
1649 extendedPortMutex.unlock();
1650 return ret;
1651}
1652
1654{
1655 double localArrivalTime=0.0;
1656
1657 extendedPortMutex.lock();
1659 extendedPortMutex.unlock();
1660
1661 return ret;
1662}
1663
1664bool RemoteControlBoard::getMotorEncodersTimed(double *encs, double *ts)
1665{
1666 double localArrivalTime=0.0;
1667
1668 extendedPortMutex.lock();
1670 std::fill_n(ts, nj, lastStamp.getTime());
1671 extendedPortMutex.unlock();
1672 return ret;
1673}
1674
1676{
1677 double localArrivalTime=0.0;
1678 extendedPortMutex.lock();
1680 extendedPortMutex.unlock();
1681 return ret;
1682}
1683
1685{
1686 double localArrivalTime=0.0;
1687 extendedPortMutex.lock();
1689 extendedPortMutex.unlock();
1690 return ret;
1691}
1692
1694{
1695 double localArrivalTime=0.0;
1696 extendedPortMutex.lock();
1698 extendedPortMutex.unlock();
1699 return ret;
1700}
1701
1703{
1704 double localArrivalTime=0.0;
1705 extendedPortMutex.lock();
1707 extendedPortMutex.unlock();
1708 return ret;
1709}
1710
1712{
1713 return get1V1I(VOCAB_MOTOR_ENCODER_NUMBER, *num);
1714}
1715
1716// END IMotorEncoder
1717
1718// BEGIN IPreciselyTimed
1719
1725{
1726 Stamp ret;
1727// mutex.lock();
1728 ret = lastStamp;
1729// mutex.unlock();
1730 return ret;
1731}
1732
1733// END IPreciselyTimed
1734
1735// BEGIN IPositionControl
1736
1738{
1739 return set1V1I1D(VOCAB_POSITION_MOVE, j, ref);
1740}
1741
1742bool RemoteControlBoard::positionMove(const int n_joint, const int *joints, const double *refs)
1743{
1744 return set1V1I1IA1DA(VOCAB_POSITION_MOVE_GROUP, n_joint, joints, refs);
1745}
1746
1747bool RemoteControlBoard::positionMove(const double *refs)
1748{
1749 return set1VDA(VOCAB_POSITION_MOVES, refs);
1750}
1751
1752bool RemoteControlBoard::getTargetPosition(const int joint, double *ref)
1753{
1754 return get1V1I1D(VOCAB_POSITION_MOVE, joint, ref);
1755}
1756
1758{
1759 return get1V1DA(VOCAB_POSITION_MOVES, refs);
1760}
1761
1762bool RemoteControlBoard::getTargetPositions(const int n_joint, const int *joints, double *refs)
1763{
1764 return get1V1I1IA1DA(VOCAB_POSITION_MOVE_GROUP, n_joint, joints, refs);
1765}
1766
1767bool RemoteControlBoard::relativeMove(int j, double delta)
1768{
1769 return set1V1I1D(VOCAB_RELATIVE_MOVE, j, delta);
1770}
1771
1772bool RemoteControlBoard::relativeMove(const int n_joint, const int *joints, const double *refs)
1773{
1774 return set1V1I1IA1DA(VOCAB_RELATIVE_MOVE_GROUP, n_joint, joints, refs);
1775}
1776
1777bool RemoteControlBoard::relativeMove(const double *deltas)
1778{
1779 return set1VDA(VOCAB_RELATIVE_MOVES, deltas);
1780}
1781
1783{
1784 return get1V1I1B(VOCAB_MOTION_DONE, j, *flag);
1785}
1786
1787bool RemoteControlBoard::checkMotionDone(const int n_joint, const int *joints, bool *flag)
1788{
1789 return get1V1I1IA1B(VOCAB_MOTION_DONE_GROUP, n_joint, joints, *flag);
1790}
1791
1793{
1794 return get1V1B(VOCAB_MOTION_DONES, *flag);
1795}
1796
1798{
1799 return set1V1I1D(VOCAB_REF_SPEED, j, sp);
1800}
1801
1802bool RemoteControlBoard::setRefSpeeds(const int n_joint, const int *joints, const double *spds)
1803{
1804 return set1V1I1IA1DA(VOCAB_REF_SPEED_GROUP, n_joint, joints, spds);
1805}
1806
1807bool RemoteControlBoard::setRefSpeeds(const double *spds)
1808{
1809 return set1VDA(VOCAB_REF_SPEEDS, spds);
1810}
1811
1813{
1814 return set1V1I1D(VOCAB_REF_ACCELERATION, j, acc);
1815}
1816
1817bool RemoteControlBoard::setRefAccelerations(const int n_joint, const int *joints, const double *accs)
1818{
1819 return set1V1I1IA1DA(VOCAB_REF_ACCELERATION_GROUP, n_joint, joints, accs);
1820}
1821
1823{
1824 return set1VDA(VOCAB_REF_ACCELERATIONS, accs);
1825}
1826
1827bool RemoteControlBoard::getRefSpeed(int j, double *ref)
1828{
1829 return get1V1I1D(VOCAB_REF_SPEED, j, ref);
1830}
1831
1832bool RemoteControlBoard::getRefSpeeds(const int n_joint, const int *joints, double *spds)
1833{
1834 return get1V1I1IA1DA(VOCAB_REF_SPEED_GROUP, n_joint, joints, spds);
1835}
1836
1838{
1839 return get1VDA(VOCAB_REF_SPEEDS, spds);
1840}
1841
1843{
1844 return get1V1I1D(VOCAB_REF_ACCELERATION, j, acc);
1845}
1846
1847bool RemoteControlBoard::getRefAccelerations(const int n_joint, const int *joints, double *accs)
1848{
1849 return get1V1I1IA1DA(VOCAB_REF_ACCELERATION_GROUP, n_joint, joints, accs);
1850}
1851
1853{
1854 return get1VDA(VOCAB_REF_ACCELERATIONS, accs);
1855}
1856
1858{
1859 return set1V1I(VOCAB_STOP, j);
1860}
1861
1862bool RemoteControlBoard::stop(const int len, const int *val1)
1863{
1864 if (!isLive()) {
1865 return false;
1866 }
1867 Bottle cmd, response;
1868 cmd.addVocab32(VOCAB_SET);
1870 cmd.addInt32(len);
1871 int i;
1872 Bottle& l1 = cmd.addList();
1873 for (i = 0; i < len; i++) {
1874 l1.addInt32(val1[i]);
1875 }
1876
1877 bool ok = rpc_p.write(cmd, response);
1878 return CHECK_FAIL(ok, response);
1879}
1880
1882{
1883 return set1V(VOCAB_STOPS);
1884}
1885
1886// END IPositionControl
1887
1888// BEGIN IJoint Fault
1889bool RemoteControlBoard::getLastJointFault(int j, int& fault, std::string& message)
1890{
1891 Bottle cmd, response;
1892
1893 cmd.addVocab32(VOCAB_GET);
1896 cmd.addInt32(j);
1897
1898 bool ok = rpc_p.write(cmd, response);
1899
1900 std::string ss = response.toString();
1901
1902 if (CHECK_FAIL(ok, response))
1903 {
1904 fault = response.get(1).asInt32();
1905 message = response.get(2).asString();
1906 return true;
1907 }
1908 return false;
1909}
1910// END IJointFault
1911
1912// BEGIN IVelocityControl
1913
1915{
1916 // return set1V1I1D(VOCAB_VELOCITY_MOVE, j, v);
1917 if (!isLive()) {
1918 return false;
1919 }
1920 CommandMessage& c = command_buffer.get();
1921 c.head.clear();
1922 c.head.addVocab32(VOCAB_VELOCITY_MOVE);
1923 c.head.addInt32(j);
1924 c.body.resize(1);
1925 memcpy(&(c.body[0]), &v, sizeof(double));
1927 return true;
1928}
1929
1931{
1932 if (!isLive()) {
1933 return false;
1934 }
1935 CommandMessage& c = command_buffer.get();
1936 c.head.clear();
1937 c.head.addVocab32(VOCAB_VELOCITY_MOVES);
1938 c.body.resize(nj);
1939 memcpy(&(c.body[0]), v, sizeof(double)*nj);
1941 return true;
1942}
1943
1944// END IVelocityControl
1945
1946// BEGIN IAmplifierControl
1947
1949{
1950 return set1V1I(VOCAB_AMP_ENABLE, j);
1951}
1952
1954{
1955 return set1V1I(VOCAB_AMP_DISABLE, j);
1956}
1957
1959{
1960 return get1VIA(VOCAB_AMP_STATUS, st);
1961}
1962
1964{
1965 return get1V1I1I(VOCAB_AMP_STATUS_SINGLE, j, st);
1966}
1967
1969{
1970 return set1V1I1D(VOCAB_AMP_MAXCURRENT, j, v);
1971}
1972
1974{
1975 return get1V1I1D(VOCAB_AMP_MAXCURRENT, j, v);
1976}
1977
1979{
1980 return get1V1I1D(VOCAB_AMP_NOMINAL_CURRENT, m, val);
1981}
1982
1983bool RemoteControlBoard::setNominalCurrent(int m, const double val)
1984{
1985 return set1V1I1D(VOCAB_AMP_NOMINAL_CURRENT, m, val);
1986}
1987
1989{
1990 return get1V1I1D(VOCAB_AMP_PEAK_CURRENT, m, val);
1991}
1992
1993bool RemoteControlBoard::setPeakCurrent(int m, const double val)
1994{
1995 return set1V1I1D(VOCAB_AMP_PEAK_CURRENT, m, val);
1996}
1997
1998bool RemoteControlBoard::getPWM(int m, double* val)
1999{
2000 double localArrivalTime = 0.0;
2001 extendedPortMutex.lock();
2003 extendedPortMutex.unlock();
2004 return ret;
2005}
2006
2007bool RemoteControlBoard::getPWMLimit(int m, double* val)
2008{
2009 return get1V1I1D(VOCAB_AMP_PWM_LIMIT, m, val);
2010}
2011
2012bool RemoteControlBoard::setPWMLimit(int m, const double val)
2013{
2014 return set1V1I1D(VOCAB_AMP_PWM_LIMIT, m, val);
2015}
2016
2018{
2019 return get1V1I1D(VOCAB_AMP_VOLTAGE_SUPPLY, m, val);
2020}
2021
2022// END IAmplifierControl
2023
2024// BEGIN IControlLimits
2025
2026bool RemoteControlBoard::setLimits(int axis, double min, double max)
2027{
2028 return set1V1I2D(VOCAB_LIMITS, axis, min, max);
2029}
2030
2031bool RemoteControlBoard::getLimits(int axis, double *min, double *max)
2032{
2033 return get1V1I2D(VOCAB_LIMITS, axis, min, max);
2034}
2035
2036bool RemoteControlBoard::setVelLimits(int axis, double min, double max)
2037{
2038 return set1V1I2D(VOCAB_VEL_LIMITS, axis, min, max);
2039}
2040
2041bool RemoteControlBoard::getVelLimits(int axis, double *min, double *max)
2042{
2043 return get1V1I2D(VOCAB_VEL_LIMITS, axis, min, max);
2044}
2045
2046// END IControlLimits
2047
2048// BEGIN IAxisInfo
2049
2050bool RemoteControlBoard::getAxisName(int j, std::string& name)
2051{
2052 return get1V1I1S(VOCAB_INFO_NAME, j, name);
2053}
2054
2056{
2057 return get1V1I1I(VOCAB_INFO_TYPE, j, (int*)&type);
2058}
2059
2060// END IAxisInfo
2061
2062// BEGIN IControlCalibration
2064{
2065 return send1V(VOCAB_CALIBRATE);
2066}
2067
2069{
2070 return send1V(VOCAB_ABORTCALIB);
2071}
2072
2074{
2075 return send1V(VOCAB_ABORTPARK);
2076}
2077
2079{
2080 return send1V(VOCAB_PARK);
2081}
2082
2083bool RemoteControlBoard::calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3)
2084{
2085 Bottle cmd, response;
2086
2088 cmd.addInt32(j);
2089 cmd.addInt32(ui);
2090 cmd.addFloat64(v1);
2091 cmd.addFloat64(v2);
2092 cmd.addFloat64(v3);
2093
2094 bool ok = rpc_p.write(cmd, response);
2095
2096 if (CHECK_FAIL(ok, response)) {
2097 return true;
2098 }
2099 return false;
2100}
2101
2103{
2104 Bottle cmd, response;
2105
2107 cmd.addInt32(j);
2108 cmd.addInt32(params.type);
2109 cmd.addFloat64(params.param1);
2110 cmd.addFloat64(params.param2);
2111 cmd.addFloat64(params.param3);
2112 cmd.addFloat64(params.param4);
2113
2114 bool ok = rpc_p.write(cmd, response);
2115
2116 if (CHECK_FAIL(ok, response)) {
2117 return true;
2118 }
2119 return false;
2120}
2121
2123{
2124 return send1V1I(VOCAB_CALIBRATE_DONE, j);
2125}
2126
2127// END IControlCalibration
2128
2129// BEGIN ITorqueControl
2130
2132{
2133 return get2V1I1D(VOCAB_TORQUE, VOCAB_REF, j, t);
2134}
2135
2137{
2139}
2140
2142{
2143 //Now we use streaming instead of rpc
2144 //return set2V1DA(VOCAB_TORQUE, VOCAB_REFS, t);
2145 if (!isLive()) {
2146 return false;
2147 }
2148 CommandMessage& c = command_buffer.get();
2149 c.head.clear();
2150 c.head.addVocab32(VOCAB_TORQUES_DIRECTS);
2151
2152 c.body.resize(nj);
2153
2154 memcpy(c.body.data(), t, sizeof(double) * nj);
2155
2157 return true;
2158}
2159
2161{
2162 //return set2V1I1D(VOCAB_TORQUE, VOCAB_REF, j, v);
2163 // use the streaming port!
2164 if (!isLive()) {
2165 return false;
2166 }
2167 CommandMessage& c = command_buffer.get();
2168 c.head.clear();
2169 // in streaming port only SET command can be sent, so it is implicit
2170 c.head.addVocab32(VOCAB_TORQUES_DIRECT);
2171 c.head.addInt32(j);
2172
2173 c.body.clear();
2174 c.body.resize(1);
2175 c.body[0] = v;
2177 return true;
2178}
2179
2180bool RemoteControlBoard::setRefTorques(const int n_joint, const int *joints, const double *t)
2181{
2182 //return set2V1I1D(VOCAB_TORQUE, VOCAB_REF, j, v);
2183 // use the streaming port!
2184 if (!isLive()) {
2185 return false;
2186 }
2187 CommandMessage& c = command_buffer.get();
2188 c.head.clear();
2189 // in streaming port only SET command can be sent, so it is implicit
2190 c.head.addVocab32(VOCAB_TORQUES_DIRECT_GROUP);
2191 c.head.addInt32(n_joint);
2192 Bottle &jointList = c.head.addList();
2193 for (int i = 0; i < n_joint; i++) {
2194 jointList.addInt32(joints[i]);
2195 }
2196 c.body.resize(n_joint);
2197 memcpy(&(c.body[0]), t, sizeof(double)*n_joint);
2199 return true;
2200}
2201
2203{
2204 Bottle cmd, response;
2205 cmd.addVocab32(VOCAB_SET);
2208 cmd.addInt32(j);
2209 Bottle& b = cmd.addList();
2210 b.addFloat64(params.bemf);
2211 b.addFloat64(params.bemf_scale);
2212 b.addFloat64(params.ktau);
2213 b.addFloat64(params.ktau_scale);
2214 b.addFloat64(params.viscousPos);
2215 b.addFloat64(params.viscousNeg);
2216 b.addFloat64(params.coulombPos);
2217 b.addFloat64(params.coulombNeg);
2218 bool ok = rpc_p.write(cmd, response);
2219 return CHECK_FAIL(ok, response);
2220}
2221
2223{
2224 Bottle cmd, response;
2225 cmd.addVocab32(VOCAB_GET);
2228 cmd.addInt32(j);
2229 bool ok = rpc_p.write(cmd, response);
2230 if (CHECK_FAIL(ok, response)) {
2231 Bottle* lp = response.get(2).asList();
2232 if (lp == nullptr) {
2233 return false;
2234 }
2235 Bottle& l = *lp;
2236 if (l.size() != 8)
2237 {
2238 yCError(REMOTECONTROLBOARD, "getMotorTorqueParams return value not understood, size != 8");
2239 return false;
2240 }
2241 params->bemf = l.get(0).asFloat64();
2242 params->bemf_scale = l.get(1).asFloat64();
2243 params->ktau = l.get(2).asFloat64();
2244 params->ktau_scale = l.get(3).asFloat64();
2245 params->viscousPos = l.get(4).asFloat64();
2246 params->viscousNeg = l.get(5).asFloat64();
2247 params->coulombPos = l.get(6).asFloat64();
2248 params->coulombNeg = l.get(7).asFloat64();
2249 return true;
2250 }
2251 return false;
2252}
2253
2255{
2256 double localArrivalTime=0.0;
2257 extendedPortMutex.lock();
2258 bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_TRQ, t, lastStamp, localArrivalTime);
2259 extendedPortMutex.unlock();
2260 return ret;
2261}
2262
2264{
2265 double localArrivalTime=0.0;
2266 extendedPortMutex.lock();
2267 bool ret = extendedIntputStatePort.getLastVector(VOCAB_TRQS, t, lastStamp, localArrivalTime);
2268 extendedPortMutex.unlock();
2269 return ret;
2270}
2271
2272bool RemoteControlBoard::getTorqueRange(int j, double *min, double* max)
2273{
2274 return get2V1I2D(VOCAB_TORQUE, VOCAB_RANGE, j, min, max);
2275}
2276
2277bool RemoteControlBoard::getTorqueRanges(double *min, double *max)
2278{
2279 return get2V2DA(VOCAB_TORQUE, VOCAB_RANGES, min, max);
2280}
2281
2282// END ITorqueControl
2283
2284// BEGIN IImpedanceControl
2285
2286bool RemoteControlBoard::getImpedance(int j, double *stiffness, double *damping)
2287{
2288 Bottle cmd, response;
2289 cmd.addVocab32(VOCAB_GET);
2292 cmd.addInt32(j);
2293 bool ok = rpc_p.write(cmd, response);
2294 if (CHECK_FAIL(ok, response)) {
2295 Bottle* lp = response.get(2).asList();
2296 if (lp == nullptr) {
2297 return false;
2298 }
2299 Bottle& l = *lp;
2300 *stiffness = l.get(0).asFloat64();
2301 *damping = l.get(1).asFloat64();
2302 return true;
2303 }
2304 return false;
2305}
2306
2308{
2309 Bottle cmd, response;
2310 cmd.addVocab32(VOCAB_GET);
2313 cmd.addInt32(j);
2314 bool ok = rpc_p.write(cmd, response);
2315 if (CHECK_FAIL(ok, response)) {
2316 Bottle* lp = response.get(2).asList();
2317 if (lp == nullptr) {
2318 return false;
2319 }
2320 Bottle& l = *lp;
2321 *offset = l.get(0).asFloat64();
2322 return true;
2323 }
2324 return false;
2325}
2326
2327bool RemoteControlBoard::setImpedance(int j, double stiffness, double damping)
2328{
2329 Bottle cmd, response;
2330 cmd.addVocab32(VOCAB_SET);
2333 cmd.addInt32(j);
2334
2335 Bottle& b = cmd.addList();
2336 b.addFloat64(stiffness);
2337 b.addFloat64(damping);
2338
2339 bool ok = rpc_p.write(cmd, response);
2340 return CHECK_FAIL(ok, response);
2341}
2342
2344{
2345 Bottle cmd, response;
2346 cmd.addVocab32(VOCAB_SET);
2349 cmd.addInt32(j);
2350
2351 Bottle& b = cmd.addList();
2352 b.addFloat64(offset);
2353
2354 bool ok = rpc_p.write(cmd, response);
2355 return CHECK_FAIL(ok, response);
2356}
2357
2358bool RemoteControlBoard::getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
2359{
2360 Bottle cmd, response;
2361 cmd.addVocab32(VOCAB_GET);
2364 cmd.addInt32(j);
2365 bool ok = rpc_p.write(cmd, response);
2366 if (CHECK_FAIL(ok, response)) {
2367 Bottle* lp = response.get(2).asList();
2368 if (lp == nullptr) {
2369 return false;
2370 }
2371 Bottle& l = *lp;
2372 *min_stiff = l.get(0).asFloat64();
2373 *max_stiff = l.get(1).asFloat64();
2374 *min_damp = l.get(2).asFloat64();
2375 *max_damp = l.get(3).asFloat64();
2376 return true;
2377 }
2378 return false;
2379}
2380
2381// END IImpedanceControl
2382
2383// BEGIN IControlMode
2384
2386{
2387 double localArrivalTime=0.0;
2388 extendedPortMutex.lock();
2389 bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_CM_CONTROL_MODE, mode, lastStamp, localArrivalTime);
2390 extendedPortMutex.unlock();
2391 return ret;
2392}
2393
2395{
2396 double localArrivalTime=0.0;
2397 extendedPortMutex.lock();
2399 extendedPortMutex.unlock();
2400 return ret;
2401}
2402
2403bool RemoteControlBoard::getControlModes(const int n_joint, const int *joints, int *modes)
2404{
2405 double localArrivalTime=0.0;
2406
2407 extendedPortMutex.lock();
2409 if(ret)
2410 {
2411 for (int i = 0; i < n_joint; i++) {
2412 modes[i] = last_wholePart.controlMode[joints[i]];
2413 }
2414 } else {
2415 ret = false;
2416 }
2417
2418 extendedPortMutex.unlock();
2419 return ret;
2420}
2421
2422bool RemoteControlBoard::setControlMode(const int j, const int mode)
2423{
2424 if (!isLive()) {
2425 return false;
2426 }
2427 Bottle cmd, response;
2428 cmd.addVocab32(VOCAB_SET);
2431 cmd.addInt32(j);
2432 cmd.addVocab32(mode);
2433
2434 bool ok = rpc_p.write(cmd, response);
2435 return CHECK_FAIL(ok, response);
2436}
2437
2438bool RemoteControlBoard::setControlModes(const int n_joint, const int *joints, int *modes)
2439{
2440 if (!isLive()) {
2441 return false;
2442 }
2443 Bottle cmd, response;
2444 cmd.addVocab32(VOCAB_SET);
2447 cmd.addInt32(n_joint);
2448 int i;
2449 Bottle& l1 = cmd.addList();
2450 for (i = 0; i < n_joint; i++) {
2451 l1.addInt32(joints[i]);
2452 }
2453
2454 Bottle& l2 = cmd.addList();
2455 for (i = 0; i < n_joint; i++) {
2456 l2.addVocab32(modes[i]);
2457 }
2458
2459 bool ok = rpc_p.write(cmd, response);
2460 return CHECK_FAIL(ok, response);
2461}
2462
2464{
2465 if (!isLive()) {
2466 return false;
2467 }
2468 Bottle cmd, response;
2469 cmd.addVocab32(VOCAB_SET);
2472
2473 Bottle& l2 = cmd.addList();
2474 for (size_t i = 0; i < nj; i++) {
2475 l2.addVocab32(modes[i]);
2476 }
2477
2478 bool ok = rpc_p.write(cmd, response);
2479 return CHECK_FAIL(ok, response);
2480}
2481
2482// END IControlMode
2483
2484// BEGIN IPositionDirect
2485
2486bool RemoteControlBoard::setPosition(int j, double ref)
2487{
2488 if (!isLive()) {
2489 return false;
2490 }
2491 CommandMessage& c = command_buffer.get();
2492 c.head.clear();
2493 c.head.addVocab32(VOCAB_POSITION_DIRECT);
2494 c.head.addInt32(j);
2495 c.body.resize(1);
2496 memcpy(&(c.body[0]), &ref, sizeof(double));
2498 return true;
2499}
2500
2501bool RemoteControlBoard::setPositions(const int n_joint, const int *joints, const double *refs)
2502{
2503 if (!isLive()) {
2504 return false;
2505 }
2506 CommandMessage& c = command_buffer.get();
2507 c.head.clear();
2508 c.head.addVocab32(VOCAB_POSITION_DIRECT_GROUP);
2509 c.head.addInt32(n_joint);
2510 Bottle &jointList = c.head.addList();
2511 for (int i = 0; i < n_joint; i++) {
2512 jointList.addInt32(joints[i]);
2513 }
2514 c.body.resize(n_joint);
2515 memcpy(&(c.body[0]), refs, sizeof(double)*n_joint);
2517 return true;
2518}
2519
2520bool RemoteControlBoard::setPositions(const double *refs)
2521{
2522 if (!isLive()) {
2523 return false;
2524 }
2525 CommandMessage& c = command_buffer.get();
2526 c.head.clear();
2527 c.head.addVocab32(VOCAB_POSITION_DIRECTS);
2528 c.body.resize(nj);
2529 memcpy(&(c.body[0]), refs, sizeof(double)*nj);
2531 return true;
2532}
2533
2534bool RemoteControlBoard::getRefPosition(const int joint, double* ref)
2535{
2536 return get1V1I1D(VOCAB_POSITION_DIRECT, joint, ref);
2537}
2538
2540{
2541 return get1V1DA(VOCAB_POSITION_DIRECTS, refs);
2542}
2543
2544bool RemoteControlBoard::getRefPositions(const int n_joint, const int* joints, double* refs)
2545{
2546 return get1V1I1IA1DA(VOCAB_POSITION_DIRECT_GROUP, n_joint, joints, refs);
2547}
2548
2549// END IPositionDirect
2550
2551// BEGIN IVelocityControl
2552
2553bool RemoteControlBoard::velocityMove(const int n_joint, const int *joints, const double *spds)
2554{
2555 // streaming port
2556 if (!isLive()) {
2557 return false;
2558 }
2559 CommandMessage& c = command_buffer.get();
2560 c.head.clear();
2561 c.head.addVocab32(VOCAB_VELOCITY_MOVE_GROUP);
2562 c.head.addInt32(n_joint);
2563 Bottle &jointList = c.head.addList();
2564 for (int i = 0; i < n_joint; i++) {
2565 jointList.addInt32(joints[i]);
2566 }
2567 c.body.resize(n_joint);
2568 memcpy(&(c.body[0]), spds, sizeof(double)*n_joint);
2570 return true;
2571}
2572
2573bool RemoteControlBoard::getRefVelocity(const int joint, double* vel)
2574{
2575 return get1V1I1D(VOCAB_VELOCITY_MOVE, joint, vel);
2576}
2577
2579{
2580 return get1VDA(VOCAB_VELOCITY_MOVES, vels);
2581}
2582
2583bool RemoteControlBoard::getRefVelocities(const int n_joint, const int* joints, double* vels)
2584{
2585 return get1V1I1IA1DA(VOCAB_VELOCITY_MOVE_GROUP, n_joint, joints, vels);
2586}
2587
2588// END IVelocityControl
2589
2590// BEGIN IInteractionMode
2591
2593{
2594 double localArrivalTime=0.0;
2595 extendedPortMutex.lock();
2596 bool ret = extendedIntputStatePort.getLastSingle(axis, VOCAB_INTERACTION_MODE, (int*) mode, lastStamp, localArrivalTime);
2597 extendedPortMutex.unlock();
2598 return ret;
2599}
2600
2602{
2603 double localArrivalTime=0.0;
2604
2605 extendedPortMutex.lock();
2607 if(ret)
2608 {
2609 for (int i = 0; i < n_joints; i++) {
2611 }
2612 } else {
2613 ret = false;
2614 }
2615
2616 extendedPortMutex.unlock();
2617 return ret;
2618}
2619
2621{
2622 double localArrivalTime=0.0;
2623 extendedPortMutex.lock();
2624 bool ret = extendedIntputStatePort.getLastVector(VOCAB_INTERACTION_MODES, (int*) modes, lastStamp, localArrivalTime);
2625 extendedPortMutex.unlock();
2626 return ret;
2627}
2628
2630{
2631 Bottle cmd, response;
2632 if (!isLive()) {
2633 return false;
2634 }
2635
2636 cmd.addVocab32(VOCAB_SET);
2639 cmd.addInt32(axis);
2640 cmd.addVocab32(mode);
2641
2642 bool ok = rpc_p.write(cmd, response);
2643 return CHECK_FAIL(ok, response);
2644}
2645
2647{
2648 Bottle cmd, response;
2649 if (!isLive()) {
2650 return false;
2651 }
2652
2653 cmd.addVocab32(VOCAB_SET);
2656 cmd.addInt32(n_joints);
2657
2658 Bottle& l1 = cmd.addList();
2659 for (int i = 0; i < n_joints; i++) {
2660 l1.addInt32(joints[i]);
2661 }
2662
2663 Bottle& l2 = cmd.addList();
2664 for (int i = 0; i < n_joints; i++)
2665 {
2666 l2.addVocab32(modes[i]);
2667 }
2668 bool ok = rpc_p.write(cmd, response);
2669 return CHECK_FAIL(ok, response);
2670}
2671
2673{
2674 Bottle cmd, response;
2675 if (!isLive()) {
2676 return false;
2677 }
2678
2679 cmd.addVocab32(VOCAB_SET);
2682
2683 Bottle& l1 = cmd.addList();
2684 for (size_t i = 0; i < nj; i++) {
2685 l1.addVocab32(modes[i]);
2686 }
2687
2688 bool ok = rpc_p.write(cmd, response);
2689 return CHECK_FAIL(ok, response);
2690}
2691
2692// END IInteractionMode
2693
2694// BEGIN IRemoteCalibrator
2695
2697{
2698 Bottle cmd, response;
2699 cmd.addVocab32(VOCAB_GET);
2702 bool ok = rpc_p.write(cmd, response);
2703 if(ok) {
2704 *isCalib = response.get(2).asInt32()!=0;
2705 } else {
2706 *isCalib = false;
2707 }
2708 return CHECK_FAIL(ok, response);
2709}
2710
2712{
2714}
2715
2717{
2718 Bottle cmd, response;
2719 cmd.addVocab32(VOCAB_SET);
2722 bool ok = rpc_p.write(cmd, response);
2723 return CHECK_FAIL(ok, response);
2724}
2725
2727{
2729}
2730
2732{
2733 Bottle cmd, response;
2734 cmd.addVocab32(VOCAB_SET);
2737 bool ok = rpc_p.write(cmd, response);
2738 yCDebug(REMOTECONTROLBOARD) << "Sent homing whole part message";
2739 return CHECK_FAIL(ok, response);
2740}
2741
2743{
2745}
2746
2748{
2749 Bottle cmd, response;
2750 cmd.addVocab32(VOCAB_SET);
2753 bool ok = rpc_p.write(cmd, response);
2754 return CHECK_FAIL(ok, response);
2755}
2756
2758{
2759 Bottle cmd, response;
2760 cmd.addVocab32(VOCAB_SET);
2763 bool ok = rpc_p.write(cmd, response);
2764 return CHECK_FAIL(ok, response);
2765}
2766
2768{
2769 Bottle cmd, response;
2770 cmd.addVocab32(VOCAB_SET);
2773 bool ok = rpc_p.write(cmd, response);
2774 return CHECK_FAIL(ok, response);
2775}
2776
2777// END IRemoteCalibrator
2778
2779// BEGIN ICurrentControl
2780
2782{
2784}
2785
2787{
2789}
2790
2792{
2793 if (!isLive()) {
2794 return false;
2795 }
2796 CommandMessage& c = command_buffer.get();
2797 c.head.clear();
2799 c.head.addVocab32(VOCAB_CURRENT_REFS);
2800 c.body.resize(nj);
2801 memcpy(&(c.body[0]), refs, sizeof(double)*nj);
2803 return true;
2804}
2805
2807{
2808 if (!isLive()) {
2809 return false;
2810 }
2811 CommandMessage& c = command_buffer.get();
2812 c.head.clear();
2814 c.head.addVocab32(VOCAB_CURRENT_REF);
2815 c.head.addInt32(j);
2816 c.body.resize(1);
2817 memcpy(&(c.body[0]), &ref, sizeof(double));
2819 return true;
2820}
2821
2822bool RemoteControlBoard::setRefCurrents(const int n_joint, const int *joints, const double *refs)
2823{
2824 if (!isLive()) {
2825 return false;
2826 }
2827 CommandMessage& c = command_buffer.get();
2828 c.head.clear();
2830 c.head.addVocab32(VOCAB_CURRENT_REF_GROUP);
2831 c.head.addInt32(n_joint);
2832 Bottle &jointList = c.head.addList();
2833 for (int i = 0; i < n_joint; i++) {
2834 jointList.addInt32(joints[i]);
2835 }
2836 c.body.resize(n_joint);
2837 memcpy(&(c.body[0]), refs, sizeof(double)*n_joint);
2839 return true;
2840}
2841
2843{
2844 double localArrivalTime=0.0;
2845 extendedPortMutex.lock();
2846 bool ret = extendedIntputStatePort.getLastVector(VOCAB_AMP_CURRENTS, vals, lastStamp, localArrivalTime);
2847 extendedPortMutex.unlock();
2848 return ret;
2849}
2850
2851bool RemoteControlBoard::getCurrent(int j, double *val)
2852{
2853 double localArrivalTime=0.0;
2854 extendedPortMutex.lock();
2855 bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_AMP_CURRENT, val, lastStamp, localArrivalTime);
2856 extendedPortMutex.unlock();
2857 return ret;
2858}
2859
2860bool RemoteControlBoard::getCurrentRange(int j, double *min, double *max)
2861{
2863}
2864
2865bool RemoteControlBoard::getCurrentRanges(double *min, double *max)
2866{
2868}
2869
2870// END ICurrentControl
2871
2872// BEGIN IPWMControl
2874{
2875 // using the streaming port
2876 if (!isLive()) {
2877 return false;
2878 }
2879 CommandMessage& c = command_buffer.get();
2880 c.head.clear();
2881 // in streaming port only SET command can be sent, so it is implicit
2882 c.head.addVocab32(VOCAB_PWMCONTROL_INTERFACE);
2883 c.head.addVocab32(VOCAB_PWMCONTROL_REF_PWM);
2884 c.head.addInt32(j);
2885
2886 c.body.clear();
2887 c.body.resize(1);
2888 c.body[0] = v;
2890 return true;
2891}
2892
2894{
2895 // using the streaming port
2896 if (!isLive()) {
2897 return false;
2898 }
2899 CommandMessage& c = command_buffer.get();
2900 c.head.clear();
2901 c.head.addVocab32(VOCAB_PWMCONTROL_INTERFACE);
2902 c.head.addVocab32(VOCAB_PWMCONTROL_REF_PWMS);
2903
2904 c.body.resize(nj);
2905
2906 memcpy(&(c.body[0]), v, sizeof(double)*nj);
2907
2909
2910 return true;
2911}
2912
2914{
2915 Bottle cmd, response;
2916 cmd.addVocab32(VOCAB_GET);
2919 cmd.addInt32(j);
2920 response.clear();
2921
2922 bool ok = rpc_p.write(cmd, response);
2923
2924 if (CHECK_FAIL(ok, response))
2925 {
2926 // ok
2927 *ref = response.get(2).asFloat64();
2928
2929 getTimeStamp(response, lastStamp);
2930 return true;
2931 } else {
2932 return false;
2933 }
2934}
2935
2937{
2939}
2940
2941bool RemoteControlBoard::getDutyCycle(int j, double *out)
2942{
2943 double localArrivalTime = 0.0;
2944 extendedPortMutex.lock();
2946 extendedPortMutex.unlock();
2947 return ret;
2948}
2949
2951{
2952 double localArrivalTime = 0.0;
2953 extendedPortMutex.lock();
2955 extendedPortMutex.unlock();
2956 return ret;
2957}
2958// END IPWMControl
constexpr yarp::conf::vocab32_t VOCAB_PARK_SINGLE_JOINT
constexpr yarp::conf::vocab32_t VOCAB_REMOTE_CALIBRATOR_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_HOMING_WHOLE_PART
constexpr yarp::conf::vocab32_t VOCAB_PARK_WHOLE_PART
constexpr yarp::conf::vocab32_t VOCAB_QUIT_PARK
constexpr yarp::conf::vocab32_t VOCAB_IS_CALIBRATOR_PRESENT
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_SINGLE_JOINT
constexpr yarp::conf::vocab32_t VOCAB_HOMING_SINGLE_JOINT
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_WHOLE_PART
constexpr yarp::conf::vocab32_t VOCAB_QUIT_CALIBRATE
define control board standard interfaces
constexpr yarp::conf::vocab32_t VOCAB_TIMESTAMP
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVE
constexpr yarp::conf::vocab32_t VOCAB_REF_SPEED
constexpr yarp::conf::vocab32_t VOCAB_STOPS
constexpr yarp::conf::vocab32_t VOCAB_MOTION_DONE
constexpr yarp::conf::vocab32_t VOCAB_STOP
constexpr yarp::conf::vocab32_t VOCAB_REF_ACCELERATIONS
constexpr yarp::conf::vocab32_t VOCAB_MOTION_DONES
constexpr yarp::conf::vocab32_t VOCAB_PROTOCOL_VERSION
constexpr yarp::conf::vocab32_t VOCAB_POSITION_MOVE
constexpr yarp::conf::vocab32_t VOCAB_REF_ACCELERATION
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVES
constexpr yarp::conf::vocab32_t VOCAB_RELATIVE_MOVE
constexpr yarp::conf::vocab32_t VOCAB_REF_SPEEDS
constexpr yarp::conf::vocab32_t VOCAB_RELATIVE_MOVES
constexpr yarp::conf::vocab32_t VOCAB_POSITION_MOVES
constexpr int PROTOCOL_VERSION_TWEAK
constexpr int PROTOCOL_VERSION_MINOR
constexpr int PROTOCOL_VERSION_MAJOR
float t
constexpr yarp::conf::vocab32_t VOCAB_LIM
Definition: GenericVocabs.h:27
constexpr yarp::conf::vocab32_t VOCAB_OUTPUTS
Definition: GenericVocabs.h:33
constexpr yarp::conf::vocab32_t VOCAB_ENABLE
Definition: GenericVocabs.h:31
constexpr yarp::conf::vocab32_t VOCAB_DISABLE
Definition: GenericVocabs.h:30
constexpr yarp::conf::vocab32_t VOCAB_REF
Definition: GenericVocabs.h:24
constexpr yarp::conf::vocab32_t VOCAB_ERRS
Definition: GenericVocabs.h:18
constexpr yarp::conf::vocab32_t VOCAB_GET
Definition: GenericVocabs.h:13
constexpr yarp::conf::vocab32_t VOCAB_OUTPUT
Definition: GenericVocabs.h:32
constexpr yarp::conf::vocab32_t VOCAB_ERR
Definition: GenericVocabs.h:17
constexpr yarp::conf::vocab32_t VOCAB_REFS
Definition: GenericVocabs.h:25
constexpr yarp::conf::vocab32_t VOCAB_RESET
Definition: GenericVocabs.h:29
constexpr yarp::conf::vocab32_t VOCAB_AXES
Definition: GenericVocabs.h:36
constexpr yarp::conf::vocab32_t VOCAB_SET
Definition: GenericVocabs.h:12
constexpr yarp::conf::vocab32_t VOCAB_OFFSET
Definition: GenericVocabs.h:23
constexpr yarp::conf::vocab32_t VOCAB_LIMS
Definition: GenericVocabs.h:28
constexpr yarp::conf::vocab32_t VOCAB_AMP_DISABLE
constexpr yarp::conf::vocab32_t VOCAB_AMP_PEAK_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_AMP_ENABLE
constexpr yarp::conf::vocab32_t VOCAB_AMP_VOLTAGE_SUPPLY
constexpr yarp::conf::vocab32_t VOCAB_AMP_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_AMP_MAXCURRENT
constexpr yarp::conf::vocab32_t VOCAB_AMP_STATUS_SINGLE
constexpr yarp::conf::vocab32_t VOCAB_AMP_STATUS
constexpr yarp::conf::vocab32_t VOCAB_AMP_NOMINAL_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_AMP_CURRENTS
constexpr yarp::conf::vocab32_t VOCAB_AMP_PWM_LIMIT
constexpr yarp::conf::vocab32_t VOCAB_INFO_TYPE
Definition: IAxisInfo.h:86
constexpr yarp::conf::vocab32_t VOCAB_INFO_NAME
Definition: IAxisInfo.h:85
constexpr yarp::conf::vocab32_t VOCAB_ABORTCALIB
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_JOINT_PARAMS
constexpr yarp::conf::vocab32_t VOCAB_ABORTPARK
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_DONE
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_JOINT
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE
constexpr yarp::conf::vocab32_t VOCAB_PARK
constexpr yarp::conf::vocab32_t VOCAB_LIMITS
constexpr yarp::conf::vocab32_t VOCAB_VEL_LIMITS
constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODE_GROUP
Definition: IControlMode.h:116
constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODES
Definition: IControlMode.h:117
constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODE
Definition: IControlMode.h:115
constexpr yarp::conf::vocab32_t VOCAB_ICONTROLMODE
Definition: IControlMode.h:113
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REF_GROUP
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REFS
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_RANGE
constexpr yarp::conf::vocab32_t VOCAB_CURRENTCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REF
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_RANGES
constexpr yarp::conf::vocab32_t VOCAB_E_RESET
Definition: IEncoders.h:204
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_ACCELERATIONS
Definition: IEncoders.h:213
constexpr yarp::conf::vocab32_t VOCAB_ENCODER
Definition: IEncoders.h:206
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_SPEEDS
Definition: IEncoders.h:211
constexpr yarp::conf::vocab32_t VOCAB_E_RESETS
Definition: IEncoders.h:205
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_SPEED
Definition: IEncoders.h:210
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_ACCELERATION
Definition: IEncoders.h:212
constexpr yarp::conf::vocab32_t VOCAB_ENCODERS
Definition: IEncoders.h:207
constexpr yarp::conf::vocab32_t VOCAB_IMPEDANCE
constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODES
constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODE
constexpr yarp::conf::vocab32_t VOCAB_INTERFACE_INTERACTION_MODE
constexpr yarp::conf::vocab32_t VOCAB_JF_GET_JOINTFAULT
Definition: IJointFault.h:42
constexpr yarp::conf::vocab32_t VOCAB_IJOINTFAULT
Definition: IJointFault.h:41
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_SPEED
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_ACCELERATION
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_SPEEDS
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_NUMBER
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_E_RESETS
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_E_RESET
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODERS
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_CPR
constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURES
Definition: IMotor.h:162
constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURE_LIMIT
Definition: IMotor.h:163
constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURE
Definition: IMotor.h:160
constexpr yarp::conf::vocab32_t VOCAB_GEARBOX_RATIO
Definition: IMotor.h:161
constexpr yarp::conf::vocab32_t VOCAB_MOTORS_NUMBER
Definition: IMotor.h:159
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_INTERFACE
Definition: IPWMControl.h:139
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_PWM_OUTPUTS
Definition: IPWMControl.h:144
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_REF_PWMS
Definition: IPWMControl.h:142
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_REF_PWM
Definition: IPWMControl.h:141
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_PWM_OUTPUT
Definition: IPWMControl.h:143
constexpr yarp::conf::vocab32_t VOCAB_PIDS
Definition: IPidControl.h:378
constexpr yarp::conf::vocab32_t VOCAB_PID
Definition: IPidControl.h:377
constexpr yarp::conf::vocab32_t VOCAB_REF_ACCELERATION_GROUP
constexpr yarp::conf::vocab32_t VOCAB_STOP_GROUP
constexpr yarp::conf::vocab32_t VOCAB_RELATIVE_MOVE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_POSITION_MOVE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_MOTION_DONE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_REF_SPEED_GROUP
constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECTS
constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECT_GROUP
constexpr yarp::conf::vocab32_t VOCAB_LIST_VARIABLES
constexpr yarp::conf::vocab32_t VOCAB_VARIABLE
constexpr yarp::conf::vocab32_t VOCAB_REMOTE_VARIABILE_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_TORQUE
constexpr yarp::conf::vocab32_t VOCAB_TRQ
constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECTS
constexpr yarp::conf::vocab32_t VOCAB_TRQS
constexpr yarp::conf::vocab32_t VOCAB_RANGE
constexpr yarp::conf::vocab32_t VOCAB_IMP_OFFSET
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_PARAMS
constexpr yarp::conf::vocab32_t VOCAB_IMP_PARAM
constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECT_GROUP
constexpr yarp::conf::vocab32_t VOCAB_RANGES
constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVE_GROUP
bool ret
const yarp::os::LogComponent & REMOTECONTROLBOARD()
void run() override
Loop function.
void setOwner(StateExtendedInputPort *o)
bool setPidReferences(const PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
bool getTorques(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
bool relativeMove(int j, double delta) override
Set relative position.
bool getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
Get the current impedandance limits for a specific joint.
bool getRefSpeeds(const int n_joint, const int *joints, double *spds) override
Get reference speed of all joints.
bool get1V1I1B(int v, int j, bool &val)
Helper method used to get a double value from the remote peer.
bool quitCalibrate() override
quitCalibrate: interrupt the calibration procedure
bool getRefPositions(double *refs) override
Get the last position reference for all axes.
yarp::os::PortReaderBuffer< yarp::sig::Vector > state_buffer
bool get1V1I1IA1B(int v, const int len, const int *val1, bool &retVal)
bool open(Searchable &config) override
Open the DeviceDriver.
bool getEncoder(int j, double *v) override
Read the value of an encoder.
bool send2V1I(int v1, int v2, int axis)
bool setPositions(const int n_joint, const int *joints, const double *refs) override
Set new reference point for all axes.
yarp::dev::impl::jointData last_wholePart
yarp::os::Port rpc_p
bool setControlMode(const int j, const int mode) override
Set the current control mode.
bool isPidEnabled(const PidControlTypeEnum &pidtype, int j, bool *enabled) override
Get the current status (enabled/disabled) of the pid.
bool getPeakCurrent(int m, double *val) override
bool getLimits(int axis, double *min, double *max) override
Get the software limits for a particular axis.
bool getPWM(int m, double *val) override
yarp::os::PortWriterBuffer< CommandMessage > command_buffer
bool get1VDA(int v, double *val)
Helper method to get an array of double from the remote peer.
bool set2V1DA(int v1, int v2, const double *val)
bool get1V1I(int code, int &v) const
Send a GET command expecting an integer value in return.
bool getNominalCurrent(int m, double *val) override
bool resetPid(const PidControlTypeEnum &pidtype, int j) override
Reset the controller of a given joint, usually sets the current status of the joint as the reference ...
bool setPeakCurrent(int m, const double val) override
bool getPidOutputs(const PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
bool send2V(int v1, int v2)
bool set1V1I(int code, int v)
Send a SET command with an additional integer valued variable and then wait for a reply.
bool setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode) override
Set the interaction mode of the robot, values can be stiff or compliant.
bool getTemperatureLimit(int m, double *val) override
Retreives the current temperature limit for a specific motor.
bool getPidError(const PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
bool getPidReference(const PidControlTypeEnum &pidtype, int j, double *ref) override
Get the current reference of the pid controller for a specific joint.
bool homingSingleJoint(int j) override
homingSingleJoint: call the homing procedure for a single joint
bool checkMotionDone(int j, bool *flag) override
Check if the current trajectory is terminated.
bool getMotorEncoderCountsPerRevolution(int m, double *cpr) override
Gets number of counts per revolution for motor encoder m.
bool setMotorEncoderCountsPerRevolution(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
ProtocolVersion protocolVersion
bool set2V1I(int v1, int v2, int axis)
bool setRefDutyCycle(int j, double v) override
Sets the reference dutycycle to a single motor.
bool getRefAccelerations(const int n_joint, const int *joints, double *accs) override
Get reference acceleration for a joint.
bool setPidErrorLimit(const PidControlTypeEnum &pidtype, int j, double limit) override
Set the error limit for the controller on a specifi joint.
bool setMaxCurrent(int j, double v) override
bool getPowerSupplyVoltage(int m, double *val) override
bool getRefVelocity(const int joint, double *vel) override
Get the last reference speed set by velocityMove for single joint.
bool getPidOutput(const PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
bool get1V1I1IA1DA(int v, const int len, const int *val1, double *val2)
bool get1V1DA(int v1, double *val)
Helper method to get an array of double from the remote peer.
bool setRefSpeed(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
bool setInteractionModes(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
Set the interaction mode of the robot for a set of joints, values can be stiff or compliant.
bool getRefCurrent(int j, double *t) override
Get the reference value of the current for a single motor.
bool getEncoders(double *encs) override
Read the position of all axes.
bool get1V1I1S(int code, int j, std::string &name)
bool set1V(int code)
Send a SET command without parameters and wait for a reply.
Stamp getLastInputStamp() override
Get the time stamp for the last read data.
bool getMotorEncoder(int j, double *v) override
Read the value of a motor encoder.
bool getTemperature(int m, double *val) override
Get temperature of a motor.
bool setPidReference(const PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
bool setImpedance(int j, double stiffness, double damping) override
Set current impedance gains (stiffness,damping) for a specific joint.
bool getMotorEncoderTimed(int j, double *v, double *t) override
Read the instantaneous position of a motor encoder.
bool getDutyCycles(double *outs) override
Gets the current dutycycle of the output of the amplifier (i.e.
bool setRefCurrent(int j, double ref) override
Set the reference value of the current for a single motor.
DiagnosticThread * diagnosticThread
bool setControlModes(const int n_joint, const int *joints, int *modes) override
Set the current control mode for a subset of axes.
bool setPid(const PidControlTypeEnum &pidtype, int j, const Pid &pid) override
Set new pid value for a joint axis.
bool set1V1I2D(int code, int j, double val1, double val2)
bool getPidErrors(const PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
bool close() override
Close the device driver and stop the port connections.
bool parkSingleJoint(int j, bool _wait=true) override
parkSingleJoint(): start the parking procedure for the single joint
bool setMotorEncoders(const double *vals) override
Set the value of all motor encoders.
yarp::dev::impl::jointData last_singleJoint
bool getTargetPositions(double *refs) override
Get the last position reference for all axes.
bool setEncoders(const double *vals) override
Set the value of all encoders.
bool set1VDA(int v, const double *val)
Helper method used to set an array of double to all axes.
bool getPid(const PidControlTypeEnum &pidtype, int j, Pid *pid) override
Get current pid value for a specific joint.
bool setPosition(int j, double ref) override
Set new position for a single axis.
bool get2V1I2D(int v1, int v2, int j, double *val1, double *val2)
bool getJointType(int j, yarp::dev::JointTypeEnum &type) override
bool resetMotorEncoder(int j) override
Reset motor encoder, single motor.
bool getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
bool setGearboxRatio(int m, const double val) override
Set the gearbox ratio for a specific motor.
bool resetMotorEncoders() override
Reset motor encoders.
bool getCurrentRanges(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
bool getAmpStatus(int *st) override
bool setRefTorque(int j, double v) override
Set the reference value of the torque for a given joint.
bool enablePid(const PidControlTypeEnum &pidtype, int j) override
Enable the pid computation for a joint.
bool calibrateWholePart() override
calibrateWholePart: call the procedure for calibrating the whole device
bool getImpedance(int j, double *stiffness, double *damping) override
Get current impedance gains (stiffness,damping,offset) for a specific joint.
bool getPidErrorLimits(const PidControlTypeEnum &pidtype, double *limits) override
Get the error limit for all controllers.
bool getMotorEncoders(double *encs) override
Read the position of all motor encoders.
bool setPids(const PidControlTypeEnum &pidtype, const Pid *pids) override
Set new pid value on multiple axes.
bool setMotorEncoder(int j, const double val) override
Set the value of the motor encoder for a given motor.
bool velocityMove(int j, double v) override
Start motion at a given speed, single joint.
bool setRemoteVariable(std::string key, const yarp::os::Bottle &val) override
bool setPidErrorLimits(const PidControlTypeEnum &pidtype, const double *limits) override
Get the error limit for the controller on all joints.
bool calibrationDone(int j) override
Check if the calibration is terminated, on a particular joint.
bool setPidOffset(const PidControlTypeEnum &pidtype, int j, double v) override
Set offset value for a given controller.
bool get2V1I1D(int v1, int v2, int j, double *val)
bool abortCalibration() override
bool setRefCurrents(const double *refs) override
Set the reference value of the currents for all motors.
bool getMotorEncoderAcceleration(int j, double *acc) override
Read the instantaneous acceleration of a motor encoder.
bool setRefTorques(const double *t) override
Set the reference value of the torque for all joints.
bool getNumberOfMotors(int *num) override
Retrieves the number of controlled axes from the current physical interface.
bool getInteractionModes(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
bool get1VIA(int v, int *val)
Helper method to get an array of integers from the remote peer.
bool set1V1I1IA1DA(int v, const int len, const int *val1, const double *val2)
bool getInteractionMode(int axis, yarp::dev::InteractionModeEnum *mode) override
Get the current interaction mode of the robot, values can be stiff or compliant.
bool getCurrentRange(int j, double *min, double *max) override
Get the full scale of the current measurement for a given motor (e.g.
bool getEncoderAcceleration(int j, double *acc) override
Read the instantaneous acceleration of an axis.
bool get1V1B(int v, bool &val)
bool getVelLimits(int axis, double *min, double *max) override
Get the software speed limits for a particular axis.
bool setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
bool send1V1I(int v, int axis)
bool get1V1I2D(int code, int axis, double *v1, double *v2)
bool resetEncoders() override
Reset encoders.
bool checkProtocolVersion(bool ignore)
bool setImpedanceOffset(int j, double offset) override
Set current force Offset for a specific joint.
bool get2V1I1IA1DA(int v1, int v2, const int n_joints, const int *joints, double *retVals, std::string functionName="")
bool getGearboxRatio(int m, double *val) override
Get the gearbox ratio for a specific motor.
bool getRefAcceleration(int j, double *acc) override
Get reference acceleration for a joint.
bool getMaxCurrent(int j, double *v) override
Returns the maximum electric current allowed for a given motor.
bool set2V2DA(int v1, int v2, const double *val1, const double *val2)
bool get2V1DA(int v1, int v2, double *val)
Helper method to get an array of double from the remote peer.
bool setRefAccelerations(const int n_joint, const int *joints, const double *accs) override
Set reference acceleration on all joints.
bool setNominalCurrent(int m, const double val) override
bool getMotorTorqueParams(int j, MotorTorqueParameters *params) override
Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
bool getRefTorques(double *t) override
Get the reference value of the torque for all joints.
bool 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).
bool getRemoteVariable(std::string key, yarp::os::Bottle &val) override
bool getRefDutyCycles(double *refs) override
Gets the last reference sent using the setRefDutyCycles function.
bool set1V2D(int code, double v)
Send a SET command and an additional double valued variable and then wait for a reply.
bool getRemoteVariablesList(yarp::os::Bottle *listOfKeys) override
bool getMotorEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of a motor encoder.
bool getTemperatures(double *vals) override
Get temperature of all the motors.
bool get2V2DA(int v1, int v2, double *val1, double *val2)
bool getLastJointFault(int j, int &fault, std::string &message) override
bool get1V1D(int code, double &v) const
Send a GET command expecting a double value in return.
yarp::os::Port command_p
bool setRefSpeeds(const int n_joint, const int *joints, const double *spds) override
Set reference speed on all joints.
bool getControlMode(int j, int *mode) override
Get the current control mode.
bool isCalibratorDevicePresent(bool *isCalib) override
isCalibratorDevicePresent: check if a calibrator device has been set
bool homingWholePart() override
homingWholePart: call the homing procedure for a the whole part/device
bool set2V1I1D(int v1, int v2, int axis, double val)
bool parkWholePart() override
parkWholePart: start the parking procedure for the whole part
bool calibrateRobot() override
Calibrate robot by using an external calibrator.
bool getControlModes(int *modes) override
Get the current control mode (multiple joints).
bool getMotorEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all motor encoders.
bool resetEncoder(int j) override
Reset encoder, single joint.
bool getDutyCycle(int j, double *out) override
Gets the current dutycycle of the output of the amplifier (i.e.
bool getTargetPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
bool send3V1I(int v1, int v2, int v3, int j)
bool getRefVelocities(double *vels) override
Get the last reference speed set by velocityMove for all joints.
bool getCurrent(int j, double *val) override
bool getAxes(int *ax) override
Get the number of controlled axes.
StateExtendedInputPort extendedIntputStatePort
bool getPidErrorLimit(const PidControlTypeEnum &pidtype, int j, double *limit) override
Get the error limit for the controller on a specific joint.
bool stop() override
Stop motion, multiple joints.
bool setPWMLimit(int m, const double val) override
bool getRefSpeed(int j, double *ref) override
Get reference speed for a joint.
bool getRefPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
bool set1V1I1D(int code, int j, double val)
Helper method to set a double value to a single axis.
bool getEncoderTimed(int j, double *v, double *t) override
Read the instantaneous acceleration of all axes.
bool setCalibrationParameters(int j, const CalibrationParameters &params) override
Start calibration, this method is very often platform specific.
bool getRefTorque(int j, double *t) override
Get the reference value of the torque for a given joint.
bool setTemperatureLimit(int m, const double val) override
Set the temperature limit for a specific motor.
bool setValWithPidType(int voc, PidControlTypeEnum type, int axis, double val)
bool calibrateSingleJoint(int j) override
calibrateSingleJoint: call the calibration procedure for the single joint
bool enableAmp(int j) override
Enable the amplifier on a specific joint.
bool getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
bool getTorqueRanges(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
bool park(bool wait=true) override
bool setRefAcceleration(int j, double acc) override
Set reference acceleration for a joint.
bool getRefDutyCycle(int j, double *ref) override
Gets the last reference sent using the setRefDutyCycle function.
bool getMotorEncodersTimed(double *encs, double *ts) override
Read the instantaneous position of all motor encoders.
bool getImpedanceOffset(int j, double *offset) override
Get current force Offset for a specific joint.
bool getValWithPidType(int voc, PidControlTypeEnum type, int j, double *val)
bool calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) override
Start calibration, this method is very often platform specific.
bool getNumberOfMotorEncoders(int *num) override
Get the number of available motor encoders.
bool disablePid(const PidControlTypeEnum &pidtype, int j) override
Disable the pid computation for a joint.
bool getMotorEncoderSpeeds(double *spds) override
Read the instantaneous speed of all motor encoders.
bool getPWMLimit(int m, double *val) override
bool getCurrents(double *vals) override
bool disableAmp(int j) override
Disable the amplifier on a specific joint.
bool setVelLimits(int axis, double min, double max) override
Set the software speed limits for a particular axis, the behavior of the control card when these limi...
bool getEncodersTimed(double *encs, double *ts) override
Read the instantaneous acceleration of all axes.
bool getPids(const PidControlTypeEnum &pidtype, Pid *pids) override
Get current pid value for a specific joint.
bool get1V1I1D(int v, int j, double *val)
Helper method used to get a double value from the remote peer.
bool getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
bool setMotorTorqueParams(int j, const MotorTorqueParameters params) override
Set a subset of motor parameters (bemf, ktau etc) useful for torque control.
bool setLimits(int axis, double min, double max) override
Set the software limits for a particular axis, the behavior of the control card when these limits are...
bool getTorqueRange(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
bool setRefDutyCycles(const double *v) override
Sets the reference dutycycle for all the motors.
bool positionMove(int j, double ref) override
Set new reference point for a single axis.
bool quitPark() override
quitPark: interrupt the park procedure
bool get1V1I1I(int v, int j, int *val)
Helper method used to get an integer value from the remote peer.
bool getAxisName(int j, std::string &name) override
bool getPidReferences(const PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all pid controllers.
bool getRefCurrents(double *t) override
Get the reference value of the currents for all motors.
void setTimeout(const double &timeout)
setTimeout, set the timeout for retrieving data
bool getLastVector(int field, double *data, Stamp &stamp, double &localArrivalTime)
bool getLastSingle(int j, int field, double *data, Stamp &stamp, double &localArrivalTime)
void getEstFrequency(int &ite, double &av, double &min, double &max)
Contains the parameters for a PID.
double kd
derivative gain
double scale
scale for the pid output
double offset
pwm offset added to the pid output
double stiction_down_val
down stiction offset added to the pid output
double stiction_up_val
up stiction offset added to the pid output
double max_output
max output
double kff
feedforward gain
double ki
integrative gain
double max_int
saturation threshold for the integrator
double kp
proportional gain
yarp::sig::VectorOf< double > motorVelocity
Definition: jointData.h:35
yarp::sig::VectorOf< double > jointAcceleration
Definition: jointData.h:31
yarp::sig::VectorOf< int > controlMode
Definition: jointData.h:45
yarp::sig::VectorOf< int > interactionMode
Definition: jointData.h:47
yarp::sig::VectorOf< double > current
Definition: jointData.h:43
yarp::sig::VectorOf< double > motorAcceleration
Definition: jointData.h:37
yarp::sig::VectorOf< double > motorPosition
Definition: jointData.h:33
yarp::sig::VectorOf< double > pwmDutycycle
Definition: jointData.h:41
yarp::sig::VectorOf< double > torque
Definition: jointData.h:39
yarp::sig::VectorOf< double > jointPosition
Definition: jointData.h:27
yarp::sig::VectorOf< double > jointVelocity
Definition: jointData.h:29
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:64
void addVocab32(yarp::conf::vocab32_t x)
Places a vocabulary item in the bottle, at the end of the list.
Definition: Bottle.cpp:164
void append(const Bottle &alt)
Append the content of the given bottle to the current list.
Definition: Bottle.cpp:380
Bottle & addList()
Places an empty nested list in the bottle, at the end of the list.
Definition: Bottle.cpp:182
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:251
void addFloat64(yarp::conf::float64_t x)
Places a 64-bit floating point number in the bottle, at the end of the list.
Definition: Bottle.cpp:158
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Definition: Bottle.cpp:277
void clear()
Empties the bottle of any objects it contains.
Definition: Bottle.cpp:121
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
Definition: Bottle.cpp:140
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition: Bottle.cpp:170
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition: Bottle.cpp:211
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
Definition: Bottle.cpp:287
std::string getName() const override
Get name of port.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void interrupt() override
Interrupt any current reads or writes attached to the port.
void useCallback(TypedReaderCallback< T > &callback) override
Set an object whose onRead method will be called when data is available.
virtual std::string getName() const
Get name of port.
Definition: Contactable.cpp:14
An abstraction for a periodic thread.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
void setStrict(bool strict=true) override
Call this to strictly keep all messages, or allow old ones to be quietly dropped.
bool write(const PortWriter &writer, const PortWriter *callback=nullptr) const override
Write an object to the port.
Definition: Port.cpp:436
bool addOutput(const std::string &name) override
Add an output connection to the specified port.
Definition: Port.cpp:353
void interrupt() override
Interrupt any current reads or writes attached to the port.
Definition: Port.cpp:383
void close() override
Stop port activity.
Definition: Port.cpp:363
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition: Port.cpp:79
Group a pair of objects to be sent and received together.
Definition: PortablePair.h:47
BODY body
An object of the second type (BODY).
Definition: PortablePair.h:57
HEAD head
An object of the first type (HEAD).
Definition: PortablePair.h:52
Preferences for the port's Quality of Service.
Definition: QosStyle.h:23
void setThreadPriority(int priority)
sets the communication thread priority level
Definition: QosStyle.h:127
bool setPacketPriority(const std::string &priority)
sets the packet priority from a string.
Definition: QosStyle.cpp:39
void setThreadPolicy(int policy)
sets the communication thread scheduling policy
Definition: QosStyle.h:137
A base class for nested structures that can be searched.
Definition: Searchable.h:56
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:21
double getTime() const
Get the time stamp.
Definition: Stamp.cpp:34
A single value (typically within a Bottle).
Definition: Value.h:43
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:222
virtual yarp::conf::vocab32_t asVocab32() const
Get vocabulary identifier as an integer.
Definition: Value.cpp:228
virtual bool asBool() const
Get boolean value.
Definition: Value.cpp:186
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:204
virtual Bottle * asList() const
Get list value.
Definition: Value.cpp:240
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
void resize(size_t size) override
Resize the vector.
Definition: Vector.h:220
T * data()
Return a pointer to the first element of the vector.
Definition: Vector.h:205
#define yCInfo(component,...)
Definition: LogComponent.h:171
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCAssert(component, x)
Definition: LogComponent.h:240
#define yCWarning(component,...)
Definition: LogComponent.h:192
#define yCDebug(component,...)
Definition: LogComponent.h:128
For streams capable of holding different kinds of content, check what they actually have.
PidControlTypeEnum
Definition: PidEnums.h:15
An interface to the operating system, including Port based communication.