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: [Fri Sep 30 15:19:57.208621 2022] [:error] [pid 15470] 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 Sep 30 15:19:57.224704 2022] [:error] [pid 15471] failed to exec() latex

Constraints are described by the constraint Jacobian

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.237792 2022] [:error] [pid 15472] failed to exec() latexgiven

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.259713 2022] [:error] [pid 15473] failed to exec() latexor

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.271491 2022] [:error] [pid 15474] failed to exec() latexwhere

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.286765 2022] [:error] [pid 15475] failed to exec() latexfor fixed joints and

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.305214 2022] [:error] [pid 15476] failed to exec() latexfor contact joints.

If we rewrite in matrix form we have:

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.319593 2022] [:error] [pid 15477] failed to exec() latex

Substitute

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.331615 2022] [:error] [pid 15478] failed to exec() latexand rearrange to get:

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.345163 2022] [:error] [pid 15479] failed to exec() latex

Left multiply top row of the matrix equation by

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.357465 2022] [:error] [pid 15480] failed to exec() latex, then eliminate

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.369182 2022] [:error] [pid 15481] failed to exec() latexfrom the top row using the equality in the second row (

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.384532 2022] [:error] [pid 15482] failed to exec() latex) and arrive at:

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.406537 2022] [:error] [pid 15483] failed to exec() latex

ODE is semi-implicit in that the Jacobians

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.419110 2022] [:error] [pid 15484] failed to exec() latexand external forces

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.430525 2022] [:error] [pid 15485] 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: [Fri Sep 30 15:19:57.458488 2022] [:error] [pid 15486] failed to exec() latex

such that:

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.474026 2022] [:error] [pid 15487] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.497225 2022] [:error] [pid 15488] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.519620 2022] [:error] [pid 15489] failed to exec() latex

In ODE's *step.cpp*,

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.541811 2022] [:error] [pid 15490] failed to exec() latexis set to

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.565174 2022] [:error] [pid 15491] failed to exec() latex, then it has consistent form with SOR PGS LCP:

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.580569 2022] [:error] [pid 15492] 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 Sep 30 15:19:57.603074 2022] [:error] [pid 15493] failed to exec() latex. It compute the

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.617635 2022] [:error] [pid 15494] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.631858 2022] [:error] [pid 15495] failed to exec() latexwithout violating the non-interpenetration or box friction conditions for the previous

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.652316 2022] [:error] [pid 15496] failed to exec() latexrows that already resolved. Suppose the length of

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.667158 2022] [:error] [pid 15497] failed to exec() latexis

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.681319 2022] [:error] [pid 15498] failed to exec() latex, the solution should be obtained after we solve the

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.694918 2022] [:error] [pid 15499] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.707530 2022] [:error] [pid 15500] 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 Sep 30 15:19:57.729014 2022] [:error] [pid 15501] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.741876 2022] [:error] [pid 15502] failed to exec() latex, with size

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.766050 2022] [:error] [pid 15503] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.789031 2022] [:error] [pid 15504] failed to exec() latex

Similarly, Non-Clamped Set

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.803660 2022] [:error] [pid 15505] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.827810 2022] [:error] [pid 15506] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.842465 2022] [:error] [pid 15508] failed to exec() latex

or

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.856827 2022] [:error] [pid 15509] failed to exec() latex

Do-not-care Set

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.879372 2022] [:error] [pid 15510] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.893436 2022] [:error] [pid 15511] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.908365 2022] [:error] [pid 15512] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.922409 2022] [:error] [pid 15513] failed to exec() latexcould be any value. The permuted index is in the order of:

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.936220 2022] [:error] [pid 15514] failed to exec() latex.

During execution of Dantzig's algorithm, the left top

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.950380 2022] [:error] [pid 15515] failed to exec() latexclamped matrix of

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.972066 2022] [:error] [pid 15516] failed to exec() latex, we denote as

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.985419 2022] [:error] [pid 15517] failed to exec() latex, always maintains with an

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:57.999464 2022] [:error] [pid 15518] 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 Sep 30 15:19:58.014379 2022] [:error] [pid 15519] failed to exec() latex, we do an LDLT factorization of matrix

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.031324 2022] [:error] [pid 15520] 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 Sep 30 15:19:58.048848 2022] [:error] [pid 15521] 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: [Fri Sep 30 15:19:58.063438 2022] [:error] [pid 15522] failed to exec() latex, update

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.086938 2022] [:error] [pid 15523] failed to exec() latexand make sure to push

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.100796 2022] [:error] [pid 15524] failed to exec() latexto one of the sets:

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.120538 2022] [:error] [pid 15525] failed to exec() latex, i.e. don't violate the first

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.137442 2022] [:error] [pid 15526] failed to exec() latexconstraints, since update on

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.150917 2022] [:error] [pid 15527] failed to exec() latexmight break the first

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.163706 2022] [:error] [pid 15528] failed to exec() latexconstraint satisfaction.

Once we finish a complete loop on

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.177926 2022] [:error] [pid 15529] 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 Sep 30 15:19:58.195479 2022] [:error] [pid 15530] 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 Sep 30 15:19:58.211048 2022] [:error] [pid 15531] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.225874 2022] [:error] [pid 15532] failed to exec() latex

If we solve for

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.240260 2022] [:error] [pid 15533] failed to exec() latexin delta-form using Gauss-Seidel, i.e.

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.255496 2022] [:error] [pid 15534] failed to exec() latex

then it follows that

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.275255 2022] [:error] [pid 15535] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.293675 2022] [:error] [pid 15536] 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: [Fri Sep 30 15:19:58.308638 2022] [:error] [pid 15537] failed to exec() latex

then

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.322317 2022] [:error] [pid 15538] failed to exec() latexupdate becomes

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.336506 2022] [:error] [pid 15539] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.361803 2022] [:error] [pid 15540] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.375804 2022] [:error] [pid 15541] failed to exec() latex, where

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.389211 2022] [:error] [pid 15542] failed to exec() latexis the relaxation parameter.

where each

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.403178 2022] [:error] [pid 15543] 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: [Fri Sep 30 15:19:58.417244 2022] [:error] [pid 15544] failed to exec() latexupdate above, constraint accelerations

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.431249 2022] [:error] [pid 15545] failed to exec() latexare updated in the following manner:

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.445602 2022] [:error] [pid 15546] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.459195 2022] [:error] [pid 15547] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Fri Sep 30 15:19:58.480896 2022] [:error] [pid 15548] 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)