sot-talos-balance  1.6.0
talos-base-estimator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017, Thomas Flayols, LAAS-CNRS
3  *
4  * This file is part of sot-torque-control.
5  * sot-torque-control is free software: you can redistribute it and/or
6  * modify it under the terms of the GNU Lesser General Public License
7  * as published by the Free Software Foundation, either version 3 of
8  * the License, or (at your option) any later version.
9  * sot-torque-control is distributed in the hope that it will be
10  * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details. You should
13  * have received a copy of the GNU Lesser General Public License along
14  * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
15  */
16 
18 #include <sot/core/debug.hh>
19 #include <dynamic-graph/factory.h>
20 #include <dynamic-graph/all-commands.h>
21 #include <sot/core/stop-watch.hh>
22 #include "pinocchio/algorithm/frames.hpp"
23 
24 namespace dynamicgraph
25 {
26  namespace sot
27  {
28  namespace talos_balance
29  {
30  namespace dg = ::dynamicgraph;
31  using namespace dg;
32  using namespace dg::command;
33  using namespace std;
34  using namespace pinocchio;
35  using boost::math::normal; // typedef provides default type is double.
36 
37  void se3Interp(const pinocchio::SE3 & s1,
38  const pinocchio::SE3 & s2,
39  const double alpha,
40  pinocchio::SE3 & s12)
41  {
42  const Eigen::Vector3d t_( s1.translation() * alpha+
43  s2.translation() * (1-alpha));
44 
45  const Eigen::Vector3d w( pinocchio::log3(s1.rotation()) * alpha +
46  pinocchio::log3(s2.rotation()) * (1-alpha) );
47 
48  s12 = pinocchio::SE3(pinocchio::exp3(w),t_);
49  }
50 
51  void rpyToMatrix(double roll,
52  double pitch, double yaw, Eigen::Matrix3d & R)
53  {
54  Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
55  Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
56  Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
57  Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
58  R = q.matrix();
59  }
60 
61  void rpyToMatrix(const Eigen::Vector3d & rpy, Eigen::Matrix3d & R)
62  {
63  return rpyToMatrix(rpy[0], rpy[1], rpy[2], R);
64  }
65 
66  void matrixToRpy(const Eigen::Matrix3d & M, Eigen::Vector3d & rpy)
67  {
68  double m = sqrt(M(2,1)*M(2,1) + M(2,2)*M(2,2));
69  rpy(1) = atan2(-M(2,0), m);
70  if( fabs(fabs(rpy(1)) - 0.5*M_PI) < 0.001 )
71  {
72  rpy(0) = 0.0;
73  rpy(2) = -atan2(M(0,1), M(1,1));
74  }
75  else
76  {
77  rpy(2) = atan2(M(1,0), M(0,0)); // alpha
78  rpy(0) = atan2(M(2,1), M(2,2)); // gamma
79  }
80  }
81 
82 /* // ATTENTION! Quaternion elements are assumed to be [w,x,y,z]
83  void quanternionMult(const Eigen::Vector4d & q1,
84  const Eigen::Vector4d & q2,
85  Eigen::Vector4d & q12)
86  {
87  q12(0) = q2(0)*q1(0)-q2(1)*q1(1)-q2(2)*q1(2)-q2(3)*q1(3);
88  q12(1) = q2(0)*q1(1)+q2(1)*q1(0)-q2(2)*q1(3)+q2(3)*q1(2);
89  q12(2) = q2(0)*q1(2)+q2(1)*q1(3)+q2(2)*q1(0)-q2(3)*q1(1);
90  q12(3) = q2(0)*q1(3)-q2(1)*q1(2)+q2(2)*q1(1)+q2(3)*q1(0);
91  }
92 
93  void pointRotationByQuaternion
94  (const Eigen::Vector3d & point,
95  const Eigen::Vector4d & quat,
96  Eigen::Vector3d & rotatedPoint)
97  {
98  const Eigen::Vector4d p4(0.0, point(0),point(1),point(2));
99  const Eigen::Vector4d quat_conj(quat(0),-quat(1),-quat(2),-quat(3));
100  Eigen::Vector4d q_tmp1,q_tmp2;
101  quanternionMult(quat,p4,q_tmp1);
102  quanternionMult(q_tmp1,quat_conj,q_tmp2);
103  rotatedPoint(0) = q_tmp2(1);
104  rotatedPoint(1) = q_tmp2(2);
105  rotatedPoint(2) = q_tmp2(3);
106  }
107 */
108 
109  inline
110  double eulerMean(double a1, double a2)
111  {
112  double mean = (a1+a2)/2.;
113  if( (a1- a2) > EIGEN_PI || (a1- a2) < -EIGEN_PI)
114  {
115  return mean > 0 ? -EIGEN_PI + mean : EIGEN_PI + mean ;
116  }
117  return mean;
118  }
119 
120  inline
121  double wEulerMean(double a1, double a2, double w1, double w2)
122  {
123  double wMean = (a1*w1+ a2*w2)/(w1+w2);
124  if ( a1-a2 >= EIGEN_PI )
125  return (EIGEN_PI*(w1-w2)/(w2+w1) - wMean)
126  < 0 ? -EIGEN_PI +
127  wMean - EIGEN_PI*(w1-w2)/(w2+w1) : EIGEN_PI +
128  wMean - EIGEN_PI*(w1-w2)/(w2+w1);
129  else if ( a1-a2 < -EIGEN_PI )
130  return (EIGEN_PI*(w2-w1)/(w1+w2) - wMean)
131  < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2) :
132  EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2);
133  return wMean;
134  }
135 
136  // inline
137  // double wEulerMean(double a1, double a2, double w1, double w2)
138  // {
139  // double wMean = (a1*w1+ a2*w2)/(w1+w2);
140  // double na,naw,pa,paw;
141  // if ( a1-a2 >= EIGEN_PI )
142  // {
143  // na = a2; //negative angle
144  // naw = w2; //negative angle weight
145  // pa = a1; //positive angle
146  // paw = w1; //positive angle weight
147  // double piFac = (paw-naw)/(naw+paw);
148  // return (EIGEN_PI*piFac - wMean) < 0 ? -EIGEN_PI +
149  // wMean - EIGEN_PI*piFac : EIGEN_PI + wMean - EIGEN_PI*piFac;
150  // }
151  // else if ( a1-a2 < -EIGEN_PI )
152  // {
153  // na = a1; //negative angle
154  // naw = w1; //negative angle weight
155  // pa = a2; //positive angle
156  // paw = w2; //positive angle weight
157  // double piFac = (paw-naw)/(naw+paw);
158  // return (EIGEN_PI*piFac - wMean) < 0 ? -EIGEN_PI + wMean -
159  // EIGEN_PI*piFac : EIGEN_PI + wMean - EIGEN_PI*piFac;
160  // }
161  // return wMean;
162  // }
163 
164 #define PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation"
165 #define PROFILE_BASE_VELOCITY_ESTIMATION "base-est velocity estimation"
166 #define PROFILE_BASE_KINEMATICS_COMPUTATION "base-est kinematics computation"
167 
168 
169 #define INPUT_SIGNALS m_joint_positionsSIN << m_joint_velocitiesSIN << \
170  m_imu_quaternionSIN << m_forceLLEGSIN << m_forceRLEGSIN << m_dforceLLEGSIN << m_dforceRLEGSIN << \
171  m_w_lf_inSIN << m_w_rf_inSIN << m_K_fb_feet_posesSIN << m_lf_ref_xyzquatSIN << m_rf_ref_xyzquatSIN << m_accelerometerSIN << m_gyroscopeSIN
172 #define OUTPUT_SIGNALS m_qSOUT << m_vSOUT << m_q_lfSOUT << m_q_rfSOUT << m_q_imuSOUT << \
173  m_w_lfSOUT << m_w_rfSOUT << m_w_lf_filteredSOUT << m_w_rf_filteredSOUT << m_lf_xyzquatSOUT << m_rf_xyzquatSOUT << \
174  m_v_acSOUT << m_a_acSOUT << m_v_kinSOUT << m_v_imuSOUT << m_v_gyrSOUT << m_v_flexSOUT
175 
178  typedef TalosBaseEstimator EntityClassName;
179 
180  /* --- DG FACTORY ---------------------------------------------------- */
181  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TalosBaseEstimator,
182  "TalosBaseEstimator");
183 
184  /* ------------------------------------------------------------------- */
185  /* --- CONSTRUCTION -------------------------------------------------- */
186  /* ------------------------------------------------------------------- */
188  TalosBaseEstimator(const std::string& name)
189  : Entity(name)
190  ,CONSTRUCT_SIGNAL_IN( joint_positions, dynamicgraph::Vector)
191  ,CONSTRUCT_SIGNAL_IN( joint_velocities, dynamicgraph::Vector)
192  ,CONSTRUCT_SIGNAL_IN( imu_quaternion, dynamicgraph::Vector)
193  ,CONSTRUCT_SIGNAL_IN( forceLLEG, dynamicgraph::Vector)
194  ,CONSTRUCT_SIGNAL_IN( forceRLEG, dynamicgraph::Vector)
195  ,CONSTRUCT_SIGNAL_IN( dforceLLEG, dynamicgraph::Vector)
196  ,CONSTRUCT_SIGNAL_IN( dforceRLEG, dynamicgraph::Vector)
197  ,CONSTRUCT_SIGNAL_IN( w_lf_in, double)
198  ,CONSTRUCT_SIGNAL_IN( w_rf_in, double)
199  ,CONSTRUCT_SIGNAL_IN( K_fb_feet_poses, double)
200  ,CONSTRUCT_SIGNAL_IN( lf_ref_xyzquat, dynamicgraph::Vector)
201  ,CONSTRUCT_SIGNAL_IN( rf_ref_xyzquat, dynamicgraph::Vector)
202  ,CONSTRUCT_SIGNAL_IN( accelerometer, dynamicgraph::Vector)
203  ,CONSTRUCT_SIGNAL_IN( gyroscope, dynamicgraph::Vector)
204  ,CONSTRUCT_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector, m_joint_positionsSIN
205  << m_joint_velocitiesSIN)
206  ,CONSTRUCT_SIGNAL_OUT(q, dynamicgraph::Vector, m_kinematics_computationsSINNER
207  << m_joint_positionsSIN
208  << m_imu_quaternionSIN
209  << m_forceLLEGSIN
210  << m_forceRLEGSIN
211  << m_w_lf_inSIN
212  << m_w_rf_inSIN
213  << m_K_fb_feet_posesSIN
214  << m_w_lf_filteredSOUT
215  << m_w_rf_filteredSOUT
216  << m_lf_ref_xyzquatSIN
217  << m_rf_ref_xyzquatSIN)
218  ,CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector, m_kinematics_computationsSINNER
219  << m_accelerometerSIN
220  << m_gyroscopeSIN
221  << m_qSOUT
222  << m_dforceLLEGSIN
223  << m_dforceRLEGSIN)
224  ,CONSTRUCT_SIGNAL_OUT(v_kin, dynamicgraph::Vector, m_vSOUT)
225  ,CONSTRUCT_SIGNAL_OUT(v_flex, dynamicgraph::Vector, m_vSOUT)
226  ,CONSTRUCT_SIGNAL_OUT(v_imu, dynamicgraph::Vector, m_vSOUT)
227  ,CONSTRUCT_SIGNAL_OUT(v_gyr, dynamicgraph::Vector, m_vSOUT)
228  ,CONSTRUCT_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector, m_qSOUT)
229  ,CONSTRUCT_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector, m_qSOUT)
230  ,CONSTRUCT_SIGNAL_OUT(a_ac, dynamicgraph::Vector, m_vSOUT)
231  ,CONSTRUCT_SIGNAL_OUT(v_ac, dynamicgraph::Vector, m_vSOUT)
232  ,CONSTRUCT_SIGNAL_OUT(q_lf, dynamicgraph::Vector, m_qSOUT)
233  ,CONSTRUCT_SIGNAL_OUT(q_rf, dynamicgraph::Vector, m_qSOUT)
234  ,CONSTRUCT_SIGNAL_OUT(q_imu, dynamicgraph::Vector, m_qSOUT
235  << m_imu_quaternionSIN)
236  ,CONSTRUCT_SIGNAL_OUT(w_lf, double, m_forceLLEGSIN)
237  ,CONSTRUCT_SIGNAL_OUT(w_rf, double, m_forceRLEGSIN)
238  ,CONSTRUCT_SIGNAL_OUT(w_lf_filtered, double, m_w_lfSOUT)
239  ,CONSTRUCT_SIGNAL_OUT(w_rf_filtered, double, m_w_rfSOUT)
240  ,m_initSucceeded(false)
241  ,m_reset_foot_pos(true)
242  ,m_w_imu(0.0)
243  ,m_zmp_std_dev_rf(1.0)
244  ,m_zmp_std_dev_lf(1.0)
245  ,m_fz_std_dev_rf(1.0)
246  ,m_fz_std_dev_lf(1.0)
247  ,m_zmp_margin_lf(0.0)
248  ,m_zmp_margin_rf(0.0)
249  {
250  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
251 
252  //m_K_rf << 4034,23770,239018,707,502,936;
253  //m_K_lf << 4034,23770,239018,707,502,936;
254  m_left_foot_sizes << 0.130, -0.100, 0.075, -0.056;
255  m_right_foot_sizes << 0.130, -0.100, 0.056, -0.075;
256 
257  /* Commands. */
258  addCommand("init",
259  makeCommandVoid2(*this, &TalosBaseEstimator::init,
260  docCommandVoid2("Initialize the entity.",
261  "time step (double)",
262  "URDF file path (string)")));
263 
264 
265  addCommand("set_fz_stable_windows_size",
266  makeCommandVoid1(*this, &TalosBaseEstimator::set_fz_stable_windows_size,
267  docCommandVoid1("Set the windows size used to detect that feet is stable",
268  "int")));
269  addCommand("set_alpha_w_filter",
270  makeCommandVoid1(*this, &TalosBaseEstimator::set_alpha_w_filter,
271  docCommandVoid1("Set the filter parameter to filter weights",
272  "double")));
273  addCommand("set_alpha_DC_acc",
274  makeCommandVoid1(*this, &TalosBaseEstimator::set_alpha_DC_acc,
275  docCommandVoid1("Set the filter parameter for removing DC from accelerometer data",
276  "double")));
277  addCommand("set_alpha_DC_vel",
278  makeCommandVoid1(*this, &TalosBaseEstimator::set_alpha_DC_vel,
279  docCommandVoid1("Set the filter parameter for removing DC from velocity integrated from acceleration",
280  "double")));
281  addCommand("reset_foot_positions",
282  makeCommandVoid0(*this, &TalosBaseEstimator::reset_foot_positions,
283  docCommandVoid0("Reset the position of the feet.")));
284  addCommand("get_imu_weight",
285  makeDirectGetter(*this,&m_w_imu,
286  docDirectGetter("Weight of imu-based orientation in sensor fusion",
287  "double")));
288  addCommand("set_imu_weight",
289  makeCommandVoid1(*this, &TalosBaseEstimator::set_imu_weight,
290  docCommandVoid1("Set the weight of imu-based orientation in sensor fusion",
291  "double")));
292  addCommand("set_zmp_std_dev_right_foot",
293  makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_std_dev_right_foot,
294  docCommandVoid1("Set the standard deviation of the ZMP measurement errors for the right foot",
295  "double")));
296  addCommand("set_zmp_std_dev_left_foot",
297  makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_std_dev_left_foot,
298  docCommandVoid1("Set the standard deviation of the ZMP measurement errors for the left foot",
299  "double")));
300  addCommand("set_normal_force_std_dev_right_foot",
302  docCommandVoid1("Set the standard deviation of the normal force measurement errors for the right foot",
303  "double")));
304  addCommand("set_normal_force_std_dev_left_foot",
306  docCommandVoid1("Set the standard deviation of the normal force measurement errors for the left foot",
307  "double")));
308  addCommand("set_stiffness_right_foot",
309  makeCommandVoid1(*this, &TalosBaseEstimator::set_stiffness_right_foot,
310  docCommandVoid1("Set the 6d stiffness of the spring at the right foot",
311  "vector")));
312  addCommand("set_stiffness_left_foot",
313  makeCommandVoid1(*this, &TalosBaseEstimator::set_stiffness_left_foot,
314  docCommandVoid1("Set the 6d stiffness of the spring at the left foot",
315  "vector")));
316  addCommand("set_right_foot_sizes",
317  makeCommandVoid1(*this, &TalosBaseEstimator::set_right_foot_sizes,
318  docCommandVoid1("Set the size of the right foot (pos x, neg x, pos y, neg y)",
319  "4d vector")));
320  addCommand("set_left_foot_sizes",
321  makeCommandVoid1(*this, &TalosBaseEstimator::set_left_foot_sizes,
322  docCommandVoid1("Set the size of the left foot (pos x, neg x, pos y, neg y)",
323  "4d vector")));
324  addCommand("set_zmp_margin_right_foot",
325  makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_margin_right_foot,
326  docCommandVoid1("Set the ZMP margin for the right foot",
327  "double")));
328  addCommand("set_zmp_margin_left_foot",
329  makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_margin_left_foot,
330  docCommandVoid1("Set the ZMP margin for the left foot",
331  "double")));
332  addCommand("set_normal_force_margin_right_foot",
334  docCommandVoid1("Set the normal force margin for the right foot",
335  "double")));
336  addCommand("set_normal_force_margin_left_foot",
338  docCommandVoid1("Set the normal force margin for the left foot",
339  "double")));
340  }
341 
342  void TalosBaseEstimator::init(const double & dt, const std::string& robotRef)
343  {
344  m_dt = dt;
345  try
346  {
347  /* Retrieve m_robot_util informations */
348  std::string localName(robotRef);
349  if (isNameInRobotUtil(localName))
350  {
351  m_robot_util = getRobotUtil(localName);
352  std::cerr << "m_robot_util:" << m_robot_util << std::endl;
353  }
354  else
355  {
356  SEND_MSG("You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
357  return;
358  }
359 
360  pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename,
361  pinocchio::JointModelFreeFlyer(), m_model);
362 
363  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
364  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
365  assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
366  m_left_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
367  m_right_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
368  m_IMU_frame_id = m_model.getFrameId(m_robot_util->m_imu_joint_name);
369  m_torso_id = m_model.frames[m_IMU_frame_id].parent;
370  m_torsoMimu = m_model.frames[m_IMU_frame_id].placement;
371  std::cerr << "IMU in Torso" << m_torsoMimu << std::endl;
372 
373  m_q_pin.setZero(m_model.nq);
374  m_q_pin[6]= 1.; // for quaternion
375  m_q_sot.setZero(m_robot_util->m_nbJoints+6);
376  m_v_pin.setZero(m_robot_util->m_nbJoints+6);
377  m_v_sot.setZero(m_robot_util->m_nbJoints+6);
378  m_v_kin.setZero(m_robot_util->m_nbJoints+6);
379  m_v_flex.setZero(m_robot_util->m_nbJoints+6);
380  m_v_imu.setZero(m_robot_util->m_nbJoints+6);
381  m_v_gyr.setZero(m_robot_util->m_nbJoints+6);
382  m_sole_M_ftSens = SE3(Matrix3::Identity(),
383  -Eigen::Map<const Vector3>(&m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ(0)));
384 
385  m_last_vel.setZero();
386  m_last_DCvel.setZero();
387  m_last_DCacc.setZero(); //this is to replace by acceleration at 1st iteration
388 
389  m_alpha_DC_acc = 0.9995;
390  m_alpha_DC_vel = 0.9995;
391  m_alpha_w_filter = 1.0;
392  m_left_foot_is_stable = true;
393  m_right_foot_is_stable = true;
397  m_isFirstIterFlag = true;
398  }
399  catch (const std::exception& e)
400  {
401  std::cout << e.what();
402  SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
403  return;
404  }
405  m_data = new pinocchio::Data(m_model);
406  m_initSucceeded = true;
407  }
408 
410  {
411  if(ws<0.0)
412  return SEND_MSG("windows_size should be a positive integer", MSG_TYPE_ERROR);
414  }
415 
417  {
418  if(a<0.0 || a>1.0)
419  return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
420  m_alpha_w_filter = a;
421  }
422 
423 
425  {
426  if(a<0.0 || a>1.0)
427  return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
428  m_alpha_DC_acc = a;
429  }
430 
432  {
433  if(a<0.0 || a>1.0)
434  return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
435  m_alpha_DC_vel = a;
436  }
437 
438 
440  {
441  m_reset_foot_pos = true;
442  }
443 
445  {
446  if(w<0.0)
447  return SEND_MSG("Imu weight must be nonnegative", MSG_TYPE_ERROR);
448  m_w_imu = w;
449  }
450 
452  {
453  if(k.size()!=6)
454  return SEND_MSG("Stiffness vector should have size 6, not "+toString(k.size()), MSG_TYPE_ERROR);
455  m_K_rf = k;
456  }
457 
459  {
460  if(k.size()!=6)
461  return SEND_MSG("Stiffness vector should have size 6, not "+toString(k.size()), MSG_TYPE_ERROR);
462  m_K_lf = k;
463  }
464 
466  {
467  if(std_dev<=0.0)
468  return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
469  m_zmp_std_dev_rf = std_dev;
470  }
471 
473  {
474  if(std_dev<=0.0)
475  return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
476  m_zmp_std_dev_lf = std_dev;
477  }
478 
480  {
481  if(std_dev<=0.0)
482  return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
483  m_fz_std_dev_rf = std_dev;
484  }
485 
487  {
488  if(std_dev<=0.0)
489  return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
490  m_fz_std_dev_lf = std_dev;
491  }
492 
494  {
495  if(s.size()!=4)
496  return SEND_MSG("Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
497  m_right_foot_sizes = s;
498  }
499 
501  {
502  if(s.size()!=4)
503  return SEND_MSG("Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
504  m_left_foot_sizes = s;
505  }
506 
508  {
509  m_zmp_margin_rf = margin;
510  }
511 
513  {
514  m_zmp_margin_lf = margin;
515  }
516 
518  {
519  m_fz_margin_rf = margin;
520  }
521 
523  {
524  m_fz_margin_lf = margin;
525  }
526 
527  void TalosBaseEstimator::compute_zmp(const Vector6 & w, Vector2 & zmp)
528  {
529  pinocchio::Force f(w);
530  f = m_sole_M_ftSens.act(f);
531  if(f.linear()[2]==0.0)
532  return;
533  zmp[0] = -f.angular()[1] / f.linear()[2];
534  zmp[1] = f.angular()[0] / f.linear()[2];
535  }
536 
537  double TalosBaseEstimator::compute_zmp_weight(const Vector2 & zmp, const Vector4 & foot_sizes,
538  double std_dev, double margin)
539  {
540  double fs0=foot_sizes[0] - margin;
541  double fs1=foot_sizes[1] + margin;
542  double fs2=foot_sizes[2] - margin;
543  double fs3=foot_sizes[3] + margin;
544 
545  if(zmp[0]>fs0 || zmp[0]<fs1 || zmp[1]>fs2 || zmp[1]<fs3)
546  return 0;
547 
548  double cdx = ((cdf(m_normal, (fs0-zmp[0])/std_dev) -
549  cdf(m_normal, (fs1-zmp[0])/std_dev))-0.5 )*2.0;
550  double cdy = ((cdf(m_normal, (fs2-zmp[1])/std_dev) -
551  cdf(m_normal, (fs3-zmp[1])/std_dev))-0.5 )*2.0;
552  return cdx*cdy;
553  }
554 
555  double TalosBaseEstimator::compute_force_weight(double fz, double std_dev, double margin)
556  {
557  if (fz<margin)
558  return 0.0;
559  return (cdf(m_normal, (fz-margin)/std_dev)-0.5)*2.0;
560  }
561 
562  void TalosBaseEstimator::reset_foot_positions_impl(const Vector6 & ftlf, const Vector6 & ftrf)
563  {
564  // compute the base position wrt each foot
565  SE3 dummy, dummy1, lfMff, rfMff;
566  m_oMrfs = SE3::Identity();
567  m_oMlfs = SE3::Identity();
568  kinematics_estimation(ftrf, m_K_rf, m_oMrfs, m_right_foot_id, rfMff, dummy, dummy1); //rfMff is obtained reading oMff becaused oMrfs is here set to Identity
569  kinematics_estimation(ftlf, m_K_lf, m_oMlfs, m_left_foot_id, lfMff, dummy, dummy1);
570 
571  // distance from ankle to ground
572  const Vector3 & ankle_2_sole_xyz = m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ;
573  const SE3 groundMfoot(Matrix3::Identity(), -1.0*ankle_2_sole_xyz);
574  lfMff = groundMfoot * lfMff;
575  rfMff = groundMfoot * rfMff;
576 
577  // set the world frame in between the feet
578  const Vector3 w( 0.5*(pinocchio::log3(lfMff.rotation())+pinocchio::log3(rfMff.rotation())) );
579  SE3 oMff = SE3(pinocchio::exp3(w), 0.5*(lfMff.translation()+rfMff.translation()));
580  // add a constant offset to make the world frame match the one in OpenHRP
581  // oMff.translation()(0) += 9.562e-03;
582 
583  m_oMlfs = oMff * lfMff.inverse() * groundMfoot;
584  m_oMrfs = oMff * rfMff.inverse() * groundMfoot;
585 
586  m_oMlfs_xyzquat.head<3>() = m_oMlfs.translation();
587  Eigen::Quaternion<double> quat_lf(m_oMlfs.rotation());
588  m_oMlfs_xyzquat(3) = quat_lf.x();
589  m_oMlfs_xyzquat(4) = quat_lf.y();
590  m_oMlfs_xyzquat(5) = quat_lf.z();
591  m_oMlfs_xyzquat(6) = quat_lf.w();
592 
593  m_oMrfs_xyzquat.head<3>() = m_oMrfs.translation();
594  Eigen::Quaternion<double> quat_rf(m_oMrfs.rotation());
595  m_oMrfs_xyzquat(3) = quat_rf.x();
596  m_oMrfs_xyzquat(4) = quat_rf.y();
597  m_oMrfs_xyzquat(5) = quat_rf.z();
598  m_oMrfs_xyzquat(6) = quat_rf.w();
599 
600  //save this poses to use it if no ref is provided
603 
604  sendMsg("Reference pos of left foot:\n"+toString(m_oMlfs), MSG_TYPE_INFO);
605  sendMsg("Reference pos of right foot:\n"+toString(m_oMrfs), MSG_TYPE_INFO);
606 
607 // kinematics_estimation(ftrf, m_K_rf, m_oMrfs, m_right_foot_id, m_oMff_rf, dummy);
608 // kinematics_estimation(ftlf, m_K_lf, m_oMlfs, m_left_foot_id, m_oMff_lf, dummy);
609 // sendMsg("Base estimation left foot:\n"+toString(m_oMff_lf), MSG_TYPE_DEBUG);
610 // sendMsg("Base estimation right foot:\n"+toString(m_oMff_rf), MSG_TYPE_DEBUG);
611 // sendMsg("Difference base estimation left-right foot:\n"+toString(m_oMff_rf.inverse()*m_oMff_lf), MSG_TYPE_DEBUG);
612 
613  m_reset_foot_pos = false;
614  }
615 
616  void TalosBaseEstimator::kinematics_estimation(const Vector6 & ft, const Vector6 & K,
617  const SE3 & oMfs, const pinocchio::FrameIndex foot_id,
618  SE3 & oMff, SE3 & oMfa, SE3 & fsMff)
619  {
620  Vector3 xyz;
621  xyz << -ft[0]/K(0), -ft[1]/K(1), -ft[2]/K(2);
622  Matrix3 R;
623  rpyToMatrix(-ft[3]/K(3), -ft[4]/K(4), -ft[5]/K(5), R);
624  const SE3 fsMfa(R, xyz); // foot sole to foot ankle
625  oMfa = oMfs*fsMfa; // world to foot ankle
626  const SE3 faMff(m_data->oMf[foot_id].inverse()); // foot ankle to free flyer
627  //~ sendMsg("faMff (foot_id="+toString(foot_id)+"):\n" + toString(faMff), MSG_TYPE_INFO);
628  //~ sendMsg("fsMfa (foot_id="+toString(foot_id)+"):\n" + toString(fsMfa), MSG_TYPE_INFO);
629  oMff = oMfa*faMff; // world to free flyer
630  fsMff = fsMfa*faMff; // foot sole to free flyer
631  }
632 
633  /* ------------------------------------------------------------------- */
634  /* --- SIGNALS ------------------------------------------------------- */
635  /* ------------------------------------------------------------------- */
636 
638  {
639  if(!m_initSucceeded)
640  {
641  SEND_WARNING_STREAM_MSG("Cannot compute signal kinematics_computations before initialization!");
642  return s;
643  }
644 
645  const Eigen::VectorXd& qj= m_joint_positionsSIN(iter); //n+6
646  const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
647  assert(qj.size()==m_robot_util->m_nbJoints && "Unexpected size of signal joint_positions");
648  assert(dq.size()==m_robot_util->m_nbJoints && "Unexpected size of signal joint_velocities");
649 
650  /* convert sot to pinocchio joint order */
651  m_robot_util->joints_sot_to_urdf(qj, m_q_pin.tail(m_robot_util->m_nbJoints));
652  m_robot_util->joints_sot_to_urdf(dq, m_v_pin.tail(m_robot_util->m_nbJoints));
653 
654  getProfiler().start(PROFILE_BASE_KINEMATICS_COMPUTATION);
655 
656  /* Compute kinematics assuming world is at free-flyer frame */
657  m_q_pin.head<6>().setZero();
658  m_q_pin(6) = 1.0;
659  m_v_pin.head<6>().setZero();
660  pinocchio::forwardKinematics(m_model, *m_data, m_q_pin, m_v_pin);
661  pinocchio::framesForwardKinematics(m_model, *m_data);
662 
663  getProfiler().stop(PROFILE_BASE_KINEMATICS_COMPUTATION);
664 
665  return s;
666  }
667 
669  {
670  if(!m_initSucceeded)
671  {
672  SEND_WARNING_STREAM_MSG("Cannot compute signal q before initialization!");
673  return s;
674  }
675  if( s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) )
676  s.resize(m_robot_util->m_nbJoints+6);
677 
678  const Eigen::VectorXd & qj = m_joint_positionsSIN(iter); //n+6
679  const Eigen::Vector4d & quatIMU_vec = m_imu_quaternionSIN(iter);
680  const Vector6 & ftrf = m_forceRLEGSIN(iter);
681  const Vector6 & ftlf = m_forceLLEGSIN(iter);
682 
683  // if the weights are not specified by the user through the input signals w_lf, w_rf
684  // then compute them
685  // if one feet is not stable, force weight to 0.0
686  double wL, wR;
687  if(m_w_lf_inSIN.isPlugged())
688  {
689  wL = m_w_lf_inSIN(iter);
690  }
691  else
692  {
693  wL = m_w_lf_filteredSOUT(iter);
694  if (m_left_foot_is_stable == false)
695  wL = 0.0;
696  }
697  if(m_w_rf_inSIN.isPlugged())
698  {
699  wR = m_w_rf_inSIN(iter);
700  }
701  else
702  {
703  wR = m_w_rf_filteredSOUT(iter);
704  if (m_right_foot_is_stable == false)
705  wR = 0.0;
706  }
707 
708  assert(qj.size()==m_robot_util->m_nbJoints && "Unexpected size of signal joint_positions");
709 
710  // if both weights are zero set them to a small positive value to avoid division by zero
711  if(wR==0.0 && wL==0.0)
712  {
713  SEND_WARNING_STREAM_MSG("The robot is flying!"+
714  ("- forceRLEG: "+toString(ftrf.transpose()))+
715  "- forceLLEG: "+toString(ftlf.transpose())+
716  "- m_right_foot_is_stable: "+toString(m_right_foot_is_stable)+
717  "- m_left_foot_is_stable: "+toString(m_left_foot_is_stable));
718  wR = 1e-3;
719  wL = 1e-3;
720  }
721 
722  m_kinematics_computationsSINNER(iter);
723 
724  if(m_reset_foot_pos)
725  reset_foot_positions_impl(ftlf, ftrf);
726 
727  getProfiler().start(PROFILE_BASE_POSITION_ESTIMATION);
728  {
729  SE3 oMlfa, oMrfa, lfsMff, rfsMff;
732 
733  // get rpy
734  const SE3 ffMtorso(m_data->oMi[m_torso_id]); // transform between freeflyer and body attached to IMU sensor (torso)
735  const SE3 torsoMff(ffMtorso.inverse()); // transform between body attached to IMU sensor (torso) and freeflyer
736 
737  Vector3 rpy_torso, rpy_torso_lf, rpy_torso_rf, rpy_torso_imu; // orientation of the body which imu is attached to. (fusion, from left kine, from right kine, from imu)
738 
739  matrixToRpy((m_oMff_lf*ffMtorso).rotation(), rpy_torso_lf);
740  matrixToRpy((m_oMff_rf*ffMtorso).rotation(), rpy_torso_rf);
741  Eigen::Quaterniond quatIMU(quatIMU_vec[0], quatIMU_vec[1], quatIMU_vec[2], quatIMU_vec[3]);
742  Eigen::Quaterniond quat_torsoMimu(m_torsoMimu.rotation()); // transform between torso and IMU
743  Eigen::Quaterniond quat_torso_imu(quatIMU*quat_torsoMimu); // quatIMU expressed in torso frame
744  //Eigen::Quaterniond quat_torso_imu(quatIMU); // quatIMU expressed in torso frame
745  matrixToRpy(quat_torso_imu.toRotationMatrix(), rpy_torso_imu);
746 
747  // average (we do not take into account the IMU yaw)
748  // double wSum = wL + wR + m_w_imu;
749  rpy_torso[0] = wEulerMean(rpy_torso_lf[0], rpy_torso_rf[0], wL, wR);
750  rpy_torso[0] = wEulerMean(rpy_torso[0], rpy_torso_imu[0], wL+wR, m_w_imu);
751  rpy_torso[1] = wEulerMean(rpy_torso_lf[1], rpy_torso_rf[1], wL, wR);
752  rpy_torso[1] = wEulerMean(rpy_torso[1], rpy_torso_imu[1], wL+wR, m_w_imu);
753  rpy_torso[2] = wEulerMean(rpy_torso_lf[2], rpy_torso_rf[2], wL, wR);
754 
755  rpyToMatrix(rpy_torso, m_oRtorso);
756  m_oRff = m_oRtorso * torsoMff.rotation();
757 
758  // translation to get foot at the right position
759  // evaluate Mrl Mlf for q with the good orientation and zero translation for freeflyer
760  const Vector3 pos_lf_ff = m_oRff * m_data->oMf[m_left_foot_id].translation();
761  const Vector3 pos_rf_ff = m_oRff * m_data->oMf[m_right_foot_id].translation();
762  // get average translation
763  m_q_pin.head<3>() = ((oMlfa.translation() - pos_lf_ff)*wL +
764  (oMrfa.translation() - pos_rf_ff)*wR) / (wL+wR);
765 
766  m_q_sot.tail(m_robot_util->m_nbJoints) = qj;
767  base_se3_to_sot(m_q_pin.head<3>(), m_oRff, m_q_sot.head<6>());
768 
769  s = m_q_sot;
770 
771  // store estimation of the base pose in SE3 format
772  const SE3 oMff_est(m_oRff, m_q_pin.head<3>());
773 
774  // feedback on feet poses
775  if(m_K_fb_feet_posesSIN.isPlugged())
776  {
777  const double K_fb = m_K_fb_feet_posesSIN(iter);
778  if (K_fb > 0.0 && m_w_imu > 0.0)
779  {
780  assert(m_w_imu > 0.0 && "Update of the feet 6d poses should not be done if wIMU = 0.0");
781  assert(K_fb < 1.0 && "Feedback gain on foot correction should be less than 1.0 (K_fb_feet_poses>1.0)");
782  //feet positions in the world according to current base estimation
783  const SE3 oMlfs_est( oMff_est*(lfsMff.inverse()) );
784  const SE3 oMrfs_est( oMff_est*(rfsMff.inverse()) );
785  //error in current foot position
786  SE3 leftDrift = m_oMlfs.inverse()*oMlfs_est;
787  SE3 rightDrift = m_oMrfs.inverse()*oMrfs_est;
788 
790  SE3 leftDrift_delta;
791  SE3 rightDrift_delta;
792  se3Interp(leftDrift ,SE3::Identity(),K_fb*wR,leftDrift_delta);
793  se3Interp(rightDrift,SE3::Identity(),K_fb*wL,rightDrift_delta);
794  // if a feet is not stable on the ground (aka flying), fully update his position
795  if (m_right_foot_is_stable == false)
796  rightDrift_delta = rightDrift;
797  if (m_left_foot_is_stable == false)
798  leftDrift_delta = leftDrift;
799  if (m_right_foot_is_stable == false && m_left_foot_is_stable == false)
800  {
801  //robot is jumping, do not update any feet position
802  rightDrift_delta = SE3::Identity();
803  leftDrift_delta = SE3::Identity();
804  }
805 // std::cout << "------------------------------------------------" << std::endl;
806 // std::cout << "Initial m_oMlfs and m_oMrfs:" << std::endl;
807 // std::cout << m_oMlfs << std::endl;
808 // std::cout << m_oMrfs << std::endl;
809 // std::cout << "leftDrift_delta and rightDrift_delta:" << std::endl;
810 // std::cout << leftDrift_delta << std::endl;
811 // std::cout << rightDrift_delta << std::endl;
812  m_oMlfs = m_oMlfs * leftDrift_delta;
813  m_oMrfs = m_oMrfs * rightDrift_delta;
814  // dedrift (x, y, z, yaw) using feet pose references
815  SE3 oMlfs_ref, oMrfs_ref;
816  if (m_lf_ref_xyzquatSIN.isPlugged() and
817  m_rf_ref_xyzquatSIN.isPlugged())
818  {
820  const Vector7 & lf_ref_xyzquat_vec = m_lf_ref_xyzquatSIN(iter);
821  const Vector7 & rf_ref_xyzquat_vec = m_rf_ref_xyzquatSIN(iter);
822  const Eigen::Quaterniond ql(m_lf_ref_xyzquatSIN(iter)(6),
823  m_lf_ref_xyzquatSIN(iter)(3),
824  m_lf_ref_xyzquatSIN(iter)(4),
825  m_lf_ref_xyzquatSIN(iter)(5));
826  const Eigen::Quaterniond qr(m_rf_ref_xyzquatSIN(iter)(6),
827  m_rf_ref_xyzquatSIN(iter)(3),
828  m_rf_ref_xyzquatSIN(iter)(4),
829  m_rf_ref_xyzquatSIN(iter)(5));
830  oMlfs_ref = SE3(ql.toRotationMatrix(), lf_ref_xyzquat_vec.head<3>());
831  oMrfs_ref = SE3(qr.toRotationMatrix(), rf_ref_xyzquat_vec.head<3>());
832  }
833  else
834  {
835  oMlfs_ref = m_oMlfs_default_ref;
836  oMrfs_ref = m_oMrfs_default_ref;
837  }
839  const Vector3 translation_feet_drift = 0.5*( ( oMlfs_ref.translation() - m_oMlfs.translation()) +
840  ( oMrfs_ref.translation() - m_oMrfs.translation()) );
842  const Vector3 V_ref = oMrfs_ref.translation() - oMlfs_ref.translation();
843  const Vector3 V_est = m_oMrfs.translation() - m_oMlfs.translation();
845  const double yaw_drift = (atan2(V_ref(1), V_ref(0)) -
846  atan2(V_est(1), V_est(0)));
847  //~ printf("yaw_drift=%lf\r\n",yaw_drift);
849  const Vector3 rpy_feet_drift(0.,0.,yaw_drift);
850  Matrix3 rotation_feet_drift;
851  rpyToMatrix(rpy_feet_drift,rotation_feet_drift);
852  const SE3 drift_to_ref(rotation_feet_drift , translation_feet_drift);
853 // std::cout << "drift_to_ref:" << std::endl;
854 // std::cout << drift_to_ref << std::endl;
855 // std::cout << "leftDrift_delta*drift_to_ref and rightDrift_delta*drift_to_ref:" << std::endl;
856 // std::cout << leftDrift_delta * drift_to_ref << std::endl;
857 // std::cout << rightDrift_delta * drift_to_ref << std::endl;
858  m_oMlfs = m_oMlfs * drift_to_ref;
859  m_oMrfs = m_oMrfs * drift_to_ref;
860 // std::cout << "Final m_oMlfs and m_oMrfs:" << std::endl;
861 // std::cout << m_oMlfs << std::endl;
862 // std::cout << m_oMrfs << std::endl;
863 // std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
864  }
865  else if (K_fb < -1e-3) // hack. If K_fb<0, disable drift compensation
866  {
867  if (m_lf_ref_xyzquatSIN.isPlugged() and
868  m_rf_ref_xyzquatSIN.isPlugged())
869  {
871  const Vector7 & lf_ref_xyzquat_vec = m_lf_ref_xyzquatSIN(iter);
872  const Vector7 & rf_ref_xyzquat_vec = m_rf_ref_xyzquatSIN(iter);
873  const Eigen::Quaterniond ql(m_lf_ref_xyzquatSIN(iter)(6),
874  m_lf_ref_xyzquatSIN(iter)(3),
875  m_lf_ref_xyzquatSIN(iter)(4),
876  m_lf_ref_xyzquatSIN(iter)(5));
877  const Eigen::Quaterniond qr(m_rf_ref_xyzquatSIN(iter)(6),
878  m_rf_ref_xyzquatSIN(iter)(3),
879  m_rf_ref_xyzquatSIN(iter)(4),
880  m_rf_ref_xyzquatSIN(iter)(5));
881  m_oMlfs = SE3(ql.toRotationMatrix(), lf_ref_xyzquat_vec.head<3>());
882  m_oMrfs = SE3(qr.toRotationMatrix(), rf_ref_xyzquat_vec.head<3>());
883  }
884  else
885  {
888  }
889  }
890  }
891 
892  // convert to xyz+quaternion format //Rq: this convertions could be done in outupt signals function?
893  m_oMlfs_xyzquat.head<3>() = m_oMlfs.translation();
894  Eigen::Quaternion<double> quat_lf(m_oMlfs.rotation());
895  m_oMlfs_xyzquat(3) = quat_lf.x();
896  m_oMlfs_xyzquat(4) = quat_lf.y();
897  m_oMlfs_xyzquat(5) = quat_lf.z();
898  m_oMlfs_xyzquat(6) = quat_lf.w();
899 
900  m_oMrfs_xyzquat.head<3>() = m_oMrfs.translation();
901  Eigen::Quaternion<double> quat_rf(m_oMrfs.rotation());
902  m_oMrfs_xyzquat(3) = quat_rf.x();
903  m_oMrfs_xyzquat(4) = quat_rf.y();
904  m_oMrfs_xyzquat(5) = quat_rf.z();
905  m_oMrfs_xyzquat(6) = quat_rf.w();
906  }
907  getProfiler().stop(PROFILE_BASE_POSITION_ESTIMATION);
908  return s;
909  }
910 
912  {
913  if(!m_initSucceeded)
914  {
915  SEND_WARNING_STREAM_MSG("Cannot compute signal lf_xyzquat before initialization!");
916  return s;
917  }
918  if(s.size()!=7)
919  s.resize(7);
920  const Eigen::VectorXd & q = m_qSOUT(iter);
921  (void) q; // disable unused variable warning
922  s = m_oMlfs_xyzquat;
923  return s;
924  }
925 
927  {
928  if(!m_initSucceeded)
929  {
930  SEND_WARNING_STREAM_MSG("Cannot compute signal rf_xyzquat before initialization!");
931  return s;
932  }
933  if(s.size()!=7)
934  s.resize(7);
935  const Eigen::VectorXd & q = m_qSOUT(iter);
936  (void) q; // disable unused variable warning
937  s = m_oMrfs_xyzquat;
938  return s;
939  }
940 
942  {
943  if(!m_initSucceeded)
944  {
945  SEND_WARNING_STREAM_MSG("Cannot compute signal q_lf before initialization!");
946  return s;
947  }
948  if( s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) )
949  s.resize(m_robot_util->m_nbJoints+6);
950 
951  const Eigen::VectorXd & q = m_qSOUT(iter);
952  s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
953  base_se3_to_sot(m_oMff_lf.translation(), m_oMff_lf.rotation(), s.head<6>());
954 
955  return s;
956  }
957 
959  {
960  if(!m_initSucceeded)
961  {
962  SEND_WARNING_STREAM_MSG("Cannot compute signal q_rf before initialization!");
963  return s;
964  }
965  if( s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) )
966  s.resize(m_robot_util->m_nbJoints+6);
967 
968  const Eigen::VectorXd & q = m_qSOUT(iter);
969  s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
970  base_se3_to_sot(m_oMff_rf.translation(), m_oMff_rf.rotation(), s.head<6>());
971 
972  return s;
973  }
974 
976  {
977  if(!m_initSucceeded)
978  {
979  SEND_WARNING_STREAM_MSG("Cannot compute signal q_imu before initialization!");
980  return s;
981  }
982  if( s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) )
983  s.resize(m_robot_util->m_nbJoints+6);
984 
985  const Eigen::VectorXd & q = m_qSOUT(iter);
986  s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
987  const SE3 ffMtorso(m_data->oMi[m_torso_id]); // transform between freeflyer and body attached to IMU sensor (torso)
988  const SE3 torsoMff(ffMtorso.inverse()); // transform between body attached to IMU sensor (torso) and freeflyer
989  const Eigen::Vector4d & quatIMU_vec = m_imu_quaternionSIN(iter);
990  //Eigen::Quaternion<double> quatIMU(quatIMU_vec);
991  Eigen::Quaterniond quatIMU(quatIMU_vec[0], quatIMU_vec[1], quatIMU_vec[2], quatIMU_vec[3]);
992  Eigen::Quaterniond quat_torsoMimu(m_torsoMimu.rotation());
993  Eigen::Quaterniond quat_torso_imu(quatIMU*quat_torsoMimu);
994  //m_oRff = quat_torso_imu.toRotationMatrix() * torsoMff.rotation();
995  base_se3_to_sot(q.head<3>(), m_oRff, s.head<6>());
996  //matrixToRpy(quat_torso_imu.toRotationMatrix(), rpy_torso_imu);
997 
998  return s;
999  }
1000 
1002  {
1003  if(!m_initSucceeded)
1004  {
1005  SEND_WARNING_STREAM_MSG("Cannot compute signal w_lf before initialization!");
1006  return s;
1007  }
1008 
1009  const Vector6 & wrench = m_forceLLEGSIN(iter);
1010  Vector2 zmp;
1011  zmp.setZero();
1012  compute_zmp(wrench, zmp);
1013  double w_zmp = compute_zmp_weight(zmp, m_left_foot_sizes,
1015  double w_fz = compute_force_weight(wrench(2), m_fz_std_dev_lf, m_fz_margin_lf);
1016  //check that foot is sensing a force greater than the margin treshold for more than 'm_fz_stable_windows_size' samples
1017  if (wrench(2) > m_fz_margin_lf)
1019  else m_lf_fz_stable_cpt = 0;
1020 
1022  {
1024  m_left_foot_is_stable = true;
1025  }
1026  else
1027  {
1028  m_left_foot_is_stable = false;
1029  }
1030  s = w_zmp*w_fz;
1031  return s;
1032  }
1033 
1035  {
1036  if(!m_initSucceeded)
1037  {
1038  SEND_WARNING_STREAM_MSG("Cannot compute signal w_rf before initialization!");
1039  return s;
1040  }
1041 
1042  const Vector6 & wrench = m_forceRLEGSIN(iter);
1043  Vector2 zmp;
1044  zmp.setZero();
1045  compute_zmp(wrench, zmp);
1046  double w_zmp = compute_zmp_weight(zmp, m_right_foot_sizes,
1048  double w_fz = compute_force_weight(wrench(2), m_fz_std_dev_rf, m_fz_margin_rf);
1049  //check that foot is sensing a force greater than the margin treshold for more than 'm_fz_stable_windows_size' samples
1050  if (wrench(2) > m_fz_margin_rf)
1052  else
1053  m_rf_fz_stable_cpt = 0;
1054 
1056  {
1058  m_right_foot_is_stable = true;
1059  }
1060  else
1061  {
1062  m_right_foot_is_stable = false;
1063  }
1064  s = w_zmp*w_fz;
1065  return s;
1066  }
1067 
1068  DEFINE_SIGNAL_OUT_FUNCTION(w_rf_filtered, double)
1069  {
1070  if(!m_initSucceeded)
1071  {
1072  SEND_WARNING_STREAM_MSG("Cannot compute signal w_rf_filtered before initialization!");
1073  return s;
1074  }
1075  double w_rf = m_w_rfSOUT(iter);
1076  m_w_rf_filtered = m_alpha_w_filter*w_rf + (1-m_alpha_w_filter)*m_w_rf_filtered; //low pass filter
1077  s = m_w_rf_filtered;
1078  return s;
1079  }
1080 
1081  DEFINE_SIGNAL_OUT_FUNCTION(w_lf_filtered, double)
1082  {
1083  if(!m_initSucceeded)
1084  {
1085  SEND_WARNING_STREAM_MSG("Cannot compute signal w_lf_filtered before initialization!");
1086  return s;
1087  }
1088  double w_lf = m_w_lfSOUT(iter);
1089  m_w_lf_filtered = m_alpha_w_filter*w_lf + (1-m_alpha_w_filter)*m_w_lf_filtered; //low pass filter
1090  s = m_w_lf_filtered;
1091  return s;
1092  }
1093 
1094 
1096  {
1097  if(!m_initSucceeded)
1098  {
1099  SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
1100  return s;
1101  }
1102  if( s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) )
1103  s.resize(m_robot_util->m_nbJoints+6);
1104 
1105  m_kinematics_computationsSINNER(iter);
1106  m_qSOUT(iter);
1107 
1108  getProfiler().start(PROFILE_BASE_VELOCITY_ESTIMATION);
1109  {
1110  const Eigen::VectorXd & dq = m_joint_velocitiesSIN(iter);
1111  const Eigen::Vector3d & acc_imu = m_accelerometerSIN(iter);
1112  const Eigen::Vector3d & gyr_imu = m_gyroscopeSIN(iter);
1113  // const Eigen::Vector4d & quatIMU_vec = m_imu_quaternionSIN(iter);
1114  const Vector6 & dftrf = m_dforceRLEGSIN(iter);
1115  const Vector6 & dftlf = m_dforceLLEGSIN(iter);
1116  assert(dq.size()==m_robot_util->m_nbJoints && "Unexpected size of signal joint_velocities");
1117 
1118  // if the weights are not specified by the user through the input signals w_lf, w_rf
1119  // then compute them
1120  // if one feet is not stable, force weight to 0.0
1121  double wL, wR;
1122  if(m_w_lf_inSIN.isPlugged())
1123  {
1124  wL = m_w_lf_inSIN(iter);
1125  }
1126  else
1127  {
1128  wL = m_w_lf_filteredSOUT(iter);
1129  if (m_left_foot_is_stable == false)
1130  wL = 0.0;
1131  }
1132  if(m_w_rf_inSIN.isPlugged())
1133  {
1134  wR = m_w_rf_inSIN(iter);
1135  }
1136  else
1137  {
1138  wR = m_w_rf_filteredSOUT(iter);
1139  if (m_right_foot_is_stable == false)
1140  wR = 0.0;
1141  }
1142  // if both weights are zero set them to a small positive value to avoid division by zero
1143  if(wR==0.0 && wL==0.0)
1144  {
1145  wR = 1e-3;
1146  wL = 1e-3;
1147  }
1148 
1149  /* Compute foot velocities */
1150  const Frame & f_lf = m_model.frames[m_left_foot_id];
1151  const Motion & v_lf_local = m_data->v[f_lf.parent];
1152  const SE3 & ffMlf = m_data->oMi[f_lf.parent];
1153  Vector6 v_kin_l = -ffMlf.act(v_lf_local).toVector(); //this is the velocity of the base in the frame of the base.
1154  v_kin_l.head<3>() = m_oRff * v_kin_l.head<3>();
1155  v_kin_l.segment<3>(3) = m_oRff * v_kin_l.segment<3>(3);
1156 
1157  const Frame & f_rf = m_model.frames[m_right_foot_id];
1158  const Motion & v_rf_local = m_data->v[f_rf.parent];
1159  const SE3 & ffMrf = m_data->oMi[f_rf.parent];
1160  Vector6 v_kin_r = -ffMrf.act(v_rf_local).toVector(); //this is the velocity of the base in the frame of the base.
1161  v_kin_r.head<3>() = m_oRff * v_kin_r.head<3>();
1162  v_kin_r.segment<3>(3) = m_oRff * v_kin_r.segment<3>(3);
1163 
1164  m_v_kin.head<6>() = (wR*v_kin_r + wL*v_kin_l)/(wL+wR);
1165 
1166  /* Compute velocity induced by the flexibility */
1167  Vector6 v_flex_l;
1168  Vector6 v_flex_r;
1169  v_flex_l << -dftlf[0]/m_K_lf(0), -dftlf[1]/m_K_lf(1), -dftlf[2]/m_K_lf(2),
1170  -dftlf[3]/m_K_lf(3), -dftlf[4]/m_K_lf(4), -dftlf[5]/m_K_lf(5);
1171  v_flex_r << -dftrf[0]/m_K_rf(0), -dftrf[1]/m_K_rf(1), -dftrf[2]/m_K_rf(2),
1172  -dftrf[3]/m_K_rf(3), -dftrf[4]/m_K_rf(4), -dftrf[5]/m_K_rf(5);
1173  const Eigen::Matrix<double, 6, 6> lfAff = ffMlf.inverse().toActionMatrix();
1174  const Eigen::Matrix<double, 6, 6> rfAff = ffMrf.inverse().toActionMatrix();
1175  Eigen::Matrix<double, 12, 6> A;
1176  A << lfAff,
1177  rfAff;
1178  Eigen::Matrix<double, 12, 1> b;
1179  b << v_flex_l,
1180  v_flex_r;
1181  //~ m_v_flex.head<6>() = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
1182  m_v_flex.head<6>() = (A.transpose() * A).ldlt().solve(A.transpose() * b);
1183  m_v_flex.head<3>() = m_oRff * m_v_flex.head<3>();
1184 
1185  /* Get an estimate of linear velocities from gyroscope only*/
1186  // we make the assumption that we are 'turning around the foot' with pure angular velocity in the ankle measured by the gyro
1187  const SE3 & ffMtorso = m_data->oMi[m_torso_id]; // transform between freeflyer and body attached to IMU sensor
1188  const Matrix3 & ffRtorso = ffMtorso.rotation();
1189  const Matrix3 ffRimu = ffRtorso * m_torsoMimu.rotation();
1190  //std::cerr << m_data->oMf[m_torso_id] << std::endl;
1191  //std::cerr << m_data->liMi[m_torso_id] << std::endl;
1192  //std::cerr << "m_torso_id" << m_torso_id << std::endl;
1193  const Matrix3 lfRimu = ffMlf.rotation().transpose() * ffRimu;
1194  const Matrix3 rfRimu = ffMrf.rotation().transpose() * ffRimu;
1195 
1196  //const SE3 torsoMimu(Matrix3::Identity(), Vector3(-0.13, 0.0, 0.118)); ///TODO Read this transform from setable parameter /// TODO chesk the sign of the translation
1197  //const SE3 ffMtorso(m_data->oMf[m_torso_id]);
1198  //const SE3 imuMff = (ffMtorso * m_torsoMimu).inverse();
1199  //gVw_a = gVo_g + gHa.act(aVb_a)-gVb_g //angular velocity in the ankle from gyro and d_enc
1200  //const Frame & f_imu = m_model.frames[m_torso_id];
1201  //const Frame & f_imu = m_model.frames[m_IMU_frame_id];
1202  Vector3 gVo_a_l = Vector3(gyr_imu(0),gyr_imu(1),gyr_imu(2)); //+ (imuMff*ffMlf).act(v_lf_local).angular() - m_data->v[f_imu.parent].angular();
1203  Vector3 gVo_a_r = Vector3(gyr_imu(0),gyr_imu(1),gyr_imu(2)); //+ (imuMff*ffMrf).act(v_rf_local).angular() - m_data->v[f_imu.parent].angular();
1204  Motion v_gyr_ankle_l( Vector3(0.,0.,0.), lfRimu * gVo_a_l);
1205  Motion v_gyr_ankle_r( Vector3(0.,0.,0.), rfRimu * gVo_a_r);
1206  Vector6 v_gyr_l = ffMlf.inverse().actInv(v_gyr_ankle_l).toVector();
1207  Vector6 v_gyr_r = ffMrf.inverse().actInv(v_gyr_ankle_r).toVector();
1208  m_v_gyr.head<6>() = (wL*v_gyr_l + wR*v_gyr_r)/(wL+wR);
1209 
1210 
1211  /* Get an estimate of linear velocities from filtered accelerometer integration */
1212 
1213  /* rotate acceleration to express it in the world frame */
1214  //~ pointRotationByQuaternion(acc_imu,quatIMU_vec,acc_world); //this is false because yaw from imuquat is drifting
1215  const Vector3 acc_world = m_oRtorso * acc_imu;
1216 
1217  /* now, the acceleration is expressed in the world frame,
1218  * so it can be written as the sum of the proper acceleration and the
1219  * constant gravity vector. We could remove this assuming a [0,0,9.81]
1220  * but we prefer to filter this signal with low frequency high pass
1221  * filter since it is robust to gravity norm error, and we know that
1222  * globaly the robot can not accelerate continuously. */
1223 
1225  if (m_isFirstIterFlag)
1226  {
1227  m_last_DCacc = acc_world; // Copy the first acceleration data
1228  m_isFirstIterFlag = false;
1229  sendMsg("iter:"+toString(iter)+"\n", MSG_TYPE_INFO);
1230  }
1231  const Vector3 DCacc = acc_world * (1-m_alpha_DC_acc) + m_last_DCacc * (m_alpha_DC_acc);
1232  //~ sendMsg("acc_world :"+toString(acc_world)+"\n", MSG_TYPE_INFO);
1233  m_last_DCacc = DCacc;
1234  const Vector3 ACacc = acc_world - DCacc;
1235  m_v_ac = ACacc;
1236  /* Then this acceleration is integrated. */
1237  const Vector3 vel = m_last_vel + ACacc * m_dt;
1238  m_last_vel = vel;
1239  /* To stabilise the integrated velocity, we add the
1240  * asumption that globaly, velocity is zero. So we remove DC again */
1241  const Vector3 DCvel = vel * (1-m_alpha_DC_vel) + m_last_DCvel * (m_alpha_DC_vel);
1242  m_last_DCvel = DCvel;
1243  const Vector3 ACvel = vel - DCvel;
1244  m_v_ac = ACvel;
1245 
1246  /* Express back velocity in the imu frame to get a full 6d velocity with the gyro*/
1247  const Vector3 imuVimu = m_oRtorso.transpose() * ACvel;
1248  /* Here we could remove dc from gyrometer to remove bias*/
1249  const Motion imuWimu(imuVimu,gyr_imu);
1250  //const Frame & f_imu = m_model.frames[m_torso_id];
1251  //const Motion & ffWtorso = m_data->v[f_imu.parent];
1252  //const SE3 ffMtorso(m_data->oMf[m_torso_id]);
1253  //const SE3 torsoMimu(Matrix3::Identity(), +1.0*Vector3(-0.13, 0.0, 0.118)); ///TODO Read this transform from setable parameter /// TODO chesk the sign of the translation
1254  const SE3 ffMimu = ffMtorso * m_torsoMimu;
1255  const Motion v= ffMimu.act(imuWimu) ;//- ffWtorso;
1256  m_v_imu.head<6>() = v.toVector();
1257  //m_v_imu.head<3>() = m_oRff * m_v_imu.head<3>();
1258 
1259 
1260  m_v_sot.head<6>() = m_v_kin.head<6>();
1261  //~ m_v_sot.head<6>() = m_v_flex.head<6>() + m_v_kin.head<6>();
1262 // m_v_sot.head<6>() = m_v_gyr.head<6>() + m_v_kin.head<6>();
1263 // m_v_sot.head<6>() = m_v_gyr.head<6>();
1264  //~ m_v_sot.head<6>() = m_v_imu.head<6>();
1265 
1266  m_v_sot.tail( m_robot_util->m_nbJoints) = dq;
1267  m_v_kin.tail( m_robot_util->m_nbJoints) = dq;
1268  m_v_flex.tail(m_robot_util->m_nbJoints) = dq;
1269  m_v_gyr.tail( m_robot_util->m_nbJoints) = dq;
1270  m_v_imu.tail( m_robot_util->m_nbJoints) = dq;
1271  s = m_v_sot;
1272 
1273  }
1274  getProfiler().stop(PROFILE_BASE_VELOCITY_ESTIMATION);
1275  return s;
1276  }
1277 
1279  {
1280  if(!m_initSucceeded)
1281  {
1282  SEND_WARNING_STREAM_MSG("Cannot compute signal v_kin before initialization!");
1283  return s;
1284  }
1285  m_vSOUT(iter);
1286  s = m_v_kin;
1287  return s;
1288  }
1289 
1291  {
1292  if(!m_initSucceeded)
1293  {
1294  SEND_WARNING_STREAM_MSG("Cannot compute signal v_flex before initialization!");
1295  return s;
1296  }
1297  m_vSOUT(iter);
1298  s = m_v_flex+m_v_kin;
1299  return s;
1300  }
1301 
1303  {
1304  if(!m_initSucceeded)
1305  {
1306  SEND_WARNING_STREAM_MSG("Cannot compute signal v_imu before initialization!");
1307  return s;
1308  }
1309  m_vSOUT(iter);
1310  s = m_v_imu;
1311  return s;
1312  }
1313 
1315  {
1316  if(!m_initSucceeded)
1317  {
1318  SEND_WARNING_STREAM_MSG("Cannot compute signal v_gyr before initialization!");
1319  return s;
1320  }
1321  m_vSOUT(iter);
1322  s = m_v_gyr;
1323  return s;
1324  }
1325 
1327  {
1328  if(!m_initSucceeded)
1329  {
1330  SEND_WARNING_STREAM_MSG("Cannot compute signal v_ac before initialization!");
1331  return s;
1332  }
1333  m_vSOUT(iter);
1334  s = m_v_ac;
1335  return s;
1336  }
1337 
1339  {
1340  if(!m_initSucceeded)
1341  {
1342  SEND_WARNING_STREAM_MSG("Cannot compute signal a_ac before initialization!");
1343  return s;
1344  }
1345  m_vSOUT(iter);
1346  s = m_a_ac;
1347  return s;
1348  }
1349 
1350  /* --- COMMANDS ---------------------------------------------------------- */
1351 
1352  /* ------------------------------------------------------------------- */
1353  /* --- ENTITY -------------------------------------------------------- */
1354  /* ------------------------------------------------------------------- */
1355 
1356  void TalosBaseEstimator::display(std::ostream& os) const
1357  {
1358  os << "TalosBaseEstimator "<<getName();
1359  try
1360  {
1361  getProfiler().report_all(3, os);
1362  }
1363  catch (ExceptionSignal e) {}
1364  }
1365  } // namespace talos_balance
1366  } // namespace sot
1367 } // namespace dynamicgraph
1368 
double eulerMean(double a1, double a2)
SE3 m_oMrfs
transformation from world to left foot sole
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: math/fwd.hh:45
double m_fz_margin_lf
margin used for computing zmp weight
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d &R)
double m_zmp_margin_rf
margin used for computing zmp weight
#define PROFILE_BASE_VELOCITY_ESTIMATION
Vector7 m_oMlfs_xyzquat
transformation from world to right foot sole
#define PROFILE_BASE_KINEMATICS_COMPUTATION
Eigen::VectorXd m_v_flex
6d robot velocities from kinematic only (encoders derivative)
double m_dt
true after the command resetFootPositions is called
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
double m_fz_std_dev_rf
standard deviation of ZMP measurement errors for left foot
double m_fz_std_dev_lf
standard deviation of normal force measurement errors for right foot
virtual void display(std::ostream &os) const
filtered weight of the estimation coming from the right foot
STL namespace.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
void se3Interp(const pinocchio::SE3 &s1, const pinocchio::SE3 &s2, const double alpha, pinocchio::SE3 &s12)
double compute_zmp_weight(const Vector2 &zmp, const Vector4 &foot_sizes, double std_dev, double margin)
void reset_foot_positions_impl(const Vector6 &ftlf, const Vector6 &ftrf)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW TalosBaseEstimator(const std::string &name)
pinocchio::SE3 m_oMff_lf
chest to imu transformation
double m_alpha_DC_acc
acceleration of the base in the world with DC component removed
double m_zmp_std_dev_lf
standard deviation of ZMP measurement errors for right foot
Vector3 m_a_ac
velocity of the base in the world with DC component removed
pinocchio::Data * m_data
Pinocchio robot model.
#define PROFILE_BASE_POSITION_ESTIMATION
Vector3 m_v_ac
6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities) ...
#define INPUT_SIGNALS
double compute_force_weight(double fz, double std_dev, double margin)
double wEulerMean(double a1, double a2, double w1, double w2)
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
Eigen::VectorXd m_v_kin
6d stiffness of left foot spring
Vector6 m_K_lf
6d stiffness of right foot spring
SE3 m_sole_M_ftSens
flag to detect first iteration and initialise velocity filters
void set_stiffness_right_foot(const dynamicgraph::Vector &k)
int m_lf_fz_stable_cpt
size of the windows used to detect that feet did not leave the ground
Eigen::VectorXd m_v_pin
robot configuration according to SoT convention
SE3 m_oMlfs
world-to-base transformation obtained through right foot
pinocchio::FrameIndex m_right_foot_id
foot sole to F/T sensor transformation
bool m_reset_foot_pos
true if the entity has been successfully initialized
bool m_right_foot_is_stable
True if left foot as been stable for the last &#39;m_fz_stable_windows_size&#39; samples. ...
void set_right_foot_sizes(const dynamicgraph::Vector &s)
double m_w_rf_filtered
filtered weight of the estimation coming from the left foot
Matrix3 m_oRff
chest orientation in the world from angular fusion
double m_zmp_std_dev_rf
weight of IMU for sensor fusion
void init(const double &dt, const std::string &urdfFile)
int m_fz_stable_windows_size
True if right foot as been stable for the last &#39;m_fz_stable_windows_size&#39; samples.
normal m_normal
Default reference for right foot pose to use if no ref is pluged.
double m_alpha_DC_vel
alpha parameter for DC blocker filter on acceleration data
double m_alpha_w_filter
alpha parameter for DC blocker filter on velocity data
pinocchio::Model m_model
filtered weight of the estimation coming from the right foot
double m_fz_margin_rf
margin used for computing normal force weight
void matrixToRpy(const Eigen::Matrix3d &M, Eigen::Vector3d &rpy)
void kinematics_estimation(const Vector6 &ft, const Vector6 &K, const SE3 &oMfs, const pinocchio::FrameIndex foot_id, SE3 &oMff, SE3 &oMfa, SE3 &fsMff)
double m_w_imu
counter for detecting for how long the feet has been stable
pinocchio::SE3 m_oMff_rf
world-to-base transformation obtained through left foot
double m_zmp_margin_lf
sizes of the left foot (pos x, neg x, pos y, neg y)
Eigen::VectorXd m_v_imu
6d robot velocities from flexibility only (force sensor derivative)
Vector4 m_left_foot_sizes
standard deviation of normal force measurement errors for left foot
#define OUTPUT_SIGNALS
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
int m_rf_fz_stable_cpt
counter for detecting for how long the feet has been stable
Vector6 m_K_rf
margin used for computing normal force weight
SE3 m_oMrfs_default_ref
Default reference for left foot pose to use if no ref is pluged.
double m_w_lf_filtered
filter parameter to filter weights (1st order low pass filter)
Eigen::VectorXd m_v_gyr
6d robot velocities form imu only (accelerometer integration + gyro)
void set_stiffness_left_foot(const dynamicgraph::Vector &k)
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: math/fwd.hh:46
Vector4 m_right_foot_sizes
sizes of the left foot (pos x, neg x, pos y, neg y)
Matrix3 m_oRtorso
robot velocities according to SoT convention