BipedalLocomotion::ReducedModelControllers::CentroidalMPC class
#include <BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h>

CentroidalMPC implements a Non-Linear Model Predictive Controller for humanoid robot locomotion with online step adjustment capabilities.

The proposed controller considers the Centroidal Dynamics of the system to compute the desired contact forces and torques and contact locations. Let us assume the presence of a high-level contact planner that generates only the contact location and timings, the CentroidalMPC objective is to implement a control law that generates feasible contact wrenches and locations while considering the Centroidal dynamics of the floating base system, and a nominal set of contact positions and timings. The control problem is formulated using the Model Predictive Control (MPC) framework.

Base classes

template<class Output> class BipedalLocomotion::System::Source<CentroidalMPCOutput>: Source is a template specialization of Advanceable and represents a block that does not contains input.

Constructors, destructors, conversion operators

CentroidalMPC(): Constructor.
~CentroidalMPC(): Destructor.

Public functions

auto initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) -> bool final: Initialize the controller.
auto setContactPhaseList(const Contacts::ContactPhaseList& contactPhaseList) -> bool: Set the contact phase list considered by the controller as nominal contact location.
auto setState(Eigen::Ref<const Eigen::Vector3d> com, Eigen::Ref<const Eigen::Vector3d> dcom, Eigen::Ref<const Eigen::Vector3d> angularMomentum) -> bool: Set the state of the centroidal dynamics.
auto setState(Eigen::Ref<const Eigen::Vector3d> com, Eigen::Ref<const Eigen::Vector3d> dcom, Eigen::Ref<const Eigen::Vector3d> angularMomentum, const Math::Wrenchd& externalWrench) -> bool: Set the state of the centroidal dynamics.
auto setReferenceTrajectory(const std::vector<Eigen::Vector3d>& com, const std::vector<Eigen::Vector3d>& angularMomentum) -> bool: Set the reference trajectories for the CoM and the centroidal angular momentum.
auto getOutput() const -> const CentroidalMPCOutput& final: Get the output of the controller.
auto isOutputValid() const -> bool final: Determines the validity of the object retrieved with getOutput()
auto advance() -> bool final: Perform one control cycle.

Function documentation

bool BipedalLocomotion::ReducedModelControllers::CentroidalMPC::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) final

Initialize the controller.

Parameters
handler	pointer to the parameter handler.
Returns	true in case of success/false otherwise.

Note

the following parameters are required by the class

Parameter Name	Type	Description	Mandatory
`sampling_time`	`double`	Sampling time of the MPC.	Yes
`time_horizon`	`double`	The time horizon of the MPC. The number of knots will be given by `floor(time_horizon / sampling_time)`	Yes
`number_of_maximum_contacts`	`int`	Integer representing the maximum number of contacts that can be established. For a bipedal is in general 2 (the feet) for a quadruped 4.	Yes
`com_weight`	`vector<double>`	Weight of the CoM in the cost function. The Vector must contain three elements that will correspond to the weight in the x y and z coordinates.	Yes
`contact_position_weight`	`double`	Weight related to the contact position regularization provided by the CentroidalMPC::setContactPhaseList	Yes
`force_rate_of_change_weight`	`vector<double>`	Weight associated to the rate of change of the contact forces. The higher the weight, the more the contact forces will be smooth.	Yes
`angular_momentum_weight`	`double`	Weight associated to the angular momentum. The higher the weight, the more the angular momentum will follow the desired one.	Yes
`contact_force_symmetry_weight`	`double`	Weight associated to the symmetry of the contact forces. The higher the weight, the more the contact forces associated to the same contact will be symmetric	Yes
`linear_solver`	`string`	Linear solver used by ipopt. Please check https://coin-or.github.io/Ipopt/#PREREQUISITES for the available solvers (default `mumps`).	Yes
`ipopt_tolerance`	`double`	Determines the convergence tolerance for the algorithm (default value is (https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_tol).	No
`ipopt_max_iteration`	`int`	The maximum number of iterations of ipopt (The default value is 3000).	No
`solver_verbosity`	`int`	Verbosity of the solver. The higher the value, the higher the verbosity (Default value is `0`)	No
`is_warm_start_enabled`	`bool`	True if the user wants to warm start the CoM, angular momentum, and contact location with the nominal value (Default `false`)	No
`is_cse_enabled`	`bool`	True if the Common subexpression elimination casadi option is enabled. This option is supported only by casadi 3.6.0 https://github.com/casadi/casadi/releases/tag/3.6.3 (Default `false` )	No

Moreover for each contact where number_of_maximum_contacts-1 it is required to define a group CONTACT_<i> that contains the following parameters

Parameter Name	Type	Description	Mandatory
`contact_name`	`string`	Name associated to the contact.	Yes
`bounding_box_upper_limit`	`vector<double>`	Upper limit of the bounding box where the adjusted contact must belong to. The limits are expressed in the contact local frame.	Yes
`bounding_box_lower_limit`	`vector<double>`	Lower limit of the bounding box where the adjusted contact must belong to. The limits are expressed in the contact local frame.	Yes
`number_of_corners`	`int`	Number of corners associated to the foot.	Yes
`corner_<j>`	`vector<double>`	Position of the corner expressed in the foot frame. I must be from 0 to number_of_corners - 1.	Yes

bool BipedalLocomotion::ReducedModelControllers::CentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList& contactPhaseList)

Set the contact phase list considered by the controller as nominal contact location.

Parameters
contactPhaseList	contact phase.
Returns	True in case of success, false otherwise.

bool BipedalLocomotion::ReducedModelControllers::CentroidalMPC::setState(Eigen::Ref<const Eigen::Vector3d> com, Eigen::Ref<const Eigen::Vector3d> dcom, Eigen::Ref<const Eigen::Vector3d> angularMomentum)

Set the state of the centroidal dynamics.

Parameters
com	position of the CoM expressed in the inertial frame.
dcom	velocity of the CoM expressed in a frame centered in the CoM and oriented as the inertial frame.
angularMomentum	centroidal angular momentum.
Returns	True in case of success, false otherwise.

bool BipedalLocomotion::ReducedModelControllers::CentroidalMPC::setState(Eigen::Ref<const Eigen::Vector3d> com, Eigen::Ref<const Eigen::Vector3d> dcom, Eigen::Ref<const Eigen::Vector3d> angularMomentum, const Math::Wrenchd& externalWrench)

Set the state of the centroidal dynamics.

Parameters
com	position of the CoM expressed in the inertial frame.
dcom	velocity of the CoM expressed in a frame centered in the CoM and oriented as the inertial frame.
angularMomentum	centroidal angular momentum.
externalWrench	optional parameter used to represent an external wrench applied to the robot CoM.
Returns	True in case of success, false otherwise.

bool BipedalLocomotion::ReducedModelControllers::CentroidalMPC::setReferenceTrajectory(const std::vector<Eigen::Vector3d>& com, const std::vector<Eigen::Vector3d>& angularMomentum)

Set the reference trajectories for the CoM and the centroidal angular momentum.

Parameters
com	desired trajectory of the CoM. The rows contain the x, y and z coordinates while the columns the position at each time instant.
angularMomentum	centroidal angular momentum. The rows contain the x, y and z coordinates while the columns the trajectory at each time instant.
Returns	True in case of success, false otherwise.

const CentroidalMPCOutput& BipedalLocomotion::ReducedModelControllers::CentroidalMPC::getOutput() const final

Get the output of the controller.

Returns	a const reference of the output of the controller.

bool BipedalLocomotion::ReducedModelControllers::CentroidalMPC::isOutputValid() const final

Determines the validity of the object retrieved with getOutput()

Returns	True if the object is valid, false otherwise.

bool BipedalLocomotion::ReducedModelControllers::CentroidalMPC::advance() final

Perform one control cycle.

Returns	True if the advance is successfull.