Implementation for MPC#

Main Class#

class B1Env.mpc.controller.MPC(horizon_length=16)#

This class solves a MPC problem in the form of

\[\begin{split}\begin{align} \min_{x}\ &\ \frac{1}{2}x^T\mathbf{H}x + \mathbf{g}^Tx\\ \mathrm{subject\ to}\ &\ \mathbf{A}x = \mathbf{b}\\ &\ \mathbf{C}x \leq \mathrm{UB}. \end{align}\end{split}\]
__init__(self, horizon_length=16)#

The __init__ function sets the problem variables and pre-computes the non-changing parts of the MPC problem.

Parameters

horizon_length (int) – the horizon length of the MPC problem

pre_construct_objective_function(self)#

Computes the non-changing part in the MPC objective function. In this case the quadratic cost matrix H is not changing.

construct_objective_function(self, x_ref_mat)#

Computes the changing part in the MPC objective function. In this case the linear cost matrix g changes each timestep.

Parameters

x_ref_mat (ndarray) – the reference motion of the centroidal model

pre_construct_inequality_constraint(self)#

Computes the non-changing part in the MPC inequality constraint. In this case the inequality constraint upper bound UB is not changing.

construct_inequality_constraint(self, M_st)#

Computes the changing part in the MPC inequality constraint. In this case the inequality constraint matrix C changes each timestep.

Parameters

M_st (ndarray) – the stance foot selection matrix

construct_equality_constraint(self, bA, bA_0, bB, x_0, M_sw)#

Computes the changing part in the MPC equality constraint. In this case both the A and b matrix changes each timestep.

Parameters

  • bA (ndarray) – the aggregated drift matrix

  • bA_0 (ndarray) – the drift matrix at the current state

  • bB (ndarray) – the aggregated control matrix

  • x_0 (ndarray) – the current centroidal dynamics state

  • M_sw (ndarray) – the swing foot selection matrix

solve(self)#

Solves the MPC problem using the computed problem configurations. Need to run construct_objective_function, construct_inequality_constraint, and construct_equality_constraint before running this function.

Utility Functions#

The utiltiy functions can be grouped into three groups, one group used to compute the objective function, one for computing the inequality constraint, and one for computing the equality constraint.

Objective Function Utilities#

B1Env.mpc.utils.get_Q_step()#

Computes the step-wise state cost matrix Q_step:

\[\mathbf{Q}_{i+1} = \mathrm{blockdiag}\Big(\mathbf{I}_3, \mathbf{0}_{3\times3}, \mathbf{I}_3, \mathbf{0}_{6\times6}\Big)\in\mathbb{R}^{15\times15}\]
Returns

step-wise state cost matrix Q_step

Return type

ndarray

B1Env.mpc.utils.get_HQ(timesteps=16)#

Computes the state cost matrix H_Q:

\[\mathbf{H}_\mathbf{Q} = 2\mathrm{blockdiag}\Big(\mathbf{Q}_1, \cdots, \mathbf{Q}_N\Big)\in\mathbb{R}^{15N\times15N}\]
Parameters

timesteps (int) – the number of timestep considered within the MPC preview horizon

Returns

state cost matrix H_Q

Return type

ndarray

B1Env.mpc.utils.get_HR(timesteps=16)#

Computes the control cost matrix H_R with shape:

\[\mathbf{H}_\mathbf{R} = 2\mathrm{blockdiag}\Big(\mathbf{R}_{0}, \cdots, \mathbf{R}_{N-1}\Big)\in\mathbb{R}^{12N\times12N}\]
Parameters

timesteps (int) – the number of timestep considered within the MPC preview horizon

Returns

state cost matrix H_R

Return type

ndarray

B1Env.mpc.utils.get_H(timesteps=16)#

Computes the quadratic cost matrix H:

\[\begin{split}\mathbf{H} = \begin{bmatrix} \mathbf{H}_\mathbf{Q} & \mathbf{0}_{15N\times12N}\\ \mathbf{0}_{12N\times15N} & \mathbf{H}_\mathbf{R} \end{bmatrix}\in\mathbb{R}^{27N\times27N}\end{split}\]
Parameters

timesteps (int) – the number of timestep considered within the MPC preview horizon

Returns

quadratic cost matrix H

Return type

ndarray

B1Env.mpc.utils.get_gx(x_ref_mat, timesteps=16)#

Computes the linear state cost matrix g_x:

\[\begin{split}\mathbf{g}_\mathbf{x} = \begin{bmatrix} -2\mathbf{Q}_{1}^T\mathbf{x}_{1}^\mathrm{ref}\\ \vdots\\ -2\mathbf{Q}_{N}^T\mathbf{x}_{N}^\mathrm{ref} \end{bmatrix}\in\mathbb{R}^{15N\times1}\end{split}\]

by taking x_ref_mat as its input

\[\begin{split}\mathbf{x}^\mathrm{ref} = \begin{bmatrix} \mathbf{x}_1^\mathrm{ref}\\ \vdots\\ \mathbf{x}_N^\mathrm{ref} \end{bmatrix}\in\mathbb{R}^{15N\times1}\end{split}\]

and performing the computation

\[\mathbf{g}_\mathbf{x} = -2\mathbf{H}_\mathbf{Q}\mathbf{x}^\mathrm{ref}\]
Parameters

  • x_ref_mat (ndarray) – the reference motion of the centroidal model

  • timesteps (int) – the number of timestep considered within the MPC preview horizon

Returns

linear state cost matrix g_x

Return type

ndarray

B1Env.mpc.utils.get_g(x_ref_mat, timesteps=16)#

Computes the linear cost matrix g:

\[\begin{split}\mathbf{g} = \begin{bmatrix} \mathbf{g}_\mathbf{x}\\ \mathbf{g}_\mathbf{f} \end{bmatrix}\in\mathbb{R}^{27N\times1}\end{split}\]

by taking x_ref_mat as its input

\[\begin{split}\mathbf{x}^\mathrm{ref} = \begin{bmatrix} \mathbf{x}_1^\mathrm{ref}\\ \vdots\\ \mathbf{x}_N^\mathrm{ref} \end{bmatrix}\in\mathbb{R}^{15N\times1}\end{split}\]
Parameters

  • x_ref_mat (ndarray) – the reference motion of the centroidal model

  • timesteps (int) – the number of timestep considered within the MPC preview horizon

Returns

linear cost matrix g

Return type

ndarray

Inequality Constraint Utilities#

B1Env.mpc.utils.get_bcf(mu=0.6)#

Computes the friction cone coefficient matrix bc_f:

\[\begin{split}\bar{\mathbf{c}}_\mathbf{f} = \begin{bmatrix} 0 & 0 & -1\\ 0 & 0 & 1\\ -1 & 0 & -\mu\\ 1 & 0 & -\mu\\ 0 & -1 & -\mu\\ 0 & 1 & -\mu \end{bmatrix}\in\mathbb{R}^{6\times3}\end{split}\]
Parameters

mu (float) – the friction coefficient mu

Returns

friction cone coefficient matrix bc_f

Return type

ndarray

B1Env.mpc.utils.get_bubf(f_min=0.001, f_max=10.0)#

Computes the friction cone constraint upper bound vector bub_f:

\[\begin{split}\bar{\mathrm{ub}}_\mathbf{f} = \begin{bmatrix} -f_\min\\ f_\max\\ 0\\ 0\\ 0\\ 0 \end{bmatrix}\end{split}\]
Parameters

  • f_min (float) – the minimum support force

  • f_max (float) – the maximum support force

Returns

friction cone constraint upper bound vector bub_f

Return type

ndarray

B1Env.mpc.utils.get_C(n_st, M_st, timesteps=16, mu=0.6)#

Computes the inequality constraint matrix C:

\[\mathbf{C} = \begin{bmatrix} \mathbf{0} & \mathbf{C}_\mathrm{f} \end{bmatrix}\in\mathbb{R}^{6n_{st}N\times27N}\]

where

\[\mathbf{C}_\mathrm{f} = \mathrm{blockdiag}\Big(\mathbf{c}_{\mathbf{f}}\mathcal{M}_{st, 0}, \cdots, \mathbf{c}_{\mathbf{f}}\mathcal{M}_{st, N-1}\Big)\in\mathbb{R}^{6n_{st}N\times12N}\]

We compute this by first computing

\[\mathbf{c}_{\mathbf{f}} = \mathrm{blockdiag}\Big(\underbrace{\bar{\mathbf{c}}_{\mathbf{f}}, \cdots, \bar{\mathbf{c}}_{\mathbf{f}}}_{n_{st}}\Big)\in\mathbb{R}^{6n_{st}\times3n_{st}}\]

and then constructing the matrix

\[\begin{split}\texttt{c_f_mat} = \begin{bmatrix} \mathbf{c}_{\mathbf{f}} & \cdots & \mathbf{0}\\ \vdots & \ddots & \vdots\\ \mathbf{0} & \cdots & \mathbf{c}_{\mathbf{f}} \end{bmatrix}\in\mathbb{R}^{6n_{st}N\times3n_{st}N}\end{split}\]

The input M_st is the matrix

\[\begin{split}\texttt{M_st} = \begin{bmatrix} \mathcal{M}_{st, 1} & \cdots & \mathbf{0}\\ \vdots & \ddots & \vdots\\ \mathbf{0} & \cdots & \mathcal{M}_{st, N} \end{bmatrix}\in\mathbb{R}^{3n_{st}N\times12N}\end{split}\]

We can then compute C_f as

\[\mathbf{C}_\mathbf{f} = \texttt{c_f_mat}\texttt{M_st} = \mathrm{blockdiag}\Big(\mathbf{c}_{\mathbf{f}}\mathcal{M}_{st, 0}, \cdots, \mathbf{c}_{\mathbf{f}}\mathcal{M}_{st, N-1}\Big)\in\mathbb{R}^{6n_{st}N\times12N}\]
Parameters

  • n_st (int) – the number of stance feet at each time instance (gait dependent)

  • M_st (ndarray) – the stance foot selection matrix

  • timesteps (int) – the number of timestep considered within the MPC preview horizon

  • mu (float) – the friction coefficient mu

Returns

inequality constraint matrix C

Return type

ndarray

B1Env.mpc.utils.get_UB(n_st, f_min=0.001, f_max=10.0, timesteps=16)#

Computes the inequality constraint upper bound UB:

\[\mathrm{UB} = \mathrm{UB}_\mathbf{f} = \mathrm{vstack}\Big(\mathrm{ub}_\mathbf{f}, \cdots, \mathrm{ub}_\mathbf{f}\Big)\in\mathbb{R}^{6n_{st}N\times1}\]

First, it computes the value of bub_f. Then, it computes the value of ub_f as

\[\mathrm{ub}_\mathbf{f} = \mathrm{vstack}\Big(\bar{\mathrm{ub}}_\mathbf{f}, \cdots, \bar{\mathrm{ub}}_\mathbf{f}\Big)\in\mathbb{R}^{6n_{st}\times1}\]
Parameters

  • n_st (int) – the number of stance feet at each time instance (gait dependent)

  • f_min (float) – the minimum support force

  • f_max (float) – the maximum support force

  • timesteps (int) – the number of timestep considered within the MPC preview horizon

Returns

inequality constraint upper bound UB

Return type

ndarray

Equality Constraint Utilities#

B1Env.mpc.utils.get_b(bA_0, x_0, n_sw, timesteps=16)#

Computes the b matrix in the QP equality constraint. The b matrix is defined as

\[\begin{split}\mathbf{b} = \begin{bmatrix} \mathbf{b}_d\\ \mathbf{b}_f \end{bmatrix}\in\mathbb{R}^{(15 + 3n_{sw})N\times1}\end{split}\]

with

\[\begin{split}\mathbf{b}_d = \begin{bmatrix} \bar{\mathbf{A}}_0\mathbf{x}_0\\ \mathbf{0} \end{bmatrix}\in\mathbb{R}^{15N\times1} \quad \text{&} \quad \mathbf{b}_f = \mathbf{0}\in\mathbb{R}^{3n_{sw}N\times1}.\end{split}\]
Parameters

  • bA_0 (ndarray) – the drift matrix at the current state

  • x_0 (ndarray) – the current centroidal dynamics state

  • n_sw (int) – the number of swing feet at each time instance (gait dependent)

  • timesteps (int) – the number of timestep considered within the MPC preview horizon

Returns

the b matrix in the QP equality constraint

Return type

ndarray

B1Env.mpc.utils.get_A(bA, bB, M_sw, n_sw, timesteps=16)#

Computes the A matrix in the QP equality constraint. The A matrix is defined as

\[\begin{split}\mathbf{A} = \begin{bmatrix} \mathbf{A}_d\\ \mathbf{A}_f \end{bmatrix}\in\mathbb{R}^{(15+3n_{sw})N\times27N}\end{split}\]

with

\[\mathbf{A}_d = \begin{bmatrix} \bar{\mathbf{A}} & \bar{\mathbf{B}} \end{bmatrix}\in\mathbb{R}^{15N\times27N} \quad \text{&} \quad \mathbf{A}_f = \begin{bmatrix} \mathbf{0} & \mathbf{M}_\mathrm{sw} \end{bmatrix}\in\mathbb{R}^{3n_{sw}N\times27N}.\]

And the matrix M_sw is defined as

\[\mathbf{M}_\mathrm{sw} = \mathrm{blockdiag}\Big(\mathcal{M}_{sw, 0}, \cdots, \mathcal{M}_{sw, N-1}\Big)\in\mathbb{R}^{3n_{sw}N\times12N}.\]
Parameters

  • bA (ndarray) – the aggregated drift matrix

  • bB (ndarray) – the aggregated control matrix

  • M_sw (ndarray) – the swing foot selection matrix

  • n_sw (int) – the number of swing feet at each time instance (gait dependent)

  • timesteps (int) – the number of timestep considered within the MPC preview horizon

Returns

the A matrix in the QP equality constraint

Return type

ndarray