AnymalStateEstimator
The state estimator for anymal.
Public Types | Public Member Functions | Protected Attributes | List of all members
tsif::ImuKinAnymalOdometryTsif Class Reference

#include <ImuKinAnymalOdometryTsif.hpp>

Inheritance diagram for tsif::ImuKinAnymalOdometryTsif:
Inheritance graph

Public Types

using Base = ImuKinAnymalOdometryTsifBase
 
template<RD::ContactEnum contact, FD::ResidualEnum residual>
using contactToResidualKV = std_utils::KeyValuePair< RD::ContactEnum, FD::ResidualEnum, contact, residual >
 
template<RD::ContactEnum contact, FD::StateEnum contactState>
using contactToContactStateKV = std_utils::KeyValuePair< RD::ContactEnum, FD::StateEnum, contact, contactState >
 
using mapContactToLandmarkPrediction = std_utils::CompileTimeMap< RD::ContactEnum, FD::ResidualEnum, contactToResidualKV< RD::ContactEnum::LF_FOOT, FD::ResidualEnum::LMK_PRD_LF >, contactToResidualKV< RD::ContactEnum::RF_FOOT, FD::ResidualEnum::LMK_PRD_RF >, contactToResidualKV< RD::ContactEnum::LH_FOOT, FD::ResidualEnum::LMK_PRD_LH >, contactToResidualKV< RD::ContactEnum::RH_FOOT, FD::ResidualEnum::LMK_PRD_RH >>
 
using mapContactToContactState = std_utils::CompileTimeMap< RD::ContactEnum, FD::StateEnum, contactToContactStateKV< RD::ContactEnum::LF_FOOT, FD::StateEnum::I_P_LF >, contactToContactStateKV< RD::ContactEnum::RF_FOOT, FD::StateEnum::I_P_RF >, contactToContactStateKV< RD::ContactEnum::LH_FOOT, FD::StateEnum::I_P_LH >, contactToContactStateKV< RD::ContactEnum::RH_FOOT, FD::StateEnum::I_P_RH >>
 
using mapContactToLandmarkUpdate = std_utils::CompileTimeMap< RD::ContactEnum, FD::ResidualEnum, contactToResidualKV< RD::ContactEnum::LF_FOOT, FD::ResidualEnum::LMK_UPD_LF >, contactToResidualKV< RD::ContactEnum::RF_FOOT, FD::ResidualEnum::LMK_UPD_RF >, contactToResidualKV< RD::ContactEnum::LH_FOOT, FD::ResidualEnum::LMK_UPD_LH >, contactToResidualKV< RD::ContactEnum::RH_FOOT, FD::ResidualEnum::LMK_UPD_RH >>
 
using RD = typename RobotModel::RD
 
- Public Types inherited from tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >
using Base = FilterWithDefinition< FilterDefinition_ >
 
using RobotModel = typename romo::RobotModel< ConcreteRobotDescription_, RobotState_ >
 
using RD = typename RobotModel::RD
 
using CT = typename ConcreteRobotDescription_::ConcreteTopology
 
using ContactEnum = typename CT::ContactEnum
 
template<typename T >
using ContactEnumContainer = std_utils::EnumArray< ContactEnum, T >
 

Public Member Functions

 ImuKinAnymalOdometryTsif (const ros::NodeHandle &node_handle)
 
 ~ImuKinAnymalOdometryTsif () override=default
 
void Init (TimePoint t) override
 
TimePoint GetLastImuStamp () const override
 
double GetLastLandmarkStamp () const override
 
void AddMeasurementLinearAccelerationWorldToImuInBase (const TimePoint &time, const Vec3 &B_f_IM) override
 
void AddMeasurementAngularVelocityWorldToBaseInBase (const TimePoint &time, const Vec3 &B_omega_IB) override
 
void AddMeasurementPositionsBaseToFootInBase (const TimePoint &time, const ContactEnumContainer< Vec3 > &B_r_BSi) override
 
void AddMeasurementContacts (const ContactEnumContainer< anymal_state_estimator::Contact > &contacts, bool fakeKinematicsUpdateActive) override
 
tsif::Vec3 GetPositionWorldToBaseInWorld () const override
 
tsif::Quat GetOrientationBaseToWorld () const override
 
tsif::Vec3 GetLinearVelocityWorldToImuInBase () const override
 
tsif::Vec3 GetAngularVelocityWorldToBaseInBase () const override
 
tsif::Vec3 GetLinearAccelerationBiasInBase () const override
 
tsif::Vec3 GetAngularVelocityBiasInBase () const override
 
int GetPositionWorldToBaseInWorldIndex () const override
 
int GetOrientationBaseToWorldIndex () const override
 
int GetLinearVelocityWorldToImuInBaseIndex () const override
 
int GetAngularVelocityWorldToBaseInBaseIndex () const override
 
int GetLinearAccelerationBiasInBaseIndex () const override
 
int GetAngularVelocityBiasInBaseIndex () const override
 
- Public Member Functions inherited from tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >
 AnymalOdometryTsif ()=default
 
 ~AnymalOdometryTsif () override=default
 
void SetInitPose (const kindr::HomTransformQuatD &T_IB_init)
 
void SetImuOffset (const Vec3 &B_r_BM)
 
void SetInitContacts (const ContactEnumContainer< Vec3 > &B_r_BSi_init)
 
kindr::HomTransformQuatD & GetInitPose ()
 
const kindr::HomTransformQuatD & GetInitPose () const
 

Protected Attributes

std::array< double, FD::StateEnum::NUM_STATES > I_init_
 
- Protected Attributes inherited from tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >
kindr::HomTransformQuatD T_IB_init_
 
ContactEnumContainer< Vec3 > B_r_BSi_init_
 
Vec3 B_r_BM_
 

Member Typedef Documentation

using tsif::ImuKinAnymalOdometryTsif::Base = ImuKinAnymalOdometryTsifBase
template<RD::ContactEnum contact, FD::StateEnum contactState>
using tsif::ImuKinAnymalOdometryTsif::contactToContactStateKV = std_utils::KeyValuePair<RD::ContactEnum, FD::StateEnum, contact, contactState>
template<RD::ContactEnum contact, FD::ResidualEnum residual>
using tsif::ImuKinAnymalOdometryTsif::contactToResidualKV = std_utils::KeyValuePair<RD::ContactEnum, FD::ResidualEnum, contact, residual>
using tsif::ImuKinAnymalOdometryTsif::mapContactToContactState = std_utils::CompileTimeMap<RD::ContactEnum, FD::StateEnum, contactToContactStateKV<RD::ContactEnum::LF_FOOT, FD::StateEnum::I_P_LF>, contactToContactStateKV<RD::ContactEnum::RF_FOOT, FD::StateEnum::I_P_RF>, contactToContactStateKV<RD::ContactEnum::LH_FOOT, FD::StateEnum::I_P_LH>, contactToContactStateKV<RD::ContactEnum::RH_FOOT, FD::StateEnum::I_P_RH>>
using tsif::ImuKinAnymalOdometryTsif::mapContactToLandmarkPrediction = std_utils::CompileTimeMap<RD::ContactEnum, FD::ResidualEnum, contactToResidualKV<RD::ContactEnum::LF_FOOT, FD::ResidualEnum::LMK_PRD_LF>, contactToResidualKV<RD::ContactEnum::RF_FOOT, FD::ResidualEnum::LMK_PRD_RF>, contactToResidualKV<RD::ContactEnum::LH_FOOT, FD::ResidualEnum::LMK_PRD_LH>, contactToResidualKV<RD::ContactEnum::RH_FOOT, FD::ResidualEnum::LMK_PRD_RH>>
using tsif::ImuKinAnymalOdometryTsif::mapContactToLandmarkUpdate = std_utils::CompileTimeMap<RD::ContactEnum, FD::ResidualEnum, contactToResidualKV<RD::ContactEnum::LF_FOOT, FD::ResidualEnum::LMK_UPD_LF>, contactToResidualKV<RD::ContactEnum::RF_FOOT, FD::ResidualEnum::LMK_UPD_RF>, contactToResidualKV<RD::ContactEnum::LH_FOOT, FD::ResidualEnum::LMK_UPD_LH>, contactToResidualKV<RD::ContactEnum::RH_FOOT, FD::ResidualEnum::LMK_UPD_RH>>
using tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >::RD = typename RobotModel::RD

Constructor & Destructor Documentation

tsif::ImuKinAnymalOdometryTsif::ImuKinAnymalOdometryTsif ( const ros::NodeHandle &  node_handle)
inlineexplicit
tsif::ImuKinAnymalOdometryTsif::~ImuKinAnymalOdometryTsif ( )
overridedefault

Member Function Documentation

void tsif::ImuKinAnymalOdometryTsif::AddMeasurementAngularVelocityWorldToBaseInBase ( const TimePoint &  time,
const Vec3 &  B_omega_IB 
)
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

void tsif::ImuKinAnymalOdometryTsif::AddMeasurementContacts ( const ContactEnumContainer< anymal_state_estimator::Contact > &  contacts,
bool  fakeKinematicsUpdateActive 
)
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

void tsif::ImuKinAnymalOdometryTsif::AddMeasurementLinearAccelerationWorldToImuInBase ( const TimePoint &  time,
const Vec3 &  B_f_IM 
)
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

void tsif::ImuKinAnymalOdometryTsif::AddMeasurementPositionsBaseToFootInBase ( const TimePoint &  time,
const ContactEnumContainer< Vec3 > &  B_r_BSi 
)
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

tsif::Vec3 tsif::ImuKinAnymalOdometryTsif::GetAngularVelocityBiasInBase ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

int tsif::ImuKinAnymalOdometryTsif::GetAngularVelocityBiasInBaseIndex ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

tsif::Vec3 tsif::ImuKinAnymalOdometryTsif::GetAngularVelocityWorldToBaseInBase ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

int tsif::ImuKinAnymalOdometryTsif::GetAngularVelocityWorldToBaseInBaseIndex ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

TimePoint tsif::ImuKinAnymalOdometryTsif::GetLastImuStamp ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

double tsif::ImuKinAnymalOdometryTsif::GetLastLandmarkStamp ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

tsif::Vec3 tsif::ImuKinAnymalOdometryTsif::GetLinearAccelerationBiasInBase ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

int tsif::ImuKinAnymalOdometryTsif::GetLinearAccelerationBiasInBaseIndex ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

tsif::Vec3 tsif::ImuKinAnymalOdometryTsif::GetLinearVelocityWorldToImuInBase ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

int tsif::ImuKinAnymalOdometryTsif::GetLinearVelocityWorldToImuInBaseIndex ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

tsif::Quat tsif::ImuKinAnymalOdometryTsif::GetOrientationBaseToWorld ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

int tsif::ImuKinAnymalOdometryTsif::GetOrientationBaseToWorldIndex ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

tsif::Vec3 tsif::ImuKinAnymalOdometryTsif::GetPositionWorldToBaseInWorld ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

int tsif::ImuKinAnymalOdometryTsif::GetPositionWorldToBaseInWorldIndex ( ) const
inlineoverridevirtual

Implements tsif::AnymalOdometryTsif< ConcreteRobotDescription_, RobotState_, FilterDefinition_ >.

void tsif::ImuKinAnymalOdometryTsif::Init ( TimePoint  t)
inlineoverride

Member Data Documentation

std::array<double, FD::StateEnum::NUM_STATES> tsif::ImuKinAnymalOdometryTsif::I_init_
protected
The documentation for this class was generated from the following file: