sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
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 <pinocchio/fwd.hpp>
28
29// include pinocchio first
30
31#include <map>
32
33#include "boost/assign.hpp"
34//#include <boost/random/normal_distribution.hpp>
35#include <boost/math/distributions/normal.hpp> // for normal_distribution
36
37/* Pinocchio */
38#include <pinocchio/algorithm/kinematics.hpp>
39#include <pinocchio/multibody/model.hpp>
40#include <pinocchio/parsers/urdf.hpp>
41
42/* HELPER */
43#include <dynamic-graph/signal-helper.h>
44
45#include <sot/core/matrix-geometry.hh>
46#include <sot/core/robot-utils.hh>
48
49namespace dynamicgraph {
50namespace sot {
51namespace torque_control {
52
53/* --------------------------------------------------------------------- */
54/* --- CLASS ----------------------------------------------------------- */
55/* --------------------------------------------------------------------- */
56
58void se3Interp(const pinocchio::SE3& s1, const pinocchio::SE3& s2,
59 const double alpha, pinocchio::SE3& s12);
60
62void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d& R);
63
65void rpyToMatrix(const Eigen::Vector3d& rpy, Eigen::Matrix3d& R);
66
68void matrixToRpy(const Eigen::Matrix3d& M, Eigen::Vector3d& rpy);
69
71void quanternionMult(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2,
72 Eigen::Vector4d& q12);
73
75void pointRotationByQuaternion(const Eigen::Vector3d& point,
76 const Eigen::Vector4d& quat,
77 Eigen::Vector3d& rotatedPoint);
78
79class SOTBASEESTIMATOR_EXPORT BaseEstimator : public ::dynamicgraph::Entity {
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;
89
90 DYNAMIC_GRAPH_ENTITY_DECL();
91
92 public:
93 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
94
95 /* --- CONSTRUCTOR ---- */
96 BaseEstimator(const std::string& name);
97
98 void init(const double& dt, const std::string& urdfFile);
99
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);
118
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);
127
128 /* --- SIGNALS --- */
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);
135 dforceLLEG,
136 dynamicgraph::Vector);
138 dforceRLEG,
139 dynamicgraph::Vector);
141 w_lf_in, double);
143 w_rf_in, double);
145 K_fb_feet_poses,
146 double);
148 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);
152
153 DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);
154
156 q, dynamicgraph::Vector);
157 DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);
159 v_kin, dynamicgraph::Vector);
162 v_flex,
163 dynamicgraph::Vector);
166 v_imu,
167 dynamicgraph::Vector);
170 v_gyr, dynamicgraph::Vector);
173 DECLARE_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector);
174 DECLARE_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector);
176 dynamicgraph::Vector);
179 dynamicgraph::Vector);
181
183 q_lf,
184 dynamicgraph::Vector);
186 q_rf,
187 dynamicgraph::Vector);
189 q_imu,
190 dynamicgraph::Vector);
192 w_lf, double);
194 w_rf, double);
196 w_lf_filtered,
197 double);
199 w_rf_filtered,
200 double);
201
202 /* --- COMMANDS --- */
203 /* --- ENTITY INHERITANCE --- */
204 virtual void display(std::ostream& os) const;
205
206 void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
207 const char* = "", int = 0) {
208 logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
209 }
210
211 protected:
212 bool
214 bool
216 double m_dt;
217 RobotUtilShrPtr m_robot_util;
218
229
230 /* Estimator parameters */
231 double m_w_imu;
248 Vector6 m_K_rf;
249 Vector6 m_K_lf;
250
251 Eigen::VectorXd m_v_kin;
253 Eigen::VectorXd m_v_flex;
255 Eigen::VectorXd m_v_imu;
257 Eigen::VectorXd m_v_gyr;
259
260 Vector3
262 Vector3 m_a_ac;
264
269
272
277
278 pinocchio::Model m_model;
279 pinocchio::Data* m_data;
280 pinocchio::SE3
282 pinocchio::SE3
292 normal m_normal;
293
296
298
299 pinocchio::FrameIndex m_right_foot_id;
300 pinocchio::FrameIndex m_left_foot_id;
301 pinocchio::FrameIndex m_IMU_body_id;
302
303 Eigen::VectorXd
305 Eigen::VectorXd m_q_sot;
306 Eigen::VectorXd
308 Eigen::VectorXd m_v_sot;
309 Matrix3 m_oRchest;
310 Matrix3 m_oRff;
311
312 /* Filter buffers*/
313 Vector3 m_last_vel;
316
317}; // class BaseEstimator
318
319} // namespace torque_control
320} // namespace sot
321} // namespace dynamicgraph
322
323#endif // #ifndef __sot_torque_control_free_flyer_locator_H__
#define SOTBASEESTIMATOR_EXPORT
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Matrix3 m_oRchest
robot velocities according to SoT convention
double m_fz_margin_lf
margin used for computing zmp weight
SE3 m_oMlfs
world-to-base transformation obtained through right foot
DECLARE_SIGNAL_IN(dforceRLEG, dynamicgraph::Vector)
derivative of left force torque sensor
DECLARE_SIGNAL_OUT(q_rf, dynamicgraph::Vector)
n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_OUT(v_gyr, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector)
left foot pose
Eigen::VectorXd m_v_pin
robot configuration according to SoT convention
Vector3 m_a_ac
velocity of the base in the world with DC component removed
DECLARE_SIGNAL_IN(imu_quaternion, dynamicgraph::Vector)
Eigen::VectorXd m_v_kin
6d stiffness of left foot spring
DECLARE_SIGNAL_IN(lf_ref_xyzquat, dynamicgraph::Vector)
RobotUtilShrPtr m_robot_util
sampling time step
DECLARE_SIGNAL_OUT(v_flex, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(w_rf_in, double)
weight of the estimation coming from the left foot
Vector3 m_last_vel
base orientation in the world
pinocchio::Data * m_data
Pinocchio robot model.
DECLARE_SIGNAL_IN(forceRLEG, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(forceLLEG, dynamicgraph::Vector)
double m_zmp_margin_rf
margin used for computing zmp weight
DECLARE_SIGNAL_IN(rf_ref_xyzquat, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(w_lf_filtered, double)
weight of the estimation coming from the right foot
DECLARE_SIGNAL_IN(w_lf_in, double)
derivative of right force torque sensor
double m_zmp_std_dev_rf
weight of IMU for sensor fusion
DECLARE_SIGNAL_OUT(w_rf_filtered, double)
filtered weight of the estimation coming from the left foot
double m_dt
true after the command resetFootPositions is called
DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector)
n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_OUT(a_ac, dynamicgraph::Vector)
right foot pose
double m_fz_margin_rf
margin used for computing normal force weight
DECLARE_SIGNAL_OUT(v_kin, dynamicgraph::Vector)
n+6 robot velocities
Matrix3 m_oRff
chest orientation in the world from angular fusion
DECLARE_SIGNAL_OUT(v_ac, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(w_lf, double)
n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_OUT(q_lf, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(q_imu, dynamicgraph::Vector)
n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector)
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention
pinocchio::SE3 m_oMff_rf
world-to-base transformation obtained through left foot
Vector6 m_K_rf
margin used for computing normal force weight
DECLARE_SIGNAL_OUT(v_imu, dynamicgraph::Vector)
bool m_reset_foot_pos
true if the entity has been successfully initialized
DECLARE_SIGNAL_IN(K_fb_feet_poses, double)
weight of the estimation coming from the right foot
Vector7 m_oMlfs_xyzquat
transformation from world to right foot sole
DECLARE_SIGNAL_IN(joint_positions, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector)
SE3 m_oMrfs
transformation from world to left foot sole
pinocchio::FrameIndex m_right_foot_id
foot sole to F/T sensor transformation
DECLARE_SIGNAL_IN(dforceLLEG, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(w_rf, double)
weight of the estimation coming from the left foot
pinocchio::SE3 m_oMff_lf
Pinocchio robot data.
Vector6 m_K_lf
6d stiffness of right foot spring
void pointRotationByQuaternion(const Eigen::Vector3d &point, const Eigen::Vector4d &quat, Eigen::Vector3d &rotatedPoint)
void quanternionMult(const Eigen::Vector4d &q1, const Eigen::Vector4d &q2, Eigen::Vector4d &q12)
void se3Interp(const pinocchio::SE3 &s1, const pinocchio::SE3 &s2, const double alpha, pinocchio::SE3 &s12)
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d &R)
void matrixToRpy(const Eigen::Matrix3d &M, Eigen::Vector3d &rpy)
to read text file
Definition: treeview.dox:22