# Model Predictive Control of Running Biped Robot
## 4 Hierarchical Motion Control
### COM Trajectory Generation Based on MPC  
To generate a COM trajectory, the D-LIPM in Section 2 is used. The position and velocity of the COM are used as the states, and the acceleration is used as the control input. The vertical and horizontal dynamic models of the COM are expressed based on the relation between position, velocity, and acceleration in discrete time as follows  
\[\hat{z}(k+1) = A\hat{z}(k)+Bu_{z}(k), \tag{15}\] \[\hat{x}(k+1) = A\hat{x}(k)+Bu_{x}(k), \tag{16}\]  
where  
\[\hat{z}(k) = \left[ \begin{array}{c}z(k)\\ \hat{z}(k)\end{array} \right],\] \[\hat{x}(k) = \left[ \begin{array}{c}x(k)\\ \hat{x}(k)\end{array} \right],\] \[A = \left[ \begin{array}{cc}1&T\\ 0&1\end{array} \right],\] \[B = \left[ \begin{array}{c}\frac{\tau^{2}}{2}\\ T\end{array} \right],\]  
and constant \(T\) denotes the computation step-time interval. Here, \(u_{x}(k)\) and \(u_{z}(k)\) are the accelerations in the horizontal and vertical directions. By using the predict models, the states of a biped robot in the prediction horizon of \(NT\) can be expressed respectively [37]:  
\[\hat{Z}(k+1) = \left[ \begin{array}{c}\hat{z}(k+1)\\ \vdots\\ \hat{z}(k+N)\end{array} \right]=\hat{A}\hat{z}(k)+\hat{B}U_{z}\in\mathbb{R}^{ 2N}, \tag{17}\] \[\hat{X}(k+1) = \left[ \begin{array}{c}\hat{x}(k+1)\\ \vdots\\ \hat{x}(k+N)\end{array} \right]=\hat{A}\hat{x}(k)+\hat{B}U_{x}\in\mathbb{R}^{ 2N},\] (18) \[\hat{Z}_{p}(k+1) = \left[ \begin{array}{c}z(k+1)\\ \vdots\\ z(k+N)\end{array} \right]=\hat{S}_{p}\hat{Z}(k+1)\in\mathbb{R}^{N},\] (19) \[\hat{X}_{p}(k+1) = \left[ \begin{array}{c}x(k+1)\\ \vdots\\ x(k+N)\end{array} \right]=\hat{S}_{p}\hat{X}(k+1)\in\mathbb{R}^{N}, \tag{20}\]where  
\[\mathcal{U}_{x} = \left[ \begin{array}{ccccc}u_{x}(k)&u_{x}(k+1)&\cdots&u_{x}(k+N-1) \end{array} \right]^{T}\in\mathbb{R}^{N},\] \[\mathcal{U}_{z} = \left[ \begin{array}{ccccc}u_{z}(k)&u_{z}(k+1)&\cdots&u_{z}(k+N-1) \end{array} \right]^{T}\in\mathbb{R}^{N},\] \[\hat{A} = \left[ \begin{array}{ccccc}A&A^{2}&\cdots&A^{N}\end{array} \right] ^{T}\in\mathbb{R}^{2N\times 2},\] \[\hat{B} = \left[ \begin{array}{ccccc}B&0&\cdots&0\\ AB&B&\cdots&0\\ \vdots&\vdots&\ddots&0\\ A^{N-1}B&A^{N-2}B&\cdots&B\end{array} \right]\in\mathbb{R}^{2N\times N},\] \[C = \left[ \begin{array}{ccccc}1&0\end{array} \right],\] \[\hat{S}_{P} = \left[ \begin{array}{ccccc}C&0&\cdots&0\\ 0&C&\cdots&0\\ \vdots&\vdots&\ddots&0\\ 0&0&\cdots&C\end{array} \right]\in\mathbb{R}^{N\times 2N}.\]  
The objective function of the vertical motion is defined to follow the reference position and velocity of the COM and keep the \(\lambda\) to the calculated reference value based on Equation (4). The minimized problem for vertical motion can be expressed as  
\[\min_{U_{z}} \alpha^{Z}_{COM}||U_{z}||^{2} + \beta^{Z}_{COM}||\dot{Z}^{ref}-\dot{Z}(k+1)||^{2} \tag{21}\] \[+ \gamma^{Z}_{COM}||U_{z}+\bar{g}-\lambda^{2}\hat{Z}_{P}(k+1)||^{2},\]  
where \(\alpha^{Z}_{COM}\), \(\beta^{Z}_{COM}\) and \(\gamma^{Z}_{COM}\) respectively denote the weights of optimization for control input, position, and \(\lambda\). \(\bar{g}\in\mathbb{R}^{N}\) consists gravitational acceleration, \(g\). \(\dot{Z}^{ref}\) denotes reference position and speed in the vertical direction at the end of each support phase, i.e., \(t_{i}=T^{i}_{s}\), which can be calculated based on Equations (5) and (6) with calculated \(\lambda\). Because the GRF in the vertical direction, \(F_{z}\triangleq m(U_{z}+\bar{g})\), must be positive value in the support phase [26], the contact force in the vertical direction is constrained:  
\[0\leq m(U_{z}+\bar{g})\leq m(U_{z,max}+\bar{g}), \tag{22}\]  
where \(m\) denotes the total mass of the biped robot; \(U_{z,max}\) denotes maximum acceleration in the vertical direction which is applied to constraint \(F_{z}\) within the desired range set by the user.