#include <sot/talos_balance/talos-base-estimator.hh>
Public Member Functions | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | TalosBaseEstimator (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) | |
6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities) More... | |
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 pinocchio::FrameIndex foot_id, SE3 &oMff, SE3 &oMfa, SE3 &fsMff) |
void | reset_foot_positions () |
void | reset_foot_positions_impl (const Vector6 &ftlf, const Vector6 &ftrf) |
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_frame_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 |
chest to imu transformation 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_oRff |
chest orientation in the world from angular fusion More... | |
Matrix3 | m_oRtorso |
robot velocities according to SoT convention 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... | |
pinocchio::FrameIndex | m_torso_id |
pinocchio::SE3 | m_torsoMimu |
Pinocchio robot data. 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 86 of file talos-base-estimator.hh.
TalosBaseEstimator | ( | const std::string & | name | ) |
Definition at line 188 of file talos-base-estimator.cpp.
References TalosBaseEstimator::init(), INPUT_SIGNALS, TalosBaseEstimator::m_left_foot_sizes, TalosBaseEstimator::m_right_foot_sizes, TalosBaseEstimator::m_w_imu, OUTPUT_SIGNALS, TalosBaseEstimator::reset_foot_positions(), TalosBaseEstimator::set_alpha_DC_acc(), TalosBaseEstimator::set_alpha_DC_vel(), TalosBaseEstimator::set_alpha_w_filter(), TalosBaseEstimator::set_fz_stable_windows_size(), TalosBaseEstimator::set_imu_weight(), TalosBaseEstimator::set_left_foot_sizes(), TalosBaseEstimator::set_normal_force_margin_left_foot(), TalosBaseEstimator::set_normal_force_margin_right_foot(), TalosBaseEstimator::set_normal_force_std_dev_left_foot(), TalosBaseEstimator::set_normal_force_std_dev_right_foot(), TalosBaseEstimator::set_right_foot_sizes(), TalosBaseEstimator::set_stiffness_left_foot(), TalosBaseEstimator::set_stiffness_right_foot(), TalosBaseEstimator::set_zmp_margin_left_foot(), TalosBaseEstimator::set_zmp_margin_right_foot(), TalosBaseEstimator::set_zmp_std_dev_left_foot(), and TalosBaseEstimator::set_zmp_std_dev_right_foot().
double compute_force_weight | ( | double | fz, |
double | std_dev, | ||
double | margin | ||
) |
Definition at line 555 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_normal.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
void compute_zmp | ( | const Vector6 & | w, |
Vector2 & | zmp | ||
) |
Definition at line 527 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_sole_M_ftSens.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
double compute_zmp_weight | ( | const Vector2 & | zmp, |
const Vector4 & | foot_sizes, | ||
double | std_dev, | ||
double | margin | ||
) |
Definition at line 537 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_normal.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
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 1356 of file talos-base-estimator.cpp.
void init | ( | const double & | dt, |
const std::string & | urdfFile | ||
) |
Definition at line 342 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_alpha_DC_acc, TalosBaseEstimator::m_alpha_DC_vel, TalosBaseEstimator::m_alpha_w_filter, TalosBaseEstimator::m_data, TalosBaseEstimator::m_dt, TalosBaseEstimator::m_fz_stable_windows_size, TalosBaseEstimator::m_IMU_frame_id, TalosBaseEstimator::m_initSucceeded, TalosBaseEstimator::m_isFirstIterFlag, TalosBaseEstimator::m_last_DCacc, TalosBaseEstimator::m_last_DCvel, TalosBaseEstimator::m_last_vel, TalosBaseEstimator::m_left_foot_id, TalosBaseEstimator::m_left_foot_is_stable, TalosBaseEstimator::m_lf_fz_stable_cpt, TalosBaseEstimator::m_model, TalosBaseEstimator::m_q_pin, TalosBaseEstimator::m_q_sot, TalosBaseEstimator::m_rf_fz_stable_cpt, TalosBaseEstimator::m_right_foot_id, TalosBaseEstimator::m_right_foot_is_stable, TalosBaseEstimator::m_robot_util, TalosBaseEstimator::m_sole_M_ftSens, TalosBaseEstimator::m_torso_id, TalosBaseEstimator::m_torsoMimu, TalosBaseEstimator::m_v_flex, TalosBaseEstimator::m_v_gyr, TalosBaseEstimator::m_v_imu, TalosBaseEstimator::m_v_kin, TalosBaseEstimator::m_v_pin, and TalosBaseEstimator::m_v_sot.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void kinematics_estimation | ( | const Vector6 & | ft, |
const Vector6 & | K, | ||
const SE3 & | oMfs, | ||
const pinocchio::FrameIndex | foot_id, | ||
SE3 & | oMff, | ||
SE3 & | oMfa, | ||
SE3 & | fsMff | ||
) |
Definition at line 616 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_data, and dynamicgraph::sot::talos_balance::rpyToMatrix().
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and TalosBaseEstimator::reset_foot_positions_impl().
void reset_foot_positions | ( | ) |
Definition at line 439 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_reset_foot_pos.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void reset_foot_positions_impl | ( | const Vector6 & | ftlf, |
const Vector6 & | ftrf | ||
) |
Definition at line 562 of file talos-base-estimator.cpp.
References TalosBaseEstimator::kinematics_estimation(), TalosBaseEstimator::m_K_lf, TalosBaseEstimator::m_K_rf, TalosBaseEstimator::m_left_foot_id, TalosBaseEstimator::m_oMlfs, TalosBaseEstimator::m_oMlfs_default_ref, TalosBaseEstimator::m_oMlfs_xyzquat, TalosBaseEstimator::m_oMrfs, TalosBaseEstimator::m_oMrfs_default_ref, TalosBaseEstimator::m_oMrfs_xyzquat, TalosBaseEstimator::m_reset_foot_pos, TalosBaseEstimator::m_right_foot_id, and TalosBaseEstimator::m_robot_util.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION().
void set_alpha_DC_acc | ( | const double & | a | ) |
Definition at line 424 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_alpha_DC_acc.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_alpha_DC_vel | ( | const double & | a | ) |
Definition at line 431 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_alpha_DC_vel.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_alpha_w_filter | ( | const double & | a | ) |
Definition at line 416 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_alpha_w_filter.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_fz_stable_windows_size | ( | const int & | ws | ) |
Definition at line 409 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_fz_stable_windows_size.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_imu_weight | ( | const double & | w | ) |
Definition at line 444 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_w_imu.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_left_foot_sizes | ( | const dynamicgraph::Vector & | s | ) |
Definition at line 500 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_left_foot_sizes.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_normal_force_margin_left_foot | ( | const double & | margin | ) |
Definition at line 522 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_fz_margin_lf.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_normal_force_margin_right_foot | ( | const double & | margin | ) |
Definition at line 517 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_fz_margin_rf.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_normal_force_std_dev_left_foot | ( | const double & | std_dev | ) |
Definition at line 486 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_fz_std_dev_lf.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_normal_force_std_dev_right_foot | ( | const double & | std_dev | ) |
Definition at line 479 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_fz_std_dev_rf.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_right_foot_sizes | ( | const dynamicgraph::Vector & | s | ) |
Definition at line 493 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_right_foot_sizes.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_stiffness_left_foot | ( | const dynamicgraph::Vector & | k | ) |
Definition at line 458 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_K_lf.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_stiffness_right_foot | ( | const dynamicgraph::Vector & | k | ) |
Definition at line 451 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_K_rf.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_zmp_margin_left_foot | ( | const double & | margin | ) |
Definition at line 512 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_zmp_margin_lf.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_zmp_margin_right_foot | ( | const double & | margin | ) |
Definition at line 507 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_zmp_margin_rf.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_zmp_std_dev_left_foot | ( | const double & | std_dev | ) |
Definition at line 472 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_zmp_std_dev_lf.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
void set_zmp_std_dev_right_foot | ( | const double & | std_dev | ) |
Definition at line 465 of file talos-base-estimator.cpp.
References TalosBaseEstimator::m_zmp_std_dev_rf.
Referenced by TalosBaseEstimator::TalosBaseEstimator().
|
protected |
velocity of the base in the world with DC component removed
Definition at line 210 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
|
protected |
acceleration of the base in the world with DC component removed
Definition at line 212 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), TalosBaseEstimator::init(), and TalosBaseEstimator::set_alpha_DC_acc().
|
protected |
alpha parameter for DC blocker filter on acceleration data
Definition at line 213 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), TalosBaseEstimator::init(), and TalosBaseEstimator::set_alpha_DC_vel().
|
protected |
alpha parameter for DC blocker filter on velocity data
Definition at line 215 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), TalosBaseEstimator::init(), and TalosBaseEstimator::set_alpha_w_filter().
|
protected |
Pinocchio robot model.
Definition at line 221 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), TalosBaseEstimator::init(), and TalosBaseEstimator::kinematics_estimation().
|
protected |
true after the command resetFootPositions is called
Definition at line 181 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
margin used for computing zmp weight
Definition at line 199 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::set_normal_force_margin_left_foot().
|
protected |
margin used for computing normal force weight
Definition at line 200 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::set_normal_force_margin_right_foot().
|
protected |
True if right foot as been stable for the last 'm_fz_stable_windows_size' samples.
Definition at line 186 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), TalosBaseEstimator::init(), and TalosBaseEstimator::set_fz_stable_windows_size().
|
protected |
standard deviation of normal force measurement errors for right foot
Definition at line 194 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::set_normal_force_std_dev_left_foot().
|
protected |
standard deviation of ZMP measurement errors for left foot
Definition at line 193 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::set_normal_force_std_dev_right_foot().
|
protected |
Definition at line 240 of file talos-base-estimator.hh.
Referenced by TalosBaseEstimator::init().
|
protected |
Definition at line 179 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
Normal distribution.
Definition at line 233 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
6d stiffness of right foot spring
Definition at line 202 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), TalosBaseEstimator::reset_foot_positions_impl(), and TalosBaseEstimator::set_stiffness_left_foot().
|
protected |
margin used for computing normal force weight
Definition at line 201 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), TalosBaseEstimator::reset_foot_positions_impl(), and TalosBaseEstimator::set_stiffness_right_foot().
|
protected |
Definition at line 251 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
Definition at line 250 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
base orientation in the world
Definition at line 249 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
|
protected |
Definition at line 184 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
standard deviation of normal force measurement errors for left foot
Definition at line 195 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), TalosBaseEstimator::set_left_foot_sizes(), and TalosBaseEstimator::TalosBaseEstimator().
|
protected |
size of the windows used to detect that feet did not leave the ground
Definition at line 187 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
filtered weight of the estimation coming from the right foot
Definition at line 220 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
Default reference for right foot pose to use if no ref is pluged.
Definition at line 231 of file talos-base-estimator.hh.
Referenced by TalosBaseEstimator::compute_force_weight(), and TalosBaseEstimator::compute_zmp_weight().
|
protected |
chest to imu transformation
Definition at line 223 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
|
protected |
world-to-base transformation obtained through left foot
Definition at line 224 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
|
protected |
world-to-base transformation obtained through right foot
Definition at line 225 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and TalosBaseEstimator::reset_foot_positions_impl().
|
protected |
Definition at line 229 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and TalosBaseEstimator::reset_foot_positions_impl().
|
protected |
transformation from world to right foot sole
Definition at line 227 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::reset_foot_positions_impl().
|
protected |
transformation from world to left foot sole
Definition at line 226 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and TalosBaseEstimator::reset_foot_positions_impl().
|
protected |
Default reference for left foot pose to use if no ref is pluged.
Definition at line 230 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and TalosBaseEstimator::reset_foot_positions_impl().
|
protected |
Definition at line 228 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::reset_foot_positions_impl().
|
protected |
chest orientation in the world from angular fusion
Definition at line 246 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
|
protected |
robot velocities according to SoT convention
Definition at line 245 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
|
protected |
Definition at line 241 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
robot configuration according to pinocchio convention
Definition at line 242 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
true if the entity has been successfully initialized
Definition at line 180 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), TalosBaseEstimator::reset_foot_positions(), and TalosBaseEstimator::reset_foot_positions_impl().
|
protected |
counter for detecting for how long the feet has been stable
Definition at line 188 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
foot sole to F/T sensor transformation
Definition at line 237 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), TalosBaseEstimator::init(), and TalosBaseEstimator::reset_foot_positions_impl().
|
protected |
True if left foot as been stable for the last 'm_fz_stable_windows_size' samples.
Definition at line 185 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
sizes of the left foot (pos x, neg x, pos y, neg y)
Definition at line 196 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), TalosBaseEstimator::set_right_foot_sizes(), and TalosBaseEstimator::TalosBaseEstimator().
|
protected |
sampling time step
Definition at line 182 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), TalosBaseEstimator::init(), and TalosBaseEstimator::reset_foot_positions_impl().
|
protected |
flag to detect first iteration and initialise velocity filters
Definition at line 235 of file talos-base-estimator.hh.
Referenced by TalosBaseEstimator::compute_zmp(), and TalosBaseEstimator::init().
|
protected |
Definition at line 239 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
Pinocchio robot data.
Definition at line 222 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities)
Definition at line 209 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
|
protected |
6d robot velocities from kinematic only (encoders derivative)
Definition at line 205 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
6d robot velocities form imu only (accelerometer integration + gyro)
Definition at line 207 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
6d robot velocities from flexibility only (force sensor derivative)
Definition at line 206 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
6d stiffness of left foot spring
Definition at line 204 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
robot configuration according to SoT convention
Definition at line 243 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
robot velocities according to pinocchio convention
Definition at line 244 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::init().
|
protected |
counter for detecting for how long the feet has been stable
Definition at line 190 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_INNER_FUNCTION(), TalosBaseEstimator::set_imu_weight(), and TalosBaseEstimator::TalosBaseEstimator().
|
protected |
filter parameter to filter weights (1st order low pass filter)
Definition at line 217 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
|
protected |
filtered weight of the estimation coming from the left foot
Definition at line 218 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION().
|
protected |
sizes of the left foot (pos x, neg x, pos y, neg y)
Definition at line 197 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::set_zmp_margin_left_foot().
|
protected |
margin used for computing zmp weight
Definition at line 198 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::set_zmp_margin_right_foot().
|
protected |
standard deviation of ZMP measurement errors for right foot
Definition at line 192 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::set_zmp_std_dev_left_foot().
|
protected |
weight of IMU for sensor fusion
Definition at line 191 of file talos-base-estimator.hh.
Referenced by dynamicgraph::sot::talos_balance::DEFINE_SIGNAL_OUT_FUNCTION(), and TalosBaseEstimator::set_zmp_std_dev_right_foot().