#include <mc_observers/KinematicInertialObserver.h>

Inheritance diagram for mc_observers::KinematicInertialObserver:

Collaboration diagram for mc_observers::KinematicInertialObserver:

Public Member Functions
	KinematicInertialObserver (const std::string &type, double dt)

void	configure (const mc_control::MCController &ctl, const mc_rtc::Configuration &config) override
	Configure observer. More...

void	reset (const mc_control::MCController &ctl) override
	Resets the estimator from given robot state Calls FLoatingBasePosObserver::reset(realRobot) to estimate the floating base position, and initializes the velocity filter with robot's floatingbase velocity realRobot.velW() More...

void	reset (const mc_control::MCController &ctl, const sva::MotionVecd &velW)
	Resets the estimator from given robot state First calls FLoatingBasePosObserver::reset(realRobot) to estimate the floating base position, and initializes the velocity filter with the given initial velocity. More...

bool	run (const mc_control::MCController &ctl) override
	Compute observer state. More...

void	update (mc_control::MCController &ctl) override
	Update the real robot state from the observed state. More...

const sva::MotionVecd &	velW () const
	Get floating-base velocity in the world frame. The velocity is obtained by finite differences of the estimated position, filtered with a simple low-pass filter. More...

Public Member Functions inherited from mc_observers::KinematicInertialPoseObserver
	KinematicInertialPoseObserver (const std::string &type, double dt)

void	configure (const mc_control::MCController &ctl, const mc_rtc::Configuration &config) override

void	reset (const mc_control::MCController &ctl) override

bool	run (const mc_control::MCController &ctl) override

void	update (mc_control::MCController &ctl) override

const sva::PTransformd &	posW () const
	Get floating-base pose in the world frame. More...

Public Member Functions inherited from mc_observers::Observer
	Observer (const std::string &type, double dt)

virtual	~Observer ()=default

void	name (const std::string &name)
	Set the observer's name. More...

const std::string &	name () const noexcept
	Returns the observer's name. More...

void	addToLogger_ (const mc_control::MCController &ctl, mc_rtc::Logger &logger, const std::string &category="")
	Add observer entries to the logger under the categrory "category + name()". More...

void	removeFromLogger_ (mc_rtc::Logger &logger, const std::string &category="")
	Remove observer from logger. More...

void	addToGUI_ (const mc_control::MCController &ctl, mc_rtc::gui::StateBuilder &gui, std::vector< std::string > category={})
	Add observer to the gui under category {category, name()}. More...

void	removeFromGUI_ (mc_rtc::gui::StateBuilder &gui, std::vector< std::string > category={})
	Remove observer from gui. More...

const std::string &	desc () const noexcept
	Short description of the observer. More...

const std::string	type () const noexcept

const std::string &	error () const noexcept

double	dt () const noexcept

Protected Member Functions
void	addToLogger (const mc_control::MCController &ctl, mc_rtc::Logger &, const std::string &category) override
	Add observer from logger. More...

void	addToGUI (const mc_control::MCController &, mc_rtc::gui::StateBuilder &, const std::vector< std::string > &category) override
	Add observer information the GUI. More...

Protected Member Functions inherited from mc_observers::KinematicInertialPoseObserver
void	addToLogger (const mc_control::MCController &ctl, mc_rtc::Logger &, const std::string &category) override
	Add observer from logger. More...

void	addToGUI (const mc_control::MCController &, mc_rtc::gui::StateBuilder &, const std::vector< std::string > &category) override
	Add observer information the GUI. More...

void	estimateOrientation (const mc_rbdyn::Robot &robot, const mc_rbdyn::Robot &realRobot)

void	estimatePosition (const mc_control::MCController &ctl)

Protected Member Functions inherited from mc_observers::Observer
virtual void	removeFromLogger (mc_rtc::Logger &, const std::string &)
	Remove observer from logger. More...

virtual void	removeFromGUI (mc_rtc::gui::StateBuilder &, const std::vector< std::string > &category)
	Remove observer from GUI. More...

Additional Inherited Members
Protected Attributes inherited from mc_observers::KinematicInertialPoseObserver
std::string	robot_

std::string	realRobot_

std::string	imuSensor_

std::string	anchorFrameFunction_ = ""
	Name of datastore entry for the anchor frame function. More...

sva::PTransformd	X_0_anchorFrame_
	Control anchor frame (provided through the datastore) More...

sva::PTransformd	X_0_anchorFrameReal_
	Real anchor frame (provided through the datastore) More...

double	maxAnchorFrameDiscontinuity_
	Threshold (norm) above wich the anchor frame is considered to have had a discontinuity. More...

bool	anchorFrameJumped_ = false

bool	firstIter_ = true

Protected Attributes inherited from mc_observers::Observer
std::string	name_
	Observer name. More...

std::string	type_
	Observer type. More...

std::string	desc_
	Short description of the observer and its configuration. More...

std::string	error_
	Descriptive error message to show if the observer failed. More...

double	dt_
	Timestep. More...

Detailed Description

Kinematics-inertial observer of the floating base position with finite differences estimation of the floating base velocity using low-pass filtering.

See also: KinematicInertialPoseObserver for details about the estimation of the floating base position.

Constructor & Destructor Documentation

◆ KinematicInertialObserver()

mc_observers::KinematicInertialObserver::KinematicInertialObserver	(	const std::string &	type,
		double	dt
	)

inline

Member Function Documentation

◆ addToGUI()

void mc_observers::KinematicInertialObserver::addToGUI	(	const mc_control::MCController &	,
		mc_rtc::gui::StateBuilder &	,
		const std::vector< std::string > &
	)

overrideprotectedvirtual

Add observer information the GUI.

Default implementation does nothing, each observer implementation is responsible for adding its own elements to the GUI.

Parameters

category Category in which to add this observer

Reimplemented from mc_observers::Observer.

◆ addToLogger()

void mc_observers::KinematicInertialObserver::addToLogger	(	const mc_control::MCController &	,
		mc_rtc::Logger &	,
		const std::string &
	)

overrideprotectedvirtual

Add observer from logger.

Default implementation does nothing, each observer implementation is responsible for adding its own log entries

Parameters

category Category in which to log this observer

Reimplemented from mc_observers::Observer.

◆ configure()

void mc_observers::KinematicInertialObserver::configure	(	const mc_control::MCController &	,
		const mc_rtc::Configuration &
	)

overridevirtual

Configure observer.

Parameters

config Configuration for this observer. Refer to the JSON Schema documentation fo each observer for details on supported entries: https://jrl-umi3218.github.io/mc_rtc/json.html#Observers-objects

Reimplemented from mc_observers::Observer.

◆ reset() [1/2]

void mc_observers::KinematicInertialObserver::reset ( const mc_control::MCController & ctl )

overridevirtual

Resets the estimator from given robot state Calls FLoatingBasePosObserver::reset(realRobot) to estimate the floating base position, and initializes the velocity filter with robot's floatingbase velocity realRobot.velW()

Parameters

ctl	Controller access

Implements mc_observers::Observer.

◆ reset() [2/2]

void mc_observers::KinematicInertialObserver::reset	(	const mc_control::MCController &	ctl,
		const sva::MotionVecd &	velW
	)

Resets the estimator from given robot state First calls FLoatingBasePosObserver::reset(realRobot) to estimate the floating base position, and initializes the velocity filter with the given initial velocity.

Parameters

realRobot	Robot state from which the observer is to be initialized.
velW	Initial velocity

◆ run()

bool mc_observers::KinematicInertialObserver::run ( const mc_control::MCController & ctl )

overridevirtual

Compute observer state.

This function is called before MCController::run()

Implements mc_observers::Observer.

◆ update()

void mc_observers::KinematicInertialObserver::update ( mc_control::MCController & ctl )

overridevirtual

Update the real robot state from the observed state.

This function is expected to write the estimated parameters to the corresponding real robot instances ctl.realRobots(), and compute the necessary information to ensure a consistent state (e.g calling forward velocity after updating joint velocities, etc).

This function is called after Observer::run() if the observer is declared to update the real robot in the pipeline configuration.

Parameters

ctl	Controller running this observer

Implements mc_observers::Observer.

◆ velW()

const sva::MotionVecd& mc_observers::KinematicInertialObserver::velW ( ) const

Get floating-base velocity in the world frame. The velocity is obtained by finite differences of the estimated position, filtered with a simple low-pass filter.

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

include/mc_observers/KinematicInertialObserver.h

Public Member Functions

Protected Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ KinematicInertialObserver()

Member Function Documentation

◆ addToGUI()

◆ addToLogger()

◆ configure()

◆ reset() [1/2]

◆ reset() [2/2]

◆ run()

◆ update()

◆ velW()