AnymalStateEstimator
The state estimator for anymal.
|
Generic interface to be used for filters performing state estimation inside AnymalStateEstimator. More...
#include <AnymalFilter.hpp>
Public Types | |
using | RobotModel = romo::RobotModel< ConcreteDescription_, RobotState_ > |
using | RD = typename RobotModel::RD |
using | CT = typename ConcreteDescription_::ConcreteTopology |
using | JointEnum = typename CT::JointEnum |
using | ContactEnum = typename CT::ContactEnum |
using | StateStatus = anymal_description::StateStatus |
using | PoseCovarianceMatrix = Eigen::Matrix< double, RD::getNumSpatialDof(), RD::getNumSpatialDof()> |
using | JointState = std_utils::EnumArray< JointEnum, any_measurements::ExtendedJointState > |
template<typename ValueType_ > | |
using | ContactEnumContainer = std_utils::EnumArray< ContactEnum, ValueType_ > |
Public Member Functions | |
AnymalFilter ()=delete | |
AnymalFilter (any_node::Node::NodeHandlePtr nh) | |
virtual | ~AnymalFilter ()=default |
virtual bool | initFilter (const kindr::HomTransformQuatD &poseBaseToOdom)=0 |
virtual bool | resetFilter (const kindr::HomTransformQuatD &poseBaseToOdom)=0 |
virtual kindr::RotationQuaternionPD | getOrientationBaseToWorld () const =0 |
virtual kindr::Velocity3D | getLinearVelocityBaseInWorldFrame () const =0 |
virtual kindr::LocalAngularVelocityPD | getAngularVelocityBaseInBaseFrame () const =0 |
virtual kindr::Position3D | getPositionWorldToBaseInWorldFrame () const =0 |
virtual Eigen::Matrix< double, 3, 1 > | getImuLinearAccelerationBias () const =0 |
virtual Eigen::Matrix< double, 3, 1 > | getImuAngularVelocityBias () const =0 |
virtual PoseCovarianceMatrix | getEstPoseInOdomCovariance () const =0 |
virtual PoseCovarianceMatrix | getEstTwistCovariance () const =0 |
virtual any_measurements::Time | getLastImuStamp () const =0 |
virtual StateStatus | getStatus () const =0 |
virtual void | processImuReadings (const any_measurements::ImuWithCovariance &imu)=0 |
virtual void | processKinematics (const JointState &joints, const ContactEnumContainer< Contact > &contacts, const bool fakeKinematicsUpdateActive)=0 |
virtual void | updateFilter ()=0 |
virtual void | addVariablesToLog () |
virtual void | setModelPtr (std::shared_ptr< RobotModel > modelPtr) |
virtual void | updateFullContactFlag (const bool hasFullContact) |
any_measurements::Time | getLastEstimatedStateStamp () const |
Protected Attributes | |
any_node::Node::NodeHandlePtr | nh_ |
Eigen::Vector3d | previousAngularVelocityMeasurement_ = Eigen::Vector3d::Zero() |
any_measurements::Time | lastEstimatedStateStamp_ |
bool | initialFullContactSet_ {false} |
Generic interface to be used for filters performing state estimation inside AnymalStateEstimator.
ConcreteDescription_ | romo::ConcreteDescription |
RobotState_ | romo::RobotState |
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::ContactEnum = typename CT::ContactEnum |
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::ContactEnumContainer = std_utils::EnumArray<ContactEnum, ValueType_> |
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::CT = typename ConcreteDescription_::ConcreteTopology |
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::JointEnum = typename CT::JointEnum |
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::JointState = std_utils::EnumArray<JointEnum, any_measurements::ExtendedJointState> |
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::PoseCovarianceMatrix = Eigen::Matrix<double, RD::getNumSpatialDof(), RD::getNumSpatialDof()> |
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::RD = typename RobotModel::RD |
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::RobotModel = romo::RobotModel<ConcreteDescription_, RobotState_> |
using anymal_state_estimator::AnymalFilter< ConcreteDescription_, RobotState_ >::StateStatus = anymal_description::StateStatus |
|
delete |
|
inlineexplicit |
|
virtualdefault |
|
inlinevirtual |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
inline |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
pure virtual |
|
inlinevirtual |
|
pure virtual |
|
inlinevirtual |
|
protected |
|
protected |
|
protected |
|
protected |