## 3 Model and Dynamics
### Dynamics  
The robot is described with the generalized coordinates \(\mathbf{q}\in\mathbb{R}^{n_{dof}}\), where \(n_{dof}=21\), with 15 internal DOF and six external DOF describing the floating base, with three translations along \(x,y,z\) and three rotations about the same axis represented with Euler angles.  
The dynamics of a rigid multi-body system such as a robot can be described by the equations of motion:  
\[\mathbf{H}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}} )=\boldsymbol{\tau}, \tag{1}\]  
where the matrix \(\mathbf{H}\) is the joint space inertia matrix, \(\ddot{\mathbf{q}}\) is the joint acceleration vector, \(\mathbf{C}\) the Coriolis, centrifugal and gravitational term and \(\boldsymbol{\tau}\) are the joint torques. Given that the robot is a floating base system, the vector of joint torques is assumed to be:  
\[\boldsymbol{\tau}=\begin{bmatrix}\mathbf{0}\\ \boldsymbol{\tau}_{A}\end{bmatrix}, \tag{2}\]  
where \(\boldsymbol{\tau}_{A}\in\mathbb{R}^{15}\) is the vector of active joint torques.  
We introduce the following set of constraints due to contacts:  
\[\mathbf{g}(\mathbf{q})=\mathbf{0}, \tag{3}\]  
which depend on the walking phases, as previously described.  
In our formulation, we assume that contacts are perfectly rigid and non-sliding and impacts are instantaneous and inelastic. Therefore, taking into account (3) in (1), we obtain:  
\[\mathbf{H}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}} )=\boldsymbol{\tau}+\mathbf{G}^{T}\boldsymbol{\lambda}, \tag{4}\]  
where \(\boldsymbol{\lambda}\) is the vector of external forces due to the contacts and \(\mathbf{G}==(\partial\mathbf{g}/\partial\mathbf{q})\) is the contact Jacobian.  
Figure 3: Feet shapes of the HeiCub robot. Three points are defined for contact modeling on each foot.  
An additional set of equations for the contact constraints is obtained by differentiation twice of Equation (3):  
\[\mathbf{G}(\mathbf{q})\mathbf{\ddot{q}}+\mathbf{\dot{G}}(\mathbf{q})\mathbf{ \dot{q}}=\mathbf{0}. \tag{5}\]  
Combining Equations (4) and (5), we obtain the following system of equations for unknown \(\mathbf{\ddot{q}}\) and \(\Lambda\):  
\[\begin{bmatrix}\mathbf{H}&\mathbf{G}^{T}\\ \mathbf{G}&\mathbf{0}\end{bmatrix}\begin{bmatrix}\mathbf{\ddot{q}}\\ -\Lambda\end{bmatrix}=\begin{bmatrix}\mathbf{\tau}-\mathbf{C}\\ -\mathbf{\gamma}\end{bmatrix}, \tag{6}\]  
where \(\mathbf{\gamma}=\mathbf{\dot{G}}(\mathbf{q})\mathbf{\dot{q}}\). This system describes the dynamics of the continuous phases of single and double supports, i.e., DS, LSS and RSS. In the case of DS, the number of contacts is six, i.e., both feet have flat contacts with the ground, while in the case of LSS and RSS, the number of contacts is three, i.e., only one foot has flat contact with the ground.  
Due to the assumptions on the impacts, the dynamics describing the instantaneous change in the generalized velocities can be obtained by integrating Equations (4) and (5) over a time singleton. The following system of equations can be written for the unknown generalized velocities after the impact \(\mathbf{\dot{q}}^{+}\) and the impulses at each constraint \(\Lambda\):  
\[\begin{bmatrix}\mathbf{H}&\mathbf{G}^{T}\\ \mathbf{G}&\mathbf{0}\end{bmatrix}\begin{bmatrix}\mathbf{\dot{q}}^{+}\\ -\Lambda\end{bmatrix}=\begin{bmatrix}\mathbf{H}\mathbf{\dot{q}}^{-}\\ \mathbf{v}^{+}\end{bmatrix}, \tag{7}\]  
where \(\mathbf{\dot{q}}^{-}\) are the generalized velocities before the impact, and \(\mathbf{v}^{+}\) the desired velocity of contact points after the impact, which is \(\mathbf{0}\) due to the previously-described assumptions. The system as in Equation (7) describes the dynamics of the impacts LTD and RTD as in Figure 2.