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 Apr 25 13:43:55.160895 2024] [:error] [pid 23645] 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 Apr 25 13:43:55.197395 2024] [:error] [pid 23646] failed to exec() latex

Constraints are described by the constraint Jacobian

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.212197 2024] [:error] [pid 23647] failed to exec() latexgiven

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.225304 2024] [:error] [pid 23648] failed to exec() latexor

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.239383 2024] [:error] [pid 23649] failed to exec() latexwhere

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.254082 2024] [:error] [pid 23650] failed to exec() latexfor fixed joints and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.270540 2024] [:error] [pid 23651] failed to exec() latexfor contact joints.

If we rewrite in matrix form we have:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.287344 2024] [:error] [pid 23652] failed to exec() latex

Substitute

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.301780 2024] [:error] [pid 23653] failed to exec() latexand rearrange to get:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.319102 2024] [:error] [pid 23654] failed to exec() latex

Left multiply top row of the matrix equation by

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.334791 2024] [:error] [pid 23655] failed to exec() latex, then eliminate

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.361685 2024] [:error] [pid 23656] failed to exec() latexfrom the top row using the equality in the second row (

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.377289 2024] [:error] [pid 23657] failed to exec() latex) and arrive at:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.392210 2024] [:error] [pid 23658] failed to exec() latex

ODE is semi-implicit in that the Jacobians

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.407453 2024] [:error] [pid 23659] failed to exec() latexand external forces

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.422481 2024] [:error] [pid 23660] 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 Apr 25 13:43:55.448017 2024] [:error] [pid 23661] failed to exec() latex

such that:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.464603 2024] [:error] [pid 23662] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.485439 2024] [:error] [pid 23665] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.503247 2024] [:error] [pid 23666] failed to exec() latex

In ODE's *step.cpp*,

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.518779 2024] [:error] [pid 23667] failed to exec() latexis set to

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.533318 2024] [:error] [pid 23668] failed to exec() latex, then it has consistent form with SOR PGS LCP:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.552060 2024] [:error] [pid 23669] 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 Apr 25 13:43:55.569289 2024] [:error] [pid 23670] failed to exec() latex. It compute the

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.585451 2024] [:error] [pid 23671] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.603431 2024] [:error] [pid 23672] 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 Apr 25 13:43:55.620425 2024] [:error] [pid 23673] failed to exec() latexrows that already resolved. Suppose the length of

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.635550 2024] [:error] [pid 23674] failed to exec() latexis

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.649612 2024] [:error] [pid 23675] failed to exec() latex, the solution should be obtained after we solve the

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.663546 2024] [:error] [pid 23676] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.678838 2024] [:error] [pid 23677] 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 Apr 25 13:43:55.702013 2024] [:error] [pid 23678] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.720047 2024] [:error] [pid 23679] failed to exec() latex, with size

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.736189 2024] [:error] [pid 23680] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.752607 2024] [:error] [pid 23681] failed to exec() latex

Similarly, Non-Clamped Set

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.767446 2024] [:error] [pid 23682] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.794917 2024] [:error] [pid 23683] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.809694 2024] [:error] [pid 23684] failed to exec() latex

or

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.823240 2024] [:error] [pid 23685] failed to exec() latex

Do-not-care Set

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.841984 2024] [:error] [pid 23686] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.855380 2024] [:error] [pid 23687] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.868263 2024] [:error] [pid 23688] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.880454 2024] [:error] [pid 23689] 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 Apr 25 13:43:55.892257 2024] [:error] [pid 23690] failed to exec() latex.

During execution of Dantzig's algorithm, the left top

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.905265 2024] [:error] [pid 23691] failed to exec() latexclamped matrix of

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.916541 2024] [:error] [pid 23692] failed to exec() latex, we denote as

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.927882 2024] [:error] [pid 23693] failed to exec() latex, always maintains with an

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.941984 2024] [:error] [pid 23694] 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 Apr 25 13:43:55.956756 2024] [:error] [pid 23695] failed to exec() latex, we do an LDLT factorization of matrix

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:55.968481 2024] [:error] [pid 23696] 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 Apr 25 13:43:55.983987 2024] [:error] [pid 23697] 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 Apr 25 13:43:56.001303 2024] [:error] [pid 23698] failed to exec() latex, update

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.016698 2024] [:error] [pid 23699] failed to exec() latexand make sure to push

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.030369 2024] [:error] [pid 23700] failed to exec() latexto one of the sets:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.042111 2024] [:error] [pid 23701] failed to exec() latex, i.e. don't violate the first

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.056622 2024] [:error] [pid 23703] failed to exec() latexconstraints, since update on

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.070255 2024] [:error] [pid 23704] failed to exec() latexmight break the first

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.083856 2024] [:error] [pid 23705] failed to exec() latexconstraint satisfaction.

Once we finish a complete loop on

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.099328 2024] [:error] [pid 23706] 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 Apr 25 13:43:56.118645 2024] [:error] [pid 23707] 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 Apr 25 13:43:56.135255 2024] [:error] [pid 23708] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.150978 2024] [:error] [pid 23709] failed to exec() latex

If we solve for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.165143 2024] [:error] [pid 23710] failed to exec() latexin delta-form using Gauss-Seidel, i.e.

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.180160 2024] [:error] [pid 23711] failed to exec() latex

then it follows that

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.196442 2024] [:error] [pid 23712] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.210705 2024] [:error] [pid 23713] 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 Apr 25 13:43:56.226410 2024] [:error] [pid 23714] failed to exec() latex

then

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.240159 2024] [:error] [pid 23715] failed to exec() latexupdate becomes

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.254263 2024] [:error] [pid 23716] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.268440 2024] [:error] [pid 23717] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.282039 2024] [:error] [pid 23718] failed to exec() latex, where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.294926 2024] [:error] [pid 23719] failed to exec() latexis the relaxation parameter.

where each

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.309553 2024] [:error] [pid 23720] 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 Apr 25 13:43:56.324015 2024] [:error] [pid 23721] failed to exec() latexupdate above, constraint accelerations

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.337995 2024] [:error] [pid 23722] failed to exec() latexare updated in the following manner:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.353083 2024] [:error] [pid 23723] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.371470 2024] [:error] [pid 23724] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 13:43:56.399239 2024] [:error] [pid 23725] 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)