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;}