Contents

## 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: [Thu Oct 28 21:01:50.747282 2021] [:error] [pid 17472] 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: [Thu Oct 28 21:01:50.766611 2021] [:error] [pid 17473] failed to exec() latex

Constraints are described by the constraint Jacobian

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.783834 2021] [:error] [pid 17474] failed to exec() latexgiven

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.799937 2021] [:error] [pid 17477] failed to exec() latexor

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.813922 2021] [:error] [pid 17478] failed to exec() latexwhere

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.827597 2021] [:error] [pid 17479] failed to exec() latexfor fixed joints and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.842776 2021] [:error] [pid 17480] failed to exec() latexfor contact joints.

If we rewrite in matrix form we have:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.860342 2021] [:error] [pid 17481] failed to exec() latex

Substitute

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.875790 2021] [:error] [pid 17482] failed to exec() latexand rearrange to get:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.893625 2021] [:error] [pid 17485] failed to exec() latex

Left multiply top row of the matrix equation by

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.909485 2021] [:error] [pid 17486] failed to exec() latex, then eliminate

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.922986 2021] [:error] [pid 17487] failed to exec() latexfrom the top row using the equality in the second row (

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.936567 2021] [:error] [pid 17488] failed to exec() latex) and arrive at:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.951126 2021] [:error] [pid 17490] failed to exec() latex

ODE is semi-implicit in that the Jacobians

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.964251 2021] [:error] [pid 17491] failed to exec() latexand external forces

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:50.979048 2021] [:error] [pid 17492] failed to exec() latexfrom 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: [Thu Oct 28 21:01:51.004198 2021] [:error] [pid 17493] failed to exec() latex

such that:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.021120 2021] [:error] [pid 17494] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.038039 2021] [:error] [pid 17495] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.054468 2021] [:error] [pid 17496] failed to exec() latex

In ODE's *step.cpp*,

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.070827 2021] [:error] [pid 17497] failed to exec() latexis set to

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.086617 2021] [:error] [pid 17498] failed to exec() latex, then it has consistent form with SOR PGS LCP:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.102508 2021] [:error] [pid 17499] 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: [Thu Oct 28 21:01:51.119912 2021] [:error] [pid 17500] failed to exec() latex. It compute the

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.135828 2021] [:error] [pid 17501] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.151598 2021] [:error] [pid 17502] failed to exec() latexwithout violating the non-interpenetration or box friction conditions for the previous

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.167802 2021] [:error] [pid 17503] failed to exec() latexrows that already resolved. Suppose the length of

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.183700 2021] [:error] [pid 17504] failed to exec() latexis

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.199515 2021] [:error] [pid 17505] failed to exec() latex, the solution should be obtained after we solve the

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.215571 2021] [:error] [pid 17506] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.231598 2021] [:error] [pid 17507] 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: [Thu Oct 28 21:01:51.251944 2021] [:error] [pid 17508] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.266554 2021] [:error] [pid 17509] failed to exec() latex, with size

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.280880 2021] [:error] [pid 17510] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.295619 2021] [:error] [pid 17511] failed to exec() latex

Similarly, Non-Clamped Set

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.309629 2021] [:error] [pid 17512] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.323433 2021] [:error] [pid 17513] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.338318 2021] [:error] [pid 17514] failed to exec() latex

or

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.353697 2021] [:error] [pid 17515] failed to exec() latex

Do-not-care Set

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.368490 2021] [:error] [pid 17516] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.383148 2021] [:error] [pid 17517] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.399769 2021] [:error] [pid 17518] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.415722 2021] [:error] [pid 17519] failed to exec() latexcould be any value. The permuted index is in the order of:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.431802 2021] [:error] [pid 17520] failed to exec() latex.

During execution of Dantzig's algorithm, the left top

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.447249 2021] [:error] [pid 17521] failed to exec() latexclamped matrix of

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.461617 2021] [:error] [pid 17522] failed to exec() latex, we denote as

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.475562 2021] [:error] [pid 17523] failed to exec() latex, always maintains with an

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.489862 2021] [:error] [pid 17524] 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: [Thu Oct 28 21:01:51.505156 2021] [:error] [pid 17525] failed to exec() latex, we do an LDLT factorization of matrix

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.519985 2021] [:error] [pid 17527] 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: [Thu Oct 28 21:01:51.535392 2021] [:error] [pid 17528] failed to exec() latexunknowns.

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: [Thu Oct 28 21:01:51.550892 2021] [:error] [pid 17529] failed to exec() latex, update

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.564925 2021] [:error] [pid 17530] failed to exec() latexand make sure to push

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.579095 2021] [:error] [pid 17531] failed to exec() latexto one of the sets:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.593315 2021] [:error] [pid 17532] failed to exec() latex, i.e. don't violate the first

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.607896 2021] [:error] [pid 17533] failed to exec() latexconstraints, since update on

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.624148 2021] [:error] [pid 17536] failed to exec() latexmight break the first

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.639556 2021] [:error] [pid 17538] failed to exec() latexconstraint satisfaction.

Once we finish a complete loop on

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.656802 2021] [:error] [pid 17540] 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: [Thu Oct 28 21:01:51.676285 2021] [:error] [pid 17543] 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: [Thu Oct 28 21:01:51.693923 2021] [:error] [pid 17545] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.711402 2021] [:error] [pid 17547] failed to exec() latex

If we solve for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.728528 2021] [:error] [pid 17549] failed to exec() latexin delta-form using Gauss-Seidel, i.e.

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.745752 2021] [:error] [pid 17551] failed to exec() latex

then it follows that

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.762940 2021] [:error] [pid 17553] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.779121 2021] [:error] [pid 17555] failed to exec() latex

Formulate the desired solution in the form of acceleration^{1} (inverse mass matrix times constraint forces), denoted by

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.797164 2021] [:error] [pid 17558] failed to exec() latex

then

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.813431 2021] [:error] [pid 17560] failed to exec() latexupdate becomes

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.830497 2021] [:error] [pid 17563] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.846738 2021] [:error] [pid 17565] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.862181 2021] [:error] [pid 17569] failed to exec() latex, where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.878933 2021] [:error] [pid 17571] failed to exec() latexis the relaxation parameter.

where each

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.894413 2021] [:error] [pid 17572] failed to exec() latexis 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: [Thu Oct 28 21:01:51.911524 2021] [:error] [pid 17574] failed to exec() latexupdate above, constraint accelerations

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.928754 2021] [:error] [pid 17576] failed to exec() latexare updated in the following manner:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.946766 2021] [:error] [pid 17581] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.963757 2021] [:error] [pid 17584] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 21:01:51.982290 2021] [:error] [pid 17587] failed to exec() latex

For more details please see the list of references.

to clarify, in

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