#include <sot/torque_control/base-estimator.hh>
Public Member Functions | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | BaseEstimator (const std::string &name) |
double | compute_force_weight (double fz, double std_dev, double margin) |
void | compute_zmp (const Vector6 &w, Vector2 &zmp) |
double | compute_zmp_weight (const Vector2 &zmp, const Vector4 &foot_sizes, double std_dev, double margin) |
DECLARE_SIGNAL_IN (joint_positions, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (joint_velocities, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (imu_quaternion, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (forceLLEG, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (forceRLEG, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (dforceLLEG, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (dforceRLEG, dynamicgraph::Vector) | |
derivative of left force torque sensor More... | |
DECLARE_SIGNAL_IN (w_lf_in, double) | |
derivative of right force torque sensor More... | |
DECLARE_SIGNAL_IN (w_rf_in, double) | |
weight of the estimation coming from the left foot More... | |
DECLARE_SIGNAL_IN (K_fb_feet_poses, double) | |
weight of the estimation coming from the right foot More... | |
DECLARE_SIGNAL_IN (lf_ref_xyzquat, dynamicgraph::Vector) | |
feed back gain to correct feet position according to last base estimation and kinematic More... | |
DECLARE_SIGNAL_IN (rf_ref_xyzquat, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (accelerometer, dynamicgraph::Vector) | |
DECLARE_SIGNAL_IN (gyroscope, dynamicgraph::Vector) | |
DECLARE_SIGNAL_INNER (kinematics_computations, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT (q, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT (v, dynamicgraph::Vector) | |
n+6 robot configuration with base6d in RPY More... | |
DECLARE_SIGNAL_OUT (v_kin, dynamicgraph::Vector) | |
n+6 robot velocities More... | |
DECLARE_SIGNAL_OUT (v_flex, dynamicgraph::Vector) | |
6d robot velocities from kinematic only (encoders derivative) More... | |
DECLARE_SIGNAL_OUT (v_imu, dynamicgraph::Vector) | |
6d robot velocities from flexibility only (force sensor derivative) More... | |
DECLARE_SIGNAL_OUT (v_gyr, dynamicgraph::Vector) | |
6d robot velocities form imu only (accelerometer integration + gyro) More... | |
DECLARE_SIGNAL_OUT (lf_xyzquat, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT (rf_xyzquat, dynamicgraph::Vector) | |
left foot pose More... | |
DECLARE_SIGNAL_OUT (a_ac, dynamicgraph::Vector) | |
right foot pose More... | |
DECLARE_SIGNAL_OUT (v_ac, dynamicgraph::Vector) | |
acceleration of the base in the world with DC component removed More... | |
DECLARE_SIGNAL_OUT (q_lf, dynamicgraph::Vector) | |
velocity of the base in the world with DC component removed More... | |
DECLARE_SIGNAL_OUT (q_rf, dynamicgraph::Vector) | |
n+6 robot configuration with base6d in RPY More... | |
DECLARE_SIGNAL_OUT (q_imu, dynamicgraph::Vector) | |
n+6 robot configuration with base6d in RPY More... | |
DECLARE_SIGNAL_OUT (w_lf, double) | |
n+6 robot configuration with base6d in RPY More... | |
DECLARE_SIGNAL_OUT (w_rf, double) | |
weight of the estimation coming from the left foot More... | |
DECLARE_SIGNAL_OUT (w_lf_filtered, double) | |
weight of the estimation coming from the right foot More... | |
DECLARE_SIGNAL_OUT (w_rf_filtered, double) | |
filtered weight of the estimation coming from the left foot More... | |
virtual void | display (std::ostream &os) const |
filtered weight of the estimation coming from the right foot More... | |
void | init (const double &dt, const std::string &urdfFile) |
void | kinematics_estimation (const Vector6 &ft, const Vector6 &K, const SE3 &oMfs, const int foot_id, SE3 &oMff, SE3 &oMfa, SE3 &fsMff) |
void | reset_foot_positions () |
void | reset_foot_positions_impl (const Vector6 &ftlf, const Vector6 &ftrf) |
void | sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0) |
void | set_alpha_DC_acc (const double &a) |
void | set_alpha_DC_vel (const double &a) |
void | set_alpha_w_filter (const double &a) |
void | set_fz_stable_windows_size (const int &ws) |
void | set_imu_weight (const double &w) |
void | set_left_foot_sizes (const dynamicgraph::Vector &s) |
void | set_normal_force_margin_left_foot (const double &margin) |
void | set_normal_force_margin_right_foot (const double &margin) |
void | set_normal_force_std_dev_left_foot (const double &std_dev) |
void | set_normal_force_std_dev_right_foot (const double &std_dev) |
void | set_right_foot_sizes (const dynamicgraph::Vector &s) |
void | set_stiffness_left_foot (const dynamicgraph::Vector &k) |
void | set_stiffness_right_foot (const dynamicgraph::Vector &k) |
void | set_zmp_margin_left_foot (const double &margin) |
void | set_zmp_margin_right_foot (const double &margin) |
void | set_zmp_std_dev_left_foot (const double &std_dev) |
void | set_zmp_std_dev_right_foot (const double &std_dev) |
Protected Attributes | |
Vector3 | m_a_ac |
velocity of the base in the world with DC component removed More... | |
double | m_alpha_DC_acc |
acceleration of the base in the world with DC component removed More... | |
double | m_alpha_DC_vel |
alpha parameter for DC blocker filter on acceleration data More... | |
double | m_alpha_w_filter |
alpha parameter for DC blocker filter on velocity data More... | |
pinocchio::Data * | m_data |
Pinocchio robot model. More... | |
double | m_dt |
true after the command resetFootPositions is called More... | |
double | m_fz_margin_lf |
margin used for computing zmp weight More... | |
double | m_fz_margin_rf |
margin used for computing normal force weight More... | |
int | m_fz_stable_windows_size |
True if right foot as been stable for the last 'm_fz_stable_windows_size' samples. More... | |
double | m_fz_std_dev_lf |
standard deviation of normal force measurement errors for right foot More... | |
double | m_fz_std_dev_rf |
standard deviation of ZMP measurement errors for left foot More... | |
pinocchio::FrameIndex | m_IMU_body_id |
bool | m_initSucceeded |
bool | m_isFirstIterFlag |
Normal distribution. More... | |
Vector6 | m_K_lf |
6d stiffness of right foot spring More... | |
Vector6 | m_K_rf |
margin used for computing normal force weight More... | |
Vector3 | m_last_DCacc |
Vector3 | m_last_DCvel |
Vector3 | m_last_vel |
base orientation in the world More... | |
pinocchio::FrameIndex | m_left_foot_id |
bool | m_left_foot_is_stable |
Vector4 | m_left_foot_sizes |
standard deviation of normal force measurement errors for left foot More... | |
int | m_lf_fz_stable_cpt |
size of the windows used to detect that feet did not leave the ground More... | |
pinocchio::Model | m_model |
filtered weight of the estimation coming from the right foot More... | |
normal | m_normal |
Default reference for right foot pose to use if no ref is pluged. More... | |
pinocchio::SE3 | m_oMff_lf |
Pinocchio robot data. More... | |
pinocchio::SE3 | m_oMff_rf |
world-to-base transformation obtained through left foot More... | |
SE3 | m_oMlfs |
world-to-base transformation obtained through right foot More... | |
SE3 | m_oMlfs_default_ref |
Vector7 | m_oMlfs_xyzquat |
transformation from world to right foot sole More... | |
SE3 | m_oMrfs |
transformation from world to left foot sole More... | |
SE3 | m_oMrfs_default_ref |
Default reference for left foot pose to use if no ref is pluged. More... | |
Vector7 | m_oMrfs_xyzquat |
Matrix3 | m_oRchest |
robot velocities according to SoT convention More... | |
Matrix3 | m_oRff |
chest orientation in the world from angular fusion More... | |
Eigen::VectorXd | m_q_pin |
Eigen::VectorXd | m_q_sot |
robot configuration according to pinocchio convention More... | |
bool | m_reset_foot_pos |
true if the entity has been successfully initialized More... | |
int | m_rf_fz_stable_cpt |
counter for detecting for how long the feet has been stable More... | |
pinocchio::FrameIndex | m_right_foot_id |
foot sole to F/T sensor transformation More... | |
bool | m_right_foot_is_stable |
True if left foot as been stable for the last 'm_fz_stable_windows_size' samples. More... | |
Vector4 | m_right_foot_sizes |
sizes of the left foot (pos x, neg x, pos y, neg y) More... | |
RobotUtilShrPtr | m_robot_util |
sampling time step More... | |
SE3 | m_sole_M_ftSens |
flag to detect first iteration and initialise velocity filters More... | |
Vector3 | m_v_ac |
6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities) More... | |
Eigen::VectorXd | m_v_flex |
6d robot velocities from kinematic only (encoders derivative) More... | |
Eigen::VectorXd | m_v_gyr |
6d robot velocities form imu only (accelerometer integration + gyro) More... | |
Eigen::VectorXd | m_v_imu |
6d robot velocities from flexibility only (force sensor derivative) More... | |
Eigen::VectorXd | m_v_kin |
6d stiffness of left foot spring More... | |
Eigen::VectorXd | m_v_pin |
robot configuration according to SoT convention More... | |
Eigen::VectorXd | m_v_sot |
robot velocities according to pinocchio convention More... | |
double | m_w_imu |
counter for detecting for how long the feet has been stable More... | |
double | m_w_lf_filtered |
filter parameter to filter weights (1st order low pass filter) More... | |
double | m_w_rf_filtered |
filtered weight of the estimation coming from the left foot More... | |
double | m_zmp_margin_lf |
sizes of the left foot (pos x, neg x, pos y, neg y) More... | |
double | m_zmp_margin_rf |
margin used for computing zmp weight More... | |
double | m_zmp_std_dev_lf |
standard deviation of ZMP measurement errors for right foot More... | |
double | m_zmp_std_dev_rf |
weight of IMU for sensor fusion More... | |
Definition at line 70 of file base-estimator.hh.
BaseEstimator | ( | const std::string & | name | ) |
Definition at line 95 of file base-estimator.cpp.
double compute_force_weight | ( | double | fz, |
double | std_dev, | ||
double | margin | ||
) |
Definition at line 382 of file base-estimator.cpp.
void compute_zmp | ( | const Vector6 & | w, |
Vector2 & | zmp | ||
) |
Definition at line 360 of file base-estimator.cpp.
double compute_zmp_weight | ( | const Vector2 & | zmp, |
const Vector4 & | foot_sizes, | ||
double | std_dev, | ||
double | margin | ||
) |
Definition at line 368 of file base-estimator.cpp.
DECLARE_SIGNAL_IN | ( | joint_positions | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | joint_velocities | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | imu_quaternion | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | forceLLEG | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | forceRLEG | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | dforceLLEG | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | dforceRLEG | , |
dynamicgraph::Vector | |||
) |
derivative of left force torque sensor
DECLARE_SIGNAL_IN | ( | w_lf_in | , |
double | |||
) |
derivative of right force torque sensor
DECLARE_SIGNAL_IN | ( | w_rf_in | , |
double | |||
) |
weight of the estimation coming from the left foot
DECLARE_SIGNAL_IN | ( | K_fb_feet_poses | , |
double | |||
) |
weight of the estimation coming from the right foot
DECLARE_SIGNAL_IN | ( | lf_ref_xyzquat | , |
dynamicgraph::Vector | |||
) |
feed back gain to correct feet position according to last base estimation and kinematic
DECLARE_SIGNAL_IN | ( | rf_ref_xyzquat | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | accelerometer | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_IN | ( | gyroscope | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_INNER | ( | kinematics_computations | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_OUT | ( | q | , |
dynamicgraph::Vector | |||
) |
DECLARE_SIGNAL_OUT | ( | v | , |
dynamicgraph::Vector | |||
) |
n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_OUT | ( | v_kin | , |
dynamicgraph::Vector | |||
) |
n+6 robot velocities
DECLARE_SIGNAL_OUT | ( | v_flex | , |
dynamicgraph::Vector | |||
) |
6d robot velocities from kinematic only (encoders derivative)
DECLARE_SIGNAL_OUT | ( | v_imu | , |
dynamicgraph::Vector | |||
) |
6d robot velocities from flexibility only (force sensor derivative)
DECLARE_SIGNAL_OUT | ( | v_gyr | , |
dynamicgraph::Vector | |||
) |
6d robot velocities form imu only (accelerometer integration + gyro)
DECLARE_SIGNAL_OUT | ( | lf_xyzquat | , |
dynamicgraph::Vector | |||
) |
6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities)
DECLARE_SIGNAL_OUT | ( | rf_xyzquat | , |
dynamicgraph::Vector | |||
) |
left foot pose
DECLARE_SIGNAL_OUT | ( | a_ac | , |
dynamicgraph::Vector | |||
) |
right foot pose
DECLARE_SIGNAL_OUT | ( | v_ac | , |
dynamicgraph::Vector | |||
) |
acceleration of the base in the world with DC component removed
DECLARE_SIGNAL_OUT | ( | q_lf | , |
dynamicgraph::Vector | |||
) |
velocity of the base in the world with DC component removed
DECLARE_SIGNAL_OUT | ( | q_rf | , |
dynamicgraph::Vector | |||
) |
n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_OUT | ( | q_imu | , |
dynamicgraph::Vector | |||
) |
n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_OUT | ( | w_lf | , |
double | |||
) |
n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_OUT | ( | w_rf | , |
double | |||
) |
weight of the estimation coming from the left foot
DECLARE_SIGNAL_OUT | ( | w_lf_filtered | , |
double | |||
) |
weight of the estimation coming from the right foot
DECLARE_SIGNAL_OUT | ( | w_rf_filtered | , |
double | |||
) |
filtered weight of the estimation coming from the left foot
|
virtual |
filtered weight of the estimation coming from the right foot
Definition at line 1044 of file base-estimator.cpp.
void init | ( | const double & | dt, |
const std::string & | urdfFile | ||
) |
Definition at line 229 of file base-estimator.cpp.
void kinematics_estimation | ( | const Vector6 & | ft, |
const Vector6 & | K, | ||
const SE3 & | oMfs, | ||
const int | foot_id, | ||
SE3 & | oMff, | ||
SE3 & | oMfa, | ||
SE3 & | fsMff | ||
) |
Definition at line 442 of file base-estimator.cpp.
void reset_foot_positions | ( | ) |
Definition at line 305 of file base-estimator.cpp.
void reset_foot_positions_impl | ( | const Vector6 & | ftlf, |
const Vector6 & | ftrf | ||
) |
Definition at line 387 of file base-estimator.cpp.
|
inline |
Definition at line 163 of file base-estimator.hh.
void set_alpha_DC_acc | ( | const double & | a | ) |
Definition at line 295 of file base-estimator.cpp.
void set_alpha_DC_vel | ( | const double & | a | ) |
Definition at line 300 of file base-estimator.cpp.
void set_alpha_w_filter | ( | const double & | a | ) |
Definition at line 290 of file base-estimator.cpp.
void set_fz_stable_windows_size | ( | const int & | ws | ) |
Definition at line 285 of file base-estimator.cpp.
void set_imu_weight | ( | const double & | w | ) |
Definition at line 307 of file base-estimator.cpp.
void set_left_foot_sizes | ( | const dynamicgraph::Vector & | s | ) |
Definition at line 347 of file base-estimator.cpp.
void set_normal_force_margin_left_foot | ( | const double & | margin | ) |
Definition at line 358 of file base-estimator.cpp.
void set_normal_force_margin_right_foot | ( | const double & | margin | ) |
Definition at line 356 of file base-estimator.cpp.
void set_normal_force_std_dev_left_foot | ( | const double & | std_dev | ) |
Definition at line 337 of file base-estimator.cpp.
void set_normal_force_std_dev_right_foot | ( | const double & | std_dev | ) |
Definition at line 332 of file base-estimator.cpp.
void set_right_foot_sizes | ( | const dynamicgraph::Vector & | s | ) |
Definition at line 342 of file base-estimator.cpp.
void set_stiffness_left_foot | ( | const dynamicgraph::Vector & | k | ) |
Definition at line 317 of file base-estimator.cpp.
void set_stiffness_right_foot | ( | const dynamicgraph::Vector & | k | ) |
Definition at line 312 of file base-estimator.cpp.
void set_zmp_margin_left_foot | ( | const double & | margin | ) |
Definition at line 354 of file base-estimator.cpp.
void set_zmp_margin_right_foot | ( | const double & | margin | ) |
Definition at line 352 of file base-estimator.cpp.
void set_zmp_std_dev_left_foot | ( | const double & | std_dev | ) |
Definition at line 327 of file base-estimator.cpp.
void set_zmp_std_dev_right_foot | ( | const double & | std_dev | ) |
Definition at line 322 of file base-estimator.cpp.
|
protected |
velocity of the base in the world with DC component removed
Definition at line 201 of file base-estimator.hh.
|
protected |
acceleration of the base in the world with DC component removed
Definition at line 203 of file base-estimator.hh.
|
protected |
alpha parameter for DC blocker filter on acceleration data
Definition at line 204 of file base-estimator.hh.
|
protected |
alpha parameter for DC blocker filter on velocity data
Definition at line 206 of file base-estimator.hh.
|
protected |
Pinocchio robot model.
Definition at line 212 of file base-estimator.hh.
|
protected |
true after the command resetFootPositions is called
Definition at line 170 of file base-estimator.hh.
|
protected |
margin used for computing zmp weight
Definition at line 189 of file base-estimator.hh.
|
protected |
margin used for computing normal force weight
Definition at line 190 of file base-estimator.hh.
|
protected |
True if right foot as been stable for the last 'm_fz_stable_windows_size' samples.
Definition at line 175 of file base-estimator.hh.
|
protected |
standard deviation of normal force measurement errors for right foot
Definition at line 184 of file base-estimator.hh.
|
protected |
standard deviation of ZMP measurement errors for left foot
Definition at line 183 of file base-estimator.hh.
|
protected |
Definition at line 229 of file base-estimator.hh.
|
protected |
Definition at line 168 of file base-estimator.hh.
|
protected |
Normal distribution.
Definition at line 223 of file base-estimator.hh.
|
protected |
6d stiffness of right foot spring
Definition at line 192 of file base-estimator.hh.
|
protected |
margin used for computing normal force weight
Definition at line 191 of file base-estimator.hh.
|
protected |
Definition at line 241 of file base-estimator.hh.
|
protected |
Definition at line 240 of file base-estimator.hh.
|
protected |
base orientation in the world
Definition at line 239 of file base-estimator.hh.
|
protected |
Definition at line 228 of file base-estimator.hh.
|
protected |
Definition at line 173 of file base-estimator.hh.
|
protected |
standard deviation of normal force measurement errors for left foot
Definition at line 185 of file base-estimator.hh.
|
protected |
size of the windows used to detect that feet did not leave the ground
Definition at line 176 of file base-estimator.hh.
|
protected |
filtered weight of the estimation coming from the right foot
Definition at line 211 of file base-estimator.hh.
|
protected |
Default reference for right foot pose to use if no ref is pluged.
Definition at line 221 of file base-estimator.hh.
|
protected |
Pinocchio robot data.
Definition at line 213 of file base-estimator.hh.
|
protected |
world-to-base transformation obtained through left foot
Definition at line 214 of file base-estimator.hh.
|
protected |
world-to-base transformation obtained through right foot
Definition at line 215 of file base-estimator.hh.
|
protected |
Definition at line 219 of file base-estimator.hh.
|
protected |
transformation from world to right foot sole
Definition at line 217 of file base-estimator.hh.
|
protected |
transformation from world to left foot sole
Definition at line 216 of file base-estimator.hh.
|
protected |
Default reference for left foot pose to use if no ref is pluged.
Definition at line 220 of file base-estimator.hh.
|
protected |
Definition at line 218 of file base-estimator.hh.
|
protected |
robot velocities according to SoT convention
Definition at line 235 of file base-estimator.hh.
|
protected |
chest orientation in the world from angular fusion
Definition at line 236 of file base-estimator.hh.
|
protected |
Definition at line 231 of file base-estimator.hh.
|
protected |
robot configuration according to pinocchio convention
Definition at line 232 of file base-estimator.hh.
|
protected |
true if the entity has been successfully initialized
Definition at line 169 of file base-estimator.hh.
|
protected |
counter for detecting for how long the feet has been stable
Definition at line 177 of file base-estimator.hh.
|
protected |
foot sole to F/T sensor transformation
Definition at line 227 of file base-estimator.hh.
|
protected |
True if left foot as been stable for the last 'm_fz_stable_windows_size' samples.
Definition at line 174 of file base-estimator.hh.
|
protected |
sizes of the left foot (pos x, neg x, pos y, neg y)
Definition at line 186 of file base-estimator.hh.
|
protected |
sampling time step
Definition at line 171 of file base-estimator.hh.
|
protected |
flag to detect first iteration and initialise velocity filters
Definition at line 225 of file base-estimator.hh.
|
protected |
6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities)
Definition at line 200 of file base-estimator.hh.
|
protected |
6d robot velocities from kinematic only (encoders derivative)
Definition at line 195 of file base-estimator.hh.
|
protected |
6d robot velocities form imu only (accelerometer integration + gyro)
Definition at line 198 of file base-estimator.hh.
|
protected |
6d robot velocities from flexibility only (force sensor derivative)
Definition at line 196 of file base-estimator.hh.
|
protected |
6d stiffness of left foot spring
Definition at line 194 of file base-estimator.hh.
|
protected |
robot configuration according to SoT convention
Definition at line 233 of file base-estimator.hh.
|
protected |
robot velocities according to pinocchio convention
Definition at line 234 of file base-estimator.hh.
|
protected |
counter for detecting for how long the feet has been stable
Definition at line 180 of file base-estimator.hh.
|
protected |
filter parameter to filter weights (1st order low pass filter)
Definition at line 208 of file base-estimator.hh.
|
protected |
filtered weight of the estimation coming from the left foot
Definition at line 209 of file base-estimator.hh.
|
protected |
sizes of the left foot (pos x, neg x, pos y, neg y)
Definition at line 187 of file base-estimator.hh.
|
protected |
margin used for computing zmp weight
Definition at line 188 of file base-estimator.hh.
|
protected |
standard deviation of ZMP measurement errors for right foot
Definition at line 182 of file base-estimator.hh.
|
protected |
weight of IMU for sensor fusion
Definition at line 181 of file base-estimator.hh.