class
FloatingBaseEstimatorFloatingBaseEstimator class contains the bare-bones implementation for a LeggedOdometry based or Extended Kalman Filter based floating base estimation algorithms for bipedal robots.
Base classes
-
template<class Output>class BipedalLocomotion::System::Source<FloatingBaseEstimators::Output>
- Source is a template specialization of Advanceable and represents a block that does not contains input.
Derived classes
- class InvariantEKFBaseEstimator
- InvariantEKFBaseEstimator class implements the Contact Aided Invariant EKF algorithm, developed by Ross Hartley, Maani Ghaffari, Ryan M.
- class LeggedOdometry
- Floating base estimation algorithm using only kinematic measurements by assuming atleast one frame on the robot is in contact with the environment.
Public types
- class ModelComputations
- iDynTree based model-specific computations class This is class is used in a required configuration step for the estimator All the model related kinematics and dynamics computations specific to the floating base estimator are contained within this class.
Constructors, destructors, conversion operators
- FloatingBaseEstimator()
- ~FloatingBaseEstimator() virtual
Public functions
-
auto initialize(std::weak_ptr<BipedalLocomotion::
ParametersHandler:: IParametersHandler> handler, std::shared_ptr<iDynTree::KinDynComputations> kindyn) -> bool - Configure generic parameters and the model.
-
auto initialize(std::weak_ptr<BipedalLocomotion::
ParametersHandler:: IParametersHandler> handler) -> bool virtual - Configure generic parameters, calling this overloaded method assumes model information is not going to be used.
- auto setIMUMeasurement(const Eigen::Vector3d& accMeas, const Eigen::Vector3d& gyroMeas) -> bool
- Set the polled IMU measurement.
- auto setContacts(const bool& lfInContact, const bool& rfInContact) -> bool
- Set feet contact states.
- auto setContactStatus(const std::string& name, const bool& contactStatus, const std::chrono::nanoseconds& switchTime, const std::chrono::nanoseconds& = std::chrono::nanoseconds::zero()) -> bool
- Set contact status.
- auto setKinematics(const Eigen::VectorXd& encoders, const Eigen::VectorXd& encoderSpeeds) -> bool
- Set kinematic measurements.
- auto setLandmarkRelativePose(const int& landmarkID, const Eigen::Quaterniond& quat, const Eigen::Vector3d& pos, const std::chrono::nanoseconds& timeNow) -> bool
- Set the relative pose of a landmark relative to the base link.
- auto advance() -> bool final
- Compute one step of the estimator.
-
auto resetEstimator(const FloatingBaseEstimators::
InternalState& newState) -> bool virtual - Reset the internal state of the estimator.
- auto resetEstimator(const Eigen::Quaterniond& newBaseOrientation, const Eigen::Vector3d& newBasePosition) -> bool virtual
- Reset the base pose estimate and consequently the internal state of the estimator.
-
auto getOutput() const -> const FloatingBaseEstimators::
Output& final - Get estimator outputs.
- auto isOutputValid() const -> bool final
- Determines the validity of the object retrieved with get()
- auto modelComputations() -> ModelComputations&
- Get ModelComputations object by reference.
Protected types
- enum class State { NotInitialized, Initialized, Running }
- Enumerator used to determine the running state of the estimator.
Protected functions
-
auto customInitialization(std::weak_ptr<BipedalLocomotion::
ParametersHandler:: IParametersHandler> handler) -> bool virtual - These custom parameter specifications should be specified by the derived class.
-
auto predictState(const FloatingBaseEstimators::
Measurements& meas, const double& dt) -> bool virtual - Propagate the states through the prediction model, if there exists any (eg.
-
auto updateKinematics(FloatingBaseEstimators::
Measurements& meas, const double& dt) -> bool virtual - Update the predicted state estimates using kinematics and contact measurements.
-
auto updateLandmarkRelativePoses(FloatingBaseEstimators::
Measurements& meas, const double& dt) -> bool virtual - Update the predicted state estimates using relative pose measurements of static landmarks in the environment.
-
auto updateBaseStateFromIMUState(const FloatingBaseEstimators::
InternalState& state, const FloatingBaseEstimators:: Measurements& meas, manif::SE3d& basePose, Eigen::Ref<Eigen::Matrix<double, 6, 1>> baseTwist) -> bool - Wrapper method for getting base state from internal IMU state.
-
auto setupOptions(std::weak_ptr<BipedalLocomotion::
ParametersHandler:: IParametersHandler> handler) -> bool - Setup estimator options.
-
auto setupSensorDevs(std::weak_ptr<BipedalLocomotion::
ParametersHandler:: IParametersHandler> handler) -> bool - Setup measurement noise standard deviations The parameters in the SensorsStdDev group are,.
-
auto setupInitialStates(std::weak_ptr<BipedalLocomotion::
ParametersHandler:: IParametersHandler> handler) -> bool - Setup initial states.
-
auto setupPriorDevs(std::weak_ptr<BipedalLocomotion::
ParametersHandler:: IParametersHandler> handler) -> bool - Setup initial state standard deviations.
Protected variables
- ModelComputations m_modelComp
- Model computations object.
-
FloatingBaseEstimators::
Options m_options - Struct holding estimator options.
-
FloatingBaseEstimators::
Measurements m_meas -
FloatingBaseEstimators::
Measurements m_measPrev - Struct holding the latest measurements that were set to the estimator.
-
FloatingBaseEstimators::
InternalState m_state -
FloatingBaseEstimators::
InternalState m_statePrev - Structs holding the curent and previous internal states of the estimation algorithm.
-
FloatingBaseEstimators::
Output m_estimatorOut - Struct holding outputs of the estimator.
-
FloatingBaseEstimators::
StateStdDev m_priors -
FloatingBaseEstimators::
StateStdDev m_stateStdDev - Struct holding standard deviations associated to prior state estimates.
-
FloatingBaseEstimators::
SensorsStdDev m_sensorsDev - Struct holding standard deviations associated to sensor measurements.
- State m_estimatorState
- State of the estimator.
- double m_dt
- Fixed time step of the estimator, in seconds.
- bool m_useIMUForAngVelEstimate
- Use IMU measurements as internal state imu angular velocity by default set to true for strap down IMU based EKF implementations, if IMU measurements not used, corresponding impl can set to false.
- bool m_useIMUVelForBaseVelComputation
- Compute base velocity using inertnal state IMU velocity.
- bool m_useModelInfo
- Flag to enable running the estimator without using the URDF model information.
- bool m_isInvEKF
- Flag to maintain soon to be deprecated functionalities currently existing only in InvEKFBaseEstimator.
Enum documentation
enum class BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: State protected
Enumerator used to determine the running state of the estimator.
Enumerators | |
---|---|
NotInitialized |
The estimator is not initialized yet call FloatingBaseEstimator::initialze method to initialize it. |
Initialized |
The estimator is initialized and ready to be used. |
Running |
The estimator is running. |
Function documentation
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: initialize(std::weak_ptr<BipedalLocomotion:: ParametersHandler:: IParametersHandler> handler,
std::shared_ptr<iDynTree::KinDynComputations> kindyn)
Configure generic parameters and the model.
Parameters | |
---|---|
handler in | configure the generic parameters for the estimator |
kindyn in | shared pointer of iDynTree kindyncomputations object (model will be loaded internally) |
Returns | True in case of success, false otherwise. |
The generic parameters include,
- sampling_period_in_s [PARAMETER|REQUIRED|Sampling period of the estimator in seconds]
- ModelInfo [GROUP|REQUIRED|URDF Model specific parameters used by the estimator]
- base_link [PARAMETER|REQUIRED|base link frame from the URDF model| Exists in "ModelInfo" GROUP]
- base_link_imu [PARAMETER|REQUIRED|IMU frame rigidly attached to thebase link from the URDF model| Exists in "ModelInfo" GROUP]
- left_foot_contact_frame [PARAMETER|REQUIRED|left foot contact frame from the URDF model| Exists in "ModelInfo" GROUP]
right_foot_contact_frame [PARAMETER|REQUIRED|right foot contact frame from the URDF model| Exists in "ModelInfo" GROUP]
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: initialize(std::weak_ptr<BipedalLocomotion:: ParametersHandler:: IParametersHandler> handler) virtual
Configure generic parameters, calling this overloaded method assumes model information is not going to be used.
Parameters | |
---|---|
handler in | configure the generic parameters for the estimator |
Returns | True in case of success, false otherwise. |
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: setIMUMeasurement(const Eigen::Vector3d& accMeas,
const Eigen::Vector3d& gyroMeas)
Set the polled IMU measurement.
Parameters | |
---|---|
accMeas in | linear accelerometer measurement expressed in the local IMU frame (m/s^2) |
gyroMeas in | gyroscope measurement expressed in the local IMU frame (rad/s) |
Returns | True in case of success, false otherwise. |
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: setContacts(const bool& lfInContact,
const bool& rfInContact)
Set feet contact states.
Parameters | |
---|---|
lfInContact in | left foot contact state [0, 1] |
rfInContact in | right foot contact state [0, 1] |
Returns | True in case of success, false otherwise. |
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: setContactStatus(const std::string& name,
const bool& contactStatus,
const std::chrono::nanoseconds& switchTime,
const std::chrono::nanoseconds& = std::chrono::nanoseconds::zero())
Set contact status.
Parameters | |
---|---|
name in | contact frame name |
contactStatus in | flag to check active contact |
switchTime in | time of switching contact |
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: setKinematics(const Eigen::VectorXd& encoders,
const Eigen::VectorXd& encoderSpeeds)
Set kinematic measurements.
Parameters | |
---|---|
encoders in | joint positions measured through encoders |
encoderSpeeds in | joint velocities measured through encoders |
Returns | True in case of success, false otherwise. |
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: setLandmarkRelativePose(const int& landmarkID,
const Eigen::Quaterniond& quat,
const Eigen::Vector3d& pos,
const std::chrono::nanoseconds& timeNow)
Set the relative pose of a landmark relative to the base link.
Parameters | |
---|---|
landmarkID in | unique landmark ID |
quat in | relative orientation of the landmark as a quaternion |
pos in | relative position of the landmark |
timeNow in | relative position of the landmark |
Returns | true in case of success, false otherwise |
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: advance() final
Compute one step of the estimator.
Returns | True in case of success, false otherwise. |
---|
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: resetEstimator(const FloatingBaseEstimators:: InternalState& newState) virtual
Reset the internal state of the estimator.
Parameters | |
---|---|
newState in | Internal state of the estimator |
Returns | True in case of success, false otherwise. |
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: resetEstimator(const Eigen::Quaterniond& newBaseOrientation,
const Eigen::Vector3d& newBasePosition) virtual
Reset the base pose estimate and consequently the internal state of the estimator.
Parameters | |
---|---|
newBaseOrientation in | base link orientation as a Eigen quaternion |
newBasePosition in | base link position |
Returns | True in case of success, false otherwise. |
const FloatingBaseEstimators:: Output& BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: getOutput() const final
Get estimator outputs.
Returns | A struct containing he estimated internal states of the estiamtor and the associated covariance matrix |
---|
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: isOutputValid() const final
Determines the validity of the object retrieved with get()
Returns | True in case of success, false otherwise. |
---|
ModelComputations& BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: modelComputations()
Get ModelComputations object by reference.
Returns | ModelComputations object providing information between considered model related quantities in the estimator like the base link, IMU, feet contact frames. |
---|
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: customInitialization(std::weak_ptr<BipedalLocomotion:: ParametersHandler:: IParametersHandler> handler) virtual protected
These custom parameter specifications should be specified by the derived class.
Parameters | |
---|---|
handler in | configure the custom parameters for the estimator |
Returns | bool |
- If setupOptions() is called from within customInitialization(), ensure the group "Options" exist. Please check the documentation of setupOptions() for relevant parameters
- If setupSensorDevs() is called from within customInitialization(), ensure the group "SensorsStdDev" exist. Please check the documentation of setupSensorDevs() for relevant parameters
- If setupInitialStates() is called from within customInitialization(), ensure the group "InitialStates" exist. Please check the documentation of setupInitialStates() for relevant parameters
- If setupPriorDevs() is called from within customInitialization(), ensure the group "PriorsStdDev" exist. Please check the documentation of setupPriorDevs() for relevant parameters
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: predictState(const FloatingBaseEstimators:: Measurements& meas,
const double& dt) virtual protected
Propagate the states through the prediction model, if there exists any (eg.
Parameters | |
---|---|
meas in | measurements passed as exogeneous inputs to the prediction model |
dt in | sampling period in seconds |
Returns | True in case of success, false otherwise. |
a strap-down IMU model)
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: updateKinematics(FloatingBaseEstimators:: Measurements& meas,
const double& dt) virtual protected
Update the predicted state estimates using kinematics and contact measurements.
Parameters | |
---|---|
meas in | measurements to update the predicted states |
dt in | sampling period in seconds |
Returns | True in case of success, false otherwise. |
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: updateLandmarkRelativePoses(FloatingBaseEstimators:: Measurements& meas,
const double& dt) virtual protected
Update the predicted state estimates using relative pose measurements of static landmarks in the environment.
Parameters | |
---|---|
meas in | measurements to update the predicted states |
dt in | sampling period in seconds |
Returns | True in case of success, false otherwise. |
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: updateBaseStateFromIMUState(const FloatingBaseEstimators:: InternalState& state,
const FloatingBaseEstimators:: Measurements& meas,
manif::SE3d& basePose,
Eigen::Ref<Eigen::Matrix<double, 6, 1>> baseTwist) protected
Wrapper method for getting base state from internal IMU state.
Parameters | |
---|---|
state in | internal state of the estimator |
meas in | previous measurement to the estimator |
basePose in | base pose as an iDynTree Transform object |
baseTwist in | mixe- trivialized base velocity as an iDynTree Twist object |
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: setupOptions(std::weak_ptr<BipedalLocomotion:: ParametersHandler:: IParametersHandler> handler) protected
Setup estimator options.
Parameters | |
---|---|
handler in | parameter handler |
Returns | True in case of success, false otherwise. |
The parameters in the Options group are,
- enable_imu_bias_estimation [PARAMETER|-|Enable/disable IMU bias estimation]
- enable_static_imu_bias_initialization [PARAMETER|-|Enable/disable IMU bias initialization assuming static configuration]
- nr_samples_for_imu_bias_initialization [PARAMETER|REQUIRED, if staticImuBiasInitializationEnabled is set to true|Number of samples for static bias initialization]
- enable_ekf_update [PARAMETER|-|Enable/disable update step of EKF (not recommended to set to false)]
acceleration_due_to_gravity [PARAMETER|-|Acceleration due to gravity, 3d vector]
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: setupSensorDevs(std::weak_ptr<BipedalLocomotion:: ParametersHandler:: IParametersHandler> handler) protected
Setup measurement noise standard deviations The parameters in the SensorsStdDev group are,.
Parameters | |
---|---|
handler in | parameter handler |
Returns | True in case of success, false otherwise. |
- accelerometer_measurement_noise_std_dev [PARAMETER|REQUIRED|Accelerometer measurement white noise deviation]
- gyroscope_measurement_noise_std_dev [PARAMETER|REQUIRED|Gyroscope measurement white noise deviation]
- accelerometer_measurement_bias_noise_std_dev [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Accelerometer measurement bias noise deviation]
- gyroscope_measurement_bias_noise_std_dev [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Gyroscope measurement bias noise deviation]
- contact_foot_linear_velocity_noise_std_dev [PARAMETER|REQUIRED|Linear velocity white noise deviation when foot is in contact]
- contact_foot_angular_velocity_noise_std_dev [PARAMETER|REQUIRED|Angular velocity white noise deviation when foot is in contact]
- swing_foot_linear_velocity_noise_std_dev [PARAMETER|REQUIRED|Linear velocity white noise deviation when foot is swinging off contact]
- swing_foot_angular_velocity_noise_std_dev [PARAMETER|REQUIRED|Angular velocity white noise deviation when foot is swinging off contact]
- forward_kinematic_measurement_noise_std_dev [PARAMETER|REQUIRED|White noise deviation in relative poses computed through forward kinematics]
encoders_measurement_noise_std_dev [PARAMETER|REQUIRED|Encoder measurement white noise deviation]
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: setupInitialStates(std::weak_ptr<BipedalLocomotion:: ParametersHandler:: IParametersHandler> handler) protected
Setup initial states.
Parameters | |
---|---|
handler in | parameter handler |
Returns | True in case of success, false otherwise. |
The parameters in the InitialStates group are,
- imu_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial IMU orientation wrt inertial frame, as a quaternion]
- imu_position_xyz [PARAMETER|REQUIRED|Initial IMU position wrt inertial frame]
- imu_linear_velocity_xyz [PARAMETER|REQUIRED|Initial IMU linear velocity wrt inertial frame, in a mixed-velocity representation]
- l_contact_frame_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial left foot contact frame orientation wrt inertial frame, as a quaternion]
- l_contact_frame_position_xyz [PARAMETER|REQUIRED|Initial left foot contact frame position wrt inertial frame]
- r_contact_frame_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial right foot contact frame orientation wrt inertial frame, as a quaternion]
- r_contact_frame_position_xyz [PARAMETER|REQUIRED|Initial right foot contact frame position wrt inertial frame]
- accelerometer_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial accelerometer bias expressed in IMU frame]
gyroscope_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial gyroscope bias expressed in IMU frame]
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: setupPriorDevs(std::weak_ptr<BipedalLocomotion:: ParametersHandler:: IParametersHandler> handler) protected
Setup initial state standard deviations.
Parameters | |
---|---|
handler in | parameter handler |
Returns | True in case of success, false otherwise. |
The parameters in the PriorsStdDev group are,
- imu_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial IMU orientation as wrt inertial frame]
- imu_position [PARAMETER|REQUIRED|Initial IMU position deviation wrt inertial frame]
- imu_linear_velocity [PARAMETER|REQUIRED|Initial IMU linear velocity deviation wrt inertial frame, in a mixed-velocity representation]
- l_contact_frame_orientation_quaternion [PARAMETER|REQUIRED|Initial left foot contact frame orientation deviation wrt inertial frame]
- l_contact_frame_position [PARAMETER|REQUIRED|Initial left foot contact frame position deviation wrt inertial frame]
- r_contact_frame_orientation_quaternion [PARAMETER|REQUIRED|Initial right foot contact frame orientation deviation wrt inertial frame]
- r_contact_frame_position [PARAMETER|REQUIRED|Initial right foot contact frame position deviation wrt inertial frame]
- accelerometer_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial accelerometer bias devitaion expressed in IMU frame]
gyroscope_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial gyroscope bias deviation expressed in IMU frame]
Variable documentation
bool BipedalLocomotion:: Estimators:: FloatingBaseEstimator:: m_useIMUVelForBaseVelComputation protected
Compute base velocity using inertnal state IMU velocity.
by default set to true for strap down IMU based EKF implementations, if IMU measurements not used, corresponding impl can set to false