YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoard_nwc_yarp.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#include <algorithm>
13
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#include <yarp/os/Vocab32.h>
23
24
26#include <yarp/dev/PolyDriver.h>
30
31#include <mutex>
32
33
34using namespace yarp::os;
35using namespace yarp::dev;
36using namespace yarp::sig;
37
38namespace {
39
40constexpr int PROTOCOL_VERSION_MAJOR = 1;
41constexpr int PROTOCOL_VERSION_MINOR = 9;
42constexpr int PROTOCOL_VERSION_TWEAK = 0;
43
44constexpr double DIAGNOSTIC_THREAD_PERIOD = 1.000;
45
46inline bool getTimeStamp(Bottle &bot, Stamp &st)
47{
48 if (bot.get(3).asVocab32()==VOCAB_TIMESTAMP)
49 {
50 //yup! we have a timestamp
51 int fr=bot.get(4).asInt32();
52 double ts=bot.get(5).asFloat64();
53 st=Stamp(fr,ts);
54 return true;
55 }
56 return false;
57}
58
59} // namespace
60
61
63 public PeriodicThread
64{
65 StateExtendedInputPort *owner{nullptr};
66 std::string ownerName;
67
68public:
70
72 {
73 owner = o;
74 ownerName = owner->getName();
75 }
76
77 void run() override
78 {
79 if (owner != nullptr)
80 {
81 if (owner->getIterations() > 100)
82 {
83 int it;
84 double av;
85 double max;
86 double min;
87 owner->getEstFrequency(it, av, min, max);
88 owner->resetStat();
90 "%s: %d msgs av:%.2lf min:%.2lf max:%.2lf [ms]",
91 ownerName.c_str(),
92 it,
93 av,
94 min,
95 max);
96 }
97
98 }
99 }
100};
101
102
104{
105 if (!m_njIsKnown)
106 {
107 auto ret = m_RPC.getAxesRPC();
108 if (ret.ret)
109 {
110 if (ret.axes >= 0)
111 {
112 m_nj = ret.axes;
113 m_njIsKnown = true;
114 }
115 }
116 }
117 return m_njIsKnown;
118}
119
121{
122 if (!parseParams(config)) { return false; }
123
125
126 //handle local Qos
129 {
130 localQos.setThreadPriority(m_local_qos_thread_priority);
131 localQos.setThreadPolicy(m_local_qos_thread_policy);
132 localQos.setPacketPriority(m_local_qos_packet_priority);
133 }
134
135 //handle remote Qos
138 {
139 remoteQos.setThreadPriority(m_remote_qos_thread_priority);
140 remoteQos.setThreadPolicy(m_remote_qos_thread_policy);
141 remoteQos.setPacketPriority(m_remote_qos_packet_priority);
142 }
143
144 //handle param writeStrict
145 if (m_writeStrict == "on")
146 {
149 yCInfo(CONTROLBOARD_NWC_YARP, "RemoteControlBoard is ENABLING the writeStrict option for all commands");
150 }
151 else if(m_writeStrict == "off")
152 {
155 yCInfo(CONTROLBOARD_NWC_YARP, "RemoteControlBoard is DISABLING the writeStrict option for all commands");
156 }
157 else if (m_writeStrict.empty())
158 {
159 //leave the default values
160 }
161 else
162 {
163 yCError(CONTROLBOARD_NWC_YARP, "Found writeStrict option with wrong value. Accepted options are 'on' or 'off'");
164 return false;
165 }
166
167 //open ports
168 bool portProblem = false;
169 if (m_local != "") {
170 std::string s1;
171 s1 = m_local;
172 s1 += "/command:o";
173 if (!command_p.open(s1)) { portProblem = true; }
174 s1 = m_local;
175 s1 += "/stateExt:i";
176 if (!extendedIntputStatePort.open(s1)) { portProblem = true; }
177 if (!portProblem)
178 {
180 }
181 }
182
183 bool connectionProblem = false;
184 if (m_remote != "" && !portProblem)
185 {
186 std::string s1 = m_remote;
187 s1 += "/nws/rpc:i";
188 std::string s2 = m_local;
189 s2 += "/rpc:o";
190 bool ok = false;
191 // RPC port needs to be tcp, therefore no carrier option is added here
192 // ok=Network::connect(s2.c_str(), s1.c_str()); //This doesn't take into consideration possible YARP_PORT_PREFIX on local ports
193 // 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
194
195 s1 = m_remote;
196 s1 += "/command:i";
197 s2 = m_local;
198 s2 += "/command:o";
199 //ok = Network::connect(s2.c_str(), s1.c_str(), carrier);
200 // ok=Network::connect(command_p.getName(), s1.c_str(), carrier); //doesn't take into consideration possible YARP_PORT_PREFIX on local ports
202 if (!ok) {
203 yCError(CONTROLBOARD_NWC_YARP, "Problem connecting to %s, is the remote device available?", s1.c_str());
204 connectionProblem = true;
205 }
206 // set the QoS preferences for the 'command' port
209 }
210
211 s1 = m_remote;
212 s1 += "/stateExt:o";
213 s2 = m_local;
214 s2 += "/stateExt:i";
215 // not checking return value for now since it is wip (different machines can have different compilation flags
216 ok = Network::connect(s1, extendedIntputStatePort.getName(), m_carrier);
217 if (ok)
218 {
219 // set the QoS preferences for the 'state' port
222 }
223 }
224 else
225 {
226 yCError(CONTROLBOARD_NWC_YARP, "Problem connecting to %s, is the remote device available?", s1.c_str());
227 connectionProblem = true;
228 }
229 }
230
232 {
235 return false;
236 }
237
239
240 // open rpc port
241 std::string local_rpc_portname = m_local + "/nwc/rpc:o";
243 {
244 yCError(CONTROLBOARD_NWC_YARP, "open() error could not open rpc port %s, check network\n", local_rpc_portname.c_str());
247 return false;
248 }
249
250 //Attach the ControlBoardMsgs to the port
252 {
253 yCError(CONTROLBOARD_NWC_YARP, "Error! Cannot attach the port as a client");
257 return false;
258 }
259
260 // connection for the rpc port
261 std::string nws_rpc_portname = m_remote + "/nws/rpc:i";
262 if (!Network::connect(local_rpc_portname, nws_rpc_portname))
263 {
264 yCError(CONTROLBOARD_NWC_YARP, "open() error could not connect to %s\n", nws_rpc_portname.c_str());
268 return false;
269 }
270
271 // Check the protocol version
273 {
277 return false;
278 }
279
280 if (!isLive())
281 {
282 if (m_remote!="")
283 {
284 yCError(CONTROLBOARD_NWC_YARP, "Problems with obtaining the number of controlled axes");
288 return false;
289 }
290 }
291
292 if (m_diagnostic)
293 {
297 }
298
299 // allocate memory for helper struct
300 // single joint
312
313 // whole part (safe here because we already got the m_nj
325
326 //check device capabilities
327 auto ret = m_RPC.getDeviceInterfacesRPC();
328 if (!ret.ret)
329 {
330 yCError(CONTROLBOARD_NWC_YARP, "Unable to getDeviceInterfacesRPC");
334 return false;
335 }
336 m_device_has_interfaces = ret.interfaces;
337
338 return true;
339}
340
342{
343 if (diagnosticThread!=nullptr)
344 {
346 delete diagnosticThread;
347 }
348
349 command_buffer.detach(); //TBC
350
351 m_rpcPort.interrupt(); // TBC
352 command_p.interrupt(); // TBC
354
358 return true;
359}
360
361#if 0
362#define LOCKMUTEX std::lock_guard<std::mutex> lg(m_mutex);
363#else
364#define LOCKMUTEX
365#endif
366
367#define CHECK_INTERFACE(VALUE) \
368 { \
369 if (auto it = m_device_has_interfaces.find(VALUE); \
370 it == m_device_has_interfaces.end() || !it->second) \
371 { \
372 yCError(CONTROLBOARD_NWC_YARP, \
373 "Device does not implement required interface: %s", \
374 VALUE); \
375 return yarp::dev::ReturnValue::return_code:: \
376 return_value_error_not_implemented_by_device; \
377 } \
378 }
379
380// BEGIN IPidControl
381
382ReturnValue ControlBoard_nwc_yarp::getAvailablePids(int j, std::vector<yarp::dev::PidControlTypeEnum>& avail)
383{
385 auto ret = m_RPC.getAvailablePidsRPC(j);
386 if (!ret.ret) {
387 yCError(CONTROLBOARD_NWC_YARP, "Unable to getAvailablePids");
388 return ret.ret;
389 }
390 avail = ret.avail;
391 return ret.ret;
392}
393
395{
397 auto ret = m_RPC.setPidRPC(pidtype, j, pid);
398 if (!ret) {
399 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPidRPC");
400 return ret;
401 }
402 return ret;
403}
404
406{
408 std::vector<Pid> pids_vec(pids, pids + m_nj);
409 auto ret = m_RPC.setPidsRPC(pidtype, pids_vec);
410 if (!ret) {
411 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPidsRPC");
412 return ret;
413 }
414 return ret;
415}
416
418{
420 auto ret = m_RPC.setPidReferenceRPC(pidtype, j, ref);
421 if (!ret) {
422 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPidReferenceRPC");
423 return ret;
424 }
425 return ret;
426}
427
429{
431 std::vector<double> pids_refs(refs, refs + m_nj);
432 auto ret = m_RPC.setPidReferencesRPC(pidtype, pids_refs);
433 if (!ret) {
434 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPidReferencesRPC");
435 return ret;
436 }
437 return ret;
438}
439
441{
443 auto ret = m_RPC.setPidErrorLimitRPC(pidtype, j, limit);
444 if (!ret) {
445 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPidErrorLimitRPC");
446 return ret;
447 }
448 return ret;
449}
450
452{
454 std::vector<double> pids_lims(limits, limits + m_nj);
455 auto ret = m_RPC.setPidErrorLimitsRPC(pidtype, pids_lims);
456 if (!ret) {
457 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPidErrorLimitsRPC");
458 return ret;
459 }
460 return ret;
461}
462
464{
466 auto ret = m_RPC.getPidErrorRPC(pidtype,j);
467 if (!ret.ret) {
468 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidErrorRPC");
469 return ret.ret;
470 }
471 *err = ret.err;
472 return ret.ret;
473}
474
476{
478 auto ret = m_RPC.getPidErrorsRPC(pidtype);
479 if (!ret.ret) {
480 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidErrorsRPC");
481 return ret.ret;
482 }
483 std::copy(ret.errs.begin(), ret.errs.end(), errs);
484 return ret.ret;
485}
486
488{
490 auto ret = m_RPC.getPidRPC(pidtype,j);
491 if (!ret.ret) {
492 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidRPC");
493 return ret.ret;
494 }
495 *pid = ret.pid;
496 return ret.ret;
497}
498
500{
502 auto ret = m_RPC.getPidsRPC(pidtype);
503 if (!ret.ret) {
504 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidsRPC");
505 return ret.ret;
506 }
507 std::copy(ret.pids.begin(), ret.pids.end(), pids);
508 return ret.ret;
509}
510
512{
514 auto ret = m_RPC.getPidReferenceRPC(pidtype,j);
515 if (!ret.ret) {
516 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidReferenceRPC");
517 return ret.ret;
518 }
519 *ref = ret.ref;
520 return ret.ret;
521}
522
524{
526 auto ret = m_RPC.getPidReferencesRPC(pidtype);
527 if (!ret.ret) {
528 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidReferencesRPC");
529 return ret.ret;
530 }
531 std::copy(ret.refs.begin(), ret.refs.end(), refs);
532 return ret.ret;
533}
534
536{
538 auto ret = m_RPC.getPidErrorLimitRPC(pidtype,j);
539 if (!ret.ret) {
540 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidErrorLimitRPC");
541 return ret.ret;
542 }
543 *limit = ret.lim;
544 return ret.ret;
545}
546
548{
550 auto ret = m_RPC.getPidErrorLimitsRPC(pidtype);
551 if (!ret.ret) {
552 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidErrorLimits");
553 return ret.ret;
554 }
555 std::copy(ret.lims.begin(), ret.lims.end(), limits);
556 return ret.ret;
557}
558
560{
562 auto ret = m_RPC.resetPidRPC(pidtype, j);
563 if (!ret) {
564 yCError(CONTROLBOARD_NWC_YARP, "Unable to resetPidRPC");
565 return ret;
566 }
567 return ret;
568}
569
571{
573 auto ret = m_RPC.disablePidRPC(pidtype, j);
574 if (!ret) {
575 yCError(CONTROLBOARD_NWC_YARP, "Unable to disablePidRPC");
576 return ret;
577 }
578 return ret;
579}
580
582{
584 auto ret = m_RPC.enablePidRPC(pidtype, j);
585 if (!ret) {
586 yCError(CONTROLBOARD_NWC_YARP, "Unable to enablePidRPC");
587 return ret;
588 }
589 return ret;
590}
591
593{
595 auto ret = m_RPC.isPidEnabledRPC(pidtype,j);
596 if (!ret.ret) {
597 yCError(CONTROLBOARD_NWC_YARP, "Unable to isPidEnabledRPC");
598 return ret.ret;
599 }
600 enabled = ret.isEnabled;
601 return ret.ret;
602}
603
605{
607 auto ret = m_RPC.getPidOutputRPC(pidtype,j);
608 if (!ret.ret) {
609 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidOutputRPC");
610 return ret.ret;
611 }
612 *out = ret.out;
613 return ret.ret;
614}
615
617{
619 auto ret = m_RPC.getPidOutputsRPC(pidtype);
620 if (!ret.ret) {
621 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidOutputsRPC");
622 return ret.ret;
623 }
624 std::copy(ret.outs.begin(), ret.outs.end(), outs);
625 return ret.ret;
626}
627
629{
630 if (m_use_streaming)
631 {
632 CHECK_INTERFACE("iPidControl")
633 if (!isLive()) {
634 return ReturnValue::return_code::return_value_error_not_ready;
635 }
637 c.head.clear();
638 c.head.addVocab32(VOCAB_PIDCONTROL_INTERFACE);
639 c.head.addVocab32(VOCAB_PIDCONTROL_SET_OFFSET);
640 c.head.addVocab32(yarp::conf::vocab32_t(pidtype));
641 c.head.addInt32(j);
642 c.body.resize(1);
643 c.body[0] = v;
645 return ReturnValue_ok;
646 }
647 else
648 {
650 auto ret = m_RPC.setPidOffsetOneRPC(pidtype,j,v);
651 if (!ret) {
652 yCError(CONTROLBOARD_NWC_YARP, "Unable to velocityMoveOneRPC");
653 return ret;
654 }
655 return ret;
656 }
657}
658
660{
661 if (m_use_streaming)
662 {
663 CHECK_INTERFACE("iPidControl")
664 if (!isLive()) {
665 return ReturnValue::return_code::return_value_error_not_ready;
666 }
668 c.head.clear();
669 c.head.addVocab32(VOCAB_PIDCONTROL_INTERFACE);
670 c.head.addVocab32(VOCAB_PIDCONTROL_SET_FEEDFORWARD);
671 c.head.addVocab32(yarp::conf::vocab32_t(pidtype));
672 c.head.addInt32(j);
673 c.body.resize(1);
674 c.body[0] = v;
676 return ReturnValue_ok;
677 }
678 else
679 {
681 auto ret = m_RPC.setPidFeedforwardOneRPC(pidtype,j,v);
682 if (!ret) {
683 yCError(CONTROLBOARD_NWC_YARP, "Unable to velocityMoveOneRPC");
684 return ret;
685 }
686 return ret;
687 }
688}
689
691{
693 auto ret = m_RPC.getPidExtraInfoRPC(pidtype,j);
694 if (!ret.ret) {
695 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidExtraInfoRPC");
696 return ret.ret;
697 }
698 info = ret.info;
699 return ret.ret;
700}
701
703{
705 auto ret = m_RPC.getPidExtraInfosRPC(pidtype);
706 if (!ret.ret) {
707 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidExtraInfosRPC");
708 return ret.ret;
709 }
710 info = ret.info;
711 return ret.ret;
712}
713
715{
717 auto ret = m_RPC.getPidOffsetRPC(pidtype,j);
718 if (!ret.ret) {
719 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidOffsetRPC");
720 return ret.ret;
721 }
722 value = ret.offset;
723 return ret.ret;
724}
725
727{
729 auto ret = m_RPC.getPidFeedforwardRPC(pidtype,j);
730 if (!ret.ret) {
731 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPidFeedforwardRPC");
732 return ret.ret;
733 }
734 value = ret.feedforward;
735 return ret.ret;
736}
737
738// END IPidControl
739
740// BEGIN IEncoder
741
743{
745 auto ret = m_RPC.resetEncoderOneRPC(j);
746 if (!ret) {
747 yCError(CONTROLBOARD_NWC_YARP, "Unable to resetEncoderOneRPC");
748 return ret;
749 }
750 return ret;
751}
752
754{
756 auto ret = m_RPC.resetEncoderAllRPC();
757 if (!ret) {
758 yCError(CONTROLBOARD_NWC_YARP, "Unable to resetEncoderAllRPC");
759 return ret;
760 }
761 return ret;
762}
763
765{
767 auto ret = m_RPC.setEncoderOneRPC(j, val);
768 if (!ret) {
769 yCError(CONTROLBOARD_NWC_YARP, "Unable to setEncoderOneRPC");
770 return ret;
771 }
772 return ret;
773}
774
776{
778 std::vector<double> temp(vals, vals + this->m_nj);
779 auto ret = m_RPC.setEncoderAllRPC(temp);
780 if (!ret) {
781 yCError(CONTROLBOARD_NWC_YARP, "Unable to setEncoderAllRPC");
782 return ret;
783 }
784 return ret;
785}
786
788{
789 double localArrivalTime = 0.0;
790
791 extendedPortMutex.lock();
793 extendedPortMutex.unlock();
794 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
795}
796
798{
799 double localArrivalTime = 0.0;
800
801 extendedPortMutex.lock();
803 *t=m_lastStamp.getTime();
804 extendedPortMutex.unlock();
805 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
806}
807
809{
810 double localArrivalTime = 0.0;
811
812 extendedPortMutex.lock();
814 extendedPortMutex.unlock();
815 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
816}
817
819{
820 double localArrivalTime=0.0;
821
822 extendedPortMutex.lock();
824 std::fill_n(ts, m_nj, m_lastStamp.getTime());
825 extendedPortMutex.unlock();
826 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
827}
828
830{
831 double localArrivalTime=0.0;
832
833 extendedPortMutex.lock();
835 extendedPortMutex.unlock();
836 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
837}
838
840{
841 double localArrivalTime=0.0;
842
843 extendedPortMutex.lock();
845 extendedPortMutex.unlock();
846 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
847}
848
850{
851 double localArrivalTime=0.0;
852 extendedPortMutex.lock();
854 extendedPortMutex.unlock();
855 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
856}
857
859{
860 double localArrivalTime=0.0;
861 extendedPortMutex.lock();
863 extendedPortMutex.unlock();
864 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
865}
866
867// END IEncoder
868
869// BEGIN IRemoteVariable
870
872{
874 auto ret = m_RPC.getRemoteVariableRPC(key);
875 if (!ret.ret) {
876 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRemoteVariable");
877 return ret.ret;
878 }
879 val = ret.val;
880 return ret.ret;
881}
882
884{
886 auto ret = m_RPC.setRemoteVariableRPC(key, val);
887 if (!ret) {
888 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRemoteVariable");
889 return ret;
890 }
891 return ret;
892}
893
894
896{
898 auto ret = m_RPC.getRemoteVariablesListRPC();
899 if (!ret.ret) {
900 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRemoteVariable");
901 return ret.ret;
902 }
903 std::copy(ret.listOfKeys.begin(), ret.listOfKeys.end(), listOfKeys);
904 return ret.ret;
905}
906
907// END IRemoteVariable
908
909// BEGIN IMotor
910
912{
914 auto ret = m_RPC.getNumberOfMotorsRPC();
915 if (!ret.ret) {
916 yCError(CONTROLBOARD_NWC_YARP, "Unable to getNumberOfMotors");
917 return ret.ret;
918 }
919 *num = ret.val;
920 return ret.ret;
921}
922
924{
925 double localArrivalTime = 0.0;
926
927 extendedPortMutex.lock();
929 extendedPortMutex.unlock();
930 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
931}
932
934{
935 double localArrivalTime = 0.0;
936
937 extendedPortMutex.lock();
939 extendedPortMutex.unlock();
940 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
941}
942
944{
946 auto ret = m_RPC.getTemperatureLimitRPC(m);
947 if (!ret.ret) {
948 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTemperatureLimit");
949 return ret.ret;
950 }
951 *val = ret.val;
952 return ret.ret;
953}
954
956{
958 auto ret = m_RPC.setTemperatureLimitRPC(m, val);
959 if (!ret) {
960 yCError(CONTROLBOARD_NWC_YARP, "Unable to setTemperatureLimit");
961 return ret;
962 }
963 return ret;
964}
965
967{
969 auto ret = m_RPC.getGearboxRatioRPC(m);
970 if (!ret.ret) {
971 yCError(CONTROLBOARD_NWC_YARP, "Unable to getGearboxRatio");
972 return ret.ret;
973 }
974 *val = ret.val;
975 return ret.ret;
976}
977
979{
981 auto ret = m_RPC.setGearboxRatioRPC(m, val);
982 if (!ret) {
983 yCError(CONTROLBOARD_NWC_YARP, "Unable to setGearboxRatio");
984 return ret;
985 }
986 return ret;
987}
988
989// END IMotor
990
991// BEGIN IMotorEncoder
992
994{
996 auto ret = m_RPC.resetMotorEncoderRPC(j);
997 if (!ret) {
998 yCError(CONTROLBOARD_NWC_YARP, "Unable to resetMotorEncoder");
999 return ret;
1000 }
1001 return ret;
1002}
1003
1005{
1006 LOCKMUTEX
1007 auto ret = m_RPC.resetMotorEncodersRPC();
1008 if (!ret) {
1009 yCError(CONTROLBOARD_NWC_YARP, "Unable to resetMotorEncoders");
1010 return ret;
1011 }
1012 return ret;
1013}
1014
1016{
1017 LOCKMUTEX
1018 auto ret = m_RPC.setMotorEncoderRPC(j, val);
1019 if (!ret) {
1020 yCError(CONTROLBOARD_NWC_YARP, "Unable to setMotorEncoder");
1021 return ret;
1022 }
1023 return ret;
1024}
1025
1027{
1028 LOCKMUTEX
1030 if (!ret) {
1031 yCError(CONTROLBOARD_NWC_YARP, "Unable to setMotorEncoderCountsPerRevolution");
1032 return ret;
1033 }
1034 return ret;
1035}
1036
1038{
1039 LOCKMUTEX
1041 if (!ret.ret) {
1042 yCError(CONTROLBOARD_NWC_YARP, "Unable to getMotorEncoderCountsPerRevolution");
1043 return ret.ret;
1044 }
1045 *cpr = ret.cpr;
1046 return ret.ret;
1047}
1048
1050{
1051 LOCKMUTEX
1052 std::vector<double> temp(vals, vals + this->m_nj);
1053 auto ret = m_RPC.setMotorEncodersAllRPC(temp);
1054 if (!ret) {
1055 yCError(CONTROLBOARD_NWC_YARP, "Unable to setMotorEncoders");
1056 return ret;
1057 }
1058 return ret;
1059}
1060
1062{
1063 double localArrivalTime = 0.0;
1064
1065 extendedPortMutex.lock();
1067 extendedPortMutex.unlock();
1068 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
1069}
1070
1072{
1073 double localArrivalTime = 0.0;
1074
1075 extendedPortMutex.lock();
1077 *t=m_lastStamp.getTime();
1078 extendedPortMutex.unlock();
1079 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
1080}
1081
1083{
1084 double localArrivalTime=0.0;
1085
1086 extendedPortMutex.lock();
1088 extendedPortMutex.unlock();
1089 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
1090}
1091
1093{
1094 double localArrivalTime=0.0;
1095
1096 extendedPortMutex.lock();
1098 std::fill_n(ts, m_nj, m_lastStamp.getTime());
1099 extendedPortMutex.unlock();
1100 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
1101}
1102
1104{
1105 double localArrivalTime=0.0;
1106 extendedPortMutex.lock();
1108 extendedPortMutex.unlock();
1109 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
1110}
1111
1113{
1114 double localArrivalTime=0.0;
1115 extendedPortMutex.lock();
1117 extendedPortMutex.unlock();
1118 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
1119}
1120
1122{
1123 double localArrivalTime=0.0;
1124 extendedPortMutex.lock();
1126 extendedPortMutex.unlock();
1127 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
1128}
1129
1131{
1132 double localArrivalTime=0.0;
1133 extendedPortMutex.lock();
1135 extendedPortMutex.unlock();
1136 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
1137}
1138
1140{
1141 LOCKMUTEX
1143 if (!ret.ret) {
1144 yCError(CONTROLBOARD_NWC_YARP, "Unable to getNumberOfMotorEncodersRPC");
1145 return ret.ret;
1146 }
1147 *num = ret.num;
1148 return ret.ret;
1149}
1150
1151// END IMotorEncoder
1152
1153// BEGIN IPreciselyTimed
1154
1160{
1161 Stamp ret;
1162// mutex.lock();
1163 ret = m_lastStamp;
1164// mutex.unlock();
1165 return ret;
1166}
1167
1168// END IPreciselyTimed
1169
1170// BEGIN IPositionControl
1171
1173{
1174 LOCKMUTEX
1175 auto ret = m_RPC.positionMoveOneRPC(j, ref);
1176 if (!ret) {
1177 yCError(CONTROLBOARD_NWC_YARP, "Unable to positionMoveOneRPC");
1178 return ret;
1179 }
1180 return ret;
1181}
1182
1183yarp::dev::ReturnValue ControlBoard_nwc_yarp::positionMove(const int n_joint, const int *joints, const double *refs)
1184{
1185 LOCKMUTEX
1186 std::vector<int> tempjoints(joints, joints + n_joint);
1187 std::vector<double> tempvals(refs, refs + n_joint);
1189 if (!ret) {
1190 yCError(CONTROLBOARD_NWC_YARP, "Unable to positionMoveGroupRPC");
1191 return ret;
1192 }
1193 return ret;
1194}
1195
1197{
1198 LOCKMUTEX
1199 std::vector<double> temp(refs, refs + this->m_nj);
1200 auto ret = m_RPC.positionMoveAllRPC(temp);
1201 if (!ret) {
1202 yCError(CONTROLBOARD_NWC_YARP, "Unable to positionMoveAllRPC");
1203 return ret;
1204 }
1205 return ret;
1206}
1207
1209{
1210 LOCKMUTEX
1211 auto ret = m_RPC.getTargetPositionOneRPC(joint);
1212 if (!ret.ret) {
1213 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTargetPositionsOneRPC");
1214 return ret.ret;
1215 }
1216 *ref = ret.val;
1217 return ret.ret;
1218}
1219
1221{
1222 LOCKMUTEX
1223 auto ret = m_RPC.getTargetPositionAllRPC();
1224 if (!ret.ret) {
1225 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTargetPositionAllRPC");
1226 return ret.ret;
1227 }
1228 std::copy(ret.val.begin(), ret.val.end(), refs);
1229 return ret.ret;
1230}
1231
1233{
1234 LOCKMUTEX
1235 std::vector<int> tempjoints(joints, joints + n_joint);
1237 if (!ret.ret) {
1238 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTargetPositionGroupRPC");
1239 return ret.ret;
1240 }
1241 std::copy(ret.val.begin(), ret.val.end(), refs);
1242 return ret.ret;
1243}
1244
1246{
1247 LOCKMUTEX
1248 auto ret = m_RPC.relativeMoveOneRPC(j, delta);
1249 if (!ret) {
1250 yCError(CONTROLBOARD_NWC_YARP, "Unable to relativeMoveOneRPC");
1251 return ret;
1252 }
1253 return ret;
1254}
1255
1256yarp::dev::ReturnValue ControlBoard_nwc_yarp::relativeMove(const int n_joint, const int *joints, const double *refs)
1257{
1258 LOCKMUTEX
1259 std::vector<int> tempjoints(joints, joints + n_joint);
1260 std::vector<double> tempvals(refs, refs + n_joint);
1262 if (!ret) {
1263 yCError(CONTROLBOARD_NWC_YARP, "Unable to relativeMoveGroupRPC");
1264 return ret;
1265 }
1266 return ret;
1267}
1268
1270{
1271 LOCKMUTEX
1272 std::vector<double> temp(deltas, deltas + this->m_nj);
1273 auto ret = m_RPC.relativeMoveAllRPC(temp);
1274 if (!ret) {
1275 yCError(CONTROLBOARD_NWC_YARP, "Unable to relativeMoveAllRPC");
1276 return ret;
1277 }
1278 return ret;
1279}
1280
1282{
1283 LOCKMUTEX
1284 auto ret = m_RPC.checkMotionDoneOneRPC(j);
1285 if (!ret.ret) {
1286 yCError(CONTROLBOARD_NWC_YARP, "Unable to checkMotionDoneOneRPC");
1287 return ret.ret;
1288 }
1289 flag = ret.flag;
1290 return ret.ret;
1291}
1292
1293yarp::dev::ReturnValue ControlBoard_nwc_yarp::checkMotionDone(const std::vector<int>& joints, bool& flag)
1294{
1295 LOCKMUTEX
1296 auto ret = m_RPC.checkMotionDoneGroupRPC(joints);
1297 if (!ret.ret) {
1298 yCError(CONTROLBOARD_NWC_YARP, "Unable to checkMotionDoneGroupRPC");
1299 return ret.ret;
1300 }
1301 flag = ret.flag;
1302 return ret.ret;
1303}
1304
1306{
1307 LOCKMUTEX
1308 auto ret = m_RPC.checkMotionDoneAllRPC();
1309 if (!ret.ret) {
1310 yCError(CONTROLBOARD_NWC_YARP, "Unable to checkMotionDoneAllRPC");
1311 return ret.ret;
1312 }
1313 flag = ret.flag;
1314 return ret.ret;
1315}
1316
1318{
1319 LOCKMUTEX
1320 auto ret = m_RPC.setTrajSpeedOneRPC(j, sp);
1321 if (!ret) {
1322 yCError(CONTROLBOARD_NWC_YARP, "Unable to setTrajSpeedOneRPC");
1323 return ret;
1324 }
1325 return ret;
1326}
1327
1328yarp::dev::ReturnValue ControlBoard_nwc_yarp::setTrajSpeeds(const int n_joint, const int *joints, const double *spds)
1329{
1330 LOCKMUTEX
1331 std::vector<int> tempjoints(joints, joints + n_joint);
1332 std::vector<double> tempvals(spds, spds + n_joint);
1334 if (!ret) {
1335 yCError(CONTROLBOARD_NWC_YARP, "Unable to setTrajSpeedGroupRPC");
1336 return ret;
1337 }
1338 return ret;
1339}
1340
1342{
1343 LOCKMUTEX
1344 std::vector<double> temp(spds, spds + this->m_nj);
1345 auto ret = m_RPC.setTrajSpeedAllRPC(temp);
1346 if (!ret) {
1347 yCError(CONTROLBOARD_NWC_YARP, "Unable to setTrajSpeedAllRPC");
1348 return ret;
1349 }
1350 return ret;
1351}
1352
1354{
1355 LOCKMUTEX
1356 auto ret = m_RPC.setTrajAccelerationOneRPC(j, acc);
1357 if (!ret) {
1358 yCError(CONTROLBOARD_NWC_YARP, "Unable to setTrajAccelerationOneRPC");
1359 return ret;
1360 }
1361 return ret;
1362}
1363
1364yarp::dev::ReturnValue ControlBoard_nwc_yarp::setTrajAccelerations(const int n_joint, const int *joints, const double *accs)
1365{
1366 LOCKMUTEX
1367 std::vector<int> tempjoints(joints, joints + n_joint);
1368 std::vector<double> tempvals(accs, accs + n_joint);
1370 if (!ret) {
1371 yCError(CONTROLBOARD_NWC_YARP, "Unable to setTrajAccelerationsGroupRPC");
1372 return ret;
1373 }
1374 return ret;
1375}
1376
1378{
1379 LOCKMUTEX
1380 std::vector<double> temp(accs, accs + this->m_nj);
1382 if (!ret) {
1383 yCError(CONTROLBOARD_NWC_YARP, "Unable to setTrajAccelerationsAllRPC");
1384 return ret;
1385 }
1386 return ret;
1387}
1388
1390{
1391 LOCKMUTEX
1392 auto ret = m_RPC.getTrajSpeedOneRPC(j);
1393 if (!ret.ret) {
1394 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTrajSpeedOneRPC");
1395 return ret.ret;
1396 }
1397 *ref = ret.val;
1398 return ret.ret;
1399}
1400
1402{
1403 LOCKMUTEX
1404 std::vector<int> tempjoints(joints, joints + n_joint);
1406 if (!ret.ret) {
1407 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTrajSpeedsGroupRPC");
1408 return ret.ret;
1409 }
1410 std::copy(ret.val.begin(), ret.val.end(), spds);
1411 return ret.ret;
1412}
1413
1415{
1416 LOCKMUTEX
1417 auto ret = m_RPC.getTrajSpeedsAllRPC();
1418 if (!ret.ret) {
1419 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTrajSpeedsAllRPC");
1420 return ret.ret;
1421 }
1422 std::copy(ret.val.begin(), ret.val.end(), spds);
1423 return ret.ret;
1424}
1425
1427{
1428 LOCKMUTEX
1429 auto ret = m_RPC.getTrajAccelerationOneRPC(j);
1430 if (!ret.ret) {
1431 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTrajAccelerationOneRPC");
1432 return ret.ret;
1433 }
1434 *acc = ret.val;
1435 return ret.ret;
1436}
1437
1439{
1440 LOCKMUTEX
1441 std::vector<int> tempjoints(joints, joints + n_joint);
1443 if (!ret.ret) {
1444 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTrajAccelerationsGroupRPC");
1445 return ret.ret;
1446 }
1447 std::copy(ret.val.begin(), ret.val.end(), accs);
1448 return ret.ret;
1449}
1450
1452{
1453 LOCKMUTEX
1454 auto ret = m_RPC.getTrajAccelerationAllRPC();
1455 if (!ret.ret) {
1456 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTrajAccelerationsAllRPC");
1457 return ret.ret;
1458 }
1459 std::copy(ret.val.begin(), ret.val.end(), accs);
1460 return ret.ret;
1461}
1462
1464{
1465 LOCKMUTEX
1466 auto ret = m_RPC.stopOneRPC(j);
1467 if (!ret) {
1468 yCError(CONTROLBOARD_NWC_YARP, "Unable to stopOneRPC");
1469 return ret;
1470 }
1471 return ret;
1472}
1473
1475{
1476 LOCKMUTEX
1477 std::vector<int> tempjoints(joints, joints + len);
1478 auto ret = m_RPC.stopGroupRPC(tempjoints);
1479 if (!ret) {
1480 yCError(CONTROLBOARD_NWC_YARP, "Unable to stopGroupRPC");
1481 return ret;
1482 }
1483 return ret;
1484}
1485
1487{
1488 LOCKMUTEX
1489 auto ret = m_RPC.stopAllRPC();
1490 if (!ret) {
1491 yCError(CONTROLBOARD_NWC_YARP, "Unable to stopAllRPC");
1492 return ret;
1493 }
1494 return ret;
1495}
1496
1497// END IPositionControl
1498
1499// BEGIN IJoint Fault
1501{
1502 LOCKMUTEX
1503 auto ret = m_RPC.getLastJointFaultRPC(j);
1504 if (!ret.ret) {
1505 yCError(CONTROLBOARD_NWC_YARP, "Unable to getLastJointFaultRPC");
1506 return ret.ret;
1507 }
1508 fault = ret.fault;
1509 message = ret.message;
1510 return ret.ret;
1511}
1512// END IJointFault
1513
1514// BEGIN IVelocityControl
1515
1517{
1518 if (m_use_streaming)
1519 {
1520 CHECK_INTERFACE("iVelocityControl")
1521 if (!isLive()) {
1522 return ReturnValue::return_code::return_value_error_not_ready;
1523 }
1525 c.head.clear();
1526 c.head.addVocab32(VOCAB_VELOCITYCONTROL_INTERFACE);
1527 c.head.addVocab32(VOCAB_VELOCITY_MOVE);
1528 c.head.addInt32(j);
1529 c.body.resize(1);
1530 memcpy(&(c.body[0]), &v, sizeof(double));
1532 return ReturnValue_ok;
1533 }
1534 else
1535 {
1536 LOCKMUTEX
1537 auto ret = m_RPC.velocityMoveOneRPC(j,v);
1538 if (!ret) {
1539 yCError(CONTROLBOARD_NWC_YARP, "Unable to velocityMoveOneRPC");
1540 return ret;
1541 }
1542 return ret;
1543 }
1544}
1545
1547{
1548 if (m_use_streaming)
1549 {
1550 CHECK_INTERFACE("iVelocityControl")
1551 if (!isLive()) {
1552 return ReturnValue::return_code::return_value_error_not_ready;
1553 }
1555 c.head.clear();
1556 c.head.addVocab32(VOCAB_VELOCITYCONTROL_INTERFACE);
1557 c.head.addVocab32(VOCAB_VELOCITY_MOVES);
1558 c.body.resize(m_nj);
1559 memcpy(&(c.body[0]), v, sizeof(double)*m_nj);
1561 return ReturnValue_ok;
1562 }
1563 else
1564 {
1565 LOCKMUTEX
1566 std::vector<double> temp(v, v + this->m_nj);
1567 auto ret = m_RPC.velocityMoveAllRPC(temp);
1568 if (!ret) {
1569 yCError(CONTROLBOARD_NWC_YARP, "Unable to velocityMoveAllRPC");
1570 return ret;
1571 }
1572 return ret;
1573 }
1574}
1575
1576// END IVelocityControl
1577
1578// BEGIN IAmplifierControl
1579
1581{
1582 LOCKMUTEX
1583 auto ret = m_RPC.enableAmpRPC(j);
1584 if (!ret) {
1585 yCError(CONTROLBOARD_NWC_YARP, "Unable to enableAmp");
1586 return ret;
1587 }
1588 return ret;
1589}
1590
1592{
1593 LOCKMUTEX
1594 auto ret = m_RPC.disableAmpRPC(j);
1595 if (!ret) {
1596 yCError(CONTROLBOARD_NWC_YARP, "Unable to disableAmpRPC");
1597 return ret;
1598 }
1599 return ret;
1600}
1601
1603{
1604 LOCKMUTEX
1605 auto ret = m_RPC.getAmpStatusAllRPC();
1606 if (!ret.ret) {
1607 yCError(CONTROLBOARD_NWC_YARP, "Unable to getAmpStatusAllRPC");
1608 return ret.ret;
1609 }
1610 std::copy(ret.val.begin(), ret.val.end(), st);
1611 return ret.ret;
1612}
1613
1615{
1616 LOCKMUTEX
1617 auto ret = m_RPC.getAmpStatusOneRPC(j);
1618 if (!ret.ret) {
1619 yCError(CONTROLBOARD_NWC_YARP, "Unable to getAmpStatusOneRPC");
1620 return ret.ret;
1621 }
1622 *st = ret.val;
1623 return ret.ret;
1624}
1625
1627{
1628 LOCKMUTEX
1629 auto ret = m_RPC.setMaxCurrentRPC(j,v);
1630 if (!ret) {
1631 yCError(CONTROLBOARD_NWC_YARP, "Unable to setMaxCurrentRPC");
1632 return ret;
1633 }
1634 return ret;
1635}
1636
1638{
1639 LOCKMUTEX
1640 auto ret = m_RPC.getMaxCurrentRPC(j);
1641 if (!ret.ret) {
1642 yCError(CONTROLBOARD_NWC_YARP, "Unable to getMaxCurrentRPC");
1643 return ret.ret;
1644 }
1645 *v = ret.val;
1646 return ret.ret;
1647}
1648
1650{
1651 LOCKMUTEX
1652 auto ret = m_RPC.getNominalCurrentRPC(m);
1653 if (!ret.ret) {
1654 yCError(CONTROLBOARD_NWC_YARP, "Unable to getNominalCurrentRPC");
1655 return ret.ret;
1656 }
1657 *val = ret.val;
1658 return ret.ret;
1659}
1660
1662{
1663 LOCKMUTEX
1664 auto ret = m_RPC.setNominalCurrentRPC(m,val);
1665 if (!ret) {
1666 yCError(CONTROLBOARD_NWC_YARP, "Unable to setNominalCurrentRPC");
1667 return ret;
1668 }
1669 return ret;
1670}
1671
1673{
1674 LOCKMUTEX
1675 auto ret = m_RPC.getPeakCurrentRPC(m);
1676 if (!ret.ret) {
1677 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPeakCurrentRPC");
1678 return ret.ret;
1679 }
1680 *val = ret.val;
1681 return ret.ret;
1682}
1683
1685{
1686 LOCKMUTEX
1687 auto ret = m_RPC.setPeakCurrentRPC(m,val);
1688 if (!ret) {
1689 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPeakCurrentRPC");
1690 return ret;
1691 }
1692 return ret;
1693}
1694
1696{
1697 double localArrivalTime = 0.0;
1698 extendedPortMutex.lock();
1700 extendedPortMutex.unlock();
1701 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
1702}
1703
1705{
1706 LOCKMUTEX
1707 auto ret = m_RPC.getPWMLimitRPC(m);
1708 if (!ret.ret) {
1709 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPWMLimitRPC");
1710 return ret.ret;
1711 }
1712 *val = ret.val;
1713 return ret.ret;
1714}
1715
1717{
1718 LOCKMUTEX
1719 auto ret = m_RPC.setPWMLimitRPC(m,val);
1720 if (!ret) {
1721 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPWMLimitRPC");
1722 return ret;
1723 }
1724 return ret;
1725}
1726
1728{
1729 LOCKMUTEX
1730 auto ret = m_RPC.getPowerSupplyVoltageRPC(m);
1731 if (!ret.ret) {
1732 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPowerSupplyVoltageRPC");
1733 return ret.ret;
1734 }
1735 *val = ret.val;
1736 return ret.ret;
1737}
1738
1739// END IAmplifierControl
1740
1741// BEGIN IControlLimits
1742
1743ReturnValue ControlBoard_nwc_yarp::setPosLimits(int axis, double min, double max)
1744{
1745 LOCKMUTEX
1746 auto ret = m_RPC.setPosLimitsRPC(axis, min, max);
1747 if (!ret) {
1748 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPosLimitsRPC");
1749 return ret;
1750 }
1751 return ret;
1752}
1753
1754ReturnValue ControlBoard_nwc_yarp::getPosLimits(int axis, double *min, double *max)
1755{
1756 LOCKMUTEX
1757 auto ret = m_RPC.getPosLimitsRPC(axis);
1758 if (!ret.ret) {
1759 yCError(CONTROLBOARD_NWC_YARP, "Unable to getPosLimitsRPC");
1760 return ret.ret;
1761 }
1762 *min = ret.min;
1763 *max = ret.max;
1764 return ret.ret;
1765}
1766
1767ReturnValue ControlBoard_nwc_yarp::setVelLimits(int axis, double min, double max)
1768{
1769 LOCKMUTEX
1770 auto ret = m_RPC.setVelLimitsRPC(axis, min, max);
1771 if (!ret) {
1772 yCError(CONTROLBOARD_NWC_YARP, "Unable to setVelLimitsRPC");
1773 return ret;
1774 }
1775 return ret;
1776}
1777
1778ReturnValue ControlBoard_nwc_yarp::getVelLimits(int axis, double *min, double *max)
1779{
1780 LOCKMUTEX
1781 auto ret = m_RPC.getVelLimitsRPC(axis);
1782 if (!ret.ret) {
1783 yCError(CONTROLBOARD_NWC_YARP, "Unable to getVelLimitsRPC");
1784 return ret.ret;
1785 }
1786 *min = ret.min;
1787 *max = ret.max;
1788 return ret.ret;
1789}
1790
1791// END IControlLimits
1792
1793// BEGIN IAxisInfo
1794
1796{
1797 LOCKMUTEX
1798 auto ret = m_RPC.getAxisNameRPC(j);
1799 if (!ret.ret) {
1800 yCError(CONTROLBOARD_NWC_YARP, "Unable to getAxisNameRPC");
1801 return ret.ret;
1802 }
1803 name = ret.name;
1804 return ret.ret;
1805}
1806
1808{
1809 LOCKMUTEX
1810 auto ret = m_RPC.getJointTypeRPC(j);
1811 if (!ret.ret) {
1812 yCError(CONTROLBOARD_NWC_YARP, "Unable to getJointTypeRPC");
1813 return ret.ret;
1814 }
1815 type = ret.joint;
1816 return ret.ret;
1817}
1818
1819// END IAxisInfo
1820
1821// BEGIN IControlCalibration
1823{
1824 LOCKMUTEX
1825 auto ret = m_RPC.calibrateRobotRPC();
1826 if (!ret) {
1827 yCError(CONTROLBOARD_NWC_YARP, "Unable to calibrateRobot");
1828 return ret;
1829 }
1830 return ret;
1831}
1832
1834{
1835 LOCKMUTEX
1836 auto ret = m_RPC.abortCalibrationRPC();
1837 if (!ret) {
1838 yCError(CONTROLBOARD_NWC_YARP, "Unable to abortCalibration");
1839 return ret;
1840 }
1841 return ret;
1842}
1843
1845{
1846 LOCKMUTEX
1847 auto ret = m_RPC.abortParkRPC();
1848 if (!ret) {
1849 yCError(CONTROLBOARD_NWC_YARP, "Unable to abortPark");
1850 return ret;
1851 }
1852 return ret;
1853}
1854
1856{
1857 LOCKMUTEX
1858 auto ret = m_RPC.parkRPC(wait);
1859 if (!ret) {
1860 yCError(CONTROLBOARD_NWC_YARP, "Unable to park");
1861 return ret;
1862 }
1863 return ret;
1864}
1865
1866yarp::dev::ReturnValue ControlBoard_nwc_yarp::calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3)
1867{
1868 LOCKMUTEX
1869 auto ret = m_RPC.calibrateAxisWithParamsRPC(j, ui, v1, v2, v3);
1870 if (!ret) {
1871 yCError(CONTROLBOARD_NWC_YARP, "Unable to calibrateAxisWithParams");
1872 return ret;
1873 }
1874 return ret;
1875}
1876
1878{
1879 LOCKMUTEX
1881 cal.param1 = params.param1;
1882 cal.param2 = params.param2;
1883 cal.param3 = params.param3;
1884 cal.param4 = params.param4;
1885 cal.param5 = params.param5;
1886 cal.paramZero = params.paramZero;
1888 if (!ret) {
1889 yCError(CONTROLBOARD_NWC_YARP, "Unable to setCalibrationParameters");
1890 return ret;
1891 }
1892 return ret;
1893}
1894
1896{
1897 LOCKMUTEX
1898 auto ret = m_RPC.calibrationDoneRPC(j);
1899 if (!ret) {
1900 yCError(CONTROLBOARD_NWC_YARP, "Unable to calibrationDone");
1901 return ret;
1902 }
1903 return ret;
1904}
1905
1906// END IControlCalibration
1907
1908// BEGIN ITorqueControl
1909
1911{
1912 LOCKMUTEX
1913 auto ret = m_RPC.getRefTorqueOneRPC(j);
1914 if (!ret.ret) {
1915 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefTorqueOneRPC");
1916 return ret.ret;
1917 }
1918 *t = ret.val;
1919 return ret.ret;
1920}
1921
1923{
1924 LOCKMUTEX
1925 auto ret = m_RPC.getRefTorqueAllRPC();
1926 if (!ret.ret) {
1927 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefTorqueAllRPC");
1928 return ret.ret;
1929 }
1930
1931 std::copy(ret.val.begin(), ret.val.end(), t);
1932 return ret.ret;
1933}
1934
1936{
1937 if (m_use_streaming)
1938 {
1939 CHECK_INTERFACE("iTorqueControl")
1940 if (!isLive()) {
1941 return ReturnValue::return_code::return_value_error_not_ready;
1942 }
1944 c.head.clear();
1945 c.head.addVocab32(VOCAB_TORQUECONTROL_INTERFACE);
1946 c.head.addVocab32(VOCAB_TORQUES_DIRECTS);
1947
1948 c.body.resize(m_nj);
1949
1950 memcpy(c.body.data(), t, sizeof(double) * m_nj);
1951
1953 return ReturnValue_ok;
1954 }
1955 else
1956 {
1957 LOCKMUTEX
1958 std::vector<double> temp(t, t + this->m_nj);
1959 auto ret = m_RPC.setRefTorqueAllRPC(temp);
1960 if (!ret) {
1961 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRefTorqueAllRPC");
1962 return ret;
1963 }
1964 return ret;
1965 }
1966}
1967
1969{
1970 if (m_use_streaming)
1971 {
1972 CHECK_INTERFACE("iTorqueControl")
1973 if (!isLive()) {
1974 return ReturnValue::return_code::return_value_error_not_ready;
1975 }
1977 c.head.clear();
1978 c.head.addVocab32(VOCAB_TORQUECONTROL_INTERFACE);
1979 c.head.addVocab32(VOCAB_TORQUES_DIRECT);
1980 c.head.addInt32(j);
1981
1982 c.body.clear();
1983 c.body.resize(1);
1984 c.body[0] = v;
1986 return ReturnValue_ok;
1987 }
1988 else
1989 {
1990 LOCKMUTEX
1991 auto ret = m_RPC.setRefTorqueOneRPC(j,v);
1992 if (!ret) {
1993 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRefTorqueOneRPC");
1994 return ret;
1995 }
1996 return ret;
1997 }
1998}
1999
2000yarp::dev::ReturnValue ControlBoard_nwc_yarp::setRefTorques(const int n_joint, const int *joints, const double *t)
2001{
2002 if (m_use_streaming)
2003 {
2004 CHECK_INTERFACE("iTorqueControl")
2005 if (!isLive()) {
2006 return ReturnValue::return_code::return_value_error_not_ready;
2007 }
2009 c.head.clear();
2010 c.head.addVocab32(VOCAB_TORQUECONTROL_INTERFACE);
2011 c.head.addVocab32(VOCAB_TORQUES_DIRECT_GROUP);
2012 c.head.addInt32(n_joint);
2013 Bottle &jointList = c.head.addList();
2014 for (int i = 0; i < n_joint; i++) {
2015 jointList.addInt32(joints[i]);
2016 }
2017 c.body.resize(n_joint);
2018 memcpy(&(c.body[0]), t, sizeof(double)*n_joint);
2020 return ReturnValue_ok;
2021 }
2022 else
2023 {
2024 LOCKMUTEX
2025 std::vector<int> tempjoints(joints, joints + n_joint);
2026 std::vector<double> tempdata(t, t + n_joint);
2028 if (!ret) {
2029 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRefTorqueGroupRPC");
2030 return ret;
2031 }
2032 return ret;
2033 }
2034}
2035
2037{
2038 LOCKMUTEX
2040 mparams.bemf = params.bemf;
2041 mparams.bemf_scale = params.bemf_scale;
2042 mparams.coulombNeg = params.coulombNeg;
2043 mparams.coulombPos = params.coulombPos;
2044 mparams.ktau = params.ktau;
2045 mparams.ktau_scale = params.ktau_scale;
2046 mparams.velocityThres = params.velocityThres;
2047 mparams.viscousNeg = params.viscousNeg;
2048 mparams.viscousPos = params.viscousPos;
2049 auto ret = m_RPC.setMotorTorqueParamsRPC(j, mparams);
2050 if (!ret) {
2051 yCError(CONTROLBOARD_NWC_YARP, "Unable to setMotorTorqueParamsRPC");
2052 return ret;
2053 }
2054 return ret;
2055}
2056
2058{
2059 LOCKMUTEX
2060 auto ret = m_RPC.getMotorTorqueParamsRPC(j);
2061 if (!ret.ret) {
2062 yCError(CONTROLBOARD_NWC_YARP, "Unable to getMotorTorqueParamsRPC");
2063 return ret.ret;
2064 }
2065 (*params).bemf = ret.params.bemf;
2066 (*params).bemf_scale = ret.params.bemf_scale;
2067 (*params).coulombNeg = ret.params.coulombNeg;
2068 (*params).coulombPos = ret.params.coulombPos;
2069 (*params).ktau = ret.params.ktau;
2070 (*params).ktau_scale = ret.params.ktau_scale;
2071 (*params).velocityThres = ret.params.velocityThres;
2072 (*params).viscousNeg = ret.params.viscousNeg;
2073 (*params).viscousPos = ret.params.viscousPos;
2074 return ret.ret;
2075}
2076
2078{
2079 double localArrivalTime=0.0;
2080 extendedPortMutex.lock();
2082 extendedPortMutex.unlock();
2083 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2084}
2085
2087{
2088 double localArrivalTime=0.0;
2089 extendedPortMutex.lock();
2091 extendedPortMutex.unlock();
2092 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2093}
2094
2096{
2097 LOCKMUTEX
2098 auto ret = m_RPC.getTorqueRangeOneRPC(j);
2099 if (!ret.ret) {
2100 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTorqueRangeOneRPC");
2101 return ret.ret;
2102 }
2103 *min = ret.min;
2104 *max = ret.max;
2105 return ret.ret;
2106}
2107
2109{
2110 LOCKMUTEX
2111 auto ret = m_RPC.getTorqueRangeAllRPC();
2112 if (!ret.ret) {
2113 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTorqueRangeAllRPC");
2114 return ret.ret;
2115 }
2116 std::copy(ret.mins.begin(), ret.mins.end(), min);
2117 std::copy(ret.maxs.begin(), ret.maxs.end(), max);
2118 return ret.ret;
2119}
2120
2121// END ITorqueControl
2122
2123// BEGIN IImpedanceControl
2124
2125yarp::dev::ReturnValue ControlBoard_nwc_yarp::getImpedance(int j, double *stiffness, double *damping)
2126{
2127 LOCKMUTEX
2128 auto ret = m_RPC.getImpedanceRPC(j);
2129 if (!ret.ret) {
2130 yCError(CONTROLBOARD_NWC_YARP, "Unable to getImpedanceRPC");
2131 return ret.ret;
2132 }
2133 *stiffness = ret.stiffness;
2134 *damping = ret.damping;
2135 return ret.ret;
2136}
2137
2139{
2140 LOCKMUTEX
2141 auto ret = m_RPC.getImpedanceOffsetRPC(j);
2142 if (!ret.ret) {
2143 yCError(CONTROLBOARD_NWC_YARP, "Unable to getImpedanceOffsetRPC");
2144 return ret.ret;
2145 }
2146 *offset = ret.offset;
2147 return ret.ret;
2148}
2149
2150yarp::dev::ReturnValue ControlBoard_nwc_yarp::setImpedance(int j, double stiffness, double damping)
2151{
2152 LOCKMUTEX
2153 auto ret = m_RPC.setImpedanceRPC(j,stiffness, damping);
2154 if (!ret) {
2155 yCError(CONTROLBOARD_NWC_YARP, "Unable to setImpedance");
2156 return ret;
2157 }
2158 return ret;
2159}
2160
2162{
2163 LOCKMUTEX
2164 auto ret = m_RPC.setImpedanceOffsetRPC(j,offset);
2165 if (!ret) {
2166 yCError(CONTROLBOARD_NWC_YARP, "Unable to setImpedanceOffset");
2167 return ret;
2168 }
2169 return ret;
2170}
2171
2172yarp::dev::ReturnValue ControlBoard_nwc_yarp::getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
2173{
2174 LOCKMUTEX
2175 auto ret = m_RPC.getCurrentImpedanceLimitRPC(j);
2176 if (!ret.ret) {
2177 yCError(CONTROLBOARD_NWC_YARP, "Unable to getCurrentImpedanceLimitRPC");
2178 return ret.ret;
2179 }
2180 *min_stiff = ret.min_stiffness;
2181 *max_stiff = ret.max_stiffness;
2182 *min_damp = ret.min_damping;
2183 *max_damp = ret.max_damping;
2184 return ret.ret;
2185}
2186
2187// END IImpedanceControl
2188
2189// BEGIN IControlMode
2190
2191ReturnValue ControlBoard_nwc_yarp::getAvailableControlModes(int j, std::vector<yarp::dev::SelectableControlModeEnum>& avail)
2192{
2193 LOCKMUTEX
2194 auto ret = m_RPC.getAvailableControlModesRPC(j);
2195 if (!ret.ret) {
2196 yCError(CONTROLBOARD_NWC_YARP, "Unable to getAvailableControlModes");
2197 return ret.ret;
2198 }
2199 avail =ret.avail;
2200 return ret.ret;
2201}
2202
2204{
2205 double localArrivalTime=0.0;
2206 extendedPortMutex.lock();
2207 int mode_tmp;
2210 extendedPortMutex.unlock();
2211 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2212}
2213
2214yarp::dev::ReturnValue ControlBoard_nwc_yarp::getControlModes(std::vector<yarp::dev::ControlModeEnum>& modes)
2215{
2216 double localArrivalTime=0.0;
2217 extendedPortMutex.lock();
2219 if(ret)
2220 {
2221 for (int i = 0; i < modes.size(); i++) {
2223 }
2224 } else {
2225 ret = false;
2226 }
2227
2228 extendedPortMutex.unlock();
2229 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2230}
2231
2232yarp::dev::ReturnValue ControlBoard_nwc_yarp::getControlModes(const std::vector<int>& joints, std::vector<yarp::dev::ControlModeEnum>& modes)
2233{
2234 double localArrivalTime=0.0;
2235
2236 extendedPortMutex.lock();
2238 if(ret)
2239 {
2240 for (int i = 0; i < joints.size(); i++) {
2242 }
2243 } else {
2244 ret = false;
2245 }
2246
2247 extendedPortMutex.unlock();
2248 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2249}
2250
2252{
2253 LOCKMUTEX
2254 auto ret = m_RPC.setControlModeOneRPC(j, mode);
2255 if (!ret) {
2256 yCError(CONTROLBOARD_NWC_YARP, "Unable to setControlMode");
2257 return ret;
2258 }
2259 return ret;
2260}
2261
2262yarp::dev::ReturnValue ControlBoard_nwc_yarp::setControlModes(const std::vector<int>& j, const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
2263{
2264 LOCKMUTEX
2265 auto ret = m_RPC.setControlModeGroupRPC(j, modes);
2266 if (!ret) {
2267 yCError(CONTROLBOARD_NWC_YARP, "Unable to setControlModes");
2268 return ret;
2269 }
2270 return ret;
2271}
2272
2273yarp::dev::ReturnValue ControlBoard_nwc_yarp::setControlModes(const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
2274{
2275 LOCKMUTEX
2276 auto ret = m_RPC.setControlModeAllRPC(modes);
2277 if (!ret) {
2278 yCError(CONTROLBOARD_NWC_YARP, "Unable to setControlModes");
2279 return ret;
2280 }
2281 return ret;
2282}
2283
2284// END IControlMode
2285
2286// BEGIN IPositionDirect
2287
2289{
2290 if (m_use_streaming)
2291 {
2292 CHECK_INTERFACE("iPositionDirect")
2293 if (!isLive()) {
2294 return ReturnValue::return_code::return_value_error_not_ready;
2295 }
2297 c.head.clear();
2299 c.head.addVocab32(VOCAB_POSITION_DIRECT);
2300 c.head.addInt32(j);
2301 c.body.resize(1);
2302 memcpy(&(c.body[0]), &ref, sizeof(double));
2304 return ReturnValue_ok;
2305 }
2306 else
2307 {
2308 LOCKMUTEX
2309 auto ret = m_RPC.setRefPositionOneRPC(j, ref);
2310 if (!ret) {
2311 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPositionOneRPC");
2312 return ret;
2313 }
2314 return ret;
2315 }
2316}
2317
2318yarp::dev::ReturnValue ControlBoard_nwc_yarp::setPositions(const int n_joint, const int *joints, const double *refs)
2319{
2320 if (m_use_streaming)
2321 {
2322 CHECK_INTERFACE("iPositionDirect")
2323 if (!isLive()) {
2324 return ReturnValue::return_code::return_value_error_not_ready;
2325 }
2327 c.head.clear();
2329 c.head.addVocab32(VOCAB_POSITION_DIRECT_GROUP);
2330 c.head.addInt32(n_joint);
2331 Bottle &jointList = c.head.addList();
2332 for (int i = 0; i < n_joint; i++) {
2333 jointList.addInt32(joints[i]);
2334 }
2335 c.body.resize(n_joint);
2336 memcpy(&(c.body[0]), refs, sizeof(double)*n_joint);
2338 return ReturnValue_ok;
2339 }
2340 else
2341 {
2342 LOCKMUTEX
2343 std::vector<int> tempjoints(joints, joints + n_joint);
2344 std::vector<double> tempdata(refs, refs + n_joint);
2346 if (!ret) {
2347 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPositionGroupRPC");
2348 return ret;
2349 }
2350 return ret;
2351 }
2352}
2353
2355{
2356 if (m_use_streaming)
2357 {
2358 CHECK_INTERFACE("iPositionDirect")
2359 if (!isLive()) {
2360 return ReturnValue::return_code::return_value_error_not_ready;
2361 }
2363 c.head.clear();
2365 c.head.addVocab32(VOCAB_POSITION_DIRECTS);
2366 c.body.resize(m_nj);
2367 memcpy(&(c.body[0]), refs, sizeof(double)*m_nj);
2369 return ReturnValue_ok;
2370 }
2371 else
2372 {
2373 LOCKMUTEX
2374 std::vector<double> temp(refs, refs + m_nj);
2375 auto ret = m_RPC.setRefPositionAllRPC(temp);
2376 if (!ret) {
2377 yCError(CONTROLBOARD_NWC_YARP, "Unable to setPositionAllRPC");
2378 return ret;
2379 }
2380 return ret;
2381 }
2382}
2383
2385{
2386 LOCKMUTEX
2387 auto ret = m_RPC.getRefPositionOneRPC(joint);
2388 if (!ret.ret) {
2389 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefPositionOneRPC");
2390 return ret.ret;
2391 }
2392 *ref = ret.val;
2393 return ret.ret;
2394}
2395
2397{
2398 LOCKMUTEX
2399 auto ret = m_RPC.getRefPositionAllRPC();
2400 if (!ret.ret) {
2401 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefPositionAllRPC");
2402 return ret.ret;
2403 }
2404 std::copy(ret.val.begin(), ret.val.end(), refs);
2405 return ret.ret;
2406}
2407
2409{
2410 LOCKMUTEX
2411 std::vector<int> tempjoints(joints, joints + n_joint);
2413 if (!ret.ret) {
2414 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefPositionGroupRPC");
2415 return ret.ret;
2416 }
2417 std::copy(ret.val.begin(), ret.val.end(), refs);
2418 return ret.ret;
2419}
2420
2421// END IPositionDirect
2422
2423// BEGIN IVelocityControl
2424
2425yarp::dev::ReturnValue ControlBoard_nwc_yarp::velocityMove(const int n_joint, const int *joints, const double *spds)
2426{
2427 if (m_use_streaming)
2428 {
2429 CHECK_INTERFACE("iVelocityControl")
2430 if (!isLive()) {
2431 return ReturnValue::return_code::return_value_error_not_ready;
2432 }
2434 c.head.clear();
2435 c.head.addVocab32(VOCAB_VELOCITYCONTROL_INTERFACE);
2436 c.head.addVocab32(VOCAB_VELOCITY_MOVE_GROUP);
2437 c.head.addInt32(n_joint);
2438 Bottle &jointList = c.head.addList();
2439 for (int i = 0; i < n_joint; i++) {
2440 jointList.addInt32(joints[i]);
2441 }
2442 c.body.resize(n_joint);
2443 memcpy(&(c.body[0]), spds, sizeof(double)*n_joint);
2445 return ReturnValue_ok;
2446 }
2447 else
2448 {
2449 LOCKMUTEX
2450 std::vector<int> tempjoints(joints, joints + n_joint);
2451 std::vector<double> tempdata(spds, spds + n_joint);
2453 if (!ret) {
2454 yCError(CONTROLBOARD_NWC_YARP, "Unable to velocityMoveGroupRPC");
2455 return ret;
2456 }
2457 return ret;
2458 }
2459}
2460
2462{
2463 LOCKMUTEX
2464 auto ret = m_RPC.getTargetVelocityOneRPC(joint);
2465 if (!ret.ret) {
2466 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTargetVelocityOneRPC");
2467 return ret.ret;
2468 }
2469 *vel = ret.val;
2470 return ret.ret;
2471}
2472
2474{
2475 LOCKMUTEX
2476 auto ret = m_RPC.getTargetVelocityAllRPC();
2477 if (!ret.ret) {
2478 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTargetVelocityAllRPC");
2479 return ret.ret;
2480 }
2481 std::copy(ret.val.begin(), ret.val.end(), vels);
2482 return ret.ret;
2483}
2484
2486{
2487 LOCKMUTEX
2488 std::vector<int> tempjoints(joints, joints + n_joint);
2490 if (!ret.ret) {
2491 yCError(CONTROLBOARD_NWC_YARP, "Unable to getTargetVelocityGroupRPC");
2492 return ret.ret;
2493 }
2494 std::copy(ret.val.begin(), ret.val.end(), vels);
2495 return ret.ret;
2496}
2497
2498// END IVelocityControl
2499
2500// BEGIN IInteractionMode
2501
2503{
2504 double localArrivalTime=0.0;
2505 extendedPortMutex.lock();
2506 int mode_tmp;
2509 extendedPortMutex.unlock();
2510 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2511}
2512
2513yarp::dev::ReturnValue ControlBoard_nwc_yarp::getInteractionModes(const std::vector<int>& joints, std::vector<yarp::dev::InteractionModeEnum>& modes)
2514{
2515 double localArrivalTime=0.0;
2516
2517 extendedPortMutex.lock();
2519 if(ret)
2520 {
2521 for (int i = 0; i < joints.size(); i++) {
2523 }
2524 } else {
2525 ret = false;
2526 }
2527
2528 extendedPortMutex.unlock();
2529 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2530}
2531
2532yarp::dev::ReturnValue ControlBoard_nwc_yarp::getInteractionModes(std::vector<yarp::dev::InteractionModeEnum>& modes)
2533{
2534 double localArrivalTime=0.0;
2535 extendedPortMutex.lock();
2537 if(ret)
2538 {
2539 for (int i = 0; i < modes.size(); i++) {
2541 }
2542 } else {
2543 ret = false;
2544 }
2545 extendedPortMutex.unlock();
2546 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2547}
2548
2550{
2551 LOCKMUTEX
2552 auto ret = m_RPC.setInteractionModeOneRPC(axis,mode);
2553 if (!ret) {
2554 yCError(CONTROLBOARD_NWC_YARP, "Unable to setInteractionModeRPC");
2555 return ret;
2556 }
2557 return ret;
2558}
2559
2560yarp::dev::ReturnValue ControlBoard_nwc_yarp::setInteractionModes(const std::vector<int>& joints, const std::vector<yarp::dev::InteractionModeEnum>& modes)
2561{
2562 LOCKMUTEX
2563 auto ret = m_RPC.setInteractionModesGroupRPC(joints, modes);
2564 if (!ret) {
2565 yCError(CONTROLBOARD_NWC_YARP, "Unable to setInteractionModesGroupRPC");
2566 return ret;
2567 }
2568 return ret;}
2569
2570yarp::dev::ReturnValue ControlBoard_nwc_yarp::setInteractionModes(const std::vector<yarp::dev::InteractionModeEnum>& modes)
2571{
2572 LOCKMUTEX
2573 auto ret = m_RPC.setInteractionModesAllRPC(modes);
2574 if (!ret) {
2575 yCError(CONTROLBOARD_NWC_YARP, "Unable to setInteractionModesAllRPC");
2576 return ret;
2577 }
2578 return ret;}
2579
2580// END IInteractionMode
2581
2582// BEGIN IRemoteCalibrator
2583
2585{
2586 LOCKMUTEX
2588 if (!ret.ret) {
2589 yCError(CONTROLBOARD_NWC_YARP, "Unable to isCalibratorDevicePresentRPC");
2590 return ret.ret;
2591 }
2592 *isCalib = ret.isPresent;
2593 return ret.ret;
2594}
2595
2597{
2598 LOCKMUTEX
2599 auto ret = m_RPC.calibrateSingleJointRPC(j);
2600 if (!ret) {
2601 yCError(CONTROLBOARD_NWC_YARP, "Unable to calibrateSingleJoint");
2602 return ret;
2603 }
2604 return ret;
2605}
2606
2608{
2609 LOCKMUTEX
2610 auto ret = m_RPC.calibrateWholePartRPC();
2611 if (!ret) {
2612 yCError(CONTROLBOARD_NWC_YARP, "Unable to calibrateWholePart");
2613 return ret;
2614 }
2615 return ret;
2616}
2617
2619{
2620 LOCKMUTEX
2621 auto ret = m_RPC.homingSingleJointRPC(j);
2622 if (!ret) {
2623 yCError(CONTROLBOARD_NWC_YARP, "Unable to homingSingleJoint");
2624 return ret;
2625 }
2626 return ret;
2627}
2628
2630{
2631 LOCKMUTEX
2632 auto ret = m_RPC.homingWholePartRPC();
2633 if (!ret) {
2634 yCError(CONTROLBOARD_NWC_YARP, "Unable to homingWholePart");
2635 return ret;
2636 }
2637 return ret;
2638}
2639
2641{
2642 LOCKMUTEX
2643 auto ret = m_RPC.parkSingleJointRPC(j,_wait);
2644 if (!ret) {
2645 yCError(CONTROLBOARD_NWC_YARP, "Unable to parkSingleJoint");
2646 return ret;
2647 }
2648 return ret;
2649}
2650
2652{
2653 LOCKMUTEX
2654 auto ret = m_RPC.parkWholePartRPC();
2655 if (!ret) {
2656 yCError(CONTROLBOARD_NWC_YARP, "Unable to parkWholePart");
2657 return ret;
2658 }
2659 return ret;
2660}
2661
2663{
2664 LOCKMUTEX
2665 auto ret = m_RPC.quitCalibrateRPC();
2666 if (!ret) {
2667 yCError(CONTROLBOARD_NWC_YARP, "Unable to quitCalibrate");
2668 return ret;
2669 }
2670 return ret;
2671}
2672
2674{
2675 LOCKMUTEX
2676 auto ret = m_RPC.quitParkRPC();
2677 if (!ret) {
2678 yCError(CONTROLBOARD_NWC_YARP, "Unable to quitPark");
2679 return ret;
2680 }
2681 return ret;
2682}
2683
2684// END IRemoteCalibrator
2685
2686// BEGIN ICurrentControl
2687
2689{
2690 LOCKMUTEX
2691 auto ret = m_RPC.getRefCurrentAllRPC();
2692 if (!ret.ret) {
2693 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefCurrentAllRPC");
2694 return ret.ret;
2695 }
2696 std::copy(ret.refs.begin(), ret.refs.end(), t);
2697 return ret.ret;
2698}
2699
2701{
2702 LOCKMUTEX
2703 auto ret = m_RPC.getRefCurrentOneRPC(j);
2704 if (!ret.ret) {
2705 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefCurrentOneRPC");
2706 return ret.ret;
2707 }
2708 *t = ret.ref;
2709 return ret.ret;
2710}
2711
2713{
2714 if (m_use_streaming)
2715 {
2716 CHECK_INTERFACE("iCurrentControl")
2717 if (!isLive()) {
2718 return ReturnValue::return_code::return_value_error_not_ready;
2719 }
2721 c.head.clear();
2722 c.head.addVocab32(VOCAB_CURRENTCONTROL_INTERFACE);
2723 c.head.addVocab32(VOCAB_CURRENT_REFS);
2724 c.body.resize(m_nj);
2725 memcpy(&(c.body[0]), refs, sizeof(double)*m_nj);
2727 return ReturnValue_ok;
2728 }
2729 else
2730 {
2731 LOCKMUTEX
2732 std::vector<double> temp(refs, refs + this->m_nj);
2733 auto ret = m_RPC.setRefCurrentAllRPC(temp);
2734 if (!ret) {
2735 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRefCurrentAllRPC");
2736 return ret;
2737 }
2738 return ret;
2739 }
2740}
2741
2743{
2744 LOCKMUTEX
2745 auto ret = m_RPC.setRefCurrentOneRPC(j,ref);
2746 if (!ret) {
2747 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRefCurrentOneRPC");
2748 return ret;
2749 }
2750 return ret;
2751}
2752
2753yarp::dev::ReturnValue ControlBoard_nwc_yarp::setRefCurrents(const int n_joint, const int *joints, const double *refs)
2754{
2755 if (m_use_streaming)
2756 {
2757 CHECK_INTERFACE("iCurrentControl")
2758 if (!isLive()) {
2759 return ReturnValue::return_code::return_value_error_not_ready;
2760 }
2762 c.head.clear();
2763 c.head.addVocab32(VOCAB_CURRENTCONTROL_INTERFACE);
2764 c.head.addVocab32(VOCAB_CURRENT_REF_GROUP);
2765 c.head.addInt32(n_joint);
2766 Bottle &jointList = c.head.addList();
2767 for (int i = 0; i < n_joint; i++) {
2768 jointList.addInt32(joints[i]);
2769 }
2770 c.body.resize(n_joint);
2771 memcpy(&(c.body[0]), refs, sizeof(double)*n_joint);
2773 return ReturnValue_ok;
2774 }
2775 else
2776 {
2777 LOCKMUTEX
2778 std::vector<int> tempjoints(joints, joints + n_joint);
2779 std::vector<double> tempdata(refs, refs + n_joint);
2781 if (!ret) {
2782 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRefCurrentGroupRPC");
2783 return ret;
2784 }
2785 return ret;
2786 }
2787}
2788
2790{
2791 if (m_use_streaming)
2792 {
2793 CHECK_INTERFACE("iCurrentControl")
2794 if (!isLive()) {
2795 return ReturnValue::return_code::return_value_error_not_ready;
2796 }
2797 double localArrivalTime=0.0;
2798 extendedPortMutex.lock();
2800 extendedPortMutex.unlock();
2801 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2802 }
2803 else
2804 {
2805 LOCKMUTEX
2806 auto ret = m_RPC.getCurrentAllRPC();
2807 if (!ret.ret) {
2808 yCError(CONTROLBOARD_NWC_YARP, "Unable to getCurrentAllRPC");
2809 return ret.ret;
2810 }
2811 std::copy(ret.currs.begin(), ret.currs.end(), vals);
2812 return ret.ret;
2813 }
2814}
2815
2817{
2818 if (m_use_streaming)
2819 {
2820 CHECK_INTERFACE("iCurrentControl")
2821 if (!isLive()) {
2822 return ReturnValue::return_code::return_value_error_not_ready;
2823 }
2824 double localArrivalTime = 0.0;
2825 extendedPortMutex.lock();
2827 extendedPortMutex.unlock();
2828 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2829 }
2830 else
2831 {
2832 LOCKMUTEX
2833 auto ret = m_RPC.getCurrentOneRPC(j);
2834 if (!ret.ret) {
2835 yCError(CONTROLBOARD_NWC_YARP, "Unable to getCurrentOneRPC");
2836 return ret.ret;
2837 }
2838 *val = ret.curr;
2839 return ret.ret;
2840 }
2841}
2842
2844{
2845 LOCKMUTEX
2846 auto ret = m_RPC.getCurrentRangeOneRPC(j);
2847 if (!ret.ret) {
2848 yCError(CONTROLBOARD_NWC_YARP, "Unable to getCurrentRangeOneRPC");
2849 return ret.ret;
2850 }
2851 *max = ret.max;
2852 *min = ret.min;
2853 return ret.ret;
2854}
2855
2857{
2858 LOCKMUTEX
2859 auto ret = m_RPC.getCurrentRangeAllRPC();
2860 if (!ret.ret) {
2861 yCError(CONTROLBOARD_NWC_YARP, "Unable to getCurrentRangeOneRPC");
2862 return ret.ret;
2863 }
2864 std::copy(ret.mins.begin(), ret.mins.end(), min);
2865 std::copy(ret.maxs.begin(), ret.maxs.end(), max);
2866 return ret.ret;
2867}
2868
2869// END ICurrentControl
2870
2871// BEGIN IPWMControl
2873{
2874 LOCKMUTEX
2875 auto ret = m_RPC.setRefDutyCycleOneRPC(j,v);
2876 if (!ret) {
2877 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRefDutyCycle");
2878 return ret;
2879 }
2880 return ret;
2881}
2882
2884{
2885 if (m_use_streaming)
2886 {
2887 CHECK_INTERFACE("iPwmControl")
2888 if (!isLive()) {
2889 return ReturnValue::return_code::return_value_error_not_ready;
2890 }
2892 c.head.clear();
2893 c.head.addVocab32(VOCAB_PWMCONTROL_INTERFACE);
2894 c.head.addVocab32(VOCAB_PWMCONTROL_REF_PWMS);
2895
2896 c.body.resize(m_nj);
2897
2898 memcpy(&(c.body[0]), v, sizeof(double)*m_nj);
2899
2901 return ReturnValue_ok;
2902 }
2903 else
2904 {
2905 LOCKMUTEX
2906 std::vector<double> temp(v, v + this->m_nj);
2907 auto ret = m_RPC.setRefDutyCycleAllRPC(temp);
2908 if (!ret) {
2909 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRefDutyCycleAllRPC");
2910 return ret;
2911 }
2912 return ret;
2913 }
2914}
2915
2917{
2918 LOCKMUTEX
2919 auto ret = m_RPC.getRefDutyCycleOneRPC(j);
2920 if (!ret.ret) {
2921 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefDutyCycle");
2922 return ret.ret;
2923 }
2924 *ref = ret.val;
2925 return ret.ret;
2926}
2927
2929{
2930 LOCKMUTEX
2931 auto ret = m_RPC.getRefDutyCycleAllRPC();
2932 if (!ret.ret) {
2933 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefDutyCycle");
2934 return ret.ret;
2935 }
2936 std::copy(ret.val.begin(), ret.val.end(), refs);
2937 return ret.ret;
2938}
2939
2941{
2942 double localArrivalTime = 0.0;
2943 extendedPortMutex.lock();
2945 extendedPortMutex.unlock();
2946 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2947}
2948
2950{
2951 double localArrivalTime = 0.0;
2952 extendedPortMutex.lock();
2954 extendedPortMutex.unlock();
2955 return ret?ReturnValue_ok:ReturnValue::return_code::return_value_error_not_ready;
2956}
2957// END IPWMControl
2958
2959// BEGIN IJointBrake
2961{
2962 LOCKMUTEX
2963 auto ret = m_RPC.isJointBrakedRPC(j);
2964 if (!ret.ret) {
2965 yCError(CONTROLBOARD_NWC_YARP, "Unable to isJointBraked");
2966 return ret.ret;
2967 }
2968 braked = ret.isBraked;
2969 return ret.ret;
2970}
2971
2973{
2974 LOCKMUTEX
2975 auto ret = m_RPC.setManualBrakeActiveRPC(j,active);
2976 if (!ret) {
2977 yCError(CONTROLBOARD_NWC_YARP, "Unable to setManualBrakeActive");
2978 return ret;
2979 }
2980 return ret;
2981}
2982
2984{
2985 LOCKMUTEX
2986 auto ret = m_RPC.setAutoBrakeEnabledRPC(j,enabled);
2987 if (!ret) {
2988 yCError(CONTROLBOARD_NWC_YARP, "Unable to setAutoBrakeEnabled");
2989 return ret;
2990 }
2991 return ret;
2992}
2993
2995{
2996 LOCKMUTEX
2997 auto ret = m_RPC.getAutoBrakeEnabledRPC(j);
2998 if (!ret.ret) {
2999 yCError(CONTROLBOARD_NWC_YARP, "Unable to getAutoBrakeEnabled");
3000 return ret.ret;
3001 }
3002 enabled = ret.enabled;
3003 return ret.ret;
3004}
3005// END IJointBrake
3006
3007// IVelocityDirect
3009{
3010 LOCKMUTEX
3011 auto ret = m_RPC.getAxesRPC();
3012 if (!ret.ret) {
3013 yCError(CONTROLBOARD_NWC_YARP, "Unable to getAxes");
3014 return ret.ret;
3015 }
3016 axes = ret.axes;
3017 return ret.ret;
3018}
3019
3021{
3022 if (m_use_streaming)
3023 {
3024 CHECK_INTERFACE("iVelocityDirect")
3025 if (!isLive()) {
3026 return ReturnValue::return_code::return_value_error_not_ready;
3027 }
3029 c.head.clear();
3031 c.head.addVocab32(VOCAB_VELOCITY_DIRECT_SET_ONE);
3032 c.head.addInt32(jnt);
3033 c.body.resize(1);
3034 c.body[0] = vel;
3036 return ReturnValue_ok;
3037 }
3038 else
3039 {
3040 LOCKMUTEX
3041 auto ret = m_RPC.setRefVelocityOneRPC(jnt,vel);
3042 if (!ret) {
3043 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRefVelocityOneRPC");
3044 return ret;
3045 }
3046 return ret;
3047 }
3048}
3049
3051{
3052 if (m_use_streaming)
3053 {
3054 CHECK_INTERFACE("iVelocityDirect")
3055 if (!isLive()) {
3056 return ReturnValue::return_code::return_value_error_not_ready;
3057 }
3059 c.head.clear();
3061 c.head.addVocab32(VOCAB_VELOCITY_DIRECT_SET_ALL);
3062 c.body.resize(m_nj);
3063 for (size_t i = 0; i < vels.size(); i++) {
3064 c.body[i]=vels[i];
3065 }
3067 return ReturnValue_ok;
3068 }
3069 else
3070 {
3071 LOCKMUTEX
3072 auto ret = m_RPC.setRefVelocityAllRPC(vels);
3073 if (!ret) {
3074 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRefVelocityAllRPC");
3075 return ret;
3076 }
3077 return ret;
3078 }
3079}
3080
3081ReturnValue ControlBoard_nwc_yarp::setRefVelocity(const std::vector<int>& jnts, const std::vector<double>& vels)
3082{
3083 if (m_use_streaming)
3084 {
3085 CHECK_INTERFACE("iVelocityDirect")
3086 if (!isLive()) {
3087 return ReturnValue::return_code::return_value_error_not_ready;
3088 }
3090 c.head.clear();
3092 c.head.addVocab32(VOCAB_VELOCITY_DIRECT_SET_GROUP);
3093 c.head.addInt32(jnts.size());
3094 Bottle &jointList = c.head.addList();
3095 for (size_t i = 0; i < jnts.size(); i++) {
3096 jointList.addInt32(jnts[i]);
3097 }
3098 c.body.resize(jnts.size());
3099 for (size_t i = 0; i < jnts.size(); i++) {
3100 c.body[i]=vels[i];
3101 }
3103 return ReturnValue_ok;
3104 }
3105 else
3106 {
3107 LOCKMUTEX
3109 if (!ret) {
3110 yCError(CONTROLBOARD_NWC_YARP, "Unable to setRefVelocityGroupRPC");
3111 return ret;
3112 }
3113 return ret;
3114 }
3115}
3116
3118{
3119 LOCKMUTEX
3120 auto ret = m_RPC.getRefVelocityOneRPC(jnt);
3121 if (!ret.ret) {
3122 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefVelocityOneRPC");
3123 return ret.ret;
3124 }
3125 vel = ret.vel;
3126 return ret.ret;
3127}
3128
3130{
3131 LOCKMUTEX
3132 auto ret = m_RPC.getRefVelocityAllRPC();
3133 if (!ret.ret) {
3134 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefVelocityAllRPC");
3135 return ret.ret;
3136 }
3137 vels = ret.vel;
3138 return ret.ret;
3139}
3140
3141ReturnValue ControlBoard_nwc_yarp::getRefVelocity(const std::vector<int>& jnts, std::vector<double>& vels)
3142{
3143 LOCKMUTEX
3144 auto ret = m_RPC.getRefVelocityGroupRPC(jnts);
3145 if (!ret.ret) {
3146 yCError(CONTROLBOARD_NWC_YARP, "Unable to getRefVelocityGroupRPC");
3147 return ret.ret;
3148 }
3149 vels = ret.vel;
3150 return ret.ret;
3151}
define control board standard interfaces
constexpr yarp::conf::vocab32_t VOCAB_TIMESTAMP
#define CHECK_INTERFACE(VALUE)
#define LOCKMUTEX
const yarp::os::LogComponent & CONTROLBOARD_NWC_YARP()
constexpr yarp::conf::vocab32_t VOCAB_AMP_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_AMP_CURRENTS
constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODES
constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODE
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REF_GROUP
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REFS
constexpr yarp::conf::vocab32_t VOCAB_CURRENTCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_ACCELERATIONS
Definition IEncoders.h:214
constexpr yarp::conf::vocab32_t VOCAB_ENCODER
Definition IEncoders.h:207
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_SPEEDS
Definition IEncoders.h:212
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_SPEED
Definition IEncoders.h:211
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_ACCELERATION
Definition IEncoders.h:213
constexpr yarp::conf::vocab32_t VOCAB_ENCODERS
Definition IEncoders.h:208
constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODES
constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODE
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_ENCODERS
constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURE
Definition IMotor.h:161
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_PWM_OUTPUTS
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_REF_PWMS
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_PWM_OUTPUT
constexpr yarp::conf::vocab32_t VOCAB_PIDCONTROL_SET_FEEDFORWARD
constexpr yarp::conf::vocab32_t VOCAB_PIDCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_PIDCONTROL_SET_OFFSET
constexpr yarp::conf::vocab32_t VOCAB_POSITIONDIRECTCONTROL_INTERFACE
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_TRQ
constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECTS
constexpr yarp::conf::vocab32_t VOCAB_TRQS
constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECT_GROUP
constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_TORQUECONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVE
constexpr yarp::conf::vocab32_t VOCAB_VELOCITYCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVES
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_DIRECT_SET_ONE
constexpr yarp::conf::vocab32_t VOCAB_VELOCITYDIRECTCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_DIRECT_SET_GROUP
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_DIRECT_SET_ALL
#define ReturnValue_ok
Definition ReturnValue.h:82
virtual return_getTargetVelocityOne getTargetVelocityOneRPC(const std::int32_t j) const
virtual return_getCurrentImpedanceLimit getCurrentImpedanceLimitRPC(const std::int32_t j) const
virtual return_getTargetPositionGroup getTargetPositionGroupRPC(const std::vector< std::int32_t > &joints) const
virtual yarp::dev::ReturnValue setPeakCurrentRPC(const std::int32_t m, const double val)
virtual return_getRefTorqueAll getRefTorqueAllRPC() const
virtual yarp::dev::ReturnValue resetMotorEncoderRPC(const std::int32_t j)
virtual yarp::dev::ReturnValue setRefCurrentOneRPC(const std::int32_t j, const double ref)
virtual return_checkMotionDoneOne checkMotionDoneOneRPC(const std::int32_t j) const
virtual yarp::dev::ReturnValue setMotorEncoderCountsPerRevolutionRPC(const std::int32_t j, const double val)
virtual return_getPidExtraInfo getPidExtraInfoRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j) const
virtual yarp::dev::ReturnValue setRefCurrentGroupRPC(const std::vector< std::int32_t > &j, const std::vector< double > &refs)
virtual yarp::dev::ReturnValue calibrateSingleJointRPC(const std::int32_t j)
virtual return_getDeviceInterfaces getDeviceInterfacesRPC() const
virtual return_getPidOutputs getPidOutputsRPC(const yarp::dev::PidControlTypeEnum pidtype) const
virtual yarp::dev::ReturnValue calibrationDoneRPC(const std::int32_t j)
virtual yarp::dev::ReturnValue setControlModeGroupRPC(const std::vector< std::int32_t > &j, const std::vector< yarp::dev::SelectableControlModeEnum > &modes)
virtual return_isCalibratorDevicePresent isCalibratorDevicePresentRPC() const
virtual yarp::dev::ReturnValue setManualBrakeActiveRPC(const std::int32_t j, const bool active)
virtual yarp::dev::ReturnValue setTrajSpeedAllRPC(const std::vector< double > &spds)
virtual yarp::dev::ReturnValue setPWMLimitRPC(const std::int32_t m, const double val)
virtual yarp::dev::ReturnValue setInteractionModeOneRPC(const std::int32_t axis, const yarp::dev::InteractionModeEnum mode)
virtual yarp::dev::ReturnValue relativeMoveAllRPC(const std::vector< double > &deltas)
virtual return_getPid getPidRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j) const
virtual yarp::dev::ReturnValue velocityMoveAllRPC(const std::vector< double > &refs)
virtual return_getPowerSupplyVoltage getPowerSupplyVoltageRPC(const std::int32_t m) const
virtual yarp::dev::ReturnValue setPosLimitsRPC(const std::int16_t j, const double min, const double max)
virtual return_getRefDutyCycleAll getRefDutyCycleAllRPC() const
virtual return_getNumberOfMotors getNumberOfMotorsRPC() const
virtual yarp::dev::ReturnValue stopAllRPC()
virtual bool checkProtocolVersion()
virtual yarp::dev::ReturnValue parkWholePartRPC()
virtual yarp::dev::ReturnValue setRefPositionGroupRPC(const std::vector< std::int32_t > &j, const std::vector< double > &value)
virtual yarp::dev::ReturnValue homingSingleJointRPC(const std::int32_t j)
virtual yarp::dev::ReturnValue disablePidRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j)
virtual yarp::dev::ReturnValue setTrajAccelerationOneRPC(const std::int32_t j, const double acc)
virtual return_getMotorTorqueParams getMotorTorqueParamsRPC(const std::int32_t j)
virtual return_getTargetVelocityAll getTargetVelocityAllRPC() const
virtual return_getPidReferences getPidReferencesRPC(const yarp::dev::PidControlTypeEnum pidtype) const
virtual return_getVelLimits getVelLimitsRPC(const std::int16_t j) const
virtual return_getTrajAccelerationOne getTrajAccelerationOneRPC(const std::int32_t j) const
virtual return_getAmpStatusAll getAmpStatusAllRPC() const
virtual yarp::dev::ReturnValue setMotorTorqueParamsRPC(const std::int32_t j, const yMotorTorqueParameters &params)
virtual return_getTorqueRangeAll getTorqueRangeAllRPC() const
virtual return_getCurrentAll getCurrentAllRPC() const
virtual return_getTrajSpeedsAll getTrajSpeedsAllRPC() const
virtual return_getMaxCurrent getMaxCurrentRPC(const std::int32_t j) const
virtual yarp::dev::ReturnValue setControlModeOneRPC(const std::int32_t j, const yarp::dev::SelectableControlModeEnum mod)
virtual yarp::dev::ReturnValue setPidErrorLimitsRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::vector< double > &limits)
virtual yarp::dev::ReturnValue setPidRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j, const yarp::dev::Pid &pid)
virtual return_getAvailableControlModes getAvailableControlModesRPC(const std::int16_t j) const
virtual yarp::dev::ReturnValue stopOneRPC(const std::int32_t j)
virtual return_getPWMLimit getPWMLimitRPC(const std::int32_t m) const
virtual return_isPidEnabled isPidEnabledRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j) const
virtual yarp::dev::ReturnValue parkSingleJointRPC(const std::int32_t j, const bool wait)
virtual return_getMotorEncoderCountsPerRevolution getMotorEncoderCountsPerRevolutionRPC(const std::int32_t m) const
virtual yarp::dev::ReturnValue disableAmpRPC(const std::int32_t j)
virtual yarp::dev::ReturnValue setEncoderOneRPC(const std::int32_t j, const double val)
virtual yarp::dev::ReturnValue calibrateRobotRPC()
virtual return_getTargetVelocityGroup getTargetVelocityGroupRPC(const std::vector< std::int32_t > &joints) const
virtual yarp::dev::ReturnValue setPidFeedforwardOneRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j, const double off)
virtual yarp::dev::ReturnValue setRefDutyCycleOneRPC(const std::int32_t j, const double v)
virtual return_getTargetPositionAll getTargetPositionAllRPC() const
virtual return_getRemoteVariable getRemoteVariableRPC(const std::string &key) const
virtual yarp::dev::ReturnValue resetPidRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j)
virtual return_getTrajSpeedOne getTrajSpeedOneRPC(const std::int32_t j) const
virtual yarp::dev::ReturnValue setRefVelocityAllRPC(const std::vector< double > &ref)
virtual yarp::dev::ReturnValue resetEncoderOneRPC(const std::int32_t j)
virtual return_getRefTorqueOne getRefTorqueOneRPC(const std::int32_t j) const
virtual yarp::dev::ReturnValue setEncoderAllRPC(const std::vector< double > &vals)
virtual yarp::dev::ReturnValue setMotorEncodersAllRPC(const std::vector< double > &vals)
virtual return_checkMotionDoneGroup checkMotionDoneGroupRPC(const std::vector< std::int32_t > &joints) const
virtual yarp::dev::ReturnValue setTrajAccelerationsGroupRPC(const std::vector< std::int32_t > &joints, const std::vector< double > &accs)
virtual yarp::dev::ReturnValue setRefPositionOneRPC(const std::int32_t j, const double value)
virtual yarp::dev::ReturnValue setTemperatureLimitRPC(const std::int32_t m, const double val)
virtual yarp::dev::ReturnValue setRefTorqueGroupRPC(const std::vector< std::int32_t > &j, const std::vector< double > &val)
virtual yarp::dev::ReturnValue setPidReferenceRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j, const double ref)
virtual yarp::dev::ReturnValue setNominalCurrentRPC(const std::int32_t m, const double val)
virtual yarp::dev::ReturnValue resetEncoderAllRPC()
virtual yarp::dev::ReturnValue setRefTorqueOneRPC(const std::int32_t j, const double val)
virtual yarp::dev::ReturnValue resetMotorEncodersRPC()
virtual return_getCurrentRangeOne getCurrentRangeOneRPC(const std::int32_t j) const
virtual return_getImpedanceOffset getImpedanceOffsetRPC(const std::int32_t j) const
virtual return_getPidOffset getPidOffsetRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j) const
virtual return_getGearboxRatio getGearboxRatioRPC(const std::int32_t m) const
virtual yarp::dev::ReturnValue setMaxCurrentRPC(const std::int32_t j, const double v)
virtual return_getRefPositionAll getRefPositionAllRPC() const
virtual yarp::dev::ReturnValue setAutoBrakeEnabledRPC(const std::int32_t j, const bool enabled)
virtual yarp::dev::ReturnValue quitCalibrateRPC()
virtual return_getAxes getAxesRPC() const
virtual yarp::dev::ReturnValue parkRPC(const bool wait)
virtual return_getAutoBrakeEnabled getAutoBrakeEnabledRPC(const std::int32_t j) const
virtual return_getNominalCurrent getNominalCurrentRPC(const std::int32_t m) const
virtual yarp::dev::ReturnValue positionMoveOneRPC(const std::int32_t j, const double ref)
virtual yarp::dev::ReturnValue quitParkRPC()
virtual yarp::dev::ReturnValue setPidReferencesRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::vector< double > &refs)
virtual yarp::dev::ReturnValue enablePidRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j)
virtual yarp::dev::ReturnValue setGearboxRatioRPC(const std::int32_t m, const double val)
virtual return_getTrajSpeedsGroup getTrajSpeedsGroupRPC(const std::vector< std::int32_t > &joints) const
virtual yarp::dev::ReturnValue relativeMoveOneRPC(const std::int32_t j, const double delta)
virtual return_getRefPositionOne getRefPositionOneRPC(const std::int32_t j) const
virtual yarp::dev::ReturnValue calibrateWholePartRPC()
virtual return_getPidExtraInfos getPidExtraInfosRPC(const yarp::dev::PidControlTypeEnum pidtype) const
virtual return_getPeakCurrent getPeakCurrentRPC(const std::int32_t m) const
virtual yarp::dev::ReturnValue setImpedanceRPC(const std::int32_t j, const double stiffness, const double damping)
virtual return_getRefVelocityOne getRefVelocityOneRPC(const std::int32_t j) const
virtual yarp::dev::ReturnValue calibrateAxisWithParamsRPC(const std::int32_t j, const std::int32_t ui, const double v1, const double v2, const double v3)
virtual yarp::dev::ReturnValue setRefTorqueAllRPC(const std::vector< double > &val)
virtual yarp::dev::ReturnValue setTrajAccelerationsAllRPC(const std::vector< double > &accs)
virtual yarp::dev::ReturnValue setImpedanceOffsetRPC(const std::int32_t j, const double offset)
virtual yarp::dev::ReturnValue setVelLimitsRPC(const std::int16_t j, const double min, const double max)
virtual yarp::dev::ReturnValue setInteractionModesGroupRPC(const std::vector< std::int32_t > &joints, const std::vector< yarp::dev::InteractionModeEnum > &modes)
virtual yarp::dev::ReturnValue stopGroupRPC(const std::vector< std::int32_t > &joints)
virtual yarp::dev::ReturnValue setRefCurrentAllRPC(const std::vector< double > &refs)
virtual return_getAmpStatusOne getAmpStatusOneRPC(const std::int32_t j) const
virtual yarp::dev::ReturnValue homingWholePartRPC()
virtual yarp::dev::ReturnValue positionMoveGroupRPC(const std::vector< std::int32_t > &joints, const std::vector< double > &refs)
virtual yarp::dev::ReturnValue setTrajSpeedOneRPC(const std::int32_t j, const double sp)
virtual return_getImpedance getImpedanceRPC(const std::int32_t j) const
virtual return_getTargetPositionOne getTargetPositionOneRPC(const std::int32_t j) const
virtual return_getPidReference getPidReferenceRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j) const
virtual return_getTorqueRangeOne getTorqueRangeOneRPC(const std::int32_t j) const
virtual return_getPidErrorLimits getPidErrorLimitsRPC(const yarp::dev::PidControlTypeEnum pidtype) const
virtual return_getPidError getPidErrorRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j) const
virtual return_getRefVelocityAll getRefVelocityAllRPC() const
virtual return_getJointType getJointTypeRPC(const std::int32_t j)
virtual return_getCurrentOne getCurrentOneRPC(const std::int32_t j) const
virtual return_getRefDutyCycleOne getRefDutyCycleOneRPC(const std::int32_t j) const
virtual yarp::dev::ReturnValue setRefVelocityOneRPC(const std::int32_t j, const double ref)
virtual yarp::dev::ReturnValue setTrajSpeedGroupRPC(const std::vector< std::int32_t > &joints, const std::vector< double > &spds)
virtual return_checkMotionDoneAll checkMotionDoneAllRPC() const
virtual yarp::dev::ReturnValue velocityMoveOneRPC(const std::int32_t j, const double ref)
virtual yarp::dev::ReturnValue setRemoteVariableRPC(const std::string &key, const yarp::os::Bottle &val)
virtual return_getCurrentRangeAll getCurrentRangeAllRPC() const
virtual yarp::dev::ReturnValue setRefVelocityGroupRPC(const std::vector< std::int32_t > &j, const std::vector< double > &ref)
virtual yarp::dev::ReturnValue setCalibrationParametersRPC(const std::int32_t j, const yCalibrationParameters &params)
virtual return_getRemoteVariablesList getRemoteVariablesListRPC() const
virtual return_getPosLimits getPosLimitsRPC(const std::int16_t j) const
virtual return_getPidErrors getPidErrorsRPC(const yarp::dev::PidControlTypeEnum pidtype) const
virtual return_getPidOutput getPidOutputRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j) const
virtual return_getRefCurrentOne getRefCurrentOneRPC(const std::int32_t j) const
virtual yarp::dev::ReturnValue enableAmpRPC(const std::int32_t j)
virtual yarp::dev::ReturnValue setControlModeAllRPC(const std::vector< yarp::dev::SelectableControlModeEnum > &modes)
virtual yarp::dev::ReturnValue relativeMoveGroupRPC(const std::vector< std::int32_t > &joints, const std::vector< double > &refs)
virtual return_getPidFeedforward getPidFeedforwardRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j) const
virtual return_getAxisName getAxisNameRPC(const std::int32_t j)
virtual yarp::dev::ReturnValue abortCalibrationRPC()
virtual yarp::dev::ReturnValue setRefDutyCycleAllRPC(const std::vector< double > &v)
virtual yarp::dev::ReturnValue setPidsRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::vector< yarp::dev::Pid > &pids)
virtual yarp::dev::ReturnValue abortParkRPC()
virtual return_getPids getPidsRPC(const yarp::dev::PidControlTypeEnum pidtype) const
virtual yarp::dev::ReturnValue velocityMoveGroupRPC(const std::vector< std::int32_t > &joints, const std::vector< double > &refs)
virtual return_isJointBraked isJointBrakedRPC(const std::int32_t j) const
virtual return_getAvailablePids getAvailablePidsRPC(const std::int16_t j) const
virtual return_getTemperatureLimit getTemperatureLimitRPC(const std::int32_t m) const
virtual return_getRefCurrentAll getRefCurrentAllRPC() const
virtual yarp::dev::ReturnValue positionMoveAllRPC(const std::vector< double > &refs)
virtual return_getRefVelocityGroup getRefVelocityGroupRPC(const std::vector< std::int32_t > &j) const
virtual yarp::dev::ReturnValue setPidOffsetOneRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j, const double off)
virtual return_getPidErrorLimit getPidErrorLimitRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j) const
virtual return_getTrajAccelerationAll getTrajAccelerationAllRPC() const
virtual return_getRefPositionGroup getRefPositionGroupRPC(const std::vector< std::int32_t > &j) const
virtual yarp::dev::ReturnValue setRefPositionAllRPC(const std::vector< double > &value)
virtual return_getNumberOfMotorEncoders getNumberOfMotorEncodersRPC() const
virtual return_getLastJointFault getLastJointFaultRPC(const std::int32_t j) const
virtual return_getTrajAccelerationGroup getTrajAccelerationGroupRPC(const std::vector< std::int32_t > &joints) const
virtual yarp::dev::ReturnValue setPidErrorLimitRPC(const yarp::dev::PidControlTypeEnum pidtype, const std::int16_t j, const double limit)
virtual yarp::dev::ReturnValue setInteractionModesAllRPC(const std::vector< yarp::dev::InteractionModeEnum > &modes)
virtual yarp::dev::ReturnValue setMotorEncoderRPC(const std::int32_t j, const double val)
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
yarp::dev::ReturnValue setCalibrationParameters(int j, const CalibrationParameters &params) override
Start calibration, this method is very often platform specific.
yarp::dev::ReturnValue setPid(const PidControlTypeEnum &pidtype, int j, const Pid &pid) override
Set new PID value for a joint axis.
yarp::dev::ReturnValue setImpedanceOffset(int j, double offset) override
Set current force Offset for a specific joint.
yarp::dev::ReturnValue calibrateWholePart() override
calibrateWholePart: call the procedure for calibrating the whole device
yarp::dev::ReturnValue setImpedance(int j, double stiffness, double damping) override
Set current impedance gains (stiffness,damping) for a specific joint.
StateExtendedInputPort extendedIntputStatePort
yarp::dev::ReturnValue getTrajAccelerations(const int n_joint, const int *joints, double *accs) override
Get reference acceleration for a joint.
yarp::dev::ReturnValue getPidErrorLimits(const PidControlTypeEnum &pidtype, double *limits) override
Get the error limit for all controllers.
yarp::dev::ReturnValue setRefDutyCycles(const double *v) override
Sets the reference dutycycle for all the motors.
yarp::dev::ReturnValue stop() override
Stop motion, multiple joints.
yarp::dev::ReturnValue getRemoteVariablesList(yarp::os::Bottle *listOfKeys) override
yarp::dev::ReturnValue getPidError(const PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
yarp::dev::ReturnValue getAmpStatus(int *st) override
yarp::dev::ReturnValue setPidErrorLimit(const PidControlTypeEnum &pidtype, int j, double limit) override
Set the error limit for the controller on a specific joint.
yarp::dev::ReturnValue parkWholePart() override
parkWholePart: start the parking procedure for the whole part
yarp::dev::ReturnValue setTrajAccelerations(const int n_joint, const int *joints, const double *accs) override
Set reference acceleration on all joints.
yarp::dev::ReturnValue getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getCurrent(int j, double *val) override
yarp::dev::ReturnValue setGearboxRatio(int m, const double val) override
Set the gearbox ratio for a specific motor.
yarp::dev::ReturnValue getRefTorques(double *t) override
Get the reference value of the torque for all joints.
yarp::dev::ReturnValue setTrajAcceleration(int j, double acc) override
Set reference acceleration for a joint.
yarp::dev::ReturnValue setPidFeedforward(const PidControlTypeEnum &pidtype, int j, double v) override
Set the feedforward value for the specified PID controller.
bool close() override
Close the device driver and stop the port connections.
yarp::dev::ReturnValue resetMotorEncoders() override
Reset motor encoders.
yarp::dev::ReturnValue getTorqueRanges(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
yarp::dev::ReturnValue setPidErrorLimits(const PidControlTypeEnum &pidtype, const double *limits) override
Get the error limit for the controller on all joints.
yarp::dev::ReturnValue getMotorEncoderCountsPerRevolution(int m, double *cpr) override
Gets number of counts per revolution for motor encoder m.
yarp::dev::ReturnValue calibrationDone(int j) override
Check if the calibration is terminated, on a particular joint.
yarp::dev::ReturnValue getRefPositions(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue checkMotionDone(int j, bool &flag) override
Check if the current trajectory is terminated.
DiagnosticThread * diagnosticThread
yarp::dev::ReturnValue setPidReferences(const PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
yarp::dev::ReturnValue getMotorEncoder(int j, double *v) override
Read the value of a motor encoder.
yarp::dev::ReturnValue setPids(const PidControlTypeEnum &pidtype, const Pid *pids) override
Set new PID value on multiple axes.
yarp::dev::JointStateData last_wholePart
yarp::dev::ReturnValue resetEncoders() override
Reset encoders.
yarp::dev::ReturnValue getControlMode(int j, yarp::dev::ControlModeEnum &mode) override
Get the current control mode.
yarp::dev::ReturnValue getTorqueRange(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
yarp::dev::ReturnValue getAvailablePids(int j, std::vector< yarp::dev::PidControlTypeEnum > &avail) override
Return a list of available pid types mode for the given joint.
yarp::dev::ReturnValue getImpedanceOffset(int j, double *offset) override
Get current force Offset for a specific joint.
yarp::dev::ReturnValue getTemperatures(double *vals) override
Get temperature of all the motors.
yarp::dev::ReturnValue getTargetPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue disablePid(const PidControlTypeEnum &pidtype, int j) override
Disable the PID computation for a joint.
yarp::dev::ReturnValue getTorques(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
yarp::dev::ReturnValue setAutoBrakeEnabled(int j, bool enabled) override
Enables/Disables the automatic joint brake.
yarp::dev::ReturnValue getMotorEncoderSpeeds(double *spds) override
Read the instantaneous speed of all motor encoders.
yarp::dev::ReturnValue getPidFeedforward(const PidControlTypeEnum &pidtype, int j, double &v) override
Get the currently set feedforward value for the specified PID controller.
yarp::dev::ReturnValue isCalibratorDevicePresent(bool *isCalib) override
isCalibratorDevicePresent: check if a calibrator device has been set
yarp::dev::ReturnValue setPositions(const int n_joint, const int *joints, const double *refs) override
Set new reference point for all axes.
yarp::dev::ReturnValue getRefCurrent(int j, double *t) override
Get the reference value of the current for a single motor.
yarp::dev::ReturnValue getInteractionMode(int axis, yarp::dev::InteractionModeEnum &mode) override
Get the current interaction mode of the robot, values can be stiff or compliant.
yarp::dev::ReturnValue enableAmp(int j) override
Enable the amplifier on a specific joint.
yarp::dev::ReturnValue getGearboxRatio(int m, double *val) override
Get the gearbox ratio for a specific motor.
yarp::dev::ReturnValue getMotorEncoderAcceleration(int j, double *acc) override
Read the instantaneous acceleration of a motor encoder.
yarp::dev::ReturnValue getCurrentRange(int j, double *min, double *max) override
Get the full scale of the current measurement for a given motor (e.g.
yarp::dev::ReturnValue getRefPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getPidReference(const PidControlTypeEnum &pidtype, int j, double *ref) override
Get the current reference of the PID controller for a specific joint.
yarp::dev::ReturnValue setInteractionModes(const std::vector< int > &joints, const std::vector< yarp::dev::InteractionModeEnum > &modes) override
Set the interaction mode of the robot for a set of joints, values can be stiff or compliant.
yarp::dev::ReturnValue getControlModes(const std::vector< int > &joints, std::vector< yarp::dev::ControlModeEnum > &modes) override
Get the current control mode for a subset of axes.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode) override
Set the interaction mode of the robot, values can be stiff or compliant.
yarp::dev::ReturnValue getDutyCycles(double *outs) override
Gets the current dutycycle of the output of the amplifier (i.e.
yarp::dev::ReturnValue getAvailableControlModes(int j, std::vector< yarp::dev::SelectableControlModeEnum > &avail) override
Return a list of available control mode for the given joint.
yarp::dev::ReturnValue getPids(const PidControlTypeEnum &pidtype, Pid *pids) override
Get current PID value for a specific joint.
yarp::dev::ReturnValue getEncoderAcceleration(int j, double *acc) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue abortPark() override
yarp::dev::ReturnValue quitCalibrate() override
quitCalibrate: interrupt the calibration procedure
yarp::dev::ReturnValue disableAmp(int j) override
Disable the amplifier on a specific joint.
yarp::dev::ReturnValue getMaxCurrent(int j, double *v) override
Returns the maximum electric current allowed for a given motor.
yarp::dev::ReturnValue getDutyCycle(int j, double *out) override
Gets the current dutycycle of the output of the amplifier (i.e.
yarp::dev::ReturnValue homingWholePart() override
homingWholePart: call the homing procedure for a the whole part/device
yarp::dev::ReturnValue getRefCurrents(double *t) override
Get the reference value of the currents for all motors.
yarp::dev::ReturnValue setMaxCurrent(int j, double v) override
yarp::dev::ReturnValue positionMove(int j, double ref) override
Set new reference point for a single axis.
yarp::dev::ReturnValue setMotorEncoderCountsPerRevolution(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
yarp::dev::ReturnValue setRefTorques(const double *t) override
Set the reference value of the torque for all joints.
yarp::dev::ReturnValue getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
std::map< std::string, bool > m_device_has_interfaces
yarp::dev::ReturnValue getRemoteVariable(std::string key, yarp::os::Bottle &val) override
yarp::dev::ReturnValue getAutoBrakeEnabled(int j, bool &enabled) const override
checks if the automatic joint brake mode is enabled or disabled.
yarp::dev::ReturnValue setMotorTorqueParams(int j, const MotorTorqueParameters params) override
Set a subset of motor parameters (bemf, ktau etc) useful for torque control.
yarp::dev::ReturnValue setPeakCurrent(int m, const double val) override
yarp::dev::ReturnValue setPosLimits(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...
yarp::dev::ReturnValue getPWM(int m, double *val) override
yarp::dev::ReturnValue setRefDutyCycle(int j, double v) override
Sets the reference dutycycle to a single motor.
yarp::dev::ReturnValue getPidExtraInfo(const PidControlTypeEnum &pidtype, int j, PidExtraInfo &info) override
Get PID extra info and measurement units for a specific joint.
yarp::dev::ReturnValue getTemperatureLimit(int m, double *val) override
Retrieves the current temperature limit for a specific motor.
yarp::dev::ReturnValue enablePid(const PidControlTypeEnum &pidtype, int j) override
Enable the PID computation for a joint.
yarp::dev::ReturnValue setTrajSpeeds(const int n_joint, const int *joints, const double *spds) override
Set reference speed on all joints.
yarp::dev::ReturnValue velocityMove(int j, double v) override
Start motion at a given speed, single joint.
yarp::dev::ReturnValue getNumberOfMotors(int *num) override
Get the number of available motors.
yarp::dev::ReturnValue getCurrents(double *vals) override
yarp::dev::ReturnValue getPWMLimit(int m, double *val) override
yarp::dev::ReturnValue getEncoders(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
yarp::dev::ReturnValue getTrajSpeed(int j, double *ref) override
Get reference speed for a joint.
yarp::dev::ReturnValue getMotorTorqueParams(int j, MotorTorqueParameters *params) override
Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
yarp::dev::ReturnValue getMotorEncoderTimed(int j, double *v, double *t) override
Read the instantaneous position of a motor encoder.
yarp::dev::ReturnValue getJointType(int j, yarp::dev::JointTypeEnum &type) override
yarp::dev::ReturnValue getRefDutyCycles(double *refs) override
Gets the last reference sent using the setRefDutyCycles function.
yarp::dev::ReturnValue getLastJointFault(int j, int &fault, std::string &message) override
yarp::dev::ReturnValue getPidErrorLimit(const PidControlTypeEnum &pidtype, int j, double *limit) override
Get the error limit for the controller on a specific joint.
yarp::dev::ReturnValue getTrajSpeeds(const int n_joint, const int *joints, double *spds) override
Get reference speed of all joints.
yarp::dev::ReturnValue getMotorEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all motor encoders.
yarp::dev::ReturnValue calibrateSingleJoint(int j) override
calibrateSingleJoint: call the calibration procedure for the single joint
yarp::dev::ReturnValue getPidOffset(const PidControlTypeEnum &pidtype, int j, double &v) override
Get the currently set offset value for the specified PID controller.
yarp::dev::ReturnValue setControlModes(const std::vector< int > &joints, const std::vector< yarp::dev::SelectableControlModeEnum > &modes) override
Set the current control mode for a subset of axes.
yarp::dev::ReturnValue getImpedance(int j, double *stiffness, double *damping) override
Get current impedance gains (stiffness,damping,offset) for a specific joint.
yarp::dev::ReturnValue setRefTorque(int j, double v) override
Set the reference value of the torque for a given joint.
yarp::dev::ReturnValue parkSingleJoint(int j, bool _wait=true) override
parkSingleJoint(): start the parking procedure for the single joint
yarp::dev::ReturnValue setPWMLimit(int m, const double val) override
bool open(Searchable &config) override
Open the DeviceDriver.
yarp::dev::ReturnValue getPidExtraInfos(const PidControlTypeEnum &pidtype, std::vector< PidExtraInfo > &info) override
Get PID extra info and measurement units for a specific joint.
yarp::dev::ReturnValue getPid(const PidControlTypeEnum &pidtype, int j, Pid *pid) override
Get current PID value for a specific joint.
yarp::dev::ReturnValue setNominalCurrent(int m, const double val) override
yarp::dev::ReturnValue getRefTorque(int j, double *t) override
Get the reference value of the torque for a given joint.
yarp::dev::ReturnValue setManualBrakeActive(int j, bool active) override
Enables/Disables manually the joint brake.
yarp::dev::ReturnValue getNumberOfMotorEncoders(int *num) override
Get the number of available motor encoders.
yarp::dev::ReturnValue abortCalibration() override
yarp::dev::ReturnValue homingSingleJoint(int j) override
homingSingleJoint: call the homing procedure for a single joint
yarp::dev::ReturnValue park(bool wait=true) override
yarp::dev::ReturnValue getAxisName(int j, std::string &name) override
yarp::dev::ReturnValue setPidReference(const PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
yarp::dev::ReturnValue resetEncoder(int j) override
Reset encoder, single joint.
yarp::dev::ReturnValue setControlMode(int j, yarp::dev::SelectableControlModeEnum mode) override
Set the current control mode.
yarp::dev::ReturnValue setPidOffset(const PidControlTypeEnum &pidtype, int j, double v) override
Set an offset value for a given controller.
yarp::dev::ReturnValue getPosLimits(int axis, double *min, double *max) override
Get the software limits for a particular axis.
yarp::dev::ReturnValue 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...
yarp::dev::ReturnValue resetMotorEncoder(int j) override
Reset motor encoder, single motor.
yarp::dev::ReturnValue getMotorEncoderSpeed(int j, double *sp) override
Read the instantaneous speed of a motor encoder.
yarp::dev::ReturnValue isPidEnabled(const PidControlTypeEnum &pidtype, int j, bool &enabled) override
Get the current status (enabled/disabled) of the PID.
yarp::dev::ReturnValue getCurrentRanges(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
yarp::dev::ReturnValue isJointBraked(int j, bool &braked) const override
Check is the joint brake is currently active.
yarp::dev::ReturnValue getTargetVelocity(const int joint, double *vel) override
Get the last reference speed set by velocityMove for single joint.
yarp::dev::ReturnValue setRefCurrents(const double *refs) override
Set the reference value of the currents for all motors.
yarp::dev::ReturnValue getMotorEncoders(double *encs) override
Read the position of all motor encoders.
yarp::dev::ReturnValue getRefDutyCycle(int j, double *ref) override
Gets the last reference sent using the setRefDutyCycle function.
yarp::dev::ReturnValue getInteractionModes(const std::vector< int > &joints, std::vector< yarp::dev::InteractionModeEnum > &modes) override
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
yarp::dev::ReturnValue setTrajSpeed(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
yarp::dev::ReturnValue getEncodersTimed(double *encs, double *ts) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue setMotorEncoders(const double *vals) override
Set the value of all motor encoders.
yarp::dev::ReturnValue setEncoders(const double *vals) override
Set the value of all encoders.
yarp::dev::ReturnValue getVelLimits(int axis, double *min, double *max) override
Get the software speed limits for a particular axis.
yarp::dev::ReturnValue getTargetVelocities(double *vels) override
Get the last reference speed set by velocityMove for all joints.
yarp::dev::ReturnValue setRefVelocity(int jnt, double vel) override
Set the velocity of single joint.
yarp::os::PortWriterBuffer< yarp::dev::CommandMessageData > command_buffer
yarp::dev::ReturnValue getPidOutputs(const PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
yarp::dev::ReturnValue getRefVelocity(const int jnt, double &vel) override
Get the last reference velocity set by setRefVelocity() for a single joint.
yarp::dev::ReturnValue calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) override
Start calibration, this method is very often platform specific.
yarp::dev::ReturnValue quitPark() override
quitPark: interrupt the park procedure
yarp::dev::ReturnValue calibrateRobot() override
Calibrate robot by using an external calibrator.
yarp::dev::ReturnValue setRefCurrent(int j, double ref) override
Set the reference value of the current for a single motor.
yarp::dev::ReturnValue getEncoder(int j, double *v) override
Read the value of an encoder.
yarp::dev::ReturnValue getPeakCurrent(int m, double *val) override
yarp::dev::ReturnValue getTargetPositions(double *refs) override
Get the last position reference for all axes.
yarp::dev::JointStateData last_singleJoint
yarp::dev::ReturnValue getTrajAcceleration(int j, double *acc) override
Get reference acceleration for a joint.
yarp::dev::ReturnValue relativeMove(int j, double delta) override
Set relative position.
yarp::dev::ReturnValue 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 ...
yarp::dev::ReturnValue setMotorEncoder(int j, const double val) override
Set the value of the motor encoder for a given motor.
yarp::dev::ReturnValue getEncoderTimed(int j, double *v, double *t) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
Get the current impedance limits for a specific joint.
yarp::dev::ReturnValue setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
Stamp getLastInputStamp() override
Get the time stamp for the last read data.
yarp::dev::ReturnValue getPowerSupplyVoltage(int m, double *val) override
yarp::dev::ReturnValue getPidOutput(const PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
yarp::dev::ReturnValue getTorque(int j, double *t) override
Get the value of the torque on a given joint (this is the feedback if you have a torque sensor).
yarp::dev::ReturnValue setRemoteVariable(std::string key, const yarp::os::Bottle &val) override
yarp::dev::ReturnValue setPosition(int j, double ref) override
Set new position for a single axis.
yarp::dev::ReturnValue getPidErrors(const PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
yarp::dev::ReturnValue setTemperatureLimit(int m, const double val) override
Set the temperature limit for a specific motor.
yarp::dev::ReturnValue getTemperature(int m, double *val) override
Get temperature of a motor.
yarp::dev::ReturnValue getMotorEncodersTimed(double *encs, double *ts) override
Read the instantaneous position of all motor encoders.
yarp::dev::ReturnValue getNominalCurrent(int m, double *val) override
yarp::dev::ReturnValue getPidReferences(const PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all PID controllers.
void run() override
Loop function.
void setOwner(StateExtendedInputPort *o)
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)
yarp::sig::VectorOf< float > pwmDutycycle
yarp::sig::VectorOf< int > controlMode
yarp::sig::VectorOf< int > interactionMode
yarp::sig::VectorOf< double > motorAcceleration
yarp::sig::VectorOf< double > jointPosition
yarp::sig::VectorOf< double > torque
yarp::sig::VectorOf< double > motorVelocity
yarp::sig::VectorOf< double > jointVelocity
yarp::sig::VectorOf< double > current
yarp::sig::VectorOf< double > jointAcceleration
yarp::sig::VectorOf< double > motorPosition
Contains the parameters for a PID.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:65
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:252
A mini-server for performing network communication in the background.
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.
static bool setConnectionQos(const std::string &src, const std::string &dest, const QosStyle &srcStyle, const QosStyle &destStyle, bool quiet=true)
Adjust the Qos preferences of a connection.
Definition Network.cpp:1141
An abstraction for a periodic thread.
PeriodicThread(double period, ShouldUseSystemClock useSystemClock=ShouldUseSystemClock::No, PeriodicThreadClock clockAccuracy=PeriodicThreadClock::Relative)
Constructor.
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...
T & get()
A synonym of PortWriterBuffer::prepare.
void write(bool forceStrict=false)
Try to write the last buffer returned by PortWriterBuffer::get.
void attach(Port &port)
Set the Port to which objects will be written.
bool addOutput(const std::string &name) override
Add an output connection to the specified port.
Definition Port.cpp:320
void interrupt() override
Interrupt any current reads or writes attached to the port.
Definition Port.cpp:347
void close() override
Stop port activity.
Definition Port.cpp:330
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition Port.cpp:79
Preferences for the port's Quality of Service.
Definition QosStyle.h:23
A base class for nested structures that can be searched.
Definition Searchable.h:31
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
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition Value.cpp:228
virtual yarp::conf::vocab32_t asVocab32() const
Get 32 bit vocabulary identifier as an integer.
Definition Value.cpp:234
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition Value.cpp:210
yarp::os::WireLink & yarp()
Get YARP state associated with this object.
Definition Wire.h:28
void resize(size_t size) override
Resize the vector.
Definition Vector.h:231
T * data()
Return a pointer to the first element of the vector.
Definition Vector.h:216
#define yCInfo(component,...)
#define yCError(component,...)
#define yCDebug(component,...)
std::int32_t vocab32_t
Definition numeric.h:78
For streams capable of holding different kinds of content, check what they actually have.
SelectableControlModeEnum
PidControlTypeEnum
Definition PidEnums.h:15
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13