A sparse MPC solver for walking motion generation (old version).: Adding inequality constraints

This page describes changes in KKT system after addition of inequality constraint to the active set.

Update of Cholesky factor

Let $\mbm{a}_i^T$ be the normal to the i-th inequality constraint (assumed to be a simple bound). Define the matrix

$\mbm{C} = \left[\begin{array}{c} \mbm{E} \\ \mbm{A}_{W}\end{array}\right]$

where the rows of $\mbm{A}_W$ contain the normals to the inequality constraints in the working set.

If a new constraint must be added to the active set, then the Schur complement must be updated:

$\frac{1}{2} \left[ \begin{array}{c} \mbm{C}\\ \mbm{a}^T\\ \end{array} \right] \mbm{H}^{-1} \left[\mbm{C}^T \quad \mbm{a}\right] = \frac{1}{2} \left[ \begin{array}{cc} \mbm{C} \mbm{H}^{-1} \mbm{C}^T & \mbm{C} \mbm{H}^{-1} \mbm{a}\\ \mbm{a}^T \mbm{H}^{-1} \mbm{C}^T & \mbm{a}^T \mbm{H}^{-1} \mbm{a}\\ \end{array} \right]$

In general the last line is

$\mbm{s_a}^T = \frac{1}{2} \left[ \mbm{a}^T \mbm{H}^{-1} \mbm{E}^T \quad \mbm{a}^T \mbm{H}^{-1} \mbm{A}_W^T \quad \mbm{a}^T \mbm{H}^{-1} \mbm{a} \right]$

Note that $\frac{1}{2} \mbm{a}^T \mbm{H}^{-1} \mbm{A}_W^T$ is a vector of zeros. While $\frac{1}{2} \mbm{a}^T \mbm{H}^{-1} \mbm{a} = \frac{1}{2\beta}$ is a scalar.

$\frac{1}{2} \mbm{a}^T \mbm{H}^{-1} \mbm{E}^T$ selects and scales one column of $\mbm{E}$ , this column corresponds to ZMP coordinates and can have at most 4 non-zero elements.

The total number of non-zero elements in the new row of Schur complement is 5 or 3 (for the last state in the preview window).

Algorithm of Cholesky factor update

Input:
    m_e % the number of equality constraints
    m_a % the current cardinality of the active set
    L   % Cholesky factor
    s_a % a row added to the Schur complement

Output:
    l   % a new (the last) row of L

    l = s_a
    first   % the index of the first !=0 element of s_a
    end = m_e + m_a + 1 % the index of the last element of s_a

    for i = l_s:m_e
        l(i) = l(i) / L(i,i)
        l(end) = l(end) - l(i)^2

        % Since ecL is sparse, no more than three subsequent elements with
        % (known) indexes 'k' <= 'end' in 'l' must be updated: 
        l(k) = l(k) - l(i) * L(k,i)

        for j = m_e+1:end-1
            l(j) = l(j) - l(i) * L_(j,i)
        end
    end

    for i = m_e+1:end-1
        l(i) = l(i) / L(i,i)
        l(end) = l(end) - l(i)^2

        for j = i+1:end-1
            l(j) = l(j) - l(i) * L(j,i)
        end
    end
    l(end) = sqrt(l(end))

Update of z

After Cholesky decomposition we get $\mbm{S}\mbm{\nu} = \mbm{L} \mbm{L}^T \mbm{\nu} = \mbm{L} \mbm{z} = \mbm{s}$

When a constraint is added to the active set, there is no need to perform full forward substitution in order to form $\mbm{\nu}$ (but full backward substitution is still required).

$\mbm{s}^{+} = -\left[\begin{array}{c} \mbm{C} \\ \mbm{a}_i^T\end{array}\right] \left((\mbm{x}+\alpha\Delta\mbm{x})+\mbm{H}^{-1}\mbm{g}\right) = -\left[\begin{array}{c} \mbm{C}\mbm{x} + \mbm{C}\mbm{H}^{-1}\mbm{g} \\ \mbm{a}_i^T(\mbm{x}+\alpha\Delta\mbm{x}) + \mbm{a}_i^T\mbm{H}^{-1}\mbm{g}\end{array}\right] = \left[\begin{array}{c} \mbm{s} \\ s_n \end{array}\right].$

Note that $\alpha\mbm{C}\Delta\mbm{x} = \mbm{0}$ , because $\Delta\mbm{x}$ is in the null space of the normals to the active constraints (stored in $\mbm{C}$ ). Hence, given $\mbm{s}$ , computing $\mbm{s}^{+}$ amounts to performing two multiplications plus one addition (note that $\mbm{H}^{-1}\mbm{g}$ is constant, and $\mbm{x}+\alpha\Delta\mbm{x}$ is already formed).

Now consider the forward substitution

$\underbrace{\left[\begin{array}{cc} \mbm{L} & \mbm{0} \\ \mbm{l}^T & \ell \end{array}\right]}_{\mbm{L}^{+}} \underbrace{\left[\begin{array}{c} \mbm{z} \\ z_n \end{array}\right]}_{\mbm{z}^{+}} = \underbrace{\left[\begin{array}{c} \mbm{s} \\ s_n \end{array}\right]}_{\mbm{s}^{+}},$

where $\left[\begin{array}{cc} \mbm{l}^T \ell \end{array}\right]$ is an appended row. These are two equations

$\\ \mbm{L}\mbm{z} = \mbm{s} \\ \mbm{l}^T\mbm{z} + bz_n = s_n$

From the second one we can compute (note that $\ell\neq0$ )

$z_n = \frac{s_n - \mbm{l}^T\mbm{z}}{\ell}.$

Hence, forming $\mbm{z}^{+}$ amounts to performing one dot product.