6 #ifndef __sot_torque_control_base_estimator_H__ 7 #define __sot_torque_control_base_estimator_H__ 14 #if defined(base_estimator_EXPORTS) 15 #define SOTBASEESTIMATOR_EXPORT __declspec(dllexport) 17 #define SOTBASEESTIMATOR_EXPORT __declspec(dllimport) 20 #define SOTBASEESTIMATOR_EXPORT 28 #include "boost/assign.hpp" 30 #include <boost/math/distributions/normal.hpp> 33 #include <pinocchio/multibody/model.hpp> 34 #include <pinocchio/parsers/urdf.hpp> 35 #include <pinocchio/algorithm/kinematics.hpp> 38 #include <dynamic-graph/signal-helper.h> 39 #include <sot/core/matrix-geometry.hh> 40 #include <sot/core/robot-utils.hh> 52 void se3Interp(
const pinocchio::SE3& s1,
const pinocchio::SE3& s2,
const double alpha, pinocchio::SE3& s12);
55 void rpyToMatrix(
double r,
double p,
double y, Eigen::Matrix3d& R);
58 void rpyToMatrix(
const Eigen::Vector3d& rpy, Eigen::Matrix3d& R);
61 void matrixToRpy(
const Eigen::Matrix3d& M, Eigen::Vector3d& rpy);
64 void quanternionMult(
const Eigen::Vector4d& q1,
const Eigen::Vector4d& q2, Eigen::Vector4d& q12);
68 Eigen::Vector3d& rotatedPoint);
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;
81 DYNAMIC_GRAPH_ENTITY_DECL();
84 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
89 void init(
const double& dt,
const std::string& urdfFile);
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);
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);
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);
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);
135 DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);
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);
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);
161 virtual void display(std::ostream& os)
const;
163 void sendMsg(
const std::string& msg, MsgType t = MSG_TYPE_INFO,
const char* =
"",
int = 0) {
164 logger_.stream(t) << (
"[" + name +
"] " + msg) <<
'\n';
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
pinocchio::FrameIndex m_left_foot_id
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)
pinocchio::FrameIndex m_IMU_body_id
double m_w_rf_filtered
filtered weight of the estimation coming from the left foot
Eigen::Matrix< double, 2, 1 > Vector2
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
bool m_left_foot_is_stable
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
bool m_isFirstIterFlag
Normal distribution.
#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 'm_fz_stable_windows_size' samples. ...
int m_fz_stable_windows_size
True if right foot as been stable for the last 'm_fz_stable_windows_size' 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)
AdmittanceController EntityClassName
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
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