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 Jul 29 12:26:52.751925 2021] [:error] [pid 24824] 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 Jul 29 12:26:52.771885 2021] [:error] [pid 24825] failed to exec() latex

Constraints are described by the constraint Jacobian

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.789243 2021] [:error] [pid 24826] failed to exec() latexgiven

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.805793 2021] [:error] [pid 24827] failed to exec() latexor

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.822049 2021] [:error] [pid 24828] failed to exec() latexwhere

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.838159 2021] [:error] [pid 24830] failed to exec() latexfor fixed joints and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.854157 2021] [:error] [pid 24831] failed to exec() latexfor contact joints.

If we rewrite in matrix form we have:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.869153 2021] [:error] [pid 24832] failed to exec() latex

Substitute

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.881874 2021] [:error] [pid 24833] failed to exec() latexand rearrange to get:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.895612 2021] [:error] [pid 24834] failed to exec() latex

Left multiply top row of the matrix equation by

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.908488 2021] [:error] [pid 24835] failed to exec() latex, then eliminate

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.921079 2021] [:error] [pid 24836] failed to exec() latexfrom the top row using the equality in the second row (

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.933833 2021] [:error] [pid 24837] failed to exec() latex) and arrive at:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.947088 2021] [:error] [pid 24838] failed to exec() latex

ODE is semi-implicit in that the Jacobians

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.960361 2021] [:error] [pid 24839] failed to exec() latexand external forces

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:52.972813 2021] [:error] [pid 24840] 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 Jul 29 12:26:52.993678 2021] [:error] [pid 24841] failed to exec() latex

such that:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.006795 2021] [:error] [pid 24842] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.019766 2021] [:error] [pid 24843] failed to exec() latex

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.032449 2021] [:error] [pid 24844] failed to exec() latex

In ODE's *step.cpp*,

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.045500 2021] [:error] [pid 24845] failed to exec() latexis set to

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.061574 2021] [:error] [pid 24846] failed to exec() latex, then it has consistent form with SOR PGS LCP:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.076054 2021] [:error] [pid 24847] 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 Jul 29 12:26:53.089427 2021] [:error] [pid 24848] failed to exec() latex. It compute the

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.101490 2021] [:error] [pid 24849] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.113188 2021] [:error] [pid 24850] 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 Jul 29 12:26:53.125840 2021] [:error] [pid 24851] failed to exec() latexrows that already resolved. Suppose the length of

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.137972 2021] [:error] [pid 24852] failed to exec() latexis

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.149823 2021] [:error] [pid 24853] failed to exec() latex, the solution should be obtained after we solve the

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.161751 2021] [:error] [pid 24854] failed to exec() latexunknown

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.173694 2021] [:error] [pid 24855] 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 Jul 29 12:26:53.186302 2021] [:error] [pid 24856] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.198062 2021] [:error] [pid 24857] failed to exec() latex, with size

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.209614 2021] [:error] [pid 24858] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.221950 2021] [:error] [pid 24859] failed to exec() latex

Similarly, Non-Clamped Set

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.233989 2021] [:error] [pid 24860] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.245585 2021] [:error] [pid 24861] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.258089 2021] [:error] [pid 24862] failed to exec() latex

or

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.270680 2021] [:error] [pid 24863] failed to exec() latex

Do-not-care Set

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.282825 2021] [:error] [pid 24864] failed to exec() latexis a set of index

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.295108 2021] [:error] [pid 24865] failed to exec() latexthat satisfies:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.309370 2021] [:error] [pid 24866] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.323391 2021] [:error] [pid 24867] 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 Jul 29 12:26:53.337341 2021] [:error] [pid 24868] failed to exec() latex.

During execution of Dantzig's algorithm, the left top

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.351593 2021] [:error] [pid 24869] failed to exec() latexclamped matrix of

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.365585 2021] [:error] [pid 24870] failed to exec() latex, we denote as

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.379315 2021] [:error] [pid 24871] failed to exec() latex, always maintains with an

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.392971 2021] [:error] [pid 24872] 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 Jul 29 12:26:53.412043 2021] [:error] [pid 24873] failed to exec() latex, we do an LDLT factorization of matrix

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.428585 2021] [:error] [pid 24874] 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 Jul 29 12:26:53.446153 2021] [:error] [pid 24875] 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 Jul 29 12:26:53.463623 2021] [:error] [pid 24876] failed to exec() latex, update

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.477231 2021] [:error] [pid 24877] failed to exec() latexand make sure to push

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.490930 2021] [:error] [pid 24878] failed to exec() latexto one of the sets:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.504837 2021] [:error] [pid 24879] failed to exec() latex, i.e. don't violate the first

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.518595 2021] [:error] [pid 24880] failed to exec() latexconstraints, since update on

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.532097 2021] [:error] [pid 24881] failed to exec() latexmight break the first

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.545627 2021] [:error] [pid 24882] failed to exec() latexconstraint satisfaction.

Once we finish a complete loop on

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.558987 2021] [:error] [pid 24883] 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 Jul 29 12:26:53.573688 2021] [:error] [pid 24884] 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 Jul 29 12:26:53.587057 2021] [:error] [pid 24885] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.600087 2021] [:error] [pid 24886] failed to exec() latex

If we solve for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.612495 2021] [:error] [pid 24887] failed to exec() latexin delta-form using Gauss-Seidel, i.e.

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.625520 2021] [:error] [pid 24888] failed to exec() latex

then it follows that

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.638460 2021] [:error] [pid 24889] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.650405 2021] [:error] [pid 24890] 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 Jul 29 12:26:53.664059 2021] [:error] [pid 24891] failed to exec() latex

then

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.678913 2021] [:error] [pid 24892] failed to exec() latexupdate becomes

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.695931 2021] [:error] [pid 24893] failed to exec() latex

and

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.712811 2021] [:error] [pid 24894] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.728787 2021] [:error] [pid 24895] failed to exec() latex, where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.744570 2021] [:error] [pid 24896] failed to exec() latexis the relaxation parameter.

where each

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.761148 2021] [:error] [pid 24897] 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 Jul 29 12:26:53.777619 2021] [:error] [pid 24898] failed to exec() latexupdate above, constraint accelerations

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.793405 2021] [:error] [pid 24899] failed to exec() latexare updated in the following manner:

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.810067 2021] [:error] [pid 24900] failed to exec() latex

for

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.825742 2021] [:error] [pid 24901] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows: [Thu Jul 29 12:26:53.842281 2021] [:error] [pid 24902] 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)