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 07:24:58.406395 2021] [:error] [pid 20022] 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 07:24:58.425696 2021] [:error] [pid 20023] failed to exec() latex

Constraints are described by the constraint Jacobian

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.441524 2021] [:error] [pid 20024] failed to exec() latexgiven

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.457049 2021] [:error] [pid 20025] failed to exec() latexor

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.472533 2021] [:error] [pid 20026] failed to exec() latexwhere

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.488121 2021] [:error] [pid 20027] failed to exec() latexfor fixed joints and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.504930 2021] [:error] [pid 20028] 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 07:24:58.524596 2021] [:error] [pid 20029] failed to exec() latex

Substitute

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.541802 2021] [:error] [pid 20030] failed to exec() latexand rearrange to get:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.560911 2021] [:error] [pid 20031] 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 07:24:58.579670 2021] [:error] [pid 20032] failed to exec() latex, then eliminate

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.597154 2021] [:error] [pid 20033] 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 07:24:58.615422 2021] [:error] [pid 20034] failed to exec() latex) and arrive at:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.633440 2021] [:error] [pid 20035] failed to exec() latex

ODE is semi-implicit in that the Jacobians

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.651249 2021] [:error] [pid 20036] failed to exec() latexand external forces

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.668689 2021] [:error] [pid 20037] 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 07:24:58.696974 2021] [:error] [pid 20038] failed to exec() latex

such that:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.717010 2021] [:error] [pid 20039] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.734109 2021] [:error] [pid 20040] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.752872 2021] [:error] [pid 20041] failed to exec() latex

In ODE's *step.cpp*,

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.769953 2021] [:error] [pid 20042] failed to exec() latexis set to

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.786724 2021] [:error] [pid 20043] 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 07:24:58.805178 2021] [:error] [pid 20044] 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 07:24:58.824218 2021] [:error] [pid 20045] failed to exec() latex. It compute the

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.840909 2021] [:error] [pid 20046] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.856753 2021] [:error] [pid 20047] 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 07:24:58.872449 2021] [:error] [pid 20048] failed to exec() latexrows that already resolved. Suppose the length of

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.888182 2021] [:error] [pid 20049] failed to exec() latexis

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.903285 2021] [:error] [pid 20050] 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 07:24:58.918747 2021] [:error] [pid 20051] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.935533 2021] [:error] [pid 20052] 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 07:24:58.952215 2021] [:error] [pid 20053] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.967310 2021] [:error] [pid 20054] failed to exec() latex, with size

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.982411 2021] [:error] [pid 20055] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:58.998276 2021] [:error] [pid 20056] failed to exec() latex

Similarly, Non-Clamped Set

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.014045 2021] [:error] [pid 20057] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.030176 2021] [:error] [pid 20058] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.047070 2021] [:error] [pid 20059] failed to exec() latex

or

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.065109 2021] [:error] [pid 20060] failed to exec() latex

Do-not-care Set

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.082818 2021] [:error] [pid 20061] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.100333 2021] [:error] [pid 20062] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.118637 2021] [:error] [pid 20063] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.136271 2021] [:error] [pid 20064] 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 07:24:59.153939 2021] [:error] [pid 20065] 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 07:24:59.172182 2021] [:error] [pid 20066] failed to exec() latexclamped matrix of

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.189926 2021] [:error] [pid 20067] failed to exec() latex, we denote as

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.210155 2021] [:error] [pid 20068] failed to exec() latex, always maintains with an

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.228006 2021] [:error] [pid 20069] 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 07:24:59.246882 2021] [:error] [pid 20070] failed to exec() latex, we do an LDLT factorization of matrix

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.264781 2021] [:error] [pid 20071] 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 07:24:59.283517 2021] [:error] [pid 20072] 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 07:24:59.302367 2021] [:error] [pid 20073] failed to exec() latex, update

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.318452 2021] [:error] [pid 20074] failed to exec() latexand make sure to push

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.333688 2021] [:error] [pid 20076] failed to exec() latexto one of the sets:

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

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.363687 2021] [:error] [pid 20078] failed to exec() latexconstraints, since update on

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.379668 2021] [:error] [pid 20079] failed to exec() latexmight break the first

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.397120 2021] [:error] [pid 20080] failed to exec() latexconstraint satisfaction.

Once we finish a complete loop on

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.414136 2021] [:error] [pid 20081] 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 07:24:59.434404 2021] [:error] [pid 20082] 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 07:24:59.451556 2021] [:error] [pid 20083] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.468345 2021] [:error] [pid 20084] failed to exec() latex

If we solve for

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

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.501481 2021] [:error] [pid 20086] failed to exec() latex

then it follows that

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.517595 2021] [:error] [pid 20087] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.532660 2021] [:error] [pid 20088] 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 07:24:59.549295 2021] [:error] [pid 20089] failed to exec() latex

then

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.564512 2021] [:error] [pid 20090] failed to exec() latexupdate becomes

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.583070 2021] [:error] [pid 20091] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.600107 2021] [:error] [pid 20092] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.615892 2021] [:error] [pid 20093] failed to exec() latex, where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.631053 2021] [:error] [pid 20094] failed to exec() latexis the relaxation parameter.

where each

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.646841 2021] [:error] [pid 20095] 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 07:24:59.663008 2021] [:error] [pid 20096] failed to exec() latexupdate above, constraint accelerations

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.678751 2021] [:error] [pid 20097] failed to exec() latexare updated in the following manner:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.696790 2021] [:error] [pid 20098] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.713076 2021] [:error] [pid 20099] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Oct 28 07:24:59.729840 2021] [:error] [pid 20100] 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)