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: [Mon Aug 10 09:13:42.330826 2020] [:error] [pid 11002] 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: [Mon Aug 10 09:13:42.352394 2020] [:error] [pid 11003] failed to exec() latex

Constraints are described by the constraint Jacobian

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.371192 2020] [:error] [pid 11004] failed to exec() latexgiven

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.389410 2020] [:error] [pid 11005] failed to exec() latexor

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.407394 2020] [:error] [pid 11006] failed to exec() latexwhere

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.425575 2020] [:error] [pid 11007] failed to exec() latexfor fixed joints and

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.444116 2020] [:error] [pid 11008] failed to exec() latexfor contact joints.

If we rewrite in matrix form we have:

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.465575 2020] [:error] [pid 11009] failed to exec() latex

Substitute

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.483074 2020] [:error] [pid 11010] failed to exec() latexand rearrange to get:

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.502612 2020] [:error] [pid 11011] failed to exec() latex

Left multiply top row of the matrix equation by

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.520709 2020] [:error] [pid 11012] failed to exec() latex, then eliminate

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.537857 2020] [:error] [pid 11013] failed to exec() latexfrom the top row using the equality in the second row (

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.555287 2020] [:error] [pid 11014] failed to exec() latex) and arrive at:

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.573991 2020] [:error] [pid 11015] failed to exec() latex

ODE is semi-implicit in that the Jacobians

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.593443 2020] [:error] [pid 11016] failed to exec() latexand external forces

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.612757 2020] [:error] [pid 11017] 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: [Mon Aug 10 09:13:42.640340 2020] [:error] [pid 11018] failed to exec() latex

such that:

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.658547 2020] [:error] [pid 11019] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.676369 2020] [:error] [pid 11020] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.694596 2020] [:error] [pid 11021] failed to exec() latex

In ODE's *step.cpp*,

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.712553 2020] [:error] [pid 11022] failed to exec() latexis set to

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.730037 2020] [:error] [pid 11023] failed to exec() latex, then it has consistent form with SOR PGS LCP:

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.748347 2020] [:error] [pid 11024] 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: [Mon Aug 10 09:13:42.766379 2020] [:error] [pid 11025] failed to exec() latex. It compute the

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.783617 2020] [:error] [pid 11026] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.800491 2020] [:error] [pid 11027] failed to exec() latexwithout violating the non-interpenetration or box friction conditions for the previous

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.817545 2020] [:error] [pid 11028] failed to exec() latexrows that already resolved. Suppose the length of

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.834716 2020] [:error] [pid 11029] failed to exec() latexis

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.849759 2020] [:error] [pid 11030] failed to exec() latex, the solution should be obtained after we solve the

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.864151 2020] [:error] [pid 11031] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.877676 2020] [:error] [pid 11032] 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: [Mon Aug 10 09:13:42.892273 2020] [:error] [pid 11033] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.905885 2020] [:error] [pid 11034] failed to exec() latex, with size

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.919617 2020] [:error] [pid 11035] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.933644 2020] [:error] [pid 11036] failed to exec() latex

Similarly, Non-Clamped Set

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.947655 2020] [:error] [pid 11037] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.961520 2020] [:error] [pid 11038] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.984354 2020] [:error] [pid 11039] failed to exec() latex

or

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:42.998656 2020] [:error] [pid 11040] failed to exec() latex

Do-not-care Set

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.012494 2020] [:error] [pid 11041] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.026054 2020] [:error] [pid 11043] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.040293 2020] [:error] [pid 11044] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.053759 2020] [:error] [pid 11045] failed to exec() latexcould be any value. The permuted index is in the order of:

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.067423 2020] [:error] [pid 11046] failed to exec() latex.

During execution of Dantzig's algorithm, the left top

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.082075 2020] [:error] [pid 11047] failed to exec() latexclamped matrix of

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.095791 2020] [:error] [pid 11048] failed to exec() latex, we denote as

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.109722 2020] [:error] [pid 11049] failed to exec() latex, always maintains with an

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.123336 2020] [:error] [pid 11050] 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: [Mon Aug 10 09:13:43.138291 2020] [:error] [pid 11051] failed to exec() latex, we do an LDLT factorization of matrix

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.151665 2020] [:error] [pid 11052] 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: [Mon Aug 10 09:13:43.166262 2020] [:error] [pid 11053] 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: [Mon Aug 10 09:13:43.180598 2020] [:error] [pid 11054] failed to exec() latex, update

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.194731 2020] [:error] [pid 11055] failed to exec() latexand make sure to push

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.212867 2020] [:error] [pid 11056] failed to exec() latexto one of the sets:

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.227357 2020] [:error] [pid 11057] failed to exec() latex, i.e. don't violate the first

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.241430 2020] [:error] [pid 11058] failed to exec() latexconstraints, since update on

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.255357 2020] [:error] [pid 11059] failed to exec() latexmight break the first

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.269695 2020] [:error] [pid 11060] failed to exec() latexconstraint satisfaction.

Once we finish a complete loop on

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.285913 2020] [:error] [pid 11061] 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: [Mon Aug 10 09:13:43.303081 2020] [:error] [pid 11062] 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: [Mon Aug 10 09:13:43.319034 2020] [:error] [pid 11063] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.338339 2020] [:error] [pid 11064] failed to exec() latex

If we solve for

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.357054 2020] [:error] [pid 11065] failed to exec() latexin delta-form using Gauss-Seidel, i.e.

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.375920 2020] [:error] [pid 11066] failed to exec() latex

then it follows that

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.394976 2020] [:error] [pid 11067] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.413416 2020] [:error] [pid 11068] 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: [Mon Aug 10 09:13:43.433767 2020] [:error] [pid 11069] failed to exec() latex

then

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.449360 2020] [:error] [pid 11070] failed to exec() latexupdate becomes

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.465936 2020] [:error] [pid 11071] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.482293 2020] [:error] [pid 11072] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.497763 2020] [:error] [pid 11073] failed to exec() latex, where

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.512490 2020] [:error] [pid 11074] failed to exec() latexis the relaxation parameter.

where each

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.527300 2020] [:error] [pid 11075] 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: [Mon Aug 10 09:13:43.542439 2020] [:error] [pid 11076] failed to exec() latexupdate above, constraint accelerations

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.556971 2020] [:error] [pid 11077] failed to exec() latexare updated in the following manner:

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.571981 2020] [:error] [pid 11078] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.586454 2020] [:error] [pid 11079] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Mon Aug 10 09:13:43.602158 2020] [:error] [pid 11080] 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)