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 27 #include <pinocchio/fwd.hpp> 33 #include "boost/assign.hpp" 35 #include <boost/math/distributions/normal.hpp> 38 #include <pinocchio/algorithm/kinematics.hpp> 39 #include <pinocchio/multibody/model.hpp> 40 #include <pinocchio/parsers/urdf.hpp> 43 #include <dynamic-graph/signal-helper.h> 45 #include <sot/core/matrix-geometry.hh> 46 #include <sot/core/robot-utils.hh> 58 void se3Interp(
const pinocchio::SE3& s1,
const pinocchio::SE3& s2,
59 const double alpha, pinocchio::SE3& s12);
62 void rpyToMatrix(
double r,
double p,
double y, Eigen::Matrix3d& R);
65 void rpyToMatrix(
const Eigen::Vector3d& rpy, Eigen::Matrix3d& R);
68 void matrixToRpy(
const Eigen::Matrix3d& M, Eigen::Vector3d& rpy);
71 void quanternionMult(
const Eigen::Vector4d& q1,
const Eigen::Vector4d& q2,
72 Eigen::Vector4d& q12);
76 const Eigen::Vector4d& quat,
77 Eigen::Vector3d& rotatedPoint);
81 typedef pinocchio::SE3 SE3;
82 typedef Eigen::Vector2d
Vector2;
83 typedef Eigen::Vector3d
Vector3;
84 typedef Eigen::Vector4d Vector4;
85 typedef dynamicgraph::sot::Vector6d
Vector6;
86 typedef dynamicgraph::sot::Vector7d Vector7;
87 typedef Eigen::Matrix3d Matrix3;
88 typedef boost::math::normal normal;
90 DYNAMIC_GRAPH_ENTITY_DECL();
93 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
98 void init(
const double& dt,
const std::string& urdfFile);
100 void set_fz_stable_windows_size(
const int& ws);
101 void set_alpha_w_filter(
const double& a);
102 void set_alpha_DC_acc(
const double& a);
103 void set_alpha_DC_vel(
const double& a);
104 void reset_foot_positions();
105 void set_imu_weight(
const double& w);
106 void set_zmp_std_dev_right_foot(
const double& std_dev);
107 void set_zmp_std_dev_left_foot(
const double& std_dev);
108 void set_normal_force_std_dev_right_foot(
const double& std_dev);
109 void set_normal_force_std_dev_left_foot(
const double& std_dev);
110 void set_stiffness_right_foot(
const dynamicgraph::Vector& k);
111 void set_stiffness_left_foot(
const dynamicgraph::Vector& k);
112 void set_right_foot_sizes(
const dynamicgraph::Vector& s);
113 void set_left_foot_sizes(
const dynamicgraph::Vector& s);
114 void set_zmp_margin_right_foot(
const double& margin);
115 void set_zmp_margin_left_foot(
const double& margin);
116 void set_normal_force_margin_right_foot(
const double& margin);
117 void set_normal_force_margin_left_foot(
const double& margin);
119 void reset_foot_positions_impl(
const Vector6& ftlf,
const Vector6& ftrf);
120 void compute_zmp(
const Vector6& w, Vector2& zmp);
121 double compute_zmp_weight(
const Vector2& zmp,
const Vector4& foot_sizes,
122 double std_dev,
double margin);
123 double compute_force_weight(
double fz,
double std_dev,
double margin);
124 void kinematics_estimation(
const Vector6& ft,
const Vector6& K,
125 const SE3& oMfs,
const int foot_id, SE3& oMff,
126 SE3& oMfa, SE3& fsMff);
129 DECLARE_SIGNAL_IN(joint_positions, dynamicgraph::Vector);
130 DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector);
131 DECLARE_SIGNAL_IN(imu_quaternion, dynamicgraph::Vector);
132 DECLARE_SIGNAL_IN(forceLLEG, dynamicgraph::Vector);
133 DECLARE_SIGNAL_IN(forceRLEG, dynamicgraph::Vector);
136 dynamicgraph::Vector);
139 dynamicgraph::Vector);
147 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);
153 DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);
156 q, dynamicgraph::Vector);
157 DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);
159 v_kin, dynamicgraph::Vector);
163 dynamicgraph::Vector);
167 dynamicgraph::Vector);
170 v_gyr, dynamicgraph::Vector);
171 DECLARE_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector);
174 DECLARE_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector);
175 DECLARE_SIGNAL_OUT(a_ac,
176 dynamicgraph::Vector);
177 DECLARE_SIGNAL_OUT(v_ac,
179 dynamicgraph::Vector);
184 dynamicgraph::Vector);
187 dynamicgraph::Vector);
190 dynamicgraph::Vector);
204 virtual void display(std::ostream& os)
const;
206 void sendMsg(
const std::string& msg, MsgType t = MSG_TYPE_INFO,
207 const char* =
"",
int = 0) {
208 logger_.stream(t) << (
"[" + name +
"] " + msg) <<
'\n';
220 bool m_right_foot_is_stable;
222 int m_fz_stable_windows_size;
224 int m_lf_fz_stable_cpt;
226 int m_rf_fz_stable_cpt;
233 double m_zmp_std_dev_lf;
235 double m_fz_std_dev_rf;
237 double m_fz_std_dev_lf;
239 Vector4 m_left_foot_sizes;
241 Vector4 m_right_foot_sizes;
243 double m_zmp_margin_lf;
252 Eigen::VectorXd m_v_flex;
254 Eigen::VectorXd m_v_imu;
256 Eigen::VectorXd m_v_gyr;
266 double m_alpha_DC_vel;
274 double m_w_rf_filtered;
289 SE3 m_oMrfs_default_ref;
323 #endif // #ifndef __sot_torque_control_free_flyer_locator_H__
SE3 m_oMlfs
world-to-base transformation obtained through right foot
bool m_reset_foot_pos
true if the entity has been successfully initialized
Vector3 m_a_ac
velocity of the base in the world with DC component removed
Eigen::Matrix< double, 3, 1 > Vector3
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
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
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
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
void matrixToRpy(const Eigen::Matrix3d &M, Eigen::Vector3d &rpy)
Eigen::Matrix< double, 6, 1 > Vector6
Vector6 m_K_rf
margin used for computing normal force weight
pinocchio::SE3 m_oMff_lf
Pinocchio robot data.
void se3Interp(const pinocchio::SE3 &s1, const pinocchio::SE3 &s2, const double alpha, pinocchio::SE3 &s12)
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
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_zmp_std_dev_rf
weight of IMU for sensor fusion
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention