## 6 Recovering Balance  
The first application of the SFPE is to recover balance in the presence of an impulsive disturbance. It is assumed that the biped is in quiet standing balance, conventionally oriented in the direction of the x axis. An impulsive horizontal force in an arbitrary direction generates a velocity of the COG. The biped, in reorienting himself, will react in two different ways, with a minimal rotation around the z axis, according to the relative direction of the pulse: if the direction is closer to his sagittal plane he will mostly move the free leg forward (or backward), if it closer to the frontal plane the motion will be mostly laterally with a side shuffle. To emulate these two distinct situations if the direction is less than 45\({}^{\circ}\) to the sagittal plane the model adopted is the standard one of Figure 2a, otherwise Figure 2b. When the angle \(\theta\) falls below a safety value and the direction is detected from the falling velocity, let say \(\left[v_{x},v_{y},v_{z}\right]^{T}\), the model (a) or (b) is selected according to \(atan2(v_{y},v_{x})\), and initial velocities are assigned to \(\omega\) and \(\gamma\) from the inverse of the first two rows of the matrix of partial velocities (29)  
\[\mathbf{V}^{\text{COG}}=[\mathbf{v}^{\text{COG}}_{\omega}\mathbf{v}^{\text{ COG}}_{\gamma}]. \tag{29}\]  
Then, the SFPE algorithm is run. This defines, among other variables, the swing foot angles \(\alpha,\alpha_{z}\) for the impact that allows recovering the balance in one step. The numerical algorithm does not converge in two cases: when the total energy after the impact is lower than the maximum potential energy, or when one step is not enough to recover the balance. In the first case the condition (27) cannot be satisfied. In the second case the bound on the maximum allowable \(\alpha\) is not satisfied.  
Two examples are presented using the two models (Figure 3). The situation of a push perfectly aligned with either the sagittal or the frontal plane is not considered, as it can be simply solved with the classical 2-D approach. In both cases the initial velocity is of 0.5 m/s and it is detected, at instant 0.5 s, when the falling angle reaches 0.1 rad. The first example, using model (a), presents the response to a push at an angle of 20\({}^{\circ}\) from the x axis, in the second, with model (b), the angle is of 110\({}^{\circ}\).  
Apart from a difference in the sign of the angles, and the velocities the two behaviours are very similar, so the case of model (b) is detailed, only.  
The original central inertia matrix of the example, the same of the biped of [20], given in the Appendix A, had the elements I12 and I23 equal zero, the correct responses of angles (speeds and positions) in Figure 4a,b are represented with solid lines. To see the differences, fictitious values different from zero have been assigned to I12 and I23, and the example rerun. The responses are indicated in the legends with a X, and plotted with dashed lines.  
From the SFPE, the flying foot final position can be forecasted and the SIP COG trajectory used as a reference, knowing the kinematics, to control the joints of a real biped robot or exoskeleton to recover balance.