Model predictive control problem. More...

#include <lipm_walking/ModelPredictiveControl.h>

Public Member Functions
	ModelPredictiveControl ()
	Initialize new problem. 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	configure (const mc_rtc::Configuration &)
	Read configuration from dictionary. More...

void	phaseDurations (double initSupportDuration, double doubleSupportDuration, double targetSupportDuration)
	Set duration of the initial single-support phase. More...

bool	buildAndSolve ()
	Build and solve the model predictive control quadratic program. More...

void	comHeight (double height)
	Set CoM height. More...

void	contacts (Contact initContact, Contact targetContact, Contact nextContact)
	Reset contacts. More...

void	initState (const mc_planning::Pendulum &pendulum)
	Set the initial CoM state. More...

unsigned	indexToHrep (unsigned i) const
	Get index of inequality constraints. More...

const Contact &	initContact () const
	Support contact in the first single-support phase. More...

unsigned	nbInitSupportSteps () const
	Number of sampling steps in the preview spent in the first single-support phase. More...

unsigned	nbDoubleSupportSteps () const
	Number of sampling steps in the preview spent in the first double-support phase. More...

const Contact &	nextContact () const
	Support contact in the third single-support phase. More...

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

const std::shared_ptr< Preview >	solution () const
	Get solution vector. More...

const Contact &	targetContact () const
	Support contact in the second single-support phase. More...

Public Attributes
Eigen::Vector2d	velWeights = {10., 10.}
	Weights of CoM velocity tracking cost. More...

double	jerkWeight = 1.
	Weight of CoM jerk regularization cost. More...

double	zmpWeight = 1000.
	Weight of reference ZMP tracking cost. More...

Static Public Attributes
static constexpr EIGEN_MAKE_ALIGNED_OPERATOR_NEW double	SAMPLING_PERIOD = 0.1
	Duration of each sampling step in [s]. More...

static constexpr unsigned	INPUT_SIZE = 2
	Input is the 2D horizontal CoM jerk. More...

static constexpr unsigned	NB_STEPS = 16
	Number of sampling steps. More...

static constexpr unsigned	STATE_SIZE
	State is the 6D stacked vector of CoM positions, velocities and accelerations. More...

Detailed Description

Model predictive control problem.

This implementation is based on "Trajectory free linear model predictive control for stable walking in the presence of strong perturbations" (Wieber, Humanoids 2006) with the addition of terminal constraints.

Definition at line 53 of file ModelPredictiveControl.h.

Constructor & Destructor Documentation

◆ ModelPredictiveControl()

lipm_walking::ModelPredictiveControl::ModelPredictiveControl ( )

Initialize new problem.

Definition at line 46 of file ModelPredictiveControl.cpp.

Member Function Documentation

◆ addGUIElements()

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

Add GUI panel.

Parameters

gui	GUI handle.

Definition at line 89 of file ModelPredictiveControl.cpp.

◆ addLogEntries()

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

Log stabilizer entries.

Parameters

logger Logger.

Definition at line 137 of file ModelPredictiveControl.cpp.

◆ buildAndSolve()

bool lipm_walking::ModelPredictiveControl::buildAndSolve ( )

Build and solve the model predictive control quadratic program.

Returns: solutionFound Did the solver find a solution?

Definition at line 338 of file ModelPredictiveControl.cpp.

◆ comHeight()

void lipm_walking::ModelPredictiveControl::comHeight ( double height )

inline

Set CoM height.

Parameters

height CoM height above contacts.

Definition at line 120 of file ModelPredictiveControl.h.

◆ configure()

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

Read configuration from dictionary.

Definition at line 78 of file ModelPredictiveControl.cpp.

◆ contacts()

void lipm_walking::ModelPredictiveControl::contacts	(	Contact	initContact,
		Contact	targetContact,
		Contact	nextContact
	)

inline

Reset contacts.

Parameters

initContact	Contact used during single-support phase.
targetContact	Contact used during double-support phases.
nextContact	Contact coming after targetContact in the plan.

Definition at line 143 of file ModelPredictiveControl.h.

◆ indexToHrep()

unsigned lipm_walking::ModelPredictiveControl::indexToHrep ( unsigned i ) const

inline

Get index of inequality constraints.

Parameters

i	Timestep in preview horizon.

Returns: hrepIndex Index of corresponding ZMP H-rep.

Note: Inequality constraints are the halfspace representation (H-rep) of a polyhedron. In this class we call them H-rep for short.

Definition at line 171 of file ModelPredictiveControl.h.

◆ initContact()

const Contact& lipm_walking::ModelPredictiveControl::initContact ( ) const

inline

Support contact in the first single-support phase.

Definition at line 179 of file ModelPredictiveControl.h.

◆ initState()

void lipm_walking::ModelPredictiveControl::initState ( const mc_planning::Pendulum & pendulum )

inline

Set the initial CoM state.

Parameters

pendulum CoM state.

Definition at line 155 of file ModelPredictiveControl.h.

◆ nbDoubleSupportSteps()

unsigned lipm_walking::ModelPredictiveControl::nbDoubleSupportSteps ( ) const

inline

Number of sampling steps in the preview spent in the first double-support phase.

Definition at line 198 of file ModelPredictiveControl.h.

◆ nbInitSupportSteps()

unsigned lipm_walking::ModelPredictiveControl::nbInitSupportSteps ( ) const

inline

Number of sampling steps in the preview spent in the first single-support phase.

Definition at line 188 of file ModelPredictiveControl.h.

◆ nextContact()

const Contact& lipm_walking::ModelPredictiveControl::nextContact ( ) const

inline

Support contact in the third single-support phase.

Definition at line 206 of file ModelPredictiveControl.h.

◆ phaseDurations()

void lipm_walking::ModelPredictiveControl::phaseDurations	(	double	initSupportDuration,
		double	doubleSupportDuration,
		double	targetSupportDuration
	)

Set duration of the initial single-support phase.

Parameters

initSupportDuration	First SSP duration.
doubleSupportDuration	First DSP duration.
targetSupportDuration	Second SSP duration.

Phase durations don't have to sum up to the total duration of the preview horizon.

If their sum is below total duration, there are two outcomes: if there is a target support phase, a second DSP phase is added from the target contact to the next (full preview mode); otherwise, the first DSP phase is extended until the end of the preview horizon (half preview mode).

If their sum exceeds total duration, phase durations are trimmed starting from the last one.

Definition at line 148 of file ModelPredictiveControl.cpp.

◆ sole()

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

inline

Set model sole properties.

Parameters

sole	Sole parameters.

Definition at line 216 of file ModelPredictiveControl.h.

◆ solution()

const std::shared_ptr<Preview> lipm_walking::ModelPredictiveControl::solution ( ) const

inline

Get solution vector.

Definition at line 224 of file ModelPredictiveControl.h.

◆ targetContact()

const Contact& lipm_walking::ModelPredictiveControl::targetContact ( ) const

inline

Support contact in the second single-support phase.

Definition at line 232 of file ModelPredictiveControl.h.

Member Data Documentation

◆ INPUT_SIZE

constexpr unsigned lipm_walking::ModelPredictiveControl::INPUT_SIZE = 2

staticconstexpr

Input is the 2D horizontal CoM jerk.

Definition at line 58 of file ModelPredictiveControl.h.

◆ jerkWeight

double lipm_walking::ModelPredictiveControl::jerkWeight = 1.

Weight of CoM jerk regularization cost.

Definition at line 252 of file ModelPredictiveControl.h.

◆ NB_STEPS

constexpr unsigned lipm_walking::ModelPredictiveControl::NB_STEPS = 16

staticconstexpr

Number of sampling steps.

Definition at line 59 of file ModelPredictiveControl.h.

◆ SAMPLING_PERIOD

constexpr double lipm_walking::ModelPredictiveControl::SAMPLING_PERIOD = 0.1

staticconstexpr

Duration of each sampling step in [s].

Definition at line 57 of file ModelPredictiveControl.h.

◆ STATE_SIZE

constexpr unsigned lipm_walking::ModelPredictiveControl::STATE_SIZE

staticconstexpr

Initial value:

=

6

State is the 6D stacked vector of CoM positions, velocities and accelerations.

Definition at line 60 of file ModelPredictiveControl.h.

◆ velWeights

Eigen::Vector2d lipm_walking::ModelPredictiveControl::velWeights = {10., 10.}

Weights of CoM velocity tracking cost.

Definition at line 251 of file ModelPredictiveControl.h.

◆ zmpWeight

double lipm_walking::ModelPredictiveControl::zmpWeight = 1000.

Weight of reference ZMP tracking cost.

Definition at line 253 of file ModelPredictiveControl.h.

Public Member Functions

Public Attributes

Static Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ModelPredictiveControl()

Member Function Documentation

◆ addGUIElements()

◆ addLogEntries()

◆ buildAndSolve()

◆ comHeight()

◆ configure()

◆ contacts()

◆ indexToHrep()

◆ initContact()

◆ initState()

◆ nbDoubleSupportSteps()

◆ nbInitSupportSteps()

◆ nextContact()

◆ phaseDurations()

◆ sole()

◆ solution()

◆ targetContact()

Member Data Documentation

◆ INPUT_SIZE

◆ jerkWeight

◆ NB_STEPS

◆ SAMPLING_PERIOD

◆ STATE_SIZE

◆ velWeights

◆ zmpWeight