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:
    
    [Wed Aug 17 16:44:43.242559 2022] [:error] [pid 22297] failed to exec() latex
    

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:

[Wed Aug 17 16:44:43.261127 2022] [:error] [pid 22298] failed to exec() latex

Constraints are described by the constraint Jacobian

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

[Wed Aug 17 16:44:43.277021 2022] [:error] [pid 22299] failed to exec() latex
given
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.292266 2022] [:error] [pid 22300] failed to exec() latex
or
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.307674 2022] [:error] [pid 22301] failed to exec() latex
where
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.322854 2022] [:error] [pid 22302] failed to exec() latex
for fixed joints and
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.337256 2022] [:error] [pid 22304] failed to exec() latex
for contact joints.

If we rewrite in matrix form we have:

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

[Wed Aug 17 16:44:43.354610 2022] [:error] [pid 22305] failed to exec() latex

Substitute

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

[Wed Aug 17 16:44:43.370116 2022] [:error] [pid 22306] failed to exec() latex
and rearrange to get:
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.386937 2022] [:error] [pid 22307] failed to exec() latex

Left multiply top row of the matrix equation by

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

[Wed Aug 17 16:44:43.402057 2022] [:error] [pid 22308] failed to exec() latex
, then eliminate
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.416078 2022] [:error] [pid 22309] failed to exec() latex
from the top row using the equality in the second row (
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.430603 2022] [:error] [pid 22310] failed to exec() latex
) and arrive at:

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

[Wed Aug 17 16:44:43.446699 2022] [:error] [pid 22311] failed to exec() latex

ODE is semi-implicit in that the Jacobians

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

[Wed Aug 17 16:44:43.462571 2022] [:error] [pid 22312] failed to exec() latex
and external forces
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.484894 2022] [:error] [pid 22313] failed to exec() latex
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:

[Wed Aug 17 16:44:43.509609 2022] [:error] [pid 22314] failed to exec() latex

such that:

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

[Wed Aug 17 16:44:43.526019 2022] [:error] [pid 22315] failed to exec() latex

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

[Wed Aug 17 16:44:43.542082 2022] [:error] [pid 22316] failed to exec() latex

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

[Wed Aug 17 16:44:43.558154 2022] [:error] [pid 22317] failed to exec() latex

In ODE's step.cpp,

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

[Wed Aug 17 16:44:43.573886 2022] [:error] [pid 22318] failed to exec() latex
is set to
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.587383 2022] [:error] [pid 22319] failed to exec() latex
, then it has consistent form with SOR PGS LCP:

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

[Wed Aug 17 16:44:43.601745 2022] [:error] [pid 22320] failed to exec() latex

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:

[Wed Aug 17 16:44:43.631568 2022] [:error] [pid 22321] failed to exec() latex
. It compute the
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.645828 2022] [:error] [pid 22322] failed to exec() latex
unknown
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.659866 2022] [:error] [pid 22323] failed to exec() latex
without violating the non-interpenetration or box friction conditions for the previous
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.673692 2022] [:error] [pid 22324] failed to exec() latex
rows that already resolved. Suppose the length of
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.686957 2022] [:error] [pid 22325] failed to exec() latex
is
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.700745 2022] [:error] [pid 22326] failed to exec() latex
, the solution should be obtained after we solve the
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.714425 2022] [:error] [pid 22327] failed to exec() latex
unknown
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.727312 2022] [:error] [pid 22328] failed to exec() latex
.

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

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

[Wed Aug 17 16:44:43.741323 2022] [:error] [pid 22329] failed to exec() latex
is a set of index
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.755403 2022] [:error] [pid 22330] failed to exec() latex
, with size
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.770577 2022] [:error] [pid 22331] failed to exec() latex
that satisfies:
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.786722 2022] [:error] [pid 22332] failed to exec() latex

Similarly, Non-Clamped Set

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

[Wed Aug 17 16:44:43.810037 2022] [:error] [pid 22333] failed to exec() latex
is a set of index
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.826034 2022] [:error] [pid 22334] failed to exec() latex
that satisfies:
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.842250 2022] [:error] [pid 22335] failed to exec() latex

or

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

[Wed Aug 17 16:44:43.858327 2022] [:error] [pid 22336] failed to exec() latex

Do-not-care Set

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

[Wed Aug 17 16:44:43.873841 2022] [:error] [pid 22337] failed to exec() latex
is a set of index
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.889133 2022] [:error] [pid 22338] failed to exec() latex
that satisfies:
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.904939 2022] [:error] [pid 22339] failed to exec() latex

where

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

[Wed Aug 17 16:44:43.920121 2022] [:error] [pid 22340] failed to exec() latex
could be any value. The permuted index is in the order of:
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.934392 2022] [:error] [pid 22341] failed to exec() latex
.

During execution of Dantzig's algorithm, the left top

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

[Wed Aug 17 16:44:43.947890 2022] [:error] [pid 22342] failed to exec() latex
clamped matrix of
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.960846 2022] [:error] [pid 22343] failed to exec() latex
, we denote as
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.973641 2022] [:error] [pid 22344] failed to exec() latex
, always maintains with an
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:43.986126 2022] [:error] [pid 22345] failed to exec() latex
(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:

[Wed Aug 17 16:44:43.999810 2022] [:error] [pid 22346] failed to exec() latex
, we do an LDLT factorization of matrix
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:44.013332 2022] [:error] [pid 22347] failed to exec() latex
, 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:

[Wed Aug 17 16:44:44.028116 2022] [:error] [pid 22348] failed to exec() latex
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:

[Wed Aug 17 16:44:44.043079 2022] [:error] [pid 22349] failed to exec() latex
, update
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:44.056638 2022] [:error] [pid 22350] failed to exec() latex
and make sure to push
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:44.070033 2022] [:error] [pid 22351] failed to exec() latex
to one of the sets:
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:44.083798 2022] [:error] [pid 22352] failed to exec() latex
, i.e. don't violate the first
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:44.098611 2022] [:error] [pid 22353] failed to exec() latex
constraints, since update on
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:44.113159 2022] [:error] [pid 22354] failed to exec() latex
might break the first
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:44.128948 2022] [:error] [pid 22355] failed to exec() latex
constraint satisfaction.

Once we finish a complete loop on

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

[Wed Aug 17 16:44:44.144997 2022] [:error] [pid 22356] failed to exec() latex
, 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:

[Wed Aug 17 16:44:44.163860 2022] [:error] [pid 22357] failed to exec() latex

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

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

[Wed Aug 17 16:44:44.180525 2022] [:error] [pid 22358] failed to exec() latex

and

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

[Wed Aug 17 16:44:44.196809 2022] [:error] [pid 22359] failed to exec() latex

If we solve for

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

[Wed Aug 17 16:44:44.212335 2022] [:error] [pid 22360] failed to exec() latex
in delta-form using Gauss-Seidel, i.e.

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

[Wed Aug 17 16:44:44.228560 2022] [:error] [pid 22361] failed to exec() latex

then it follows that

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

[Wed Aug 17 16:44:44.245074 2022] [:error] [pid 22362] failed to exec() latex

for

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

[Wed Aug 17 16:44:44.268159 2022] [:error] [pid 22363] failed to exec() latex

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:

[Wed Aug 17 16:44:44.285008 2022] [:error] [pid 22364] failed to exec() latex

then

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

[Wed Aug 17 16:44:44.300576 2022] [:error] [pid 22365] failed to exec() latex
update becomes
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:44.316568 2022] [:error] [pid 22366] failed to exec() latex

and

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

[Wed Aug 17 16:44:44.332694 2022] [:error] [pid 22367] failed to exec() latex

for

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

[Wed Aug 17 16:44:44.348211 2022] [:error] [pid 22368] failed to exec() latex
, where
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:44.363631 2022] [:error] [pid 22369] failed to exec() latex
is the relaxation parameter.

where each

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

[Wed Aug 17 16:44:44.380837 2022] [:error] [pid 22370] failed to exec() latex
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:

[Wed Aug 17 16:44:44.397197 2022] [:error] [pid 22371] failed to exec() latex
update above, constraint accelerations
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Aug 17 16:44:44.412894 2022] [:error] [pid 22372] failed to exec() latex
are updated in the following manner:

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

[Wed Aug 17 16:44:44.429389 2022] [:error] [pid 22373] failed to exec() latex

for

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

[Wed Aug 17 16:44:44.445147 2022] [:error] [pid 22374] failed to exec() latex

where

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

[Wed Aug 17 16:44:44.461166 2022] [:error] [pid 22375] failed to exec() latex

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)

Wiki: physics_ode/ODE (last edited 2015-02-25 03:59:31 by Ying)