sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
base-estimator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017, Thomas Flayols, LAAS-CNRS
3  *
4  */
5 
6 #include "pinocchio/algorithm/frames.hpp"
7 #include <dynamic-graph/factory.h>
8 #include <sot/core/debug.hh>
9 
12 #include <sot/core/stop-watch.hh>
13 
14 namespace dynamicgraph {
15 namespace sot {
16 namespace torque_control {
17 namespace dg = ::dynamicgraph;
18 using namespace dg;
19 using namespace dg::command;
20 using namespace std;
21 using namespace pinocchio;
22 using boost::math::normal; // typedef provides default type is double.
23 
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));
26 
27  const Eigen::Vector3d w(pinocchio::log3(s1.rotation()) * alpha + pinocchio::log3(s2.rotation()) * (1 - alpha));
28 
29  s12 = pinocchio::SE3(pinocchio::exp3(w), t_);
30 }
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;
36  R = q.matrix();
37 }
38 
39 void rpyToMatrix(const Eigen::Vector3d& rpy, Eigen::Matrix3d& R) { return rpyToMatrix(rpy[0], rpy[1], rpy[2], R); }
40 
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) {
45  rpy(0) = 0.0;
46  rpy(2) = -atan2(M(0, 1), M(1, 1));
47  } else {
48  rpy(2) = atan2(M(1, 0), M(0, 0)); // alpha
49  rpy(0) = atan2(M(2, 1), M(2, 2)); // gamma
50  }
51 }
52 
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);
58 }
59 
60 void pointRotationByQuaternion(const Eigen::Vector3d& point, const Eigen::Vector4d& quat,
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;
65  quanternionMult(quat, p4, q_tmp1);
66  quanternionMult(q_tmp1, quat_conj, q_tmp2);
67  rotatedPoint(0) = q_tmp2(1);
68  rotatedPoint(1) = q_tmp2(2);
69  rotatedPoint(2) = q_tmp2(3);
70 }
71 
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"
75 
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
84 
87 typedef BaseEstimator EntityClassName;
88 
89 /* --- DG FACTORY ---------------------------------------------------- */
90 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(BaseEstimator, "BaseEstimator");
91 
92 /* ------------------------------------------------------------------- */
93 /* --- CONSTRUCTION -------------------------------------------------- */
94 /* ------------------------------------------------------------------- */
95 BaseEstimator::BaseEstimator(const std::string& name)
96  : Entity(name),
97  CONSTRUCT_SIGNAL_IN(joint_positions, dynamicgraph::Vector),
98  CONSTRUCT_SIGNAL_IN(joint_velocities, dynamicgraph::Vector),
99  CONSTRUCT_SIGNAL_IN(imu_quaternion, dynamicgraph::Vector),
100  CONSTRUCT_SIGNAL_IN(forceLLEG, dynamicgraph::Vector),
101  CONSTRUCT_SIGNAL_IN(forceRLEG, dynamicgraph::Vector),
102  CONSTRUCT_SIGNAL_IN(dforceLLEG, dynamicgraph::Vector),
103  CONSTRUCT_SIGNAL_IN(dforceRLEG, 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),
110  CONSTRUCT_SIGNAL_IN(gyroscope, dynamicgraph::Vector),
111  CONSTRUCT_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector,
112  m_joint_positionsSIN << m_joint_velocitiesSIN),
113  CONSTRUCT_SIGNAL_OUT(q, dynamicgraph::Vector,
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),
118  CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector,
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),
138  m_w_imu(0.0),
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) {
145  Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
146 
147  m_K_rf << 4034, 23770, 239018, 707, 502, 936;
148  m_K_lf << 4034, 23770, 239018, 707, 502, 936;
149  m_left_foot_sizes << 0.130, -0.100, 0.075, -0.056;
150  m_right_foot_sizes << 0.130, -0.100, 0.056, -0.075;
151 
152  /* Commands. */
153  addCommand("init", makeCommandVoid2(
154  *this, &BaseEstimator::init,
155  docCommandVoid2("Initialize the entity.", "time step (double)", "URDF file path (string)")));
156 
157  addCommand("set_fz_stable_windows_size",
158  makeCommandVoid1(*this, &BaseEstimator::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",
161  makeCommandVoid1(*this, &BaseEstimator::set_alpha_w_filter,
162  docCommandVoid1("Set the filter parameter to filter weights", "double")));
163  addCommand(
164  "set_alpha_DC_acc",
165  makeCommandVoid1(*this, &BaseEstimator::set_alpha_DC_acc,
166  docCommandVoid1("Set the filter parameter for removing DC from accelerometer data", "double")));
167  addCommand("set_alpha_DC_vel",
168  makeCommandVoid1(
170  docCommandVoid1("Set the filter parameter for removing DC from velocity integrated from acceleration",
171  "double")));
172  addCommand("reset_foot_positions", makeCommandVoid0(*this, &BaseEstimator::reset_foot_positions,
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",
178  makeCommandVoid1(*this, &BaseEstimator::set_imu_weight,
179  docCommandVoid1("Set the weight of imu-based orientation in sensor fusion", "double")));
180  addCommand(
181  "set_zmp_std_dev_right_foot",
182  makeCommandVoid1(
184  docCommandVoid1("Set the standard deviation of the ZMP measurement errors for the right foot", "double")));
185  addCommand(
186  "set_zmp_std_dev_left_foot",
187  makeCommandVoid1(
189  docCommandVoid1("Set the standard deviation of the ZMP measurement errors for the left foot", "double")));
190  addCommand(
191  "set_normal_force_std_dev_right_foot",
192  makeCommandVoid1(
194  docCommandVoid1("Set the standard deviation of the normal force measurement errors for the right foot",
195  "double")));
196  addCommand("set_normal_force_std_dev_left_foot",
197  makeCommandVoid1(
199  docCommandVoid1("Set the standard deviation of the normal force measurement errors for the left foot",
200  "double")));
201  addCommand("set_stiffness_right_foot",
202  makeCommandVoid1(*this, &BaseEstimator::set_stiffness_right_foot,
203  docCommandVoid1("Set the 6d stiffness of the spring at the right foot", "vector")));
204  addCommand("set_stiffness_left_foot",
205  makeCommandVoid1(*this, &BaseEstimator::set_stiffness_left_foot,
206  docCommandVoid1("Set the 6d stiffness of the spring at the left foot", "vector")));
207  addCommand(
208  "set_right_foot_sizes",
209  makeCommandVoid1(*this, &BaseEstimator::set_right_foot_sizes,
210  docCommandVoid1("Set the size of the right foot (pos x, neg x, pos y, neg y)", "4d vector")));
211  addCommand(
212  "set_left_foot_sizes",
213  makeCommandVoid1(*this, &BaseEstimator::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",
216  makeCommandVoid1(*this, &BaseEstimator::set_zmp_margin_right_foot,
217  docCommandVoid1("Set the ZMP margin for the right foot", "double")));
218  addCommand("set_zmp_margin_left_foot",
219  makeCommandVoid1(*this, &BaseEstimator::set_zmp_margin_left_foot,
220  docCommandVoid1("Set the ZMP margin for the left foot", "double")));
221  addCommand("set_normal_force_margin_right_foot",
222  makeCommandVoid1(*this, &BaseEstimator::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",
225  makeCommandVoid1(*this, &BaseEstimator::set_normal_force_margin_left_foot,
226  docCommandVoid1("Set the normal force margin for the left foot", "double")));
227 }
228 
229 void BaseEstimator::init(const double& dt, const std::string& robotRef) {
230  m_dt = dt;
231  try {
232  /* Retrieve m_robot_util informations */
233  std::string localName(robotRef);
234  if (isNameInRobotUtil(localName)) {
235  m_robot_util = getRobotUtil(localName);
236  std::cerr << "m_robot_util:" << m_robot_util << std::endl;
237  } else {
238  SEND_MSG("You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
239  return;
240  }
241 
242  pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(), m_model);
243 
244  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
245  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
246  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
247  m_left_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
248  m_right_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
249  m_IMU_body_id = m_model.getFrameId(m_robot_util->m_imu_joint_name);
250 
251  m_q_pin.setZero(m_model.nq);
252  m_q_pin[6] = 1.; // for quaternion
253  m_q_sot.setZero(m_robot_util->m_nbJoints + 6);
254  m_v_pin.setZero(m_robot_util->m_nbJoints + 6);
255  m_v_sot.setZero(m_robot_util->m_nbJoints + 6);
256  m_v_kin.setZero(m_robot_util->m_nbJoints + 6);
257  m_v_flex.setZero(m_robot_util->m_nbJoints + 6);
258  m_v_imu.setZero(m_robot_util->m_nbJoints + 6);
259  m_v_gyr.setZero(m_robot_util->m_nbJoints + 6);
260  m_sole_M_ftSens = SE3(Matrix3::Identity(),
261  -Eigen::Map<const Vector3>(&m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ(0)));
262 
263  m_last_vel.setZero();
264  m_last_DCvel.setZero();
265  m_last_DCacc.setZero(); // this is to replace by acceleration at 1st iteration
266 
267  m_alpha_DC_acc = 0.9995;
268  m_alpha_DC_vel = 0.9995;
269  m_alpha_w_filter = 1.0;
270  m_left_foot_is_stable = true;
271  m_right_foot_is_stable = true;
275  m_isFirstIterFlag = true;
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);
279  return;
280  }
281  m_data = new pinocchio::Data(m_model);
282  m_initSucceeded = true;
283 }
284 
286  if (ws < 0.0) return SEND_MSG("windows_size should be a positive integer", MSG_TYPE_ERROR);
288 }
289 
290 void BaseEstimator::set_alpha_w_filter(const double& a) {
291  if (a < 0.0 || a > 1.0) return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
292  m_alpha_w_filter = a;
293 }
294 
295 void BaseEstimator::set_alpha_DC_acc(const double& a) {
296  if (a < 0.0 || a > 1.0) return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
297  m_alpha_DC_acc = a;
298 }
299 
300 void BaseEstimator::set_alpha_DC_vel(const double& a) {
301  if (a < 0.0 || a > 1.0) return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
302  m_alpha_DC_vel = a;
303 }
304 
306 
307 void BaseEstimator::set_imu_weight(const double& w) {
308  if (w < 0.0) return SEND_MSG("Imu weight must be nonnegative", MSG_TYPE_ERROR);
309  m_w_imu = w;
310 }
311 
312 void BaseEstimator::set_stiffness_right_foot(const dynamicgraph::Vector& k) {
313  if (k.size() != 6) return SEND_MSG("Stiffness vector should have size 6, not " + toString(k.size()), MSG_TYPE_ERROR);
314  m_K_rf = k;
315 }
316 
317 void BaseEstimator::set_stiffness_left_foot(const dynamicgraph::Vector& k) {
318  if (k.size() != 6) return SEND_MSG("Stiffness vector should have size 6, not " + toString(k.size()), MSG_TYPE_ERROR);
319  m_K_lf = k;
320 }
321 
322 void BaseEstimator::set_zmp_std_dev_right_foot(const double& std_dev) {
323  if (std_dev <= 0.0) return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
324  m_zmp_std_dev_rf = std_dev;
325 }
326 
327 void BaseEstimator::set_zmp_std_dev_left_foot(const double& std_dev) {
328  if (std_dev <= 0.0) return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
329  m_zmp_std_dev_lf = std_dev;
330 }
331 
333  if (std_dev <= 0.0) return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
334  m_fz_std_dev_rf = std_dev;
335 }
336 
338  if (std_dev <= 0.0) return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
339  m_fz_std_dev_lf = std_dev;
340 }
341 
342 void BaseEstimator::set_right_foot_sizes(const dynamicgraph::Vector& s) {
343  if (s.size() != 4) return SEND_MSG("Foot size vector should have size 4, not " + toString(s.size()), MSG_TYPE_ERROR);
344  m_right_foot_sizes = s;
345 }
346 
347 void BaseEstimator::set_left_foot_sizes(const dynamicgraph::Vector& s) {
348  if (s.size() != 4) return SEND_MSG("Foot size vector should have size 4, not " + toString(s.size()), MSG_TYPE_ERROR);
349  m_left_foot_sizes = s;
350 }
351 
352 void BaseEstimator::set_zmp_margin_right_foot(const double& margin) { m_zmp_margin_rf = margin; }
353 
354 void BaseEstimator::set_zmp_margin_left_foot(const double& margin) { m_zmp_margin_lf = margin; }
355 
357 
358 void BaseEstimator::set_normal_force_margin_left_foot(const double& margin) { m_fz_margin_lf = margin; }
359 
360 void BaseEstimator::compute_zmp(const Vector6& w, Vector2& zmp) {
361  pinocchio::Force f(w);
362  f = m_sole_M_ftSens.act(f);
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];
366 }
367 
368 double BaseEstimator::compute_zmp_weight(const Vector2& zmp, const Vector4& foot_sizes, double std_dev,
369  double margin) {
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;
374 
375  if (zmp[0] > fs0 || zmp[0] < fs1 || zmp[1] > fs2 || zmp[1] < fs3) return 0;
376 
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;
379  return cdx * cdy;
380 }
381 
382 double BaseEstimator::compute_force_weight(double fz, double std_dev, double margin) {
383  if (fz < margin) return 0.0;
384  return (cdf(m_normal, (fz - margin) / std_dev) - 0.5) * 2.0;
385 }
386 
387 void BaseEstimator::reset_foot_positions_impl(const Vector6& ftlf, const Vector6& ftrf) {
388  // compute the base position wrt each foot
389  SE3 dummy, dummy1, lfMff, rfMff;
390  m_oMrfs = SE3::Identity();
391  m_oMlfs = SE3::Identity();
392  kinematics_estimation(ftrf, m_K_rf, m_oMrfs, static_cast<int>(m_right_foot_id), rfMff, dummy,
393  dummy1); // rfMff is obtain reading oMff becaused oMrfs is here set to Identity
394  kinematics_estimation(ftlf, m_K_lf, m_oMlfs, static_cast<int>(m_left_foot_id), lfMff, dummy, dummy1);
395 
396  // distance from ankle to ground
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;
401 
402  // set the world frame in between the feet
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()));
405  // add a constant offset to make the world frame match the one in OpenHRP
406  oMff.translation()(0) += 9.562e-03;
407 
408  m_oMlfs = oMff * lfMff.inverse() * groundMfoot;
409  m_oMrfs = oMff * rfMff.inverse() * groundMfoot;
410 
411  m_oMlfs_xyzquat.head<3>() = m_oMlfs.translation();
412  Eigen::Quaternion<double> quat_lf(m_oMlfs.rotation());
413  m_oMlfs_xyzquat(3) = quat_lf.w();
414  m_oMlfs_xyzquat(4) = quat_lf.x();
415  m_oMlfs_xyzquat(5) = quat_lf.y();
416  m_oMlfs_xyzquat(6) = quat_lf.z();
417 
418  m_oMrfs_xyzquat.head<3>() = m_oMrfs.translation();
419  Eigen::Quaternion<double> quat_rf(m_oMrfs.rotation());
420  m_oMrfs_xyzquat(3) = quat_rf.w();
421  m_oMrfs_xyzquat(4) = quat_rf.x();
422  m_oMrfs_xyzquat(5) = quat_rf.y();
423  m_oMrfs_xyzquat(6) = quat_rf.z();
424 
425  // save this poses to use it if no ref is provided
428 
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);
431 
432  // kinematics_estimation(ftrf, m_K_rf, m_oMrfs, m_right_foot_id, m_oMff_rf, dummy);
433  // kinematics_estimation(ftlf, m_K_lf, m_oMlfs, m_left_foot_id, m_oMff_lf, dummy);
434  // sendMsg("Base estimation left foot:\n"+toString(m_oMff_lf), MSG_TYPE_DEBUG);
435  // sendMsg("Base estimation right foot:\n"+toString(m_oMff_rf), MSG_TYPE_DEBUG);
436  // sendMsg("Difference base estimation left-right foot:\n"+toString(m_oMff_rf.inverse()*m_oMff_lf),
437  // MSG_TYPE_DEBUG);
438 
439  m_reset_foot_pos = false;
440 }
441 
442 void BaseEstimator::kinematics_estimation(const Vector6& ft, const Vector6& K, const SE3& oMfs, const int foot_id,
443  SE3& oMff, SE3& oMfa, SE3& fsMff) {
444  Vector3 xyz;
445  xyz << -ft[0] / K(0), -ft[1] / K(1), -ft[2] / K(2);
446  Matrix3 R;
447  rpyToMatrix(-ft[3] / K(3), -ft[4] / K(4), -ft[5] / K(5), R);
448  const SE3 fsMfa(R, xyz); // foot sole to foot ankle
449  oMfa = oMfs * fsMfa; // world to foot ankle
450  const SE3 faMff(m_data->oMf[foot_id].inverse()); // foot ankle to free flyer
451  //~ sendMsg("faMff (foot_id="+toString(foot_id)+"):\n" + toString(faMff), MSG_TYPE_INFO);
452  //~ sendMsg("fsMfa (foot_id="+toString(foot_id)+"):\n" + toString(fsMfa), MSG_TYPE_INFO);
453  oMff = oMfa * faMff; // world to free flyer
454  fsMff = fsMfa * faMff; // foot sole to free flyer
455 }
456 
457 /* ------------------------------------------------------------------- */
458 /* --- SIGNALS ------------------------------------------------------- */
459 /* ------------------------------------------------------------------- */
460 
461 DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector) {
462  if (!m_initSucceeded) {
463  SEND_WARNING_STREAM_MSG("Cannot compute signal kinematics_computations before initialization!");
464  return s;
465  }
466 
467  const Eigen::VectorXd& qj = m_joint_positionsSIN(iter); // n+6
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");
473 
474  /* convert sot to pinocchio joint order */
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));
477 
478  getProfiler().start(PROFILE_BASE_KINEMATICS_COMPUTATION);
479 
480  /* Compute kinematics assuming world is at free-flyer frame */
481  m_q_pin.head<6>().setZero();
482  m_q_pin(6) = 1.0;
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);
486 
487  getProfiler().stop(PROFILE_BASE_KINEMATICS_COMPUTATION);
488 
489  return s;
490 }
491 
492 DEFINE_SIGNAL_OUT_FUNCTION(q, dynamicgraph::Vector) {
493  if (!m_initSucceeded) {
494  SEND_WARNING_STREAM_MSG("Cannot compute signal q before initialization!");
495  return s;
496  }
497  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
498  s.resize(m_robot_util->m_nbJoints + 6);
499 
500  const Eigen::VectorXd& qj = m_joint_positionsSIN(iter); // n+6
501  const Eigen::Vector4d& quatIMU_vec = m_imu_quaternionSIN(iter);
502  const Vector6& ftrf = m_forceRLEGSIN(iter);
503  const Vector6& ftlf = m_forceLLEGSIN(iter);
504 
505  // if the weights are not specified by the user through the input signals w_lf, w_rf
506  // then compute them
507  // if one feet is not stable, force weight to 0.0
508  double wL, wR;
509  if (m_w_lf_inSIN.isPlugged())
510  wL = m_w_lf_inSIN(iter);
511  else {
512  wL = m_w_lf_filteredSOUT(iter);
513  if (m_left_foot_is_stable == false) wL = 0.0;
514  }
515  if (m_w_rf_inSIN.isPlugged())
516  wR = m_w_rf_inSIN(iter);
517  else {
518  wR = m_w_rf_filteredSOUT(iter);
519  if (m_right_foot_is_stable == false) wR = 0.0;
520  }
521 
522  assert(qj.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
523  "Unexpected size of signal joint_positions");
524 
525  // if both weights are zero set them to a small positive value to avoid division by zero
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));
531  wR = 1e-3;
532  wL = 1e-3;
533  }
534 
535  m_kinematics_computationsSINNER(iter);
536 
537  if (m_reset_foot_pos) reset_foot_positions_impl(ftlf, ftrf);
538 
539  getProfiler().start(PROFILE_BASE_POSITION_ESTIMATION);
540  {
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);
544 
545  // get rpy
546  const SE3 ffMchest(m_data->oMf[m_IMU_body_id]); // transform between freeflyer and body attached to IMU sensor
547  const SE3 chestMff(ffMchest.inverse()); // transform between body attached to IMU sensor and freeflyer
548 
549  Vector3 rpy_chest, rpy_chest_lf, rpy_chest_rf,
550  rpy_chest_imu; // orientation of the body which imu is attached to. (fusion, from left kine, from right kine,
551  // from imu)
552 
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);
557 
558  // average (we do not take into account the IMU yaw)
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);
563 
564  rpyToMatrix(rpy_chest, m_oRchest);
565  m_oRff = m_oRchest * chestMff.rotation();
566 
567  // translation to get foot at the right position
568  // evaluate Mrl Mlf for q with the good orientation and zero translation for freeflyer
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();
571  // get average translation
572  m_q_pin.head<3>() = ((oMlfa.translation() - pos_lf_ff) * wL + (oMrfa.translation() - pos_rf_ff) * wR) / (wL + wR);
573 
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>());
576 
577  s = m_q_sot;
578 
579  // store estimation of the base pose in SE3 format
580  const SE3 oMff_est(m_oRff, m_q_pin.head<3>());
581 
582  // feedback on feet poses
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)");
588  // feet positions in the world according to current base estimation
589  const SE3 oMlfs_est(oMff_est * (lfsMff.inverse()));
590  const SE3 oMrfs_est(oMff_est * (rfsMff.inverse()));
591  // error in current foot position
592  SE3 leftDrift = m_oMlfs.inverse() * oMlfs_est;
593  SE3 rightDrift = m_oMrfs.inverse() * oMrfs_est;
594 
596  SE3 leftDrift_delta;
597  SE3 rightDrift_delta;
598  se3Interp(leftDrift, SE3::Identity(), K_fb * wR, leftDrift_delta);
599  se3Interp(rightDrift, SE3::Identity(), K_fb * wL, rightDrift_delta);
600  // if a feet is not stable on the ground (aka flying), fully update his position
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) {
604  // robot is jumping, do not update any feet position
605  rightDrift_delta = SE3::Identity();
606  leftDrift_delta = SE3::Identity();
607  }
608  m_oMlfs = m_oMlfs * leftDrift_delta;
609  m_oMrfs = m_oMrfs * rightDrift_delta;
610  // dedrift (x, y, z, yaw) using feet pose references
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>());
622  } else {
623  oMlfs_ref = m_oMlfs_default_ref;
624  oMrfs_ref = m_oMrfs_default_ref;
625  }
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)));
634  //~ printf("yaw_drift=%lf\r\n",yaw_drift);
636  const Vector3 rpy_feet_drift(0., 0., yaw_drift);
637  Matrix3 rotation_feet_drift;
638  rpyToMatrix(rpy_feet_drift, 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;
642  }
643  }
644  // convert to xyz+quaternion format //Rq: this convertions could be done in outupt signals function?
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();
651 
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();
658  }
659  getProfiler().stop(PROFILE_BASE_POSITION_ESTIMATION);
660  return s;
661 }
662 
663 DEFINE_SIGNAL_OUT_FUNCTION(lf_xyzquat, dynamicgraph::Vector) {
664  if (!m_initSucceeded) {
665  SEND_WARNING_STREAM_MSG("Cannot compute signal lf_xyzquat before initialization! iter" + toString(iter));
666  return s;
667  }
668  if (s.size() != 7) s.resize(7);
669  s = m_oMlfs_xyzquat;
670  return s;
671 }
672 
673 DEFINE_SIGNAL_OUT_FUNCTION(rf_xyzquat, dynamicgraph::Vector) {
674  if (!m_initSucceeded) {
675  SEND_WARNING_STREAM_MSG("Cannot compute signal rf_xyzquat before initialization! iter" + toString(iter));
676  return s;
677  }
678  if (s.size() != 7) s.resize(7);
679  s = m_oMrfs_xyzquat;
680  return s;
681 }
682 
683 DEFINE_SIGNAL_OUT_FUNCTION(q_lf, dynamicgraph::Vector) {
684  if (!m_initSucceeded) {
685  SEND_WARNING_STREAM_MSG("Cannot compute signal q_lf before initialization!");
686  return s;
687  }
688  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
689  s.resize(m_robot_util->m_nbJoints + 6);
690 
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>());
694 
695  return s;
696 }
697 
698 DEFINE_SIGNAL_OUT_FUNCTION(q_rf, dynamicgraph::Vector) {
699  if (!m_initSucceeded) {
700  SEND_WARNING_STREAM_MSG("Cannot compute signal q_rf before initialization!");
701  return s;
702  }
703  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
704  s.resize(m_robot_util->m_nbJoints + 6);
705 
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>());
709 
710  return s;
711 }
712 
713 DEFINE_SIGNAL_OUT_FUNCTION(q_imu, dynamicgraph::Vector) {
714  if (!m_initSucceeded) {
715  SEND_WARNING_STREAM_MSG("Cannot compute signal q_imu before initialization!");
716  return s;
717  }
718  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
719  s.resize(m_robot_util->m_nbJoints + 6);
720 
721  const Eigen::VectorXd& q = m_qSOUT(iter);
722  s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
723 
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>());
727 
728  return s;
729 }
730 
732  if (!m_initSucceeded) {
733  SEND_WARNING_STREAM_MSG("Cannot compute signal w_lf before initialization!");
734  return s;
735  }
736 
737  const Vector6& wrench = m_forceLLEGSIN(iter);
738  Vector2 zmp;
739  zmp.setZero();
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);
743  // check that foot is sensing a force greater than the margin treshold for more than 'm_fz_stable_windows_size'
744  // samples
745  if (wrench(2) > m_fz_margin_lf)
746  m_lf_fz_stable_cpt++;
747  else
748  m_lf_fz_stable_cpt = 0;
749 
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;
753  } else {
754  m_left_foot_is_stable = false;
755  }
756  s = w_zmp * w_fz;
757  return s;
758 }
759 
761  if (!m_initSucceeded) {
762  SEND_WARNING_STREAM_MSG("Cannot compute signal w_rf before initialization!");
763  return s;
764  }
765 
766  const Vector6& wrench = m_forceRLEGSIN(iter);
767  Vector2 zmp;
768  zmp.setZero();
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);
772  // check that foot is sensing a force greater than the margin treshold for more than 'm_fz_stable_windows_size'
773  // samples
774  if (wrench(2) > m_fz_margin_rf)
775  m_rf_fz_stable_cpt++;
776  else
777  m_rf_fz_stable_cpt = 0;
778 
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;
782  } else {
783  m_right_foot_is_stable = false;
784  }
785  s = w_zmp * w_fz;
786  return s;
787 }
788 
789 DEFINE_SIGNAL_OUT_FUNCTION(w_rf_filtered, double) {
790  if (!m_initSucceeded) {
791  SEND_WARNING_STREAM_MSG("Cannot compute signal w_rf_filtered before initialization!");
792  return s;
793  }
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; // low pass filter
796  s = m_w_rf_filtered;
797  return s;
798 }
799 
800 DEFINE_SIGNAL_OUT_FUNCTION(w_lf_filtered, double) {
801  if (!m_initSucceeded) {
802  SEND_WARNING_STREAM_MSG("Cannot compute signal w_lf_filtered before initialization!");
803  return s;
804  }
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; // low pass filter
807  s = m_w_lf_filtered;
808  return s;
809 }
810 
811 DEFINE_SIGNAL_OUT_FUNCTION(v, dynamicgraph::Vector) {
812  if (!m_initSucceeded) {
813  SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
814  return s;
815  }
816  if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
817  s.resize(m_robot_util->m_nbJoints + 6);
818 
819  m_kinematics_computationsSINNER(iter);
820  m_qSOUT(iter);
821 
822  getProfiler().start(PROFILE_BASE_VELOCITY_ESTIMATION);
823  {
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");
831 
832  // if the weights are not specified by the user through the input signals w_lf, w_rf
833  // then compute them
834  // if one feet is not stable, force weight to 0.0
835  double wL, wR;
836  if (m_w_lf_inSIN.isPlugged())
837  wL = m_w_lf_inSIN(iter);
838  else {
839  wL = m_w_lf_filteredSOUT(iter);
840  if (m_left_foot_is_stable == false) wL = 0.0;
841  }
842  if (m_w_rf_inSIN.isPlugged())
843  wR = m_w_rf_inSIN(iter);
844  else {
845  wR = m_w_rf_filteredSOUT(iter);
846  if (m_right_foot_is_stable == false) wR = 0.0;
847  }
848  // if both weights are zero set them to a small positive value to avoid division by zero
849  if (wR == 0.0 && wL == 0.0) {
850  wR = 1e-3;
851  wL = 1e-3;
852  }
853 
854  /* Compute foot velocities */
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(); // this is the velocity of the base in the frame of the base.
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);
861 
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(); // this is the velocity of the base in the frame of the base.
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);
868 
869  m_v_kin.head<6>() = (wR * v_kin_r + wL * v_kin_l) / (wL + wR);
870 
871  /* Compute velocity induced by the flexibility */
872  Vector6 v_flex_l;
873  Vector6 v_flex_r;
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;
881  A << lfAff, rfAff;
882  Eigen::Matrix<double, 12, 1> b;
883  b << v_flex_l, v_flex_r;
884  //~ m_v_flex.head<6>() = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
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>();
887 
888  /* Get an estimate of linear velocities from gyroscope only*/
889  // we make the asumtion than we are 'turning around the foot' with pure angular velocity in the ankle measured by
890  // the gyro
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;
894 
895  const SE3 chestMimu(
896  Matrix3::Identity(),
897  Vector3(
898  -0.13, 0.0,
899  0.118));
900  const SE3 ffMchest(m_data->oMf[m_IMU_body_id]);
901  const SE3 imuMff = (ffMchest * chestMimu).inverse();
902  // gVw_a = gVo_g + gHa.act(aVb_a)-gVb_g //angular velocity in the ankle from gyro and d_enc
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);
913 
914  /* Get an estimate of linear velocities from filtered accelerometer integration */
915 
916  /* rotate acceleration to express it in the world frame */
917  //~ pointRotationByQuaternion(acc_imu,quatIMU_vec,acc_world); //this is false because yaw from imuquat is drifting
918  const Vector3 acc_world = m_oRchest * acc_imu;
919 
920  /* now, the acceleration is expressed in the world frame,
921  * so it can be written as the sum of the proper acceleration and the
922  * constant gravity vector. We could remove this assuming a [0,0,9.81]
923  * but we prefer to filter this signal with low frequency high pass
924  * filter since it is robust to gravity norm error, and we know that
925  * globaly the robot can not accelerate continuously. */
926 
928  if (m_isFirstIterFlag) {
929  m_last_DCacc = acc_world; // Copy the first acceleration data
930  m_isFirstIterFlag = false;
931  sendMsg("iter:" + toString(iter) + "\n", MSG_TYPE_INFO);
932  }
933  const Vector3 DCacc = acc_world * (1 - m_alpha_DC_acc) + m_last_DCacc * (m_alpha_DC_acc);
934  //~ sendMsg("acc_world :"+toString(acc_world)+"\n", MSG_TYPE_INFO);
935  m_last_DCacc = DCacc;
936  const Vector3 ACacc = acc_world - DCacc;
937  m_v_ac = ACacc;
938  /* Then this acceleration is integrated. */
939  const Vector3 vel = m_last_vel + ACacc * m_dt;
940  m_last_vel = vel;
941  /* To stabilise the integrated velocity, we add the
942  * asumption that globaly, velocity is zero. So we remove DC again */
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;
946  m_v_ac = ACvel;
947 
948  /* Express back velocity in the imu frame to get a full 6d velocity with the gyro*/
949  const Vector3 imuVimu = m_oRchest.transpose() * ACvel;
950  /* Here we could remove dc from gyrometer to remove bias*/
951  const Motion imuWimu(imuVimu, gyr_imu);
952  // const Frame & f_imu = m_model.frames[m_IMU_body_id];
953  // const SE3 ffMchest(m_data->oMf[m_IMU_body_id]);
954  // const SE3 chestMimu(Matrix3::Identity(), +1.0*Vector3(-0.13, 0.0, 0.118)); ///TODO Read this transform from
955  // setable parameter /// TODO chesk the sign of the translation
956  const SE3 ffMimu = ffMchest * chestMimu;
957  const Motion v = ffMimu.act(imuWimu); //- ffWchest;
958  m_v_imu.head<6>() = v.toVector();
959  m_v_imu.head<3>() = m_oRff * m_v_imu.head<3>();
960 
961  //~ m_v_sot.head<6>() = m_v_kin.head<6>();
962  //~ m_v_sot.head<6>() = m_v_flex.head<6>() + m_v_kin.head<6>();
963  m_v_sot.head<6>() = m_v_gyr.head<6>() + m_v_kin.head<6>();
964  // m_v_sot.head<6>() = m_v_gyr.head<6>();
965  //~ m_v_sot.head<6>() = m_v_imu.head<6>();
966 
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;
972  s = m_v_sot;
973  }
974  getProfiler().stop(PROFILE_BASE_VELOCITY_ESTIMATION);
975  return s;
976 }
977 
978 DEFINE_SIGNAL_OUT_FUNCTION(v_kin, dynamicgraph::Vector) {
979  if (!m_initSucceeded) {
980  SEND_WARNING_STREAM_MSG("Cannot compute signal v_kin before initialization!");
981  return s;
982  }
983  m_vSOUT(iter);
984  s = m_v_kin;
985  return s;
986 }
987 
988 DEFINE_SIGNAL_OUT_FUNCTION(v_flex, dynamicgraph::Vector) {
989  if (!m_initSucceeded) {
990  SEND_WARNING_STREAM_MSG("Cannot compute signal v_flex before initialization!");
991  return s;
992  }
993  m_vSOUT(iter);
994  s = m_v_flex + m_v_kin;
995  return s;
996 }
997 
998 DEFINE_SIGNAL_OUT_FUNCTION(v_imu, dynamicgraph::Vector) {
999  if (!m_initSucceeded) {
1000  SEND_WARNING_STREAM_MSG("Cannot compute signal v_imu before initialization!");
1001  return s;
1002  }
1003  m_vSOUT(iter);
1004  s = m_v_imu;
1005  return s;
1006 }
1007 
1008 DEFINE_SIGNAL_OUT_FUNCTION(v_gyr, dynamicgraph::Vector) {
1009  if (!m_initSucceeded) {
1010  SEND_WARNING_STREAM_MSG("Cannot compute signal v_gyr before initialization!");
1011  return s;
1012  }
1013  m_vSOUT(iter);
1014  s = m_v_gyr;
1015  return s;
1016 }
1017 
1018 DEFINE_SIGNAL_OUT_FUNCTION(v_ac, dynamicgraph::Vector) {
1019  if (!m_initSucceeded) {
1020  SEND_WARNING_STREAM_MSG("Cannot compute signal v_ac before initialization!");
1021  return s;
1022  }
1023  m_vSOUT(iter);
1024  s = m_v_ac;
1025  return s;
1026 }
1027 
1028 DEFINE_SIGNAL_OUT_FUNCTION(a_ac, dynamicgraph::Vector) {
1029  if (!m_initSucceeded) {
1030  SEND_WARNING_STREAM_MSG("Cannot compute signal a_ac before initialization!");
1031  return s;
1032  }
1033  m_vSOUT(iter);
1034  s = m_a_ac;
1035  return s;
1036 }
1037 
1038 /* --- COMMANDS ---------------------------------------------------------- */
1039 
1040 /* ------------------------------------------------------------------- */
1041 /* --- ENTITY -------------------------------------------------------- */
1042 /* ------------------------------------------------------------------- */
1043 
1044 void BaseEstimator::display(std::ostream& os) const {
1045  os << "BaseEstimator " << getName();
1046  try {
1047  getProfiler().report_all(3, os);
1048  } catch (ExceptionSignal e) {
1049  }
1050 }
1051 } // namespace torque_control
1052 } // namespace sot
1053 } // namespace dynamicgraph
dynamicgraph::sot::torque_control::BaseEstimator::set_alpha_DC_vel
void set_alpha_DC_vel(const double &a)
Definition: base-estimator.cpp:300
dynamicgraph::sot::torque_control::BaseEstimator::reset_foot_positions
void reset_foot_positions()
Definition: base-estimator.cpp:305
dynamicgraph::sot::torque_control::BaseEstimator::m_fz_margin_rf
double m_fz_margin_rf
margin used for computing normal force weight
Definition: base-estimator.hh:190
dynamicgraph::sot::torque_control::BaseEstimator::reset_foot_positions_impl
void reset_foot_positions_impl(const Vector6 &ftlf, const Vector6 &ftrf)
Definition: base-estimator.cpp:387
dynamicgraph::sot::torque_control::Vector3
Eigen::Matrix< double, 3, 1 > Vector3
Definition: admittance-controller.cpp:46
dynamicgraph::sot::torque_control::pointRotationByQuaternion
void pointRotationByQuaternion(const Eigen::Vector3d &point, const Eigen::Vector4d &quat, Eigen::Vector3d &rotatedPoint)
Definition: base-estimator.cpp:60
dynamicgraph::sot::torque_control::BaseEstimator::m_normal
normal m_normal
Default reference for right foot pose to use if no ref is pluged.
Definition: base-estimator.hh:221
dynamicgraph::sot::torque_control::BaseEstimator::set_normal_force_margin_right_foot
void set_normal_force_margin_right_foot(const double &margin)
Definition: base-estimator.cpp:356
dynamicgraph::sot::torque_control::BaseEstimator::set_normal_force_std_dev_right_foot
void set_normal_force_std_dev_right_foot(const double &std_dev)
Definition: base-estimator.cpp:332
dynamicgraph::sot::torque_control::BaseEstimator::m_K_lf
Vector6 m_K_lf
6d stiffness of right foot spring
Definition: base-estimator.hh:192
dynamicgraph::sot::torque_control::BaseEstimator::set_stiffness_right_foot
void set_stiffness_right_foot(const dynamicgraph::Vector &k)
Definition: base-estimator.cpp:312
dynamicgraph::sot::torque_control::BaseEstimator::m_data
pinocchio::Data * m_data
Pinocchio robot model.
Definition: base-estimator.hh:212
dynamicgraph::sot::torque_control::BaseEstimator::m_robot_util
RobotUtilShrPtr m_robot_util
sampling time step
Definition: base-estimator.hh:171
dynamicgraph::sot::torque_control::BaseEstimator::m_initSucceeded
bool m_initSucceeded
Definition: base-estimator.hh:168
dynamicgraph::sot::torque_control::BaseEstimator::m_dt
double m_dt
true after the command resetFootPositions is called
Definition: base-estimator.hh:170
dynamicgraph::sot::torque_control::BaseEstimator::set_normal_force_margin_left_foot
void set_normal_force_margin_left_foot(const double &margin)
Definition: base-estimator.cpp:358
dynamicgraph::sot::torque_control::BaseEstimator::m_v_flex
Eigen::VectorXd m_v_flex
6d robot velocities from kinematic only (encoders derivative)
Definition: base-estimator.hh:195
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::BaseEstimator::set_zmp_std_dev_left_foot
void set_zmp_std_dev_left_foot(const double &std_dev)
Definition: base-estimator.cpp:327
dynamicgraph::sot::torque_control::BaseEstimator::m_q_sot
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention
Definition: base-estimator.hh:232
dynamicgraph::sot::torque_control::BaseEstimator::m_oMrfs
SE3 m_oMrfs
transformation from world to left foot sole
Definition: base-estimator.hh:216
dynamicgraph::sot::torque_control::BaseEstimator::m_right_foot_is_stable
bool m_right_foot_is_stable
True if left foot as been stable for the last 'm_fz_stable_windows_size' samples.
Definition: base-estimator.hh:174
PROFILE_BASE_VELOCITY_ESTIMATION
#define PROFILE_BASE_VELOCITY_ESTIMATION
Definition: base-estimator.cpp:73
dynamicgraph::sot::torque_control::BaseEstimator::kinematics_estimation
void kinematics_estimation(const Vector6 &ft, const Vector6 &K, const SE3 &oMfs, const int foot_id, SE3 &oMff, SE3 &oMfa, SE3 &fsMff)
Definition: base-estimator.cpp:442
dynamicgraph::sot::torque_control::BaseEstimator::m_zmp_std_dev_lf
double m_zmp_std_dev_lf
standard deviation of ZMP measurement errors for right foot
Definition: base-estimator.hh:182
dynamicgraph::sot::torque_control::BaseEstimator::m_oMrfs_xyzquat
Vector7 m_oMrfs_xyzquat
Definition: base-estimator.hh:218
dynamicgraph::sot::torque_control::BaseEstimator::m_w_imu
double m_w_imu
counter for detecting for how long the feet has been stable
Definition: base-estimator.hh:180
dynamicgraph::sot::torque_control::BaseEstimator::m_fz_std_dev_rf
double m_fz_std_dev_rf
standard deviation of ZMP measurement errors for left foot
Definition: base-estimator.hh:183
dynamicgraph::sot::torque_control::BaseEstimator::compute_zmp
void compute_zmp(const Vector6 &w, Vector2 &zmp)
Definition: base-estimator.cpp:360
dynamicgraph::sot::torque_control::rpyToMatrix
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d &R)
Definition: base-estimator.cpp:31
dynamicgraph::sot::torque_control::BaseEstimator::init
void init(const double &dt, const std::string &urdfFile)
Definition: base-estimator.cpp:229
dynamicgraph::sot::torque_control::BaseEstimator::m_fz_stable_windows_size
int m_fz_stable_windows_size
True if right foot as been stable for the last 'm_fz_stable_windows_size' samples.
Definition: base-estimator.hh:175
commands-helper.hh
dynamicgraph::sot::torque_control::BaseEstimator::m_alpha_DC_acc
double m_alpha_DC_acc
acceleration of the base in the world with DC component removed
Definition: base-estimator.hh:203
dynamicgraph::sot::torque_control::BaseEstimator::m_reset_foot_pos
bool m_reset_foot_pos
true if the entity has been successfully initialized
Definition: base-estimator.hh:169
dynamicgraph::sot::torque_control::BaseEstimator::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: base-estimator.hh:163
dynamicgraph::sot::torque_control::BaseEstimator::set_stiffness_left_foot
void set_stiffness_left_foot(const dynamicgraph::Vector &k)
Definition: base-estimator.cpp:317
dynamicgraph::sot::torque_control::BaseEstimator::compute_force_weight
double compute_force_weight(double fz, double std_dev, double margin)
Definition: base-estimator.cpp:382
dynamicgraph::sot::torque_control::BaseEstimator::m_oMlfs_xyzquat
Vector7 m_oMlfs_xyzquat
transformation from world to right foot sole
Definition: base-estimator.hh:217
dynamicgraph::sot::torque_control::BaseEstimator::compute_zmp_weight
double compute_zmp_weight(const Vector2 &zmp, const Vector4 &foot_sizes, double std_dev, double margin)
Definition: base-estimator.cpp:368
dynamicgraph::sot::torque_control::BaseEstimator::set_left_foot_sizes
void set_left_foot_sizes(const dynamicgraph::Vector &s)
Definition: base-estimator.cpp:347
dynamicgraph::sot::torque_control::BaseEstimator::m_zmp_std_dev_rf
double m_zmp_std_dev_rf
weight of IMU for sensor fusion
Definition: base-estimator.hh:181
PROFILE_BASE_POSITION_ESTIMATION
#define PROFILE_BASE_POSITION_ESTIMATION
Definition: base-estimator.cpp:72
dynamicgraph::sot::torque_control::BaseEstimator::BaseEstimator
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BaseEstimator(const std::string &name)
Definition: base-estimator.cpp:95
dynamicgraph::sot::torque_control::BaseEstimator::m_v_kin
Eigen::VectorXd m_v_kin
6d stiffness of left foot spring
Definition: base-estimator.hh:194
dynamicgraph::sot::torque_control::BaseEstimator::m_v_pin
Eigen::VectorXd m_v_pin
robot configuration according to SoT convention
Definition: base-estimator.hh:233
dynamicgraph::sot::torque_control::BaseEstimator::m_left_foot_sizes
Vector4 m_left_foot_sizes
standard deviation of normal force measurement errors for left foot
Definition: base-estimator.hh:185
OUTPUT_SIGNALS
#define OUTPUT_SIGNALS
Definition: base-estimator.cpp:80
dynamicgraph::sot::torque_control::quanternionMult
void quanternionMult(const Eigen::Vector4d &q1, const Eigen::Vector4d &q2, Eigen::Vector4d &q12)
Definition: base-estimator.cpp:53
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::BaseEstimator::m_last_vel
Vector3 m_last_vel
base orientation in the world
Definition: base-estimator.hh:239
dynamicgraph::sot::torque_control::BaseEstimator::m_left_foot_is_stable
bool m_left_foot_is_stable
Definition: base-estimator.hh:173
dynamicgraph::sot::torque_control::BaseEstimator::m_zmp_margin_lf
double m_zmp_margin_lf
sizes of the left foot (pos x, neg x, pos y, neg y)
Definition: base-estimator.hh:187
dynamicgraph::sot::torque_control::BaseEstimator::m_q_pin
Eigen::VectorXd m_q_pin
Definition: base-estimator.hh:231
dynamicgraph::sot::torque_control::dummy
int dummy
Definition: torque-offset-estimator.cpp:30
dynamicgraph::sot::torque_control::DEFINE_SIGNAL_INNER_FUNCTION
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
Definition: base-estimator.cpp:461
dynamicgraph::sot::torque_control::se3Interp
void se3Interp(const pinocchio::SE3 &s1, const pinocchio::SE3 &s2, const double alpha, pinocchio::SE3 &s12)
Definition: base-estimator.cpp:24
dynamicgraph::sot::torque_control::Vector2
Eigen::Matrix< double, 2, 1 > Vector2
Definition: inverse-dynamics-balance-controller.cpp:192
dynamicgraph::sot::torque_control::BaseEstimator::m_oMrfs_default_ref
SE3 m_oMrfs_default_ref
Default reference for left foot pose to use if no ref is pluged.
Definition: base-estimator.hh:220
dynamicgraph::sot::torque_control::BaseEstimator::m_fz_std_dev_lf
double m_fz_std_dev_lf
standard deviation of normal force measurement errors for right foot
Definition: base-estimator.hh:184
INPUT_SIGNALS
#define INPUT_SIGNALS
Definition: base-estimator.cpp:76
dynamicgraph::sot::torque_control::BaseEstimator::m_right_foot_sizes
Vector4 m_right_foot_sizes
sizes of the left foot (pos x, neg x, pos y, neg y)
Definition: base-estimator.hh:186
dynamicgraph::sot::torque_control::BaseEstimator::m_oMlfs_default_ref
SE3 m_oMlfs_default_ref
Definition: base-estimator.hh:219
dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
dynamicgraph::sot::torque_control::BaseEstimator::m_model
pinocchio::Model m_model
filtered weight of the estimation coming from the right foot
Definition: base-estimator.hh:211
dynamicgraph::sot::torque_control::Vector6
Eigen::Matrix< double, 6, 1 > Vector6
Definition: admittance-controller.cpp:47
dynamicgraph::sot::torque_control::BaseEstimator::display
virtual void display(std::ostream &os) const
filtered weight of the estimation coming from the right foot
Definition: base-estimator.cpp:1044
dynamicgraph::sot::torque_control::BaseEstimator::m_K_rf
Vector6 m_K_rf
margin used for computing normal force weight
Definition: base-estimator.hh:191
dynamicgraph::sot::torque_control::BaseEstimator::m_v_imu
Eigen::VectorXd m_v_imu
6d robot velocities from flexibility only (force sensor derivative)
Definition: base-estimator.hh:196
PROFILE_BASE_KINEMATICS_COMPUTATION
#define PROFILE_BASE_KINEMATICS_COMPUTATION
Definition: base-estimator.cpp:74
dynamicgraph::sot::torque_control::BaseEstimator::set_fz_stable_windows_size
void set_fz_stable_windows_size(const int &ws)
Definition: base-estimator.cpp:285
dynamicgraph::sot::torque_control::BaseEstimator::set_right_foot_sizes
void set_right_foot_sizes(const dynamicgraph::Vector &s)
Definition: base-estimator.cpp:342
dynamicgraph::sot::torque_control::BaseEstimator::set_alpha_DC_acc
void set_alpha_DC_acc(const double &a)
Definition: base-estimator.cpp:295
dynamicgraph::sot::torque_control::BaseEstimator::m_lf_fz_stable_cpt
int m_lf_fz_stable_cpt
size of the windows used to detect that feet did not leave the ground
Definition: base-estimator.hh:176
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::BaseEstimator::set_zmp_margin_right_foot
void set_zmp_margin_right_foot(const double &margin)
Definition: base-estimator.cpp:352
dynamicgraph::sot::torque_control::BaseEstimator::m_alpha_DC_vel
double m_alpha_DC_vel
alpha parameter for DC blocker filter on acceleration data
Definition: base-estimator.hh:204
dynamicgraph::sot::torque_control::BaseEstimator::set_normal_force_std_dev_left_foot
void set_normal_force_std_dev_left_foot(const double &std_dev)
Definition: base-estimator.cpp:337
dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
Definition: admittance-controller.cpp:160
base-estimator.hh
dynamicgraph::sot::torque_control::BaseEstimator::m_v_gyr
Eigen::VectorXd m_v_gyr
6d robot velocities form imu only (accelerometer integration + gyro)
Definition: base-estimator.hh:198
dynamicgraph::sot::torque_control::BaseEstimator::m_last_DCacc
Vector3 m_last_DCacc
Definition: base-estimator.hh:241
dynamicgraph::sot::torque_control::BaseEstimator::m_rf_fz_stable_cpt
int m_rf_fz_stable_cpt
counter for detecting for how long the feet has been stable
Definition: base-estimator.hh:177
dynamicgraph::sot::torque_control::BaseEstimator::m_sole_M_ftSens
SE3 m_sole_M_ftSens
flag to detect first iteration and initialise velocity filters
Definition: base-estimator.hh:225
dynamicgraph::sot::torque_control::BaseEstimator::set_zmp_margin_left_foot
void set_zmp_margin_left_foot(const double &margin)
Definition: base-estimator.cpp:354
dynamicgraph::sot::torque_control::BaseEstimator::m_v_sot
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
Definition: base-estimator.hh:234
dynamicgraph::sot::torque_control::BaseEstimator::set_alpha_w_filter
void set_alpha_w_filter(const double &a)
Definition: base-estimator.cpp:290
dynamicgraph::sot::torque_control::matrixToRpy
void matrixToRpy(const Eigen::Matrix3d &M, Eigen::Vector3d &rpy)
Definition: base-estimator.cpp:41
dynamicgraph::sot::torque_control::BaseEstimator::m_isFirstIterFlag
bool m_isFirstIterFlag
Normal distribution.
Definition: base-estimator.hh:223
dynamicgraph::sot::torque_control::BaseEstimator::set_zmp_std_dev_right_foot
void set_zmp_std_dev_right_foot(const double &std_dev)
Definition: base-estimator.cpp:322
dynamicgraph::sot::torque_control::BaseEstimator::m_last_DCvel
Vector3 m_last_DCvel
Definition: base-estimator.hh:240
dynamicgraph::sot::torque_control::BaseEstimator::m_zmp_margin_rf
double m_zmp_margin_rf
margin used for computing zmp weight
Definition: base-estimator.hh:188
dynamicgraph::sot::torque_control::BaseEstimator::m_fz_margin_lf
double m_fz_margin_lf
margin used for computing zmp weight
Definition: base-estimator.hh:189
dynamicgraph::sot::torque_control::BaseEstimator::m_right_foot_id
pinocchio::FrameIndex m_right_foot_id
foot sole to F/T sensor transformation
Definition: base-estimator.hh:227
dynamicgraph::sot::torque_control::BaseEstimator::set_imu_weight
void set_imu_weight(const double &w)
Definition: base-estimator.cpp:307
dynamicgraph::sot::torque_control::BaseEstimator::m_alpha_w_filter
double m_alpha_w_filter
alpha parameter for DC blocker filter on velocity data
Definition: base-estimator.hh:206
dynamicgraph::sot::torque_control::BaseEstimator::m_IMU_body_id
pinocchio::FrameIndex m_IMU_body_id
Definition: base-estimator.hh:229
dynamicgraph::sot::torque_control::BaseEstimator::m_left_foot_id
pinocchio::FrameIndex m_left_foot_id
Definition: base-estimator.hh:228
dynamicgraph::sot::torque_control::BaseEstimator::m_oMlfs
SE3 m_oMlfs
world-to-base transformation obtained through right foot
Definition: base-estimator.hh:215