#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) | |
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) | |
DECLARE_SIGNAL_OUT (v_imu, dynamicgraph::Vector) | |
DECLARE_SIGNAL_OUT (v_gyr, dynamicgraph::Vector) | |
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) | |
DECLARE_SIGNAL_OUT (q_lf, dynamicgraph::Vector) | |
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 |
double | m_alpha_DC_vel |
double | m_alpha_w_filter |
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 |
double | m_fz_std_dev_lf |
double | m_fz_std_dev_rf |
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 |
int | m_lf_fz_stable_cpt |
pinocchio::Model | m_model |
normal | m_normal |
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 |
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 |
pinocchio::FrameIndex | m_right_foot_id |
foot sole to F/T sensor transformation More... | |
bool | m_right_foot_is_stable |
Vector4 | m_right_foot_sizes |
RobotUtilShrPtr | m_robot_util |
sampling time step More... | |
SE3 | m_sole_M_ftSens |
Vector3 | m_v_ac |
Eigen::VectorXd | m_v_flex |
Eigen::VectorXd | m_v_gyr |
Eigen::VectorXd | m_v_imu |
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 |
double | m_w_lf_filtered |
double | m_w_rf_filtered |
double | m_zmp_margin_lf |
double | m_zmp_margin_rf |
margin used for computing zmp weight More... | |
double | m_zmp_std_dev_lf |
double | m_zmp_std_dev_rf |
weight of IMU for sensor fusion More... | |
Definition at line 79 of file base-estimator.hh.
BaseEstimator | ( | const std::string & | name | ) |
Definition at line 108 of file base-estimator.cpp.
double compute_force_weight | ( | double | fz, |
double | std_dev, | ||
double | margin | ||
) |
Definition at line 497 of file base-estimator.cpp.
void compute_zmp | ( | const Vector6 & | w, |
Vector2 & | zmp | ||
) |
Definition at line 468 of file base-estimator.cpp.
double compute_zmp_weight | ( | const Vector2 & | zmp, |
const Vector4 & | foot_sizes, | ||
double | std_dev, | ||
double | margin | ||
) |
Definition at line 476 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 1254 of file base-estimator.cpp.
void init | ( | const double & | dt, |
const std::string & | urdfFile | ||
) |
Definition at line 291 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 566 of file base-estimator.cpp.
void reset_foot_positions | ( | ) |
Definition at line 384 of file base-estimator.cpp.
void reset_foot_positions_impl | ( | const Vector6 & | ftlf, |
const Vector6 & | ftrf | ||
) |
Definition at line 503 of file base-estimator.cpp.
|
inline |
Definition at line 206 of file base-estimator.hh.
void set_alpha_DC_acc | ( | const double & | a | ) |
Definition at line 372 of file base-estimator.cpp.
void set_alpha_DC_vel | ( | const double & | a | ) |
Definition at line 378 of file base-estimator.cpp.
void set_alpha_w_filter | ( | const double & | a | ) |
Definition at line 366 of file base-estimator.cpp.
void set_fz_stable_windows_size | ( | const int & | ws | ) |
Definition at line 359 of file base-estimator.cpp.
void set_imu_weight | ( | const double & | w | ) |
Definition at line 386 of file base-estimator.cpp.
void set_left_foot_sizes | ( | const dynamicgraph::Vector & | s | ) |
Definition at line 444 of file base-estimator.cpp.
void set_normal_force_margin_left_foot | ( | const double & | margin | ) |
Definition at line 464 of file base-estimator.cpp.
void set_normal_force_margin_right_foot | ( | const double & | margin | ) |
Definition at line 460 of file base-estimator.cpp.
void set_normal_force_std_dev_left_foot | ( | const double & | std_dev | ) |
Definition at line 429 of file base-estimator.cpp.
void set_normal_force_std_dev_right_foot | ( | const double & | std_dev | ) |
Definition at line 422 of file base-estimator.cpp.
void set_right_foot_sizes | ( | const dynamicgraph::Vector & | s | ) |
Definition at line 436 of file base-estimator.cpp.
void set_stiffness_left_foot | ( | const dynamicgraph::Vector & | k | ) |
Definition at line 400 of file base-estimator.cpp.
void set_stiffness_right_foot | ( | const dynamicgraph::Vector & | k | ) |
Definition at line 392 of file base-estimator.cpp.
void set_zmp_margin_left_foot | ( | const double & | margin | ) |
Definition at line 456 of file base-estimator.cpp.
void set_zmp_margin_right_foot | ( | const double & | margin | ) |
Definition at line 452 of file base-estimator.cpp.
void set_zmp_std_dev_left_foot | ( | const double & | std_dev | ) |
Definition at line 415 of file base-estimator.cpp.
void set_zmp_std_dev_right_foot | ( | const double & | std_dev | ) |
Definition at line 408 of file base-estimator.cpp.
|
protected |
velocity of the base in the world with DC component removed
Definition at line 262 of file base-estimator.hh.
|
protected |
acceleration of the base in the world with DC component removed
Definition at line 265 of file base-estimator.hh.
|
protected |
alpha parameter for DC blocker filter on acceleration data
Definition at line 267 of file base-estimator.hh.
|
protected |
alpha parameter for DC blocker filter on velocity data
Definition at line 270 of file base-estimator.hh.
|
protected |
Pinocchio robot model.
Definition at line 279 of file base-estimator.hh.
|
protected |
true after the command resetFootPositions is called
Definition at line 216 of file base-estimator.hh.
|
protected |
margin used for computing zmp weight
Definition at line 246 of file base-estimator.hh.
|
protected |
margin used for computing normal force weight
Definition at line 247 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 223 of file base-estimator.hh.
|
protected |
standard deviation of normal force measurement errors for right foot
Definition at line 238 of file base-estimator.hh.
|
protected |
standard deviation of ZMP measurement errors for left foot
Definition at line 236 of file base-estimator.hh.
|
protected |
Definition at line 301 of file base-estimator.hh.
|
protected |
Definition at line 213 of file base-estimator.hh.
|
protected |
Normal distribution.
Definition at line 294 of file base-estimator.hh.
|
protected |
6d stiffness of right foot spring
Definition at line 249 of file base-estimator.hh.
|
protected |
margin used for computing normal force weight
Definition at line 248 of file base-estimator.hh.
|
protected |
Definition at line 315 of file base-estimator.hh.
|
protected |
Definition at line 314 of file base-estimator.hh.
|
protected |
base orientation in the world
Definition at line 313 of file base-estimator.hh.
|
protected |
Definition at line 300 of file base-estimator.hh.
|
protected |
Definition at line 219 of file base-estimator.hh.
|
protected |
standard deviation of normal force measurement errors for left foot
Definition at line 240 of file base-estimator.hh.
|
protected |
size of the windows used to detect that feet did not leave the ground
Definition at line 225 of file base-estimator.hh.
|
protected |
filtered weight of the estimation coming from the right foot
Definition at line 278 of file base-estimator.hh.
|
protected |
Default reference for right foot pose to use if no ref is pluged
Definition at line 292 of file base-estimator.hh.
|
protected |
Pinocchio robot data.
Definition at line 281 of file base-estimator.hh.
|
protected |
world-to-base transformation obtained through left foot
Definition at line 283 of file base-estimator.hh.
|
protected |
world-to-base transformation obtained through right foot
Definition at line 284 of file base-estimator.hh.
|
protected |
Definition at line 288 of file base-estimator.hh.
|
protected |
transformation from world to right foot sole
Definition at line 286 of file base-estimator.hh.
|
protected |
transformation from world to left foot sole
Definition at line 285 of file base-estimator.hh.
|
protected |
Default reference for left foot pose to use if no ref is pluged
Definition at line 290 of file base-estimator.hh.
|
protected |
Definition at line 287 of file base-estimator.hh.
|
protected |
robot velocities according to SoT convention
Definition at line 309 of file base-estimator.hh.
|
protected |
chest orientation in the world from angular fusion
Definition at line 310 of file base-estimator.hh.
|
protected |
Definition at line 304 of file base-estimator.hh.
|
protected |
robot configuration according to pinocchio convention
Definition at line 305 of file base-estimator.hh.
|
protected |
true if the entity has been successfully initialized
Definition at line 215 of file base-estimator.hh.
|
protected |
counter for detecting for how long the feet has been stable
Definition at line 227 of file base-estimator.hh.
|
protected |
foot sole to F/T sensor transformation
Definition at line 299 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 221 of file base-estimator.hh.
|
protected |
sizes of the left foot (pos x, neg x, pos y, neg y)
Definition at line 242 of file base-estimator.hh.
|
protected |
sampling time step
Definition at line 217 of file base-estimator.hh.
|
protected |
flag to detect first iteration and initialise velocity filters
Definition at line 297 of file base-estimator.hh.
|
protected |
6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities)
Definition at line 261 of file base-estimator.hh.
|
protected |
6d robot velocities from kinematic only (encoders derivative)
Definition at line 253 of file base-estimator.hh.
|
protected |
6d robot velocities form imu only (accelerometer integration + gyro)
Definition at line 257 of file base-estimator.hh.
|
protected |
6d robot velocities from flexibility only (force sensor derivative)
Definition at line 255 of file base-estimator.hh.
|
protected |
6d stiffness of left foot spring
Definition at line 251 of file base-estimator.hh.
|
protected |
robot configuration according to SoT convention
Definition at line 307 of file base-estimator.hh.
|
protected |
robot velocities according to pinocchio convention
Definition at line 308 of file base-estimator.hh.
|
protected |
counter for detecting for how long the feet has been stable
Definition at line 231 of file base-estimator.hh.
|
protected |
filter parameter to filter weights (1st order low pass filter)
Definition at line 273 of file base-estimator.hh.
|
protected |
filtered weight of the estimation coming from the left foot
Definition at line 275 of file base-estimator.hh.
|
protected |
sizes of the left foot (pos x, neg x, pos y, neg y)
Definition at line 244 of file base-estimator.hh.
|
protected |
margin used for computing zmp weight
Definition at line 245 of file base-estimator.hh.
|
protected |
standard deviation of ZMP measurement errors for right foot
Definition at line 234 of file base-estimator.hh.
|
protected |
weight of IMU for sensor fusion
Definition at line 232 of file base-estimator.hh.