## 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 acceleration1 (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.

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

2024-06-15 12:58