21 #include <yarp/os/BufferedPort.h>
22 #include <yarp/os/Time.h>
23 #include <yarp/os/Network.h>
24 #include <yarp/os/PeriodicThread.h>
25 #include <yarp/os/Stamp.h>
26 #include <yarp/sig/Vector.h>
27 #include <yarp/dev/PolyDriver.h>
28 #include <yarp/dev/ControlBoardInterfaces.h>
34 #include <yarp/os/Log.h>
35 #include <yarp/os/LogStream.h>
37 using namespace yarp::os;
38 using namespace yarp::sig;
39 using namespace yarp::math;
46 Vector gravityCompensatorThread::evalVelUp(
const Vector &
x)
52 return linEstUp->estimate(el);
55 Vector gravityCompensatorThread::evalVelLow(
const Vector &
x)
61 return linEstLow->estimate(el);
64 Vector gravityCompensatorThread::evalAccUp(
const Vector &
x)
70 return quadEstUp->estimate(el);
73 Vector gravityCompensatorThread::evalAccLow(
const Vector &
x)
79 return quadEstLow->estimate(el);
82 void gravityCompensatorThread::init_upper()
88 encoders_arm_left.resize(jnt,0.0);
90 F_iDyn_LArm.resize(6,0.0);
91 Offset_LArm.resize(6,0.0);
93 dq_larm.resize(7,0.0);
94 d2q_larm.resize(7,0.0);
99 encoders_arm_right.resize(jnt,0.0);
100 F_RArm.resize(6,0.0);
101 F_iDyn_RArm.resize(6,0.0);
102 Offset_RArm.resize(6,0.0);
103 q_rarm.resize(7,0.0);
104 dq_rarm.resize(7,0.0);
105 d2q_rarm.resize(7,0.0);
110 encoders_head.resize(jnt,0.0);
111 q_head.resize(3,0.0);
112 dq_head.resize(3,0.0);
113 d2q_head.resize(3,0.0);
116 all_q_up.resize(allJnt,0.0);
117 all_dq_up.resize(allJnt,0.0);
118 all_d2q_up.resize(allJnt,0.0);
119 ampli_LA.resize(7);ampli_LA=1.0;
120 ampli_RA.resize(7);ampli_RA=1.0;
123 void gravityCompensatorThread::init_lower()
129 encoders_leg_left.resize(jnt,0.0);
130 q_lleg.resize(6,0.0);
131 dq_lleg.resize(6,0.0);
132 d2q_lleg.resize(6,0.0);
137 encoders_leg_right.resize(jnt,0.0);
138 q_rleg.resize(6,0.0);
139 dq_rleg.resize(6,0.0);
140 d2q_rleg.resize(6,0.0);
145 encoders_torso.resize(jnt,0.0);
146 q_torso.resize(3,0.0);
147 dq_torso.resize(3,0.0);
148 d2q_torso.resize(3,0.0);
151 all_q_low.resize(allJnt,0.0);
152 all_dq_low.resize(allJnt,0.0);
153 all_d2q_low.resize(allJnt,0.0);
154 ampli_TO.resize(3);ampli_TO=1.0;
155 ampli_LL.resize(6);ampli_LL=1.0;
156 ampli_RL.resize(6);ampli_RL=1.0;
160 void gravityCompensatorThread::setLowerMeasure()
162 icub->lowerTorso->setAng(
"torso",
CTRL_DEG2RAD * q_torso);
163 icub->lowerTorso->setDAng(
"torso",
CTRL_DEG2RAD * dq_torso);
164 icub->lowerTorso->setD2Ang(
"torso",
CTRL_DEG2RAD * d2q_torso);
166 icub->lowerTorso->setAng(
"left_leg",
CTRL_DEG2RAD * q_lleg);
167 icub->lowerTorso->setDAng(
"left_leg",
CTRL_DEG2RAD * dq_lleg);
168 icub->lowerTorso->setD2Ang(
"left_leg",
CTRL_DEG2RAD * d2q_lleg);
170 icub->lowerTorso->setAng(
"right_leg",
CTRL_DEG2RAD * q_rleg);
171 icub->lowerTorso->setDAng(
"right_leg",
CTRL_DEG2RAD * dq_rleg);
172 icub->lowerTorso->setD2Ang(
"right_leg",
CTRL_DEG2RAD * d2q_rleg);
175 void gravityCompensatorThread::setUpperMeasure()
178 icub->upperTorso->setAng(
"left_arm",
CTRL_DEG2RAD * q_larm);
179 icub->upperTorso->setAng(
"right_arm",
CTRL_DEG2RAD * q_rarm);
180 icub->upperTorso->setDAng(
"head",
CTRL_DEG2RAD * dq_head);
181 icub->upperTorso->setDAng(
"left_arm",
CTRL_DEG2RAD * dq_larm);
182 icub->upperTorso->setDAng(
"right_arm",
CTRL_DEG2RAD * dq_rarm);
183 icub->upperTorso->setD2Ang(
"head",
CTRL_DEG2RAD * d2q_head);
184 icub->upperTorso->setD2Ang(
"left_arm",
CTRL_DEG2RAD * d2q_larm);
185 icub->upperTorso->setD2Ang(
"right_arm",
CTRL_DEG2RAD * d2q_rarm);
186 icub->upperTorso->setInertialMeasure(w0,dw0,d2p0);
190 PolyDriver *_ddRA, PolyDriver *_ddH, PolyDriver *_ddLL,
191 PolyDriver *_ddRL, PolyDriver *_ddT,
version_tag icub_type,
192 bool _inertial_enabled) : PeriodicThread((double)_rate/1000.0), ddLA(_ddLA),
193 ddRA(_ddRA), ddLL(_ddLL), ddRL(_ddRL),
194 ddH(_ddH), ddT(_ddT), gravity_torques_LA(7,0.0),
195 gravity_torques_RA(7,0.0), exec_torques_LA(7,0.0),
196 exec_torques_RA(7,0.0), externalcmd_torques_LA(7,0.0),
197 externalcmd_torques_RA(7,0.0), gravity_torques_TO(3,0.0),
198 gravity_torques_LL(6,0.0), gravity_torques_RL(6,0.0),
199 exec_torques_TO(3,0.0), exec_torques_LL(6,0.0),
200 exec_torques_RL(6,0.0), externalcmd_torques_TO(3,0.0),
201 externalcmd_torques_LL(6,0.0), externalcmd_torques_RL(6,0.0)
205 wholeBodyName = std::move(_wholeBodyName);
208 iencs_arm_left =
nullptr;
209 iencs_arm_right =
nullptr;
210 iencs_head =
nullptr;
211 iCtrlMode_arm_left =
nullptr;
212 iCtrlMode_arm_right =
nullptr;
213 iCtrlMode_torso =
nullptr;
214 iIntMode_arm_left =
nullptr;
215 iIntMode_arm_right =
nullptr;
216 iIntMode_torso =
nullptr;
217 iImp_torso =
nullptr;
218 iTqs_torso =
nullptr;
219 iImp_arm_left =
nullptr;
220 iTqs_arm_left =
nullptr;
221 iImp_arm_right =
nullptr;
222 iTqs_arm_right =
nullptr;
223 iencs_leg_left =
nullptr;
224 iencs_leg_right =
nullptr;
225 iencs_torso =
nullptr;
226 iCtrlMode_leg_left =
nullptr;
227 iCtrlMode_leg_right =
nullptr;
228 iIntMode_leg_left =
nullptr;
229 iIntMode_leg_right =
nullptr;
230 iImp_leg_left =
nullptr;
231 iTqs_leg_left =
nullptr;
232 iImp_leg_right =
nullptr;
233 iTqs_leg_right =
nullptr;
234 isCalibrated =
false;
235 inertial_enabled=_inertial_enabled;
238 port_inertial =
nullptr;
239 la_additional_offset =
nullptr;
240 ra_additional_offset =
nullptr;
241 ll_additional_offset =
nullptr;
242 rl_additional_offset =
nullptr;
243 to_additional_offset =
nullptr;
244 left_arm_exec_torques =
nullptr;
245 right_arm_exec_torques =
nullptr;
246 left_leg_exec_torques =
nullptr;
247 right_leg_exec_torques =
nullptr;
248 torso_exec_torques =
nullptr;
249 left_arm_gravity_torques =
nullptr;
250 right_arm_gravity_torques =
nullptr;
251 left_leg_gravity_torques =
nullptr;
252 right_leg_gravity_torques =
nullptr;
253 torso_gravity_torques =
nullptr;
299 if (inertial_enabled)
301 inertial = port_inertial->read(waitMeasure);
302 if(inertial!=
nullptr)
304 sz = inertial->length();
305 inertial_measurements.resize(sz) ;
306 inertial_measurements= *inertial;
307 d2p0[0] = inertial_measurements[0];
308 d2p0[1] = inertial_measurements[1];
309 d2p0[2] = inertial_measurements[2];
347 {b &= iencs_leg_left->getEncoders(encoders_leg_left.data());}
349 {encoders_leg_left.zero();}
352 {b &= iencs_leg_right->getEncoders(encoders_leg_right.data());}
354 {encoders_leg_right.zero();}
357 {b &= iencs_torso->getEncoders(encoders_torso.data());}
359 {encoders_torso.zero();}
361 for (
size_t i=0;i<q_torso.length();i++)
363 q_torso(i) = encoders_torso(2-i);
364 all_q_low(i) = q_torso(i);
366 for (
size_t i=0;i<q_lleg.length();i++)
368 q_lleg(i) = encoders_leg_left(i);
369 all_q_low(q_torso.length()+i) = q_lleg(i);
371 for (
size_t i=0;i<q_rleg.length();i++)
373 q_rleg(i) = encoders_leg_right(i);
374 all_q_low(q_torso.length()+q_lleg.length()+i) = q_rleg(i);
407 {b &= iencs_arm_left->getEncoders(encoders_arm_left.data());}
409 {encoders_arm_left.zero();}
412 {b &= iencs_arm_right->getEncoders(encoders_arm_right.data());}
414 {encoders_arm_right.zero();}
417 {b &= iencs_head->getEncoders(encoders_head.data());}
419 {encoders_head.zero();}
421 for (
size_t i=0;i<q_head.length();i++)
423 q_head(i) = encoders_head(i);
424 all_q_up(i) = q_head(i);
426 for (
size_t i=0;i<q_larm.length();i++)
428 q_larm(i) = encoders_arm_left(i);
429 all_q_up(q_head.length()+i) = q_larm(i);
431 for (
size_t i=0;i<q_rarm.length();i++)
433 q_rarm(i) = encoders_arm_right(i);
434 all_q_up(q_head.length()+q_larm.length()+i) = q_rarm(i);
465 port_inertial=
new BufferedPort<Vector>;
466 if (!port_inertial->open(
"/gravityCompensator/inertial:i")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
468 la_additional_offset=
new BufferedPort<Vector>;
469 if (!la_additional_offset->open(
"/gravityCompensator/left_arm/ctrl_offset:i")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
470 ra_additional_offset=
new BufferedPort<Vector>;
471 if (!ra_additional_offset->open(
"/gravityCompensator/right_arm/ctrl_offset:i")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
472 ll_additional_offset=
new BufferedPort<Vector>;
473 if (!ll_additional_offset->open(
"/gravityCompensator/left_leg/ctrl_offset:i")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
474 rl_additional_offset=
new BufferedPort<Vector>;
475 if (!rl_additional_offset->open(
"/gravityCompensator/right_leg/ctrl_offset:i")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
476 to_additional_offset=
new BufferedPort<Vector>;
477 if (!to_additional_offset->open(
"/gravityCompensator/torso/ctrl_offset:i")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
479 left_arm_exec_torques =
new BufferedPort<Vector>;
480 if (!left_arm_exec_torques->open(
"/gravityCompensator/left_arm/exec_torques:o")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
481 right_arm_exec_torques =
new BufferedPort<Vector>;
482 if (!right_arm_exec_torques->open(
"/gravityCompensator/right_arm/exec_torques:o")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
483 left_leg_exec_torques =
new BufferedPort<Vector>;
484 if (!left_leg_exec_torques->open(
"/gravityCompensator/left_leg/exec_torques:o")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
485 right_leg_exec_torques =
new BufferedPort<Vector>;
486 if (!right_leg_exec_torques->open(
"/gravityCompensator/right_leg/exec_torques:o")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
487 torso_exec_torques =
new BufferedPort<Vector>;
488 if (!torso_exec_torques->open(
"/gravityCompensator/torso/exec_torques:o")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
490 left_arm_gravity_torques =
new BufferedPort<Vector>;
491 if (!left_arm_gravity_torques->open(
"/gravityCompensator/left_arm/gravity_torques:o")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
492 right_arm_gravity_torques =
new BufferedPort<Vector>;
493 if (!right_arm_gravity_torques->open(
"/gravityCompensator/right_arm/gravity_torques:o")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
494 left_leg_gravity_torques =
new BufferedPort<Vector>;
495 if (!left_leg_gravity_torques->open(
"/gravityCompensator/left_leg/gravity_torques:o")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
496 right_leg_gravity_torques =
new BufferedPort<Vector>;
497 if (!right_leg_gravity_torques->open(
"/gravityCompensator/right_leg/gravity_torques:o")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
498 torso_gravity_torques =
new BufferedPort<Vector>;
499 if (!torso_gravity_torques->open(
"/gravityCompensator/torso/gravity_torques:o")) {yError(
"Another gravityCompensator module is running? quitting");
return false;}
502 if (ddLA) ddLA->view(iencs_arm_left);
503 if (ddRA) ddRA->view(iencs_arm_right);
504 if (ddH) ddH->view(iencs_head);
505 if (ddLL) ddLL->view(iencs_leg_left);
506 if (ddRL) ddRL->view(iencs_leg_right);
507 if (ddT) ddT->view(iencs_torso);
509 if (ddLA) ddLA->view(iCtrlMode_arm_left);
510 if (ddRA) ddRA->view(iCtrlMode_arm_right);
511 if (ddLA) ddLA->view(iIntMode_arm_left);
512 if (ddRA) ddRA->view(iIntMode_arm_right);
513 if (ddLA) ddLA->view(iImp_arm_left);
514 if (ddLA) ddLA->view(iTqs_arm_left);
515 if (ddRA) ddRA->view(iImp_arm_right);
516 if (ddRA) ddRA->view(iTqs_arm_right);
518 if (ddT) ddT->view(iCtrlMode_torso);
519 if (ddT) ddT->view(iIntMode_torso);
520 if (ddT) ddT->view(iImp_torso);
521 if (ddT) ddT->view(iTqs_torso);
523 if (ddLL) ddLL->view(iCtrlMode_leg_left);
524 if (ddRL) ddRL->view(iCtrlMode_leg_right);
525 if (ddLL) ddLL->view(iIntMode_leg_left);
526 if (ddRL) ddRL->view(iIntMode_leg_right);
527 if (ddLL) ddLL->view(iImp_leg_left);
528 if (ddLL) ddLL->view(iTqs_leg_left);
529 if (ddRL) ddRL->view(iImp_leg_right);
530 if (ddRL) ddRL->view(iTqs_leg_right);
542 left_arm_ctrlJnt = 5;
543 right_arm_ctrlJnt = 5;
544 left_leg_ctrlJnt = 4;
545 right_leg_ctrlJnt = 4;
552 F_ext_up.resize(6,3);
554 F_ext_low.resize(6,3);
556 inertial_measurements.resize(12);
557 inertial_measurements.zero();
566 case VOCAB_CM_UNKNOWN: yError(
"UNKNOWN \n");
break;
578 if (iCtrlMode ==
nullptr)
579 {yError(
"ControlMode interface already closed, unable to reset compensation offset.\n");
return;}
581 {yError(
"TorqueControl interface already closed, unable to reset compensation offset.\n");
return;}
582 if (iIntMode ==
nullptr)
583 {yError(
"InteractionMode interface already closed, unable to reset compensation offset.\n");
return;}
585 {yError(
"Impedance interface already closed, unable to reset compensation offset.\n");
return;}
590 for(
int i=0;i<part_ctrlJnt;i++)
592 iImp->setImpedanceOffset(i,0.0);
593 iTqs->setRefTorque(i,0.0);
599 for(
int i=0;i<part_ctrlJnt;i++)
602 yarp::dev::InteractionModeEnum int_mode;
603 iCtrlMode->getControlMode(i,&ctrl_mode);
604 iIntMode->getInteractionMode(i,&int_mode);
608 case VOCAB_CM_CURRENT:
611 case VOCAB_CM_UNKNOWN:
612 case VOCAB_CM_HW_FAULT:
615 case VOCAB_CM_TORQUE:
616 iTqs->setRefTorque(i,command[i]);
619 case VOCAB_CM_POSITION:
620 case VOCAB_CM_POSITION_DIRECT:
622 case VOCAB_CM_VELOCITY:
623 if (int_mode == VOCAB_IM_COMPLIANT)
625 iImp->setImpedanceOffset(i,command[i]);
633 case VOCAB_CM_IMPEDANCE_POS:
634 case VOCAB_CM_IMPEDANCE_VEL:
635 iImp->setImpedanceOffset(i,command[i]);
639 if (s_part==
"torso" && i==3)
645 yError(
"Unknown control mode (part: %s jnt:%d).\n",s_part.c_str(), i);
655 static int delay_check=0;
661 yWarning (
"network delays detected (%d/10)\n", delay_check);
664 yError (
"gravityCompensatorThread lost connection with wholeBodyDynamics.\n");
687 Vector
tmp;
tmp.resize(3);
689 gravity_torques_TO[0] =
tmp [2];
690 gravity_torques_TO[1] =
tmp [1];
691 gravity_torques_TO[2] =
tmp [0];
700 yDebug (
"TORQUES: %s *** \n\n", torques_TO.toString().c_str());
701 yDebug (
"LL TORQUES: %s *** \n\n", torques_LL.toString().c_str());
702 yDebug (
"RL TORQUES: %s *** \n\n", torques_RL.toString().c_str());
706 Vector *offset_input_la = la_additional_offset->read(
false);
707 if(offset_input_la!=
nullptr)
709 auto size = (offset_input_la->size() < 7) ? offset_input_la->size():7;
710 for (
size_t i=0; i<size; i++)
711 {externalcmd_torques_LA[i]=(*offset_input_la)[i];}
713 Vector *offset_input_ra = ra_additional_offset->read(
false);
714 if(offset_input_ra!=
nullptr)
716 auto size = (offset_input_ra->size() < 7) ? offset_input_ra->size():7;
717 for (
size_t i=0; i<size; i++)
718 {externalcmd_torques_RA[i]=(*offset_input_ra)[i];}
720 Vector *offset_input_ll = ll_additional_offset->read(
false);
721 if(offset_input_ll!=
nullptr)
723 auto size = (offset_input_ll->size() < 6) ? offset_input_ll->size():6;
724 for (
size_t i=0; i<size; i++)
725 {externalcmd_torques_LL[i]=(*offset_input_ll)[i];}
727 Vector *offset_input_rl = rl_additional_offset->read(
false);
728 if(offset_input_rl!=
nullptr)
730 auto size = (offset_input_rl->size() < 6) ? offset_input_rl->size():6;
731 for (
size_t i=0; i<size; i++)
732 {externalcmd_torques_RL[i]=(*offset_input_rl)[i];}
734 Vector *offset_input_to = to_additional_offset->read(
false);
735 if(offset_input_to!=
nullptr)
737 auto size = (offset_input_to->size() < 3) ? offset_input_to->size():3;
738 for (
size_t i=0; i<size; i++)
739 {externalcmd_torques_TO[i]=(*offset_input_to)[i];}
747 exec_torques_LA = ampli_LA*gravity_torques_LA + externalcmd_torques_LA;
748 exec_torques_RA = ampli_RA*gravity_torques_RA + externalcmd_torques_RA;
749 exec_torques_LL = ampli_LL*gravity_torques_LL + externalcmd_torques_LL;
750 exec_torques_RL = ampli_RL*gravity_torques_RL + externalcmd_torques_RL;
751 exec_torques_TO = ampli_TO*gravity_torques_TO + externalcmd_torques_TO;
755 exec_torques_LA = ampli_LA*gravity_torques_LA;
756 exec_torques_RA = ampli_RA*gravity_torques_RA;
757 exec_torques_LL = ampli_LL*gravity_torques_LL;
758 exec_torques_RL = ampli_RL*gravity_torques_RL;
759 exec_torques_TO = ampli_TO*gravity_torques_TO;
766 exec_torques_LA = externalcmd_torques_LA;
767 exec_torques_RA = externalcmd_torques_RA;
768 exec_torques_LL = externalcmd_torques_LL;
769 exec_torques_RL = externalcmd_torques_RL;
770 exec_torques_TO = externalcmd_torques_TO;
774 externalcmd_torques_LA.zero();
775 externalcmd_torques_RA.zero();
776 externalcmd_torques_LL.zero();
777 externalcmd_torques_RL.zero();
778 externalcmd_torques_TO.zero();
783 static yarp::os::Stamp timestamp;
785 if (iCtrlMode_arm_left)
787 feedFwdGravityControl(left_arm_ctrlJnt,
"left_arm", iCtrlMode_arm_left,iTqs_arm_left,iImp_arm_left,iIntMode_arm_left,exec_torques_LA);
788 if (left_arm_exec_torques && left_arm_exec_torques->getOutputCount()>0)
790 left_arm_exec_torques->prepare() = exec_torques_LA;
791 left_arm_exec_torques->setEnvelope(timestamp);
792 left_arm_exec_torques->write();
794 if (left_arm_gravity_torques && left_arm_gravity_torques->getOutputCount()>0)
796 left_arm_gravity_torques->prepare() = gravity_torques_LA;
797 left_arm_gravity_torques->setEnvelope(timestamp);
798 left_arm_gravity_torques->write();
801 if (iCtrlMode_arm_right)
803 feedFwdGravityControl(right_arm_ctrlJnt,
"right_arm", iCtrlMode_arm_right,iTqs_arm_right,iImp_arm_right,iIntMode_arm_right,exec_torques_RA);
804 if (right_arm_exec_torques && right_arm_exec_torques->getOutputCount()>0)
806 right_arm_exec_torques->prepare() = exec_torques_RA;
807 right_arm_exec_torques->setEnvelope(timestamp);
808 right_arm_exec_torques->write();
810 if (right_arm_gravity_torques && right_arm_gravity_torques->getOutputCount()>0)
812 right_arm_gravity_torques->prepare() = gravity_torques_RA;
813 right_arm_gravity_torques->setEnvelope(timestamp);
814 right_arm_gravity_torques->write();
819 feedFwdGravityControl(torso_ctrlJnt,
"torso", iCtrlMode_torso,iTqs_torso,iImp_torso,iIntMode_torso,exec_torques_TO);
820 if (torso_exec_torques && torso_exec_torques->getOutputCount()>0)
822 torso_exec_torques->prepare() = exec_torques_TO;
823 torso_exec_torques->setEnvelope(timestamp);
824 torso_exec_torques->write();
826 if (torso_gravity_torques && torso_gravity_torques->getOutputCount()>0)
828 torso_gravity_torques->prepare() = gravity_torques_TO;
829 torso_gravity_torques->setEnvelope(timestamp);
830 torso_gravity_torques->write();
833 if (iCtrlMode_leg_left)
835 feedFwdGravityControl(left_leg_ctrlJnt,
"left_leg", iCtrlMode_leg_left,iTqs_leg_left,iImp_leg_left,iIntMode_leg_left,exec_torques_LL);
836 if (left_leg_exec_torques && left_leg_exec_torques->getOutputCount()>0)
838 left_leg_exec_torques->prepare() = exec_torques_LL;
839 left_leg_exec_torques->setEnvelope(timestamp);
840 left_leg_exec_torques->write();
842 if (left_leg_gravity_torques && left_leg_gravity_torques->getOutputCount()>0)
844 left_leg_gravity_torques->prepare() = gravity_torques_LL;
845 left_leg_gravity_torques->setEnvelope(timestamp);
846 left_leg_gravity_torques->write();
849 if (iCtrlMode_leg_right)
851 feedFwdGravityControl(right_leg_ctrlJnt,
"right_leg", iCtrlMode_leg_right,iTqs_leg_right,iImp_leg_right,iIntMode_leg_right,exec_torques_RL);
852 if (right_leg_exec_torques && right_leg_exec_torques->getOutputCount()>0)
854 right_leg_exec_torques->prepare() = exec_torques_RL;
855 right_leg_exec_torques->setEnvelope(timestamp);
856 right_leg_exec_torques->write();
858 if (right_leg_gravity_torques && right_leg_gravity_torques->getOutputCount()>0)
860 right_leg_gravity_torques->prepare() = gravity_torques_RL;
861 right_leg_gravity_torques->setEnvelope(timestamp);
862 right_leg_gravity_torques->write();
868 if(Network::exists(
"/"+wholeBodyName+
"/filtered/inertial:o"))
870 yInfo(
"connection exists! starting calibration...\n");
876 Network::connect(
"/"+wholeBodyName+
"/filtered/inertial:o",
"/gravityCompensator/inertial:i");
894 yDebug(
"encoders: %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf\n", encoders_arm_left(0), encoders_arm_left(1), encoders_arm_left(2), encoders_arm_left(3), encoders_arm_left(4), encoders_arm_left(5), encoders_arm_left(6));
895 yDebug(
"left arm: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_LA(0), gravity_torques_LA(1), gravity_torques_LA(2), gravity_torques_LA(3), gravity_torques_LA(4), gravity_torques_LA(5), gravity_torques_LA(6));
896 yDebug(
"right arm: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_RA(0), gravity_torques_RA(1), gravity_torques_RA(2), gravity_torques_RA(3), gravity_torques_RA(4), gravity_torques_RA(5), gravity_torques_RA(6));
897 yDebug(
"left leg: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_LL(0), gravity_torques_LL(1), gravity_torques_LL(2), gravity_torques_LL(3), gravity_torques_LL(4), gravity_torques_LL(5));
898 yDebug(
"right leg: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_RL(0), gravity_torques_RL(1), gravity_torques_RL(2), gravity_torques_RL(3), gravity_torques_RL(4), gravity_torques_RL(5));
899 yDebug(
"inertial: %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf\n", d2p0(0), d2p0(1), d2p0(2), w0(0), w0(1), w0(2), dw0(0), dw0(1), dw0(2));
903 yInfo(
"waiting for connections from wholeBodyDynamics (port: %s)...\n", wholeBodyName.c_str());
911 externalcmd_torques_LA.zero();
912 externalcmd_torques_RA.zero();
913 externalcmd_torques_LL.zero();
914 externalcmd_torques_RL.zero();
915 externalcmd_torques_TO.zero();
916 gravity_torques_LA.zero();
917 gravity_torques_RA.zero();
918 gravity_torques_LL.zero();
919 gravity_torques_RL.zero();
920 gravity_torques_TO.zero();
921 exec_torques_LA.zero();
922 exec_torques_RA.zero();
923 exec_torques_LL.zero();
924 exec_torques_RL.zero();
925 exec_torques_TO.zero();
927 if (iCtrlMode_arm_left)
929 yInfo(
"Setting gravity compensation offset to zero, left arm\n");
930 feedFwdGravityControl(left_arm_ctrlJnt,
"left_arm",iCtrlMode_arm_left,iTqs_arm_left,iImp_arm_left,iIntMode_arm_left,exec_torques_LA,
true);
932 if (iCtrlMode_arm_right)
934 yInfo(
"Setting gravity compensation offset to zero, right arm\n");
935 feedFwdGravityControl(right_arm_ctrlJnt,
"right_arm",iCtrlMode_arm_right,iTqs_arm_right,iImp_arm_right,iIntMode_arm_right,exec_torques_RA,
true);
937 if (iCtrlMode_leg_left)
939 yInfo(
"Setting gravity compensation offset to zero, left leg\n");
940 feedFwdGravityControl(left_leg_ctrlJnt,
"left_leg", iCtrlMode_leg_left,iTqs_leg_left,iImp_leg_left,iIntMode_leg_left,exec_torques_LL,
true);
942 if (iCtrlMode_leg_right)
944 yInfo(
"Setting gravity compensation offset to zero, right leg\n");
945 feedFwdGravityControl(right_leg_ctrlJnt,
"right_leg",iCtrlMode_leg_right,iTqs_leg_right,iImp_leg_right,iIntMode_leg_right,exec_torques_RL,
true);
949 yInfo(
"Setting gravity compensation offset to zero, torso\n");
950 feedFwdGravityControl(torso_ctrlJnt,
"torso",iCtrlMode_torso,iTqs_torso,iImp_torso,iIntMode_torso,exec_torques_TO,
true);
954 left_arm_exec_torques->interrupt();
955 right_arm_exec_torques->interrupt();
956 left_leg_exec_torques->interrupt();
957 right_leg_exec_torques->interrupt();
958 torso_exec_torques->interrupt();
959 left_arm_exec_torques->close();
960 right_arm_exec_torques->close();
961 left_leg_exec_torques->close();
962 right_leg_exec_torques->close();
963 torso_exec_torques->close();
965 left_arm_gravity_torques->interrupt();
966 right_arm_gravity_torques->interrupt();
967 left_leg_gravity_torques->interrupt();
968 right_leg_gravity_torques->interrupt();
969 torso_gravity_torques->interrupt();
970 left_arm_gravity_torques->close();
971 right_arm_gravity_torques->close();
972 left_leg_gravity_torques->close();
973 right_leg_gravity_torques->close();
974 torso_gravity_torques->close();
976 if (left_arm_exec_torques) {
delete left_arm_exec_torques; left_arm_exec_torques =
nullptr;}
977 if (right_arm_exec_torques) {
delete right_arm_exec_torques; right_arm_exec_torques =
nullptr;}
978 if (left_leg_exec_torques) {
delete left_leg_exec_torques; left_leg_exec_torques =
nullptr;}
979 if (right_leg_exec_torques) {
delete right_leg_exec_torques; right_leg_exec_torques =
nullptr;}
980 if (torso_exec_torques) {
delete torso_exec_torques; torso_exec_torques =
nullptr;}
982 if (left_arm_gravity_torques) {
delete left_arm_gravity_torques; left_arm_gravity_torques =
nullptr;}
983 if (right_arm_gravity_torques) {
delete right_arm_gravity_torques; right_arm_gravity_torques =
nullptr;}
984 if (left_leg_gravity_torques) {
delete left_leg_gravity_torques; left_leg_gravity_torques =
nullptr;}
985 if (right_leg_gravity_torques) {
delete right_leg_gravity_torques; right_leg_gravity_torques =
nullptr;}
986 if (torso_gravity_torques) {
delete torso_gravity_torques; torso_gravity_torques =
nullptr;}
988 if (linEstUp) {
delete linEstUp; linEstUp =
nullptr;}
989 if (quadEstUp) {
delete quadEstUp; quadEstUp =
nullptr;}
990 if (linEstLow) {
delete linEstLow; linEstLow =
nullptr;}
991 if (quadEstLow) {
delete quadEstLow; quadEstLow =
nullptr;}
994 port_inertial->interrupt();
995 port_inertial->close();
996 la_additional_offset->interrupt();
997 la_additional_offset->close();
998 ra_additional_offset->interrupt();
999 ra_additional_offset->close();
1000 ll_additional_offset->interrupt();
1001 ll_additional_offset->close();
1002 rl_additional_offset->interrupt();
1003 rl_additional_offset->close();
1004 to_additional_offset->interrupt();
1005 to_additional_offset->close();
1006 if (icub) {
delete icub; icub=
nullptr;}
bool getUpperEncodersSpeedAndAcceleration()
bool getLowerEncodersSpeedAndAcceleration()
bool readAndUpdate(bool waitMeasure=false)
gravityCompensatorThread(std::string _wholeBodyName, int _rate, PolyDriver *_ddLA, PolyDriver *_ddRA, PolyDriver *_ddH, PolyDriver *_ddLL, PolyDriver *_ddRL, PolyDriver *_ddT, version_tag icub_type, bool _inertial_enabled)
void feedFwdGravityControl(int part_ctrlJnt, const std::string &s_part, IControlMode *iCtrlMode, ITorqueControl *iTqs, IImpedanceControl *iImp, IInteractionMode *iIntMode, const Vector &command, bool releasing=false)
bool threadInit() override
void setZeroJntAngVelAcc()
void threadRelease() override
Adaptive window linear fitting to estimate the first derivative.
Basic element for adaptive polynomial fitting.
Adaptive window quadratic fitting to estimate the second derivative.
A class for connecting UpperTorso and LowerTorso of the iCub, then getting the WholeBody in the dynam...
iCubUpperTorso * upperTorso
pointer to UpperTorso = head + right arm + left arm
iCubLowerTorso * lowerTorso
pointer to LowerTorso = torso + right leg + left leg
void attachLowerTorso(const yarp::sig::Vector &FM_right_leg, const yarp::sig::Vector &FM_left_leg)
Connect upper and lower torso: this procedure handles the exchange of kinematic and wrench variables ...
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
bool solveKinematics(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Main function to manage the exchange of kinematic information among the limbs attached to the node.
virtual bool solveWrench()
Main function to manage the exchange of wrench information among the limbs attached to the node.
yarp::sig::Vector getTorques(const std::string &limbType)
Return the chosen limb torques, as a Nx1 vector.
yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false)
Redefinition from iDynSensorNode.
iDyn::iDynLimb * left
left limb
yarp::sig::Vector getTorsoAngAcc() const
Return the torso angular acceleration.
yarp::sig::Vector getTorsoLinAcc() const
Return the torso linear acceleration.
iDyn::iDynLimb * right
right limb
bool setInertialMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Set the inertial sensor measurements on the central-up limb.
yarp::sig::Vector getTorsoAngVel() const
Return the torso angular velocity.
@ GRAVITY_COMPENSATION_ON
@ GRAVITY_COMPENSATION_OFF