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: [Wed Oct 28 20:19:42.670972 2020] [:error] [pid 21234] 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 Oct 28 20:19:42.691392 2020] [:error] [pid 21235] failed to exec() latex

Constraints are described by the constraint Jacobian

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.708046 2020] [:error] [pid 21236] failed to exec() latexgiven

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.723913 2020] [:error] [pid 21237] failed to exec() latexor

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.740521 2020] [:error] [pid 21238] failed to exec() latexwhere

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.758595 2020] [:error] [pid 21239] failed to exec() latexfor fixed joints and

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.776329 2020] [:error] [pid 21240] failed to exec() latexfor contact joints.

If we rewrite in matrix form we have:

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.796781 2020] [:error] [pid 21241] failed to exec() latex

Substitute

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.815153 2020] [:error] [pid 21242] failed to exec() latexand rearrange to get:

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.834878 2020] [:error] [pid 21243] failed to exec() latex

Left multiply top row of the matrix equation by

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.853153 2020] [:error] [pid 21244] failed to exec() latex, then eliminate

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.871264 2020] [:error] [pid 21245] failed to exec() latexfrom the top row using the equality in the second row (

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.889238 2020] [:error] [pid 21246] failed to exec() latex) and arrive at:

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.907320 2020] [:error] [pid 21247] failed to exec() latex

ODE is semi-implicit in that the Jacobians

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.925975 2020] [:error] [pid 21248] failed to exec() latexand external forces

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.944211 2020] [:error] [pid 21249] 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: [Wed Oct 28 20:19:42.973145 2020] [:error] [pid 21250] failed to exec() latex

such that:

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:42.991704 2020] [:error] [pid 21251] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.009810 2020] [:error] [pid 21252] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.028324 2020] [:error] [pid 21254] failed to exec() latex

In ODE's *step.cpp*,

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.046773 2020] [:error] [pid 21255] failed to exec() latexis set to

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.063079 2020] [:error] [pid 21256] failed to exec() latex, then it has consistent form with SOR PGS LCP:

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.081004 2020] [:error] [pid 21257] 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 Oct 28 20:19:43.097610 2020] [:error] [pid 21258] failed to exec() latex. It compute the

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.114542 2020] [:error] [pid 21259] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.131236 2020] [:error] [pid 21260] failed to exec() latexwithout violating the non-interpenetration or box friction conditions for the previous

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.148477 2020] [:error] [pid 21261] failed to exec() latexrows that already resolved. Suppose the length of

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.165010 2020] [:error] [pid 21262] failed to exec() latexis

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.181682 2020] [:error] [pid 21263] failed to exec() latex, the solution should be obtained after we solve the

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.199159 2020] [:error] [pid 21264] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.216393 2020] [:error] [pid 21265] 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 Oct 28 20:19:43.233822 2020] [:error] [pid 21266] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.250679 2020] [:error] [pid 21267] failed to exec() latex, with size

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.267444 2020] [:error] [pid 21268] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.284867 2020] [:error] [pid 21269] failed to exec() latex

Similarly, Non-Clamped Set

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.302389 2020] [:error] [pid 21270] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.319014 2020] [:error] [pid 21271] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.336693 2020] [:error] [pid 21272] failed to exec() latex

or

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.354425 2020] [:error] [pid 21273] failed to exec() latex

Do-not-care Set

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.371780 2020] [:error] [pid 21274] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.388796 2020] [:error] [pid 21275] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.406713 2020] [:error] [pid 21276] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.424186 2020] [:error] [pid 21277] failed to exec() latexcould be any value. The permuted index is in the order of:

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.441865 2020] [:error] [pid 21278] failed to exec() latex.

During execution of Dantzig's algorithm, the left top

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.459544 2020] [:error] [pid 21279] failed to exec() latexclamped matrix of

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.476846 2020] [:error] [pid 21280] failed to exec() latex, we denote as

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.493797 2020] [:error] [pid 21281] failed to exec() latex, always maintains with an

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.510880 2020] [:error] [pid 21282] 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 Oct 28 20:19:43.528988 2020] [:error] [pid 21283] failed to exec() latex, we do an LDLT factorization of matrix

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.546068 2020] [:error] [pid 21284] 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 Oct 28 20:19:43.564261 2020] [:error] [pid 21285] 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: [Wed Oct 28 20:19:43.580539 2020] [:error] [pid 21286] failed to exec() latex, update

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.594316 2020] [:error] [pid 21287] failed to exec() latexand make sure to push

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.608318 2020] [:error] [pid 21288] failed to exec() latexto one of the sets:

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.624851 2020] [:error] [pid 21289] failed to exec() latex, i.e. don't violate the first

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.641978 2020] [:error] [pid 21290] failed to exec() latexconstraints, since update on

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.657274 2020] [:error] [pid 21291] failed to exec() latexmight break the first

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.674672 2020] [:error] [pid 21292] failed to exec() latexconstraint satisfaction.

Once we finish a complete loop on

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.689846 2020] [:error] [pid 21293] 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 Oct 28 20:19:43.706394 2020] [:error] [pid 21294] 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 Oct 28 20:19:43.721065 2020] [:error] [pid 21295] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.736442 2020] [:error] [pid 21296] failed to exec() latex

If we solve for

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.750510 2020] [:error] [pid 21297] failed to exec() latexin delta-form using Gauss-Seidel, i.e.

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.764864 2020] [:error] [pid 21298] failed to exec() latex

then it follows that

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.779169 2020] [:error] [pid 21299] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.792764 2020] [:error] [pid 21300] 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: [Wed Oct 28 20:19:43.808154 2020] [:error] [pid 21301] failed to exec() latex

then

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.821764 2020] [:error] [pid 21302] failed to exec() latexupdate becomes

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.835960 2020] [:error] [pid 21303] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.850055 2020] [:error] [pid 21304] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.864287 2020] [:error] [pid 21305] failed to exec() latex, where

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.877490 2020] [:error] [pid 21306] failed to exec() latexis the relaxation parameter.

where each

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.891312 2020] [:error] [pid 21307] 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: [Wed Oct 28 20:19:43.906123 2020] [:error] [pid 21308] failed to exec() latexupdate above, constraint accelerations

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.920785 2020] [:error] [pid 21309] failed to exec() latexare updated in the following manner:

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.935698 2020] [:error] [pid 21310] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.949772 2020] [:error] [pid 21311] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Wed Oct 28 20:19:43.964681 2020] [:error] [pid 21312] 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)