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__