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