17 #ifndef __sot_torque_control_base_estimator_H__ 18 #define __sot_torque_control_base_estimator_H__ 25 # if defined (base_estimator_EXPORTS) 26 # define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllexport) 28 # define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllimport) 31 # define TALOS_BASE_ESTIMATOR_EXPORT 39 #include <dynamic-graph/signal-helper.h> 40 #include <sot/core/matrix-geometry.hh> 42 #include "boost/assign.hpp" 44 #include <boost/math/distributions/normal.hpp> 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> 54 namespace talos_balance {
62 void se3Interp(
const pinocchio::SE3 & s1,
const pinocchio::SE3 & s2,
const double alpha, pinocchio::SE3 & s12);
65 void rpyToMatrix(
double r,
double p,
double y, Eigen::Matrix3d & R);
68 void rpyToMatrix(
const Eigen::Vector3d & rpy, Eigen::Matrix3d & R);
71 void matrixToRpy(
const Eigen::Matrix3d & M, Eigen::Vector3d & rpy);
74 void quanternionMult(
const Eigen::Vector4d & q1,
const Eigen::Vector4d & q2, Eigen::Vector4d & q12);
77 void pointRotationByQuaternion(
const Eigen::Vector3d & point,
const Eigen::Vector4d & quat, Eigen::Vector3d & rotatedPoint);
83 double wEulerMean(
double a1,
double a2,
double w1,
double w2);
86 :public::dynamicgraph::Entity
89 typedef pinocchio::SE3 SE3;
90 typedef Eigen::Vector2d Vector2;
91 typedef Eigen::Vector3d
Vector3;
92 typedef Eigen::Vector4d Vector4;
94 typedef Vector7d Vector7;
95 typedef Eigen::Matrix3d Matrix3;
96 typedef boost::math::normal normal;
98 DYNAMIC_GRAPH_ENTITY_DECL();
101 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
106 void init(
const double & dt,
const std::string& urdfFile);
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);
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);
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);
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);
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);
175 virtual void display( std::ostream& os )
const;
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
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
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
RobotUtilShrPtr m_robot_util
sampling time step
AdmittanceControllerEndEffector EntityClassName
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.
bool m_isFirstIterFlag
Normal distribution.
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 'm_fz_stable_windows_size' samples. ...
pinocchio::FrameIndex m_left_foot_id
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
Vector3 m_last_vel
base orientation in the world
int m_fz_stable_windows_size
True if right foot as been stable for the last 'm_fz_stable_windows_size' samples.
normal m_normal
Default reference for right foot pose to use if no ref is pluged.
pinocchio::SE3 m_torsoMimu
Pinocchio robot data.
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
pinocchio::FrameIndex m_IMU_frame_id
double m_fz_margin_rf
margin used for computing normal force weight
pinocchio::FrameIndex m_torso_id
void matrixToRpy(const Eigen::Matrix3d &M, Eigen::Vector3d &rpy)
double m_w_imu
counter for detecting for how long the feet has been stable
bool m_left_foot_is_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
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