# Quadrupedal Robots Whole-Body Motion Control Based on Centroidal Momentum Dynamics
## 2 Methods
### Application to Quadrupedal Robot Dynamic Motion
#### 2.3.3 QP Formulation  
Given a admissible momentum rate change \(\dot{h}_{G,d}=\left[\bar{I}_{G,d}^{T}\quad k_{G,d}^{T}\right]^{T}\), we design a QP to solve for a vector \(\chi_{d}=\left[\bar{\theta}_{d}^{T}\quad f_{d}^{T}\right]^{T}\in\mathbb{R}^{18 +k}\) of the desired joint accelerations and GRFs for the full robot dynamics that minimizes Equation (27), which is a quadratic motion cost to track the reference rate of change of the centroidal momentum as closely as possible while satisfying the dynamic, input, and contact constraints of the full quadruped robot dynamics.  
\[\underbrace{\left[A\quad 0\right]}_{E}\chi=\underbrace{\dot{h}_{G,d}-\dot{A} \theta}_{b} \tag{26}\]  
Minimize  
\[(E\chi-b)^{T}Q\left(E\chi-b\right)+f^{T}Wf \tag{27}\]  
Subject to  
\[\left[H_{11}\quad H_{1*}\quad-J_{body}^{T}\right]\chi_{d}=C_{1}\dot{q}+G_{1} \tag{28}\]  
\[\left[I_{stance}\quad 0_{K\times 24-K}\right]\chi_{d}=-\ \left[J_{stance}\quad 0_{K \times 12-K}\right]\dot{\theta}\ -\alpha\left[I_{stance}\quad 0_{K\times 12-K} \right]\dot{\theta} \tag{29}\]  
\[\left[I_{swing}\quad 0_{18+K\times 18+K}\right]\chi_{d}=-\ \left[I_{swing}\quad 0_{K \times K}\right]\dot{\theta}+p_{swing} \tag{30}\]  
\[f_{i,d}^{z}\geq 0,\left|\mu f_{i,d}^{z}\right|\geq f_{i,d}^{x}\left|\mu f_{i,d }^{z}\right|\geq f_{i,d}^{y} \tag{31}\]  
\[\tau_{min}-C_{*}\dot{q}-G_{*}\leq\left[H_{1}\quad H_{*}\quad-J_{leg}^{T} \right]\chi_{d}\leq\tau_{max}-C_{*}\dot{q}-G_{*} \tag{32}\]  
The matrices \(Q\) and \(W\) are weighting matrices. The relationship between the accelerations \(\ddot{\theta}\) and the GRFs \(f\) can be found though the floating base system dynamics equation constraint (28). Since legs in contact with the ground are not allowed to move, Equation (29) is a no-slip constraint on the footcontacts, requiring that their acceleration be negatively proportional to the velocity. To improve the track of the desired motion determined by the high level controller, we constrain the joint accelerations and by implementing operational space controllers with feed-forward reference acceleration and a motion dependent feedback state in the constraint in Equation (30). Where  
\[\dot{p}_{swing} =\dot{p}_{swing,d}+k_{p}(p_{swing,d}-p_{swing})+k_{i}\int{(p_{ swing,d}-p_{swing})dt}+k_{d}(\dot{p}_{swing,d}-\dot{p}_{swing}) \tag{33}\]  
To avoid slipping, the constraint ensures that contact forces should be constrained to lie in the friction cone, which is aligned normal to the contact surface, where \(\mu\) is the friction coefficient. To write these as linear constraints, we approximate the friction cones with pyramids, which was proposed by [PERSON] in [36]. Equation (32) ensure that the joint actuation torques cannot exceed the rated torques.  
The formulation is solved based on Active-set method. This QP is solved for every controller time step for the joint acceleration and the GRFs. Given the desired joint acceleration and the GRFs, we compute the reference joint torques though  
\[\tau=\left[H_{*1}\quad H_{**}\quad-J_{leg}^{T}\right]\chi_{d}+C_{*}\dot{q}+G_{*} \tag{34}\]