#include <MIQPController.h>

Collaboration diagram for MIQPController:

Public Member Functions
	MIQPController (MIQPParameters params, ocra::Model::Ptr robotModel, std::shared_ptr< StepController > stepController, const Eigen::MatrixXd &comStateRef)
virtual	~MIQPController ()
virtual bool	threadInit ()
virtual void	threadRelease ()
virtual void	run ()
void	setCOMStateRefInPreviewWindow (unsigned int k, Eigen::VectorXd &comStateRef)
void	getSolution (Eigen::VectorXd &X_kn)
Public Attributes
yarp::os::Semaphore	semaphore
Protected Member Functions
void	setLinearPartObjectiveFunction ()
void	setLowerAndUpperBounds ()
void	updateStateVector ()
void	buildAh (int dt, Eigen::MatrixXd &output)
void	buildBh (int dt, Eigen::MatrixXd &output)
void	buildQ (Eigen::MatrixXd &output)
void	buildT (Eigen::MatrixXd &output)
void	buildC_H (Eigen::MatrixXd &output)
void	buildC_P (Eigen::MatrixXd &output)
void	buildC_B (Eigen::MatrixXd &output)
void	buildH_N (Eigen::MatrixXd &output)
void	buildNb (Eigen::MatrixXd &output, double wb)
void	buildNx (Eigen::MatrixXd &output)
void	buildGenericRegMat (MIQP::InputVectorIndex whichVariable, double weight, Eigen::MatrixXd &output)
void	buildSw (Eigen::MatrixXd &output, MIQPParameters miqpParams)
void	buildPreviewStateMatrix (const Eigen::MatrixXd &C, Eigen::MatrixXd &P)
void	buildPreviewInputMatrix (const Eigen::MatrixXd &C, Eigen::MatrixXd &R)
void	buildEqualityConstraintsMatrices (const Eigen::VectorXd &x_k, Eigen::MatrixXd &Aeq, Eigen::VectorXd &Beq)
void	updateEqualityConstraints (const Eigen::VectorXd &x_k, Eigen::VectorXd &Beq)
void	buildRegularizationTerms (MIQPParameters &miqpParams)
void	buildCoMJerkReg (MIQPParameters &miqpParams)
void	buildAvoidOneFootRestReg (MIQPParameters &miqpParams)
void	buildMinimizeSteppingReg (MIQPParameters &miqpParams)
void	setBinaryVariables ()
void	writeToFile (const double &time, const Eigen::VectorXd &X_kn, std::string &home)
Private Attributes
ocra::Model::Ptr	_robotModel
std::shared_ptr< StepController >	_stepController
MIQPParameters	_miqpParams
Eigen::MatrixXd	_comStateRef
int	_period
bool	_addRegularization
Eigen::VectorXd	_linearTermTransObjFunc
Eigen::VectorXd	_lb
Eigen::VectorXd	_ub
std::string	_variablesNames [INPUT_VECTOR_SIZE]
Eigen::VectorXd	_xi_k
Eigen::VectorXd	_X_kn
Eigen::MatrixXd	_Ah
Eigen::MatrixXd	_Bh
Eigen::MatrixXd	_Q
Eigen::MatrixXd	_T
Eigen::MatrixXd	_C_H
Eigen::MatrixXd	_C_P
Eigen::MatrixXd	_C_B
Eigen::MatrixXd	_P_H
Eigen::MatrixXd	_P_P
Eigen::MatrixXd	_P_B
Eigen::MatrixXd	_R_H
Eigen::MatrixXd	_R_P
Eigen::MatrixXd	_R_B
Eigen::MatrixXd	_Sw
Eigen::MatrixXd	_Nb
Eigen::MatrixXd	_Nx
Eigen::MatrixXd	_H_N
Eigen::MatrixXd	_Aineq
Eigen::VectorXd	_Bineq
Eigen::MatrixXd	_Aeq
Eigen::VectorXd	_Beq
Eigen::MatrixXd	_Ci_eq
Eigen::VectorXd	_fcbar_eq
Eigen::MatrixXd	_rhs_2_eq
Eigen::GurobiDense	_eigGurobi
std::shared_ptr < MIQPLinearConstraints >	_constraints
std::shared_ptr< MIQP::MIQPState >	_state
Eigen::VectorXd	_H_N_r
unsigned int	_k
Eigen::MatrixXd	_S_wu
Eigen::MatrixXd	_S_gamma
Eigen::MatrixXd	_P_Gamma
Eigen::MatrixXd	_R_Gamma
Eigen::VectorXd	_One_Gamma
Eigen::MatrixXd	_S_alpha
Eigen::MatrixXd	_S_beta
Eigen::MatrixXd	_P_Alpha
Eigen::MatrixXd	_R_Alpha
Eigen::MatrixXd	_P_Beta
Eigen::MatrixXd	_R_Beta

Constructor & Destructor Documentation

MIQPController::MIQPController	(	MIQPParameters	params,
		ocra::Model::Ptr	robotModel,
		std::shared_ptr< StepController >	stepController,
		const Eigen::MatrixXd &	comStateRef
	)

Constructor.

Parameters:

params	MIQP parameters robotModel Pointer to the robot model instantiated by the containing client (walking-client)
comStateRef	Reference to a matrix of CoM state references. The k-th row contains the desired CoM state at time k

MIQPController::~MIQPController ( ) [virtual]

Destructor

Member Function Documentation

void MIQPController::buildAh	(	int	dt,
		Eigen::MatrixXd &	output
	)		`[protected]`

Builds \(A_h\).

Parameters:

	dt	Thread period in which this class is instantiated.
[out]	Output	passed to matrix reference.

See also:: _Ah

void MIQPController::buildAvoidOneFootRestReg ( MIQPParameters & miqpParams ) [protected]

void MIQPController::buildBh	(	int	dt,
		Eigen::MatrixXd &	output
	)		`[protected]`

Builds \(B_h\).

Parameters:

	dt	Thread period in which this classed in instantiated.
[out]	Output	passed to matrix reference.

See also:: _Bh

void MIQPController::buildC_B ( Eigen::MatrixXd & output ) [protected]

Builds \(\mathbf{C}_B\).

See also:: _C_B

Parameters:

[out] Output passed to matrix reference.

void MIQPController::buildC_H ( Eigen::MatrixXd & output ) [protected]

Builds \(\mathbf{C}_H\).

See also:: _C_H

Parameters:

[out] Output passed to matrix reference.

void MIQPController::buildC_P ( Eigen::MatrixXd & output ) [protected]

Builds \(\mathbf{C}_P\).

See also:: _C_P

Parameters:

[out] Output passed to matrix reference.

void MIQPController::buildCoMJerkReg ( MIQPParameters & miqpParams ) [protected]

void MIQPController::buildEqualityConstraintsMatrices	(	const Eigen::VectorXd &	x_k,
		Eigen::MatrixXd &	Aeq,
		Eigen::VectorXd &	Beq
	)		`[protected]`

Builds the equality constraints matrices _Aeq, _Beq. For the time being, the equality constraints are composed of only the so-called Simultaneity constraints of the problem, which guarantee that allowing a discontinuity of one of the bounds ( \(\mathbf{a},\mathbf{b}\)) in one direction simultaneously allows a discontinuity in the orthogonal direction. This requirement writes:

\[ \forall i, \alpha_{x_i} + \beta_{x_i} = \alpha_{y_i} + \beta_{y_i} \]

Which then in a preview window of size \(N\) writes:

\[ \left[\begin{array}{cccc} \mathbf{C}_i\mathbf{T} & 0 & \cdots & 0 \\ \mathbf{C}_i\mathbf{Q}\mathbf{T} & \mathbf{C}_i\mathbf{T} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{C}_i\mathbf{Q}^{N-1}\mathbf{T} & \mathbf{C}_i\mathbf{Q}^{N-2}\mathbf{T} & \cdots & \mathbf{C}_i\mathbf{T} \end{array}\right] = \left[\begin{array}{c} \mathbf{f}_c\\ \mathbf{f}_c\\ \vdots\\ \mathbf{f}_c \end{array}\right] - \left[\begin{array}{c} \mathbf{C}_i\mathbf{Q}\\ \mathbf{C}_i\mathbf{Q}^2\\ \vdots\\ \mathbf{C}_i\mathbf{Q}^N \end{array}\right] \mathbf{\xi}_k \]

Parameters:

[in]	x_k	Current state vector.
[out]	Aeq	Reference to output matrix.
[out]	Beq	Reference to right hand side vector.

void MIQPController::buildGenericRegMat	(	MIQP::InputVectorIndex	whichVariable,
		double	weight,
		Eigen::MatrixXd &	output
	)		`[protected]`

Builds a generic regularization matrix for a specified variable

void MIQPController::buildH_N ( Eigen::MatrixXd & output ) [protected]

Builds matrix \(\mathbf{H}_N\).

See also:: _H_N

Parameters:

[out] Output passed to matrix reference.

void MIQPController::buildMinimizeSteppingReg ( MIQPParameters & miqpParams ) [protected]

void MIQPController::buildNb	(	Eigen::MatrixXd &	output,
		double	wb
	)		`[protected]`

Builds matrix \(\mathbf{N}_b\).

Parameters:

[in]	wb	Weight of the balance performance cost.
[out]	Output	passed to matrix reference.

See also:: _Nb

void MIQPController::buildNx ( Eigen::MatrixXd & output ) [protected]

Builds matrix \(\mathbf{N}_x\).

Parameters:

[out] Output Passed to matrix reference.

See also:: _Nx

void MIQPController::buildPreviewInputMatrix	(	const Eigen::MatrixXd &	C,
		Eigen::MatrixXd &	R
	)		`[protected]`

Builds a Preview Input Matrix for a preview window of size \(N\) with the following form

\[ \mathbf{R} = \left[\begin{array}{cccc} \mathbf{C}\mathbf{T} & 0 & \cdots & 0 \\ \mathbf{C}\mathbf{Q}\mathbf{T} & \mathbf{C}\mathbf{T} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{C}\mathbf{Q}^{N-1}\mathbf{T} & \mathbf{C}\mathbf{Q}^{N-2}\mathbf{T} & \cdots & \mathbf{C}\mathbf{T} \end{array}\right] \]

Parameters:

	C	Output matrix from a state space representation.
[out]	R	Output.

See also:: _Q, _T

void MIQPController::buildPreviewStateMatrix	(	const Eigen::MatrixXd &	C,
		Eigen::MatrixXd &	P
	)		`[protected]`

Builds a Preview State Matrix for a preview window of size \(N\) with the following form:

\[ \mathbf{P} = \left[\begin{array}{c} \mathbf{C} \mathbf{Q} \\ \vdots\\ \mathbf{C} \mathbf{Q}^N \end{array}\right] \]

Parameters:

C	Output matrix from a state space representation.
P	Output.

See also:: _Q, _T

void MIQPController::buildQ ( Eigen::MatrixXd & output ) [protected]

Builds the matrix \(\mathbf{Q}\).

Parameters:

[out] Output passed to matrix reference.

See also:: _Q

void MIQPController::buildRegularizationTerms ( MIQPParameters & miqpParams ) [protected]

Builds regularization terms. For the moment, these include:

Avoid resting on one foot
CoM jerk regularization terms

void MIQPController::buildSw	(	Eigen::MatrixXd &	output,
		MIQPParameters	miqpParams
	)		`[protected]`

Builds matrix \(\mathbf{S}_w\)

Parameters:

[in]	miqpParams	list of MIQP parameters indicating also what kind of CoM state reference are going to be tracked.
[out]	Output	passed to matrix reference.

See also:: _Sw, MIQPParameters

void MIQPController::buildT ( Eigen::MatrixXd & output ) [protected]

Builds the matrix \(\mathbf{T}\).

Parameters:

[out] Output passed to matrix reference.

See also:: _T

void MIQPController::getSolution ( Eigen::VectorXd & X_kn )

void MIQPController::run ( ) [virtual]

Main loop of this thread. Performs the following operations:

Updates the state vector.
Updates the state-dependent RHS of the constraints.
Sets the "moving" CoM reference in the current preview window.
Solves the optimization problem.
Retrieves the solution.

See also:: updateStateVector(), MIQPLinearConstraints::updateRHS(), setCOMStateRefInPreviewWindow(), setLinearPartObjectiveFunction(), Eigen::GurobiDense::solve()

Todo:

Watch out! _eigGurobi will add a 1/2. Therefore the 2. Check that this is correct.

The expression for _H_N is missing regularization terms

void MIQPController::setBinaryVariables ( ) [protected]

void MIQPController::setCOMStateRefInPreviewWindow	(	unsigned int	k,
		Eigen::VectorXd &	comStateRef
	)

Takes \(N\) elements from an original trajectory of desired CoM states from state \(k\)

Parameters:

comStateRef Sets _H_N_r for a preview window of size N from time k

void MIQPController::setLinearPartObjectiveFunction ( ) [protected]

Sets _linearTermTransObjFunc, which corresponds to the linear part of the objective function of the MIQP

See also:: _linearTermTransObjFunc

void MIQPController::setLowerAndUpperBounds ( ) [protected]

Sets _lb and _ub

See also:: _lb _ub

bool MIQPController::threadInit ( ) [virtual]

Performs all the initialization of the MIQP controller such as:

Initializing the Gurobi model.
Set lower and upper bounds of the optimization variables.
Create the LTI matrices of the constraints (inequality and equality constraints).
Set variable types.
Setup the eigen-gurobi object.

void MIQPController::threadRelease ( ) [virtual]

Deallocates in memory. In particular, that of the Gurobi environment.

void MIQPController::updateEqualityConstraints	(	const Eigen::VectorXd &	x_k,
		Eigen::VectorXd &	Beq
	)		`[protected]`

Updates the RHS of the equality constraints.

Parameters:

[in]	x_k	System state vector at time k.
[out]	Beq	Output matrix. It should be a reference to _Beq.

See also:: buildEqualityConstraintsMatrices(), _Beq

void MIQPController::updateStateVector ( ) [protected]

Updates the state vector _xi_k, i.e. \(xi_k\)

See also:: _xi_k

void MIQPController::writeToFile	(	const double &	time,
		const Eigen::VectorXd &	X_kn,
		std::string &	home
	)		`[protected]`

Writes optimization result to file for plotting.

Parameters:

time	Timestamp.
X_kn	result of optimization.
home	Root directory where to write the file.

Member Data Documentation

bool MIQPController::_addRegularization [private]

Add regularization terms to cost function?

Eigen::MatrixXd MIQPController::_Aeq [private]

Matrix A of the equality constraints of the MIQP problem, i.e.

\[ \mathbf{A}_{\text{eq}} \mathbf{\mathcal{X}} = \mathbf{b}_{\text{eq}} \]

See also:: buildEqualityConstraintsMatrices()

Eigen::MatrixXd MIQPController::_Ah [private]

State matrix \(A_h\) from the CoM jerk integration scheme.

\[ \mathbf{h}_{k+1} = \mathbf{A}_h \mathbf{h}_k + \mathbf{B}_h \mathbf{u}_k \]

It is a constant matrix of size \(6\times6\) equal to:

\[ \mathbf{A_h} = \left[ \begin{array}{ccc} \mathbf{I}_2 & \delta t \mathbf{I}_2 & \frac{\delta t^2}{2} \mathbf{I}_2 \\ 0 & \mathbf{I}_2 & \delta t \mathbf{I}_2 \\ 0 & 0 & \mathbf{I}_2 \end{array} \right] \]

Size: \([6\times6]\)

Eigen::MatrixXd MIQPController::_Aineq [private]

Inequality matrix of the MIQP problem, i.e.

\[ A_{\text{ineq}} \mathcal{X} \leq b_{\text{ineq}} \]

Set by MIQPLinearConstraints::getConstraintsMatrixA().

Eigen::VectorXd MIQPController::_Beq [private]

RHS of the equality constraints of the MIQP problem, i.e.

\[ \mathbf{A}_{\text{eq}} x = \mathbf{b}_{\text{eq}} \]

See also:: buildEqualityConstraintsMatrices()

Eigen::MatrixXd MIQPController::_Bh [private]

Input Matrix \(B_h\) from the CoM jerk integration scheme.

\[ \mathbf{h}_{k+1} = \mathbf{A}_h \mathbf{h}_k + \mathbf{B}_h \mathbf{u}_k \]

It is constant of size \(6\times2\) and equal to:

\[ \mathbf{B_h} = \left[ \begin{array}{c} \frac{\delta^3}{6}\mathbf{I}_2 \\ \frac{\delta t^2}{2} \mathbf{I}_2 \\ \delta t \mathbf{I}_2 \end{array} \right] \]

Size: \([6\times2]\)

Eigen::VectorXd MIQPController::_Bineq [private]

RHS inequality vector \(b_{\text{ineq}}\) of the MIQP problem, i.e.

\[ A_{\text{ineq}} x \leq b_{\text{ineq}} \]

Set by MIQPLinearConstraints::getRHS()

Eigen::MatrixXd MIQPController::_C_B [private]

Output matrix \(\mathbf{C_B}\) of the BoS state space representation

\begin{align*} \mathbf{\xi}_{k+1|k} &= \mathbf{Q} \xi_{k|k} + \mathbf{T}\mathcal{X}_{k+1|k} \\ \mathbf{r}_{k|k} & =\mathbf{C}_B \xi_{k|k} \end{align*}

Where

\[ \mathbf{C}_B = \frac{1}{2} \left[ \begin{array}{ccc} \mathbf{I}_{2\times2} & \mathbf{I}_{2\times2} & \mathbf{0}_{2\times12} \end{array}\right] \]

Size: \([2\times12]\)

Eigen::MatrixXd MIQPController::_C_H [private]

Output matrix \(\mathbf{C}_H\) of the CoM state space representation

\begin{align*} \mathbf{\xi}_{k+1|k} &= \mathbf{Q} \xi_{k|k} + \mathbf{T}\mathcal{X}_{k+1|k} \\ \hat{\mathbf{h}}_{k|k} & =\mathbf{C}_H \xi_{k|k} \end{align*}

Where

\[ \mathbf{C}_H = \left[ \begin{array}{cc} \mathbf{0}_{6\times10} & \mathbf{I}_{6\times6} \end{array} \right] \]

Size: \([6\times16]\)

Eigen::MatrixXd MIQPController::_C_P [private]

Output matrix \(\mathbf{C_P}\) of the CoP state space representation

\begin{align*} \mathbf{\xi}_{k+1|k} &= \mathbf{Q} \xi_{k|k} + \mathbf{T}\mathcal{X}_{k+1|k} \\ \mathbf{p}_{k|k} & =\mathbf{C}_P \xi_{k|k} \end{align*}

Where

\[ \mathbf{C}_P = \left[ \begin{array}{ccccc} \mathbf{0}_{2\times10} & \mathbf{I}_{2\times2} & \mathbf{0}_{2\times2} & -\frac{c_z}{g}\mathbf{I}_{2\times2} \end{array}\right] \]

Size: \([2\times16]\)

Eigen::MatrixXd MIQPController::_Ci_eq [private]

Matrix \(\mathbf{C}_i\) from the Simultaneity equality constraint

\[ \mathbf{C}_i \mathbf{\xi}_i = 0 \]

Eigen::MatrixXd MIQPController::_comStateRef [private]

Matrix of CoM state references. Every row corresponds to the CoM state at a given time step.

Size: \(n\times6\) Where \(n\) is the length of the trajectory.

std::shared_ptr<MIQPLinearConstraints> MIQPController::_constraints [private]

Linear constraints object

Eigen::GurobiDense MIQPController::_eigGurobi [private]

eigen-gurobi object

Eigen::VectorXd MIQPController::_fcbar_eq [private]

constant part of the RHS of the Simultaneity euqality constraint.

Eigen::MatrixXd MIQPController::_H_N [private]

Positive definite matrix \(\mathbf{H}_N\) with the coefficients of the quadratic objective of the MIQP for the walking MPC problem:

\[ \underset{\mathcal{X}}{\text{min}} \; \mathcal{X}^T \mathbf{H}_N \mathcal{X} + \mathbf{d}^T \mathcal{X} \]

For the time being this is:

\[ \mathbf{R}_H^T \mathbf{S}_w \mathbf{R}_H + (\mathbf{R}_P - \mathbf{R}_B)^T \mathbf{N}_b (\mathbf{R}_P - \mathbf{R}_B) \]

Note:: But this expression is missing the regularizing terms of the cost function. Current size is \(12N\times12N\)

Eigen::VectorXd MIQPController::_H_N_r [private]

Reference CoM state in preview window. Size: [6N]

unsigned int MIQPController::_k [private]

Current iteration

Eigen::VectorXd MIQPController::_lb [private]

Vector of lower bounds of the input vector \(\mathcal{X}\)

Size: \([6\times1]\)

Eigen::VectorXd MIQPController::_linearTermTransObjFunc [private]

Contains the state-dependent coefficients of the linear term of the objective function of the MIQP for the walking MPC problem.

\[ \underset{\mathcal{X}}{\text{min}} \; \mathcal{X}^T \mathbf{H}_N \mathcal{X} + \mathbf{d}^T \mathcal{X} \]

For the time being, it does not contain any resulting term from regularizing terms.

\[ \mathbf{d}^T = -2(\mathbf{H}_N^r - \mathbf{P}_H \xi_k)^T \mathbf{S}_w \mathbf{R}_H + 2[(\mathbf{P}_P - \mathbf{P}_B) \xi_k]^T \mathbf{N}_b(\mathbf{R}_P - \mathbf{R}_B) \]

See also:: setLinearPartObjectiveFunction()

MIQPParameters MIQPController::_miqpParams [private]

Object containing basic MIQP parameters.

Eigen::MatrixXd MIQPController::_Nb [private]

\(\mathbf{N}_b\) is a diagonal weighting matrix whose scalar weights help define the compromise between balance robustness and tracking performance.

Eigen::MatrixXd MIQPController::_Nx [private]

\(N_x\) is a diagonal weighting matrix to include the consideration given to the size of the CoM jerks in \(\mathcal{X}\). Part of the regularization terms of the quadratic coefficients _H_N.

Eigen::VectorXd MIQPController::_One_Gamma [private]

Eigen::MatrixXd MIQPController::_P_Alpha [private]

Eigen::MatrixXd MIQPController::_P_B [private]

Matrix \(\mathbf{P}_B\) in the equation for the preview center of BoS output matrix \(\mathbf{R}_{k,N} = \left\{ \mathbf{r}_{k+1|k}, \mathbf{r}_{k+2|k}, \dots, \mathbf{r}_{k+N|k} \right\}\)

For a preview window of size \(N\):

\[ \mathbf{R}_{k,N} = \mathbf{P}_B \mathbf{\xi}_k + \mathbf{R}_B \mathcal{X}_{k,N} \]

Where:

\[ \mathbf{P}_B = \left[\begin{array}{c} \mathbf{C}_B \mathbf{Q} \\ \vdots\\ \mathbf{C}_B \mathbf{Q}^N \end{array}\right] \]

Size: \([2N\times16]\)

See also:: _C_B, _Q

Eigen::MatrixXd MIQPController::_P_Beta [private]

Eigen::MatrixXd MIQPController::_P_Gamma [private]

Eigen::MatrixXd MIQPController::_P_H [private]

Matrix \(\mathbf{P}_H\) in the equation for the preview CoM output matrix \(\mathbf{H}_{k,N} = \left\{ \mathbf{h}_{k+1|k}, \mathbf{h}_{k+2|k}, \dots, \mathbf{h}_{k+N|k} \right\}\).

For a preview window of size \(N\):

\[ \mathbf{H}_{k,N} = \mathbf{P}_H \mathbf{\xi}_k + \mathbf{R}_H \mathcal{X}_{k,N} \]

Where:

\begin{align*} \mathbf{P}_H = \left[\begin{array}{c} \mathbf{C}_H \mathbf{Q} \\ \vdots\\ \mathbf{C}_H \mathbf{Q}^N \end{array}\right] \end{align*}

Size: \([6N\times16]\)

See also:: _C_H, _Q

Eigen::MatrixXd MIQPController::_P_P [private]

Matrix \(\mathbf{P}_p\) in the equation for the preview CoP output matrix \(\mathbf{P}_{k,N} = \left\{ \mathbf{p}_{k+1|k}, \mathbf{p}_{k+2|k}, \dots, \mathbf{p}_{k+N|k} \right\}\).

For a preview window of size \(N\):

\[ \mathbf{P}_{k,N} = \mathbf{P}_p \mathbf{\xi}_k + \mathbf{R}_p \mathcal{X}_{k,N} \]

Where:

\begin{align*} \mathbf{P}_P = \left[\begin{array}{c} \mathbf{C}_P \mathbf{Q} \\ \vdots\\ \mathbf{C}_P \mathbf{Q}^N \end{array}\right] \end{align*}

Size: \([2N\times16]\)

See also:: _C_P, _Q

int MIQPController::_period [private]

Thread period in milliseconds

Eigen::MatrixXd MIQPController::_Q [private]

Matrix \(\mathbf{Q}\) in preview state model:

\[ \mathbf{\xi}_{k+1|k} = \mathbf{Q} \xi_{k|k} + \mathbf{T}\mathcal{X}_{k+1|k} \]

\[ \mathbf{Q} = \left[\begin{array}{cc} \mathbf{0}_{10\times10} & \mathbf{0}_{10\times6}\\ \mathbf{0}_{6\times10} & \mathbf{A_h}_{6\times6} \end{array}\right] \]

Size: \([16\times16]\)

See also:: _Ah

Eigen::MatrixXd MIQPController::_R_Alpha [private]

Eigen::MatrixXd MIQPController::_R_B [private]

Matrix \(\mathbf{R}_B\) in the equation for the preview center of BoS output matrix \(\mathbf{R}_{k,N} = \left\{ \mathbf{r}_{k+1|k}, \mathbf{r}_{k+2|k}, \dots, \mathbf{r}_{k+N|k} \right\}\)

For a preview window of size \(N\):

\[ \mathbf{R}_{k,N} = \mathbf{P}_B \mathbf{\xi}_k + \mathbf{R}_B \mathcal{X}_{k,N} \]

Where:

\[ \mathbf{R}_B = \left[\begin{array}{cccc} \mathbf{C}_B\mathbf{T} & 0 & \cdots & 0 \\ \mathbf{C}_B\mathbf{Q}\mathbf{T} & \mathbf{C}_B\mathbf{T} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{C}_B\mathbf{Q}^{N-1}\mathbf{T} & \mathbf{C}_B\mathbf{Q}^{N-2}\mathbf{T} & \cdots & \mathbf{C}_B\mathbf{T} \end{array}\right] \]

Size: \([2N\times12N]\)

See also:: _C_B, _T, _Q

Eigen::MatrixXd MIQPController::_R_Beta [private]

Eigen::MatrixXd MIQPController::_R_Gamma [private]

Eigen::MatrixXd MIQPController::_R_H [private]

Input matrix \(\mathbf{R}_H\) in the equation for the preview CoM output matrix \(\mathbf{H}_{k,N} = \left\{ \mathbf{h}_{k+1|k}, \mathbf{h}_{k+2|k}, \dots, \mathbf{h}_{k+N|k} \right\}\).

For a preview window of size \(N\):

\[ \mathbf{H}_{k,N} = \mathbf{P}_H \mathbf{\xi}_k + \mathbf{R}_H \mathcal{X}_{k,N} \]

Where:

\[ \mathbf{R}_H = \left[\begin{array}{cccc} \mathbf{C}_H\mathbf{T} & 0 & \cdots & 0 \\ \mathbf{C}_H\mathbf{Q}\mathbf{T} & \mathbf{C}_H\mathbf{T} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{C}_H\mathbf{Q}^{N-1}\mathbf{T} & \mathbf{C}_H\mathbf{Q}^{N-2}\mathbf{T} & \cdots & \mathbf{C}_H\mathbf{T} \end{array}\right] \]

Size: \([6N\times12N]\)

See also:: _C_H, _Q, _T

Eigen::MatrixXd MIQPController::_R_P [private]

Matrix \(\mathbf{R}_p\) in the equation for the preview CoP output matrix \(\mathbf{P}_{k,N} = \left\{ \mathbf{p}_{k+1|k}, \mathbf{p}_{k+2|k}, \dots, \mathbf{p}_{k+N|k} \right\}\).

For a preview window of size \(N\):

\[ \mathbf{P}_{k,N} = \mathbf{P}_p \mathbf{\xi}_k + \mathbf{R}_p \mathcal{X}_{k,N} \]

Where:

\[ \mathbf{R}_P = \left[\begin{array}{cccc} \mathbf{C}_P\mathbf{T} & 0 & \cdots & 0 \\ \mathbf{C}_P\mathbf{Q}\mathbf{T} & \mathbf{C}_P\mathbf{T} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{C}_P\mathbf{Q}^{N-1}\mathbf{T} & \mathbf{C}_P\mathbf{Q}^{N-2}\mathbf{T} & \cdots & \mathbf{C}_P\mathbf{T} \end{array}\right] \]

Size: \([2N\times12N]\)

See also:: _C_P, _Q, _T

Eigen::MatrixXd MIQPController::_rhs_2_eq [private]

State matrix of the RHS of the Simultaneity equality constraint.

See also:: buildEqualityConstraintsMatrices()

ocra::Model::Ptr MIQPController::_robotModel [private]

Eigen::MatrixXd MIQPController::_S_alpha [private]

Eigen::MatrixXd MIQPController::_S_beta [private]

Eigen::MatrixXd MIQPController::_S_gamma [private]

Eigen::MatrixXd MIQPController::_S_wu [private]

std::shared_ptr<MIQP::MIQPState> MIQPController::_state [private]

MIQP State

std::shared_ptr<StepController> MIQPController::_stepController [private]

Eigen::MatrixXd MIQPController::_Sw [private]

\(\mathbf{S_w}\) is a \(6N\times6N\) diagonal weighting selection matrix, defining whether position, velocity and/or acceleration of the CoM are tracked in each of the two horizontal directions.

Todo:: For the moment this has been hardcoded to take into account only CoM velocity references.

Eigen::MatrixXd MIQPController::_T [private]

Matrix \(\mathbf{T}\) in preview state model:

\[ \mathbf{\xi}_{k+1|k} = \mathbf{Q} \xi_{k|k} + \mathbf{T}\mathcal{X}_{k+1|k} \]

\[ \mathbf{T} = \left[\begin{array}{cc} \mathbf{I}_{10\times10} & \mathbf{0}_{10\times2}\\ \mathbf{0}_{6\times10} & \mathbf{B_h}_{6\times2} \end{array}\right] \]

Size: \([16\times12]\)

See also:: _Bh

Eigen::VectorXd MIQPController::_ub [private]

Vector of upper bounds of the input vector \(\mathcal{X}\)

Size: \([6\times1]\)

std::string MIQPController::_variablesNames[INPUT_VECTOR_SIZE] [private]

Eigen::VectorXd MIQPController::_X_kn [private]

Solution \(\mathcal{X}_{k,N}\) of the MIQP problem

Eigen::VectorXd MIQPController::_xi_k [private]

State vector \(\xi_k\) of the MIQP problem:

\[ \begin{array}{ccccccccc} [\mathbf{a} & \mathbf{b} & \mathbf{\alpha} & \mathbf{\beta} & \delta & \gamma & \mathbf{h} & \dot{\mathbf{h}} & \ddot{\mathbf{h}}] \end{array} \]

Where \(\mathbf{a} \in \mathbb{R}^2\) are the upper bounds of the base of support (BoS), \(b \in \mathbb{R}^2\) are the lower bounds, \(\mathbf{\alpha} \in \mathbb{R}^2\) the rising edges of \(\mathbf{a}\), \(\mathbf{\beta} \in \mathbb{R}^2\) are the falling edges of \(\mathbf{b}\), \(\delta\) indicates the potential change from double support to single support, while \(\gamma\) indicates whether the robot is in single support (SS) or double support (DS). \(\mathbf{h}, \dot{\mathbf{h}}, \ddot{\mathbf{h}}\)

yarp::os::Semaphore MIQPController::semaphore

Semaphore protecting the isInitialized variable.

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

ocra-wbi-plugins/ocra-icub-clients/walking-client/include/walking-client/MIQPController.h
ocra-wbi-plugins/ocra-icub-clients/walking-client/src/MIQPController.cpp

Public Member Functions

Public Attributes

Protected Member Functions

Private Attributes

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation