AnymalStateEstimator
The state estimator for anymal.
anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ > Class Template Reference

The StateEstimator implementation for an anymal without any custom attachments. More...

#include <AnymalStateEstimatorBasic.hpp>

Inheritance diagram for anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >:
Inheritance graph

Public Types

using NodeBase = any_node::Node
 
using EstimatorBase = AnymalStateEstimatorBase< Filter_ >
 
using QuadrupedModel = quadruped_model::QuadrupedModel
 
using QuadrupedFramesGenerator = quadruped_model::QuadrupedFramesGenerator< anymal_description::AnymalConcreteDescription, quadruped_model::QuadrupedState >
 
using QuadrupedState = quadruped_model::QuadrupedState
 
using AD = anymal_description::AnymalDescription
 
using ActuatorEnum = typename RD::ActuatorEnum
 
using ContactEnum = typename RD::ContactEnum
 
using BranchEnum = typename RD::BranchEnum
 
- Public Types inherited from anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >
using NodeBase = any_node::Node
 
using Base = any_state_estimator::AnyStateEstimator< RobotContainersRos_ >
 
using RobotModel = romo::RobotModel< ConcreteDescription_, RobotState_ >
 
using ContactForceEstimation = romo::ContactForceEstimation< ConcreteDescription_, RobotState_ >
 
using RD = typename RobotModel::RD
 
using CT = typename ConcreteDescription_::ConcreteTopology
 
using BranchEnum = typename RD::BranchEnum
 
using BodyEnum = typename RD::BodyEnum
 
using JointEnum = typename RD::JointEnum
 
using ContactEnum = typename RD::ContactEnum
 
using ActuatorEnum = typename RD::ActuatorEnum
 
using ForceCalibratorCommandsShm = std_utils::EnumArray< ContactEnum, robot_utils::ForceCalibratorCommand >
 
using ForceCalibratorCommandsRos = robot_utils_ros::ForceCalibratorCommands
 
using PoseWithCovarianceShm = any_measurements::PoseWithCovariance
 
using PoseWithCovarianceRos = geometry_msgs::PoseWithCovarianceStamped
 
using OdometryRos = nav_msgs::Odometry
 
using JointStateShm = std_utils::EnumArray< JointEnum, any_measurements::ExtendedJointState >
 
using JointStateRos = any_msgs::ExtendedJointState
 
using TwistShm = any_measurements::TwistWithCovariance
 
using TwistRos = geometry_msgs::TwistWithCovarianceStamped
 
using QuadrupedFramesGeneratorBase = quadruped_model::QuadrupedFramesGeneratorBase< ConcreteDescription_, RobotState_ >
 
using StateStatus = anymal_description::StateStatus
 
template<typename ValueType_ >
using ContactEnumContainer = std_utils::EnumArray< ContactEnum, ValueType_ >
 
template<typename Msg_ , typename MsgRos_ >
using ForceCalibratorCommandConversionTrait = robot_utils_ros::ConversionTraits< RD, Msg_, MsgRos_ >
 
using ActuatorReadingsContainer = typename RobotContainerRos::ActuatorReadingsRos
 
using ActuatorReadingsShm = typename ActuatorReadingsContainer::type
 
using ActuatorReadingsRos = typename ActuatorReadingsContainer::msgType
 
using ImuContainer = typename RobotContainerRos::ImuRos
 
using ImuShm = typename ImuContainer::type
 
using ImuRos = typename ImuContainer::msgType
 
using RobotStateContainer = typename RobotContainerRos::RobotStateRos
 
using RobotStateRos = typename RobotStateContainer::msgType
 
using RobotStateShm = typename RobotStateContainer::type
 
- Public Types inherited from any_state_estimator::AnyStateEstimator< RobotContainersRos_ >
using RobotContainer = typename RobotContainersRos_::ConcreteContainers
 
using RobotContainerRos = RobotContainersRos_
 
using RobotStateContainer = typename RobotContainerRos::RobotStateRos
 
using ActuatorReadingsContainer = typename RobotContainerRos::ActuatorReadingsRos
 
using ImuContainer = typename RobotContainerRos::ImuRos
 
using ImuShm = typename ImuContainer::type
 
using ImuRos = typename ImuContainer::msgType
 
using RobotStateShm = typename RobotStateContainer::type
 
using RobotStateRos = typename RobotStateContainer::msgType
 
using ActuatorReadingsShm = typename ActuatorReadingsContainer::type
 
using ActuatorReadingsRos = typename ActuatorReadingsContainer::msgType
 

Public Member Functions

 AnymalStateEstimatorBasic ()=delete
 
 AnymalStateEstimatorBasic (any_node::Node::NodeHandlePtr nh)
 
 ~AnymalStateEstimatorBasic () override=default
 
- Public Member Functions inherited from anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >
 AnymalStateEstimator ()=delete
 
 AnymalStateEstimator (any_node::Node::NodeHandlePtr nh)
 
 ~AnymalStateEstimator () override=default
 
- Public Member Functions inherited from any_state_estimator::AnyStateEstimator< RobotContainersRos_ >
 AnyStateEstimator ()=delete
 
 AnyStateEstimator (any_node::Node::NodeHandlePtr nh)
 Constructor. More...
 
 ~AnyStateEstimator () override=default
 
bool init () override
 
void preCleanup () override
 
void cleanup () override
 
bool update (const any_worker::WorkerEvent &event)
 Is called periodically to update the estimator when running standalone. More...
 

Protected Member Functions

void initModel () override
 Sets the model and frames generator ptr. More...
 
void resetModelState (const kindr::HomTransformQuatD &pose) override
 Resets base pose of the robotModelPtr to pose and the actuators according to the current measurements. More...
 
void initializeRobotStateMsgs () override
 Initializes robot specific ros messages. More...
 
void fillJointStates () override
 Fills the joint states with new measurements. More...
 
void updateEstimatedState () override
 Updates the robot specific estimated state. More...
 
- Protected Member Functions inherited from anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >
void initImpl () override
 Initializes further objects. More...
 
bool resetEstimator (const kindr::HomTransformQuatD &pose) override
 Resets the state estimator to pose. More...
 
bool resetEstimatorHere () override
 Resets the state estimator at the current pose. More...
 
void readParameters () override
 Reads parameters from the parameter server. More...
 
void initializeMessages () override
 Initializes messages in derived class. More...
 
void initializePublishers () override
 Initializes additional publishers in the derived class. More...
 
void initializeSubscribers () override
 Initializes additional subscribers in the derived class. More...
 
void advertiseServices () override
 Initializes services. More...
 
void receiveMeasurements () override
 Receives measurements from your subscriber. Gets called before preprocessMeasurements. More...
 
void preprocessMeasurements () override
 Preprocesses the measurements. Gets called before advancing the estimator. More...
 
void advanceEstimator () override
 Advances the estimator by updating the filter. Gets called before setting the estimator output. More...
 
void setOutput () override
 Prepare the output to be published. Gets called before publishing. More...
 
void publish () override
 Publishes desired output via cosmo. More...
 
void publishRos () override
 Publishes data over ros. More...
 
void addVariablesToLog () override
 Adds variables to the signal logger. More...
 
virtual void updateAdditionalFrameTransforms ()
 Optionally implements setting additional frame transforms in updateEstimatedRobotState. More...
 
void forceCalibratorCommandsCallback (const ForceCalibratorCommandsShm &msg)
 Handles incoming forcecalibrator commands from the highlevel controller. More...
 
bool toggleZeroVelocityUpdatesService (any_msgs::Toggle::Request &req, any_msgs::Toggle::Response &res)
 Enables and disables zero velocity updates. If enabled, the estimator will try to keep the base fixed with respect to the world when contact is lost completely. Also, contact timeouts are circumvented in this case. More...
 
void notify (notification::Level level, const std::string &name, const std::string &description, const std::vector< std::string > &outputDevices=std::vector< std::string >{})
 Sends notifications over various channels as specified in the estimator parameters. More...
 
bool calibrateContactForcesService (any_msgs::SetUInt32::Request &req, any_msgs::SetUInt32::Response &res)
 Sets the calibrators in calibration mode. More...
 
bool commandForceCalibratorsService (anymal_state_estimator::ForceCalibratorCommand::Request &req, anymal_state_estimator::ForceCalibratorCommand::Response &res)
 Handles incoming force calibrator commands. More...
 
bool configureForceCalibratorsService (anymal_state_estimator::ForceCalibratorConfig::Request &req, anymal_state_estimator::ForceCalibratorConfig::Response &res)
 Handles incoming force calibrator configurations. More...
 
void initializeForceCalibrators ()
 
void initializeContactDetectors ()
 
void initializeContactWrenchReaders ()
 
void updateContactForces ()
 Processes measured or estimated contact forces using the calibrators and detects whether the endeffectors are in contact or not. More...
 
void updateEstimatedRobotState ()
 Updates the estimated quadrupedState with the newest estimate from the filter. More...
 
void setEstimatedPoses ()
 
void updateEstimatorStatus ()
 
- Protected Member Functions inherited from any_state_estimator::AnyStateEstimator< RobotContainersRos_ >
void update ()
 The main update function of the estimator. Calls receiveMeasurements, preprocessMeasurements, advanceEstimator, setOutput and publish. More...
 
void startWorkers ()
 Starts workers for the synced update and ros publishing. More...
 
bool updateSynced (const any_worker::WorkerEvent &event)
 Calls the update method and afterwards sleeps until the next iteration. More...
 
bool publishRos (const any_worker::WorkerEvent &event)
 Publishes data over ros via publishRos() and afterwards sleeps until the next update has finished. More...
 
void imuCallback (const ImuShm &msg)
 Handles incoming imu messages. More...
 
void actuatorReadingsCallback (const ActuatorReadingsShm &msg)
 Handles incoming actuator reading messages. More...
 
bool resetService (any_state_estimator_msgs::ResetStateEstimator::Request &req, any_state_estimator_msgs::ResetStateEstimator::Response &res)
 Wraps reset estimator, is called by a ros service. More...
 
bool resetHereService (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
 Wraps reset estimator here, is called by a ros service. More...
 

Additional Inherited Members

- Protected Attributes inherited from anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >
bool hasSensorError_ {false}
 
bool useKfeContactEstimation_ {false}
 
bool useContactForceEstimation_ {false}
 
bool hasFullContact_ {false}
 
bool waitingForFullContact_ {true}
 
bool waitingForInitializationDuration_ {true}
 
bool fakeKinematicsUpdateActive_ {false}
 
std::atomic< bool > zeroVelocityUpdatesEnabled_ {false}
 
std::atomic< bool > usePoseMeas_ {true}
 
bool publishRosOdometryMsg_ {false}
 
Eigen::Matrix< double, 3, 1 > imuLinearAccelerationBias_ {Eigen::Matrix<double,3,1>::Zero()}
 
Eigen::Matrix< double, 3, 1 > imuAngularVelocityBias_ {Eigen::Matrix<double,3,1>::Zero()}
 
double initializationDuration_ {0.0}
 
double fullContactTime_ {std::numeric_limits<double>::max()}
 
double sensorWarningTime_ {0.0}
 
unsigned int noContactZeroVelUpdIterationCounter_ {0u}
 
unsigned int noContactZeroVelUpdIterationThreshold_ {1u}
 
unsigned int contactZeroVelUpdIterationCounter_ {0u}
 
unsigned int contactZeroVelUpdIterationThreshold_ {1u}
 
std::string baseFrameId_ {""}
 
std::string odomFrameId_ {""}
 
std::mutex mutexFilter_
 
std::mutex mutexForceCalibrators_
 
std::mutex mutexForceCalibratorCommands_
 
Filter_ filter_
 
std::shared_ptr< RobotModelrobotModelPtr_ {nullptr}
 
std::unique_ptr< QuadrupedFramesGeneratorBaseframesGeneratorPtr_ {nullptr}
 
StateStatus estimatorStatus_
 
RobotState_ filterState_
 
std::atomic< unsigned int > contactFilterCoefficient_ {1u}
 
unsigned int contactFilterCoefficientUpdate_ {1u}
 
unsigned int contactFilterCoefficientInit_ {200u}
 
ContactEnumContainer< unsigned int > consecutiveContactCount_ {0u}
 
ContactEnumContainer< std::unique_ptr< ContactWrenchInterface > > contactWrenchReaders_
 
ContactEnumContainer< std::unique_ptr< ContactWrenchPublisher > > contactWrenchPublishers_
 
ContactEnumContainer< std::unique_ptr< robot_utils::ForceCalibratorBase > > forceCalibrators_
 
ContactEnumContainer< std::unique_ptr< robot_utils::ContactDetectorBase > > contactDetectors_
 
ContactEnumContainer< Contactcontacts_
 
ContactEnumContainer< bool > useForceCalibrators_
 
std::shared_ptr< ContactForceEstimationcontactForceEstimator_ {nullptr}
 
RobotStateRos rosEstRobotState_
 
JointStateShm measJointStates_
 
JointStateRos measJointStatesRos_
 
PoseWithCovarianceShm estPoseInOdom_
 
PoseWithCovarianceRos estPoseInOdomRos_
 
TwistShm estTwist_
 
TwistRos estTwistRos_
 
OdometryRos estOdometryRos_
 
ForceCalibratorCommandsShm forceCalibratorCommands_
 
unsigned int forceCalibratorMissCount_ {0u}
 
unsigned int contactWrenchPublisherMissCount_ {0u}
 
unsigned int forceCalibratorPublisherMissCount_ {0u}
 
unsigned int jointStatePublisherMissCount_ {0u}
 
unsigned int jointStateThrottlePublisherMissCount_ {0u}
 
unsigned int robotStateThrottleMissCount_ {0u}
 
unsigned int imuMissCount_ {0u}
 
unsigned int actuatorReadingMissCount_ {0u}
 
unsigned int imuPublisherMissCount_ {0u}
 
unsigned int imuThrottleMissCount_ {0u}
 
unsigned int robotStatePublisherMissCount_ {0u}
 
unsigned int actuatorReadingsPublisherMissCount_ {0u}
 
unsigned int jointStateThrottleCounter_ {0u}
 
unsigned int poseInOdomThrottleCounter_ {0u}
 
unsigned int twistThrottleCounter_ {0u}
 
unsigned int imuThrottleCounter_ {0u}
 
unsigned int robotStateThrottleCounter_ {0u}
 
unsigned int imuThrottleDecimation_ {1u}
 
unsigned int twistThrottleDecimation_ {1u}
 
unsigned int poseInOdomThrottleDecimation_ {1u}
 
unsigned int jointStateThrottleDecimation_ {1u}
 
unsigned int robotStateThrottleDecimation_ {1u}
 
cosmo_ros::SubscriberRosPtr< ForceCalibratorCommandsShm, ForceCalibratorCommandsRos, ForceCalibratorCommandConversionTraitforceCalibratorCommandSubscriber_
 
cosmo_ros::PublisherRosPtr< ForceCalibratorCommandsShm, ForceCalibratorCommandsRos, ForceCalibratorCommandConversionTraitforceCalibratorCommandsPublisher_
 
cosmo_ros::PublisherRosPtr< PoseWithCovarianceShm, PoseWithCovarianceRos, any_measurements_ros::ConversionTraits > estPoseInOdomPublisher_
 
cosmo_ros::PublisherRosPtr< TwistShm, TwistRos, any_measurements_ros::ConversionTraits > estTwistPublisher_
 
cosmo_ros::PublisherRosPtr< JointStateShm, JointStateRos, any_measurements_ros::ConversionTraits > jointStatePublisher_
 
cosmo_ros::PublisherRosPtr< ImuShm, ImuRos, ImuContainer::template ConversionTrait > imuPublisher_
 
cosmo_ros::PublisherRosPtr< ActuatorReadingsShm, ActuatorReadingsRos, ActuatorReadingsContainer::template ConversionTrait > actuatorReadingsPublisher_
 
cosmo_ros::PublisherRosPtr< JointStateShm, JointStateRos, any_measurements_ros::ConversionTraits > jointStateThrottlePublisher_
 
cosmo_ros::PublisherRosPtr< RobotStateShm, RobotStateRos, RobotStateContainer::template ConversionTrait > robotStateThrottlePublisher_
 
cosmo_ros::PublisherRosPtr< ImuShm, ImuRos, ImuContainer::template ConversionTrait > imuPublisherThrottle_
 
cosmo_ros::PublisherRosPtr< PoseWithCovarianceShm, PoseWithCovarianceRos, any_measurements_ros::ConversionTraits > poseInOdomPublisherThrottle_
 
cosmo_ros::PublisherRosPtr< TwistShm, TwistRos, any_measurements_ros::ConversionTraits > twistPublisherThrottle_
 
ros::Publisher odometryRosPublisher_
 
std::shared_ptr< notification::NotificationPublisher > notificationPublisher_
 
ros::ServiceServer toggleZeroVelocityUpdatesService_
 
ros::ServiceServer setForceCalibrationAlhpaFilterService_
 
ros::ServiceServer setForceCalibrationMaxMahalanobisDistanceService_
 
ros::ServiceServer calibrateContactForceService_
 
ros::ServiceServer commandForceCalibratorsService_
 
ros::ServiceServer configureForceCalibratorsService_
 
- Protected Attributes inherited from any_state_estimator::AnyStateEstimator< RobotContainersRos_ >
double timeStep_ {0.0}
 
double initializationDuration_ {0.0}
 
double iterDurationMs_ {0.0}
 
double updateDurationMs_ {0.0}
 
RobotStateShm estimatedState_
 
ImuShm imu_
 
ActuatorReadingsShm actuatorReadings_
 
std::string syncSlaveName_
 
std::string syncMasterName_
 
std::unique_ptr< cosmo::SyncSlave > syncSlave_
 
std::unique_ptr< cosmo::SyncMaster > syncMaster_
 
const std::chrono::microseconds receiveMaxLockTime_ {std::chrono::microseconds{50}}
 
const std::chrono::microseconds sendMaxLockTime_ {std::chrono::microseconds{50}}
 
cosmo_ros::SubscriberRosPtr< ImuShm, ImuRos, ImuContainer::template ConversionTrait > imuSubscriber_
 
cosmo_ros::SubscriberRosPtr< ActuatorReadingsShm, ActuatorReadingsRos, ActuatorReadingsContainer::template ConversionTrait > actuatorReadingsSubscriber_
 
cosmo_ros::PublisherRosPtr< RobotStateShm, RobotStateRos, RobotStateContainer::template ConversionTrait > robotStatePublisher_
 
ros::ServiceServer resetService_
 
ros::ServiceServer resetHereService_
 
bool isSimulation_
 
bool isStandalone_
 
std::condition_variable_any cvUpdate_
 
std::atomic< bool > stopUpdating_ {true}
 
std::mutex mutexUpdate_
 
std::mutex mutexStateEstimator_
 
std::chrono::time_point< std::chrono::steady_clock > timePrevIteration_
 
std::atomic< unsigned long > updateCounter_ {0lu}
 

Detailed Description

template<typename Filter_>
class anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >

The StateEstimator implementation for an anymal without any custom attachments.

Template Parameters
Filter_The filter type. Needs to derive from AnymalFilter

Member Typedef Documentation

template<typename Filter_ >
using anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >::ActuatorEnum = typename RD::ActuatorEnum
template<typename Filter_ >
using anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::AD = anymal_description::AnymalDescription
template<typename Filter_ >
using anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >::BranchEnum = typename RD::BranchEnum
template<typename Filter_ >
using anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >::ContactEnum = typename RD::ContactEnum
template<typename Filter_ >
using anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::EstimatorBase = AnymalStateEstimatorBase<Filter_>
template<typename Filter_ >
using anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::NodeBase = any_node::Node
template<typename Filter_ >
using anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::QuadrupedFramesGenerator = quadruped_model::QuadrupedFramesGenerator<anymal_description::AnymalConcreteDescription, quadruped_model::QuadrupedState>
template<typename Filter_ >
using anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::QuadrupedModel = quadruped_model::QuadrupedModel
template<typename Filter_ >
using anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::QuadrupedState = quadruped_model::QuadrupedState

Constructor & Destructor Documentation

template<typename Filter_ >
anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::AnymalStateEstimatorBasic ( )
delete
template<typename Filter_ >
anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::AnymalStateEstimatorBasic ( any_node::Node::NodeHandlePtr  nh)
explicit
template<typename Filter_ >
anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::~AnymalStateEstimatorBasic ( )
overridedefault

Member Function Documentation

template<typename Filter_ >
void anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::fillJointStates ( )
overrideprotectedvirtual
template<typename Filter_ >
void anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::initializeRobotStateMsgs ( )
overrideprotectedvirtual
template<typename Filter_ >
void anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::initModel ( )
overrideprotectedvirtual
template<typename Filter_ >
void anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::resetModelState ( const kindr::HomTransformQuatD &  pose)
overrideprotectedvirtual

Resets base pose of the robotModelPtr to pose and the actuators according to the current measurements.

Parameters
[in]poseThe pose to reset

Implements anymal_state_estimator::AnymalStateEstimator< ConcreteDescription_, RobotState_, RobotContainersRos_, Filter_ >.

template<typename Filter_ >
void anymal_state_estimator::AnymalStateEstimatorBasic< Filter_ >::updateEstimatedState ( )
overrideprotectedvirtual

The documentation for this class was generated from the following file: