Drake C++ Documentation

Both truncation and rounding errors can prevent constraints from being exactly satisfied.

For example, consider the bilateral holonomic constraint equation gₚ(t,q) = 0. Even if gₚ(t₀,q(t₀)) = ġₚ(t₀,q(t₀),v(t₀)) = g̈ₚ(t₀,q(t₀),v(t₀),v̇(t₀)) = 0, it is often true that gₚ(t₁,q(t₁)) will be nonzero for sufficiently large Δt = t₁ - t₀. One way to address this constraint "drift" is through dynamic stabilization. In particular, holonomic unilateral constraints that have been differentiated twice with respect to time can be modified from:

0 ≤ g̈ₚ  ⊥  λ ≥ 0


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

and holonomic bilateral constraints can be modified from:

g̈ₚ = 0


g̈ₚ + 2αġₚ + β²gₚ = 0

for non-negative scalar α and real β (α and β can also represent diagonal matrices for greater generality). α and β, which both have units of 1/sec (i.e., the reciprocal of unit time) are described more fully in [Baumgarte 1972]. The use of α and β above make correcting position-level (gₚ) and velocity-level (gₚ̇) holonomic constraint errors analogous to the dynamic problem of stabilizing a damped harmonic oscillator. Given that analogy, 2α is the viscous damping coefficient and β² the stiffness coefficient. These two coefficients make predicting the motion of the oscillator challenging to interpret, so one typically converts them to undamped angular frequency (ω₀) and damping ratio (ζ) via the following equations:

ω₀² = β²
ζ = α/β

To eliminate constraint errors as quickly as possible, one strategy used in commercial software uses ζ=1, implying critical damping, and undamped angular frequency ω₀ that is high enough to correct errors rapidly but low enough to avoid computational stiffness. Picking that parameter is considered to be more art than science (see [Ascher 1995]). Given desired ω₀ and ζ, α and β are set using the equations above.