18 #include <sot/core/debug.hh> 19 #include <dynamic-graph/factory.h> 20 #include <dynamic-graph/all-commands.h> 21 #include <sot/core/stop-watch.hh> 22 #include "pinocchio/algorithm/frames.hpp" 28 namespace talos_balance
30 namespace dg = ::dynamicgraph;
35 using boost::math::normal;
38 const pinocchio::SE3 & s2,
42 const Eigen::Vector3d t_( s1.translation() * alpha+
43 s2.translation() * (1-alpha));
45 const Eigen::Vector3d w( pinocchio::log3(s1.rotation()) * alpha +
46 pinocchio::log3(s2.rotation()) * (1-alpha) );
48 s12 = pinocchio::SE3(pinocchio::exp3(w),t_);
52 double pitch,
double yaw, Eigen::Matrix3d & R)
54 Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
55 Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
56 Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
57 Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
61 void rpyToMatrix(
const Eigen::Vector3d & rpy, Eigen::Matrix3d & R)
66 void matrixToRpy(
const Eigen::Matrix3d & M, Eigen::Vector3d & rpy)
68 double m = sqrt(M(2,1)*M(2,1) + M(2,2)*M(2,2));
69 rpy(1) = atan2(-M(2,0), m);
70 if( fabs(fabs(rpy(1)) - 0.5*M_PI) < 0.001 )
73 rpy(2) = -atan2(M(0,1), M(1,1));
77 rpy(2) = atan2(M(1,0), M(0,0));
78 rpy(0) = atan2(M(2,1), M(2,2));
112 double mean = (a1+a2)/2.;
113 if( (a1- a2) > EIGEN_PI || (a1- a2) < -EIGEN_PI)
115 return mean > 0 ? -EIGEN_PI + mean : EIGEN_PI + mean ;
121 double wEulerMean(
double a1,
double a2,
double w1,
double w2)
123 double wMean = (a1*w1+ a2*w2)/(w1+w2);
124 if ( a1-a2 >= EIGEN_PI )
125 return (EIGEN_PI*(w1-w2)/(w2+w1) - wMean)
127 wMean - EIGEN_PI*(w1-w2)/(w2+w1) : EIGEN_PI +
128 wMean - EIGEN_PI*(w1-w2)/(w2+w1);
129 else if ( a1-a2 < -EIGEN_PI )
130 return (EIGEN_PI*(w2-w1)/(w1+w2) - wMean)
131 < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2) :
132 EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2);
164 #define PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation" 165 #define PROFILE_BASE_VELOCITY_ESTIMATION "base-est velocity estimation" 166 #define PROFILE_BASE_KINEMATICS_COMPUTATION "base-est kinematics computation" 169 #define INPUT_SIGNALS m_joint_positionsSIN << m_joint_velocitiesSIN << \ 170 m_imu_quaternionSIN << m_forceLLEGSIN << m_forceRLEGSIN << m_dforceLLEGSIN << m_dforceRLEGSIN << \ 171 m_w_lf_inSIN << m_w_rf_inSIN << m_K_fb_feet_posesSIN << m_lf_ref_xyzquatSIN << m_rf_ref_xyzquatSIN << m_accelerometerSIN << m_gyroscopeSIN 172 #define OUTPUT_SIGNALS m_qSOUT << m_vSOUT << m_q_lfSOUT << m_q_rfSOUT << m_q_imuSOUT << \ 173 m_w_lfSOUT << m_w_rfSOUT << m_w_lf_filteredSOUT << m_w_rf_filteredSOUT << m_lf_xyzquatSOUT << m_rf_xyzquatSOUT << \ 174 m_v_acSOUT << m_a_acSOUT << m_v_kinSOUT << m_v_imuSOUT << m_v_gyrSOUT << m_v_flexSOUT 182 "TalosBaseEstimator");
197 ,CONSTRUCT_SIGNAL_IN( w_lf_in, double)
198 ,CONSTRUCT_SIGNAL_IN( w_rf_in, double)
199 ,CONSTRUCT_SIGNAL_IN( K_fb_feet_poses, double)
204 ,CONSTRUCT_SIGNAL_INNER(kinematics_computations,
dynamicgraph::
Vector, m_joint_positionsSIN
205 << m_joint_velocitiesSIN)
207 << m_joint_positionsSIN
208 << m_imu_quaternionSIN
213 << m_K_fb_feet_posesSIN
214 << m_w_lf_filteredSOUT
215 << m_w_rf_filteredSOUT
216 << m_lf_ref_xyzquatSIN
217 << m_rf_ref_xyzquatSIN)
219 << m_accelerometerSIN
235 << m_imu_quaternionSIN)
236 ,CONSTRUCT_SIGNAL_OUT(w_lf, double, m_forceLLEGSIN)
237 ,CONSTRUCT_SIGNAL_OUT(w_rf, double, m_forceRLEGSIN)
238 ,CONSTRUCT_SIGNAL_OUT(w_lf_filtered, double, m_w_lfSOUT)
239 ,CONSTRUCT_SIGNAL_OUT(w_rf_filtered, double, m_w_rfSOUT)
240 ,m_initSucceeded(false)
241 ,m_reset_foot_pos(true)
243 ,m_zmp_std_dev_rf(1.0)
244 ,m_zmp_std_dev_lf(1.0)
245 ,m_fz_std_dev_rf(1.0)
246 ,m_fz_std_dev_lf(1.0)
247 ,m_zmp_margin_lf(0.0)
248 ,m_zmp_margin_rf(0.0)
260 docCommandVoid2(
"Initialize the entity.",
261 "time step (double)",
262 "URDF file path (string)")));
265 addCommand(
"set_fz_stable_windows_size",
267 docCommandVoid1(
"Set the windows size used to detect that feet is stable",
269 addCommand(
"set_alpha_w_filter",
271 docCommandVoid1(
"Set the filter parameter to filter weights",
273 addCommand(
"set_alpha_DC_acc",
275 docCommandVoid1(
"Set the filter parameter for removing DC from accelerometer data",
277 addCommand(
"set_alpha_DC_vel",
279 docCommandVoid1(
"Set the filter parameter for removing DC from velocity integrated from acceleration",
281 addCommand(
"reset_foot_positions",
283 docCommandVoid0(
"Reset the position of the feet.")));
284 addCommand(
"get_imu_weight",
285 makeDirectGetter(*
this,&
m_w_imu,
286 docDirectGetter(
"Weight of imu-based orientation in sensor fusion",
288 addCommand(
"set_imu_weight",
290 docCommandVoid1(
"Set the weight of imu-based orientation in sensor fusion",
292 addCommand(
"set_zmp_std_dev_right_foot",
294 docCommandVoid1(
"Set the standard deviation of the ZMP measurement errors for the right foot",
296 addCommand(
"set_zmp_std_dev_left_foot",
298 docCommandVoid1(
"Set the standard deviation of the ZMP measurement errors for the left foot",
300 addCommand(
"set_normal_force_std_dev_right_foot",
302 docCommandVoid1(
"Set the standard deviation of the normal force measurement errors for the right foot",
304 addCommand(
"set_normal_force_std_dev_left_foot",
306 docCommandVoid1(
"Set the standard deviation of the normal force measurement errors for the left foot",
308 addCommand(
"set_stiffness_right_foot",
310 docCommandVoid1(
"Set the 6d stiffness of the spring at the right foot",
312 addCommand(
"set_stiffness_left_foot",
314 docCommandVoid1(
"Set the 6d stiffness of the spring at the left foot",
316 addCommand(
"set_right_foot_sizes",
318 docCommandVoid1(
"Set the size of the right foot (pos x, neg x, pos y, neg y)",
320 addCommand(
"set_left_foot_sizes",
322 docCommandVoid1(
"Set the size of the left foot (pos x, neg x, pos y, neg y)",
324 addCommand(
"set_zmp_margin_right_foot",
326 docCommandVoid1(
"Set the ZMP margin for the right foot",
328 addCommand(
"set_zmp_margin_left_foot",
330 docCommandVoid1(
"Set the ZMP margin for the left foot",
332 addCommand(
"set_normal_force_margin_right_foot",
334 docCommandVoid1(
"Set the normal force margin for the right foot",
336 addCommand(
"set_normal_force_margin_left_foot",
338 docCommandVoid1(
"Set the normal force margin for the left foot",
348 std::string localName(robotRef);
349 if (isNameInRobotUtil(localName))
352 std::cerr <<
"m_robot_util:" <<
m_robot_util << std::endl;
356 SEND_MSG(
"You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
360 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename,
361 pinocchio::JointModelFreeFlyer(),
m_model);
371 std::cerr <<
"IMU in Torso" <<
m_torsoMimu << std::endl;
383 -Eigen::Map<const Vector3>(&
m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ(0)));
399 catch (
const std::exception& e)
401 std::cout << e.what();
402 SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
412 return SEND_MSG(
"windows_size should be a positive integer", MSG_TYPE_ERROR);
419 return SEND_MSG(
"alpha should be in [0..1]", MSG_TYPE_ERROR);
427 return SEND_MSG(
"alpha should be in [0..1]", MSG_TYPE_ERROR);
434 return SEND_MSG(
"alpha should be in [0..1]", MSG_TYPE_ERROR);
447 return SEND_MSG(
"Imu weight must be nonnegative", MSG_TYPE_ERROR);
454 return SEND_MSG(
"Stiffness vector should have size 6, not "+toString(k.size()), MSG_TYPE_ERROR);
461 return SEND_MSG(
"Stiffness vector should have size 6, not "+toString(k.size()), MSG_TYPE_ERROR);
468 return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
475 return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
482 return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
489 return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
496 return SEND_MSG(
"Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
503 return SEND_MSG(
"Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
529 pinocchio::Force f(w);
531 if(f.linear()[2]==0.0)
533 zmp[0] = -f.angular()[1] / f.linear()[2];
534 zmp[1] = f.angular()[0] / f.linear()[2];
538 double std_dev,
double margin)
540 double fs0=foot_sizes[0] - margin;
541 double fs1=foot_sizes[1] + margin;
542 double fs2=foot_sizes[2] - margin;
543 double fs3=foot_sizes[3] + margin;
545 if(zmp[0]>fs0 || zmp[0]<fs1 || zmp[1]>fs2 || zmp[1]<fs3)
548 double cdx = ((cdf(
m_normal, (fs0-zmp[0])/std_dev) -
549 cdf(
m_normal, (fs1-zmp[0])/std_dev))-0.5 )*2.0;
550 double cdy = ((cdf(
m_normal, (fs2-zmp[1])/std_dev) -
551 cdf(
m_normal, (fs3-zmp[1])/std_dev))-0.5 )*2.0;
559 return (cdf(
m_normal, (fz-margin)/std_dev)-0.5)*2.0;
565 SE3 dummy, dummy1, lfMff, rfMff;
572 const Vector3 & ankle_2_sole_xyz =
m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ;
573 const SE3 groundMfoot(Matrix3::Identity(), -1.0*ankle_2_sole_xyz);
574 lfMff = groundMfoot * lfMff;
575 rfMff = groundMfoot * rfMff;
578 const Vector3 w( 0.5*(pinocchio::log3(lfMff.rotation())+pinocchio::log3(rfMff.rotation())) );
579 SE3 oMff = SE3(pinocchio::exp3(w), 0.5*(lfMff.translation()+rfMff.translation()));
583 m_oMlfs = oMff * lfMff.inverse() * groundMfoot;
584 m_oMrfs = oMff * rfMff.inverse() * groundMfoot;
587 Eigen::Quaternion<double> quat_lf(
m_oMlfs.rotation());
594 Eigen::Quaternion<double> quat_rf(
m_oMrfs.rotation());
604 sendMsg(
"Reference pos of left foot:\n"+toString(
m_oMlfs), MSG_TYPE_INFO);
605 sendMsg(
"Reference pos of right foot:\n"+toString(
m_oMrfs), MSG_TYPE_INFO);
617 const SE3 & oMfs,
const pinocchio::FrameIndex foot_id,
618 SE3 & oMff, SE3 & oMfa, SE3 & fsMff)
621 xyz << -ft[0]/K(0), -ft[1]/K(1), -ft[2]/K(2);
623 rpyToMatrix(-ft[3]/K(3), -ft[4]/K(4), -ft[5]/K(5), R);
624 const SE3 fsMfa(R, xyz);
626 const SE3 faMff(
m_data->oMf[foot_id].inverse());
641 SEND_WARNING_STREAM_MSG(
"Cannot compute signal kinematics_computations before initialization!");
645 const Eigen::VectorXd& qj= m_joint_positionsSIN(iter);
646 const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
647 assert(qj.size()==
m_robot_util->m_nbJoints &&
"Unexpected size of signal joint_positions");
648 assert(dq.size()==
m_robot_util->m_nbJoints &&
"Unexpected size of signal joint_velocities");
672 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q before initialization!");
678 const Eigen::VectorXd & qj = m_joint_positionsSIN(iter);
679 const Eigen::Vector4d & quatIMU_vec = m_imu_quaternionSIN(iter);
680 const Vector6 & ftrf = m_forceRLEGSIN(iter);
681 const Vector6 & ftlf = m_forceLLEGSIN(iter);
687 if(m_w_lf_inSIN.isPlugged())
689 wL = m_w_lf_inSIN(iter);
693 wL = m_w_lf_filteredSOUT(iter);
697 if(m_w_rf_inSIN.isPlugged())
699 wR = m_w_rf_inSIN(iter);
703 wR = m_w_rf_filteredSOUT(iter);
708 assert(qj.size()==
m_robot_util->m_nbJoints &&
"Unexpected size of signal joint_positions");
711 if(wR==0.0 && wL==0.0)
713 SEND_WARNING_STREAM_MSG(
"The robot is flying!"+
714 (
"- forceRLEG: "+toString(ftrf.transpose()))+
715 "- forceLLEG: "+toString(ftlf.transpose())+
722 m_kinematics_computationsSINNER(iter);
729 SE3 oMlfa, oMrfa, lfsMff, rfsMff;
735 const SE3 torsoMff(ffMtorso.inverse());
737 Vector3 rpy_torso, rpy_torso_lf, rpy_torso_rf, rpy_torso_imu;
741 Eigen::Quaterniond quatIMU(quatIMU_vec[0], quatIMU_vec[1], quatIMU_vec[2], quatIMU_vec[3]);
742 Eigen::Quaterniond quat_torsoMimu(
m_torsoMimu.rotation());
743 Eigen::Quaterniond quat_torso_imu(quatIMU*quat_torsoMimu);
745 matrixToRpy(quat_torso_imu.toRotationMatrix(), rpy_torso_imu);
749 rpy_torso[0] =
wEulerMean(rpy_torso_lf[0], rpy_torso_rf[0], wL, wR);
751 rpy_torso[1] =
wEulerMean(rpy_torso_lf[1], rpy_torso_rf[1], wL, wR);
753 rpy_torso[2] =
wEulerMean(rpy_torso_lf[2], rpy_torso_rf[2], wL, wR);
763 m_q_pin.head<3>() = ((oMlfa.translation() - pos_lf_ff)*wL +
764 (oMrfa.translation() - pos_rf_ff)*wR) / (wL+wR);
775 if(m_K_fb_feet_posesSIN.isPlugged())
777 const double K_fb = m_K_fb_feet_posesSIN(iter);
778 if (K_fb > 0.0 &&
m_w_imu > 0.0)
780 assert(
m_w_imu > 0.0 &&
"Update of the feet 6d poses should not be done if wIMU = 0.0");
781 assert(K_fb < 1.0 &&
"Feedback gain on foot correction should be less than 1.0 (K_fb_feet_poses>1.0)");
783 const SE3 oMlfs_est( oMff_est*(lfsMff.inverse()) );
784 const SE3 oMrfs_est( oMff_est*(rfsMff.inverse()) );
786 SE3 leftDrift =
m_oMlfs.inverse()*oMlfs_est;
787 SE3 rightDrift =
m_oMrfs.inverse()*oMrfs_est;
791 SE3 rightDrift_delta;
792 se3Interp(leftDrift ,SE3::Identity(),K_fb*wR,leftDrift_delta);
793 se3Interp(rightDrift,SE3::Identity(),K_fb*wL,rightDrift_delta);
796 rightDrift_delta = rightDrift;
798 leftDrift_delta = leftDrift;
802 rightDrift_delta = SE3::Identity();
803 leftDrift_delta = SE3::Identity();
815 SE3 oMlfs_ref, oMrfs_ref;
816 if (m_lf_ref_xyzquatSIN.isPlugged() and
817 m_rf_ref_xyzquatSIN.isPlugged())
820 const Vector7 & lf_ref_xyzquat_vec = m_lf_ref_xyzquatSIN(iter);
821 const Vector7 & rf_ref_xyzquat_vec = m_rf_ref_xyzquatSIN(iter);
822 const Eigen::Quaterniond ql(m_lf_ref_xyzquatSIN(iter)(6),
823 m_lf_ref_xyzquatSIN(iter)(3),
824 m_lf_ref_xyzquatSIN(iter)(4),
825 m_lf_ref_xyzquatSIN(iter)(5));
826 const Eigen::Quaterniond qr(m_rf_ref_xyzquatSIN(iter)(6),
827 m_rf_ref_xyzquatSIN(iter)(3),
828 m_rf_ref_xyzquatSIN(iter)(4),
829 m_rf_ref_xyzquatSIN(iter)(5));
830 oMlfs_ref = SE3(ql.toRotationMatrix(), lf_ref_xyzquat_vec.head<3>());
831 oMrfs_ref = SE3(qr.toRotationMatrix(), rf_ref_xyzquat_vec.head<3>());
839 const Vector3 translation_feet_drift = 0.5*( ( oMlfs_ref.translation() -
m_oMlfs.translation()) +
840 ( oMrfs_ref.translation() -
m_oMrfs.translation()) );
842 const Vector3 V_ref = oMrfs_ref.translation() - oMlfs_ref.translation();
845 const double yaw_drift = (atan2(V_ref(1), V_ref(0)) -
846 atan2(V_est(1), V_est(0)));
849 const Vector3 rpy_feet_drift(0.,0.,yaw_drift);
850 Matrix3 rotation_feet_drift;
852 const SE3 drift_to_ref(rotation_feet_drift , translation_feet_drift);
865 else if (K_fb < -1e-3)
867 if (m_lf_ref_xyzquatSIN.isPlugged() and
868 m_rf_ref_xyzquatSIN.isPlugged())
871 const Vector7 & lf_ref_xyzquat_vec = m_lf_ref_xyzquatSIN(iter);
872 const Vector7 & rf_ref_xyzquat_vec = m_rf_ref_xyzquatSIN(iter);
873 const Eigen::Quaterniond ql(m_lf_ref_xyzquatSIN(iter)(6),
874 m_lf_ref_xyzquatSIN(iter)(3),
875 m_lf_ref_xyzquatSIN(iter)(4),
876 m_lf_ref_xyzquatSIN(iter)(5));
877 const Eigen::Quaterniond qr(m_rf_ref_xyzquatSIN(iter)(6),
878 m_rf_ref_xyzquatSIN(iter)(3),
879 m_rf_ref_xyzquatSIN(iter)(4),
880 m_rf_ref_xyzquatSIN(iter)(5));
881 m_oMlfs = SE3(ql.toRotationMatrix(), lf_ref_xyzquat_vec.head<3>());
882 m_oMrfs = SE3(qr.toRotationMatrix(), rf_ref_xyzquat_vec.head<3>());
894 Eigen::Quaternion<double> quat_lf(
m_oMlfs.rotation());
901 Eigen::Quaternion<double> quat_rf(
m_oMrfs.rotation());
915 SEND_WARNING_STREAM_MSG(
"Cannot compute signal lf_xyzquat before initialization!");
920 const Eigen::VectorXd & q = m_qSOUT(iter);
930 SEND_WARNING_STREAM_MSG(
"Cannot compute signal rf_xyzquat before initialization!");
935 const Eigen::VectorXd & q = m_qSOUT(iter);
945 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_lf before initialization!");
951 const Eigen::VectorXd & q = m_qSOUT(iter);
962 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_rf before initialization!");
968 const Eigen::VectorXd & q = m_qSOUT(iter);
979 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_imu before initialization!");
985 const Eigen::VectorXd & q = m_qSOUT(iter);
988 const SE3 torsoMff(ffMtorso.inverse());
989 const Eigen::Vector4d & quatIMU_vec = m_imu_quaternionSIN(iter);
991 Eigen::Quaterniond quatIMU(quatIMU_vec[0], quatIMU_vec[1], quatIMU_vec[2], quatIMU_vec[3]);
992 Eigen::Quaterniond quat_torsoMimu(
m_torsoMimu.rotation());
993 Eigen::Quaterniond quat_torso_imu(quatIMU*quat_torsoMimu);
995 base_se3_to_sot(q.head<3>(),
m_oRff, s.head<6>());
1005 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_lf before initialization!");
1009 const Vector6 & wrench = m_forceLLEGSIN(iter);
1038 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_rf before initialization!");
1042 const Vector6 & wrench = m_forceRLEGSIN(iter);
1072 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_rf_filtered before initialization!");
1075 double w_rf = m_w_rfSOUT(iter);
1085 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_lf_filtered before initialization!");
1088 double w_lf = m_w_lfSOUT(iter);
1099 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v before initialization!");
1105 m_kinematics_computationsSINNER(iter);
1110 const Eigen::VectorXd & dq = m_joint_velocitiesSIN(iter);
1111 const Eigen::Vector3d & acc_imu = m_accelerometerSIN(iter);
1112 const Eigen::Vector3d & gyr_imu = m_gyroscopeSIN(iter);
1114 const Vector6 & dftrf = m_dforceRLEGSIN(iter);
1115 const Vector6 & dftlf = m_dforceLLEGSIN(iter);
1116 assert(dq.size()==
m_robot_util->m_nbJoints &&
"Unexpected size of signal joint_velocities");
1122 if(m_w_lf_inSIN.isPlugged())
1124 wL = m_w_lf_inSIN(iter);
1128 wL = m_w_lf_filteredSOUT(iter);
1132 if(m_w_rf_inSIN.isPlugged())
1134 wR = m_w_rf_inSIN(iter);
1138 wR = m_w_rf_filteredSOUT(iter);
1143 if(wR==0.0 && wL==0.0)
1151 const Motion & v_lf_local =
m_data->v[f_lf.parent];
1152 const SE3 & ffMlf =
m_data->oMi[f_lf.parent];
1153 Vector6 v_kin_l = -ffMlf.act(v_lf_local).toVector();
1154 v_kin_l.head<3>() =
m_oRff * v_kin_l.head<3>();
1155 v_kin_l.segment<3>(3) =
m_oRff * v_kin_l.segment<3>(3);
1158 const Motion & v_rf_local =
m_data->v[f_rf.parent];
1159 const SE3 & ffMrf =
m_data->oMi[f_rf.parent];
1160 Vector6 v_kin_r = -ffMrf.act(v_rf_local).toVector();
1161 v_kin_r.head<3>() =
m_oRff * v_kin_r.head<3>();
1162 v_kin_r.segment<3>(3) =
m_oRff * v_kin_r.segment<3>(3);
1164 m_v_kin.head<6>() = (wR*v_kin_r + wL*v_kin_l)/(wL+wR);
1173 const Eigen::Matrix<double, 6, 6> lfAff = ffMlf.inverse().toActionMatrix();
1174 const Eigen::Matrix<double, 6, 6> rfAff = ffMrf.inverse().toActionMatrix();
1175 Eigen::Matrix<double, 12, 6> A;
1178 Eigen::Matrix<double, 12, 1> b;
1182 m_v_flex.head<6>() = (A.transpose() * A).ldlt().solve(A.transpose() * b);
1188 const Matrix3 & ffRtorso = ffMtorso.rotation();
1189 const Matrix3 ffRimu = ffRtorso *
m_torsoMimu.rotation();
1193 const Matrix3 lfRimu = ffMlf.rotation().transpose() * ffRimu;
1194 const Matrix3 rfRimu = ffMrf.rotation().transpose() * ffRimu;
1204 Motion v_gyr_ankle_l(
Vector3(0.,0.,0.), lfRimu * gVo_a_l);
1205 Motion v_gyr_ankle_r(
Vector3(0.,0.,0.), rfRimu * gVo_a_r);
1206 Vector6 v_gyr_l = ffMlf.inverse().actInv(v_gyr_ankle_l).toVector();
1207 Vector6 v_gyr_r = ffMrf.inverse().actInv(v_gyr_ankle_r).toVector();
1208 m_v_gyr.head<6>() = (wL*v_gyr_l + wR*v_gyr_r)/(wL+wR);
1229 sendMsg(
"iter:"+toString(iter)+
"\n", MSG_TYPE_INFO);
1234 const Vector3 ACacc = acc_world - DCacc;
1243 const Vector3 ACvel = vel - DCvel;
1249 const Motion imuWimu(imuVimu,gyr_imu);
1255 const Motion v= ffMimu.act(imuWimu) ;
1256 m_v_imu.head<6>() = v.toVector();
1282 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_kin before initialization!");
1294 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_flex before initialization!");
1306 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_imu before initialization!");
1318 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_gyr before initialization!");
1330 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_ac before initialization!");
1342 SEND_WARNING_STREAM_MSG(
"Cannot compute signal a_ac before initialization!");
1358 os <<
"TalosBaseEstimator "<<getName();
1361 getProfiler().report_all(3, os);
1363 catch (ExceptionSignal e) {}
void set_zmp_std_dev_left_foot(const double &std_dev)
double eulerMean(double a1, double a2)
void set_alpha_DC_vel(const double &a)
SE3 m_oMrfs
transformation from world to left foot sole
Eigen::Matrix< Scalar, 3, 1 > Vector3
double m_fz_margin_lf
margin used for computing zmp weight
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d &R)
double m_zmp_margin_rf
margin used for computing zmp weight
#define PROFILE_BASE_VELOCITY_ESTIMATION
Vector7 m_oMlfs_xyzquat
transformation from world to right foot sole
#define PROFILE_BASE_KINEMATICS_COMPUTATION
Eigen::VectorXd m_v_flex
6d robot velocities from kinematic only (encoders derivative)
double m_dt
true after the command resetFootPositions is called
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
double m_fz_std_dev_rf
standard deviation of ZMP measurement errors for left foot
double m_fz_std_dev_lf
standard deviation of normal force measurement errors for right foot
RobotUtilShrPtr m_robot_util
sampling time step
virtual void display(std::ostream &os) const
filtered weight of the estimation coming from the right foot
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
void se3Interp(const pinocchio::SE3 &s1, const pinocchio::SE3 &s2, const double alpha, pinocchio::SE3 &s12)
double compute_zmp_weight(const Vector2 &zmp, const Vector4 &foot_sizes, double std_dev, double margin)
void reset_foot_positions_impl(const Vector6 &ftlf, const Vector6 &ftrf)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW TalosBaseEstimator(const std::string &name)
void set_imu_weight(const double &w)
pinocchio::SE3 m_oMff_lf
chest to imu transformation
double m_alpha_DC_acc
acceleration of the base in the world with DC component removed
void set_normal_force_std_dev_right_foot(const double &std_dev)
double m_zmp_std_dev_lf
standard deviation of ZMP measurement errors for right foot
Vector3 m_a_ac
velocity of the base in the world with DC component removed
pinocchio::Data * m_data
Pinocchio robot model.
bool m_isFirstIterFlag
Normal distribution.
void set_zmp_margin_right_foot(const double &margin)
#define PROFILE_BASE_POSITION_ESTIMATION
Vector3 m_v_ac
6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities) ...
double compute_force_weight(double fz, double std_dev, double margin)
double wEulerMean(double a1, double a2, double w1, double w2)
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
void set_fz_stable_windows_size(const int &ws)
Eigen::VectorXd m_v_kin
6d stiffness of left foot spring
Vector6 m_K_lf
6d stiffness of right foot spring
SE3 m_sole_M_ftSens
flag to detect first iteration and initialise velocity filters
void set_stiffness_right_foot(const dynamicgraph::Vector &k)
int m_lf_fz_stable_cpt
size of the windows used to detect that feet did not leave the ground
Eigen::VectorXd m_v_pin
robot configuration according to SoT convention
SE3 m_oMlfs
world-to-base transformation obtained through right foot
pinocchio::FrameIndex m_right_foot_id
foot sole to F/T sensor transformation
bool m_reset_foot_pos
true if the entity has been successfully initialized
bool m_right_foot_is_stable
True if left foot as been stable for the last 'm_fz_stable_windows_size' samples. ...
pinocchio::FrameIndex m_left_foot_id
void set_right_foot_sizes(const dynamicgraph::Vector &s)
double m_w_rf_filtered
filtered weight of the estimation coming from the left foot
Matrix3 m_oRff
chest orientation in the world from angular fusion
double m_zmp_std_dev_rf
weight of IMU for sensor fusion
void init(const double &dt, const std::string &urdfFile)
void set_normal_force_margin_right_foot(const double &margin)
Vector3 m_last_vel
base orientation in the world
int m_fz_stable_windows_size
True if right foot as been stable for the last 'm_fz_stable_windows_size' samples.
normal m_normal
Default reference for right foot pose to use if no ref is pluged.
pinocchio::SE3 m_torsoMimu
Pinocchio robot data.
double m_alpha_DC_vel
alpha parameter for DC blocker filter on acceleration data
double m_alpha_w_filter
alpha parameter for DC blocker filter on velocity data
pinocchio::Model m_model
filtered weight of the estimation coming from the right foot
void compute_zmp(const Vector6 &w, Vector2 &zmp)
void set_normal_force_margin_left_foot(const double &margin)
void set_alpha_DC_acc(const double &a)
pinocchio::FrameIndex m_IMU_frame_id
double m_fz_margin_rf
margin used for computing normal force weight
pinocchio::FrameIndex m_torso_id
void matrixToRpy(const Eigen::Matrix3d &M, Eigen::Vector3d &rpy)
void kinematics_estimation(const Vector6 &ft, const Vector6 &K, const SE3 &oMfs, const pinocchio::FrameIndex foot_id, SE3 &oMff, SE3 &oMfa, SE3 &fsMff)
void set_zmp_margin_left_foot(const double &margin)
double m_w_imu
counter for detecting for how long the feet has been stable
bool m_left_foot_is_stable
pinocchio::SE3 m_oMff_rf
world-to-base transformation obtained through left foot
void set_zmp_std_dev_right_foot(const double &std_dev)
double m_zmp_margin_lf
sizes of the left foot (pos x, neg x, pos y, neg y)
Eigen::VectorXd m_v_imu
6d robot velocities from flexibility only (force sensor derivative)
Vector4 m_left_foot_sizes
standard deviation of normal force measurement errors for left foot
void set_normal_force_std_dev_left_foot(const double &std_dev)
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
int m_rf_fz_stable_cpt
counter for detecting for how long the feet has been stable
void set_alpha_w_filter(const double &a)
void set_left_foot_sizes(const dynamicgraph::Vector &s)
Vector6 m_K_rf
margin used for computing normal force weight
SE3 m_oMrfs_default_ref
Default reference for left foot pose to use if no ref is pluged.
double m_w_lf_filtered
filter parameter to filter weights (1st order low pass filter)
void reset_foot_positions()
Eigen::VectorXd m_v_gyr
6d robot velocities form imu only (accelerometer integration + gyro)
void set_stiffness_left_foot(const dynamicgraph::Vector &k)
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention
Eigen::Matrix< Scalar, 6, 1 > Vector6
Vector4 m_right_foot_sizes
sizes of the left foot (pos x, neg x, pos y, neg y)
Matrix3 m_oRtorso
robot velocities according to SoT convention
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)