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:
    
    [Fri Mar 29 09:00:51.477651 2024] [:error] [pid 9873] 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:

[Fri Mar 29 09:00:51.497195 2024] [:error] [pid 9874] failed to exec() latex

Constraints are described by the constraint Jacobian

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

[Fri Mar 29 09:00:51.512127 2024] [:error] [pid 9875] failed to exec() latex
given
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:51.541689 2024] [:error] [pid 9876] failed to exec() latex
or
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:51.581970 2024] [:error] [pid 9877] failed to exec() latex
where
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:51.595685 2024] [:error] [pid 9878] failed to exec() latex
for fixed joints and
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:51.610337 2024] [:error] [pid 9879] failed to exec() latex
for contact joints.

If we rewrite in matrix form we have:

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

[Fri Mar 29 09:00:51.632113 2024] [:error] [pid 9880] failed to exec() latex

Substitute

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

[Fri Mar 29 09:00:51.651141 2024] [:error] [pid 9881] failed to exec() latex
and rearrange to get:
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:51.710384 2024] [:error] [pid 9882] failed to exec() latex

Left multiply top row of the matrix equation by

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

[Fri Mar 29 09:00:51.726403 2024] [:error] [pid 9883] failed to exec() latex
, then eliminate
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:51.741551 2024] [:error] [pid 9884] failed to exec() latex
from the top row using the equality in the second row (
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:51.757484 2024] [:error] [pid 9885] failed to exec() latex
) and arrive at:

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

[Fri Mar 29 09:00:51.776337 2024] [:error] [pid 9886] failed to exec() latex

ODE is semi-implicit in that the Jacobians

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

[Fri Mar 29 09:00:51.792825 2024] [:error] [pid 9887] failed to exec() latex
and external forces
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:51.807501 2024] [:error] [pid 9888] 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:

[Fri Mar 29 09:00:51.858662 2024] [:error] [pid 9889] failed to exec() latex

such that:

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

[Fri Mar 29 09:00:51.878556 2024] [:error] [pid 9890] failed to exec() latex

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

[Fri Mar 29 09:00:51.915486 2024] [:error] [pid 9891] failed to exec() latex

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

[Fri Mar 29 09:00:51.934036 2024] [:error] [pid 9892] failed to exec() latex

In ODE's step.cpp,

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

[Fri Mar 29 09:00:51.952045 2024] [:error] [pid 9893] failed to exec() latex
is set to
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:51.969757 2024] [:error] [pid 9894] failed to exec() latex
, then it has consistent form with SOR PGS LCP:

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

[Fri Mar 29 09:00:51.988484 2024] [:error] [pid 9895] 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:

[Fri Mar 29 09:00:52.007407 2024] [:error] [pid 9896] failed to exec() latex
. It compute the
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.025301 2024] [:error] [pid 9897] failed to exec() latex
unknown
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.042455 2024] [:error] [pid 9898] 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:

[Fri Mar 29 09:00:52.080770 2024] [:error] [pid 9899] failed to exec() latex
rows that already resolved. Suppose the length of
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.114847 2024] [:error] [pid 9900] failed to exec() latex
is
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.129712 2024] [:error] [pid 9901] failed to exec() latex
, the solution should be obtained after we solve the
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.145751 2024] [:error] [pid 9902] failed to exec() latex
unknown
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.160416 2024] [:error] [pid 9903] 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:

[Fri Mar 29 09:00:52.175453 2024] [:error] [pid 9904] failed to exec() latex
is a set of index
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.189915 2024] [:error] [pid 9905] failed to exec() latex
, with size
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.203014 2024] [:error] [pid 9906] failed to exec() latex
that satisfies:
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.218086 2024] [:error] [pid 9908] failed to exec() latex

Similarly, Non-Clamped Set

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

[Fri Mar 29 09:00:52.232446 2024] [:error] [pid 9909] failed to exec() latex
is a set of index
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.247468 2024] [:error] [pid 9910] failed to exec() latex
that satisfies:
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.262218 2024] [:error] [pid 9911] failed to exec() latex

or

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

[Fri Mar 29 09:00:52.277280 2024] [:error] [pid 9912] failed to exec() latex

Do-not-care Set

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

[Fri Mar 29 09:00:52.292565 2024] [:error] [pid 9913] failed to exec() latex
is a set of index
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.307650 2024] [:error] [pid 9914] failed to exec() latex
that satisfies:
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.329757 2024] [:error] [pid 9915] failed to exec() latex

where

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

[Fri Mar 29 09:00:52.345518 2024] [:error] [pid 9916] 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:

[Fri Mar 29 09:00:52.372226 2024] [:error] [pid 9917] failed to exec() latex
.

During execution of Dantzig's algorithm, the left top

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

[Fri Mar 29 09:00:52.390097 2024] [:error] [pid 9918] failed to exec() latex
clamped matrix of
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.407310 2024] [:error] [pid 9919] failed to exec() latex
, we denote as
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.424305 2024] [:error] [pid 9920] failed to exec() latex
, always maintains with an
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.444737 2024] [:error] [pid 9921] 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:

[Fri Mar 29 09:00:52.463432 2024] [:error] [pid 9922] failed to exec() latex
, we do an LDLT factorization of matrix
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.481408 2024] [:error] [pid 9923] 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:

[Fri Mar 29 09:00:52.523483 2024] [:error] [pid 9924] 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:

[Fri Mar 29 09:00:52.541477 2024] [:error] [pid 9925] failed to exec() latex
, update
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.559512 2024] [:error] [pid 9926] failed to exec() latex
and make sure to push
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.574679 2024] [:error] [pid 9927] failed to exec() latex
to one of the sets:
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.588842 2024] [:error] [pid 9928] failed to exec() latex
, i.e. don't violate the first
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.603441 2024] [:error] [pid 9929] failed to exec() latex
constraints, since update on
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.617328 2024] [:error] [pid 9930] failed to exec() latex
might break the first
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.654228 2024] [:error] [pid 9931] failed to exec() latex
constraint satisfaction.

Once we finish a complete loop on

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

[Fri Mar 29 09:00:52.669211 2024] [:error] [pid 9932] 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:

[Fri Mar 29 09:00:52.688347 2024] [:error] [pid 9933] 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:

[Fri Mar 29 09:00:52.705166 2024] [:error] [pid 9934] failed to exec() latex

and

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

[Fri Mar 29 09:00:52.721334 2024] [:error] [pid 9935] failed to exec() latex

If we solve for

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

[Fri Mar 29 09:00:52.738481 2024] [:error] [pid 9936] failed to exec() latex
in delta-form using Gauss-Seidel, i.e.

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

[Fri Mar 29 09:00:52.779191 2024] [:error] [pid 9937] failed to exec() latex

then it follows that

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

[Fri Mar 29 09:00:52.797240 2024] [:error] [pid 9938] failed to exec() latex

for

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

[Fri Mar 29 09:00:52.812566 2024] [:error] [pid 9939] 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:

[Fri Mar 29 09:00:52.830539 2024] [:error] [pid 9940] failed to exec() latex

then

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

[Fri Mar 29 09:00:52.845139 2024] [:error] [pid 9941] failed to exec() latex
update becomes
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.859521 2024] [:error] [pid 9942] failed to exec() latex

and

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

[Fri Mar 29 09:00:52.874187 2024] [:error] [pid 9943] failed to exec() latex

for

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

[Fri Mar 29 09:00:52.887732 2024] [:error] [pid 9944] failed to exec() latex
, where
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.900455 2024] [:error] [pid 9945] failed to exec() latex
is the relaxation parameter.

where each

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

[Fri Mar 29 09:00:52.914802 2024] [:error] [pid 9946] 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:

[Fri Mar 29 09:00:52.929966 2024] [:error] [pid 9947] failed to exec() latex
update above, constraint accelerations
latex error! exitcode was 2 (signal 0), transscript follows:

[Fri Mar 29 09:00:52.948136 2024] [:error] [pid 9948] failed to exec() latex
are updated in the following manner:

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

[Fri Mar 29 09:00:52.969147 2024] [:error] [pid 9949] failed to exec() latex

for

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

[Fri Mar 29 09:00:52.986416 2024] [:error] [pid 9950] failed to exec() latex

where

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

[Fri Mar 29 09:00:53.004924 2024] [:error] [pid 9951] 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)