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));
83 const Eigen::Vector4d & q2,
84 Eigen::Vector4d & q12)
86 q12(0) = q2(0)*q1(0)-q2(1)*q1(1)-q2(2)*q1(2)-q2(3)*q1(3);
87 q12(1) = q2(0)*q1(1)+q2(1)*q1(0)-q2(2)*q1(3)+q2(3)*q1(2);
88 q12(2) = q2(0)*q1(2)+q2(1)*q1(3)+q2(2)*q1(0)-q2(3)*q1(1);
89 q12(3) = q2(0)*q1(3)-q2(1)*q1(2)+q2(2)*q1(1)+q2(3)*q1(0);
93 (
const Eigen::Vector3d & point,
94 const Eigen::Vector4d & quat,
95 Eigen::Vector3d & rotatedPoint)
97 const Eigen::Vector4d p4(0.0, point(0),point(1),point(2));
98 const Eigen::Vector4d quat_conj(quat(0),-quat(1),-quat(2),-quat(3));
99 Eigen::Vector4d q_tmp1,q_tmp2;
102 rotatedPoint(0) = q_tmp2(1);
103 rotatedPoint(1) = q_tmp2(2);
104 rotatedPoint(2) = q_tmp2(3);
110 double mean = (a1+a2)/2.;
111 if( (a1- a2) > EIGEN_PI || (a1- a2) < -EIGEN_PI)
113 return mean > 0 ? -EIGEN_PI + mean : EIGEN_PI + mean ;
119 double wEulerMean(
double a1,
double a2,
double w1,
double w2)
121 double wMean = (a1*w1+ a2*w2)/(w1+w2);
122 if ( a1-a2 >= EIGEN_PI )
123 return (EIGEN_PI*(w1-w2)/(w2+w1) - wMean)
125 wMean - EIGEN_PI*(w1-w2)/(w2+w1) : EIGEN_PI +
126 wMean - EIGEN_PI*(w1-w2)/(w2+w1);
127 else if ( a1-a2 < -EIGEN_PI )
128 return (EIGEN_PI*(w2-w1)/(w1+w2) - wMean)
129 < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2) :
130 EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2);
162 #define PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation" 163 #define PROFILE_BASE_VELOCITY_ESTIMATION "base-est velocity estimation" 164 #define PROFILE_BASE_KINEMATICS_COMPUTATION "base-est kinematics computation" 167 #define INPUT_SIGNALS m_joint_positionsSIN << m_joint_velocitiesSIN << \ 168 m_imu_quaternionSIN << m_forceLLEGSIN << m_forceRLEGSIN << m_dforceLLEGSIN << m_dforceRLEGSIN << \ 169 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 170 #define OUTPUT_SIGNALS m_qSOUT << m_vSOUT << m_q_lfSOUT << m_q_rfSOUT << m_q_imuSOUT << \ 171 m_w_lfSOUT << m_w_rfSOUT << m_w_lf_filteredSOUT << m_w_rf_filteredSOUT << m_lf_xyzquatSOUT << m_rf_xyzquatSOUT << \ 172 m_v_acSOUT << m_a_acSOUT << m_v_kinSOUT << m_v_imuSOUT << m_v_gyrSOUT << m_v_flexSOUT 180 "TalosBaseEstimator");
195 ,CONSTRUCT_SIGNAL_IN( w_lf_in, double)
196 ,CONSTRUCT_SIGNAL_IN( w_rf_in, double)
197 ,CONSTRUCT_SIGNAL_IN( K_fb_feet_poses, double)
202 ,CONSTRUCT_SIGNAL_INNER(kinematics_computations,
dynamicgraph::
Vector, m_joint_positionsSIN
203 << m_joint_velocitiesSIN)
205 << m_joint_positionsSIN
206 << m_imu_quaternionSIN
211 << m_K_fb_feet_posesSIN
212 << m_w_lf_filteredSOUT
213 << m_w_rf_filteredSOUT
214 << m_lf_ref_xyzquatSIN
215 << m_rf_ref_xyzquatSIN)
217 << m_accelerometerSIN
233 << m_imu_quaternionSIN)
234 ,CONSTRUCT_SIGNAL_OUT(w_lf, double, m_forceLLEGSIN)
235 ,CONSTRUCT_SIGNAL_OUT(w_rf, double, m_forceRLEGSIN)
236 ,CONSTRUCT_SIGNAL_OUT(w_lf_filtered, double, m_w_lfSOUT)
237 ,CONSTRUCT_SIGNAL_OUT(w_rf_filtered, double, m_w_rfSOUT)
238 ,m_initSucceeded(false)
239 ,m_reset_foot_pos(true)
241 ,m_zmp_std_dev_rf(1.0)
242 ,m_zmp_std_dev_lf(1.0)
243 ,m_fz_std_dev_rf(1.0)
244 ,m_fz_std_dev_lf(1.0)
245 ,m_zmp_margin_lf(0.0)
246 ,m_zmp_margin_rf(0.0)
258 docCommandVoid2(
"Initialize the entity.",
259 "time step (double)",
260 "URDF file path (string)")));
263 addCommand(
"set_fz_stable_windows_size",
265 docCommandVoid1(
"Set the windows size used to detect that feet is stable",
267 addCommand(
"set_alpha_w_filter",
269 docCommandVoid1(
"Set the filter parameter to filter weights",
271 addCommand(
"set_alpha_DC_acc",
273 docCommandVoid1(
"Set the filter parameter for removing DC from accelerometer data",
275 addCommand(
"set_alpha_DC_vel",
277 docCommandVoid1(
"Set the filter parameter for removing DC from velocity integrated from acceleration",
279 addCommand(
"reset_foot_positions",
281 docCommandVoid0(
"Reset the position of the feet.")));
282 addCommand(
"get_imu_weight",
283 makeDirectGetter(*
this,&
m_w_imu,
284 docDirectGetter(
"Weight of imu-based orientation in sensor fusion",
286 addCommand(
"set_imu_weight",
288 docCommandVoid1(
"Set the weight of imu-based orientation in sensor fusion",
290 addCommand(
"set_zmp_std_dev_right_foot",
292 docCommandVoid1(
"Set the standard deviation of the ZMP measurement errors for the right foot",
294 addCommand(
"set_zmp_std_dev_left_foot",
296 docCommandVoid1(
"Set the standard deviation of the ZMP measurement errors for the left foot",
298 addCommand(
"set_normal_force_std_dev_right_foot",
300 docCommandVoid1(
"Set the standard deviation of the normal force measurement errors for the right foot",
302 addCommand(
"set_normal_force_std_dev_left_foot",
304 docCommandVoid1(
"Set the standard deviation of the normal force measurement errors for the left foot",
306 addCommand(
"set_stiffness_right_foot",
308 docCommandVoid1(
"Set the 6d stiffness of the spring at the right foot",
310 addCommand(
"set_stiffness_left_foot",
312 docCommandVoid1(
"Set the 6d stiffness of the spring at the left foot",
314 addCommand(
"set_right_foot_sizes",
316 docCommandVoid1(
"Set the size of the right foot (pos x, neg x, pos y, neg y)",
318 addCommand(
"set_left_foot_sizes",
320 docCommandVoid1(
"Set the size of the left foot (pos x, neg x, pos y, neg y)",
322 addCommand(
"set_zmp_margin_right_foot",
324 docCommandVoid1(
"Set the ZMP margin for the right foot",
326 addCommand(
"set_zmp_margin_left_foot",
328 docCommandVoid1(
"Set the ZMP margin for the left foot",
330 addCommand(
"set_normal_force_margin_right_foot",
332 docCommandVoid1(
"Set the normal force margin for the right foot",
334 addCommand(
"set_normal_force_margin_left_foot",
336 docCommandVoid1(
"Set the normal force margin for the left foot",
346 std::string localName(robotRef);
347 if (isNameInRobotUtil(localName))
350 std::cerr <<
"m_robot_util:" <<
m_robot_util << std::endl;
354 SEND_MSG(
"You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
358 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename,
359 pinocchio::JointModelFreeFlyer(),
m_model);
369 std::cerr <<
"IMU in Torso" <<
m_torsoMimu << std::endl;
381 -Eigen::Map<const Vector3>(&
m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ(0)));
397 catch (
const std::exception& e)
399 std::cout << e.what();
400 SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
410 return SEND_MSG(
"windows_size should be a positive integer", MSG_TYPE_ERROR);
417 return SEND_MSG(
"alpha should be in [0..1]", MSG_TYPE_ERROR);
425 return SEND_MSG(
"alpha should be in [0..1]", MSG_TYPE_ERROR);
432 return SEND_MSG(
"alpha should be in [0..1]", MSG_TYPE_ERROR);
445 return SEND_MSG(
"Imu weight must be nonnegative", MSG_TYPE_ERROR);
452 return SEND_MSG(
"Stiffness vector should have size 6, not "+toString(k.size()), MSG_TYPE_ERROR);
459 return SEND_MSG(
"Stiffness vector should have size 6, not "+toString(k.size()), MSG_TYPE_ERROR);
466 return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
473 return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
480 return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
487 return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
494 return SEND_MSG(
"Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
501 return SEND_MSG(
"Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
527 pinocchio::Force f(w);
529 if(f.linear()[2]==0.0)
531 zmp[0] = -f.angular()[1] / f.linear()[2];
532 zmp[1] = f.angular()[0] / f.linear()[2];
536 double std_dev,
double margin)
538 double fs0=foot_sizes[0] - margin;
539 double fs1=foot_sizes[1] + margin;
540 double fs2=foot_sizes[2] - margin;
541 double fs3=foot_sizes[3] + margin;
543 if(zmp[0]>fs0 || zmp[0]<fs1 || zmp[1]>fs2 || zmp[1]<fs3)
546 double cdx = ((cdf(
m_normal, (fs0-zmp[0])/std_dev) -
547 cdf(
m_normal, (fs1-zmp[0])/std_dev))-0.5 )*2.0;
548 double cdy = ((cdf(
m_normal, (fs2-zmp[1])/std_dev) -
549 cdf(
m_normal, (fs3-zmp[1])/std_dev))-0.5 )*2.0;
557 return (cdf(
m_normal, (fz-margin)/std_dev)-0.5)*2.0;
563 SE3 dummy, dummy1, lfMff, rfMff;
570 const Vector3 & ankle_2_sole_xyz =
m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ;
571 const SE3 groundMfoot(Matrix3::Identity(), -1.0*ankle_2_sole_xyz);
572 lfMff = groundMfoot * lfMff;
573 rfMff = groundMfoot * rfMff;
576 const Vector3 w( 0.5*(pinocchio::log3(lfMff.rotation())+pinocchio::log3(rfMff.rotation())) );
577 SE3 oMff = SE3(pinocchio::exp3(w), 0.5*(lfMff.translation()+rfMff.translation()));
581 m_oMlfs = oMff * lfMff.inverse() * groundMfoot;
582 m_oMrfs = oMff * rfMff.inverse() * groundMfoot;
585 Eigen::Quaternion<double> quat_lf(
m_oMlfs.rotation());
592 Eigen::Quaternion<double> quat_rf(
m_oMrfs.rotation());
602 sendMsg(
"Reference pos of left foot:\n"+toString(
m_oMlfs), MSG_TYPE_INFO);
603 sendMsg(
"Reference pos of right foot:\n"+toString(
m_oMrfs), MSG_TYPE_INFO);
615 const SE3 & oMfs,
const pinocchio::FrameIndex foot_id,
616 SE3 & oMff, SE3 & oMfa, SE3 & fsMff)
619 xyz << -ft[0]/K(0), -ft[1]/K(1), -ft[2]/K(2);
621 rpyToMatrix(-ft[3]/K(3), -ft[4]/K(4), -ft[5]/K(5), R);
622 const SE3 fsMfa(R, xyz);
624 const SE3 faMff(
m_data->oMf[foot_id].inverse());
639 SEND_WARNING_STREAM_MSG(
"Cannot compute signal kinematics_computations before initialization!");
643 const Eigen::VectorXd& qj= m_joint_positionsSIN(iter);
644 const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
645 assert(qj.size()==
m_robot_util->m_nbJoints &&
"Unexpected size of signal joint_positions");
646 assert(dq.size()==
m_robot_util->m_nbJoints &&
"Unexpected size of signal joint_velocities");
670 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q before initialization!");
676 const Eigen::VectorXd & qj = m_joint_positionsSIN(iter);
677 const Eigen::Vector4d & quatIMU_vec = m_imu_quaternionSIN(iter);
678 const Vector6 & ftrf = m_forceRLEGSIN(iter);
679 const Vector6 & ftlf = m_forceLLEGSIN(iter);
685 if(m_w_lf_inSIN.isPlugged())
687 wL = m_w_lf_inSIN(iter);
691 wL = m_w_lf_filteredSOUT(iter);
695 if(m_w_rf_inSIN.isPlugged())
697 wR = m_w_rf_inSIN(iter);
701 wR = m_w_rf_filteredSOUT(iter);
706 assert(qj.size()==
m_robot_util->m_nbJoints &&
"Unexpected size of signal joint_positions");
709 if(wR==0.0 && wL==0.0)
711 SEND_WARNING_STREAM_MSG(
"The robot is flying!"+
712 (
"- forceRLEG: "+toString(ftrf.transpose()))+
713 "- forceLLEG: "+toString(ftlf.transpose())+
720 m_kinematics_computationsSINNER(iter);
727 SE3 oMlfa, oMrfa, lfsMff, rfsMff;
733 const SE3 torsoMff(ffMtorso.inverse());
735 Vector3 rpy_torso, rpy_torso_lf, rpy_torso_rf, rpy_torso_imu;
739 Eigen::Quaterniond quatIMU(quatIMU_vec[0], quatIMU_vec[1], quatIMU_vec[2], quatIMU_vec[3]);
740 Eigen::Quaterniond quat_torsoMimu(
m_torsoMimu.rotation());
741 Eigen::Quaterniond quat_torso_imu(quatIMU*quat_torsoMimu);
743 matrixToRpy(quat_torso_imu.toRotationMatrix(), rpy_torso_imu);
747 rpy_torso[0] =
wEulerMean(rpy_torso_lf[0], rpy_torso_rf[0], wL, wR);
749 rpy_torso[1] =
wEulerMean(rpy_torso_lf[1], rpy_torso_rf[1], wL, wR);
751 rpy_torso[2] =
wEulerMean(rpy_torso_lf[2], rpy_torso_rf[2], wL, wR);
761 m_q_pin.head<3>() = ((oMlfa.translation() - pos_lf_ff)*wL +
762 (oMrfa.translation() - pos_rf_ff)*wR) / (wL+wR);
773 if(m_K_fb_feet_posesSIN.isPlugged())
775 const double K_fb = m_K_fb_feet_posesSIN(iter);
776 if (K_fb > 0.0 &&
m_w_imu > 0.0)
778 assert(
m_w_imu > 0.0 &&
"Update of the feet 6d poses should not be done if wIMU = 0.0");
779 assert(K_fb < 1.0 &&
"Feedback gain on foot correction should be less than 1.0 (K_fb_feet_poses>1.0)");
781 const SE3 oMlfs_est( oMff_est*(lfsMff.inverse()) );
782 const SE3 oMrfs_est( oMff_est*(rfsMff.inverse()) );
784 SE3 leftDrift =
m_oMlfs.inverse()*oMlfs_est;
785 SE3 rightDrift =
m_oMrfs.inverse()*oMrfs_est;
789 SE3 rightDrift_delta;
790 se3Interp(leftDrift ,SE3::Identity(),K_fb*wR,leftDrift_delta);
791 se3Interp(rightDrift,SE3::Identity(),K_fb*wL,rightDrift_delta);
794 rightDrift_delta = rightDrift;
796 leftDrift_delta = leftDrift;
800 rightDrift_delta = SE3::Identity();
801 leftDrift_delta = SE3::Identity();
806 SE3 oMlfs_ref, oMrfs_ref;
807 if (m_lf_ref_xyzquatSIN.isPlugged() and
808 m_rf_ref_xyzquatSIN.isPlugged())
811 const Vector7 & lf_ref_xyzquat_vec = m_lf_ref_xyzquatSIN(iter);
812 const Vector7 & rf_ref_xyzquat_vec = m_rf_ref_xyzquatSIN(iter);
813 const Eigen::Quaterniond ql(m_lf_ref_xyzquatSIN(iter)(3),
814 m_lf_ref_xyzquatSIN(iter)(4),
815 m_lf_ref_xyzquatSIN(iter)(5),
816 m_lf_ref_xyzquatSIN(iter)(6));
817 const Eigen::Quaterniond qr(m_rf_ref_xyzquatSIN(iter)(3),
818 m_rf_ref_xyzquatSIN(iter)(4),
819 m_rf_ref_xyzquatSIN(iter)(5),
820 m_rf_ref_xyzquatSIN(iter)(6));
821 oMlfs_ref = SE3(ql.toRotationMatrix(), lf_ref_xyzquat_vec.head<3>());
822 oMrfs_ref = SE3(qr.toRotationMatrix(), rf_ref_xyzquat_vec.head<3>());
830 const Vector3 translation_feet_drift = 0.5*( ( oMlfs_ref.translation() -
m_oMlfs.translation()) +
831 ( oMrfs_ref.translation() -
m_oMrfs.translation()) );
833 const Vector3 V_ref = oMrfs_ref.translation() - oMlfs_ref.translation();
836 const double yaw_drift = (atan2(V_ref(1), V_ref(0)) -
837 atan2(V_est(1), V_est(0)));
840 const Vector3 rpy_feet_drift(0.,0.,yaw_drift);
841 Matrix3 rotation_feet_drift;
843 const SE3 drift_to_ref(rotation_feet_drift , translation_feet_drift);
850 Eigen::Quaternion<double> quat_lf(
m_oMlfs.rotation());
857 Eigen::Quaternion<double> quat_rf(
m_oMrfs.rotation());
871 SEND_WARNING_STREAM_MSG(
"Cannot compute signal lf_xyzquat before initialization!");
876 const Eigen::VectorXd & q = m_qSOUT(iter);
886 SEND_WARNING_STREAM_MSG(
"Cannot compute signal rf_xyzquat before initialization!");
891 const Eigen::VectorXd & q = m_qSOUT(iter);
901 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_lf before initialization!");
907 const Eigen::VectorXd & q = m_qSOUT(iter);
918 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_rf before initialization!");
924 const Eigen::VectorXd & q = m_qSOUT(iter);
935 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_imu before initialization!");
941 const Eigen::VectorXd & q = m_qSOUT(iter);
944 const SE3 torsoMff(ffMtorso.inverse());
945 const Eigen::Vector4d & quatIMU_vec = m_imu_quaternionSIN(iter);
947 Eigen::Quaterniond quatIMU(quatIMU_vec[0], quatIMU_vec[1], quatIMU_vec[2], quatIMU_vec[3]);
948 Eigen::Quaterniond quat_torsoMimu(
m_torsoMimu.rotation());
949 Eigen::Quaterniond quat_torso_imu(quatIMU*quat_torsoMimu);
951 base_se3_to_sot(q.head<3>(),
m_oRff, s.head<6>());
961 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_lf before initialization!");
965 const Vector6 & wrench = m_forceLLEGSIN(iter);
994 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_rf before initialization!");
998 const Vector6 & wrench = m_forceRLEGSIN(iter);
1028 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_rf_filtered before initialization!");
1031 double w_rf = m_w_rfSOUT(iter);
1041 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_lf_filtered before initialization!");
1044 double w_lf = m_w_lfSOUT(iter);
1055 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v before initialization!");
1061 m_kinematics_computationsSINNER(iter);
1066 const Eigen::VectorXd & dq = m_joint_velocitiesSIN(iter);
1067 const Eigen::Vector3d & acc_imu = m_accelerometerSIN(iter);
1068 const Eigen::Vector3d & gyr_imu = m_gyroscopeSIN(iter);
1070 const Vector6 & dftrf = m_dforceRLEGSIN(iter);
1071 const Vector6 & dftlf = m_dforceLLEGSIN(iter);
1072 assert(dq.size()==
m_robot_util->m_nbJoints &&
"Unexpected size of signal joint_velocities");
1078 if(m_w_lf_inSIN.isPlugged())
1080 wL = m_w_lf_inSIN(iter);
1084 wL = m_w_lf_filteredSOUT(iter);
1088 if(m_w_rf_inSIN.isPlugged())
1090 wR = m_w_rf_inSIN(iter);
1094 wR = m_w_rf_filteredSOUT(iter);
1099 if(wR==0.0 && wL==0.0)
1107 const Motion & v_lf_local =
m_data->v[f_lf.parent];
1108 const SE3 & ffMlf =
m_data->oMi[f_lf.parent];
1109 Vector6 v_kin_l = -ffMlf.act(v_lf_local).toVector();
1110 v_kin_l.head<3>() =
m_oRff * v_kin_l.head<3>();
1111 v_kin_l.segment<3>(3) =
m_oRff * v_kin_l.segment<3>(3);
1114 const Motion & v_rf_local =
m_data->v[f_rf.parent];
1115 const SE3 & ffMrf =
m_data->oMi[f_rf.parent];
1116 Vector6 v_kin_r = -ffMrf.act(v_rf_local).toVector();
1117 v_kin_r.head<3>() =
m_oRff * v_kin_r.head<3>();
1118 v_kin_r.segment<3>(3) =
m_oRff * v_kin_r.segment<3>(3);
1120 m_v_kin.head<6>() = (wR*v_kin_r + wL*v_kin_l)/(wL+wR);
1129 const Eigen::Matrix<double, 6, 6> lfAff = ffMlf.inverse().toActionMatrix();
1130 const Eigen::Matrix<double, 6, 6> rfAff = ffMrf.inverse().toActionMatrix();
1131 Eigen::Matrix<double, 12, 6> A;
1134 Eigen::Matrix<double, 12, 1> b;
1138 m_v_flex.head<6>() = (A.transpose() * A).ldlt().solve(A.transpose() * b);
1144 const Matrix3 & ffRtorso = ffMtorso.rotation();
1145 const Matrix3 ffRimu = ffRtorso *
m_torsoMimu.rotation();
1149 const Matrix3 lfRimu = ffMlf.rotation().transpose() * ffRimu;
1150 const Matrix3 rfRimu = ffMrf.rotation().transpose() * ffRimu;
1160 Motion v_gyr_ankle_l(
Vector3(0.,0.,0.), lfRimu * gVo_a_l);
1161 Motion v_gyr_ankle_r(
Vector3(0.,0.,0.), rfRimu * gVo_a_r);
1162 Vector6 v_gyr_l = ffMlf.inverse().actInv(v_gyr_ankle_l).toVector();
1163 Vector6 v_gyr_r = ffMrf.inverse().actInv(v_gyr_ankle_r).toVector();
1164 m_v_gyr.head<6>() = (wL*v_gyr_l + wR*v_gyr_r)/(wL+wR);
1185 sendMsg(
"iter:"+toString(iter)+
"\n", MSG_TYPE_INFO);
1190 const Vector3 ACacc = acc_world - DCacc;
1199 const Vector3 ACvel = vel - DCvel;
1205 const Motion imuWimu(imuVimu,gyr_imu);
1211 const Motion v= ffMimu.act(imuWimu) ;
1212 m_v_imu.head<6>() = v.toVector();
1238 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_kin before initialization!");
1250 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_flex before initialization!");
1262 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_imu before initialization!");
1274 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_gyr before initialization!");
1286 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_ac before initialization!");
1298 SEND_WARNING_STREAM_MSG(
"Cannot compute signal a_ac before initialization!");
1314 os <<
"TalosBaseEstimator "<<getName();
1317 getProfiler().report_all(3, os);
1319 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)
void pointRotationByQuaternion(const Eigen::Vector3d &point, const Eigen::Vector4d &quat, Eigen::Vector3d &rotatedPoint)
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
void quanternionMult(const Eigen::Vector4d &q1, const Eigen::Vector4d &q2, Eigen::Vector4d &q12)
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)