BipedalLocomotion::Estimators::LeggedOdometry class
#include <BipedalLocomotion/FloatingBaseEstimators/LeggedOdometry.h>

Floating base estimation algorithm using only kinematic measurements by assuming atleast one frame on the robot is in contact with the environment.

Configuration parameters,

GroupParameterTypeRequiredDescription
LeggedOdom-groupYesConfiguration group for initializing the legged odometry block
initial_fixed_framestringYesFrame on the URDF model of the robot assumed to be fixed (in contact) during the initialization phase
initial_ref_frame_for_worldstringNoFrame on the URDF model of the robot to be used as a reference to initialize the inertial frame for the estimation. If not present, the initial fixed frame is assumed as the reference
initial_world_orientation_in_ref_framevector of 4 doublesNoOrientation of the inertial frame wrt the the reference frame as quaternion wxyz. If not present, assumed to be unit quaternion.
initial_world_position_in_ref_framevector of 3 doublesNoPosition of the inertial frame wrt the reference frame as xyz. If not present,assumed to be zero position
switching_patternstringNoOptions: latest, lastActive, useExternal. Switching pattern to decide the fixed frame from the set of all frames in contact. latest chooses the recently switched contact frame, lastActive chooses the earliest switched contact frame. Default is latest
vel_computation_methodstringNoOptions: single, multiAvg, multiLS, multLSJvel. Method used for computing the floating base velocity using the fixed frame constraints. single uses only the fixed frame, multiAvg computes a simple average from all the contact frames, multLS uses a least square solution and multLSJVel regualrizes the least square solution by augmenting joint velocities in the computation. Default is multiLS.
wLindoubleNoweight used for linear velocity in the least square computation of base velocity. If not present, set to 1.0
wAngdoubleNoweight used for angular velocity in the least square computation of base velocity. If not present, set to 0.5
wJVeldoubleNoweight used for linear velocity in the least square (with joint velocities) computation of base velocity. If not present, set to 10.0
regdoubleNoregularization used for matrix inversion operation in least square. If not present, set to 1e-6

Base classes

class FloatingBaseEstimator
FloatingBaseEstimator class contains the bare-bones implementation for a LeggedOdometry based or Extended Kalman Filter based floating base estimation algorithms for bipedal robots.

Constructors, destructors, conversion operators

LeggedOdometry()
Constructor.
~LeggedOdometry()
Destructor (necessary for PIMPL idiom)

Public functions

auto resetEstimator() -> bool
Reset the internal state of the estimator using the initialized parameters.
auto resetEstimator(const std::string& refFramForWorld, const Eigen::Quaterniond& worldOrientationInRefFrame, const Eigen::Vector3d& worldPositionInRefFrame) -> bool
Reset the internal state of the estimator by setting a new reference for the inertial frame.
auto changeFixedFrame(const std::ptrdiff_t& frameIndex) -> bool
Change fixed frame externally.
auto changeFixedFrame(const std::string& frameName) -> bool
Change fixed frame externally.
auto changeFixedFrame(const std::ptrdiff_t& frameIndex, const Eigen::Quaterniond& frameOrientationInWorld, const Eigen::Vector3d& framePositionInWorld) -> bool
Change fixed frame externally by mentioning the world_H_frame transform.
auto getFixedFrameIdx() -> int
Get the index of the frame currently in contact used for the legged odometry computations.
auto getFixedFramePose() const -> manif::SE3d&
Get the psoe of the current fixed frame in the inertial frame.
auto resetEstimator(const FloatingBaseEstimators::InternalState& newState) -> bool
To prevent function hiding due to overloading of virtual methods.
auto resetEstimator(const Eigen::Quaterniond& newBaseOrientation, const Eigen::Vector3d& newBasePosition) -> bool
To prevent function hiding due to overloading of virtual methods.

Protected functions

auto customInitialization(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) -> bool override
These custom parameter specifications should be specified by the derived class.
auto updateKinematics(FloatingBaseEstimators::Measurements& meas, const double& dt) -> bool override
Update the base state estimate using kinematics and contact measurements.

Function documentation

bool BipedalLocomotion::Estimators::LeggedOdometry::resetEstimator()

Reset the internal state of the estimator using the initialized parameters.

Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::LeggedOdometry::resetEstimator(const std::string& refFramForWorld, const Eigen::Quaterniond& worldOrientationInRefFrame, const Eigen::Vector3d& worldPositionInRefFrame)

Reset the internal state of the estimator by setting a new reference for the inertial frame.

Parameters
refFramForWorld in frame from the model as a reference to set the new inertial frame
worldOrientationInRefFrame in orientation of the inertial frame wrt to the reference frame
worldPositionInRefFrame in position of the inertial frame wrt to the reference frame
Returns True in case of success, false otherwise.

bool BipedalLocomotion::Estimators::LeggedOdometry::changeFixedFrame(const std::ptrdiff_t& frameIndex)

Change fixed frame externally.

Parameters
frameIndex in valid frame index from the model

bool BipedalLocomotion::Estimators::LeggedOdometry::changeFixedFrame(const std::ptrdiff_t& frameIndex, const Eigen::Quaterniond& frameOrientationInWorld, const Eigen::Vector3d& framePositionInWorld)

Change fixed frame externally by mentioning the world_H_frame transform.

Parameters
frameIndex in valid frame from the model
frameOrientationInWorld in orientation of the fixed frame wrt the inertial frame
framePositionInWorld in position of the fixed frame wrt the inertial frame

int BipedalLocomotion::Estimators::LeggedOdometry::getFixedFrameIdx()

Get the index of the frame currently in contact used for the legged odometry computations.

Returns Index of the frame in contact, invalid frame index if no contact.

manif::SE3d& BipedalLocomotion::Estimators::LeggedOdometry::getFixedFramePose() const

Get the psoe of the current fixed frame in the inertial frame.

Returns const ref of pose of the fixed frame

bool BipedalLocomotion::Estimators::LeggedOdometry::customInitialization(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) override protected

These custom parameter specifications should be specified by the derived class.

Parameters
handler in configure the custom parameters for the estimator
Returns bool

bool BipedalLocomotion::Estimators::LeggedOdometry::updateKinematics(FloatingBaseEstimators::Measurements& meas, const double& dt) override protected

Update the base state estimate using kinematics and contact measurements.

Parameters
meas in measurements to update states
dt in sampling period in seconds
Returns True in case of success, false otherwise.