ODE Solvers

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

In general, rigid body simulators solve

• Kinematics constraints
• Collision and contact constraints

• Rigid body dynamics

  latex error! exitcode was 2 (signal 0), transscript follows:
  
  

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

latex error! exitcode was 2 (signal 0), transscript follows:

Constraints are described by the constraint Jacobian

latex error! exitcode was 2 (signal 0), transscript follows:

given

latex error! exitcode was 2 (signal 0), transscript follows:

or

latex error! exitcode was 2 (signal 0), transscript follows:

where

latex error! exitcode was 2 (signal 0), transscript follows:

for fixed joints and

latex error! exitcode was 2 (signal 0), transscript follows:

for contact joints.

If we rewrite in matrix form we have:

latex error! exitcode was 2 (signal 0), transscript follows:

Substitute

latex error! exitcode was 2 (signal 0), transscript follows:

and rearrange to get:

latex error! exitcode was 2 (signal 0), transscript follows:

Left multiply top row of the matrix equation by

latex error! exitcode was 2 (signal 0), transscript follows:

, then eliminate

latex error! exitcode was 2 (signal 0), transscript follows:

from the top row using the equality in the second row (

latex error! exitcode was 2 (signal 0), transscript follows:

) and arrive at:

latex error! exitcode was 2 (signal 0), transscript follows:

ODE is semi-implicit in that the Jacobians

latex error! exitcode was 2 (signal 0), transscript follows:

and external forces

latex error! exitcode was 2 (signal 0), transscript follows:

from the previous time step are used throughout the iterations.

Solvers

ODE ships with two default solvers

• Dantzig's Agorithm dWorldStep()

  • This algorithm will attempt to achieve a numerically exact solution. It is about one order of magnitude slower than SOR PGS LCP solver and its convergence behavior is less predictable in practice.

• Successive Over-Relaxation (SOR) Projected Gauss-Seidel (PGS) LCP solver dWorldQuickStep()

  • Essentially a Gauss-Seidel algorithm with solution vector projected into the allowable solution space at every update. The PR2 robot simulations default to this algorithm running at 1kHz (to match mechanism controller update rate of the real robot).

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:

latex error! exitcode was 2 (signal 0), transscript follows:

such that:

latex error! exitcode was 2 (signal 0), transscript follows:

latex error! exitcode was 2 (signal 0), transscript follows:

latex error! exitcode was 2 (signal 0), transscript follows:

In ODE's step.cpp,

latex error! exitcode was 2 (signal 0), transscript follows:

is set to

latex error! exitcode was 2 (signal 0), transscript follows:

, then it has consistent form with SOR PGS LCP:

latex error! exitcode was 2 (signal 0), transscript follows:

The Dantzig algorithm applies to more general BLCP. It incrementally computes intermediate solutions for each entry in the unknown vector:

latex error! exitcode was 2 (signal 0), transscript follows:

. It compute the

latex error! exitcode was 2 (signal 0), transscript follows:

unknown

latex error! exitcode was 2 (signal 0), transscript follows:

without violating the non-interpenetration or box friction conditions for the previous

latex error! exitcode was 2 (signal 0), transscript follows:

rows that already resolved. Suppose the length of

latex error! exitcode was 2 (signal 0), transscript follows:

is

latex error! exitcode was 2 (signal 0), transscript follows:

, the solution should be obtained after we solve the

latex error! exitcode was 2 (signal 0), transscript follows:

unknown

latex error! exitcode was 2 (signal 0), transscript follows:

.

We first define the different sets based on properties of unknowns: Clamped Set

latex error! exitcode was 2 (signal 0), transscript follows:

is a set of index

latex error! exitcode was 2 (signal 0), transscript follows:

, with size

latex error! exitcode was 2 (signal 0), transscript follows:

that satisfies:

latex error! exitcode was 2 (signal 0), transscript follows:

Similarly, Non-Clamped Set

latex error! exitcode was 2 (signal 0), transscript follows:

is a set of index

latex error! exitcode was 2 (signal 0), transscript follows:

that satisfies:

latex error! exitcode was 2 (signal 0), transscript follows:

or

latex error! exitcode was 2 (signal 0), transscript follows:

Do-not-care Set

latex error! exitcode was 2 (signal 0), transscript follows:

is a set of index

latex error! exitcode was 2 (signal 0), transscript follows:

that satisfies:

latex error! exitcode was 2 (signal 0), transscript follows:

where

latex error! exitcode was 2 (signal 0), transscript follows:

could be any value. The permuted index is in the order of:

latex error! exitcode was 2 (signal 0), transscript follows:

.

During execution of Dantzig's algorithm, the left top

latex error! exitcode was 2 (signal 0), transscript follows:

clamped matrix of

latex error! exitcode was 2 (signal 0), transscript follows:

, we denote as

latex error! exitcode was 2 (signal 0), transscript follows:

, always maintains with an

latex error! exitcode was 2 (signal 0), transscript follows:

(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

latex error! exitcode was 2 (signal 0), transscript follows:

, we do an LDLT factorization of matrix

latex error! exitcode was 2 (signal 0), transscript follows:

, 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

latex error! exitcode was 2 (signal 0), transscript follows:

unknowns.

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

Assume

latex error! exitcode was 2 (signal 0), transscript follows:

, update

latex error! exitcode was 2 (signal 0), transscript follows:

and make sure to push

latex error! exitcode was 2 (signal 0), transscript follows:

to one of the sets:

latex error! exitcode was 2 (signal 0), transscript follows:

, i.e. don't violate the first

latex error! exitcode was 2 (signal 0), transscript follows:

constraints, since update on

latex error! exitcode was 2 (signal 0), transscript follows:

might break the first

latex error! exitcode was 2 (signal 0), transscript follows:

constraint satisfaction.

Once we finish a complete loop on

latex error! exitcode was 2 (signal 0), transscript follows:

, 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.

latex error! exitcode was 2 (signal 0), transscript follows:

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

latex error! exitcode was 2 (signal 0), transscript follows:

and

latex error! exitcode was 2 (signal 0), transscript follows:

If we solve for

latex error! exitcode was 2 (signal 0), transscript follows:

in delta-form using Gauss-Seidel, i.e.

latex error! exitcode was 2 (signal 0), transscript follows:

then it follows that

latex error! exitcode was 2 (signal 0), transscript follows:

for

latex error! exitcode was 2 (signal 0), transscript follows:

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

latex error! exitcode was 2 (signal 0), transscript follows:

then

latex error! exitcode was 2 (signal 0), transscript follows:

update becomes

latex error! exitcode was 2 (signal 0), transscript follows:

and

latex error! exitcode was 2 (signal 0), transscript follows:

for

latex error! exitcode was 2 (signal 0), transscript follows:

, where

latex error! exitcode was 2 (signal 0), transscript follows:

is the relaxation parameter.

where each

latex error! exitcode was 2 (signal 0), transscript follows:

is projected into its corresponding solution space depending on the type of constraint specified.

At every iteration, for each

latex error! exitcode was 2 (signal 0), transscript follows:

update above, constraint accelerations

latex error! exitcode was 2 (signal 0), transscript follows:

are updated in the following manner:

latex error! exitcode was 2 (signal 0), transscript follows:

for

latex error! exitcode was 2 (signal 0), transscript follows:

where

latex error! exitcode was 2 (signal 0), transscript follows:

For more details please see the list of references.