24 #include <yarp/os/all.h>
25 #include <yarp/sig/all.h>
26 #include <yarp/dev/all.h>
35 using namespace yarp::os;
36 using namespace yarp::sig;
37 using namespace yarp::math;
44 #define TEST_LEG_SENSOR
45 #define MEASURE_FROM_FOOT
50 cout<<
"Received an invalid joint index to filter"<<endl;
57 xv[1][j] = input / 1.157889499e+01;
59 yv[1][j] = (xv[0][j] + xv[1][j] ) + ( 0.8272719460 * yv[0][j] );
63 Vector inverseDynamics::evalVelUp(
const Vector &
x)
69 return linEstUp->estimate(el);
72 Vector inverseDynamics::evalVelLow(
const Vector &
x)
78 return linEstLow->estimate(el);
81 Vector inverseDynamics::eval_domega(
const Vector &
x)
87 return InertialEst->estimate(el);
90 Vector inverseDynamics::evalAccUp(
const Vector &
x)
96 return quadEstUp->estimate(el);
99 Vector inverseDynamics::evalAccLow(
const Vector &
x)
105 return quadEstLow->estimate(el);
108 void inverseDynamics::init_upper()
114 encoders_arm_left.resize(jnt,0.0);
115 F_LArm.resize(6,0.0);
116 F_iDyn_LArm.resize(6,0.0);
117 Offset_LArm.resize(6,0.0);
122 encoders_arm_right.resize(jnt,0.0);
123 F_RArm.resize(6,0.0);
124 F_iDyn_RArm.resize(6,0.0);
125 Offset_RArm.resize(6,0.0);
130 encoders_head.resize(jnt,0.0);
133 current_status.all_q_up.resize(allJnt,0.0);
134 current_status.all_dq_up.resize(allJnt,0.0);
135 current_status.all_d2q_up.resize(allJnt,0.0);
137 FM_sens_up.resize(6,2); FM_sens_up.zero();
140 void inverseDynamics::init_lower()
146 encoders_leg_left.resize(jnt,0.0);
147 F_LLeg.resize(6,0.0);
148 F_iDyn_LLeg.resize(6,0.0);
149 Offset_LLeg.resize(6,0.0);
150 F_LFoot.resize(6,0.0);
151 F_iDyn_LFoot.resize(6,0.0);
152 Offset_LFoot.resize(6,0.0);
157 encoders_leg_right.resize(jnt,0.0);
158 F_RLeg.resize(6,0.0);
159 F_iDyn_RLeg.resize(6,0.0);
160 Offset_RLeg.resize(6,0.0);
161 F_RFoot.resize(6,0.0);
162 F_iDyn_RFoot.resize(6,0.0);
163 Offset_RFoot.resize(6,0.0);
168 encoders_torso.resize(jnt,0.0);
171 current_status.all_q_low.resize(allJnt,0.0);
172 current_status.all_dq_low.resize(allJnt,0.0);
173 current_status.all_d2q_low.resize(allJnt,0.0);
176 FM_sens_low.resize(6,2); FM_sens_low.zero();
183 for (
int i=0; i<7; i++)
184 iint_arm_left->setInteractionMode(i,VOCAB_IM_STIFF);
188 for (
int i=0; i<7; i++)
189 iint_arm_right->setInteractionMode(i,VOCAB_IM_STIFF);
193 for (
int i=0; i<6; i++)
194 iint_leg_left->setInteractionMode(i,VOCAB_IM_STIFF);
198 for (
int i=0; i<6; i++)
199 iint_leg_right->setInteractionMode(i,VOCAB_IM_STIFF);
203 for (
int i=0; i<3; i++)
204 iint_torso->setInteractionMode(i,VOCAB_IM_STIFF);
209 for (
int i=0; i<7; i++)
211 int mode =0; icmd_arm_left->getControlMode(i, &mode);
212 if (mode==VOCAB_CM_TORQUE) icmd_arm_left->setControlMode(i, VOCAB_CM_POSITION);
218 for (
int i=0; i<7; i++)
220 int mode =0; icmd_arm_right->getControlMode(i, &mode);
221 if (mode==VOCAB_CM_TORQUE) icmd_arm_right->setControlMode(i, VOCAB_CM_POSITION);
227 for (
int i=0; i<6; i++)
229 int mode =0; icmd_leg_left->getControlMode(i, &mode);
230 if (mode==VOCAB_CM_TORQUE) icmd_leg_left->setControlMode(i, VOCAB_CM_POSITION);
236 for (
int i=0; i<6; i++)
238 int mode =0; icmd_leg_right->getControlMode(i, &mode);
239 if (mode==VOCAB_CM_TORQUE) icmd_leg_right->setControlMode(i, VOCAB_CM_POSITION);
245 for (
int i=0; i<3; i++)
247 int mode =0; icmd_torso->getControlMode(i, &mode);
248 if (mode==VOCAB_CM_TORQUE) icmd_torso->setControlMode(i, VOCAB_CM_POSITION);
253 inverseDynamics::inverseDynamics(
int _rate, PolyDriver *_ddAL, PolyDriver *_ddAR, PolyDriver *_ddH, PolyDriver *_ddLL, PolyDriver *_ddLR, PolyDriver *_ddT,
string _robot_name,
string _local_name,
version_tag _icub_type,
bool _autoconnect) : PeriodicThread((double)_rate/1000.0), ddAL(_ddAL), ddAR(_ddAR), ddH(_ddH), ddLL(_ddLL), ddLR(_ddLR), ddT(_ddT), robot_name(_robot_name), icub_type(_icub_type), local_name(_local_name), zero_sens_tolerance (1
e-12)
255 status_queue_size = 10;
256 autoconnect = _autoconnect;
268 skinContactsTimestamp = 0.0;
292 port_inertial_thread=
new BufferedPort<Vector>;
293 port_ft_arm_left=
new BufferedPort<Vector>;
294 port_ft_arm_right=
new BufferedPort<Vector>;
295 port_ft_leg_left=
new BufferedPort<Vector>;
296 port_ft_leg_right=
new BufferedPort<Vector>;
297 port_ft_foot_left=
new BufferedPort<Vector>;
298 port_ft_foot_right=
new BufferedPort<Vector>;
299 port_RATorques =
new BufferedPort<Bottle>;
300 port_LATorques =
new BufferedPort<Bottle>;
301 port_RLTorques =
new BufferedPort<Bottle>;
302 port_LLTorques =
new BufferedPort<Bottle>;
303 port_RWTorques =
new BufferedPort<Bottle>;
304 port_LWTorques =
new BufferedPort<Bottle>;
305 port_TOTorques =
new BufferedPort<Bottle>;
306 port_HDTorques =
new BufferedPort<Bottle>;
307 port_external_wrench_RA =
new BufferedPort<Vector>;
308 port_external_wrench_LA =
new BufferedPort<Vector>;
309 port_external_wrench_RL =
new BufferedPort<Vector>;
310 port_external_wrench_LL =
new BufferedPort<Vector>;
311 port_external_wrench_RF =
new BufferedPort<Vector>;
312 port_external_wrench_LF =
new BufferedPort<Vector>;
313 #ifdef TEST_LEG_SENSOR
314 port_sensor_wrench_RL =
new BufferedPort<Vector>;
315 port_sensor_wrench_LL =
new BufferedPort<Vector>;
316 port_model_wrench_RL =
new BufferedPort<Vector>;
317 port_model_wrench_LL =
new BufferedPort<Vector>;
319 port_external_wrench_TO =
new BufferedPort<Vector>;
320 port_external_cartesian_wrench_RA =
new BufferedPort<Vector>;
321 port_external_cartesian_wrench_LA =
new BufferedPort<Vector>;
322 port_external_cartesian_wrench_RL =
new BufferedPort<Vector>;
323 port_external_cartesian_wrench_LL =
new BufferedPort<Vector>;
324 port_external_cartesian_wrench_RF =
new BufferedPort<Vector>;
325 port_external_cartesian_wrench_LF =
new BufferedPort<Vector>;
326 port_skin_contacts =
new BufferedPort<skinContactList>;
327 port_com_all =
new BufferedPort<Vector>;
328 port_com_lb =
new BufferedPort<Vector>;
329 port_com_ub =
new BufferedPort<Vector>;
330 port_com_la =
new BufferedPort<Vector>;
331 port_com_ra =
new BufferedPort<Vector>;
332 port_com_ll =
new BufferedPort<Vector>;
333 port_com_rl =
new BufferedPort<Vector>;
334 port_com_to =
new BufferedPort<Vector>;
335 port_com_hd =
new BufferedPort<Vector>;
336 port_com_all_foot =
new BufferedPort<Vector>;
337 port_monitor =
new BufferedPort<Vector>;
338 port_contacts =
new BufferedPort<skinContactList>;
339 port_dumpvel =
new BufferedPort<Vector>;
340 port_external_ft_arm_left =
new BufferedPort<Vector>;
341 port_external_ft_arm_right =
new BufferedPort<Vector>;
342 port_external_ft_leg_left =
new BufferedPort<Vector>;
343 port_external_ft_leg_right =
new BufferedPort<Vector>;
344 port_COM_vel =
new BufferedPort<Vector>;
345 port_COM_Jacobian =
new BufferedPort<Matrix>;
346 port_all_velocities =
new BufferedPort<Vector>;
347 port_all_positions =
new BufferedPort<Vector>;
348 port_root_position_mat =
new BufferedPort<Matrix>;
349 port_root_position_vec =
new BufferedPort<Vector>;
351 port_inertial_thread->open(
string(
"/"+local_name+
"/inertial:i").c_str());
352 port_ft_arm_left->open(
string(
"/"+local_name+
"/left_arm/FT:i").c_str());
353 port_ft_arm_right->open(
string(
"/"+local_name+
"/right_arm/FT:i").c_str());
354 port_ft_leg_left->open(
string(
"/"+local_name+
"/left_leg/FT:i").c_str());
355 port_ft_leg_right->open(
string(
"/"+local_name+
"/right_leg/FT:i").c_str());
356 port_ft_foot_left->open(
string(
"/"+local_name+
"/left_foot/FT:i").c_str());
357 port_ft_foot_right->open(
string(
"/"+local_name+
"/right_foot/FT:i").c_str());
358 port_RATorques->open(
string(
"/"+local_name+
"/right_arm/Torques:o").c_str());
359 port_LATorques->open(
string(
"/"+local_name+
"/left_arm/Torques:o").c_str());
360 port_RLTorques->open(
string(
"/"+local_name+
"/right_leg/Torques:o").c_str());
361 port_LLTorques->open(
string(
"/"+local_name+
"/left_leg/Torques:o").c_str());
362 port_RWTorques->open(
string(
"/"+local_name+
"/right_wrist/Torques:o").c_str());
363 port_LWTorques->open(
string(
"/"+local_name+
"/left_wrist/Torques:o").c_str());
364 port_TOTorques->open(
string(
"/"+local_name+
"/torso/Torques:o").c_str());
365 port_HDTorques->open(
string(
"/"+local_name+
"/head/Torques:o").c_str());
366 port_external_wrench_RA->open(
string(
"/"+local_name+
"/right_arm/endEffectorWrench:o").c_str());
367 port_external_wrench_LA->open(
string(
"/"+local_name+
"/left_arm/endEffectorWrench:o").c_str());
368 port_external_wrench_RL->open(
string(
"/"+local_name+
"/right_leg/endEffectorWrench:o").c_str());
369 port_external_wrench_LL->open(
string(
"/"+local_name+
"/left_leg/endEffectorWrench:o").c_str());
370 port_external_wrench_RF->open(
string(
"/"+local_name+
"/right_foot/endEffectorWrench:o").c_str());
371 port_external_wrench_LF->open(
string(
"/"+local_name+
"/left_foot/endEffectorWrench:o").c_str());
372 #ifdef TEST_LEG_SENSOR
373 port_sensor_wrench_RL->open(
string(
"/"+local_name+
"/right_leg/sensorWrench:o").c_str());
374 port_sensor_wrench_LL->open(
string(
"/"+local_name+
"/left_leg/sensorWrench:o").c_str());
375 port_model_wrench_RL->open(
string(
"/"+local_name+
"/right_leg/modelWrench:o").c_str());
376 port_model_wrench_LL->open(
string(
"/"+local_name+
"/left_leg/modelWrench:o").c_str());
378 port_external_cartesian_wrench_RA->open(
string(
"/"+local_name+
"/right_arm/cartesianEndEffectorWrench:o").c_str());
379 port_external_cartesian_wrench_LA->open(
string(
"/"+local_name+
"/left_arm/cartesianEndEffectorWrench:o").c_str());
380 port_external_cartesian_wrench_RL->open(
string(
"/"+local_name+
"/right_leg/cartesianEndEffectorWrench:o").c_str());
381 port_external_cartesian_wrench_LL->open(
string(
"/"+local_name+
"/left_leg/cartesianEndEffectorWrench:o").c_str());
382 port_external_cartesian_wrench_RF->open(
string(
"/"+local_name+
"/right_foot/cartesianEndEffectorWrench:o").c_str());
383 port_external_cartesian_wrench_LF->open(
string(
"/"+local_name+
"/left_foot/cartesianEndEffectorWrench:o").c_str());
384 port_external_wrench_TO->open(
string(
"/"+local_name+
"/torso/Wrench:o").c_str());
385 port_com_all->open(
string(
"/"+local_name+
"/com:o").c_str());
386 port_com_lb->open (
string(
"/"+local_name+
"/lower_body/com:o").c_str());
387 port_com_ub->open (
string(
"/"+local_name+
"/upper_body/com:o").c_str());
388 port_com_la ->open(
string(
"/"+local_name+
"/left_arm/com:o").c_str());
389 port_com_ra ->open(
string(
"/"+local_name+
"/right_arm/com:o").c_str());
390 port_com_ll ->open(
string(
"/"+local_name+
"/left_leg/com:o").c_str());
391 port_com_rl ->open(
string(
"/"+local_name+
"/right_leg/com:o").c_str());
392 port_com_hd ->open(
string(
"/"+local_name+
"/head/com:o").c_str());
393 port_com_to ->open(
string(
"/"+local_name+
"/torso/com:o").c_str());
394 port_com_all_foot->open(
string(
"/"+local_name+
"/com_foot:o").c_str());
395 port_skin_contacts->open(
string(
"/"+local_name+
"/skin_contacts:i").c_str());
396 port_monitor->open(
string(
"/"+local_name+
"/monitor:o").c_str());
397 port_contacts->open(
string(
"/"+local_name+
"/contacts:o").c_str());
398 port_dumpvel->open(
string(
"/"+local_name+
"/va:o").c_str());
399 port_external_ft_arm_left->open(
string(
"/"+local_name+
"/left_arm/ext_ft_sens:o").c_str());
400 port_external_ft_arm_right->open(
string(
"/"+local_name+
"/right_arm/ext_ft_sens:o").c_str());
401 port_external_ft_leg_left->open(
string(
"/"+local_name+
"/left_leg/ext_ft_sens:o").c_str());
402 port_external_ft_leg_right->open(
string(
"/"+local_name+
"/right_leg/ext_ft_sens:o").c_str());
403 port_COM_vel->open(
string(
"/"+local_name+
"/com_vel:o").c_str());
404 port_COM_Jacobian->open(
string(
"/"+local_name+
"/com_jacobian:o").c_str());
405 port_all_velocities->open(
string(
"/"+local_name+
"/all_velocities:o").c_str());
406 port_all_positions->open(
string(
"/"+local_name+
"/all_positions:o").c_str());
407 port_root_position_mat->open(
string(
"/"+local_name+
"/root_position_mat:o").c_str());
408 port_root_position_vec->open(
string(
"/"+local_name+
"/root_position_vec:o").c_str());
410 yInfo (
"Waiting for port connections");
414 Network::connect(
string(
"/"+local_name+
"/filtered/inertial:o").c_str(),
string(
"/"+local_name+
"/inertial:i").c_str(),
"tcp",
false);
415 Network::connect(
string(
"/"+robot_name+
"/inertial").c_str(),
string(
"/"+local_name+
"/unfiltered/inertial:i").c_str(),
"tcp",
false);
416 Network::connect(
string(
"/"+robot_name+
"/left_arm/analog:o").c_str(),
string(
"/"+local_name+
"/left_arm/FT:i").c_str(),
"tcp",
false);
417 Network::connect(
string(
"/"+robot_name+
"/right_arm/analog:o").c_str(),
string(
"/"+local_name+
"/right_arm/FT:i").c_str(),
"tcp",
false);
418 Network::connect(
string(
"/"+robot_name+
"/left_leg/analog:o").c_str(),
string(
"/"+local_name+
"/left_leg/FT:i").c_str(),
"tcp",
false);
419 Network::connect(
string(
"/"+robot_name+
"/right_leg/analog:o").c_str(),
string(
"/"+local_name+
"/right_leg/FT:i").c_str(),
"tcp",
false);
420 Network::connect(
string(
"/"+robot_name+
"/left_foot/analog:o").c_str(),
string(
"/"+local_name+
"/left_foot/FT:i").c_str(),
"tcp",
false);
421 Network::connect(
string(
"/"+robot_name+
"/right_foot/analog:o").c_str(),
string(
"/"+local_name+
"/right_foot/FT:i").c_str(),
"tcp",
false);
423 Network::connect(
string(
"/"+local_name+
"/left_arm/Torques:o").c_str(),
string(
"/"+robot_name+
"/joint_vsens/left_arm:i").c_str(),
"tcp",
false);
424 Network::connect(
string(
"/"+local_name+
"/right_arm/Torques:o").c_str(),
string(
"/"+robot_name+
"/joint_vsens/right_arm:i").c_str(),
"tcp",
false);
425 Network::connect(
string(
"/"+local_name+
"/left_leg/Torques:o").c_str(),
string(
"/"+robot_name+
"/joint_vsens/left_leg:i").c_str(),
"tcp",
false);
426 Network::connect(
string(
"/"+local_name+
"/right_leg/Torques:o").c_str(),
string(
"/"+robot_name+
"/joint_vsens/right_leg:i").c_str(),
"tcp",
false);
427 Network::connect(
string(
"/"+local_name+
"/torso/Torques:o").c_str(),
string(
"/"+robot_name+
"/joint_vsens/torso:i").c_str(),
"tcp",
false);
429 if (Network::exists(
string(
"/"+robot_name+
"/joint_vsens/left_wrist:i").c_str()))
430 Network::connect(
string(
"/"+local_name+
"/left_wrist/Torques:o").c_str(),
string(
"/"+robot_name+
"/joint_vsens/left_wrist:i").c_str(),
"tcp",
false);
431 if (Network::exists(
string(
"/"+robot_name+
"/joint_vsens/right_wrist:i").c_str()))
432 Network::connect(
string(
"/"+local_name+
"/right_wrist/Torques:o").c_str(),
string(
"/"+robot_name+
"/joint_vsens/right_wrist:i").c_str(),
"tcp",
false);
434 yInfo (
"Ports connected");
437 if (ddAL) {ddAL->view(iencs_arm_left); ddAL->view(iint_arm_left); ddAL->view(icmd_arm_left);}
438 if (ddAR) {ddAR->view(iencs_arm_right); ddAR->view(iint_arm_right); ddAR->view(icmd_arm_right);}
439 if (ddH) {ddH->view(iencs_head); ddH ->view(iint_head); ddH ->view(icmd_head);}
440 if (ddLL) {ddLL->view(iencs_leg_left); ddLL->view(iint_leg_left); ddLL->view(icmd_leg_left);}
441 if (ddLR) {ddLR->view(iencs_leg_right); ddLR->view(iint_leg_right); ddLR->view(icmd_leg_right);}
442 if (ddT) {ddT->view(iencs_torso); ddT ->view(iint_torso); ddT ->view(icmd_torso);}
457 F_ext_up.resize(6,3);
459 F_ext_low.resize(6,3);
461 F_ext_left_arm.resize(6,0.0);
462 F_ext_right_arm.resize(6,0.0);
463 F_ext_cartesian_left_arm.resize(6,0.0);
464 F_ext_cartesian_right_arm.resize(6,0.0);
465 F_ext_left_leg.resize(6,0.0);
466 F_ext_right_leg.resize(6,0.0);
467 F_ext_cartesian_left_leg.resize(6,0.0);
468 F_ext_cartesian_right_leg.resize(6,0.0);
469 F_ext_left_foot.resize(6,0.0);
470 F_ext_right_foot.resize(6,0.0);
471 F_ext_cartesian_left_foot.resize(6,0.0);
472 F_ext_cartesian_right_foot.resize(6,0.0);
473 com_jac.resize(6,32);
479 yInfo(
"threadInit: waiting for port connections... \n\n");
482 Vector *dummy = port_inertial_thread->read(
true);
486 for(
size_t i=0; i<status_queue_size; i++)
492 yError(
"A problem occured during the initial readAndUpdate(), stopping... \n");
508 for (
size_t i=0; i<this->
all_dq_low.size(); i++)
514 for (
size_t i=0; i<this->
all_dq_up.size(); i++)
546 static int delay_check=0;
550 yWarning (
"network delays detected (%d/10)\n", delay_check);
553 yError (
"inverseDynamics thread lost connection with iCubInterface.\n");
563 F_LArm = -1.0 * (current_status.
ft_arm_left-Offset_LArm);
564 F_RArm = -1.0 * (current_status.
ft_arm_right-Offset_RArm);
565 F_LLeg = -1.0 * (current_status.
ft_leg_left-Offset_LLeg);
566 F_RLeg = -1.0 * (current_status.
ft_leg_right-Offset_RLeg);
567 F_LFoot = -1.0 * (current_status.
ft_foot_left-Offset_LFoot);
568 F_RFoot = -1.0 * (current_status.
ft_foot_right-Offset_RFoot);
576 not_moving_status.push_front(current_status);
577 if (not_moving_status.size()>status_queue_size)
579 not_moving_status.pop_back();
586 not_moving_status.clear();
603 #ifdef DEBUG_PERFORMANCE
604 static double meanTime = 0.0;
605 static double startTime = 0;
606 startTime = Time::now();
611 #ifdef DEBUG_PERFORMANCE
612 meanTime += Time::now()-startTime;
613 yDebug(
"Mean uppertorso NE time: %.4f\n", meanTime/getIterations());
617 #ifdef DEBUG_KINEMATICS
619 yDebug (
"\nHEAD: %s \n", d2p0.toString().c_str());
629 #ifdef DEBUG_KINEMATICS
631 yDebug (
"LOWTORSO->UP: %s *** %s *** %s\n",
634 cub->lowerTorso->up->getLinAcc(2).toString().c_str());
638 yDebug (
"LOWTORSO->RI: %s *** %s *** %s *** %s\n",
644 yDebug (
"LOWTORSO->LE: %s *** %s *** %s *** %s\n",
654 yDebug (
"LOWTORSO->UP: %s *** %s \n",
658 yDebug (
"LOWTORSO->RO: %s *** %s \n",
674 HDTorques[0] = HDtmp [0];
675 HDTorques[1] = HDtmp [1];
676 HDTorques[2] = HDtmp [2];
678 TOTorques[0] = TOtmp [2];
679 TOTorques[1] = TOtmp [1];
680 TOTorques[2] = TOtmp [0];
684 yDebug (
"TORQUES: %s *** \n\n", TOTorques.toString().c_str());
692 if (ddLR)
writeTorque(RLTorques, 2, port_RLTorques);
693 if (ddLL)
writeTorque(LLTorques, 2, port_LLTorques);
697 Vector com_all(7), com_ll(7), com_rl(7), com_la(7),com_ra(7), com_hd(7), com_to(7), com_lb(7), com_ub(7);
698 double mass_all , mass_ll , mass_rl , mass_la ,mass_ra , mass_hd, mass_to, mass_lb, mass_ub;
699 Vector com_v; com_v.resize(3); com_v.zero();
700 Vector all_dq; all_dq.resize(32,1); all_dq.zero();
701 Vector all_q; all_q.resize(32,1); all_q.zero();
704 yarp::sig::Vector com_all_foot; com_all_foot.resize(3); com_all_foot.zero();
705 yarp::sig::Matrix rTf; rTf.resize(4,4); rTf.zero();
706 yarp::sig::Matrix fTr; fTr.resize(4,4); fTr.zero();
707 yarp::sig::Matrix lastRotTrans; lastRotTrans.resize(4,4); lastRotTrans.zero();
708 lastRotTrans(2,0)=lastRotTrans(3,3)=lastRotTrans(0,2)=1;
709 lastRotTrans(1,1)=-1;
714 fTr.setSubmatrix(rTf.submatrix(0,2,0,2).transposed(), 0, 0);
715 fTr.setSubcol(-1*fTr.submatrix(0,2,0,2)*rTf.subcol(0,3,3), 0, 3);
719 lastRotTrans(1,3)=-rTf(1,3);
720 lastRotTrans(2,3)= -rTf(0,3);
744 com_lb.push_back(mass_lb);
745 com_ub.push_back(mass_ub);
746 com_all.push_back(mass_all);
747 com_ll.push_back (mass_ll);
748 com_rl.push_back (mass_rl);
749 com_la.push_back (mass_la);
750 com_ra.push_back (mass_ra);
751 com_hd.push_back (mass_hd);
752 com_to.push_back (mass_to);
756 com_all.push_back(com_v[0]);
757 com_all.push_back(com_v[1]);
758 com_all.push_back(com_v[2]);
762 com_all.push_back(0);
763 com_all.push_back(0);
764 com_all.push_back(0);
767 #ifdef MEASURE_FROM_FOOT
768 com_all_foot.setSubvector(0,com_all.subVector(0,2));
769 com_all_foot.push_back(1);
770 com_all_foot = fTr*com_all_foot;
776 mass_all=mass_ll=mass_rl=mass_la=mass_ra=mass_hd=mass_to=0.0;
777 com_all.zero(); com_ll.zero(); com_rl.zero(); com_la.zero(); com_ra.zero(); com_hd.zero(); com_to.zero();
783 dynContacts.insert(dynContacts.begin(), contactListR.begin(), contactListR.end());
786 bool contactFound=
false;
787 for(
unsigned int i=0; i<dynContacts.size(); i++)
789 cId = dynContacts[i].getId();
790 for(
unsigned int j=0; j<skinContacts.size(); j++)
792 if(cId == skinContacts[j].getId())
794 skinContacts[j].setForceMoment( dynContacts[i].getForceMoment() );
796 j = skinContacts.size();
801 skinContacts.push_back(
skinContact(dynContacts[i]));
802 contactFound =
false;
817 skinContacts.push_back(left_leg_contact);
818 skinContacts.push_back(right_leg_contact);
822 bool skin_lleg_found =
false;
823 bool skin_rleg_found =
false;
824 for(
unsigned int j=0; j<skinContacts.size(); j++)
828 skinContacts[j].setForceMoment(F_ext_left_leg);
829 skin_lleg_found =
true;
833 skinContacts[j].setForceMoment(F_ext_right_leg);
834 skin_rleg_found =
true;
837 if (!skin_rleg_found) {skinContacts.push_back(right_leg_contact);}
838 if (!skin_lleg_found) {skinContacts.push_back(left_leg_contact);}
842 F_ext_cartesian_left_arm = F_ext_cartesian_right_arm =
zeros(6);
843 F_ext_cartesian_left_leg = F_ext_cartesian_right_leg =
zeros(6);
851 F_ext_sens_right_arm = F_RArm - F_sensor_up.getCol(0);
852 F_ext_sens_left_arm = F_LArm - F_sensor_up.getCol(1);
854 #ifdef TEST_LEG_SENSOR
855 setUpperMeasure(
true);
856 setLowerMeasure(
true);
860 F_ext_sens_right_leg = F_RLeg - F_sensor_low.getCol(0);
861 F_ext_sens_left_leg = F_LLeg - F_sensor_low.getCol(1);
863 #ifdef TEST_LEG_SENSOR
864 F_mdl_right_leg = F_sensor_low.getCol(0);
865 F_mdl_left_leg = F_sensor_low.getCol(1);
866 F_sns_right_leg = F_RLeg;
867 F_sns_left_leg = F_LLeg;
876 yarp::sig::Vector tmp1,tmp2;
877 tmp1 = F_ext_left_arm.subVector(0,2); tmp1.push_back(0.0); tmp1 = ahl * tmp1;
878 tmp2 = F_ext_left_arm.subVector(3,5); tmp2.push_back(0.0); tmp2 = ahl * tmp2;
879 for (
int i=0; i<3; i++) F_ext_cartesian_left_arm[i] = tmp1[i];
880 for (
int i=3; i<6; i++) F_ext_cartesian_left_arm[i] = tmp2[i-3];
881 double n1=
norm(F_ext_cartesian_left_arm.subVector(0,2));
882 double n2=
norm(F_ext_cartesian_left_arm.subVector(3,5));
883 F_ext_cartesian_left_arm.push_back(n1);
884 F_ext_cartesian_left_arm.push_back(n2);
886 tmp1 = F_ext_right_arm.subVector(0,2); tmp1.push_back(0.0); tmp1 = ahr * tmp1;
887 tmp2 = F_ext_right_arm.subVector(3,5); tmp2.push_back(0.0); tmp2 = ahr * tmp2;
888 for (
int i=0; i<3; i++) F_ext_cartesian_right_arm[i] = tmp1[i];
889 for (
int i=3; i<6; i++) F_ext_cartesian_right_arm[i] = tmp2[i-3];
890 n1=
norm(F_ext_cartesian_right_arm.subVector(0,2));
891 n2=
norm(F_ext_cartesian_right_arm.subVector(3,5));
892 F_ext_cartesian_right_arm.push_back(n1);
893 F_ext_cartesian_right_arm.push_back(n2);
895 tmp1 = F_ext_left_leg.subVector(0,2); tmp1.push_back(0.0); tmp1 = lhl * tmp1;
896 tmp2 = F_ext_left_leg.subVector(3,5); tmp2.push_back(0.0); tmp2 = lhl * tmp2;
897 for (
int i=0; i<3; i++) F_ext_cartesian_left_leg[i] = tmp1[i];
898 for (
int i=3; i<6; i++) F_ext_cartesian_left_leg[i] = tmp2[i-3];
900 tmp1 = F_ext_right_leg.subVector(0,2); tmp1.push_back(0.0); tmp1 = lhr * tmp1;
901 tmp2 = F_ext_right_leg.subVector(3,5); tmp2.push_back(0.0); tmp2 = lhr * tmp2;
902 for (
int i=0; i<3; i++) F_ext_cartesian_right_leg[i] = tmp1[i];
903 for (
int i=3; i<6; i++) F_ext_cartesian_right_leg[i] = tmp2[i-3];
906 yarp::sig::Vector angles (3);
908 angles[1] = -90/180.0*
M_PI;
910 yarp::sig::Matrix r1 = yarp::math::euler2dcm(angles);
911 yarp::sig::Matrix ilhl = yarp::math::SE3inv(lhl);
912 yarp::sig::Matrix foot_root_mat = r1*ilhl;
914 yarp::sig::Vector foot_tmp = yarp::math::dcm2rpy(foot_root_mat);
916 yarp::sig::Vector foot_root_vec (6,0.0);
917 foot_root_vec[3] = foot_root_mat[0][3]*1000;
918 foot_root_vec[4] = foot_root_mat[1][3]*1000;
919 foot_root_vec[5] = foot_root_mat[2][3]*1000;
920 foot_root_vec[0] = foot_tmp[0]*180.0/
M_PI;
921 foot_root_vec[1] = foot_tmp[1]*180.0/
M_PI;
922 foot_root_vec[2] = foot_tmp[2]*180.0/
M_PI;
928 Matrix foot_hn(4,4); foot_hn.zero();
929 foot_hn(0,2)=1;foot_hn(0,3)=-7.75;
934 tmp1 = F_LFoot.subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
935 tmp2 = F_LFoot.subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;
936 for (
int i=0; i<3; i++) F_ext_left_foot[i] = tmp1[i];
937 for (
int i=3; i<6; i++) F_ext_left_foot[i] = tmp2[i-3];
939 tmp1 = F_RFoot.subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
940 tmp2 = F_RFoot.subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;
941 for (
int i=0; i<3; i++) F_ext_right_foot[i] = tmp1[i];
942 for (
int i=3; i<6; i++) F_ext_right_foot[i] = tmp2[i-3];
944 tmp1 = F_ext_left_foot.subVector(0,2); tmp1.push_back(0.0); tmp1 = lhl * tmp1;
945 tmp2 = F_ext_left_foot.subVector(3,5); tmp2.push_back(0.0); tmp2 = lhl * tmp2;
946 for (
int i=0; i<3; i++) F_ext_cartesian_left_foot[i] = tmp1[i];
947 for (
int i=3; i<6; i++) F_ext_cartesian_left_foot[i] = tmp2[i-3];
949 tmp1 = F_ext_right_foot.subVector(0,2); tmp1.push_back(0.0); tmp1 = lhr * tmp1;
950 tmp2 = F_ext_right_foot.subVector(3,5); tmp2.push_back(0.0); tmp2 = lhr * tmp2;
951 for (
int i=0; i<3; i++) F_ext_cartesian_right_foot[i] = tmp1[i];
952 for (
int i=3; i<6; i++) F_ext_cartesian_right_foot[i] = tmp2[i-3];
965 broadcastData<Vector> (com_v, port_COM_vel);
966 broadcastData<Vector> (all_dq, port_all_velocities);
967 broadcastData<Vector> (all_q, port_all_positions);
968 broadcastData<Matrix> (com_jac, port_COM_Jacobian);
970 broadcastData<Vector> (com_all, port_com_all);
971 broadcastData<Vector> (com_lb, port_com_lb);
972 broadcastData<Vector> (com_ub, port_com_ub);
973 broadcastData<Vector> (com_ll, port_com_ll);
974 broadcastData<Vector> (com_rl, port_com_rl);
975 broadcastData<Vector> (com_la, port_com_la);
976 broadcastData<Vector> (com_ra, port_com_ra);
977 broadcastData<Vector> (com_hd, port_com_hd);
978 broadcastData<Vector> (com_to, port_com_to);
979 broadcastData<Vector> (com_all_foot, port_com_all_foot);
981 broadcastData<Vector> (F_up, port_external_wrench_TO);
982 broadcastData<Vector> (F_ext_right_arm, port_external_wrench_RA);
983 broadcastData<Vector> (F_ext_left_arm, port_external_wrench_LA);
984 broadcastData<Vector> (F_ext_cartesian_right_arm, port_external_cartesian_wrench_RA);
985 broadcastData<Vector> (F_ext_cartesian_left_arm, port_external_cartesian_wrench_LA);
986 broadcastData<Vector> (F_ext_right_leg, port_external_wrench_RL);
987 broadcastData<Vector> (F_ext_left_leg, port_external_wrench_LL);
988 broadcastData<Vector> (F_ext_right_foot, port_external_wrench_RF);
989 broadcastData<Vector> (F_ext_left_foot, port_external_wrench_LF);
990 #ifdef TEST_LEG_SENSOR
991 broadcastData<Vector> (F_sns_right_leg, port_sensor_wrench_RL);
992 broadcastData<Vector> (F_sns_left_leg, port_sensor_wrench_LL);
993 broadcastData<Vector> (F_mdl_right_leg, port_model_wrench_RL);
994 broadcastData<Vector> (F_mdl_left_leg, port_model_wrench_LL);
996 broadcastData<Vector> (F_ext_cartesian_right_leg, port_external_cartesian_wrench_RL);
997 broadcastData<Vector> (F_ext_cartesian_left_leg, port_external_cartesian_wrench_LL);
998 broadcastData<Vector> (F_ext_cartesian_right_foot, port_external_cartesian_wrench_RF);
999 broadcastData<Vector> (F_ext_cartesian_left_foot, port_external_cartesian_wrench_LF);
1000 broadcastData<skinContactList>( skinContacts, port_contacts);
1001 broadcastData<Vector> (F_ext_sens_right_arm, port_external_ft_arm_right);
1002 broadcastData<Vector> (F_ext_sens_left_arm, port_external_ft_arm_left);
1003 broadcastData<Vector> (F_ext_sens_right_leg, port_external_ft_leg_right);
1004 broadcastData<Vector> (F_ext_sens_left_leg, port_external_ft_leg_left);
1006 broadcastData<Matrix> (foot_root_mat, port_root_position_mat);
1007 broadcastData<Vector> (foot_root_vec, port_root_position_vec);
1012 yInfo(
"Closing the linear estimator\n");
1023 yInfo(
"Closing the quadratic estimator\n");
1034 yInfo(
"Closing the inertial estimator\n");
1041 yInfo(
"Closing RATorques port\n");
1043 yInfo(
"Closing LATorques port\n");
1045 yInfo(
"Closing RLTorques port\n");
1047 yInfo(
"Closing LLTorques port\n");
1049 yInfo(
"Closing RWTorques port\n");
1051 yInfo(
"Closing LWTorques port\n");
1053 yInfo(
"Closing TOTorques port\n");
1055 yInfo(
"Closing HDTorques port\n");
1057 yInfo(
"Closing external_wrench_RA port\n");
1059 yInfo(
"Closing external_wrench_LA port\n");
1061 #ifdef TEST_LEG_SENSOR
1067 yInfo(
"Closing cartesian_external_wrench_RA port\n");
1068 closePort(port_external_cartesian_wrench_RA);
1069 yInfo(
"Closing cartesian_external_wrench_LA port\n");
1070 closePort(port_external_cartesian_wrench_LA);
1071 yInfo(
"Closing external_wrench_RL port\n");
1073 yInfo(
"Closing external_wrench_LL port\n");
1075 yInfo(
"Closing cartesian_external_wrench_RL port\n");
1076 closePort(port_external_cartesian_wrench_RL);
1077 yInfo(
"Closing cartesian_external_wrench_LL port\n");
1078 closePort(port_external_cartesian_wrench_LL);
1079 yInfo(
"Closing external_wrench_RF port\n");
1081 yInfo(
"Closing external_wrench_LF port\n");
1083 yInfo(
"Closing cartesian_external_wrench_RF port\n");
1084 closePort(port_external_cartesian_wrench_RF);
1085 yInfo(
"Closing cartesian_external_wrench_LF port\n");
1086 closePort(port_external_cartesian_wrench_LF);
1087 yInfo(
"Closing external_wrench_TO port\n");
1089 yInfo(
"Closing COM ports\n");
1101 yInfo(
"Closing inertial port\n");
1103 yInfo(
"Closing ft_arm_right port\n");
1105 yInfo(
"Closing ft_arm_left port\n");
1107 yInfo(
"Closing ft_leg_right port\n");
1109 yInfo(
"Closing ft_leg_left port\n");
1111 yInfo(
"Closing ft_foot_right port\n");
1113 yInfo(
"Closing ft_foot_left port\n");
1115 yInfo(
"Closing skin_contacts port\n");
1117 yInfo(
"Closing monitor port\n");
1119 yInfo(
"Closing dump port\n");
1121 yInfo(
"Closing contacts port\n");
1123 yInfo(
"Closing external_ft_arm_left port\n");
1125 yInfo(
"Closing external_ft_arm_right port\n");
1127 yInfo(
"Closing external_ft_leg_left port\n");
1129 yInfo(
"Closing external_ft_leg_right port\n");
1131 yInfo(
"Close COM velocity port\n");
1133 yInfo(
"Closing COM Jacobian port\n");
1135 yInfo(
"Closing All Velocities port\n");
1137 yInfo(
"Closing All Positions port\n");
1139 yInfo(
"Closing Foot/Root port\n");
1143 if (icub) {
delete icub; icub=0;}
1144 if (icub_sens) {
delete icub_sens; icub=0;}
1161 if (_port && _port->getOutputCount()>0)
1163 _port->setEnvelope(this->timestamp);
1164 _port->prepare() = _values ;
1172 a.addInt32(_address);
1173 for(
size_t i=0;i<_values.length();i++)
1174 a.addFloat64(_values(i));
1175 _port->prepare() = a;
1181 yInfo(
"calibrateOffset: starting calibration... \n");
1187 Offset_LFoot.zero();
1188 Offset_RFoot.zero();
1191 setUpperMeasure(
true);
1192 setLowerMeasure(
true);
1194 size_t Ntrials = previous_status.size();
1195 list<iCubStatus>::iterator it=previous_status.begin();
1202 for (it=previous_status.begin() ; it != previous_status.end(); it++ )
1209 F_iDyn_LArm = -1.0 * F_sensor_up.getCol(1);
1210 F_iDyn_RArm = -1.0 * F_sensor_up.getCol(0);
1211 F_iDyn_LLeg = -1.0 * F_sensor_low.getCol(1);
1212 F_iDyn_RLeg = -1.0 * F_sensor_low.getCol(0);
1213 F_iDyn_LFoot = Vector(6,0.0);
1214 F_iDyn_RFoot = Vector(6,0.0);
1216 Offset_LArm = Offset_LArm + (it->ft_arm_left-F_iDyn_LArm);
1217 Offset_RArm = Offset_RArm + (it->ft_arm_right-F_iDyn_RArm);
1218 Offset_LLeg = Offset_LLeg + (it->ft_leg_left-F_iDyn_LLeg);
1219 Offset_RLeg = Offset_RLeg + (it->ft_leg_right-F_iDyn_RLeg);
1220 Offset_LFoot = Offset_LFoot + (it->ft_foot_left-F_iDyn_LFoot);
1221 Offset_RFoot = Offset_RFoot + (it->ft_foot_right-F_iDyn_RFoot);
1224 Offset_LArm = 1.0/(double)Ntrials * Offset_LArm;
1225 Offset_RArm = 1.0/(double)Ntrials * Offset_RArm;
1226 Offset_LLeg = 1.0/(double)Ntrials * Offset_LLeg;
1227 Offset_RLeg = 1.0/(double)Ntrials * Offset_RLeg;
1228 Offset_LFoot = 1.0/(double)Ntrials * Offset_LFoot;
1229 Offset_RFoot = 1.0/(double)Ntrials * Offset_RFoot;
1231 it=previous_status.begin();
1233 yDebug(
"Ntrials: %d\n", (
int)Ntrials);
1234 yDebug(
"F_LArm: %s\n", it->ft_arm_left.toString().c_str());
1235 yDebug(
"F_idyn_LArm: %s\n", F_iDyn_LArm.toString().c_str());
1236 yDebug(
"F_RArm: %s\n", it->ft_arm_right.toString().c_str());
1237 yDebug(
"F_idyn_RArm: %s\n", F_iDyn_RArm.toString().c_str());
1238 yDebug(
"F_LLeg: %s\n", it->ft_leg_left.toString().c_str());
1239 yDebug(
"F_idyn_LLeg: %s\n", F_iDyn_LLeg.toString().c_str());
1240 yDebug(
"F_RLeg: %s\n", it->ft_leg_right.toString().c_str());
1241 yDebug(
"F_idyn_RLeg: %s\n", F_iDyn_RLeg.toString().c_str());
1243 yDebug(
"Left Arm: %s\n", Offset_LArm.toString().c_str());
1244 yDebug(
"Right Arm: %s\n", Offset_RArm.toString().c_str());
1245 yDebug(
"Left Leg: %s\n", Offset_LLeg.toString().c_str());
1246 yDebug(
"Right Leg: %s\n", Offset_RLeg.toString().c_str());
1247 yDebug(
"Left Foot: %s\n", Offset_LFoot.toString().c_str());
1248 yDebug(
"Right Foot: %s\n", Offset_RFoot.toString().c_str());
1259 Vector*
tmp=
nullptr;
1260 if (waitMeasure) yInfo(
"Trying to connect to left arm sensor...");
1263 tmp = port_ft_arm_left->read(waitMeasure);
1273 if (waitMeasure) yDebug(
"done. \n");
1278 Vector*
tmp=
nullptr;
1279 if (waitMeasure) yInfo(
"Trying to connect to right arm sensor...");
1282 tmp = port_ft_arm_right->read(waitMeasure);
1292 if (waitMeasure) yInfo(
"done. \n");
1295 setUpperMeasure(
_init);
1300 Vector*
tmp=
nullptr;
1301 if (waitMeasure) yInfo(
"Trying to connect to left leg sensor...");
1304 tmp = port_ft_leg_left->read(waitMeasure);
1314 if (waitMeasure) yInfo(
"done. \n");
1318 Vector*
tmp=
nullptr;
1319 if (waitMeasure) yInfo(
"Trying to connect to right leg sensor...");
1322 tmp = port_ft_leg_right->read(waitMeasure);
1332 if (waitMeasure) yInfo(
"done. \n");
1338 Vector*
tmp=
nullptr;
1339 if (waitMeasure) yInfo(
"Trying to connect to left foot sensor...");
1342 tmp = port_ft_foot_left->read(
false);
1352 if (waitMeasure) yInfo(
"done. \n");
1356 Vector*
tmp=
nullptr;
1357 if (waitMeasure) yInfo(
"Trying to connect to right foot sensor...");
1360 tmp = port_ft_foot_right->read(
false);
1370 if (waitMeasure) yInfo(
"done. \n");
1374 setLowerMeasure(
_init);
1377 if (waitMeasure) yInfo(
"Trying to connect to inertial sensor...");
1378 Vector *inertial = port_inertial_thread->read(waitMeasure);
1379 if (waitMeasure) yInfo(
"done. \n");
1382 if(inertial!=
nullptr)
1385 #ifdef DEBUG_FIXED_INERTIAL
1388 (*inertial)[2] = 9.81;
1401 #ifdef DEBUG_PRINT_INERTIAL
1402 yDebug (
"meas_w (rad/s): %3.3f, %3.3f, %3.3f \n", w0[0], w0[1], w0[2]);
1403 yDebug (
"meas_dwo(rad/s): %3.3f, %3.3f, %3.3f \n", dw0[0], dw0[1], dw0[2]);
1409 previous_status.push_front(current_status);
1410 if (previous_status.size()>status_queue_size) previous_status.pop_back();
1419 {b &= iencs_leg_left->getEncoders(encoders_leg_left.data());}
1421 {encoders_leg_left.zero();}
1423 if (iencs_leg_right)
1424 {b &= iencs_leg_right->getEncoders(encoders_leg_right.data());}
1426 {encoders_leg_right.zero();}
1429 {b &= iencs_torso->getEncoders(encoders_torso.data());}
1431 {encoders_torso.zero();}
1433 for (
size_t i=0;i<3;i++)
1435 current_status.
all_q_low(i) = encoders_torso(2-i);
1437 for (
size_t i=0;i<6;i++)
1439 current_status.
all_q_low(3+i) = encoders_leg_left(i);
1441 for (
size_t i=0;i<6;i++)
1443 current_status.
all_q_low(3+6+i) = encoders_leg_right(i);
1455 if (iencs_arm_left) b &= iencs_arm_left->getEncoders(encoders_arm_left.data());
1456 else encoders_arm_left.zero();
1457 if (iencs_arm_right) b &= iencs_arm_right->getEncoders(encoders_arm_right.data());
1458 else encoders_arm_right.zero();
1459 if (iencs_head) b &= iencs_head->getEncoders(encoders_head.data());
1460 else encoders_head.zero();
1462 for (
size_t i=0;i<3;i++)
1464 current_status.
all_q_up(i) = encoders_head(i);
1466 for (
size_t i=0;i<7;i++)
1468 current_status.
all_q_up(3+i) = encoders_arm_left(i);
1470 for (
size_t i=0;i<7;i++)
1472 current_status.
all_q_up(3+7+i) = encoders_arm_right(i);
1480 void inverseDynamics::setLowerMeasure(
bool _init)
1512 void inverseDynamics::setUpperMeasure(
bool _init)
1550 void inverseDynamics::addSkinContacts()
1555 skinContactsTimestamp = Time::now();
1568 for(
auto & it : contactsPerBp)
1569 if(it.second.size()>1)
1570 for(
auto & c : it.second)
1571 if(c.getActiveTaxels()<10)
1577 skinContacts = contactsPerBp[
LEFT_ARM];
1578 skinContacts.insert(skinContacts.end(), contactsPerBp[
RIGHT_ARM].begin(), contactsPerBp[
RIGHT_ARM].end());
1580 else if(Time::now()-skinContactsTimestamp>
SKIN_EVENTS_TIMEOUT && skinContactsTimestamp!=0.0)
1584 skinContacts.clear();
1591 Vector monitorData(0);
1594 for(
int i=0;i<3;i++) monitorData.push_back(temp(i));
1596 for(
int i=0;i<3;i++) monitorData.push_back(temp(i));
1598 for(
int i=0;i<3;i++) monitorData.push_back(temp(i));
1602 for(
size_t i=0;i<3;i++){
1604 for(
double j : temp) monitorData.push_back(j);
1606 for(
size_t i=0;i<3;i++){
1608 for(
double j : temp) monitorData.push_back(j);
1612 for(
size_t i=0;i<3;i++)
1614 for(
size_t i=0;i<3;i++)
1616 for(
size_t i=0;i<3;i++){
1618 for(
size_t j=0;j<temp.size();j++)
1619 monitorData.push_back(temp(j));
1621 for(
int i=0;i<3;i++)
1623 port_monitor->prepare() = monitorData;
1624 port_monitor->write();
1633 for(i=0; i< current_status.
all_q_up.size(); i++)
1634 dump.push_back(current_status.
all_q_up[i]);
1635 for(i=0; i< current_status.
all_dq_up.size(); i++)
1636 dump.push_back(current_status.
all_dq_up[i]);
1637 for(i=0; i< current_status.
all_d2q_up.size(); i++)
1638 dump.push_back(current_status.
all_d2q_up[i]);
1639 for(i=0; i< current_status.
all_q_low.size(); i++)
1640 dump.push_back(current_status.
all_q_low[i]);
1641 for(i=0; i< current_status.
all_dq_low.size(); i++)
1642 dump.push_back(current_status.
all_dq_low[i]);
1643 for(i=0; i< current_status.
all_d2q_low.size(); i++)
1646 port_dumpvel->prepare() = dump;
1647 port_dumpvel->write();
bool checkIcubNotMoving()
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...
bool EXPERIMENTAL_getCOMvelocity(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &vel, yarp::sig::Vector &dq)
Retrieves a 3x1 vector containing the velocity of the robot COM.
iCubUpperTorso * upperTorso
pointer to UpperTorso = head + right arm + left arm
bool getAllPositions(yarp::sig::Vector &pos)
Retrieves a vector containing the anglular position of all the iCub joints, ordered in this way: left...
bool EXPERIMENTAL_computeCOMjacobian()
Performs the computation of the center of mass jacobian of the whole iCub.
bool computeCOM()
Performs the computation of the center of mass (COM) of the whole iCub.
iCubLowerTorso * lowerTorso
pointer to LowerTorso = torso + right leg + left leg
bool getCOM(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &COM, double &mass)
Retrieves the result of the last COM computation.
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 ...
bool EXPERIMENTAL_getCOMjacobian(iCub::skinDynLib::BodyPart which_part, yarp::sig::Matrix &jac)
Retrieves the result of the last COM jacobian computation.
yarp::sig::Vector getAngAcc(const unsigned int i) const
Returns the i-th link angular acceleration.
const yarp::sig::Vector & getLinAccCOM(const unsigned int i) const
Returns the i-th link linear acceleration of the COM.
const yarp::sig::Vector & getForce(const unsigned int iLink) const
Returns the i-th link force.
const yarp::sig::Vector & getMoment(const unsigned int iLink) const
Returns the i-th link moment.
yarp::sig::Vector getLinAcc(const unsigned int i) const
Returns the i-th link linear acceleration.
yarp::sig::Vector getAngVel(const unsigned int i) const
Returns the i-th link angular velocity.
yarp::sig::Vector getAngAcc() const
Return the node angular acceleration.
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.
yarp::sig::Vector getAngVel() const
Return the node angular velocity.
yarp::sig::Vector getLinAcc() const
Return the node linear acceleration.
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.
iDyn::iDynContactSolver * leftSensor
Build the node.
yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false)
Redefinition from iDynSensorNode.
iDyn::iDynLimb * left
left limb
yarp::sig::Vector setDAng(const std::string &limbType, const yarp::sig::Vector &_dq)
Set joints angle velocity in the chosen limb.
yarp::sig::Vector getTorsoAngAcc() const
Return the torso angular acceleration.
bool setSensorMeasurement(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left)
Set the FT sensor measurements on the sensor in right and left limb.
yarp::sig::Vector setD2Ang(const std::string &limbType, const yarp::sig::Vector &_ddq)
Set joints angle acceleration in the chosen limb.
yarp::sig::Vector getTorsoLinAcc() const
Return the torso linear acceleration.
yarp::sig::Matrix getHLeft()
Return HLeft, i.e.
yarp::sig::Vector getTorsoForce() const
Return the torso force.
iDyn::iDynLimb * right
right limb
iDyn::iDynContactSolver * rightSensor
right FT sensor and solver
iDyn::iDynLimb * up
central-up limb
yarp::sig::Vector setAng(const std::string &limbType, const yarp::sig::Vector &_q)
Set joints angle position in the chosen limb.
yarp::sig::Matrix getHUp()
Return HUp, i.e.
yarp::sig::Vector getTorsoMoment() const
Return the torso moment.
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.
yarp::sig::Matrix getHRight()
Return HRight, i.e.
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
void threadRelease() override
void broadcastData(T &_values, BufferedPort< T > *_port)
bool getLowerEncodersSpeedAndAcceleration()
void setZeroJntAngVelAcc()
void closePort(Contactable *_port)
bool readAndUpdate(bool waitMeasure=false, bool _init=false)
void calibrateOffset(calib_enum calib_code=CALIB_ALL)
bool threadInit() override
void writeTorque(Vector _values, int _address, BufferedPort< Bottle > *_port)
inverseDynamics(int _rate, PolyDriver *_ddAL, PolyDriver *_ddAR, PolyDriver *_ddH, PolyDriver *_ddLL, PolyDriver *_ddLR, PolyDriver *_ddT, string _robot_name, string _local_name, version_tag icub_type, bool _autoconnect=false)
bool getUpperEncodersSpeedAndAcceleration()
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
BodyPart getBodyPart(SkinPart s)
Get the body part associated to the specified skin part.
double lpf_ord1_3hz(double input, int j)
constexpr int8_t MAX_FILTER_ORDER
constexpr float_t SKIN_EVENTS_TIMEOUT