[Documentation] [TitleIndex] [WordIndex

ODE Solvers

Please see the ODE's users manual for general ODE documentation.

In general, rigid body simulators solve

ODE's constraint solver uses a full coordinate system approach and enforces joint and contact constraints as posed by the linear complementarity problem (LCP).

Basic Governing Equations of Constrained Dynamics

Before we discuss the solvers, here is a very brief note here on the governing dynamics equations. Simple Euler's discretization yields

$$
 \overline{\overline{M}} \left( \overline{v}^{n+1} - \overline{v}^{n} \right) - h \overline{\overline{J}}^{T} \overline{\lambda} = h \overline{f}_{ext}
$$

Constraints are described by the constraint Jacobian $$J$$ given $$\overline{\overline{J}} \dot{\overline{v}} = -\overline{k}$$ or $$\overline{\overline{J}} \overline{v} = \overline{c} $$ where $$c_i = 0$$ for fixed joints and $$c_i >=0$$ for contact joints.

If we rewrite in matrix form we have:

$\Biggl[$
\begin{tabular}{c c}
 $\overline{\overline{M}}$ & $-\overline{\overline{J}}^{t}$ \\
 $\overline{\overline{J}}$ & $0$                            \\
\end{tabular}$\Biggr]$
$\Biggl[$
\begin{tabular}{c}
 $\dot{\overline{v}}$ \\
 $\overline\lambda$
\end{tabular}$\Biggr]$
 $= $
$\Biggl[$
\begin{tabular}{c}
 $\overline{f}_{ext}$ \\
 $-\overline{k}$
\end{tabular}
$\Biggr]$

Substitute $$\dot{v_i}=\frac{v_i^{n+1}-v_i^{n}}{h}$$ and rearrange to get:

$\Biggl[$
\begin{tabular}{c c}
 $\frac{1}{h}\overline{\overline{M}}$ & $-\overline{\overline{J}}^{t}$ \\
 $\overline{\overline{J}}$ & $0$                            \\
\end{tabular}$\Biggr]$
$\Biggl[$
\begin{tabular}{c}
 $\overline{v}^{n+1}$ \\
 $\overline\lambda$
\end{tabular}$\Biggr]$
 $= $
$\Biggl[$
\begin{tabular}{c}
 $\overline{f}_{ext} + \frac{\overline{\overline{M}}\overline{v}^{n}}{h}$ \\
 $-\overline{c}$
\end{tabular}
$\Biggr]$

Left multiply top row of the matrix equation by $$\overline{\overline{J}} \overline{\overline{M}}^{-1}$$, then eliminate $$\overline{v}^{n+1}$$ from the top row using the equality in the second row ($$\overline{\overline{j}} \overline{v}^{n+1} = \overline{c}$$) and arrive at:

$$ [\overline{\overline{J}}\overline{\overline{M}}^{-1}\overline{\overline{J}}^{T}] \overline\lambda = \frac{\overline{c}}{h} - \overline{\overline{J}}[ \frac{\overline{v}^{n}}{h} + \overline{\overline{M}}^{-1}\overline{f}_{ext} ]$$

ODE is semi-implicit in that the Jacobians $$J$$ and external forces $$f_{ext}$$ from the previous time step are used throughout the iterations.

Solvers

ODE ships with two default solvers

Dantzig's Agorithm

Please refer to step.cpp for implementation details. Various references contain discussions on this algorithm, see 2.7.1 in Michael Cline, "Rigid Body Simulation with Contacts and Constraints" for example. See also the Cottle and Dantzig book for details, Baraff extended the Dantzig algorithm to include friction in his SIGGRAPH 1994 paper. Also, chapter 14 of Murilo Coutinho's book "Guide to Dynamic Simulations of Rigid Bodies and Particle Systems" has detailed introduction to both Dantzig's algorithm and Baraff's friction extention.

The Dantzig algorithm solves general BLCP (Linear Complementarity Problem with Bounds), which has the form:

Solve:

$$  \overline{\overline{A}} \overline{\lambda} =  \overline{b} + \overline{\omega} $$

such that:

$$  \lambda_i = lo, \ \ \  \omega_i > 0$$

$$  \lambda_i = hi, \ \ \  \omega_i < 0$$

$$ lo <  \lambda_i < hi, \ \ \  \omega_i = 0$$

In ODE's step.cpp, $$\overline{\omega}$$ is set to $$\overline{0}$$, then it has consistent form with SOR PGS LCP:

$$ \overline{\overline{A}} \overline{\lambda} = \overline{b} $$

The Dantzig algorithm applies to more general BLCP. It incrementally computes intermediate solutions for each entry in the unknown vector: $$\overline{\lambda}$$. It compute the $$i^{th}$$ unknown $$\overline{\lambda}_i$$ without violating the non-interpenetration or box friction conditions for the previous $$i$$ rows that already resolved. Suppose the length of $$\overline{\lambda}$$ is $$m$$, the solution should be obtained after we solve the $$m^{th}$$ unknown $$\overline{\lambda}_m$$.

We first define the different sets based on properties of unknowns: Clamped Set $$C$$ is a set of index $$i$$, with size $$nC$$ that satisfies:

$$ lo <  \lambda_i < hi, \ \ \  \omega_i = 0$$

Similarly, Non-Clamped Set $$N$$ is a set of index $$i$$ that satisfies:

$$  \lambda_i = lo, \ \ \  \omega_i > 0$$

or

$$  \lambda_i = hi, \ \ \  \omega_i < 0$$

Do-not-care Set $$R$$ is a set of index $$i$$ that satisfies:

$$  \lambda_i = 0$$

where $$\omega_i$$ could be any value. The permuted index is in the order of: $$[C; N; R]$$.

During execution of Dantzig's algorithm, the left top $$nC \times nC$$ clamped matrix of $$\overline{\overline{A}}$$, we denote as $$\overline{\overline{AC}}$$, always maintains with an $$L*D*L^{\prime}$$(LDLT) factorization.

Procedures of Dantzig's algorithm are: If we have only bounded constraints (bilateral constraints with lower and upper bounds), then all the indices are mapped to set $$C$$, we do an LDLT factorization of matrix $$\overline{\overline{A}}$$ , then solve the LDLT system, we are done.

Else if we have a mixture of unbounded and unbounded constraints, Dantzig algorithm does LDLT factorization and solve the first $$nC$$ unknowns.

When we hit the first friction constraint, compute the corresponding lower and upper bound, using normal force at the same contact.

Assume $$\overline{\lambda}_{i+1, ..., m} = 0$$, update $$\overline{\lambda}_{i}$$ and make sure to push $$\overline{\lambda}_{i=0,1,2, ... i-1}$$ to one of the sets: $$[C; N; R]$$, i.e. don't violate the first$$(i-1)$$ constraints, since update on $$\overline{\lambda}_{i}$$ might break the first $$(i-1)$$ constraint satisfaction.

Once we finish a complete loop on $$\overline{\lambda}_{i=0,1,2, ..., m}$$, the solution is found.

SOR PGS LCP

As implemented in ODE's quickstep.cpp, and reiterating the solution procedure from several popular literatures here.

We are essentially solving a system of linear equations where the solution space is non-negative in parts of the system.

$$ \overline{\overline{A}} \overline{\lambda} = \overline{b} $$

where based on the derivations from governing equations in the previous section,

$$ \overline{b} = \frac{\overline{c}}{h} - \overline{\overline{J}}[ \frac{\overline{v}^{n}}{h} + \overline{\overline{M}}^{-1}\overline{f}_{ext} ]$$

and

$$ \overline{\overline{A}} = [\overline{\overline{J}}\overline{\overline{M}}^{-1}\overline{\overline{J}}^{T}] $$

If we solve for $$\overline{\lambda}$$ in delta-form using Gauss-Seidel, i.e.

$$\delta_i = \lambda_i^{n ew} - \lambda_i^{old}$$

then it follows that

$$ \delta_i = \frac{b_i}{A_{ii}} - \sum_{j=1}^{N_{constrai nts}}{\frac{A_{ij}}{A_{ii}} \lambda_j }$$

for $$ i = 1,...,N_{constra i n t s} $$

Formulate the desired solution in the form of acceleration1 (inverse mass matrix times constraint forces), denoted by

$$ \overline{a}_c =  \overline{\overline{M}}^{-1}\overline{f}_c = \overline{\overline{M}}^{-1}\overline{\overline{J}}^{T} \overline\lambda $$

then $$\overline\lambda$$ update becomes

$$ \delta_i = \frac{b_i}{A_{ii}} -  \frac{  \sum_{k=1}^{ N_{D O F}} J_{ik} a_{c_{k}} }{A_{ii}}  $$

and

$$ \lambda_i^{n ew} = \lambda_i^{old} + \omega \delta_i $$

for $$ i = 1,...,N_{constra i n t s} $$, where $$\omega$$ is the relaxation parameter.

where each $$\lambda_i^{n e w}$$ is projected into its corresponding solution space depending on the type of constraint specified.

At every iteration, for each $$i$$ update above, constraint accelerations $$a_{c_{k}}$$ are updated in the following manner:

$$a_{c_{k}}^{n+1} = a_{c_{k}}^{n} + G_{ki} (\omega \delta_{i})$$

for $$ k = 1 , ... , N_{DOF}$$

where

$$ \overline{\overline{G}} \equiv \overline{\overline{M}}^{-1} \overline{\overline{J}}^{T}$$

For more details please see the list of references.

  1. to clarify, in quickstep.cpp, $$\bar{a}_c$$ is denoted by variable fc as of svn revision 1675 (1)


2024-11-09 14:43