Drake C++ Documentation

Consider two points pᵢ and pⱼ on rigid bodies i and j, respectively, and assume that at a certain configuration of the two bodies, ᶜq, the two points are coincident at a single location in space, p(ᶜq).

To constrain the motion of pᵢ and pⱼ to the contact surface as the bodies move, one can introduce the constraint g(q) = n(q)ᵀ(pᵢ(q) - pⱼ(q)), where n(q) is the common surface normal expressed in the world frame. gₚ(q) is a unilateral constraint, meaning that complementarity constraints are necessary (see Constraint types):

0 ≤ gₚ  ⊥  λ ≥ 0

Differentiating gₚ(.) with respect to time

As usual, gₚ(.) must be differentiated (with respect to time) once to use the contact constraint in velocity-level constraint formulation or twice to use it in a force/acceleration-level formulation. Differentiating gₚ(q) once with respect to time yields:

ġₚ(q,v) = nᵀ(ṗᵢ - ṗⱼ) + ṅᵀ(pᵢ - pⱼ);

one more differentiation with respect to time yields:

g̈ₚ(q,v,v̇) = nᵀ(p̈ᵢ - p̈ⱼ) + 2ṅᵀ(ṗᵢ - ṗⱼ) + n̈ᵀ(pᵢ - pⱼ).

The non-negativity condition on the constraint force magnitudes (λ ≥ 0) keeps the contact force along the contact normal compressive, as is consistent with a non-adhesive contact model.

A more substantial discussion on the kinematics of contact can be found in [Pfeiffer 1996], Ch. 4.

Figure 1: Illustration of the interpretation of non-interpenetration constraints when two boxes are interpenetrating (right). The boxes prior to contact are shown at left, and are shown in the middle figure at the initial time of contact; the surface normal n̂ is shown in this figure as well. The bodies interpenetrate over time as the constraint becomes violated (e.g., by constraint drift). Nevertheless, n̂ is tracked over time from its initial direction (and definition relative to the blue body). σ represents the signed distance the bodies must be translated along n̂ so that they are osculating (kissing).

Constraint softening

As discussed in A stable discretization strategy, the non-interpenetration constraint can be softened by adding a term to the time derivative of the equation:

0 ≤ ġₚ  ⊥  λ ≥ 0

resulting in:

0 ≤ ġₚ + γλ  ⊥  λ ≥ 0

for γ ≥ 0.

Constraint stabilization

As discussed in Constraint stabilization, the drift induced by solving an Index-0 DAE or DCP (that has been reduced from an Index-3 DAE or DCP) can be mitigated through one of several strategies, one of which is Baumgarte Stabilization. The typical application of Baumgarte Stabilization will use the second time derivative of the complementarity condition:

0 ≤ g̈ₚ  ⊥  λ ≥ 0

Baumgarte Stabilization would be layered on top of this equation, resulting in:

0 ≤ g̈ₚ + 2αġₚ + β²gₚ  ⊥  λ ≥ 0

Effects of constraint softening and stabilization on constraint problem data