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