A sparse MPC solver for walking motion generation (old version).: Derivation of the matrix of equality constraints

$N = 4$

$\\ \tilde{\mbm{c}}_1 = \mbm{A}\tilde{\mbm{c}}_0 + \tilde{\mbm{B}}\mbm{u}_0, \\ \tilde{\mbm{c}}_2 = \mbm{A}\tilde{\mbm{c}}_1 + \tilde{\mbm{B}}\mbm{u}_1, \\ \tilde{\mbm{c}}_3 = \mbm{A}\tilde{\mbm{c}}_2 + \tilde{\mbm{B}}\mbm{u}_2, \\ \tilde{\mbm{c}}_4 = \mbm{A}\tilde{\mbm{c}}_3 + \tilde{\mbm{B}}\mbm{u}_3, \\ \tilde{\mbm{c}}_5 = \mbm{A}\tilde{\mbm{c}}_4 + \tilde{\mbm{B}}\mbm{u}_4.$

$\mbm{E}_c = \left[ \begin{array}{ccccc} -\mbm{I} & \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{A} & -\mbm{I} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{A} & -\mbm{I} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{A} & -\mbm{I} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{A} & -\mbm{I} \\ \end{array} \right], \quad \tilde{\mbm{E}}_u = \left[ \begin{array}{ccccc} \tilde{\mbm{B}} & \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \tilde{\mbm{B}} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \tilde{\mbm{B}} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{B}} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{B}}\\ \end{array} \right], \quad$

$\mbm{e} = \left[ \begin{array}{c} -\mbm{A}\tilde{\mbm{c}}_0 \\ \mbm{0} \\ \mbm{0} \\ \vdots \\ \mbm{0} \end{array} \right].$

$\\ \bar{\mbm{R}}_1\bar{\mbm{c}}_1 = \mbm{A}\bar{\mbm{R}}_0\bar{\mbm{c}}_0 + \tilde{\mbm{B}}\mbm{u}_0, \\ \bar{\mbm{R}}_2\bar{\mbm{c}}_2 = \mbm{A}\bar{\mbm{R}}_1\bar{\mbm{c}}_1 + \tilde{\mbm{B}}\mbm{u}_1, \\ \bar{\mbm{R}}_3\bar{\mbm{c}}_3 = \mbm{A}\bar{\mbm{R}}_2\bar{\mbm{c}}_2 + \tilde{\mbm{B}}\mbm{u}_2, \\ \bar{\mbm{R}}_4\bar{\mbm{c}}_4 = \mbm{A}\bar{\mbm{R}}_3\bar{\mbm{c}}_3 + \tilde{\mbm{B}}\mbm{u}_3, \\ \bar{\mbm{R}}_5\bar{\mbm{c}}_5 = \mbm{A}\bar{\mbm{R}}_4\bar{\mbm{c}}_4 + \tilde{\mbm{B}}\mbm{u}_4.$

$\bar{\mbm{E}}_c = \left[ \begin{array}{ccccc} -\bar{\mbm{R}}_1 & \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{A}\bar{\mbm{R}}_1 & -\bar{\mbm{R}}_2 & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{A}\bar{\mbm{R}}_2 & -\bar{\mbm{R}}_3 & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{A}\bar{\mbm{R}}_3 & -\bar{\mbm{R}}_4 & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{A}\bar{\mbm{R}}_4 & -\bar{\mbm{R}}_5 \\ \end{array} \right], \quad \tilde{\mbm{E}}_u = \left[ \begin{array}{ccccc} \tilde{\mbm{B}} & \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \tilde{\mbm{B}} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \tilde{\mbm{B}} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{B}} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{B}}\\ \end{array} \right], \quad$

$\mbm{e} = \left[ \begin{array}{c} -\mbm{A}\bar{\mbm{R}}_0\bar{\mbm{c}}_0 \\ \mbm{0} \\ \mbm{0} \\ \vdots \\ \mbm{0} \end{array} \right].$