YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
xHandControlBoard.cpp
Go to the documentation of this file.
1// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
2// SPDX-License-Identifier: BSD-2-Clause
3#include "xHandControlBoard.h"
5#include <yarp/os/LogStream.h>
6
8YARP_LOG_COMPONENT(CB, "yarp.device.xHandControlBoard")
9
10# define rad2deg(x) ((x)*180.0/M_PI)
11# define deg2rad(x) ((x)*M_PI/180.0)
12
14 : yarp::os::PeriodicThread(0.001)
15{
16}
17
21
23{
24 if (!parseParams(config))
25 {
26 yCError(CB) << "Failed to parse parameters for xHandControlBoard.";
27 return false;
28 }
29
30 // Params check
31 if(m_GENERAL_AxisName.size() != EV_HAND_JOINT_NUM)
32 {
33 yCError(CB) << "Number of Axis names provided (" << m_GENERAL_AxisName.size() << ") does not match the number of joints supported by the hand (" << EV_HAND_JOINT_NUM << ").";
34 return false;
35 }
36 if(m_GENERAL_AxisMap.size() != EV_HAND_JOINT_NUM)
37 {
38 yCError(CB) << "Number of Axis map entries provided (" << m_GENERAL_AxisMap.size() << ") does not match the number of joints supported by the hand (" << EV_HAND_JOINT_NUM << ").";
39 return false;
40 }
41 if(m_GENERAL_AxisType.size() != EV_HAND_JOINT_NUM)
42 {
43 yCError(CB) << "Number of Axis types provided (" << m_GENERAL_AxisType.size() << ") does not match the number of joints supported by the hand (" << EV_HAND_JOINT_NUM << ").";
44 return false;
45 }
46 if(m_LIMITS_jntPosMax.size() != EV_HAND_JOINT_NUM)
47 {
48 yCError(CB) << "Number of joint position max limits provided (" << m_LIMITS_jntPosMax.size() << ") does not match the number of joints supported by the hand (" << EV_HAND_JOINT_NUM << ").";
49 return false;
50 }
51 if(m_LIMITS_jntPosMin.size() != EV_HAND_JOINT_NUM)
52 {
53 yCError(CB) << "Number of joint position min limits provided (" << m_LIMITS_jntPosMin.size() << ") does not match the number of joints supported by the hand (" << EV_HAND_JOINT_NUM << ").";
54 return false;
55 }
56 if(m_LIMITS_jntVelMin.size() != EV_HAND_JOINT_NUM)
57 {
58 yCError(CB) << "Number of joint velocity min limits provided (" << m_LIMITS_jntVelMin.size() << ") does not match the number of joints supported by the hand (" << EV_HAND_JOINT_NUM << ").";
59 return false;
60 }
61 if(m_LIMITS_jntVelMax.size() != EV_HAND_JOINT_NUM)
62 {
63 yCError(CB) << "Number of joint velocity max limits provided (" << m_LIMITS_jntVelMax.size() << ") does not match the number of joints supported by the hand (" << EV_HAND_JOINT_NUM << ").";
64 return false;
65 }
66 for(size_t i=0; i<EV_HAND_JOINT_NUM; i++)
67 {
68 if(m_LIMITS_jntPosMin[i] >= m_LIMITS_jntPosMax[i])
69 {
70 yCError(CB) << "Joint position limits for joint " << i << " are invalid: min limit (" << m_LIMITS_jntPosMin[i] << ") >= max limit (" << m_LIMITS_jntPosMax[i] << ").";
71 return false;
72 }
73 }
74 for(size_t i=0; i<EV_HAND_JOINT_NUM; i++){
75 if(m_LIMITS_jntVelMin[i] >= m_LIMITS_jntVelMax[i]){
76 yCError(CB) << "Joint velocity limits for joint " << i << " are invalid: min limit (" << m_LIMITS_jntVelMin[i] << ") >= max limit (" << m_LIMITS_jntVelMax[i] << ").";
77 return false;
78 }
79 }
80
81 // Looking for available devices
82 std::vector<std::string> ifnames = m_XHCtrl.enumerate_devices(m_connection_type);
83 if (ifnames.empty()) {
84 yCError(CB) << "No devices found for connection type: " << m_connection_type;
85 return false;
86 }
87
88 std::string debugMsg{};
89
90 xhand_control::ErrorStruct ret;
91 std::vector<std::string>::iterator ifnames_it{};
92 if (m_connection_type == "RS485"){
93
94 ifnames_it = std::find(ifnames.begin(), ifnames.end(), m_RS485_port);
95
96 if (ifnames_it == ifnames.end()){
97 for (const auto& ifname : ifnames) {debugMsg+=ifname + " ";}
98 yCError(CB) << "Specified RS485 port" << m_RS485_port << "not found among available devices: " << debugMsg;
99 return false;
100 }
101
102 ret = m_XHCtrl.open_serial(m_RS485_port, m_RS485_baudrate);
103 }
104 else if (m_connection_type == "EtherCAT"){
105 ifnames_it = std::find(ifnames.begin(), ifnames.end(), m_ETHERCAT_eth_ifname);
106
107 if (ifnames_it == ifnames.end()){
108 for (const auto& ifname : ifnames) {debugMsg+=ifname + " ";}
109 yCError(CB) << "Specified EtherCAT interface" << m_ETHERCAT_eth_ifname << "not found among available devices: " << debugMsg;
110 return false;
111 }
112
113 ret = m_XHCtrl.open_ethercat(m_ETHERCAT_eth_ifname);
114 }
115 else{
116 yCError(CB) << "Unknown connection type: " << m_connection_type;
117 return false;
118 }
119
120 if (!ret){
121 yCError(CB) << "Failed to open " << m_connection_type;
122 printErrorStruct(ret);
123 return false;
124 }
125
126 yDebug() << "Successfully connected to " << (m_connection_type == "RS485" ? m_RS485_port : m_ETHERCAT_eth_ifname);
127
128 std::vector<uint8_t> hand_list = m_XHCtrl.list_hands_id();
129 if (hand_list.empty()){
130 yCError(CB) << "No hands found on the network.";
131 return false;
132 }
133
134 debugMsg.clear();
135 for (const auto& hand : hand_list) {debugMsg+=std::to_string(hand) + " ";}
136 yCDebug(CB) << "Hand IDs:" << debugMsg;
137 m_id = hand_list[0]; //<---------------------------------------------------------------- For now, we support only one hand !!!
138 yCDebug(CB) << "Using hand with ID:" << static_cast<int>(m_id);
139
140 //start the rateThread
141 bool init = this->yarp::os::PeriodicThread::start();
142 if(!init)
143 {
144 yCError(CB) << "Unable to start xHandControlBoard periodic thread"; ;
145 return false;
146 }
147
148 // Give some time to the hand to be ready
149 double delay = 1.0;
150 yCInfo(CB) << "Waiting " << delay << " s for the hand to be ready...";
152
153 m_ppids = new yarp::dev::Pid[EV_HAND_JOINT_NUM];
154 m_vpids = new yarp::dev::Pid[EV_HAND_JOINT_NUM];
155 m_cpids = new yarp::dev::Pid[EV_HAND_JOINT_NUM];
156 m_tpids = new yarp::dev::Pid[EV_HAND_JOINT_NUM];
157 m_controlModes = new int [EV_HAND_JOINT_NUM];
158
159 // Initialize hand command structure
160 memset(&m_handCommand, 0, sizeof(m_handCommand));
161 for(uint16_t i = 0; i < EV_HAND_JOINT_NUM; i++){
162 m_handCommand.finger_command[i].id = i;
163 double enc = 0.0;
164 getEncoder(i, &enc);
165 m_handCommand.finger_command[i].position = static_cast<float>(deg2rad(enc));
166 m_handCommand.finger_command[i].kp = m_PID_kp;
167 m_handCommand.finger_command[i].ki = m_PID_ki;
168 m_handCommand.finger_command[i].kd = m_PID_kd;
169 m_handCommand.finger_command[i].mode = m_MODE_mode;
170 m_handCommand.finger_command[i].tor_max = m_TORQUE_tor_max;
171 }
172
173 sendCommandToHand(m_id, m_handCommand);
174
175 for (size_t i=0; i< EV_HAND_JOINT_NUM; i++)setControlMode(i,VOCAB_CM_POSITION);
176 yarp::dev::Pid positionPidInit(m_PID_kp, m_PID_kd, m_PID_ki, 0.0, 1.0, 1.0);
177 std::vector<yarp::dev::Pid> positionPids(EV_HAND_JOINT_NUM, positionPidInit);
178 setPids(VOCAB_PIDTYPE_POSITION, positionPids.data());
179
180 return true;
181}
182
184{
185 if (this->isRunning()){
186 std::lock_guard lock(m_mutex);
188 }
189
190 m_XHCtrl.close_device();
191
192 delete [] m_ppids;
193 delete [] m_vpids;
194 delete [] m_cpids;
195 delete [] m_tpids;
196 delete[] m_controlModes;
197
198 return true;
199}
200
201void yarp::dev::xHandControlBoard::printErrorStruct(const xhand_control::ErrorStruct& err){
202 yCError(CB) << " Read state failed:"
203 << "\n error_code:" << err.error_code
204 << "\n error_message:" << err.error_message
205 << "\n error_type: " << err.error_details.error_type
206 << "\n error_name: " << err.error_details.error_name;
207}
208
210{
211 switch (pidtype)
212 {
214 m_ppids[j] = p;
215 m_handCommand.finger_command[j].kp = p.kp;
216 m_handCommand.finger_command[j].ki = p.ki;
217 m_handCommand.finger_command[j].kd = p.kd;
218 sendCommandToHand(m_id, m_handCommand);
219 break;
221 m_vpids[j] = p;
222 break;
224 m_cpids[j] = p;
225 break;
227 m_tpids[j] = p;
228 break;
229 default:
230 return false;
231 break;
232 }
233 return true;
234}
235
237{
238 bool ret = true;
239 switch (pidtype)
240 {
242 for (int j = 0; j < EV_HAND_JOINT_NUM; j++){ ret &= setPid(pidtype, j, ps[j]); }
243 break;
245 for (int j = 0; j < EV_HAND_JOINT_NUM; j++){ ret &= setPid(pidtype, j, ps[j]); }
246 break;
248 for (int j = 0; j < EV_HAND_JOINT_NUM; j++){ ret &= setPid(pidtype, j, ps[j]); }
249 break;
251 for (int j = 0; j < EV_HAND_JOINT_NUM; j++){ ret &= setPid(pidtype, j, ps[j]); }
252 break;
253 default:
254 ret = false;
255 break;
256 }
257 return ret;
258}
259
261{
262 return false;
263}
264
266{
267 return false;
268}
269
271{
272 return false;
273}
274
276{
277 return false;
278}
279
281{
282 return false;
283}
284
286{
287 return false;
288}
289
291{
292 return false;
293}
294
296{
297 return false;
298}
299
301{
302 return false;
303}
304
306{
307 switch (pidtype)
308 {
310 *p=m_ppids[j];
311 break;
313 *p=m_vpids[j];
314 break;
316 *p=m_cpids[j];
317 break;
319 *p=m_tpids[j];
320 break;
321 default:
322 return false;
323 break;
324 }
325 return true;
326}
327
329{
330 bool ret = true;
331 switch (pidtype)
332 {
334 for (int j = 0; j < EV_HAND_JOINT_NUM; j++){ ret &=getPid(pidtype, j, &pids[j]); }
335 break;
337 for (int j = 0; j < EV_HAND_JOINT_NUM; j++){ ret &=getPid(pidtype, j, &pids[j]); }
338 break;
340 for (int j = 0; j < EV_HAND_JOINT_NUM; j++){ ret &=getPid(pidtype, j, &pids[j]); }
341 break;
343 for (int j = 0; j < EV_HAND_JOINT_NUM; j++){ ret &=getPid(pidtype, j, &pids[j]); }
344 break;
345 default:
346 ret = false;
347 break;
348 }
349 return ret;
350}
351
353{
354 return false;
355}
356
358{
359 return false;
360}
361
363{
364 return false;
365}
366
368{
369 return false;
370}
371
373{
374 return false;
375}
376
378{
379 return false;
380}
381
383{
384 return false;
385}
386
388{
389 return false;
390}
391
393{
394 *ax = EV_HAND_JOINT_NUM;
395 return true;
396}
397
399{
400 return setPosition(j, ref);
401}
402
404{
405 return setPositions(refs);
406}
407
408bool yarp::dev::xHandControlBoard::positionMove(const int n_joints, const int* joints, const double* refs)
409{
410 return setPositions(n_joints, joints, refs);
411}
412
413bool yarp::dev::xHandControlBoard::getTargetPosition(const int joint, double* ref)
414{
415 std::lock_guard lock(m_mutex);
416 *ref = rad2deg(static_cast<double>(m_handState.state.finger_state[joint].position));
417 return true;
418}
419
421{
422 std::lock_guard lock(m_mutex);
423 for(uint16_t i = 0; i < EV_HAND_JOINT_NUM; i++){refs[i] = rad2deg(static_cast<double>(m_handCommand.finger_command[i].position));}
424 return true;
425}
426
427bool yarp::dev::xHandControlBoard::getTargetPositions(const int n_joint, const int* joints, double* refs)
428{
429 std::lock_guard lock(m_mutex);
430 for(uint16_t i = 0; i < n_joint; i++){refs[i] = rad2deg(static_cast<double>(m_handCommand.finger_command[joints[i]].position));}
431 return true;
432}
433
435{
436 return false;
437}
438
440{
441 return false;
442}
443
444bool yarp::dev::xHandControlBoard::relativeMove(const int n_joints, const int* joints, const double* deltas)
445{
446 return false;
447}
448
450{
451 yCDebugOnce(CB) << "checkMotionDone: not implemented yet.";
452 return true;
453}
454
456{
457 yCDebugOnce(CB) << "checkMotionDone: not implemented yet.";
458 return true;
459}
460
461bool yarp::dev::xHandControlBoard::checkMotionDone(const int n_joints, const int* joints, bool* flags)
462{
463 yCDebugOnce(CB) << "checkMotionDone: not implemented yet.";
464 return true;
465}
466
468{
469 return false;
470}
471
473{
474 return false;
475}
476
477bool yarp::dev::xHandControlBoard::setRefSpeeds(const int n_joints, const int* joints, const double* spds)
478{
479 return false;
480}
481
483{
484 return false;
485}
486
488{
489 return false;
490}
491
492bool yarp::dev::xHandControlBoard::setRefAccelerations(const int n_joints, const int* joints, const double* accs)
493{
494 return false;
495}
496
498{
499 yCDebugOnce(CB) << "getRefSpeed: not implemented yet.";
500 *ref=0.0;
501 return true;
502}
503
505{
506 return false;
507}
508
509bool yarp::dev::xHandControlBoard::getRefSpeeds(const int n_joints, const int* joints, double* spds)
510{
511 return false;
512}
513
515{
516 return false;
517}
518
520{
521 return false;
522}
523
524bool yarp::dev::xHandControlBoard::getRefAccelerations(const int n_joints, const int* joints, double* accs)
525{
526 return false;
527}
528
530{
531 return false;
532}
533
535{
536 return false;
537}
538
539bool yarp::dev::xHandControlBoard::stop(const int n_joints, const int* joints)
540{
541 return false;
542}
543
544bool yarp::dev::xHandControlBoard::getLastJointFault(int j, int& fault, std::string& message)
545{
546 return false;
547}
548
550{
551 return false;
552}
553
555{
556 return false;
557}
558
560{
561 return false;
562}
563
565{
566 return false;
567}
568
570{
571 return false;
572}
573
575{
576 return false;
577}
578
580{
581 std::lock_guard lock(m_mutex);
582 *v = rad2deg(static_cast<double>(m_handState.state.finger_state[j].position));
583 return true;
584}
585
587{
588 std::lock_guard lock(m_mutex);
589 for(size_t f=0; f< EV_HAND_JOINT_NUM; f++){encs[f] = rad2deg(static_cast<double>(m_handState.state.finger_state[f].position));}
590 return true;
591}
592
594{
595 std::lock_guard lock(m_mutex);
596 for(size_t i=0; i< EV_HAND_JOINT_NUM; i++){
597 encs[i] = rad2deg(static_cast<double>(m_handState.state.finger_state[i].position));
598 t[i] = m_handState.timestamp;
599 }
600 return true;
601}
602
603bool yarp::dev::xHandControlBoard::getEncoderTimed(int j, double* v, double* t)
604{
605 return false;
606}
607
609{
610 return false;
611}
612
614{
615 return false;
616}
617
619{
620 return false;
621}
622
624{
625 return false;
626}
627
629{
630 return false;
631}
632
634{
635 return false;
636}
637
639{
640 return false;
641}
642
644{
645 return false;
646}
647
649{
650 return false;
651}
652
654{
655 return false;
656}
657
659{
660 return false;
661}
662
664{
665 return false;
666}
667
669{
670 return false;
671}
672
674{
675 return false;
676}
677
679{
680 return false;
681}
682
684{
685 return false;
686}
687
689{
690 return false;
691}
692
694{
695 return false;
696}
697
699{
700 return false;
701}
702
704{
705 return false;
706}
707
709{
710 return false;
711}
712
714{
715 return false;
716}
717
719{
720 return false;
721}
722
724{
725 return false;
726}
727
729{
730 return false;
731}
732
734{
735 return false;
736}
737
739{
740 return false;
741}
742
744{
745 return false;
746}
747
749{
750 return false;
751}
752
754{
755 return false;
756}
757
759{
760 return false;
761}
762
763bool yarp::dev::xHandControlBoard::setPWMLimit(int m, const double val)
764{
765 return false;
766}
767
769{
770 return false;
771}
772
773bool yarp::dev::xHandControlBoard::setLimits(int j, double min, double max)
774{
775 m_LIMITS_jntPosMin[j] = min;
776 m_LIMITS_jntPosMax[j] = max;
777 return true;
778}
779
780bool yarp::dev::xHandControlBoard::getLimits(int j, double* min, double* max)
781{
782 *min = m_LIMITS_jntPosMin[j];
783 *max = m_LIMITS_jntPosMax[j];
784 return true;
785}
786
787bool yarp::dev::xHandControlBoard::setVelLimits(int j, double min, double max)
788{
789 m_LIMITS_jntVelMin[j] = min;
790 m_LIMITS_jntVelMax[j] = max;
791 return true;
792}
793
794bool yarp::dev::xHandControlBoard::getVelLimits(int j, double* min, double* max)
795{
796 *min = m_LIMITS_jntVelMin[j];
797 *max = m_LIMITS_jntVelMax[j];
798 return true;
799}
800
802{
803 return false;
804}
805
807{
808 return false;
809}
810
812{
813 return false;
814}
815
817{
818 return false;
819}
820
825
827{
828 return false;
829}
830
832{
833 return false;
834}
835
837{
838 return false;
839}
840
842{
843 return false;
844}
845
847{
848 return false;
849}
850
852{
853 return false;
854}
855
857{
858 return false;
859}
860
862{
863 return false;
864}
865
866bool yarp::dev::xHandControlBoard::calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3)
867{
868 return false;
869}
870
875
877{
878 return false;
879}
880
882{
883 return false;
884}
885
887{
888 return false;
889}
890
892{
893 return false;
894}
895
897{
898 return false;
899}
900
902{
903 return false;
904}
905
907{
908 return false;
909}
910
912{
913 return false;
914}
915
917{
918 return false;
919}
920
922{
923 return false;
924}
925
926bool yarp::dev::xHandControlBoard::getAxisName(int j, std::string& name)
927{
928 name = m_GENERAL_AxisName[j];
929 return true;
930}
931
933{
934 if(m_GENERAL_AxisType[j] == "revolute"){
936 return true;
937 }
938 else if(m_GENERAL_AxisType[j] == "prismatic"){
940 return true;
941 }
942 else{
943 return false;
944 }
945}
946
948{
949 return false;
950}
951
953{
954 yCDebugOnce(CB) << "getRefTorque: not implemented yet.";
955 *t = 0.0;
956 return true;
957}
958
960{
961 return false;
962}
963
965{
966 return false;
967}
968
969bool yarp::dev::xHandControlBoard::setRefTorques(const int n_joint, const int* joints, const double* t)
970{
971 return false;
972}
973
978
983
984bool yarp::dev::xHandControlBoard::setImpedance(int j, double stiff, double damp)
985{
986 return false;
987}
988
990{
991 return false;
992}
993
995{
996 return false;
997}
998
1000{
1001 return false;
1002}
1003
1004bool yarp::dev::xHandControlBoard::getTorqueRange(int j, double* min, double* max)
1005{
1006 return false;
1007}
1008
1010{
1011 return false;
1012}
1013
1014bool yarp::dev::xHandControlBoard::getImpedance(int j, double* stiff, double* damp)
1015{
1016 return false;
1017}
1018
1020{
1021 return false;
1022}
1023
1024bool yarp::dev::xHandControlBoard::getCurrentImpedanceLimit(int j, double* min_stiff, double* max_stiff, double* min_damp, double* max_damp)
1025{
1026 return false;
1027}
1028
1030{
1031 *mode = m_controlModes[j];
1032 return true;
1033}
1034
1036{
1037 bool ret = true;
1038 for(int j=0; j< EV_HAND_JOINT_NUM; j++){ret = ret && getControlMode(j, &modes[j]);}
1039 return ret;
1040}
1041
1042bool yarp::dev::xHandControlBoard::getControlModes(const int n_joint, const int* joints, int* modes)
1043{
1044 bool ret = true;
1045 for(int j=0; j< n_joint; j++){ret = ret && getControlMode(joints[j], &modes[j]);}
1046 return ret;
1047}
1048
1049bool yarp::dev::xHandControlBoard::setControlMode(const int j, const int mode)
1050{
1051 if (yarpToXhandControlModes.find(mode) == yarpToXhandControlModes.end())
1052 {
1053 yCError(CB) << "Control mode" << mode << "not supported. Supported modes are: VOCAB_CM_IDLE, VOCAB_CM_POSITION and VOCAB_CM_POSITION_DIRECT.";
1054 return false;
1055 }
1056
1058 {
1059 m_handCommand.finger_command[j].position = m_handState.state.finger_state[j].position;
1060 }
1061
1062 m_controlModes[j] = mode;
1063 m_handCommand.finger_command[j].mode = m_MODE_mode = yarpToXhandControlModes[mode];
1064 sendCommandToHand(m_id, m_handCommand);
1065
1066 return true;
1067
1068}
1069
1070bool yarp::dev::xHandControlBoard::setControlModes(const int n_joints, const int* joints, int* modes)
1071{
1072 bool ret = true;
1073 for(int i=0; i<n_joints; i++) {ret &= setControlMode(joints[i], modes[i]);}
1074 return ret;
1075}
1076
1078{
1079 bool ret = true;
1080 for (int j = 0; j < EV_HAND_JOINT_NUM; j++){ ret &= setControlMode(j, modes[j]); }
1081
1082 return ret;
1083}
1084
1086{
1087 m_handCommand.finger_command[j].position = deg2rad(ref);
1088 return sendCommandToHand(m_id, m_handCommand);
1089}
1090
1091bool yarp::dev::xHandControlBoard::setPositions(const int n_joints, const int* joints, const double* dpos)
1092{
1093 for(uint16_t i = 0; i < n_joints; i++){m_handCommand.finger_command[joints[i]].position = deg2rad(dpos[i]);}
1094 return sendCommandToHand(m_id, m_handCommand);
1095}
1096
1098{
1099 for(uint16_t i = 0; i < EV_HAND_JOINT_NUM; i++){m_handCommand.finger_command[i].position = deg2rad(refs[i]);}
1100 return sendCommandToHand(m_id, m_handCommand);
1101}
1102
1103bool yarp::dev::xHandControlBoard::getRefPosition(const int joint, double* ref)
1104{
1105 return false;
1106}
1107
1109{
1110 return false;
1111}
1112
1113bool yarp::dev::xHandControlBoard::getRefPositions(const int n_joint, const int* joints, double* refs)
1114{
1115 return false;
1116}
1117
1122
1123bool yarp::dev::xHandControlBoard::velocityMove(const int n_joints, const int* joints, const double* spds)
1124{
1125 return false;
1126}
1127
1128bool yarp::dev::xHandControlBoard::getRefVelocity(const int joint, double* vel)
1129{
1130 yCDebugOnce(CB) << "getRefVelocity: not implemented yet.";
1131 *vel=0.0;
1132 return true;
1133}
1134
1136{
1137 return false;
1138}
1139
1140bool yarp::dev::xHandControlBoard::getRefVelocities(const int n_joint, const int* joints, double* vels)
1141{
1142 return false;
1143}
1144
1149
1151{
1152 return false;
1153}
1154
1159
1164
1166{
1167 return false;
1168}
1169
1174
1176{
1177 return false;
1178}
1179
1181{
1182 return false;
1183}
1184
1186{
1187 return false;
1188}
1189
1191{
1192 return false;
1193}
1194
1196{
1197 return false;
1198}
1199
1201{
1202 return false;
1203}
1204
1206{
1207 return false;
1208}
1209
1211{
1212 return false;
1213}
1214
1215bool yarp::dev::xHandControlBoard::getCurrentRange(int m, double* min, double* max)
1216{
1217 return false;
1218}
1219
1221{
1222 return false;
1223}
1224
1226{
1227 return false;
1228}
1229
1231{
1232 return false;
1233}
1234
1235bool yarp::dev::xHandControlBoard::setRefCurrents(const int n_motor, const int* motors, const double* currs)
1236{
1237 return false;
1238}
1239
1241{
1242 return false;
1243}
1244
1246{
1247 return false;
1248}
1249
1251{
1252 std::pair<xhand_control::ErrorStruct, HandState_t> ret = m_XHCtrl.read_state(m_id, true);
1253
1254 if (!ret.first) {
1255 printErrorStruct(ret.first);
1256 yCError(CB) << "[run] See error(s) above.";
1257 }
1258
1259 std::lock_guard lock(m_mutex);
1260 m_handState.timestamp = yarp::os::Time::now();
1261 m_handState.state = ret.second;
1262}
1263
1265{
1266 return true;
1267}
1272
1273bool yarp::dev::xHandControlBoard::sendCommandToHand(const uint8_t hand_id, const HandCommand_t& command)
1274{
1275 auto err = m_XHCtrl.send_command(hand_id, command);
1276
1277 if (!err){
1278 printErrorStruct(err);
1279 yCError(CB) << "[sendCommandToHand] See error(s) above.";
1280 return false;
1281 }
1282
1283 return true;
1284}
#define yDebug(...)
Definition Log.h:275
IRemoteCalibrator interface is meant to remotize the access of the calibration device in order to all...
double ki
integrative gain
double kp
proportional gain
double kd
derivative gain
Contains the parameters for a PID.
bool setLimits(int j, double min, double max) override
bool getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters *params) override
Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
bool setMotorEncoders(const double *vals) override
Set the value of all motor encoders.
bool getPid(const yarp::dev::PidControlTypeEnum &pidtype, int j, yarp::dev::Pid *p) override
Get current PID value for a specific joint.
bool setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params) override
Set a subset of motor parameters (bemf, ktau etc) useful for torque control.
bool getRemoteVariable(std::string key, yarp::os::Bottle &val) override
bool setPeakCurrent(int m, const double val) override
bool setRefCurrent(int m, double curr) override
Set the reference value of the current for a single motor.
bool resetEncoders() override
Reset encoders.
bool resetMotorEncoder(int m) override
Reset motor encoder, single motor.
bool homingWholePart() override
homingWholePart: call the homing procedure for a the whole part/device
bool disableAmp(int j) override
Disable the amplifier on a specific joint.
bool getRefDutyCycle(int m, double *ref) override
Gets the last reference sent using the setRefDutyCycle function.
bool getRefAcceleration(int j, double *acc) override
bool resetMotorEncoders() override
Reset motor encoders.
bool getRefAccelerations(double *accs) override
bool setControlMode(const int j, const int mode) override
bool setPidErrorLimits(const yarp::dev::PidControlTypeEnum &pidtype, const double *limits) override
Get the error limit for the controller on all joints.
bool getEncodersTimed(double *encs, double *t) override
Read the instantaneous acceleration of all axes.
bool getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
bool velocityMove(int j, double v) override
Start motion at a given speed, single joint.
bool getPidErrorLimits(const yarp::dev::PidControlTypeEnum &pidtype, double *limits) override
Get the error limit for all controllers.
bool disablePid(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Disable the PID computation for a joint.
bool getMotorEncoderAcceleration(int m, double *acc) override
Read the instantaneous acceleration of a motor encoder.
bool setPWMLimit(int m, const double val) override
bool getNominalCurrent(int m, double *val) override
bool getPeakCurrent(int m, double *val) override
bool getPidReferences(const yarp::dev::PidControlTypeEnum &pidtype, double *refs) override
Get the current reference of all PID controllers.
bool getCurrentRange(int m, double *min, double *max) override
Get the full scale of the current measurement for a given motor (e.g.
bool getMotorEncoder(int m, double *v) override
Read the value of a motor encoder.
bool checkMotionDone(int j, bool *flag) override
bool setControlModes(const int n_joints, const int *joints, int *modes) override
bool getImpedanceOffset(int j, double *offset) override
Get current force Offset for a specific joint.
bool setMotorEncoder(int m, const double val) override
Set the value of the motor encoder for a given motor.
bool getTorqueRanges(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
bool positionMove(int j, double ref) override
Set new reference point for a single axis.
bool getNumberOfMotorEncoders(int *num) override
Get the number of available motor encoders.
bool getTorques(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
bool getTargetPositions(double *refs) override
Get the last position reference for all axes.
bool getDutyCycle(int m, double *val) override
Gets the current dutycycle of the output of the amplifier (i.e.
bool getRefVelocity(const int joint, double *vel) override
bool enableAmp(int j) override
Enable the amplifier on a specific joint.
bool getLastJointFault(int j, int &fault, std::string &message) override
bool setRefTorque(int j, double t) override
Set the reference value of the torque for a given joint.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool getEncoderAcceleration(int j, double *acc) override
Read the instantaneous acceleration of an axis.
bool setPid(const yarp::dev::PidControlTypeEnum &pidtype, int j, const yarp::dev::Pid &p) override
Set new PID value for a joint axis.
yarp::os::Stamp getLastInputStamp() override
Return the time stamp relative to the last acquisition.
bool getMotorEncoderCountsPerRevolution(int m, double *cpr) override
Gets number of counts per revolution for motor encoder m.
bool parkSingleJoint(int j, bool _wait=true) override
parkSingleJoint(): start the parking procedure for the single joint
bool getPWMLimit(int m, double *val) override
bool homingSingleJoint(int j) override
homingSingleJoint: call the homing procedure for a single joint
bool getMotorEncodersTimed(double *encs, double *t) override
Read the instantaneous position of all motor encoders.
bool getTemperatureLimit(int m, double *val) override
Retrieves the current temperature limit for a specific motor.
bool setPidReferences(const yarp::dev::PidControlTypeEnum &pidtype, const double *refs) override
Set the controller reference, multiple axes.
bool 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.
bool getRefCurrents(double *currs) override
Get the reference value of the currents for all motors.
bool setRefAccelerations(const double *accs) override
bool getPidErrorLimit(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *limit) override
Get the error limit for the controller on a specific joint.
bool getMotorEncoderSpeed(int m, double *sp) override
Read the instantaneous speed of a motor encoder.
bool getTorque(int j, double *t) override
Get the value of the torque on a given joint (this is the feedback if you have a torque sensor).
bool getMotorEncoders(double *encs) override
Read the position of all motor encoders.
bool setInteractionMode(int j, yarp::dev::InteractionModeEnum mode) override
Set the interaction mode of the robot, values can be stiff or compliant.
bool getTargetPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
bool getMotorEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all motor encoders.
bool setImpedanceOffset(int j, double offset) override
Set current force Offset for a specific joint.
bool getRefVelocities(double *vels) override
bool setPidErrorLimit(const yarp::dev::PidControlTypeEnum &pidtype, int j, double limit) override
Set the error limit for the controller on a specific joint.
bool setCalibrationParameters(int j, const yarp::dev::CalibrationParameters &params) override
Start calibration, this method is very often platform specific.
bool setRefDutyCycle(int m, double ref) override
Sets the reference dutycycle to a single motor.
bool setGearboxRatio(int m, const double val) override
Set the gearbox ratio for a specific motor.
bool stop() override
Stop motion, multiple joints.
bool getRefDutyCycles(double *refs) override
Gets the last reference sent using the setRefDutyCycles function.
bool calibrateWholePart() override
calibrateWholePart: call the procedure for calibrating the whole device
bool enablePid(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Enable the PID computation for a joint.
bool setPids(const yarp::dev::PidControlTypeEnum &pidtype, const yarp::dev::Pid *ps) override
Set new PID value on multiple axes.
bool isPidEnabled(const yarp::dev::PidControlTypeEnum &pidtype, int j, bool *enabled) override
bool setRefAcceleration(int j, double acc) override
bool setMotorEncoderCountsPerRevolution(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
bool getAmpStatus(int *st) override
bool getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
bool getControlMode(int j, int *mode) override
bool setImpedance(int j, double stiff, double damp) override
Set current impedance gains (stiffness,damping) for a specific joint.
bool getMotorEncoderTimed(int m, double *v, double *t) override
Read the instantaneous position of a motor encoder.
void threadRelease() override
Release method.
bool getRefPosition(const int joint, double *ref) override
Get the last position reference for the specified axis.
bool setRefCurrents(const double *currs) override
Set the reference value of the currents for all motors.
bool getCurrent(int m, double *curr) override
Get the instantaneous current measurement for a single motor.
bool relativeMove(int j, double delta) override
Set relative position.
bool getAxisName(int j, std::string &name) override
bool getNumberOfMotors(int *num) override
Retrieves the number of controlled motors from the current physical interface.
bool setRefDutyCycles(const double *refs) override
Sets the reference dutycycle for all the motors.
bool getCurrentRanges(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
bool calibrateSingleJoint(int j) override
calibrateSingleJoint: call the calibration procedure for the single joint
bool getPidErrors(const yarp::dev::PidControlTypeEnum &pidtype, double *errs) override
Get the error of all joints.
bool getEncoders(double *encs) override
Read the position of all axes.
bool getPidError(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *err) override
Get the current error for a joint.
bool getMotorEncoderSpeeds(double *spds) override
Read the instantaneous speed of all motor encoders.
bool calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) override
Start calibration, this method is very often platform specific.
void run() override
Loop function.
bool getRefTorques(double *refs) override
Get the reference value of the torque for all joints.
bool setVelLimits(int j, double min, double max) override
Set the software speed limits for a particular axis, the behavior of the control card when these limi...
bool getJointType(int j, yarp::dev::JointTypeEnum &type) override
yarp::dev::IRemoteCalibrator * getCalibratorDevice() override
getCalibratorDevice: return the pointer stored with the setCalibratorDevice
bool getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
bool getImpedance(int j, double *stiff, double *damp) override
Get current impedance gains (stiffness,damping,offset) for a specific joint.
bool getPidOutput(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *out) override
Get the output of the controller (e.g.
bool setPidOffset(const yarp::dev::PidControlTypeEnum &pidtype, int j, double v) override
Set an offset value for a given controller.
bool setInteractionModes(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
bool getTemperature(int m, double *val) override
Get temperature of a motor.
bool threadInit() override
Initialization method.
bool getRemoteVariablesList(yarp::os::Bottle *listOfKeys) override
bool quitCalibrate() override
quitCalibrate: interrupt the calibration procedure
bool getInteractionModes(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
bool resetPid(const yarp::dev::PidControlTypeEnum &pidtype, int j) override
Reset the controller of a given joint, usually sets the current status of the joint as the reference ...
bool getPids(const yarp::dev::PidControlTypeEnum &pidtype, yarp::dev::Pid *pids) override
Get current PID value for a specific joint.
bool setPosition(int j, double ref) override
Set new position for a single axis.
bool getPWM(int m, double *val) override
bool getRefSpeeds(double *spds) override
bool getPidReference(const yarp::dev::PidControlTypeEnum &pidtype, int j, double *ref) override
Get the current reference of the PID controller for a specific joint.
bool calibrationDone(int j) override
Check if the calibration is terminated, on a particular joint.
bool getPidOutputs(const yarp::dev::PidControlTypeEnum &pidtype, double *outs) override
Get the output of the controllers (e.g.
bool resetEncoder(int j) override
Reset encoder, single joint.
bool getRefPositions(double *refs) override
Get the last position reference for all axes.
bool getVelLimits(int j, double *min, double *max) override
Get the software speed limits for a particular axis.
bool setRefTorques(const double *t) override
Set the reference value of the torque for all joints.
bool getRefCurrent(int m, double *curr) override
Get the reference value of the current for a single motor.
bool getCurrents(double *currs) override
Get the instantaneous current measurement for all motors.
bool setNominalCurrent(int m, const double val) override
bool setPositions(const int n_joints, const int *joints, const double *dpos) override
Set new reference point for all axes.
bool getEncoderTimed(int j, double *v, double *t) override
Read the instantaneous acceleration of all axes.
bool getTemperatures(double *vals) override
Get temperature of all the motors.
bool getEncoder(int j, double *v) override
Read the value of an encoder.
bool getPowerSupplyVoltage(int m, double *val) override
bool getInteractionMode(int j, yarp::dev::InteractionModeEnum *mode) override
bool quitPark() override
quitPark: interrupt the park procedure
bool getTorqueRange(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
bool setRefSpeed(int j, double sp) override
bool setMaxCurrent(int j, double v) override
bool setPidReference(const yarp::dev::PidControlTypeEnum &pidtype, int j, double ref) override
Set the controller reference for a given axis.
bool getRefTorque(int j, double *t) override
Get the reference value of the torque for a given joint.
bool getRefSpeed(int j, double *ref) override
bool setTemperatureLimit(int m, const double val) override
Set the temperature limit for a specific motor.
bool getGearboxRatio(int m, double *val) override
Get the gearbox ratio for a specific motor.
bool setEncoders(const double *vals) override
Set the value of all encoders.
bool getMaxCurrent(int j, double *v) override
Returns the maximum electric current allowed for a given motor.
bool getDutyCycles(double *vals) override
Gets the current dutycycle of the output of the amplifier (i.e.
bool setRefSpeeds(const double *spds) override
bool close() override
Close the DeviceDriver.
bool isCalibratorDevicePresent(bool *isCalib) override
isCalibratorDevicePresent: check if a calibrator device has been set
bool setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
bool getControlModes(int *modes) override
bool parkWholePart() override
parkWholePart: start the parking procedure for the whole part
bool getLimits(int j, double *min, double *max) override
bool setRemoteVariable(std::string key, const yarp::os::Bottle &val) override
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:65
void askToStop()
Stop the thread.
bool start()
Call this to start the thread.
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
#define yCInfo(component,...)
#define yCError(component,...)
#define yCDebugOnce(component,...)
#define YARP_DECLARE_LOG_COMPONENT(name)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
PidControlTypeEnum
Definition PidEnums.h:15
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
void delay(double seconds)
Wait for a certain number of seconds.
Definition Time.cpp:111
The main, catch-all namespace for YARP.
Definition dirs.h:16
#define deg2rad(x)
const yarp::os::LogComponent & CB()
#define rad2deg(x)