#include <mc_observers/KinematicInertialPoseObserver.h>

Inheritance diagram for mc_observers::KinematicInertialPoseObserver:

Collaboration diagram for mc_observers::KinematicInertialPoseObserver:

Public Member Functions
	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...

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...

Protected Attributes
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.

This estimator relies on the feet contact location and the IMU orientation to estimate the floating base position and orientation. Note that this is a very simple kinematic inertial estimator that does not take advantage of closed-loop kinematic chains. As a result, the estimate using the left foot contact as an anchor point and the one using the right foot might differ slightly.

See https://scaron.info/teaching/floating-base-estimation.html for technical details on the derivation of this simple estimator.

and for comparison between simple kinematic-inertial estimators the paper

"Experimental Evaluation of Simple Estimators for Humanoid Robots" by Thomas Flayols, Andrea Del Prete, Patrick Wensing, Alexis Mifsud, Mehdi Benallegue, Olivier Stasse https://hal.archives-ouvertes.fr/hal-01574819/document

Constructor & Destructor Documentation

◆ KinematicInertialPoseObserver()

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

inline

Initialize floating base observer

Member Function Documentation

◆ addToGUI()

void mc_observers::KinematicInertialPoseObserver::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::KinematicInertialPoseObserver::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::KinematicInertialPoseObserver::configure	(	const mc_control::MCController &	ctl,
		const mc_rtc::Configuration &	config
	)

overridevirtual

Configure observer

Reimplemented from mc_observers::Observer.

◆ estimateOrientation()

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

protected

Compute floating-base orientation based on new observed gravity vector.

Parameters

robot	Control robot model.
realRobot	Measured robot state.

Note: Prior to mc_rtc 1.5, there was an important bug in the implementation concerning how roll and pitch from sensor measurement was merged with the yaw from control. This bug was fixed in mc_rtc 1.5, you might experience behaviour changes if you were using the old implementation.

◆ estimatePosition()

void mc_observers::KinematicInertialPoseObserver::estimatePosition ( const mc_control::MCController & ctl )

protected

Compute floating-base position from the estimated orientation and a kinematic anchor frame

Parameters

robot	Control robot model.
realRobot	Measurements robot model.

The new position is chosen so that the origin of the real anchor frame coincides with the control anchor frame.

◆ posW()

const sva::PTransformd& mc_observers::KinematicInertialPoseObserver::posW ( ) const

inline

Get floating-base pose in the world frame.

◆ reset()

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

overridevirtual

Reset floating base estimate from the current real robot state

Implements mc_observers::Observer.

◆ run()

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

overridevirtual

Update floating-base estimation of real robot (IMU + kinematics)

Requires an anchor frame to be provided as a datastore callback. This should be a frame in-between the robot contacts, and its trajectory should be continuous. See https://jrl-umi3218.github.io/mc_rtc/tutorials/recipes/observers.html for futher information.

Example:

datastore().make_call("KinematicAnchorFrame::" + robot().name(), [this](const mc_rbdyn::Robot & robot) {
  return sva::interpolate(robot.surfacePose(leftFootSurface_), robot.surfacePose(rightFootSurface_), 0.5);
});

See also: estimateOrientation(const mc_rbdyn::Robot & robot, const mc_rbdyn::Robot & realRobot);; estimatePosition(const mc_control::MCController & ctl);

Implements mc_observers::Observer.

◆ update()

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

overridevirtual

Write observed floating-base transform to the robot's configuration

Implements mc_observers::Observer.

Member Data Documentation

◆ anchorFrameFunction_

std::string mc_observers::KinematicInertialPoseObserver::anchorFrameFunction_ = ""

protected

Name of datastore entry for the anchor frame function.

◆ anchorFrameJumped_

bool mc_observers::KinematicInertialPoseObserver::anchorFrameJumped_ = false

protected

◆ firstIter_

bool mc_observers::KinematicInertialPoseObserver::firstIter_ = true

protected

Detects whether the anchor frame had a discontinuity

◆ imuSensor_

std::string mc_observers::KinematicInertialPoseObserver::imuSensor_

protected

BodySensor containting IMU readings

◆ maxAnchorFrameDiscontinuity_

double mc_observers::KinematicInertialPoseObserver::maxAnchorFrameDiscontinuity_

protected

Initial value:

=

0.01

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

◆ realRobot_

std::string mc_observers::KinematicInertialPoseObserver::realRobot_

protected

Corresponding real robot (default main real robot)

◆ robot_

std::string mc_observers::KinematicInertialPoseObserver::robot_

protected

Robot to observe (default main robot)

◆ X_0_anchorFrame_

sva::PTransformd mc_observers::KinematicInertialPoseObserver::X_0_anchorFrame_

protected

Initial value:

=

sva::PTransformd::Identity()

Control anchor frame (provided through the datastore)

◆ X_0_anchorFrameReal_

sva::PTransformd mc_observers::KinematicInertialPoseObserver::X_0_anchorFrameReal_

protected

Initial value:

=

sva::PTransformd::Identity()

Real anchor frame (provided through the datastore)

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

include/mc_observers/KinematicInertialPoseObserver.h

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ KinematicInertialPoseObserver()

Member Function Documentation

◆ addToGUI()

◆ addToLogger()

◆ configure()

◆ estimateOrientation()

◆ estimatePosition()

◆ posW()

◆ reset()

◆ run()

◆ update()

Member Data Documentation

◆ anchorFrameFunction_

◆ anchorFrameJumped_

◆ firstIter_

◆ imuSensor_

◆ maxAnchorFrameDiscontinuity_

◆ realRobot_

◆ robot_

◆ X_0_anchorFrame_

◆ X_0_anchorFrameReal_