6 #include "pinocchio/algorithm/frames.hpp" 7 #include <dynamic-graph/factory.h> 8 #include <sot/core/debug.hh> 12 #include <sot/core/stop-watch.hh> 17 namespace dg = ::dynamicgraph;
22 using boost::math::normal;
24 void se3Interp(
const pinocchio::SE3& s1,
const pinocchio::SE3& s2,
const double alpha, pinocchio::SE3& s12) {
25 const Eigen::Vector3d t_(s1.translation() * alpha + s2.translation() * (1 - alpha));
27 const Eigen::Vector3d w(pinocchio::log3(s1.rotation()) * alpha + pinocchio::log3(s2.rotation()) * (1 - alpha));
29 s12 = pinocchio::SE3(pinocchio::exp3(w), t_);
31 void rpyToMatrix(
double roll,
double pitch,
double yaw, Eigen::Matrix3d& R) {
32 Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
33 Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
34 Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
35 Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
41 void matrixToRpy(
const Eigen::Matrix3d& M, Eigen::Vector3d& rpy) {
42 double m = sqrt(M(2, 1) * M(2, 1) + M(2, 2) * M(2, 2));
43 rpy(1) = atan2(-M(2, 0), m);
44 if (fabs(fabs(rpy(1)) - 0.5 * M_PI) < 0.001) {
46 rpy(2) = -atan2(M(0, 1), M(1, 1));
48 rpy(2) = atan2(M(1, 0), M(0, 0));
49 rpy(0) = atan2(M(2, 1), M(2, 2));
53 void quanternionMult(
const Eigen::Vector4d& q1,
const Eigen::Vector4d& q2, Eigen::Vector4d& q12) {
54 q12(0) = q2(0) * q1(0) - q2(1) * q1(1) - q2(2) * q1(2) - q2(3) * q1(3);
55 q12(1) = q2(0) * q1(1) + q2(1) * q1(0) - q2(2) * q1(3) + q2(3) * q1(2);
56 q12(2) = q2(0) * q1(2) + q2(1) * q1(3) + q2(2) * q1(0) - q2(3) * q1(1);
57 q12(3) = q2(0) * q1(3) - q2(1) * q1(2) + q2(2) * q1(1) + q2(3) * q1(0);
61 Eigen::Vector3d& rotatedPoint) {
62 const Eigen::Vector4d p4(0.0, point(0), point(1), point(2));
63 const Eigen::Vector4d quat_conj(quat(0), -quat(1), -quat(2), -quat(3));
64 Eigen::Vector4d q_tmp1, q_tmp2;
67 rotatedPoint(0) = q_tmp2(1);
68 rotatedPoint(1) = q_tmp2(2);
69 rotatedPoint(2) = q_tmp2(3);
72 #define PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation" 73 #define PROFILE_BASE_VELOCITY_ESTIMATION "base-est velocity estimation" 74 #define PROFILE_BASE_KINEMATICS_COMPUTATION "base-est kinematics computation" 76 #define INPUT_SIGNALS \ 77 m_joint_positionsSIN << m_joint_velocitiesSIN << m_imu_quaternionSIN << m_forceLLEGSIN << m_forceRLEGSIN \ 78 << m_dforceLLEGSIN << m_dforceRLEGSIN << m_w_lf_inSIN << m_w_rf_inSIN << m_K_fb_feet_posesSIN \ 79 << m_lf_ref_xyzquatSIN << m_rf_ref_xyzquatSIN << m_accelerometerSIN << m_gyroscopeSIN 80 #define OUTPUT_SIGNALS \ 81 m_qSOUT << m_vSOUT << m_v_kinSOUT << m_v_flexSOUT << m_v_imuSOUT << m_v_gyrSOUT << m_lf_xyzquatSOUT \ 82 << m_rf_xyzquatSOUT << m_a_acSOUT << m_v_acSOUT << m_q_lfSOUT << m_q_rfSOUT << m_q_imuSOUT << m_w_lfSOUT \ 83 << m_w_rfSOUT << m_w_lf_filteredSOUT << m_w_rf_filteredSOUT 97 CONSTRUCT_SIGNAL_IN(joint_positions,
dynamicgraph::Vector),
98 CONSTRUCT_SIGNAL_IN(joint_velocities,
dynamicgraph::Vector),
99 CONSTRUCT_SIGNAL_IN(imu_quaternion,
dynamicgraph::Vector),
104 CONSTRUCT_SIGNAL_IN(w_lf_in, double),
105 CONSTRUCT_SIGNAL_IN(w_rf_in, double),
106 CONSTRUCT_SIGNAL_IN(K_fb_feet_poses, double),
107 CONSTRUCT_SIGNAL_IN(lf_ref_xyzquat,
dynamicgraph::Vector),
108 CONSTRUCT_SIGNAL_IN(rf_ref_xyzquat,
dynamicgraph::Vector),
109 CONSTRUCT_SIGNAL_IN(accelerometer,
dynamicgraph::Vector),
111 CONSTRUCT_SIGNAL_INNER(kinematics_computations,
dynamicgraph::Vector,
112 m_joint_positionsSIN << m_joint_velocitiesSIN),
114 m_kinematics_computationsSINNER
115 << m_joint_positionsSIN << m_imu_quaternionSIN << m_forceLLEGSIN << m_forceRLEGSIN
116 << m_w_lf_inSIN << m_w_rf_inSIN << m_K_fb_feet_posesSIN << m_w_lf_filteredSOUT
117 << m_w_rf_filteredSOUT << m_lf_ref_xyzquatSIN << m_rf_ref_xyzquatSIN),
119 m_kinematics_computationsSINNER << m_accelerometerSIN << m_gyroscopeSIN << m_qSOUT
120 << m_dforceLLEGSIN << m_dforceRLEGSIN),
121 CONSTRUCT_SIGNAL_OUT(v_kin,
dynamicgraph::Vector, m_vSOUT),
122 CONSTRUCT_SIGNAL_OUT(v_flex,
dynamicgraph::Vector, m_vSOUT),
123 CONSTRUCT_SIGNAL_OUT(v_imu,
dynamicgraph::Vector, m_vSOUT),
124 CONSTRUCT_SIGNAL_OUT(v_gyr,
dynamicgraph::Vector, m_vSOUT),
125 CONSTRUCT_SIGNAL_OUT(lf_xyzquat,
dynamicgraph::Vector, m_qSOUT),
126 CONSTRUCT_SIGNAL_OUT(rf_xyzquat,
dynamicgraph::Vector, m_qSOUT),
127 CONSTRUCT_SIGNAL_OUT(a_ac,
dynamicgraph::Vector, m_vSOUT),
128 CONSTRUCT_SIGNAL_OUT(v_ac,
dynamicgraph::Vector, m_vSOUT),
129 CONSTRUCT_SIGNAL_OUT(q_lf,
dynamicgraph::Vector, m_qSOUT),
130 CONSTRUCT_SIGNAL_OUT(q_rf,
dynamicgraph::Vector, m_qSOUT),
131 CONSTRUCT_SIGNAL_OUT(q_imu,
dynamicgraph::Vector, m_qSOUT << m_imu_quaternionSIN),
132 CONSTRUCT_SIGNAL_OUT(w_lf, double, m_forceLLEGSIN),
133 CONSTRUCT_SIGNAL_OUT(w_rf, double, m_forceRLEGSIN),
134 CONSTRUCT_SIGNAL_OUT(w_lf_filtered, double, m_w_lfSOUT),
135 CONSTRUCT_SIGNAL_OUT(w_rf_filtered, double, m_w_rfSOUT),
136 m_initSucceeded(false),
137 m_reset_foot_pos(true),
139 m_zmp_std_dev_rf(1.0),
140 m_zmp_std_dev_lf(1.0),
141 m_fz_std_dev_rf(1.0),
142 m_fz_std_dev_lf(1.0),
143 m_zmp_margin_lf(0.0),
144 m_zmp_margin_rf(0.0) {
147 m_K_rf << 4034, 23770, 239018, 707, 502, 936;
148 m_K_lf << 4034, 23770, 239018, 707, 502, 936;
153 addCommand(
"init", makeCommandVoid2(
155 docCommandVoid2(
"Initialize the entity.",
"time step (double)",
"URDF file path (string)")));
157 addCommand(
"set_fz_stable_windows_size",
159 docCommandVoid1(
"Set the windows size used to detect that feet is stable",
"int")));
160 addCommand(
"set_alpha_w_filter",
162 docCommandVoid1(
"Set the filter parameter to filter weights",
"double")));
166 docCommandVoid1(
"Set the filter parameter for removing DC from accelerometer data",
"double")));
167 addCommand(
"set_alpha_DC_vel",
170 docCommandVoid1(
"Set the filter parameter for removing DC from velocity integrated from acceleration",
173 docCommandVoid0(
"Reset the position of the feet.")));
174 addCommand(
"get_imu_weight",
175 makeDirectGetter(*
this, &
m_w_imu,
176 docDirectGetter(
"Weight of imu-based orientation in sensor fusion",
"double")));
177 addCommand(
"set_imu_weight",
179 docCommandVoid1(
"Set the weight of imu-based orientation in sensor fusion",
"double")));
181 "set_zmp_std_dev_right_foot",
184 docCommandVoid1(
"Set the standard deviation of the ZMP measurement errors for the right foot",
"double")));
186 "set_zmp_std_dev_left_foot",
189 docCommandVoid1(
"Set the standard deviation of the ZMP measurement errors for the left foot",
"double")));
191 "set_normal_force_std_dev_right_foot",
194 docCommandVoid1(
"Set the standard deviation of the normal force measurement errors for the right foot",
196 addCommand(
"set_normal_force_std_dev_left_foot",
199 docCommandVoid1(
"Set the standard deviation of the normal force measurement errors for the left foot",
201 addCommand(
"set_stiffness_right_foot",
203 docCommandVoid1(
"Set the 6d stiffness of the spring at the right foot",
"vector")));
204 addCommand(
"set_stiffness_left_foot",
206 docCommandVoid1(
"Set the 6d stiffness of the spring at the left foot",
"vector")));
208 "set_right_foot_sizes",
210 docCommandVoid1(
"Set the size of the right foot (pos x, neg x, pos y, neg y)",
"4d vector")));
212 "set_left_foot_sizes",
214 docCommandVoid1(
"Set the size of the left foot (pos x, neg x, pos y, neg y)",
"4d vector")));
215 addCommand(
"set_zmp_margin_right_foot",
217 docCommandVoid1(
"Set the ZMP margin for the right foot",
"double")));
218 addCommand(
"set_zmp_margin_left_foot",
220 docCommandVoid1(
"Set the ZMP margin for the left foot",
"double")));
221 addCommand(
"set_normal_force_margin_right_foot",
223 docCommandVoid1(
"Set the normal force margin for the right foot",
"double")));
224 addCommand(
"set_normal_force_margin_left_foot",
226 docCommandVoid1(
"Set the normal force margin for the left foot",
"double")));
233 std::string localName(robotRef);
234 if (isNameInRobotUtil(localName)) {
236 std::cerr <<
"m_robot_util:" <<
m_robot_util << std::endl;
238 SEND_MSG(
"You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
242 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(),
m_model);
261 -Eigen::Map<const Vector3>(&
m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ(0)));
276 }
catch (
const std::exception& e) {
277 std::cout << e.what();
278 SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
286 if (ws < 0.0)
return SEND_MSG(
"windows_size should be a positive integer", MSG_TYPE_ERROR);
291 if (a < 0.0 || a > 1.0)
return SEND_MSG(
"alpha should be in [0..1]", MSG_TYPE_ERROR);
296 if (a < 0.0 || a > 1.0)
return SEND_MSG(
"alpha should be in [0..1]", MSG_TYPE_ERROR);
301 if (a < 0.0 || a > 1.0)
return SEND_MSG(
"alpha should be in [0..1]", MSG_TYPE_ERROR);
308 if (w < 0.0)
return SEND_MSG(
"Imu weight must be nonnegative", MSG_TYPE_ERROR);
313 if (k.size() != 6)
return SEND_MSG(
"Stiffness vector should have size 6, not " + toString(k.size()), MSG_TYPE_ERROR);
318 if (k.size() != 6)
return SEND_MSG(
"Stiffness vector should have size 6, not " + toString(k.size()), MSG_TYPE_ERROR);
323 if (std_dev <= 0.0)
return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
328 if (std_dev <= 0.0)
return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
333 if (std_dev <= 0.0)
return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
338 if (std_dev <= 0.0)
return SEND_MSG(
"Standard deviation must be a positive number", MSG_TYPE_ERROR);
343 if (s.size() != 4)
return SEND_MSG(
"Foot size vector should have size 4, not " + toString(s.size()), MSG_TYPE_ERROR);
348 if (s.size() != 4)
return SEND_MSG(
"Foot size vector should have size 4, not " + toString(s.size()), MSG_TYPE_ERROR);
361 pinocchio::Force f(w);
363 if (f.linear()[2] == 0.0)
return;
364 zmp[0] = -f.angular()[1] / f.linear()[2];
365 zmp[1] = f.angular()[0] / f.linear()[2];
370 double fs0 = foot_sizes[0] - margin;
371 double fs1 = foot_sizes[1] + margin;
372 double fs2 = foot_sizes[2] - margin;
373 double fs3 = foot_sizes[3] + margin;
375 if (zmp[0] > fs0 || zmp[0] < fs1 || zmp[1] > fs2 || zmp[1] < fs3)
return 0;
377 double cdx = ((cdf(
m_normal, (fs0 - zmp[0]) / std_dev) - cdf(
m_normal, (fs1 - zmp[0]) / std_dev)) - 0.5) * 2.0;
378 double cdy = ((cdf(
m_normal, (fs2 - zmp[1]) / std_dev) - cdf(
m_normal, (fs3 - zmp[1]) / std_dev)) - 0.5) * 2.0;
383 if (fz < margin)
return 0.0;
384 return (cdf(
m_normal, (fz - margin) / std_dev) - 0.5) * 2.0;
389 SE3
dummy, dummy1, lfMff, rfMff;
397 const Vector3& ankle_2_sole_xyz =
m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ;
398 const SE3 groundMfoot(Matrix3::Identity(), -1.0 * ankle_2_sole_xyz);
399 lfMff = groundMfoot * lfMff;
400 rfMff = groundMfoot * rfMff;
403 const Vector3 w(0.5 * (pinocchio::log3(lfMff.rotation()) + pinocchio::log3(rfMff.rotation())));
404 SE3 oMff = SE3(pinocchio::exp3(w), 0.5 * (lfMff.translation() + rfMff.translation()));
406 oMff.translation()(0) += 9.562e-03;
408 m_oMlfs = oMff * lfMff.inverse() * groundMfoot;
409 m_oMrfs = oMff * rfMff.inverse() * groundMfoot;
412 Eigen::Quaternion<double> quat_lf(
m_oMlfs.rotation());
419 Eigen::Quaternion<double> quat_rf(
m_oMrfs.rotation());
429 sendMsg(
"Reference pos of left foot:\n" + toString(
m_oMlfs), MSG_TYPE_INFO);
430 sendMsg(
"Reference pos of right foot:\n" + toString(
m_oMrfs), MSG_TYPE_INFO);
443 SE3& oMff, SE3& oMfa, SE3& fsMff) {
445 xyz << -ft[0] / K(0), -ft[1] / K(1), -ft[2] / K(2);
447 rpyToMatrix(-ft[3] / K(3), -ft[4] / K(4), -ft[5] / K(5), R);
448 const SE3 fsMfa(R, xyz);
450 const SE3 faMff(
m_data->oMf[foot_id].inverse());
454 fsMff = fsMfa * faMff;
462 if (!m_initSucceeded) {
463 SEND_WARNING_STREAM_MSG(
"Cannot compute signal kinematics_computations before initialization!");
467 const Eigen::VectorXd& qj = m_joint_positionsSIN(iter);
468 const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
469 assert(qj.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
470 "Unexpected size of signal joint_positions");
471 assert(dq.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
472 "Unexpected size of signal joint_velocities");
475 m_robot_util->joints_sot_to_urdf(qj, m_q_pin.tail(m_robot_util->m_nbJoints));
476 m_robot_util->joints_sot_to_urdf(dq, m_v_pin.tail(m_robot_util->m_nbJoints));
481 m_q_pin.head<6>().setZero();
483 m_v_pin.head<6>().setZero();
484 pinocchio::forwardKinematics(m_model, *m_data, m_q_pin, m_v_pin);
485 pinocchio::framesForwardKinematics(m_model, *m_data);
493 if (!m_initSucceeded) {
494 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q before initialization!");
497 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6))
498 s.resize(m_robot_util->m_nbJoints + 6);
500 const Eigen::VectorXd& qj = m_joint_positionsSIN(iter);
501 const Eigen::Vector4d& quatIMU_vec = m_imu_quaternionSIN(iter);
502 const Vector6& ftrf = m_forceRLEGSIN(iter);
503 const Vector6& ftlf = m_forceLLEGSIN(iter);
509 if (m_w_lf_inSIN.isPlugged())
510 wL = m_w_lf_inSIN(iter);
512 wL = m_w_lf_filteredSOUT(iter);
513 if (m_left_foot_is_stable ==
false) wL = 0.0;
515 if (m_w_rf_inSIN.isPlugged())
516 wR = m_w_rf_inSIN(iter);
518 wR = m_w_rf_filteredSOUT(iter);
519 if (m_right_foot_is_stable ==
false) wR = 0.0;
522 assert(qj.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
523 "Unexpected size of signal joint_positions");
526 if (wR == 0.0 && wL == 0.0) {
527 SEND_WARNING_STREAM_MSG(
"The robot is flying!" + (
"- forceRLEG: " + toString(ftrf.transpose())) +
528 "- forceLLEG: " + toString(ftlf.transpose()) +
529 "- m_right_foot_is_stable: " + toString(m_right_foot_is_stable) +
530 "- m_left_foot_is_stable: " + toString(m_left_foot_is_stable));
535 m_kinematics_computationsSINNER(iter);
537 if (m_reset_foot_pos) reset_foot_positions_impl(ftlf, ftrf);
541 SE3 oMlfa, oMrfa, lfsMff, rfsMff;
542 kinematics_estimation(ftrf, m_K_rf, m_oMrfs, static_cast<int>(m_right_foot_id), m_oMff_rf, oMrfa, rfsMff);
543 kinematics_estimation(ftlf, m_K_lf, m_oMlfs, static_cast<int>(m_left_foot_id), m_oMff_lf, oMlfa, lfsMff);
546 const SE3 ffMchest(m_data->oMf[m_IMU_body_id]);
547 const SE3 chestMff(ffMchest.inverse());
549 Vector3 rpy_chest, rpy_chest_lf, rpy_chest_rf,
553 matrixToRpy((m_oMff_lf * ffMchest).rotation(), rpy_chest_lf);
554 matrixToRpy((m_oMff_rf * ffMchest).rotation(), rpy_chest_rf);
555 Eigen::Quaternion<double> quatIMU(quatIMU_vec[0], quatIMU_vec[1], quatIMU_vec[2], quatIMU_vec[3]);
556 matrixToRpy(quatIMU.toRotationMatrix(), rpy_chest_imu);
559 double wSum = wL + wR + m_w_imu;
560 rpy_chest(0) = (rpy_chest_lf[0] * wL + rpy_chest_rf[0] * wR + rpy_chest_imu[0] * m_w_imu) / wSum;
561 rpy_chest(1) = (rpy_chest_lf[1] * wL + rpy_chest_rf[1] * wR + rpy_chest_imu[1] * m_w_imu) / wSum;
562 rpy_chest(2) = (rpy_chest_lf[2] * wL + rpy_chest_rf[2] * wR) / (wL + wR);
565 m_oRff = m_oRchest * chestMff.rotation();
569 const Vector3 pos_lf_ff = m_oRff * m_data->oMf[m_left_foot_id].translation();
570 const Vector3 pos_rf_ff = m_oRff * m_data->oMf[m_right_foot_id].translation();
572 m_q_pin.head<3>() = ((oMlfa.translation() - pos_lf_ff) * wL + (oMrfa.translation() - pos_rf_ff) * wR) / (wL + wR);
574 m_q_sot.tail(m_robot_util->m_nbJoints) = qj;
575 base_se3_to_sot(m_q_pin.head<3>(), m_oRff, m_q_sot.head<6>());
580 const SE3 oMff_est(m_oRff, m_q_pin.head<3>());
583 if (m_K_fb_feet_posesSIN.isPlugged()) {
584 const double K_fb = m_K_fb_feet_posesSIN(iter);
585 if (K_fb > 0.0 && m_w_imu > 0.0) {
586 assert(m_w_imu > 0.0 &&
"Update of the feet 6d poses should not be done if wIMU = 0.0");
587 assert(K_fb < 1.0 &&
"Feedback gain on foot correction should be less than 1.0 (K_fb_feet_poses>1.0)");
589 const SE3 oMlfs_est(oMff_est * (lfsMff.inverse()));
590 const SE3 oMrfs_est(oMff_est * (rfsMff.inverse()));
592 SE3 leftDrift = m_oMlfs.inverse() * oMlfs_est;
593 SE3 rightDrift = m_oMrfs.inverse() * oMrfs_est;
597 SE3 rightDrift_delta;
598 se3Interp(leftDrift, SE3::Identity(), K_fb * wR, leftDrift_delta);
599 se3Interp(rightDrift, SE3::Identity(), K_fb * wL, rightDrift_delta);
601 if (m_right_foot_is_stable ==
false) rightDrift_delta = rightDrift;
602 if (m_left_foot_is_stable ==
false) leftDrift_delta = leftDrift;
603 if (m_right_foot_is_stable ==
false && m_left_foot_is_stable ==
false) {
605 rightDrift_delta = SE3::Identity();
606 leftDrift_delta = SE3::Identity();
608 m_oMlfs = m_oMlfs * leftDrift_delta;
609 m_oMrfs = m_oMrfs * rightDrift_delta;
611 SE3 oMlfs_ref, oMrfs_ref;
612 if (m_lf_ref_xyzquatSIN.isPlugged() and m_rf_ref_xyzquatSIN.isPlugged()) {
614 const Vector7& lf_ref_xyzquat_vec = m_lf_ref_xyzquatSIN(iter);
615 const Vector7& rf_ref_xyzquat_vec = m_rf_ref_xyzquatSIN(iter);
616 const Eigen::Quaterniond ql(m_lf_ref_xyzquatSIN(iter)(3), m_lf_ref_xyzquatSIN(iter)(4),
617 m_lf_ref_xyzquatSIN(iter)(5), m_lf_ref_xyzquatSIN(iter)(6));
618 const Eigen::Quaterniond qr(m_rf_ref_xyzquatSIN(iter)(3), m_rf_ref_xyzquatSIN(iter)(4),
619 m_rf_ref_xyzquatSIN(iter)(5), m_rf_ref_xyzquatSIN(iter)(6));
620 oMlfs_ref = SE3(ql.toRotationMatrix(), lf_ref_xyzquat_vec.head<3>());
621 oMrfs_ref = SE3(qr.toRotationMatrix(), rf_ref_xyzquat_vec.head<3>());
623 oMlfs_ref = m_oMlfs_default_ref;
624 oMrfs_ref = m_oMrfs_default_ref;
627 const Vector3 translation_feet_drift = 0.5 * ((oMlfs_ref.translation() - m_oMlfs.translation()) +
628 (oMrfs_ref.translation() - m_oMrfs.translation()));
630 const Vector3 V_ref = oMrfs_ref.translation() - oMlfs_ref.translation();
631 const Vector3 V_est = m_oMrfs.translation() - m_oMlfs.translation();
633 const double yaw_drift = (atan2(V_ref(1), V_ref(0)) - atan2(V_est(1), V_est(0)));
636 const Vector3 rpy_feet_drift(0., 0., yaw_drift);
637 Matrix3 rotation_feet_drift;
639 const SE3 drift_to_ref(rotation_feet_drift, translation_feet_drift);
640 m_oMlfs = m_oMlfs * drift_to_ref;
641 m_oMrfs = m_oMrfs * drift_to_ref;
645 m_oMlfs_xyzquat.head<3>() = m_oMlfs.translation();
646 Eigen::Quaternion<double> quat_lf(m_oMlfs.rotation());
647 m_oMlfs_xyzquat(3) = quat_lf.w();
648 m_oMlfs_xyzquat(4) = quat_lf.x();
649 m_oMlfs_xyzquat(5) = quat_lf.y();
650 m_oMlfs_xyzquat(6) = quat_lf.z();
652 m_oMrfs_xyzquat.head<3>() = m_oMrfs.translation();
653 Eigen::Quaternion<double> quat_rf(m_oMrfs.rotation());
654 m_oMrfs_xyzquat(3) = quat_rf.w();
655 m_oMrfs_xyzquat(4) = quat_rf.x();
656 m_oMrfs_xyzquat(5) = quat_rf.y();
657 m_oMrfs_xyzquat(6) = quat_rf.z();
664 if (!m_initSucceeded) {
665 SEND_WARNING_STREAM_MSG(
"Cannot compute signal lf_xyzquat before initialization! iter" + toString(iter));
668 if (s.size() != 7) s.resize(7);
674 if (!m_initSucceeded) {
675 SEND_WARNING_STREAM_MSG(
"Cannot compute signal rf_xyzquat before initialization! iter" + toString(iter));
678 if (s.size() != 7) s.resize(7);
684 if (!m_initSucceeded) {
685 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_lf before initialization!");
688 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6))
689 s.resize(m_robot_util->m_nbJoints + 6);
691 const Eigen::VectorXd& q = m_qSOUT(iter);
692 s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
693 base_se3_to_sot(m_oMff_lf.translation(), m_oMff_lf.rotation(), s.head<6>());
699 if (!m_initSucceeded) {
700 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_rf before initialization!");
703 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6))
704 s.resize(m_robot_util->m_nbJoints + 6);
706 const Eigen::VectorXd& q = m_qSOUT(iter);
707 s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
708 base_se3_to_sot(m_oMff_rf.translation(), m_oMff_rf.rotation(), s.head<6>());
714 if (!m_initSucceeded) {
715 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_imu before initialization!");
718 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6))
719 s.resize(m_robot_util->m_nbJoints + 6);
721 const Eigen::VectorXd& q = m_qSOUT(iter);
722 s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
724 const Eigen::Vector4d& quatIMU_vec = m_imu_quaternionSIN(iter);
725 Eigen::Quaternion<double> quatIMU(quatIMU_vec);
726 base_se3_to_sot(q.head<3>(), quatIMU.toRotationMatrix(), s.head<6>());
732 if (!m_initSucceeded) {
733 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_lf before initialization!");
737 const Vector6& wrench = m_forceLLEGSIN(iter);
740 compute_zmp(wrench, zmp);
741 double w_zmp = compute_zmp_weight(zmp, m_left_foot_sizes, m_zmp_std_dev_lf, m_zmp_margin_lf);
742 double w_fz = compute_force_weight(wrench(2), m_fz_std_dev_lf, m_fz_margin_lf);
745 if (wrench(2) > m_fz_margin_lf)
746 m_lf_fz_stable_cpt++;
748 m_lf_fz_stable_cpt = 0;
750 if (m_lf_fz_stable_cpt >= m_fz_stable_windows_size) {
751 m_lf_fz_stable_cpt = m_fz_stable_windows_size;
752 m_left_foot_is_stable =
true;
754 m_left_foot_is_stable =
false;
761 if (!m_initSucceeded) {
762 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_rf before initialization!");
766 const Vector6& wrench = m_forceRLEGSIN(iter);
769 compute_zmp(wrench, zmp);
770 double w_zmp = compute_zmp_weight(zmp, m_right_foot_sizes, m_zmp_std_dev_rf, m_zmp_margin_rf);
771 double w_fz = compute_force_weight(wrench(2), m_fz_std_dev_rf, m_fz_margin_rf);
774 if (wrench(2) > m_fz_margin_rf)
775 m_rf_fz_stable_cpt++;
777 m_rf_fz_stable_cpt = 0;
779 if (m_rf_fz_stable_cpt >= m_fz_stable_windows_size) {
780 m_rf_fz_stable_cpt = m_fz_stable_windows_size;
781 m_right_foot_is_stable =
true;
783 m_right_foot_is_stable =
false;
790 if (!m_initSucceeded) {
791 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_rf_filtered before initialization!");
794 double w_rf = m_w_rfSOUT(iter);
795 m_w_rf_filtered = m_alpha_w_filter * w_rf + (1 - m_alpha_w_filter) * m_w_rf_filtered;
801 if (!m_initSucceeded) {
802 SEND_WARNING_STREAM_MSG(
"Cannot compute signal w_lf_filtered before initialization!");
805 double w_lf = m_w_lfSOUT(iter);
806 m_w_lf_filtered = m_alpha_w_filter * w_lf + (1 - m_alpha_w_filter) * m_w_lf_filtered;
812 if (!m_initSucceeded) {
813 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v before initialization!");
816 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6))
817 s.resize(m_robot_util->m_nbJoints + 6);
819 m_kinematics_computationsSINNER(iter);
824 const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
825 const Eigen::Vector3d& acc_imu = m_accelerometerSIN(iter);
826 const Eigen::Vector3d& gyr_imu = m_gyroscopeSIN(iter);
827 const Vector6& dftrf = m_dforceRLEGSIN(iter);
828 const Vector6& dftlf = m_dforceLLEGSIN(iter);
829 assert(dq.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
830 "Unexpected size of signal joint_velocities");
836 if (m_w_lf_inSIN.isPlugged())
837 wL = m_w_lf_inSIN(iter);
839 wL = m_w_lf_filteredSOUT(iter);
840 if (m_left_foot_is_stable ==
false) wL = 0.0;
842 if (m_w_rf_inSIN.isPlugged())
843 wR = m_w_rf_inSIN(iter);
845 wR = m_w_rf_filteredSOUT(iter);
846 if (m_right_foot_is_stable ==
false) wR = 0.0;
849 if (wR == 0.0 && wL == 0.0) {
855 const Frame& f_lf = m_model.frames[m_left_foot_id];
856 const Motion v_lf_local = m_data->v[f_lf.parent];
857 const SE3 ffMlf = m_data->oMi[f_lf.parent];
858 Vector6 v_kin_l = -ffMlf.act(v_lf_local).toVector();
859 v_kin_l.head<3>() = m_oRff * v_kin_l.head<3>();
860 v_kin_l.segment<3>(3) = m_oRff * v_kin_l.segment<3>(3);
862 const Frame& f_rf = m_model.frames[m_right_foot_id];
863 const Motion v_rf_local = m_data->v[f_rf.parent];
864 const SE3 ffMrf = m_data->oMi[f_rf.parent];
865 Vector6 v_kin_r = -ffMrf.act(v_rf_local).toVector();
866 v_kin_r.head<3>() = m_oRff * v_kin_r.head<3>();
867 v_kin_r.segment<3>(3) = m_oRff * v_kin_r.segment<3>(3);
869 m_v_kin.head<6>() = (wR * v_kin_r + wL * v_kin_l) / (wL + wR);
874 v_flex_l << -dftlf[0] / m_K_lf(0), -dftlf[1] / m_K_lf(1), -dftlf[2] / m_K_lf(2), -dftlf[3] / m_K_lf(3),
875 -dftlf[4] / m_K_lf(4), -dftlf[5] / m_K_lf(5);
876 v_flex_r << -dftrf[0] / m_K_rf(0), -dftrf[1] / m_K_rf(1), -dftrf[2] / m_K_rf(2), -dftrf[3] / m_K_rf(3),
877 -dftrf[4] / m_K_rf(4), -dftrf[5] / m_K_rf(5);
878 const Eigen::Matrix<double, 6, 6> lfAff = ffMlf.inverse().toActionMatrix();
879 const Eigen::Matrix<double, 6, 6> rfAff = ffMrf.inverse().toActionMatrix();
880 Eigen::Matrix<double, 12, 6> A;
882 Eigen::Matrix<double, 12, 1> b;
883 b << v_flex_l, v_flex_r;
885 m_v_flex.head<6>() = (A.transpose() * A).ldlt().solve(A.transpose() * b);
886 m_v_flex.head<3>() = m_oRff * m_v_flex.head<3>();
891 const Matrix3 ffRimu = (m_data->oMf[m_IMU_body_id]).rotation();
892 const Matrix3 lfRimu = ffMlf.rotation().transpose() * ffRimu;
893 const Matrix3 rfRimu = ffMrf.rotation().transpose() * ffRimu;
900 const SE3 ffMchest(m_data->oMf[m_IMU_body_id]);
901 const SE3 imuMff = (ffMchest * chestMimu).inverse();
903 const Frame& f_imu = m_model.frames[m_IMU_body_id];
904 Vector3 gVo_a_l =
Vector3(gyr_imu(0), gyr_imu(1), gyr_imu(2)) + (imuMff * ffMlf).act(v_lf_local).angular() -
905 m_data->v[f_imu.parent].angular();
906 Vector3 gVo_a_r =
Vector3(gyr_imu(0), gyr_imu(1), gyr_imu(2)) + (imuMff * ffMrf).act(v_rf_local).angular() -
907 m_data->v[f_imu.parent].angular();
908 Motion v_gyr_ankle_l(
Vector3(0., 0., 0.), lfRimu * gVo_a_l);
909 Motion v_gyr_ankle_r(
Vector3(0., 0., 0.), rfRimu * gVo_a_r);
910 Vector6 v_gyr_l = -ffMlf.inverse().act(v_gyr_ankle_l).toVector();
911 Vector6 v_gyr_r = -ffMrf.inverse().act(v_gyr_ankle_r).toVector();
912 m_v_gyr.head<6>() = (wL * v_gyr_l + wR * v_gyr_r) / (wL + wR);
918 const Vector3 acc_world = m_oRchest * acc_imu;
928 if (m_isFirstIterFlag) {
929 m_last_DCacc = acc_world;
930 m_isFirstIterFlag =
false;
931 sendMsg(
"iter:" + toString(iter) +
"\n", MSG_TYPE_INFO);
933 const Vector3 DCacc = acc_world * (1 - m_alpha_DC_acc) + m_last_DCacc * (m_alpha_DC_acc);
935 m_last_DCacc = DCacc;
936 const Vector3 ACacc = acc_world - DCacc;
939 const Vector3 vel = m_last_vel + ACacc * m_dt;
943 const Vector3 DCvel = vel * (1 - m_alpha_DC_vel) + m_last_DCvel * (m_alpha_DC_vel);
944 m_last_DCvel = DCvel;
945 const Vector3 ACvel = vel - DCvel;
949 const Vector3 imuVimu = m_oRchest.transpose() * ACvel;
951 const Motion imuWimu(imuVimu, gyr_imu);
956 const SE3 ffMimu = ffMchest * chestMimu;
957 const Motion v = ffMimu.act(imuWimu);
958 m_v_imu.head<6>() = v.toVector();
959 m_v_imu.head<3>() = m_oRff * m_v_imu.head<3>();
963 m_v_sot.head<6>() = m_v_gyr.head<6>() + m_v_kin.head<6>();
967 m_v_sot.tail(m_robot_util->m_nbJoints) = dq;
968 m_v_kin.tail(m_robot_util->m_nbJoints) = dq;
969 m_v_flex.tail(m_robot_util->m_nbJoints) = dq;
970 m_v_gyr.tail(m_robot_util->m_nbJoints) = dq;
971 m_v_imu.tail(m_robot_util->m_nbJoints) = dq;
979 if (!m_initSucceeded) {
980 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_kin before initialization!");
989 if (!m_initSucceeded) {
990 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_flex before initialization!");
994 s = m_v_flex + m_v_kin;
999 if (!m_initSucceeded) {
1000 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_imu before initialization!");
1009 if (!m_initSucceeded) {
1010 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_gyr before initialization!");
1019 if (!m_initSucceeded) {
1020 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_ac before initialization!");
1029 if (!m_initSucceeded) {
1030 SEND_WARNING_STREAM_MSG(
"Cannot compute signal a_ac before initialization!");
1045 os <<
"BaseEstimator " << getName();
1047 getProfiler().report_all(3, os);
1048 }
catch (ExceptionSignal e) {
SE3 m_oMlfs
world-to-base transformation obtained through right foot
double m_fz_std_dev_lf
standard deviation of normal force measurement errors for right foot
double m_zmp_margin_lf
sizes of the left foot (pos x, neg x, pos y, neg y)
double m_fz_std_dev_rf
standard deviation of ZMP measurement errors for left foot
double compute_force_weight(double fz, double std_dev, double margin)
Vector4 m_right_foot_sizes
sizes of the left foot (pos x, neg x, pos y, neg y)
bool m_reset_foot_pos
true if the entity has been successfully initialized
Eigen::VectorXd m_v_gyr
6d robot velocities form imu only (accelerometer integration + gyro)
void set_stiffness_right_foot(const dynamicgraph::Vector &k)
virtual void display(std::ostream &os) const
filtered weight of the estimation coming from the right foot
#define PROFILE_BASE_VELOCITY_ESTIMATION
SE3 m_sole_M_ftSens
flag to detect first iteration and initialise velocity filters
void set_alpha_DC_acc(const double &a)
int m_lf_fz_stable_cpt
size of the windows used to detect that feet did not leave the ground
Eigen::Matrix< double, 3, 1 > Vector3
Eigen::VectorXd m_v_imu
6d robot velocities from flexibility only (force sensor derivative)
void set_alpha_w_filter(const double &a)
void set_right_foot_sizes(const dynamicgraph::Vector &s)
pinocchio::FrameIndex m_right_foot_id
foot sole to F/T sensor transformation
void set_zmp_margin_left_foot(const double &margin)
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d &R)
void set_zmp_std_dev_right_foot(const double &std_dev)
void set_left_foot_sizes(const dynamicgraph::Vector &s)
void set_fz_stable_windows_size(const int &ws)
void set_zmp_std_dev_left_foot(const double &std_dev)
void compute_zmp(const Vector6 &w, Vector2 &zmp)
pinocchio::FrameIndex m_left_foot_id
SE3 m_oMrfs
transformation from world to left foot sole
Eigen::VectorXd m_v_kin
6d stiffness of left foot spring
int m_rf_fz_stable_cpt
counter for detecting for how long the feet has been stable
void set_alpha_DC_vel(const double &a)
RobotUtilShrPtr m_robot_util
sampling time step
void set_normal_force_std_dev_left_foot(const double &std_dev)
void quanternionMult(const Eigen::Vector4d &q1, const Eigen::Vector4d &q2, Eigen::Vector4d &q12)
pinocchio::FrameIndex m_IMU_body_id
void reset_foot_positions()
Eigen::Matrix< double, 2, 1 > Vector2
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
void set_normal_force_margin_left_foot(const double &margin)
bool m_left_foot_is_stable
Vector6 m_K_lf
6d stiffness of right foot spring
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
Vector4 m_left_foot_sizes
standard deviation of normal force measurement errors for left foot
bool m_isFirstIterFlag
Normal distribution.
double m_fz_margin_rf
margin used for computing normal force weight
void kinematics_estimation(const Vector6 &ft, const Vector6 &K, const SE3 &oMfs, const int foot_id, SE3 &oMff, SE3 &oMfa, SE3 &fsMff)
Vector7 m_oMlfs_xyzquat
transformation from world to right foot sole
double m_zmp_std_dev_lf
standard deviation of ZMP measurement errors for right foot
bool m_right_foot_is_stable
True if left foot as been stable for the last 'm_fz_stable_windows_size' samples. ...
int m_fz_stable_windows_size
True if right foot as been stable for the last 'm_fz_stable_windows_size' samples.
void matrixToRpy(const Eigen::Matrix3d &M, Eigen::Vector3d &rpy)
double m_alpha_w_filter
alpha parameter for DC blocker filter on velocity data
Eigen::Matrix< double, 6, 1 > Vector6
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BaseEstimator(const std::string &name)
Vector6 m_K_rf
margin used for computing normal force weight
void set_zmp_margin_right_foot(const double &margin)
void reset_foot_positions_impl(const Vector6 &ftlf, const Vector6 &ftrf)
void set_stiffness_left_foot(const dynamicgraph::Vector &k)
double compute_zmp_weight(const Vector2 &zmp, const Vector4 &foot_sizes, double std_dev, double margin)
Eigen::VectorXd m_v_flex
6d robot velocities from kinematic only (encoders derivative)
void se3Interp(const pinocchio::SE3 &s1, const pinocchio::SE3 &s2, const double alpha, pinocchio::SE3 &s12)
normal m_normal
Default reference for right foot pose to use if no ref is pluged.
SE3 m_oMrfs_default_ref
Default reference for left foot pose to use if no ref is pluged.
double m_fz_margin_lf
margin used for computing zmp weight
#define PROFILE_BASE_POSITION_ESTIMATION
void pointRotationByQuaternion(const Eigen::Vector3d &point, const Eigen::Vector4d &quat, Eigen::Vector3d &rotatedPoint)
AdmittanceController EntityClassName
void set_normal_force_std_dev_right_foot(const double &std_dev)
double m_alpha_DC_acc
acceleration of the base in the world with DC component removed
void set_normal_force_margin_right_foot(const double &margin)
pinocchio::Model m_model
filtered weight of the estimation coming from the right foot
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Vector3 m_last_vel
base orientation in the world
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
void set_imu_weight(const double &w)
double m_dt
true after the command resetFootPositions is called
double m_zmp_margin_rf
margin used for computing zmp weight
Eigen::VectorXd m_v_pin
robot configuration according to SoT convention
#define PROFILE_BASE_KINEMATICS_COMPUTATION
pinocchio::Data * m_data
Pinocchio robot model.
double m_alpha_DC_vel
alpha parameter for DC blocker filter on acceleration data
double m_zmp_std_dev_rf
weight of IMU for sensor fusion
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention
double m_w_imu
counter for detecting for how long the feet has been stable
void init(const double &dt, const std::string &urdfFile)