# Model Predictive Control of Running Biped Robot
## 4 Hierarchical Motion Control
### Momentum Control Based on Optimization  
In this paper, a QP-based momentum control, taking into account the constraints of the workspace, is proposed to follow the desired COM trajectory. When the floating-base of the biped robot has \(n\) degree-of-freedom (DOF), as shown in Figure 4, its linear and angular momentum, \(\hat{K}\) and \(\hat{H}\), are described by  
\[\hat{L}\triangleq\left[ \begin{array}{c}\hat{K}\\ \hat{H}\end{array} \right]=M_{B}(q_{B},\theta)\hat{q}_{B}+H_{\theta}\hat{\theta}, \tag{27}\]where \(q_{B}\triangleq[q_{l}^{T}\quad q_{a}^{T}]^{T}\in\mathbb{R}^{n}\) is the position and orientation vector of the hip link with respect to the supporting foot; \(q_{l}\) and \(q_{a}\) are respectively the position and orientation of the hip link; \(\theta\) denotes the joint angle of the arms and legs; \(\hat{K}\) and \(\hat{H}\) are respectively the linear and angular momentum of the robot; and \(M_{B}\) and \(H_{\theta}\) are the inertia matrices which indicate how the velocity of hip link and all the joints affect the linear and angular momentum respectively. For the momentum control, the dynamics model is defined based on Equation (27) and the relation between position, velocity, and acceleration in discrete time:  
\[\dot{q}_{B}^{k+1} = A\dot{q}_{B}^{k}+Bu_{B}, \tag{28}\] \[\hat{L}^{k+1} = \hat{M}_{B}\ddot{q}_{B}^{k+1}+H_{\theta}\dot{\theta}, \tag{29}\]  
where  
\[\dot{q}_{B} = \left[ \begin{array}{c}q_{B}^{k}\\ \dot{q}_{B}^{k}\end{array} \right]\in\mathbb{R}^{2n},\] \[A = \left[ \begin{array}{cc}I_{n}&TI_{n}\\ \mathbf{0}_{n\times n}&I_{n}\end{array} \right]\in\mathbb{R}^{2n\times 2n},\] \[B = \left[ \begin{array}{cc}\frac{T^{2}}{2}I_{n}&TI_{n}\\ \mathbf{0}_{n\times n}&M_{B}\end{array} \right]\in\mathbb{R}^{n\times 2n},\]  
\(\mathbf{0}_{n\times n}\) and \(I_{n}\) denote \(n\times n\) zero and identity matrix, respectively; \(u_{B}\in\mathbb{R}^{n}\) denotes the acceleration of the hip link; and \(T\) denotes the sampling time of the controller. By using the dynamic model, the optimization problem is formulated as  
\[\min_{u_{B}}\quad\alpha_{B}^{L}||u_{B}||^{2}+\beta_{B}^{L}||q_{B}^{ref}-\dot{q }_{B}^{k+1}||^{2}+\gamma_{B}^{L}||\hat{L}^{ref}-\hat{L}^{k+1}||^{2}, \tag{30}\]  
where \(\alpha_{B}^{L}\), \(\beta_{B}^{L}\) and \(\gamma_{B}^{L}\) are the weights. Typically, weights in many optimization problems are constant [23; 26; 31]. However, for \(\gamma_{B}^{L}\), a variable weight with respect to the velocity of the biped robot is used instead:  
\[\gamma_{B}^{L}=\gamma_{B}||1+\rho\dot{x}_{COM}||, \tag{31}\]  
where \(\gamma_{B}\) is its initial weight for momentum; \(\dot{x}_{com}\) is the actual horizontal velocity of the biped robot; and \(\rho\) is a constant scale factor for the velocity of the robot. With this, the effect of the actual velocity of the robot is reflected in the momentum control. The optimization problem for momentum control, i.e., Equation (30), can be expressed as a canonical quadratic problem as  
\[\min_{u_{B}}\quad\frac{1}{2}u_{B}^{T}Q_{B}u_{B}+P_{B}^{T}u_{B}, \tag{32}\]  
subject to  
\[\dot{q}_{l}<\dot{q}_{B}^{k+1}<\dot{q}_{u}\]  
where  
\[Q_{B} = \alpha_{B}^{L}E+\beta_{B}^{L}\hat{B}^{T}\hat{B}+\gamma_{B}^{L}( \hat{M}_{B}\hat{B})^{T}(\hat{M}_{B}\hat{B}),\] \[P_{B} = (\hat{A}\dot{q}_{B}-\dot{q}_{B}^{ref})^{T}\hat{B}+(\hat{M}_{B}\hat {A}\dot{q}_{B}-\hat{L}^{ref}+H_{\theta}\dot{\theta})^{T}(\hat{M}_{B}\hat{B}),\]  
where \(\hat{L}^{ref}\triangleq[K^{ref}\quad H^{ref}]^{T}\) denotes reference momentum with \(K^{ref}\) and \(H^{ref}\) respectively denoting the linear and angular reference momentum; and \(\dot{q}_{u}\) and \(\dot{q}_{l}\) respectively denote upper and lower boundary of workspace.