sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
base-estimator.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2017, Thomas Flayols, LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_base_estimator_H__
7 #define __sot_torque_control_base_estimator_H__
8 
9 /* --------------------------------------------------------------------- */
10 /* --- API ------------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12 
13 #if defined(WIN32)
14 #if defined(base_estimator_EXPORTS)
15 #define SOTBASEESTIMATOR_EXPORT __declspec(dllexport)
16 #else
17 #define SOTBASEESTIMATOR_EXPORT __declspec(dllimport)
18 #endif
19 #else
20 #define SOTBASEESTIMATOR_EXPORT
21 #endif
22 
23 /* --------------------------------------------------------------------- */
24 /* --- INCLUDE --------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 
27 #include <map>
28 #include "boost/assign.hpp"
29 //#include <boost/random/normal_distribution.hpp>
30 #include <boost/math/distributions/normal.hpp> // for normal_distribution
31 
32 /* Pinocchio */
33 #include <pinocchio/multibody/model.hpp>
34 #include <pinocchio/parsers/urdf.hpp>
35 #include <pinocchio/algorithm/kinematics.hpp>
36 
37 /* HELPER */
38 #include <dynamic-graph/signal-helper.h>
39 #include <sot/core/matrix-geometry.hh>
40 #include <sot/core/robot-utils.hh>
42 
43 namespace dynamicgraph {
44 namespace sot {
45 namespace torque_control {
46 
47 /* --------------------------------------------------------------------- */
48 /* --- CLASS ----------------------------------------------------------- */
49 /* --------------------------------------------------------------------- */
50 
52 void se3Interp(const pinocchio::SE3& s1, const pinocchio::SE3& s2, const double alpha, pinocchio::SE3& s12);
53 
55 void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d& R);
56 
58 void rpyToMatrix(const Eigen::Vector3d& rpy, Eigen::Matrix3d& R);
59 
61 void matrixToRpy(const Eigen::Matrix3d& M, Eigen::Vector3d& rpy);
62 
64 void quanternionMult(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2, Eigen::Vector4d& q12);
65 
67 void pointRotationByQuaternion(const Eigen::Vector3d& point, const Eigen::Vector4d& quat,
68  Eigen::Vector3d& rotatedPoint);
69 
70 class SOTBASEESTIMATOR_EXPORT BaseEstimator : public ::dynamicgraph::Entity {
72  typedef pinocchio::SE3 SE3;
73  typedef Eigen::Vector2d Vector2;
74  typedef Eigen::Vector3d Vector3;
75  typedef Eigen::Vector4d Vector4;
76  typedef dynamicgraph::sot::Vector6d Vector6;
77  typedef dynamicgraph::sot::Vector7d Vector7;
78  typedef Eigen::Matrix3d Matrix3;
79  typedef boost::math::normal normal;
80 
81  DYNAMIC_GRAPH_ENTITY_DECL();
82 
83  public:
84  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
85 
86  /* --- CONSTRUCTOR ---- */
87  BaseEstimator(const std::string& name);
88 
89  void init(const double& dt, const std::string& urdfFile);
90 
91  void set_fz_stable_windows_size(const int& ws);
92  void set_alpha_w_filter(const double& a);
93  void set_alpha_DC_acc(const double& a);
94  void set_alpha_DC_vel(const double& a);
95  void reset_foot_positions();
96  void set_imu_weight(const double& w);
97  void set_zmp_std_dev_right_foot(const double& std_dev);
98  void set_zmp_std_dev_left_foot(const double& std_dev);
99  void set_normal_force_std_dev_right_foot(const double& std_dev);
100  void set_normal_force_std_dev_left_foot(const double& std_dev);
101  void set_stiffness_right_foot(const dynamicgraph::Vector& k);
102  void set_stiffness_left_foot(const dynamicgraph::Vector& k);
103  void set_right_foot_sizes(const dynamicgraph::Vector& s);
104  void set_left_foot_sizes(const dynamicgraph::Vector& s);
105  void set_zmp_margin_right_foot(const double& margin);
106  void set_zmp_margin_left_foot(const double& margin);
107  void set_normal_force_margin_right_foot(const double& margin);
108  void set_normal_force_margin_left_foot(const double& margin);
109 
110  void reset_foot_positions_impl(const Vector6& ftlf, const Vector6& ftrf);
111  void compute_zmp(const Vector6& w, Vector2& zmp);
112  double compute_zmp_weight(const Vector2& zmp, const Vector4& foot_sizes, double std_dev, double margin);
113  double compute_force_weight(double fz, double std_dev, double margin);
114  void kinematics_estimation(const Vector6& ft, const Vector6& K, const SE3& oMfs, const int foot_id, SE3& oMff,
115  SE3& oMfa, SE3& fsMff);
116 
117  /* --- SIGNALS --- */
118  DECLARE_SIGNAL_IN(joint_positions, dynamicgraph::Vector);
119  DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector);
120  DECLARE_SIGNAL_IN(imu_quaternion, dynamicgraph::Vector);
121  DECLARE_SIGNAL_IN(forceLLEG, dynamicgraph::Vector);
122  DECLARE_SIGNAL_IN(forceRLEG, dynamicgraph::Vector);
123  DECLARE_SIGNAL_IN(dforceLLEG, dynamicgraph::Vector);
124  DECLARE_SIGNAL_IN(dforceRLEG, dynamicgraph::Vector);
125  DECLARE_SIGNAL_IN(w_lf_in, double);
126  DECLARE_SIGNAL_IN(w_rf_in, double);
127  DECLARE_SIGNAL_IN(
128  K_fb_feet_poses,
129  double);
130  DECLARE_SIGNAL_IN(lf_ref_xyzquat, dynamicgraph::Vector);
131  DECLARE_SIGNAL_IN(rf_ref_xyzquat, dynamicgraph::Vector);
132  DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
133  DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
134 
135  DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);
136 
137  DECLARE_SIGNAL_OUT(q, dynamicgraph::Vector);
138  DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);
139  DECLARE_SIGNAL_OUT(v_kin, dynamicgraph::Vector);
140  DECLARE_SIGNAL_OUT(v_flex,
141  dynamicgraph::Vector);
142  DECLARE_SIGNAL_OUT(v_imu,
143  dynamicgraph::Vector);
144  DECLARE_SIGNAL_OUT(v_gyr, dynamicgraph::Vector);
145  DECLARE_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector);
147  DECLARE_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector);
148  DECLARE_SIGNAL_OUT(a_ac, dynamicgraph::Vector);
149  DECLARE_SIGNAL_OUT(v_ac, dynamicgraph::Vector);
150 
151  DECLARE_SIGNAL_OUT(q_lf, dynamicgraph::Vector);
152  DECLARE_SIGNAL_OUT(q_rf, dynamicgraph::Vector);
153  DECLARE_SIGNAL_OUT(q_imu, dynamicgraph::Vector);
154  DECLARE_SIGNAL_OUT(w_lf, double);
155  DECLARE_SIGNAL_OUT(w_rf, double);
156  DECLARE_SIGNAL_OUT(w_lf_filtered, double);
157  DECLARE_SIGNAL_OUT(w_rf_filtered, double);
158 
159  /* --- COMMANDS --- */
160  /* --- ENTITY INHERITANCE --- */
161  virtual void display(std::ostream& os) const;
162 
163  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
164  logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
165  }
166 
167  protected:
170  double m_dt;
171  RobotUtilShrPtr m_robot_util;
172 
178 
179  /* Estimator parameters */
180  double m_w_imu;
189  double m_fz_margin_lf;
190  double m_fz_margin_rf;
191  Vector6 m_K_rf;
192  Vector6 m_K_lf;
193 
194  Eigen::VectorXd m_v_kin;
195  Eigen::VectorXd m_v_flex;
196  Eigen::VectorXd m_v_imu;
197  Eigen::VectorXd
199 
200  Vector3 m_v_ac;
201  Vector3 m_a_ac;
202 
203  double m_alpha_DC_acc;
204  double m_alpha_DC_vel;
205 
207 
210 
211  pinocchio::Model m_model;
212  pinocchio::Data* m_data;
213  pinocchio::SE3 m_oMff_lf;
214  pinocchio::SE3 m_oMff_rf;
215  SE3 m_oMlfs;
216  SE3 m_oMrfs;
221  normal m_normal;
222 
224 
226 
227  pinocchio::FrameIndex m_right_foot_id;
228  pinocchio::FrameIndex m_left_foot_id;
229  pinocchio::FrameIndex m_IMU_body_id;
230 
231  Eigen::VectorXd m_q_pin;
232  Eigen::VectorXd m_q_sot;
233  Eigen::VectorXd m_v_pin;
234  Eigen::VectorXd m_v_sot;
235  Matrix3 m_oRchest;
236  Matrix3 m_oRff;
237 
238  /* Filter buffers*/
239  Vector3 m_last_vel;
240  Vector3 m_last_DCvel;
241  Vector3 m_last_DCacc;
242 
243 }; // class BaseEstimator
244 
245 } // namespace torque_control
246 } // namespace sot
247 } // namespace dynamicgraph
248 
249 #endif // #ifndef __sot_torque_control_free_flyer_locator_H__
double m_w_lf_filtered
filter parameter to filter weights (1st order low pass filter)
SE3 m_oMlfs
world-to-base transformation obtained through right foot
Vector3 m_v_ac
6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities) ...
double m_fz_std_dev_lf
standard deviation of normal force measurement errors for right foot
double m_zmp_margin_lf
sizes of the left foot (pos x, neg x, pos y, neg y)
double m_fz_std_dev_rf
standard deviation of ZMP measurement errors for left foot
Vector4 m_right_foot_sizes
sizes of the left foot (pos x, neg x, pos y, neg y)
bool m_reset_foot_pos
true if the entity has been successfully initialized
Eigen::VectorXd m_v_gyr
6d robot velocities form imu only (accelerometer integration + gyro)
Vector3 m_a_ac
velocity of the base in the world with DC component removed
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::Matrix< double, 3, 1 > Vector3
Eigen::VectorXd m_v_imu
6d robot velocities from flexibility only (force sensor derivative)
pinocchio::FrameIndex m_right_foot_id
foot sole to F/T sensor transformation
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d &R)
Matrix3 m_oRchest
robot velocities according to SoT convention
SE3 m_oMrfs
transformation from world to left foot sole
Eigen::VectorXd m_v_kin
6d stiffness of left foot spring
int m_rf_fz_stable_cpt
counter for detecting for how long the feet has been stable
RobotUtilShrPtr m_robot_util
sampling time step
Matrix3 m_oRff
chest orientation in the world from angular fusion
void quanternionMult(const Eigen::Vector4d &q1, const Eigen::Vector4d &q2, Eigen::Vector4d &q12)
double m_w_rf_filtered
filtered weight of the estimation coming from the left foot
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
Vector6 m_K_lf
6d stiffness of right foot spring
pinocchio::SE3 m_oMff_rf
world-to-base transformation obtained through left foot
Vector4 m_left_foot_sizes
standard deviation of normal force measurement errors for left foot
#define SOTBASEESTIMATOR_EXPORT
double m_fz_margin_rf
margin used for computing normal force weight
Vector7 m_oMlfs_xyzquat
transformation from world to right foot sole
double m_zmp_std_dev_lf
standard deviation of ZMP measurement errors for right foot
bool m_right_foot_is_stable
True if left foot as been stable for the last &#39;m_fz_stable_windows_size&#39; samples. ...
int m_fz_stable_windows_size
True if right foot as been stable for the last &#39;m_fz_stable_windows_size&#39; samples.
void matrixToRpy(const Eigen::Matrix3d &M, Eigen::Vector3d &rpy)
double m_alpha_w_filter
alpha parameter for DC blocker filter on velocity data
Eigen::Matrix< double, 6, 1 > Vector6
Vector6 m_K_rf
margin used for computing normal force weight
Eigen::VectorXd m_v_flex
6d robot velocities from kinematic only (encoders derivative)
pinocchio::SE3 m_oMff_lf
Pinocchio robot data.
void se3Interp(const pinocchio::SE3 &s1, const pinocchio::SE3 &s2, const double alpha, pinocchio::SE3 &s12)
normal m_normal
Default reference for right foot pose to use if no ref is pluged.
SE3 m_oMrfs_default_ref
Default reference for left foot pose to use if no ref is pluged.
double m_fz_margin_lf
margin used for computing zmp weight
void pointRotationByQuaternion(const Eigen::Vector3d &point, const Eigen::Vector4d &quat, Eigen::Vector3d &rotatedPoint)
double m_alpha_DC_acc
acceleration of the base in the world with DC component removed
pinocchio::Model m_model
filtered weight of the estimation coming from the right foot
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Vector3 m_last_vel
base orientation in the world
double m_dt
true after the command resetFootPositions is called
double m_zmp_margin_rf
margin used for computing zmp weight
Eigen::VectorXd m_v_pin
robot configuration according to SoT convention
pinocchio::Data * m_data
Pinocchio robot model.
double m_alpha_DC_vel
alpha parameter for DC blocker filter on acceleration data
double m_zmp_std_dev_rf
weight of IMU for sensor fusion
to read text file
Definition: treeview.dox:22
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention
double m_w_imu
counter for detecting for how long the feet has been stable