A Novel Paradigm for Controlling Navigation and Walk in Biped Robotics

1. Introduction

Originally balance in the gait of biped robots was achieved controlling that the pressure center under the soles stayed in the supporting polygon of the feet during the whole step. The terminology of zero moment point (ZMP) was introduced [1].

In that period, fundamental was the work of Kajita [2,3] that introduced, using the inverted pendulum and imposing a constant hight of the center of gravity (COG) the linearized inverted pendulum model (LIPM). He showed with the LIPM that in a flat surface the trajectories of the pressure point in the frontal and sagittal planes were decoupled, and a simple linear relationship linked the ZMP with the projection of the COG position and COG acceleration on the two horizontal axes ($ZM{P}_{x,y}=CO{G}_{x,y}-\frac{CO{G}_z}{g}·{\ddot{COG}}_{x,y}$).

In order to be able to transfer the needed torque to the ground the feet were maintained flat, with a walk of the robot unrealistic and energetically inefficient.

In a second phase the rotation of the feet with respect to the ground was introduced. The control, during the gait, was divided in phases, where during one of the phases, when on the tip of the foot, the control was underactuated [4,5].

In reality, human-like gait, with its mix of fully actuated and underactuated phases (where walking during one of the phases is a “controlled falling”) is more complex [6]. Push recovery, walking on rough terrain, and agile footstep control are active research topics [7].

On a completely different line of approach, in the realm of passive walkers and of hybrid zero dynamics [8,9,10], starting from the passive motion of the rimless wheel falling on an inclined surface, and ending to the inverted pendulum with a compass, stability of the gait in the whole was proven in spite of dynamical instability inside each step.

The model used was the spherical inverted pendulum (SIP) in polar coordinate system, i.e., the 2 DOFs of the pendulum are the rotation around the vertical axis, and one of the horizontal axes [11,12]. With the SIP model the problem of gait is intertwined with the estimation in 3-D of the swing foot placement at the collision with the ground (FPE) [13,14,15,16,17]. This has been obtained using energy relationships, observing that the total energy, and the partial derivative of the kinetic energy with respect to the rotational velocity (i.e., the angular momentum) along the vertical axis are constant during a step.

In a first paper [18] this author using the SIP model with 2 DOFs achieved an omnidirectional walk without any torque control, but simply, exploiting the fall of the pendulum and the anelastic restitution of the collision of the swing foot with the ground. At each step, before the next, the initial rotational velocities after the collision were properly increased to cope with the losses, and the angles of the swing leg were set to define the position of the next foot placement for balance and for navigation. So doing, the direction, the speed of the walk, the length of the steps where controlled.

To overcome certain limitations, expecially the COG sway in the frontal plane, in a second paper [19] the SIP model, always with 2 DOFs, was extended adding a pelvis width and introducing the distance between the hips of the two legs.

In this paper, the model of this represention is, one more time, extended with 3 rotational DOFs, where pelvis width, distance of the supporing feet and sway of COG in the frontal plane, can be controlled. Moreover, to allow to go up and down staircases one further prismatic DOF is added to modify the length of the supporting leg during the step.

With respect to the classical gait control the problem has here been reversed. Instead of two phases of the step (double and single stance): based on controlling foot torques during double support, and the first part of single support when the foot is flat, and handling underactuated control when the robot is on the foot’s tip, the whole step is always in free fall, and the changes of the velocities, with respect to the values given by the collision, before the next step are virtually the results, of a (impulsive) torque action during the double stance phase, that here has time period zero.

In Section 2 the model is presented. The gait control is reviewed from previous papers [18,19], by addding some new aspect, in Section 3. Examples of going up and down the staircases, intertwined with the navigation is presented in Section 4. The Foot placement estimation for ending in equilibrium a walk is discussed in Section 5. Conclusions and forecast of further extensions are in Section 6. Appendices report from [18], in Appendix A the Kane’s method used for the simulation, and in Appendix B the simbolic formulas for FPE on the balanced arrival.

2. The Spherical Inverted Pendulum with Pelvis Width and Changing Length of the Supporting Leg

The model adopted in this paper adds, with respect to the original SIP, in the compass, the width of the pelvis and the distance between the hips of the two legs. It has 3 rotational DOFs, instead of 2, for walking on a flat horizontal surface, and one more prismatic DOF on the supporting leg for going up and down staircases. In fact the control of the only three rotational DOFs is not enough in this case. Its kinematics is represented in Figure 1a. The axes of the local frames of the segments have a similar disposition as the inertial frame N. The multibody is composed of four segments: the supporting leg, composed of two parts connected with a prismatic link, the flying leg and the pelvis. The two legs are massless. Only the pelvis, representing also the upper body of the biped, has a mass and an inertia.The prismatic link in the supporting leg, $le{g}_1$, is controlled in position by a force $F_1$, through a spring and a damper (Figure 1b), using a step reference when climbing and descending staircases, and it is connected to the pelvis through the joints with angles $2·{\alpha }_1$ and $2·{\alpha }_{z1}$. These angles are constant during a step and are set, at the beginning of each step, to maintain the orientation of the pelvis in the hybrid model at the pivot switching. The 3 DOFs offered to the SIP by the supporting leg are represented by the joints of angles ${\theta }_z$, $\theta$ and ${\theta }_x$. They define, in body coordinate 3-2-1 (ZYX), the orientation of the leg’s frame. The length of the swing leg, $le{g}_2$, is set and maintained according to the height of the steps of the staircase (shorter when climbing, at the full length when descending). The leg is connected to the pelvis through the joints of angles $2·\alpha$ and $2·{\alpha }_z$. Also these angles are constant during a step and are set, at each step, to define the future position of the swing foot at the ground collision. The characterist points of the model are the two feet ($F_1$ and $F_2$), the two hips ($P_1$ and $P_2$) and the COG. The two legs have identical length when walking on a flat horizontal surface, while the supporting leg has a transition from an initial value, identical to the swing leg length, to a final value during a step going up or down a staircase.

A parameter, called here $"u"$, assuming values $\pm 1$, accounts for right and left supporting foot in the hybrid simulation, otherwise the models are identical in the two cases.

When the prismatic joint is locked, the model has 3 DOF plus 3 positions of the supporting foot $x,y,z$. The dynamics is of order 12 with configuration variables ${\theta }_z,\theta ,{\theta }_x,x,y,z$, and corresponding motion variables $\gamma ,\omega ,{\gamma }_x,u_1,u_2,u_3$. However, a non-holonomic constraint imposes a fixed position of the pivot foot ($u_1=u_2=u_3=0$) during the swing. When moving on a staircase, the prismatic joint adds 2 degrees to the dynamics: position l with velocity $ul$ contributing to the length $L_1$ of the supporing leg (i.e., ${\theta }_z,\theta ,{\theta }_x,l,x,y,z$ and $\gamma ,\omega ,{\gamma }_x,ul,u_1,u_2,u_3$). The non-holonomic constraint is released, only, at the collision of the swing foot with the ground to define, through anelastic restitution, the initial motion values for the next step ${\gamma }^{+},{\omega }^{+},{\gamma }_x^{+},u_1^{+},u_2^{+},u_3^{+}$, and in the case of $u{l}^{+}$. For all the details of the dynamics refer to the father paper [18] and to the appendices Appendix A and Appendix B.

Switching Supporting Foot after Ground Collision

The touch down is given when the vertical position of foot $F_2$ reaches the ground, and the anelastic collision defines ${\gamma }^{+}$, ${\omega }^{+}$, ${\gamma }_x^{+},u_1^{+},u_2^{+},u_3^{+}$ and $u{l}^{+}$. The switching of pivot feet is computed in two steps, through non-linear least squares:

• by equating the direction cosine matrix of the swing leg frame at the touch down to a new frame expressed by orientation angles in body coordinate 3-2-1 (ZYX), and assigning these angles, as initial values, to ${{\theta }_z}^{+}$ , ${\theta }^{+}$ and ${{\theta }_x}^{+}$ to the new supporting leg;
• computing ${{\alpha }_1}^{+}$ and ${{\alpha }_{z1}}^{+}$ in order to guarantee the constancy of the direction of the y axis of the pelvis local frame before and after the switching of the pivot foot and resetting to zero the previous rotation along this axis.

3. The Gait

In the new paradigm the control of the gait of the hybrid system is performed by assigning initial values to $\gamma$, $\omega$, ${\gamma }_x$, ($ul=0$) and to $le{g}_2$$\alpha$ and ${\alpha }_z$ at the beginning of each step. In previous works [18,19] the model with 2 DOFs behaved as a 3-D inverted pendulum and, in particular [19], the y axis of the pelvis always remained horizontal during the whole step. Global stability was assured assigning any reasonable initial values of $\gamma$, $\omega$. In the present case the model with 3 DOF adds a rotation of the pelvis along its x axis. Global stability can be achieved only if the rotation along the x axis is regulated in cloed loop by controlling al the beginning of each step the initial value of ${\gamma }_x^{+}$ (responsible for the motion) from the samples ${\theta }_x^{+}$ after the previous collision. This was achieved alternatively with a sampled data PID feedback or a self tuning regulator [20]. The self tuning regulator, in particular, was adopted to minimize the variance of the oscillations of ${\theta }_x$.

The expressions of section 5 of [18], properly modified in Appendix B, for FPE, at difference of [13,14,15,16], are not directly used here at each step; they are exploited to impose a stop at the end of the walk.

In closed loop, a perturbation to $\alpha$ is chosen to control the desired step length, to ${\alpha }_z$ to control the desired distance, along the y axis, of the supporting foot, the COG sway and also the offset with respect to the desired baseline in the navigation (as usual, the real time, sampled data, control is achieved by assigning at the beginning of every step the initial values of the motion variables and of the flying leg angles for the next step, based on the final angles and position values of the flying foot at touchdown of the previous step.). The gait is initiated giving an initial condition to ${\theta }_z,\gamma ,\theta ,\omega ,{\theta }_x,{\gamma }_x$, or, simply, from a standing up balance by leaving the pendulum to fall forward. Each step is concluded when the swing foot touches the ground (the vertical coordinate of point $F_2$ becomes zero, Equation (A22)). From the impact Equation (A20) the new motion variables ${\gamma }^{+},{\omega }^{+},{\gamma }_x^{+},u_1^{+},u_2^{+},u_3^{+}$ are determined (consequently the kinetic energy results, also), and from Equations (A24) and (A25) the starting values of ${{\theta }_z}^{+},{\theta }^{+},{\theta }_x^{+},{\alpha }_1^{+}$ and ${\alpha }_{z1}^{+}$ for a new step are computed. The gait is maintained by increasing at each step ${\omega }^{+}$ and ${\gamma }^{+}$, resulting after impact, to compensate for the reduction of kinetic energy: the first to guarantee the desired gait cadence, the latter to correct the direction of walk. Vice versa, ${\gamma }_x$ is controlled in closed loop directly from ${\theta }_x^{+}$ to maintain horizontality and minimize the oscillations of the pelvis around the x axis.

In the present model the two legs have not mass and inertia. So, the motions of the angles $\alpha$ and ${\alpha }_z$ are istantaneous and energy free. In a horizontal ground the only energy contribution to mantain the walk is given by proper impulsive forces and torques just after the impact to modify the velocities ${\gamma }^{+}$, ${\omega }^{+}$ and ${\gamma }_x^{+}$ resulting from the impact. This emulates, in a real walk, the contribution given by the biped in the brief double support phase and in the period of single support when the foot is flat and able to transfer torques. On a staircase, the length of the supporting leg has to be controlled during the whole step and the energy related to its motion has to be added.

Six control variables are identified to control six objectives of the walk: $average$$horizontality$ and variance of $oscillation$ around the x axis of the pelvis, the $cadence$, $steplength$, $distance$$between$$feet$, y$offset$ with respect to the baseline of walk, and $direction$$of$$walk$ (Even if interacting each other, each of the six variables predominantly controls one of the six objectives). After each impact, at the start of step k they are

$$\begin{array}{c}{\delta }_{{\gamma }_x}\left(k\right)\Rightarrow {\gamma }_x\left(k\right)=-gain·{\theta }_x^{+}(horizontality of pelvis)\\ {\delta }_{\omega }\left(k\right)\Rightarrow \omega \left(k\right)={\omega }^{+}+{\delta }_{\omega }\left(k\right)\left(cadence\right)\\ {\delta }_{\alpha }\left(k\right)\Rightarrow \alpha \left(k\right)={\alpha }_0+{\delta }_{\alpha }\left(k\right)(step length)\\ {\delta }_{sway}\left(k\right)·u+{\delta }_y\left(k\right)\Rightarrow {\alpha }_z\left(k\right)=({{\alpha }_z}_0+{\delta }_{sway}\left(k\right))·u+{\delta }_y\left(k\right)\\ (spacing between feet and y offset)\\ {\delta }_{\gamma }\left(k\right)\Rightarrow \gamma \left(k\right)={\gamma }^{+}+{\delta }_{\gamma }\left(k\right)\left(direction\right)\end{array}$$

(1)

where u assumes the values $+1,-1$ according to the right or left foot support.

It must be noted that no periodic reference is tracked. The whole gait style (cadence, length of the step, offset with respect to the baseline of walk-through a side shuffle, spacing between the two feet and direction) can be changed at each step.

The next Figure 2 and Figure 3 show a sample of a typical rectilinear walk on an horizontal ground. In Figure 2b the ZMP is estimated using the Kajita’s formulas.

The two Figure 4, vice versa, show the total energy and the angular momentum about the vertical axis during the ascending of a staircase.

As it can be seen in Figure 4 the total energy si not constant in the first period of the step time, due to the change of length of the supporting leg. In this case the properties of a pure ballistic motion are lost because an energy is injected into the system during the step.

4. Going Up and Down the Staircases

The strategy of Equation (1) adds to the initial conditions of ${\gamma }_z$ and $\omega$ the values needed to compensate the reduction of rotational energy due to the impact, but not to move the pendulum in a vertical direction. For this, the prismatic link has been added. If going up the staircase (Figure 5), the flying leg is shortened, with respect to the full length, of the value of the stair’s step. This same length will be the initial value of the next supporting leg, that during the first period of the step will have a transition to full length. Vice versa, if going down (Figure 5) , the flying leg is at full length, and the supporting leg will be shortened, during the step, from the full length to a value corresponding to the heigth of the stair’s step. In the paper this transition is achieved with a force control of the prismatic link tracking with a PD filter a step reference. The values of the filter allow to model, also, the rigidity and damping of the knee.

Two examples are presented. The first (Figure 6a) embeds, also, a navigation control on a spiral staircase with a ray of 5 meters. The second (Figure 6b) shows the $CO{G}_y$ and $CO{G}_z$ going up and down a rectilinear staircase. In the second example let note a perturbation in the sway on the frontal plane when the direction is changed, but it is quickly recovered.

5. Foot Placement Estimation

The formulas of the appendices Appendix A and Appendix B for the foot placement estimation are used for stopping in equilibrium at the end of the walk.

On a staircase the computation is performed when the transient on l has been concluded ($ul\approx 0$) and in the formulas the transient is ignored. However, for simplicity an example on horizontal ground is shown in Figure 7.

Obviously, reaching the quasi-equilibrium position after the last contact, given the pelvis width, the biped has to move in double support. In the example it happens at instant 3.5 sec. At this instant let note the non perfect zeroing of the kinetic energy and momentum (hence the angular velocities). This is due to the fact that, for simplicy, the FPE computation is performed at the maximum hight of the COG as a function of $\theta$ and ${\theta }_x$, but this value in reality is not reached.

6. Conclusions

With this model of the biped the SIP has been extended with a minimum increase of the dynamics. It is possible to control in real time all parameters of the gait with an omni-directional walk on the flat ground or on the staircases. However, the purely ballistic motion of the pendulum is lost. Then, during the walking step the constancy of the total energy, on which is based the FPE, is no longer true. Nevertheless, FPE can, still, be evaluated performing the computations after the transient, setting in the formulas $u{l}^{+}=0$ and adopting two models before and after the last contact with the length of the supporting leg at its initial and final value, respectively.

Two extensions of the present approach are envisaged:

• adding a compliance to the swing leg to accomodate roughness in the ground and inducing a finite double support period [21];
• using the present approach for an alternative control, with respect to [22], of a complete robot with 12 DOFs.

Appendix A.1. Generalized Coordinates and Speeds

A multi-body system, which possesses n degrees of freedom, is represented by a state with a n-dimensional vector $\mathbf{q}$ of configuration variables (generalized coordinates) and an identical dimension vector $\mathbf{u}$ of generalized speeds called also motion variables, that could be any nonsingular combination of the time derivatives of the generalized coordinates that describe the configuration of a system. These are the kinematical differential equations:

$$u_r=\sum _{i=1,\cdots ,n}{Y}_{ri}\dot{q_i},r=1,\cdots ,n$$

(A1)

$Y_{ri}$ may be in general nonlinear in the configuration variables so that the equations of motion can take on a particularly compact (and thus computationally efficient) form with the effective use of generalized speeds.

Appendix A.2. Partial Velocities and Angular Velocities

Partial velocities of each point (partial angular velocity of each body) are the n three-dimensional vectors expressing the velocities of that point (angular velocity of that body) as a linear combination of the generalized speeds. Let be ${\mathbf{v}}^B$ the translational velocity of a point B and ${\omega }^P$ the rotational velocity of a body P with respect to the inertial reference frame, then

$$\begin{array}{c}{\mathbf{v}}^B=\sum _{r=1...n}{\mathbf{v}}_r^B{u}_r\\ {\omega }^P=\sum _{r=1...n}{\omega }_r^P{u}_r\end{array}$$

(A2)

where ${\mathbf{v}}_r^B$ and ${\omega }_r^P$ are the rth partial velocity and partial angular velocity of B and P, respectively.

Appendix A.3. Generalized Active and Inertia Forces

The n generalized forces acting on a system are constructed by the scalar product (projection) of all contributing forces and torques on the partial velocities and partial angular velocities of the points and bodies they are applied to.

Let us consider a system composed by N bodies $P_i$, where the torque ${\mathbf{T}}^{P_i}$, and force ${\mathbf{R}}^{B_i}$ applied to a point $B_i$ of $P_i$ are the equivalent resultant (“replacement” [23]) of all active forces and torques applied to $P_i$. Then

$$F_r^{P_i}={\omega }_r^{P_i}·{{\mathbf{T}}_i}^{P_i}+{\mathbf{v}}_r^{B_i}·{\mathbf{R}}_r^{B_i}$$

(A3)

is the rth generalized active force acting on $P_i$ and

$$F_r=\sum _{i=1,\cdots ,N}{F}_r^{P_i}$$

(A4)

the rth generalized active force acting on the whole system. Identically for the inertia forces, indicated as $F_r^{*}$.

The dynamical equations for an n degree of freedom system are formed out from generalized active and inertial forces $F_r^{*}$

$$F_r+F_r^{*}=0,r=1,\cdots ,n.$$

(A5)

These are known as Kane’s dynamical equations.

They result in a n-dimensional system of second order differential equations ($2n$ order state variable representation) on generalized coordinates and speeds

$$\overline{\mathbf{M}}\left(\mathbf{q}\right)\dot{\mathbf{u}}+\overline{\mathbf{C}}(\mathbf{q},\mathbf{u})\mathbf{u}+\overline{\mathbf{G}}\left(\mathbf{q}\right)-\overline{\mathit{\Gamma }}(\mathbf{q},\mathbf{u},\tau )=\mathbf{0},$$

(A6)

where the parameter definitions are similar but not identical of the classical Lagrangian form and more efficient computationally [25].

Appendix A.4. Non-Holonomic Constraints

When m constraints on the motion variables are added to the model, only $n-m$ generalized speeds are independents.The system is, then, called a non-holonomic system. The non-holonomic constraints are expressed as a set of m linear relationships between dependent and independent generalized speeds of the type

$$u_r=\sum _{i=1,\cdots ,p}{A}_{ri}{u}_i,r=p+1,\cdots ,n,$$

(A7)

with $p=n-m$. In this case, selected the independent speeds, the Kane’s method immediately offers the minimal $2p$ order state variable representation from

$${\tilde{F}}_r+{\tilde{F}}_r^{*}=0,r=1,\cdots ,p,$$

(A8)

where Kane calls ${\tilde{F}}_r$ and ${\tilde{F}}_r^{*}$ non-holonomic generalized active and inertial forces, while the remaining m original redundant equations resolve themselves in the expressions of the m reaction forces/torques returned by the constraints. Because the Kane’s method is fundamentally based on the projection of forces on a tangent space on which the system dynamics are constrained to evolve, spanned by the partial velocities, reaction forces/torques result from the projection on its null-space.

Moreover, it is always possible to handle an holonomic (configuration) constraint as if it is non-holonomic, that is, to treat it as a motion constraint. This is particularly advantageous to represents the spherical inverted pendulum with a compass during a step, where in the first phase non-holonomic constrains allow pivoting on the supporting leg, and in the second phase, releasing the non-holonomic constraints the impact of the swing leg with the ground can be represented.

Appendix A.5. Unilateral Constraints and Collision

As a consequence of switching between different non-holonomic models during gait, unilateral constraints and collisions cannot be ignored.

Clearly, adopting non-holonomic dynamics assuming points of the feet fixed to the ground is valid for bilateral constraints (ignoring eventual detachment from the ground and slipping). In the approaches known as hybrid complementarity dynamical systems based on forward dynamics [26] the necessary conditions for satisfying unilateral constraints are directly embedded into the model. Vice versa, a minimalistic view is adopted here, noting that in a physiological gait, normally, bilateral constraints on the feet are not assumed to be violated. Hence, we design a priori walking strategies and we test through the simulator that this effectively occurs, by monitoring, a posteriori, reaction forces for the conditions:

$$F_{z_{foo{t}_i}}>0,i=1,2$$

(A9)

and

$$|F_{j_{foo{t}_i}}|<\mu F_{z_{foo{t}_i}},j=x,y,i=1,2.$$

(A10)

Obviously, the control we propose cannot adapt itself to pathological conditions, such as a slipping surface.

For the second point, mechanics of the collision of the swing foot to the ground has to be considered, when switching to the next step causes the transfer of final conditions of the generalized speeds of one phase to the initial conditions of the successive. With reasonable assumptions of non-slipping and anelastic restitution the reaction impulsive force ${\mathbf{F}}^B$ at the impact point B and the initial conditions of the generalized speeds for the new phase $\mathbf{u}\left(t^{+}\right)$ can be computed. Also for this aspect, Autolev offers all needed mechanical expressions.

The following analysis is based on two concepts: generalized impulse and generalized momentum [23,27]. Indicate, as usual, with ${\mathbf{v}}_r^B$ the r-th component of the partial velocity vectors of the point B (the swing foot), the generalized impulse at the point B at the contact with the ground at instant $t^{-}$ is defined as the scalar product of the integral of the reaction impulsive force ${\mathbf{F}}^B\delta (t-\tau )$ in the time interval $t^{-}÷t^{+}$ with the corresponding partial velocities

$$I_r\approx {\mathbf{v}}_r^B{\left(t^{-}\right)}^T·{\mathbf{F}}^B,r=1,\cdots ,n,$$

(A11)

the generalized momentum is defined as the partial derivative of the kinetic energy K with respect to the r-th generalized speed

$$p_r\left(t\right)=\partial K/\partial u_r,r=1,\cdots ,n,$$

(A12)

then, Kane proves that

$$I_r\approx p_r\left(t^{+}\right)-p_r\left(t^{-}\right).$$

(A13)

Indicate the matrices

$${\mathbf{V}}^B=({\mathbf{v}}_1^B\left(t^{-}\right)\cdots {\mathbf{v}}_n^B\left(t^{-}\right))$$

(A14)

$$\mathbf{P}=\{\partial p_i\left(t^{-}\right)/\partial u_j\},i,j=1,\cdots ,n$$

(A15)

of vectors of partial velocities, and of partial derivatives of $p_r\left(t\right)$ with respect to the generalized speeds, and the vectors

$$\mathbf{I}={[I_1\cdots I_n]}^T={{\mathbf{V}}^B}^T·{\mathbf{F}}^B$$

(A16)

$$\mathbf{u}\left(t\right)={[u_1\left(t\right)\cdots u_n\left(t\right)]}^T$$

(A17)

$${\mathbf{v}}^B\left(t\right)={\mathbf{V}}^B·\mathbf{u}\left(t\right)$$

(A18)

$${[p_1\left(t\right),\cdots ,p_n\left(t\right)]}^T=\mathbf{P}·\mathbf{u}\left(t\right)$$

(A19)

of generalized impulses, of generalized speeds, of the velocity of point B and of generalized momenta, respectively.

Then, taking into account from (A16) to (A19), considering that ${\mathbf{v}}^B\left(t^{-}\right)$ is known and ${\mathbf{v}}^B\left(t^{+}\right)$ is zero, assuming non-slipping condition and inelastic collision, the following system of equations is solved to derive the unknown ${\mathbf{F}}^B$ and $\mathbf{u}\left(t^{+}\right)$:

$$\left[\begin{array}{c}-\mathbf{P}\mathbf{u}\left(t^{-}\right)\\ 0\end{array}\right]=\left[\begin{array}{cc}{\mathbf{V}}^B{\left(t\right)}^T& -\mathbf{P}\\ 0& {\mathbf{V}}^B\left(t\right)\end{array}\right]·\left[\begin{array}{c}{\mathbf{F}}^B\\ \mathbf{u}\left(t^{+}\right)\end{array}\right]$$

(A20)

An essentially similar equation was discussed in [6]. At the solution, along with the velocity $\mathbf{u}\left(t^{+}\right)$ after the impact, it must be verified that the impulsive force ${\mathbf{F}}^B$ satisfies the conditions of unilateral constraint (A9) and (A10).