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:
    
    [Tue Oct 04 16:58:42.497301 2022] [:error] [pid 12788] 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:

[Tue Oct 04 16:58:42.512018 2022] [:error] [pid 12789] failed to exec() latex

Constraints are described by the constraint Jacobian

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

[Tue Oct 04 16:58:42.523298 2022] [:error] [pid 12790] failed to exec() latex
given
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.533693 2022] [:error] [pid 12791] failed to exec() latex
or
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.544015 2022] [:error] [pid 12792] failed to exec() latex
where
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.566471 2022] [:error] [pid 12793] failed to exec() latex
for fixed joints and
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.577039 2022] [:error] [pid 12794] failed to exec() latex
for contact joints.

If we rewrite in matrix form we have:

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

[Tue Oct 04 16:58:42.589346 2022] [:error] [pid 12795] failed to exec() latex

Substitute

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

[Tue Oct 04 16:58:42.600531 2022] [:error] [pid 12796] failed to exec() latex
and rearrange to get:
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.613072 2022] [:error] [pid 12797] failed to exec() latex

Left multiply top row of the matrix equation by

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

[Tue Oct 04 16:58:42.633216 2022] [:error] [pid 12798] failed to exec() latex
, then eliminate
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.647949 2022] [:error] [pid 12799] failed to exec() latex
from the top row using the equality in the second row (
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.660147 2022] [:error] [pid 12800] failed to exec() latex
) and arrive at:

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

[Tue Oct 04 16:58:42.690583 2022] [:error] [pid 12801] failed to exec() latex

ODE is semi-implicit in that the Jacobians

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

[Tue Oct 04 16:58:42.702570 2022] [:error] [pid 12802] failed to exec() latex
and external forces
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.732188 2022] [:error] [pid 12803] 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:

[Tue Oct 04 16:58:42.751682 2022] [:error] [pid 12804] failed to exec() latex

such that:

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

[Tue Oct 04 16:58:42.762928 2022] [:error] [pid 12805] failed to exec() latex

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

[Tue Oct 04 16:58:42.782658 2022] [:error] [pid 12806] failed to exec() latex

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

[Tue Oct 04 16:58:42.794141 2022] [:error] [pid 12807] failed to exec() latex

In ODE's step.cpp,

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

[Tue Oct 04 16:58:42.805232 2022] [:error] [pid 12808] failed to exec() latex
is set to
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.820844 2022] [:error] [pid 12809] failed to exec() latex
, then it has consistent form with SOR PGS LCP:

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

[Tue Oct 04 16:58:42.832355 2022] [:error] [pid 12810] 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:

[Tue Oct 04 16:58:42.853599 2022] [:error] [pid 12811] failed to exec() latex
. It compute the
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.875828 2022] [:error] [pid 12812] failed to exec() latex
unknown
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.900536 2022] [:error] [pid 12813] 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:

[Tue Oct 04 16:58:42.915063 2022] [:error] [pid 12814] failed to exec() latex
rows that already resolved. Suppose the length of
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.925935 2022] [:error] [pid 12815] failed to exec() latex
is
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.936648 2022] [:error] [pid 12816] failed to exec() latex
, the solution should be obtained after we solve the
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.947451 2022] [:error] [pid 12817] failed to exec() latex
unknown
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.957945 2022] [:error] [pid 12818] 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:

[Tue Oct 04 16:58:42.969263 2022] [:error] [pid 12819] failed to exec() latex
is a set of index
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:42.995191 2022] [:error] [pid 12820] failed to exec() latex
, with size
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.005072 2022] [:error] [pid 12821] failed to exec() latex
that satisfies:
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.015903 2022] [:error] [pid 12822] failed to exec() latex

Similarly, Non-Clamped Set

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

[Tue Oct 04 16:58:43.026264 2022] [:error] [pid 12823] failed to exec() latex
is a set of index
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.034366 2022] [:error] [pid 12824] failed to exec() latex
that satisfies:
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.043237 2022] [:error] [pid 12825] failed to exec() latex

or

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

[Tue Oct 04 16:58:43.053443 2022] [:error] [pid 12826] failed to exec() latex

Do-not-care Set

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

[Tue Oct 04 16:58:43.072205 2022] [:error] [pid 12827] failed to exec() latex
is a set of index
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.098246 2022] [:error] [pid 12829] failed to exec() latex
that satisfies:
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.109666 2022] [:error] [pid 12830] failed to exec() latex

where

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

[Tue Oct 04 16:58:43.120321 2022] [:error] [pid 12831] 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:

[Tue Oct 04 16:58:43.131403 2022] [:error] [pid 12832] failed to exec() latex
.

During execution of Dantzig's algorithm, the left top

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

[Tue Oct 04 16:58:43.154026 2022] [:error] [pid 12833] failed to exec() latex
clamped matrix of
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.164310 2022] [:error] [pid 12834] failed to exec() latex
, we denote as
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.175007 2022] [:error] [pid 12835] failed to exec() latex
, always maintains with an
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.185824 2022] [:error] [pid 12836] 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:

[Tue Oct 04 16:58:43.196579 2022] [:error] [pid 12837] failed to exec() latex
, we do an LDLT factorization of matrix
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.205281 2022] [:error] [pid 12838] 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:

[Tue Oct 04 16:58:43.215010 2022] [:error] [pid 12839] 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:

[Tue Oct 04 16:58:43.224840 2022] [:error] [pid 12840] failed to exec() latex
, update
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.233518 2022] [:error] [pid 12841] failed to exec() latex
and make sure to push
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.246231 2022] [:error] [pid 12842] failed to exec() latex
to one of the sets:
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.266255 2022] [:error] [pid 12847] failed to exec() latex
, i.e. don't violate the first
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.274841 2022] [:error] [pid 12848] failed to exec() latex
constraints, since update on
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.284620 2022] [:error] [pid 12849] failed to exec() latex
might break the first
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.297298 2022] [:error] [pid 12850] failed to exec() latex
constraint satisfaction.

Once we finish a complete loop on

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

[Tue Oct 04 16:58:43.307931 2022] [:error] [pid 12851] 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:

[Tue Oct 04 16:58:43.319253 2022] [:error] [pid 12852] 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:

[Tue Oct 04 16:58:43.328480 2022] [:error] [pid 12853] failed to exec() latex

and

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

[Tue Oct 04 16:58:43.338581 2022] [:error] [pid 12854] failed to exec() latex

If we solve for

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

[Tue Oct 04 16:58:43.348826 2022] [:error] [pid 12855] failed to exec() latex
in delta-form using Gauss-Seidel, i.e.

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

[Tue Oct 04 16:58:43.360324 2022] [:error] [pid 12856] failed to exec() latex

then it follows that

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

[Tue Oct 04 16:58:43.370637 2022] [:error] [pid 12857] failed to exec() latex

for

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

[Tue Oct 04 16:58:43.379890 2022] [:error] [pid 12858] 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:

[Tue Oct 04 16:58:43.390697 2022] [:error] [pid 12859] failed to exec() latex

then

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

[Tue Oct 04 16:58:43.401277 2022] [:error] [pid 12860] failed to exec() latex
update becomes
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.412368 2022] [:error] [pid 12861] failed to exec() latex

and

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

[Tue Oct 04 16:58:43.422799 2022] [:error] [pid 12862] failed to exec() latex

for

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

[Tue Oct 04 16:58:43.432701 2022] [:error] [pid 12863] failed to exec() latex
, where
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.454884 2022] [:error] [pid 12864] failed to exec() latex
is the relaxation parameter.

where each

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

[Tue Oct 04 16:58:43.471488 2022] [:error] [pid 12865] 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:

[Tue Oct 04 16:58:43.482364 2022] [:error] [pid 12866] failed to exec() latex
update above, constraint accelerations
latex error! exitcode was 2 (signal 0), transscript follows:

[Tue Oct 04 16:58:43.493188 2022] [:error] [pid 12867] failed to exec() latex
are updated in the following manner:

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

[Tue Oct 04 16:58:43.503622 2022] [:error] [pid 12868] failed to exec() latex

for

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

[Tue Oct 04 16:58:43.513330 2022] [:error] [pid 12869] failed to exec() latex

where

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

[Tue Oct 04 16:58:43.524116 2022] [:error] [pid 12870] 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)