sot-talos-balance  1.5.0
talos-base-estimator.hh
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 
17 #ifndef __sot_torque_control_base_estimator_H__
18 #define __sot_torque_control_base_estimator_H__
19 
20 /* --------------------------------------------------------------------- */
21 /* --- API ------------------------------------------------------------- */
22 /* --------------------------------------------------------------------- */
23 
24 #if defined (WIN32)
25 # if defined (base_estimator_EXPORTS)
26 # define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllexport)
27 # else
28 # define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllimport)
29 # endif
30 #else
31 # define TALOS_BASE_ESTIMATOR_EXPORT
32 #endif
33 
34 
35 /* --------------------------------------------------------------------- */
36 /* --- INCLUDE --------------------------------------------------------- */
37 /* --------------------------------------------------------------------- */
38 
39 #include <dynamic-graph/signal-helper.h>
40 #include <sot/core/matrix-geometry.hh>
41 #include <map>
42 #include "boost/assign.hpp"
43 //#include <boost/random/normal_distribution.hpp>
44 #include <boost/math/distributions/normal.hpp> // for normal_distribution
45 
46 /* Pinocchio */
47 #include <pinocchio/multibody/model.hpp>
48 #include <pinocchio/parsers/urdf.hpp>
49 #include <pinocchio/algorithm/kinematics.hpp>
50 #include <sot/core/robot-utils.hh>
51 
52 namespace dynamicgraph {
53  namespace sot {
54  namespace talos_balance {
55 
56  /* --------------------------------------------------------------------- */
57  /* --- CLASS ----------------------------------------------------------- */
58  /* --------------------------------------------------------------------- */
59 
60 
62  void se3Interp(const pinocchio::SE3 & s1, const pinocchio::SE3 & s2, const double alpha, pinocchio::SE3 & s12);
63 
65  void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d & R);
66 
68  void rpyToMatrix(const Eigen::Vector3d & rpy, Eigen::Matrix3d & R);
69 
71  void matrixToRpy(const Eigen::Matrix3d & M, Eigen::Vector3d & rpy);
72 
74  void quanternionMult(const Eigen::Vector4d & q1, const Eigen::Vector4d & q2, Eigen::Vector4d & q12);
75 
77  void pointRotationByQuaternion(const Eigen::Vector3d & point,const Eigen::Vector4d & quat, Eigen::Vector3d & rotatedPoint);
78 
80  double eulerMean(double a1, double a2);
81 
83  double wEulerMean(double a1, double a2, double w1, double w2);
84 
86  :public::dynamicgraph::Entity
87  {
89  typedef pinocchio::SE3 SE3;
90  typedef Eigen::Vector2d Vector2;
91  typedef Eigen::Vector3d Vector3;
92  typedef Eigen::Vector4d Vector4;
93  typedef Vector6d Vector6;
94  typedef Vector7d Vector7;
95  typedef Eigen::Matrix3d Matrix3;
96  typedef boost::math::normal normal;
97 
98  DYNAMIC_GRAPH_ENTITY_DECL();
99 
100  public:
101  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
102 
103  /* --- CONSTRUCTOR ---- */
104  TalosBaseEstimator( const std::string & name );
105 
106  void init(const double & dt, const std::string& urdfFile);
107 
108  void set_fz_stable_windows_size(const int & ws);
109  void set_alpha_w_filter(const double & a);
110  void set_alpha_DC_acc(const double & a);
111  void set_alpha_DC_vel(const double & a);
112  void reset_foot_positions();
113  void set_imu_weight(const double & w);
114  void set_zmp_std_dev_right_foot(const double & std_dev);
115  void set_zmp_std_dev_left_foot(const double & std_dev);
116  void set_normal_force_std_dev_right_foot(const double & std_dev);
117  void set_normal_force_std_dev_left_foot(const double & std_dev);
118  void set_stiffness_right_foot(const dynamicgraph::Vector & k);
119  void set_stiffness_left_foot(const dynamicgraph::Vector & k);
120  void set_right_foot_sizes(const dynamicgraph::Vector & s);
121  void set_left_foot_sizes(const dynamicgraph::Vector & s);
122  void set_zmp_margin_right_foot(const double & margin);
123  void set_zmp_margin_left_foot(const double & margin);
124  void set_normal_force_margin_right_foot(const double & margin);
125  void set_normal_force_margin_left_foot(const double & margin);
126 
127  void reset_foot_positions_impl(const Vector6 & ftlf, const Vector6 & ftrf);
128  void compute_zmp(const Vector6 & w, Vector2 & zmp);
129  double compute_zmp_weight(const Vector2 & zmp, const Vector4 & foot_sizes,
130  double std_dev, double margin);
131  double compute_force_weight(double fz, double std_dev, double margin);
132  void kinematics_estimation(const Vector6 & ft, const Vector6 & K,
133  const SE3 & oMfs, const pinocchio::FrameIndex foot_id,
134  SE3 & oMff, SE3& oMfa, SE3& fsMff);
135 
136  /* --- SIGNALS --- */
137  DECLARE_SIGNAL_IN(joint_positions, dynamicgraph::Vector);
138  DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector);
139  DECLARE_SIGNAL_IN(imu_quaternion, dynamicgraph::Vector);
140  DECLARE_SIGNAL_IN(forceLLEG, dynamicgraph::Vector);
141  DECLARE_SIGNAL_IN(forceRLEG, dynamicgraph::Vector);
142  DECLARE_SIGNAL_IN(dforceLLEG, dynamicgraph::Vector);
143  DECLARE_SIGNAL_IN(dforceRLEG, dynamicgraph::Vector);
144  DECLARE_SIGNAL_IN(w_lf_in, double);
145  DECLARE_SIGNAL_IN(w_rf_in, double);
146  DECLARE_SIGNAL_IN(K_fb_feet_poses, double);
147  DECLARE_SIGNAL_IN(lf_ref_xyzquat, dynamicgraph::Vector);
148  DECLARE_SIGNAL_IN(rf_ref_xyzquat, dynamicgraph::Vector);
149  DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
150  DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
151 
152  DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);
153 
154  DECLARE_SIGNAL_OUT(q, dynamicgraph::Vector);
155  DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);
156  DECLARE_SIGNAL_OUT(v_kin, dynamicgraph::Vector);
157  DECLARE_SIGNAL_OUT(v_flex, dynamicgraph::Vector);
158  DECLARE_SIGNAL_OUT(v_imu, dynamicgraph::Vector);
159  DECLARE_SIGNAL_OUT(v_gyr, dynamicgraph::Vector);
160  DECLARE_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector);
161  DECLARE_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector);
162  DECLARE_SIGNAL_OUT(a_ac, dynamicgraph::Vector);
163  DECLARE_SIGNAL_OUT(v_ac, dynamicgraph::Vector);
164 
165  DECLARE_SIGNAL_OUT(q_lf, dynamicgraph::Vector);
166  DECLARE_SIGNAL_OUT(q_rf, dynamicgraph::Vector);
167  DECLARE_SIGNAL_OUT(q_imu, dynamicgraph::Vector);
168  DECLARE_SIGNAL_OUT(w_lf, double);
169  DECLARE_SIGNAL_OUT(w_rf, double);
170  DECLARE_SIGNAL_OUT(w_lf_filtered, double);
171  DECLARE_SIGNAL_OUT(w_rf_filtered, double);
172 
173  /* --- COMMANDS --- */
174  /* --- ENTITY INHERITANCE --- */
175  virtual void display( std::ostream& os ) const;
176 
177  protected:
180  double m_dt;
181  RobotUtilShrPtr m_robot_util;
182 
188  /* Estimator parameters */
189  double m_w_imu;
198  double m_fz_margin_lf;
199  double m_fz_margin_rf;
200  Vector6 m_K_rf;
201  Vector6 m_K_lf;
202 
203  Eigen::VectorXd m_v_kin;
204  Eigen::VectorXd m_v_flex;
205  Eigen::VectorXd m_v_imu;
206  Eigen::VectorXd m_v_gyr;
207 
208  Vector3 m_v_ac;
209  Vector3 m_a_ac;
210 
211  double m_alpha_DC_acc;
212  double m_alpha_DC_vel;
213 
215 
218 
219  pinocchio::Model m_model;
220  pinocchio::Data *m_data;
221  pinocchio::SE3 m_torsoMimu;
222  pinocchio::SE3 m_oMff_lf;
223  pinocchio::SE3 m_oMff_rf;
224  SE3 m_oMlfs;
225  SE3 m_oMrfs;
230  normal m_normal;
231 
233 
235 
236  pinocchio::FrameIndex m_right_foot_id;
237  pinocchio::FrameIndex m_left_foot_id;
238  pinocchio::FrameIndex m_torso_id;
239  pinocchio::FrameIndex m_IMU_frame_id;
240  Eigen::VectorXd m_q_pin;
241  Eigen::VectorXd m_q_sot;
242  Eigen::VectorXd m_v_pin;
243  Eigen::VectorXd m_v_sot;
244  Matrix3 m_oRtorso;
245  Matrix3 m_oRff;
246 
247  /* Filter buffers*/
248  Vector3 m_last_vel;
249  Vector3 m_last_DCvel;
250  Vector3 m_last_DCacc;
251 
252  }; // class TalosBaseEstimator
253 
254  } // namespace talos_balance
255  } // namespace sot
256 } // namespace dynamicgraph
257 
258 
259 
260 #endif // #ifndef __sot_torque_control_free_flyer_locator_H__
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
Vector7 m_oMlfs_xyzquat
transformation from world to right foot sole
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
void se3Interp(const pinocchio::SE3 &s1, const pinocchio::SE3 &s2, const double alpha, pinocchio::SE3 &s12)
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.
Vector3 m_v_ac
6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities) ...
double wEulerMean(double a1, double a2, double w1, double w2)
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
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. ...
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
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)
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
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)
#define TALOS_BASE_ESTIMATOR_EXPORT
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