AnymalStateEstimator
The state estimator for anymal.
|
#include <ImuKinAnymalOdometryTsif.hpp>
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 |
![]() | |
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 |
![]() | |
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_ |
![]() | |
kindr::HomTransformQuatD | T_IB_init_ |
ContactEnumContainer< Vec3 > | B_r_BSi_init_ |
Vec3 | B_r_BM_ |
using tsif::ImuKinAnymalOdometryTsif::contactToContactStateKV = std_utils::KeyValuePair<RD::ContactEnum, FD::StateEnum, contact, contactState> |
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 |
|
inlineexplicit |
|
overridedefault |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverridevirtual |
|
inlineoverride |
|
protected |