# Quadrupedal Robots Whole-Body Motion Control Based on Centroidal Momentum Dynamics
## 2 Methods
### Dynamics Equation of Floating Base Systems  
Dynamic-based approaches play an important role in legged robot dynamic locomotion because they have strong disturbance resisting ability. The modelling presents an inherent complexity, mainly due to the joint variables must satisfy several loop-closure constraint equations. Several algorithms have been used to the dynamic modelling of floating base systems. In this paper, we employ the Newton-Euler inverse dynamics algorithm organized into a closed-form [24] to establish the dynamics equation of the quadruped robot. Consider the quadrupedal robot shown in Figure 1. For modelling purposes, we attach a base reference frame to the robot and assume that the base frame can freely move relative to a fixed inertial frame. Figure 1 illustrates this representation by showing the 6 virtual degrees of freedom (\(\theta_{b}\in\mathbb{R}^{6}\) is the position and orientation of the coordinate system attached to the robot base, measured with respect to the fixed inertial frame). As shown in Figure 2. The system can be partitioned into four subsystems (the inertia tensor for the body is also divided into four parts). We can write the dynamic equation for the four chains as follows:  
\[H_{i}(\theta_{i})\ddot{\theta_{i}}+C(\theta_{i},\theta_{i})+G_{i}(\theta_{i}) \text{=}{S_{i}}^{T}\tau_{i}+{j_{i}}^{T}f_{i},\ i=RF,LF,LH,RH \tag{1}\]  
where \(H_{i}(\theta_{i})\in\mathbb{R}^{9\times 9}\), \(C_{i}(\theta_{i},\theta_{i})\in\mathbb{R}^{9}\), \(G_{i}(\theta_{i})\in\mathbb{R}^{9}\), \(\theta_{i}\text{=}\left[{{r_{b}}^{T}}\quad{{q_{b}}^{T}}\quad{{q_{i}}^{T}} \right]^{T}\in\mathbb{R}^{9}\), \(S_{i}\text{=}\left|{0_{3\times 6}}\;{{I_{3\times 3}}}\right|\) and \(\tau_{i}\in\mathbb{R}^{3}\) denote the mass matrix, the force vector as the sum of the Coriolis and centrifugal forces, gravitational forces, the actuated joint selection matrix and the actuator torque, respectively. Limbs in contact with the ground are modelled as point contacts (with 3 forces). The Ground Reaction Forces (GRFs) at the contact points are mapped to the joint space torques through the Jacobian \(J_{stance,i}=[J_{stance,i}^{body}\;J_{stance,i}^{leg}]\in\mathbb{R}^{3\times 9}\). Figure 1: The base frame attached to the robot is connected to the inertial frame via 6 unactuated virtual DOFs. Figure 2: Segmentation of the floating base systems. For floating-base systems, such as a quadruped robot, the dynamic in Equation (1) can be divided into floating-base and actuated components  
\[ \begin{bmatrix}H_{i}^{11}(\theta_{i})&H_{i}^{1*}(\theta_{i})\\ H_{i}^{*1}(\theta_{i})&H_{i}^{**}(\theta_{i})\end{bmatrix}\begin{bmatrix}\ddot{ \theta}_{b}\\ \dot{q}_{i}\end{bmatrix} + \begin{bmatrix}C_{i}^{1}(\theta_{i},\dot{\theta}_{i}) \\ C_{i}^{*}(\theta_{i},\dot{\theta}_{i})\end{bmatrix} + \begin{bmatrix}G_{i}^{1}( \theta_{i})\\ G_{i}^{*}(\theta_{i})\end{bmatrix} = \begin{bmatrix}\begin{array}{cc}{J_{stance, i}^{body}}^{T}f_{i}\\ \tau_{i}+{J_{stance,j}^{leg}}^{T}f_{i}\end{array}\end{bmatrix}  \tag{2}\]  
where \(\theta_{b}\)=\( \begin{bmatrix}{r_{b}}^{T}&{q_{b}}^{T}\end{bmatrix} ^{T}\in\mathbb{R}^{6}\). All the four subsystems are clustered together, and we obtain the dynamics equation of the quadruped robot in a closed form  
\[H(\theta)\dot{\theta}+C(\theta,\dot{\theta})+G(\theta)\text{=}S^{T}\tau+J^{T}\,f \tag{3}\]  
where  
\[H(\theta)= \begin{bmatrix}\sum\limits_{i=RF}^{RH}H_{i}^{1}(\theta_{i})&H_{RF}^ {1*}(\theta_{RF})&H_{LF}^{1*}(\theta_{LF})&H_{LB}^{1*}(\theta_{LB})&H_{RB}^{1* }(\theta_{RB})\\ H_{RF}^{1}(\theta_{RF})&H_{RF}^{**}(\theta_{RF})&0&\dots&0\\ H_{LF}^{1}(\theta_{LF})&0&H_{LF}^{**}(\theta_{LF})&\dots&0\\ H_{LB}^{*1}(\theta_{LB})&\vdots&\vdots&H_{LB}^{**}(\theta_{LB})&0\\ H_{RB}^{*1}(\theta_{RB})&0&0&0&H_{RB}^{**}(\theta_{RB})\end{bmatrix} \in\mathbb{ R}^{18\times 18} \tag{4}\]  
\[C(\theta)=