Detailed Description

Model Predictive Control problem for walking pattern generation [determine_solution_structure.m, form_rotation_matrices.m, form_foot_pos_matrices.m, form_condensing_matrices.m].

Definition at line 21 of file mpc_wpg.h.

#include <mpc_wpg.h>

Inheritance diagram for humoto::wpg04::MPCforWPG:

Public Member Functions
	MPCforWPG ()
	Constructor. More...

	MPCforWPG (const humoto::wpg04::MPCParameters &mpc_parameters)
	Constructor. More...

humoto::rigidbody::PointMassState	getCoMState (const humoto::wpg04::Model &model, const std::size_t time_instant_ms)
	Computes CoM state at the given time instant. More...

humoto::rigidbody::RigidBodyState	getFootLandingState (const humoto::wpg04::Model &model) const
	Get landing state of a foot. More...

humoto::wpg04::ModelState	getNextModelState (const humoto::Solution &solution, const humoto::walking::StanceFiniteStateMachine &stance_fsm, const humoto::wpg04::Model &model)
	Get next model state. More...

humoto::wpg04::ModelState	getNextModelState (const humoto::walking::StanceFiniteStateMachine &stance_fsm, const humoto::wpg04::Model &model) const
	Get next model state. More...

std::size_t	getPreviewHorizonLength () const
	Returns length of the preview horizon. More...

void	guessSolution (Solution &solution_guess, const Solution &old_solution) const
	Guess solution. More...

void	initSolutionStructure (humoto::Solution &solution) const
	Initialize structure of the given solution based on the internally stored solution structure. More...

void	log (humoto::Logger &logger, const LogEntryName &parent=LogEntryName(), const std::string &name="mpcwpg") const
	Log. More...

void	parseSolution (const humoto::Solution &solution)
	Process solution. More...

void	setParameters (const humoto::wpg04::MPCParameters &mpc_parameters)
	Set parameters. More...

ControlProblemStatus::Status	update (const humoto::wpg04::Model &model, const humoto::walking::StanceFiniteStateMachine &stance_fsm, const humoto::wpg04::WalkParameters &walk_parameters)
	Update control problem. More...

Public Attributes
Eigen::VectorXd	cop_profile

Eigen::VectorXd	cstate_profile

Eigen::VectorXd	dcop_profile

Eigen::VectorXd	footpos_profile

Eigen::MatrixXd	Ir_

humoto::wpg04::PreviewHorizon	preview_horizon_

etools::DiagonalBlockMatrix< 2, 2 >	R_

etools::DiagonalBlockMatrix< 2, 2 >	Rh_

Eigen::MatrixXd	S_

Eigen::MatrixXd	s_

Eigen::MatrixXd	Sdz_

Eigen::MatrixXd	sdz_

Eigen::MatrixXd	V0_

Eigen::MatrixXd	V_

etools::SelectionMatrix	velocity_selector_

Eigen::MatrixXd	Vfp_

Eigen::MatrixXd	vfp_

Static Protected Member Functions
template<int t_num_vars, int t_num_controls, typename t_AMatrix , typename t_BMatrix , etools::MatrixSparsityType::Type t_bmatrix_sparsity_type>
static void	condense (etools::GenericBlockMatrix< t_num_vars, t_num_vars > &S, etools::BlockMatrix< t_num_vars, t_num_controls, t_bmatrix_sparsity_type > &U, const std::vector< t_AMatrix > &A, const std::vector< t_BMatrix > &B)
	Create the condensed matrices (S,U) of a Model Predictive Control problem such that X = Sx0 + Uu. More...

template<typename t_AMatrix , typename t_BMatrix >
static void	condense (Eigen::MatrixXd &S, Eigen::MatrixXd &U, const std::vector< t_AMatrix > &A, const std::vector< t_BMatrix > &B)
	Create the condensed matrices (S,U) of a Model Predictive Control problem such that X = Sx0 + Uu. More...

template<int t_num_vars, int t_num_outputs, typename t_DMatrix , typename t_EMatrix >
static void	condenseOutput (etools::GenericBlockMatrix< t_num_outputs, t_num_vars > &Ox, std::vector< etools::LeftLowerTriangularBlockMatrix< t_num_outputs, etools::MatrixBlockSizeType::DYNAMIC > > &Ou, const std::vector< t_DMatrix > &D, const std::vector< t_EMatrix > &E, const etools::GenericBlockMatrix< t_num_vars, t_num_vars > &Ux, const std::vector< etools::LeftLowerTriangularBlockMatrix< t_num_vars, etools::MatrixBlockSizeType::DYNAMIC > > &Uu)
	Condense output of the system. More...

template<typename t_DMatrix , typename t_EMatrix >
static void	condenseOutput (Eigen::MatrixXd &Ox, Eigen::MatrixXd &Ou, const t_DMatrix &D, const t_EMatrix &E, const Eigen::MatrixXd &Ux, const Eigen::MatrixXd &Uu)
	Condense output of the system. More...

template<int t_num_vars, int t_num_controls, int t_num_outputs, typename t_DMatrix , typename t_EMatrix >
static void	condenseOutput (etools::GenericBlockMatrix< t_num_outputs, t_num_vars > &Ox, etools::LeftLowerTriangularBlockMatrix< t_num_outputs, t_num_controls > &Ou, const std::vector< t_DMatrix > &D, const std::vector< t_EMatrix > &E, const etools::GenericBlockMatrix< t_num_vars, t_num_vars > &Ux, const etools::LeftLowerTriangularBlockMatrix< t_num_vars, t_num_controls > &Uu)
	Condense output of the system. More...

static void	condenseTimeInvariant (Eigen::MatrixXd &S, Eigen::MatrixXd &U, const std::size_t preview_horizon_len, const Eigen::MatrixXd &A, const Eigen::MatrixXd &B)
	Create the condensed matrices (S,U) of a Time Invariant (constant A,B) Model Predictive Control problem such that X = Sx0 + Uu. More...

Protected Attributes
SolutionStructure	sol_structure_

Private Member Functions
void	formFootPosMatrices (const humoto::wpg04::Model &model)
	Create the foot position matrices. More...

void	formRotationMatrices ()
	Create the rotation matrices of the MPC problem. More...

void	getFootLandingState (humoto::rigidbody::RigidBodyState &foot_state, const humoto::wpg04::Model &model, const humoto::LeftOrRight::Type landing_foot) const
	Determine landing state of a foot. More...

humoto::wpg04::ModelState	initializeNextModelState (const humoto::walking::StanceFiniteStateMachine &stance_fsm, const humoto::wpg04::Model &model) const
	Initialize next model state. More...

Private Attributes
humoto::wpg04::MPCParameters	mpc_parameters_

bool	solution_is_parsed_

Constructor & Destructor Documentation

◆ MPCforWPG() [1/2]

humoto::wpg04::MPCforWPG::MPCforWPG ( )

inline

Constructor.

Definition at line 261 of file mpc_wpg.h.

◆ MPCforWPG() [2/2]

humoto::wpg04::MPCforWPG::MPCforWPG ( const humoto::wpg04::MPCParameters & mpc_parameters )

inlineexplicit

Constructor.

Parameters

[in] mpc_parameters parameters of the MPC

Definition at line 272 of file mpc_wpg.h.

Member Function Documentation

◆ condense() [1/2]

template<int t_num_vars, int t_num_controls, typename t_AMatrix , typename t_BMatrix , etools::MatrixSparsityType::Type t_bmatrix_sparsity_type>

static void humoto::MPC::condense	(	etools::GenericBlockMatrix< t_num_vars, t_num_vars > &	S,
		etools::BlockMatrix< t_num_vars, t_num_controls, t_bmatrix_sparsity_type > &	U,
		const std::vector< t_AMatrix > &	A,
		const std::vector< t_BMatrix > &	B
	)

inlinestaticprotectedinherited

Create the condensed matrices (S,U) of a Model Predictive Control problem such that X = S*x0 + U*u.

Template Parameters

t_num_vars	number of variables
t_num_controls	number of controls
t_AMatrix	A matrix, should be an Eigen matrix or a scalar
t_BMatrix	B matrix, should be an Eigen matrix

Parameters

[out]	S
[out]	U
[in]	A
[in]	B

Definition at line 321 of file control_problem.h.

◆ condense() [2/2]

template<typename t_AMatrix , typename t_BMatrix >

static void humoto::MPC::condense	(	Eigen::MatrixXd &	S,
		Eigen::MatrixXd &	U,
		const std::vector< t_AMatrix > &	A,
		const std::vector< t_BMatrix > &	B
	)

inlinestaticprotectedinherited

Create the condensed matrices (S,U) of a Model Predictive Control problem such that X = S*x0 + U*u.

Template Parameters

t_AMatrix	A matrix, should be an Eigen matrix or a scalar
t_BMatrix	B matrix, should be an Eigen matrix

Parameters

[out]	S
[out]	U
[in]	A
[in]	B

Definition at line 378 of file control_problem.h.

◆ condenseOutput() [1/3]

template<int t_num_vars, int t_num_outputs, typename t_DMatrix , typename t_EMatrix >

static void humoto::MPC::condenseOutput	(	etools::GenericBlockMatrix< t_num_outputs, t_num_vars > &	Ox,
		std::vector< etools::LeftLowerTriangularBlockMatrix< t_num_outputs, etools::MatrixBlockSizeType::DYNAMIC > > &	Ou,
		const std::vector< t_DMatrix > &	D,
		const std::vector< t_EMatrix > &	E,
		const etools::GenericBlockMatrix< t_num_vars, t_num_vars > &	Ux,
		const std::vector< etools::LeftLowerTriangularBlockMatrix< t_num_vars, etools::MatrixBlockSizeType::DYNAMIC > > &	Uu
	)

inlinestaticprotectedinherited

Condense output of the system.

output = Ox*x0 + Ou*(u0,...,uN)

Template Parameters

t_num_vars	number of state variables
t_num_outputs	number of output variables
t_DMatrix	D matrix, should be an Eigen matrix or a scalar
t_EMatrix	E matrix, should be an Eigen matrix or a scalar

Parameters

[out]	Ox	matrix Ox
[out]	Ou	vector of Ou matrices
[in]	D	vector of D matrices
[in]	E	vector of E matrices
[in]	Ux	matrix Ux
[in]	Uu	vector of Uu matrices

Definition at line 129 of file control_problem.h.

◆ condenseOutput() [2/3]

template<typename t_DMatrix , typename t_EMatrix >

static void humoto::MPC::condenseOutput	(	Eigen::MatrixXd &	Ox,
		Eigen::MatrixXd &	Ou,
		const t_DMatrix &	D,
		const t_EMatrix &	E,
		const Eigen::MatrixXd &	Ux,
		const Eigen::MatrixXd &	Uu
	)

inlinestaticprotectedinherited

Condense output of the system.

output = Ox*x0 + Ou*(u0,...,uN)

Template Parameters

t_num_vars	number of state variables
t_num_controls	number of control variables
t_num_outputs	number of output variables
t_DMatrix	D matrix, should be an Eigen matrix or a scalar
t_EMatrix	E matrix, should be an Eigen matrix or a scalar

Parameters

[out]	Ox	matrix Ox
[out]	Ou	matrix Ou
[in]	D	D matrix
[in]	E	E matrix
[in]	Ux	matrix Ux
[in]	Uu	matrix Uu

Definition at line 209 of file control_problem.h.

◆ condenseOutput() [3/3]

template<int t_num_vars, int t_num_controls, int t_num_outputs, typename t_DMatrix , typename t_EMatrix >

static void humoto::MPC::condenseOutput	(	etools::GenericBlockMatrix< t_num_outputs, t_num_vars > &	Ox,
		etools::LeftLowerTriangularBlockMatrix< t_num_outputs, t_num_controls > &	Ou,
		const std::vector< t_DMatrix > &	D,
		const std::vector< t_EMatrix > &	E,
		const etools::GenericBlockMatrix< t_num_vars, t_num_vars > &	Ux,
		const etools::LeftLowerTriangularBlockMatrix< t_num_vars, t_num_controls > &	Uu
	)

inlinestaticprotectedinherited

Condense output of the system.

output = Ox*x0 + Ou*(u0,...,uN)

Template Parameters

t_num_vars	number of state variables
t_num_controls	number of control variables
t_num_outputs	number of output variables
t_DMatrix	D matrix, should be an Eigen matrix or a scalar
t_EMatrix	E matrix, should be an Eigen matrix or a scalar

Parameters

[out]	Ox	matrix Ox
[out]	Ou	matrix Ou
[in]	D	vector of D matrices
[in]	E	vector of E matrices
[in]	Ux	matrix Ux
[in]	Uu	matrix Uu

Definition at line 268 of file control_problem.h.

◆ condenseTimeInvariant()

static void humoto::MPC::condenseTimeInvariant	(	Eigen::MatrixXd &	S,
		Eigen::MatrixXd &	U,
		const std::size_t	preview_horizon_len,
		const Eigen::MatrixXd &	A,
		const Eigen::MatrixXd &	B
	)

inlinestaticprotectedinherited

Create the condensed matrices (S,U) of a Time Invariant (constant A,B) Model Predictive Control problem such that X = S*x0 + U*u.

Parameters

[out]	S
[out]	U
[in]	preview_horizon_len	length of the preview horizon
[in]	A
[in]	B

Definition at line 428 of file control_problem.h.

◆ formFootPosMatrices()

void humoto::wpg04::MPCforWPG::formFootPosMatrices ( const humoto::wpg04::Model & model )

inlineprivate

Create the foot position matrices.

Parameters

[in] model

Definition at line 61 of file mpc_wpg.h.

◆ formRotationMatrices()

void humoto::wpg04::MPCforWPG::formRotationMatrices ( )

inlineprivate

Create the rotation matrices of the MPC problem.

Definition at line 32 of file mpc_wpg.h.

◆ getCoMState()

humoto::rigidbody::PointMassState humoto::wpg04::MPCforWPG::getCoMState	(	const humoto::wpg04::Model &	model,
		const std::size_t	time_instant_ms
	)

inline

Computes CoM state at the given time instant.

Parameters

[in]	model	model
[in]	time_instant_ms	time instant

Returns: CoM state.

Definition at line 555 of file mpc_wpg.h.

◆ getFootLandingState() [1/2]

void humoto::wpg04::MPCforWPG::getFootLandingState	(	humoto::rigidbody::RigidBodyState &	foot_state,
		const humoto::wpg04::Model &	model,
		const humoto::LeftOrRight::Type	landing_foot
	)		const

inlineprivate

Determine landing state of a foot.

Parameters

[out]	foot_state	state of the landing foot
[in]	model	model
[in]	landing_foot	left or right

Definition at line 181 of file mpc_wpg.h.

◆ getFootLandingState() [2/2]

humoto::rigidbody::RigidBodyState humoto::wpg04::MPCforWPG::getFootLandingState ( const humoto::wpg04::Model & model ) const

inline

Get landing state of a foot.

Parameters

[in] model model

Returns: state of the foot

Definition at line 467 of file mpc_wpg.h.

◆ getNextModelState() [1/2]

humoto::wpg04::ModelState humoto::wpg04::MPCforWPG::getNextModelState	(	const humoto::Solution &	solution,
		const humoto::walking::StanceFiniteStateMachine &	stance_fsm,
		const humoto::wpg04::Model &	model
	)

inline

Get next model state.

Parameters

[in]	solution	solution
[in]	stance_fsm	walking finite state machine (must be updated before)
[in]	model	model

Returns: next model state.

Definition at line 431 of file mpc_wpg.h.

◆ getNextModelState() [2/2]

humoto::wpg04::ModelState humoto::wpg04::MPCforWPG::getNextModelState	(	const humoto::walking::StanceFiniteStateMachine &	stance_fsm,
		const humoto::wpg04::Model &	model
	)		const

inline

Get next model state.

Parameters

[in]	stance_fsm	walking finite state machine (must be updated before)
[in]	model	model

Returns: next model state.

Definition at line 450 of file mpc_wpg.h.

◆ getPreviewHorizonLength()

std::size_t humoto::wpg04::MPCforWPG::getPreviewHorizonLength ( ) const

inline

Returns length of the preview horizon.

Returns: length of preview horizon

Definition at line 613 of file mpc_wpg.h.

◆ guessSolution()

void humoto::wpg04::MPCforWPG::guessSolution	(	Solution &	solution_guess,
		const Solution &	old_solution
	)		const

inlinevirtual

Guess solution.

Parameters

[out]	solution_guess	solution guess
[in]	old_solution	old solution

Reimplemented from humoto::ControlProblem.

Definition at line 502 of file mpc_wpg.h.

◆ initializeNextModelState()

humoto::wpg04::ModelState humoto::wpg04::MPCforWPG::initializeNextModelState	(	const humoto::walking::StanceFiniteStateMachine &	stance_fsm,
		const humoto::wpg04::Model &	model
	)		const

inlineprivate

Initialize next model state.

Parameters

[in]	stance_fsm	walking finite state machine (must be updated before)
[in]	model	model

Returns: next model state.

Definition at line 95 of file mpc_wpg.h.

◆ initSolutionStructure()

void humoto::ControlProblem::initSolutionStructure ( humoto::Solution & solution ) const

inlineinherited

Initialize structure of the given solution based on the internally stored solution structure.

Parameters

[out] solution solution

Attention: This method is called automatically.

Definition at line 64 of file control_problem.h.

◆ log()

void humoto::wpg04::MPCforWPG::log	(	humoto::Logger &	logger,
		const LogEntryName &	parent = `LogEntryName()`,
		const std::string &	name = `"mpcwpg"`
	)		const

inlinevirtual

Log.

Parameters

[in,out]	logger	logger
[in]	parent	parent
[in]	name	name

Implements humoto::ControlProblem.

Definition at line 626 of file mpc_wpg.h.

◆ parseSolution()

void humoto::wpg04::MPCforWPG::parseSolution ( const humoto::Solution & solution )

inline

Process solution.

Parameters

[in] solution solution

Definition at line 400 of file mpc_wpg.h.

◆ setParameters()

void humoto::wpg04::MPCforWPG::setParameters ( const humoto::wpg04::MPCParameters & mpc_parameters )

inline

Set parameters.

Parameters

[in] mpc_parameters

Definition at line 285 of file mpc_wpg.h.

◆ update()

ControlProblemStatus::Status humoto::wpg04::MPCforWPG::update	(	const humoto::wpg04::Model &	model,
		const humoto::walking::StanceFiniteStateMachine &	stance_fsm,
		const humoto::wpg04::WalkParameters &	walk_parameters
	)

inline

Update control problem.

Parameters

[in]	model	model of the system
[in]	stance_fsm	walking finite state machine
[in]	walk_parameters

Returns: ControlProblemStatus::OK/ControlProblemStatusSTOPPED

Definition at line 301 of file mpc_wpg.h.

Member Data Documentation

◆ cop_profile

Eigen::VectorXd humoto::wpg04::MPCforWPG::cop_profile

Definition at line 251 of file mpc_wpg.h.

◆ cstate_profile

Eigen::VectorXd humoto::wpg04::MPCforWPG::cstate_profile

Definition at line 253 of file mpc_wpg.h.

◆ dcop_profile

Eigen::VectorXd humoto::wpg04::MPCforWPG::dcop_profile

Definition at line 252 of file mpc_wpg.h.

◆ footpos_profile

Eigen::VectorXd humoto::wpg04::MPCforWPG::footpos_profile

Definition at line 254 of file mpc_wpg.h.

◆ Ir_

Eigen::MatrixXd humoto::wpg04::MPCforWPG::Ir_

Definition at line 236 of file mpc_wpg.h.

◆ mpc_parameters_

humoto::wpg04::MPCParameters humoto::wpg04::MPCforWPG::mpc_parameters_

private

Definition at line 25 of file mpc_wpg.h.

◆ preview_horizon_

humoto::wpg04::PreviewHorizon humoto::wpg04::MPCforWPG::preview_horizon_

Definition at line 230 of file mpc_wpg.h.

◆ R_

etools::DiagonalBlockMatrix<2,2> humoto::wpg04::MPCforWPG::R_

Definition at line 233 of file mpc_wpg.h.

◆ Rh_

etools::DiagonalBlockMatrix<2,2> humoto::wpg04::MPCforWPG::Rh_

Definition at line 234 of file mpc_wpg.h.

◆ S_

Eigen::MatrixXd humoto::wpg04::MPCforWPG::S_

Definition at line 242 of file mpc_wpg.h.

◆ s_

Eigen::MatrixXd humoto::wpg04::MPCforWPG::s_

Definition at line 243 of file mpc_wpg.h.

◆ Sdz_

Eigen::MatrixXd humoto::wpg04::MPCforWPG::Sdz_

Definition at line 247 of file mpc_wpg.h.

◆ sdz_

Eigen::MatrixXd humoto::wpg04::MPCforWPG::sdz_

Definition at line 248 of file mpc_wpg.h.

◆ sol_structure_

SolutionStructure humoto::ControlProblem::sol_structure_

protectedinherited

Definition at line 43 of file control_problem.h.

◆ solution_is_parsed_

bool humoto::wpg04::MPCforWPG::solution_is_parsed_

private

Definition at line 24 of file mpc_wpg.h.

◆ V0_

Eigen::MatrixXd humoto::wpg04::MPCforWPG::V0_

Definition at line 238 of file mpc_wpg.h.

◆ V_

Eigen::MatrixXd humoto::wpg04::MPCforWPG::V_

Definition at line 237 of file mpc_wpg.h.

◆ velocity_selector_

etools::SelectionMatrix humoto::wpg04::MPCforWPG::velocity_selector_

Definition at line 245 of file mpc_wpg.h.

◆ Vfp_

Eigen::MatrixXd humoto::wpg04::MPCforWPG::Vfp_

Definition at line 239 of file mpc_wpg.h.

◆ vfp_

Eigen::MatrixXd humoto::wpg04::MPCforWPG::vfp_

Definition at line 240 of file mpc_wpg.h.

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

modules/wpg04/include/wpg04/mpc_wpg.h

Detailed Description

Public Member Functions

Public Attributes

Static Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Constructor & Destructor Documentation

◆ MPCforWPG() [1/2]

◆ MPCforWPG() [2/2]

Member Function Documentation

◆ condense() [1/2]

◆ condense() [2/2]

◆ condenseOutput() [1/3]

◆ condenseOutput() [2/3]

◆ condenseOutput() [3/3]

◆ condenseTimeInvariant()

◆ formFootPosMatrices()

◆ formRotationMatrices()

◆ getCoMState()

◆ getFootLandingState() [1/2]

◆ getFootLandingState() [2/2]

◆ getNextModelState() [1/2]

◆ getNextModelState() [2/2]

◆ getPreviewHorizonLength()

◆ guessSolution()

◆ initializeNextModelState()

◆ initSolutionStructure()

◆ log()

◆ parseSolution()

◆ setParameters()

◆ update()

Member Data Documentation

◆ cop_profile

◆ cstate_profile

◆ dcop_profile

◆ footpos_profile

◆ Ir_

◆ mpc_parameters_

◆ preview_horizon_

◆ R_

◆ Rh_

◆ S_

◆ s_

◆ Sdz_

◆ sdz_

◆ sol_structure_

◆ solution_is_parsed_

◆ V0_

◆ V_

◆ velocity_selector_

◆ Vfp_

◆ vfp_