lipm_walking::Stabilizer Struct Reference

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
robotRobot model.
pendulumCoM state reference placeholder.
dtController 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
guiGUI handle.

Definition at line 127 of file Stabilizer.cpp.

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

Log stabilizer entries.

Parameters
loggerLogger.

Definition at line 76 of file Stabilizer.cpp.

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

Add tasks to QP solver.

Parameters
solverQP 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
wrenchWrench 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
footTaskSwing foot task.
contactTarget 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
solverQP 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
robotsRobots 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
footTaskOne 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
footTaskOne of leftFootTask or rightFootTask.
contactTarget 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
footTaskOne 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
soleSole 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
comPosition of the center of mass.
comdVelocity of the center of mass.
wrenchNet contact wrench in the inertial frame.
leftFootRatioDesired 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
soleSole 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.