humoto
Public Member Functions | Public Attributes | Static Public Attributes | Static Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes | List of all members
humoto::pepper_mpc::MPCforMG Class Reference

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 20 of file mpc_mg.h.

#include <mpc_mg.h>

Inheritance diagram for humoto::pepper_mpc::MPCforMG:
Inheritance graph

Public Member Functions

 MPCforMG ()
 Constructor. More...
 
 MPCforMG (const humoto::pepper_mpc::MPCParameters &mpc_parameters)
 Constructor. More...
 
humoto::pepper_mpc::ModelState getModelState (const humoto::pepper_mpc::Model &model, const std::size_t time_instant_ms)
 Computes model state at the given time instant. More...
 
humoto::pepper_mpc::ModelState getModelState (const humoto::pepper_mpc::Model &model, const double time_instant)
 Computes model state at the given time instant. More...
 
humoto::pepper_mpc::ModelState getNextModelState (const humoto::Solution &solution, const humoto::pepper_mpc::Model &model)
 Get next model state. More...
 
humoto::pepper_mpc::ModelState getNextModelState (const humoto::pepper_mpc::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="mpcmg") const
 Log. More...
 
void parseSolution (const humoto::Solution &solution)
 Process solution. More...
 
void setParameters (const humoto::pepper_mpc::MPCParameters &mpc_parameters)
 Set parameters. More...
 
void shift (MotionParameters &motion_parameters, const std::ptrdiff_t time_shift_ms=DEFAULT_PREVIEW_HORIZON_SHIFT)
 Shift preview horizon. More...
 
void shift (std::deque< MotionParameters > &motion_parameters_deque, const std::ptrdiff_t time_shift_ms=DEFAULT_PREVIEW_HORIZON_SHIFT)
 Update control problem. More...
 
template<class t_MotionParameters >
ControlProblemStatus::Status update (const t_MotionParameters &motion_parameters, const Model &model)
 Update control problem. More...
 
template<class t_MotionParameters >
ControlProblemStatus::Status updateAndShift (t_MotionParameters &motion_parameters, const Model &model, const std::ptrdiff_t time_shift_ms=DEFAULT_PREVIEW_HORIZON_SHIFT)
 Update control problem & shift preview horizon. More...
 

Public Attributes

Eigen::MatrixXd Aas_
 
Eigen::MatrixXd Ajs_
 
Eigen::MatrixXd Ap_
 
Eigen::MatrixXd Apd_
 
Eigen::MatrixXd Aps_
 
Eigen::VectorXd base_controls_
 
Eigen::VectorXd base_jerk_profile_
 
Eigen::VectorXd body_controls_
 
Eigen::VectorXd cop_profile_
 
Eigen::VectorXd cstate_profile_
 
humoto::pepper_mpc::MPCParameters mpc_parameters_
 
humoto::pepper_mpc::PreviewHorizon preview_horizon_
 
etools::DiagonalBlockMatrix< 2, 2 > R_
 
Eigen::MatrixXd S_
 
Eigen::VectorXd s_
 
Eigen::VectorXd sas_
 
Eigen::VectorXd sjs_
 
Eigen::VectorXd sp_
 
Eigen::VectorXd spd_
 
Eigen::VectorXd sps_
 

Static Public Attributes

static const std::ptrdiff_t DEFAULT_PREVIEW_HORIZON_SHIFT = -1
 

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 = S*x0 + U*u. 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 = S*x0 + U*u. 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 = S*x0 + U*u. More...
 

Protected Attributes

SolutionStructure sol_structure_
 

Private Member Functions

void formRotationMatrices ()
 Create the rotation matrices of the MPC problem. More...
 
humoto::pepper_mpc::ModelState getModelState (const humoto::pepper_mpc::Model &model, const std::size_t interval_index, const double interval_offset)
 Computes model state at the given time instant. More...
 
humoto::pepper_mpc::ModelState initializeNextModelState (const humoto::pepper_mpc::Model &model) const
 Initialize next model state. More...
 
void updateMatrices (const Model &model)
 Condense and recompute all dependent matrices. More...
 

Private Attributes

bool solution_is_parsed_
 

Constructor & Destructor Documentation

◆ MPCforMG() [1/2]

humoto::pepper_mpc::MPCforMG::MPCforMG ( )
inline

Constructor.

Definition at line 380 of file mpc_mg.h.

◆ MPCforMG() [2/2]

humoto::pepper_mpc::MPCforMG::MPCforMG ( const humoto::pepper_mpc::MPCParameters mpc_parameters)
inlineexplicit

Constructor.

Parameters
[in]mpc_parametersparameters of the MPC

Definition at line 391 of file mpc_mg.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_varsnumber of variables
t_num_controlsnumber of controls
t_AMatrixA matrix, should be an Eigen matrix or a scalar
t_BMatrixB 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_AMatrixA matrix, should be an Eigen matrix or a scalar
t_BMatrixB 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_varsnumber of state variables
t_num_outputsnumber of output variables
t_DMatrixD matrix, should be an Eigen matrix or a scalar
t_EMatrixE matrix, should be an Eigen matrix or a scalar
Parameters
[out]Oxmatrix Ox
[out]Ouvector of Ou matrices
[in]Dvector of D matrices
[in]Evector of E matrices
[in]Uxmatrix Ux
[in]Uuvector 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_varsnumber of state variables
t_num_controlsnumber of control variables
t_num_outputsnumber of output variables
t_DMatrixD matrix, should be an Eigen matrix or a scalar
t_EMatrixE matrix, should be an Eigen matrix or a scalar
Parameters
[out]Oxmatrix Ox
[out]Oumatrix Ou
[in]DD matrix
[in]EE matrix
[in]Uxmatrix Ux
[in]Uumatrix 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_varsnumber of state variables
t_num_controlsnumber of control variables
t_num_outputsnumber of output variables
t_DMatrixD matrix, should be an Eigen matrix or a scalar
t_EMatrixE matrix, should be an Eigen matrix or a scalar
Parameters
[out]Oxmatrix Ox
[out]Oumatrix Ou
[in]Dvector of D matrices
[in]Evector of E matrices
[in]Uxmatrix Ux
[in]Uumatrix 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_lenlength of the preview horizon
[in]A
[in]B

Definition at line 428 of file control_problem.h.

◆ formRotationMatrices()

void humoto::pepper_mpc::MPCforMG::formRotationMatrices ( )
inlineprivate

Create the rotation matrices of the MPC problem.

Definition at line 30 of file mpc_mg.h.

◆ getModelState() [1/3]

humoto::pepper_mpc::ModelState humoto::pepper_mpc::MPCforMG::getModelState ( const humoto::pepper_mpc::Model model,
const std::size_t  interval_index,
const double  interval_offset 
)
inlineprivate

Computes model state at the given time instant.

Parameters
[in]modelmodel
[in]interval_indexindex of an interval in the preview horizon
[in]interval_offsetoffset from the start of the given interval
Returns
model state.

Definition at line 275 of file mpc_mg.h.

◆ getModelState() [2/3]

humoto::pepper_mpc::ModelState humoto::pepper_mpc::MPCforMG::getModelState ( const humoto::pepper_mpc::Model model,
const std::size_t  time_instant_ms 
)
inline

Computes model state at the given time instant.

Parameters
[in]modelmodel
[in]time_instant_mstime instant
Returns
model state.

Definition at line 659 of file mpc_mg.h.

◆ getModelState() [3/3]

humoto::pepper_mpc::ModelState humoto::pepper_mpc::MPCforMG::getModelState ( const humoto::pepper_mpc::Model model,
const double  time_instant 
)
inline

Computes model state at the given time instant.

Parameters
[in]modelmodel
[in]time_instantdouble time instant
Returns
model state.

Definition at line 680 of file mpc_mg.h.

◆ getNextModelState() [1/2]

humoto::pepper_mpc::ModelState humoto::pepper_mpc::MPCforMG::getNextModelState ( const humoto::Solution solution,
const humoto::pepper_mpc::Model model 
)
inline

Get next model state.

Parameters
[in]solutionsolution
[in]modelmodel
Returns
next model state.

Definition at line 581 of file mpc_mg.h.

◆ getNextModelState() [2/2]

humoto::pepper_mpc::ModelState humoto::pepper_mpc::MPCforMG::getNextModelState ( const humoto::pepper_mpc::Model model) const
inline

Get next model state.

Parameters
[in]modelmodel
Returns
next model state.

Definition at line 598 of file mpc_mg.h.

◆ getPreviewHorizonLength()

std::size_t humoto::pepper_mpc::MPCforMG::getPreviewHorizonLength ( ) const
inline

Returns length of the preview horizon.

Returns
length of preview horizon

Definition at line 699 of file mpc_mg.h.

◆ guessSolution()

void humoto::pepper_mpc::MPCforMG::guessSolution ( Solution solution_guess,
const Solution old_solution 
) const
inlinevirtual

Guess solution.

Parameters
[in]old_solutionold solution
[out]solution_guesssolution guess

Reimplemented from humoto::ControlProblem.

Definition at line 613 of file mpc_mg.h.

◆ initializeNextModelState()

humoto::pepper_mpc::ModelState humoto::pepper_mpc::MPCforMG::initializeNextModelState ( const humoto::pepper_mpc::Model model) const
inlineprivate

Initialize next model state.

Parameters
[in]modelmodel
Returns
next model state.

Definition at line 49 of file mpc_mg.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]solutionsolution
Attention
This method is called automatically.

Definition at line 64 of file control_problem.h.

◆ log()

void humoto::pepper_mpc::MPCforMG::log ( humoto::Logger logger,
const LogEntryName parent = LogEntryName(),
const std::string &  name = "mpcmg" 
) const
inlinevirtual

Log.

Parameters
[in,out]loggerlogger
[in]parentparent
[in]namename

Implements humoto::ControlProblem.

Definition at line 713 of file mpc_mg.h.

◆ parseSolution()

void humoto::pepper_mpc::MPCforMG::parseSolution ( const humoto::Solution solution)
inline

Process solution.

Parameters
[in]solutionsolution

Definition at line 559 of file mpc_mg.h.

◆ setParameters()

void humoto::pepper_mpc::MPCforMG::setParameters ( const humoto::pepper_mpc::MPCParameters mpc_parameters)
inline

Set parameters.

Parameters
[in]mpc_parameters

Definition at line 403 of file mpc_mg.h.

◆ shift() [1/2]

void humoto::pepper_mpc::MPCforMG::shift ( MotionParameters motion_parameters,
const std::ptrdiff_t  time_shift_ms = DEFAULT_PREVIEW_HORIZON_SHIFT 
)
inline

Shift preview horizon.

Parameters
[in,out]motion_parametersduration_ms parameter is reduced by time_shift_ms
[in]time_shift_msshift of the preview horizon on the next iteration

Definition at line 450 of file mpc_mg.h.

◆ shift() [2/2]

void humoto::pepper_mpc::MPCforMG::shift ( std::deque< MotionParameters > &  motion_parameters_deque,
const std::ptrdiff_t  time_shift_ms = DEFAULT_PREVIEW_HORIZON_SHIFT 
)
inline

Update control problem.

Parameters
[in,out]motion_parameters_dequeduration_ms parameter is reduced by time_shift_ms
[in]time_shift_msshift of the preview horizon on the next iteration

Definition at line 478 of file mpc_mg.h.

◆ update()

template<class t_MotionParameters >
ControlProblemStatus::Status humoto::pepper_mpc::MPCforMG::update ( const t_MotionParameters &  motion_parameters,
const Model model 
)
inline

Update control problem.

Template Parameters
t_MotionParametersMotionParameters or deque<MotionParameters>
Parameters
[in]motion_parameters
[in]modelmodel of the system
Returns
ControlProblemStatus::OK/ControlProblemStatusSTOPPED

Definition at line 421 of file mpc_mg.h.

◆ updateAndShift()

template<class t_MotionParameters >
ControlProblemStatus::Status humoto::pepper_mpc::MPCforMG::updateAndShift ( t_MotionParameters &  motion_parameters,
const Model model,
const std::ptrdiff_t  time_shift_ms = DEFAULT_PREVIEW_HORIZON_SHIFT 
)
inline

Update control problem & shift preview horizon.

Template Parameters
t_MotionParametersMotionParameters or deque<MotionParameters>
Parameters
[in,out]motion_parametersduration_ms parameter is reduced by time_shift_ms
[in]modelmodel of the system
[in]time_shift_msshift of the preview horizon on the next iteration
Returns
ControlProblemStatus::OK/ControlProblemStatusSTOPPED

Definition at line 539 of file mpc_mg.h.

◆ updateMatrices()

void humoto::pepper_mpc::MPCforMG::updateMatrices ( const Model model)
inlineprivate

Condense and recompute all dependent matrices.

Parameters
[in]model

Definition at line 80 of file mpc_mg.h.

Member Data Documentation

◆ Aas_

Eigen::MatrixXd humoto::pepper_mpc::MPCforMG::Aas_

Definition at line 349 of file mpc_mg.h.

◆ Ajs_

Eigen::MatrixXd humoto::pepper_mpc::MPCforMG::Ajs_

Definition at line 361 of file mpc_mg.h.

◆ Ap_

Eigen::MatrixXd humoto::pepper_mpc::MPCforMG::Ap_

Definition at line 345 of file mpc_mg.h.

◆ Apd_

Eigen::MatrixXd humoto::pepper_mpc::MPCforMG::Apd_

Definition at line 357 of file mpc_mg.h.

◆ Aps_

Eigen::MatrixXd humoto::pepper_mpc::MPCforMG::Aps_

Definition at line 353 of file mpc_mg.h.

◆ base_controls_

Eigen::VectorXd humoto::pepper_mpc::MPCforMG::base_controls_

Definition at line 370 of file mpc_mg.h.

◆ base_jerk_profile_

Eigen::VectorXd humoto::pepper_mpc::MPCforMG::base_jerk_profile_

Definition at line 367 of file mpc_mg.h.

◆ body_controls_

Eigen::VectorXd humoto::pepper_mpc::MPCforMG::body_controls_

Definition at line 371 of file mpc_mg.h.

◆ cop_profile_

Eigen::VectorXd humoto::pepper_mpc::MPCforMG::cop_profile_

Definition at line 366 of file mpc_mg.h.

◆ cstate_profile_

Eigen::VectorXd humoto::pepper_mpc::MPCforMG::cstate_profile_

Definition at line 365 of file mpc_mg.h.

◆ DEFAULT_PREVIEW_HORIZON_SHIFT

const std::ptrdiff_t humoto::pepper_mpc::MPCforMG::DEFAULT_PREVIEW_HORIZON_SHIFT = -1
static

Definition at line 332 of file mpc_mg.h.

◆ mpc_parameters_

humoto::pepper_mpc::MPCParameters humoto::pepper_mpc::MPCforMG::mpc_parameters_

Definition at line 373 of file mpc_mg.h.

◆ preview_horizon_

humoto::pepper_mpc::PreviewHorizon humoto::pepper_mpc::MPCforMG::preview_horizon_

Definition at line 334 of file mpc_mg.h.

◆ R_

etools::DiagonalBlockMatrix<2,2> humoto::pepper_mpc::MPCforMG::R_

Definition at line 337 of file mpc_mg.h.

◆ S_

Eigen::MatrixXd humoto::pepper_mpc::MPCforMG::S_

Definition at line 340 of file mpc_mg.h.

◆ s_

Eigen::VectorXd humoto::pepper_mpc::MPCforMG::s_

Definition at line 341 of file mpc_mg.h.

◆ sas_

Eigen::VectorXd humoto::pepper_mpc::MPCforMG::sas_

Definition at line 348 of file mpc_mg.h.

◆ sjs_

Eigen::VectorXd humoto::pepper_mpc::MPCforMG::sjs_

Definition at line 360 of file mpc_mg.h.

◆ sol_structure_

SolutionStructure humoto::ControlProblem::sol_structure_
protectedinherited

Definition at line 43 of file control_problem.h.

◆ solution_is_parsed_

bool humoto::pepper_mpc::MPCforMG::solution_is_parsed_
private

Definition at line 23 of file mpc_mg.h.

◆ sp_

Eigen::VectorXd humoto::pepper_mpc::MPCforMG::sp_

Definition at line 344 of file mpc_mg.h.

◆ spd_

Eigen::VectorXd humoto::pepper_mpc::MPCforMG::spd_

Definition at line 356 of file mpc_mg.h.

◆ sps_

Eigen::VectorXd humoto::pepper_mpc::MPCforMG::sps_

Definition at line 352 of file mpc_mg.h.


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