Walking stabilization based on linear inverted pendulum tracking. More...

#include <lipm_walking/Stabilizer.h>

Public Member Functions
	Stabilizer (const mc_rbdyn::Robot &robot, const Pendulum &pendulum, double dt)
	Initialize stabilizer. More...

void	addGUIElements (std::shared_ptr< mc_rtc::gui::StateBuilder > gui)
	Add GUI panel. More...

void	addLogEntries (mc_rtc::Logger &logger)
	Log stabilizer entries. More...

void	addTasks (mc_solver::QPSolver &solver)
	Add tasks to QP solver. More...

void	disable ()
	Disable all feedback components. More...

Eigen::Vector3d	computeZMP (const sva::ForceVecd &wrench) const
	Compute ZMP of a wrench in the output frame. More...

void	configure (const mc_rtc::Configuration &)
	Read configuration from dictionary. More...

bool	detectTouchdown (const std::shared_ptr< mc_tasks::force::CoPTask > footTask, const Contact &contact)
	Detect foot touchdown based on both force and distance. More...

void	reconfigure ()
	Apply stored configuration. More...

void	removeTasks (mc_solver::QPSolver &solver)
	Remove tasks from QP solver. More...

void	reset (const mc_rbdyn::Robots &robots)
	Reset CoM and foot CoP tasks. More...

void	run ()
	Update QP task targets. More...

void	seekTouchdown (std::shared_ptr< mc_tasks::force::CoPTask > footTask)
	Configure foot task for contact seeking. More...

void	setContact (std::shared_ptr< mc_tasks::force::CoPTask > footTask, const Contact &contact)
	Configure foot task for contact at a given location. More...

void	setSwingFoot (std::shared_ptr< mc_tasks::force::CoPTask > footTask)
	Configure foot task for swinging. More...

ContactState	contactState ()
	Get contact state. More...

void	contactState (ContactState contactState)
	Set desired contact state. More...

void	sole (const Sole &sole)
	Set model sole properties. More...

void	updateState (const Eigen::Vector3d &com, const Eigen::Vector3d &comd, const sva::ForceVecd &wrench, double leftFootRatio)
	Update real-robot state. More...

void	wrenchFaceMatrix (const Sole &sole)
	Update H-representation of contact wrench cones. More...

Eigen::Vector3d	zmp () const
	ZMP target after wrench distribution. More...

Public Attributes
Contact	leftFootContact
	Current left foot contact. More...

Contact	rightFootContact
	Current right foot contact. More...

std::shared_ptr< mc_tasks::CoMTask >	comTask
	CoM position task. More...

std::shared_ptr< mc_tasks::force::CoPTask >	leftFootTask
	Left foot hybrid position/force control task. More...

std::shared_ptr< mc_tasks::force::CoPTask >	rightFootTask
	Right foot hybrid position/force control task. More...

Static Public Attributes
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW constexpr double	MAX_AVERAGE_DCM_ERROR = 0.05
	Maximum average (integral) DCM error in [m]. More...

static constexpr double	MAX_COM_ADMITTANCE = 20
	Maximum admittance for CoM admittance control. More...

static constexpr double	MAX_COP_ADMITTANCE = 0.1
	Maximum CoP admittance for foot damping control. More...

static constexpr double	MAX_DCM_D_GAIN = 2.
	Maximum DCM derivative gain (no unit) More...

static constexpr double	MAX_DCM_I_GAIN = 100.
	Maximum DCM average integral gain in [Hz]. More...

static constexpr double	MAX_DCM_P_GAIN = 20.
	Maximum DCM proportional gain in [Hz]. More...

static constexpr double	MAX_DFZ_ADMITTANCE
	Maximum admittance in [s] / [kg] for foot force difference control. More...

static constexpr double	MAX_DFZ_DAMPING
	Maximum normalized damping in [Hz] for foot force difference control. More...

static constexpr double	MAX_FDC_RX_VEL
	Maximum x-axis angular velocity in [rad] / [s] for foot damping control. More...

static constexpr double	MAX_FDC_RY_VEL
	Maximum y-axis angular velocity in [rad] / [s] for foot damping control. More...

static constexpr double	MAX_FDC_RZ_VEL
	Maximum z-axis angular velocity in [rad] / [s] for foot damping control. More...

static constexpr double	MAX_ZMPCC_COM_OFFSET = 0.05
	Maximum CoM offset due to admittance control in [m]. More...

static constexpr double	MIN_DSP_FZ = 15.
	Minimum normal contact force in [N] for DSP, used to avoid low-force targets when close to contact switches. More...

Detailed Description

Walking stabilization based on linear inverted pendulum tracking.

Stabilization bridges the gap between the open-loop behavior of the pendulum state reference (feedforward controls) and feedback read from state estimation. In our case, feedback is done on the DCM of the LIPM:

\[ \dot{\xi} = \dot{\xi}^{d} + k_p (\xi^d - \xi) + k_i \int (\xi^d - \xi) \]

Which boils down into corresponding formulas for the CoP and CoM acceleration targets.

Definition at line 58 of file Stabilizer.h.

Constructor & Destructor Documentation

lipm_walking::Stabilizer::Stabilizer	(	const mc_rbdyn::Robot &	robot,
		const Pendulum &	pendulum,
		double	dt
	)

Initialize stabilizer.

Parameters

robot	Robot model.
pendulum	CoM state reference placeholder.
dt	Controller timestep.

Definition at line 70 of file Stabilizer.cpp.

Member Function Documentation

void lipm_walking::Stabilizer::addGUIElements ( std::shared_ptr< mc_rtc::gui::StateBuilder > gui )

Add GUI panel.

Parameters

gui	GUI handle.

Definition at line 127 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::addLogEntries ( mc_rtc::Logger & logger )

Log stabilizer entries.

Parameters

logger Logger.

Definition at line 76 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::addTasks ( mc_solver::QPSolver & solver )

Add tasks to QP solver.

Parameters

solver QP solver to add tasks to.

Definition at line 358 of file Stabilizer.cpp.

Eigen::Vector3d lipm_walking::Stabilizer::computeZMP ( const sva::ForceVecd & wrench ) const

Compute ZMP of a wrench in the output frame.

Parameters

wrench Wrench at the origin of the world frame.

Definition at line 474 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::configure ( const mc_rtc::Configuration & config )

Read configuration from dictionary.

Definition at line 245 of file Stabilizer.cpp.

ContactState lipm_walking::Stabilizer::contactState ( )

inline

Get contact state.

Returns: contactState Contact state.

Definition at line 200 of file Stabilizer.h.

void lipm_walking::Stabilizer::contactState ( ContactState contactState )

inline

Set desired contact state.

Definition at line 208 of file Stabilizer.h.

bool lipm_walking::Stabilizer::detectTouchdown	(	const std::shared_ptr< mc_tasks::force::CoPTask >	footTask,
		const Contact &	contact
	)

Detect foot touchdown based on both force and distance.

Parameters

footTask	Swing foot task.
contact	Target contact.

Definition at line 400 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::disable ( )

Disable all feedback components.

Definition at line 233 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::reconfigure ( )

Apply stored configuration.

Definition at line 251 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::removeTasks ( mc_solver::QPSolver & solver )

Remove tasks from QP solver.

Parameters

solver QP solver to remove tasks from.

Definition at line 365 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::reset ( const mc_rbdyn::Robots & robots )

Reset CoM and foot CoP tasks.

Parameters

robots Robots where the task will be applied.

Definition at line 308 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::run ( )

Update QP task targets.

This function is called once the reference has been updated.

Definition at line 490 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::seekTouchdown ( std::shared_ptr< mc_tasks::force::CoPTask > footTask )

Configure foot task for contact seeking.

Parameters

footTask One of leftFootTask or rightFootTask.

This function has no effect when the measured normal force is already higher than the target. Otherwise, it will set a positive admittance along the z-axis of the contact frame.

Definition at line 412 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::setContact	(	std::shared_ptr< mc_tasks::force::CoPTask >	footTask,
		const Contact &	contact
	)

Configure foot task for contact at a given location.

Parameters

footTask	One of leftFootTask or rightFootTask.
contact	Target contact location.

Definition at line 372 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::setSwingFoot ( std::shared_ptr< mc_tasks::force::CoPTask > footTask )

Configure foot task for swinging.

Parameters

footTask One of leftFootTask or rightFootTask.

Foot target is reset to the current frame pose.

Definition at line 393 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::sole ( const Sole & sole )

inline

Set model sole properties.

Parameters

sole	Sole parameters.

Definition at line 218 of file Stabilizer.h.

void lipm_walking::Stabilizer::updateState	(	const Eigen::Vector3d &	com,
		const Eigen::Vector3d &	comd,
		const sva::ForceVecd &	wrench,
		double	leftFootRatio
	)

Update real-robot state.

Parameters

com	Position of the center of mass.
comd	Velocity of the center of mass.
wrench	Net contact wrench in the inertial frame.
leftFootRatio	Desired force distribution ratio for left foot.

Definition at line 518 of file Stabilizer.cpp.

void lipm_walking::Stabilizer::wrenchFaceMatrix ( const Sole & sole )

inline

Update H-representation of contact wrench cones.

Parameters

sole	Sole parameters.

See https://hal.archives-ouvertes.fr/hal-02108449/document for technical details on the derivation of this formula.

Definition at line 247 of file Stabilizer.h.

Eigen::Vector3d lipm_walking::Stabilizer::zmp ( ) const

inline

ZMP target after wrench distribution.

Returns: zmp ZMP target in the inertial frame.

Definition at line 279 of file Stabilizer.h.

Member Data Documentation

std::shared_ptr<mc_tasks::CoMTask> lipm_walking::Stabilizer::comTask

CoM position task.

Definition at line 389 of file Stabilizer.h.

Contact lipm_walking::Stabilizer::leftFootContact

Current left foot contact.

Definition at line 387 of file Stabilizer.h.

std::shared_ptr<mc_tasks::force::CoPTask> lipm_walking::Stabilizer::leftFootTask

Left foot hybrid position/force control task.

Definition at line 390 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_AVERAGE_DCM_ERROR = 0.05

static

Maximum average (integral) DCM error in [m].

Definition at line 62 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_COM_ADMITTANCE = 20

static

Maximum admittance for CoM admittance control.

Definition at line 63 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_COP_ADMITTANCE = 0.1

static

Maximum CoP admittance for foot damping control.

Definition at line 64 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_DCM_D_GAIN = 2.

static

Maximum DCM derivative gain (no unit)

Definition at line 65 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_DCM_I_GAIN = 100.

static

Maximum DCM average integral gain in [Hz].

Definition at line 66 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_DCM_P_GAIN = 20.

static

Maximum DCM proportional gain in [Hz].

Definition at line 67 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_DFZ_ADMITTANCE

static

Initial value:

=

5e-4

Maximum admittance in [s] / [kg] for foot force difference control.

Definition at line 68 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_DFZ_DAMPING

static

Initial value:

=

10.

Maximum normalized damping in [Hz] for foot force difference control.

Definition at line 70 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_FDC_RX_VEL

static

Initial value:

=

0.2

Maximum x-axis angular velocity in [rad] / [s] for foot damping control.

Definition at line 72 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_FDC_RY_VEL

static

Initial value:

=

0.2

Maximum y-axis angular velocity in [rad] / [s] for foot damping control.

Definition at line 74 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_FDC_RZ_VEL

static

Initial value:

=

0.2

Maximum z-axis angular velocity in [rad] / [s] for foot damping control.

Definition at line 76 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MAX_ZMPCC_COM_OFFSET = 0.05

static

Maximum CoM offset due to admittance control in [m].

Definition at line 78 of file Stabilizer.h.

constexpr double lipm_walking::Stabilizer::MIN_DSP_FZ = 15.

static

Minimum normal contact force in [N] for DSP, used to avoid low-force targets when close to contact switches.

Definition at line 79 of file Stabilizer.h.

Contact lipm_walking::Stabilizer::rightFootContact

Current right foot contact.

Definition at line 388 of file Stabilizer.h.

std::shared_ptr<mc_tasks::force::CoPTask> lipm_walking::Stabilizer::rightFootTask

Right foot hybrid position/force control task.

Definition at line 391 of file Stabilizer.h.

Public Member Functions

Public Attributes

Static Public Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation