Contents

  1. ODE Solvers
    1. Basic Governing Equations of Constrained Dynamics
    2. Solvers
      1. Dantzig's Agorithm
      2. SOR PGS LCP

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:

[Sat Apr 20 07:52:24.623301 2024] [:error] [pid 6857] 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:

[Sat Apr 20 07:52:24.641135 2024] [:error] [pid 6858] failed to exec() latex

Constraints are described by the constraint Jacobian 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.655353 2024] [:error] [pid 6859] failed to exec() latex
given 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.668645 2024] [:error] [pid 6860] failed to exec() latex
or 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.681680 2024] [:error] [pid 6861] failed to exec() latex
where 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.694177 2024] [:error] [pid 6862] failed to exec() latex
for fixed joints and 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.708507 2024] [:error] [pid 6863] failed to exec() latex
for contact joints.

If we rewrite in matrix form we have: 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.726552 2024] [:error] [pid 6864] failed to exec() latex

Substitute 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.741175 2024] [:error] [pid 6865] failed to exec() latex
and rearrange to get: 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.757030 2024] [:error] [pid 6866] failed to exec() latex

Left multiply top row of the matrix equation by 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.770969 2024] [:error] [pid 6867] failed to exec() latex
, then eliminate 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.784192 2024] [:error] [pid 6868] failed to exec() latex
from the top row using the equality in the second row (
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.797673 2024] [:error] [pid 6869] failed to exec() latex
) and arrive at: 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.812904 2024] [:error] [pid 6870] failed to exec() latex

ODE is semi-implicit in that the Jacobians 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.828382 2024] [:error] [pid 6871] failed to exec() latex
and external forces 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.842213 2024] [:error] [pid 6872] failed to exec() latex
from 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:

[Sat Apr 20 07:52:24.866287 2024] [:error] [pid 6873] failed to exec() latex

such that: 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.882176 2024] [:error] [pid 6874] failed to exec() latex


latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.898702 2024] [:error] [pid 6875] failed to exec() latex


latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.914810 2024] [:error] [pid 6876] failed to exec() latex

In ODE's step.cpp, 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.928800 2024] [:error] [pid 6877] failed to exec() latex
is set to 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.940555 2024] [:error] [pid 6878] failed to exec() latex
, then it has consistent form with SOR PGS LCP: 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.955412 2024] [:error] [pid 6879] 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:

[Sat Apr 20 07:52:24.968635 2024] [:error] [pid 6880] failed to exec() latex
. It compute the 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.980736 2024] [:error] [pid 6881] failed to exec() latex
unknown 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:24.993170 2024] [:error] [pid 6882] failed to exec() latex
without violating the non-interpenetration or box friction conditions for the previous 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.006885 2024] [:error] [pid 6883] failed to exec() latex
rows that already resolved. Suppose the length of 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.023264 2024] [:error] [pid 6884] failed to exec() latex
is 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.038720 2024] [:error] [pid 6885] failed to exec() latex
, the solution should be obtained after we solve the 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.055796 2024] [:error] [pid 6886] failed to exec() latex
unknown 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.071719 2024] [:error] [pid 6887] 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:

[Sat Apr 20 07:52:25.088753 2024] [:error] [pid 6888] failed to exec() latex
is a set of index 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.104353 2024] [:error] [pid 6889] failed to exec() latex
, with size 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.120105 2024] [:error] [pid 6890] failed to exec() latex
that satisfies: 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.136648 2024] [:error] [pid 6891] failed to exec() latex

Similarly, Non-Clamped Set 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.153073 2024] [:error] [pid 6892] failed to exec() latex
is a set of index 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.167605 2024] [:error] [pid 6893] failed to exec() latex
that satisfies: 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.181731 2024] [:error] [pid 6894] failed to exec() latex

or 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.196210 2024] [:error] [pid 6895] failed to exec() latex

Do-not-care Set 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.210232 2024] [:error] [pid 6896] failed to exec() latex
is a set of index 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.225191 2024] [:error] [pid 6897] failed to exec() latex
that satisfies: 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.241115 2024] [:error] [pid 6898] failed to exec() latex

where 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.256737 2024] [:error] [pid 6899] failed to exec() latex
could be any value. The permuted index is in the order of: 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.272071 2024] [:error] [pid 6900] failed to exec() latex
.

During execution of Dantzig's algorithm, the left top 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.288919 2024] [:error] [pid 6901] failed to exec() latex
clamped matrix of 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.304627 2024] [:error] [pid 6903] failed to exec() latex
, we denote as 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.317063 2024] [:error] [pid 6904] failed to exec() latex
, always maintains with an 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.328904 2024] [:error] [pid 6905] 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:

[Sat Apr 20 07:52:25.342170 2024] [:error] [pid 6906] failed to exec() latex
, we do an LDLT factorization of matrix 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.353875 2024] [:error] [pid 6907] 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:

[Sat Apr 20 07:52:25.368505 2024] [:error] [pid 6908] failed to exec() latex
unknowns.

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:

[Sat Apr 20 07:52:25.385063 2024] [:error] [pid 6909] failed to exec() latex
, update 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.398754 2024] [:error] [pid 6910] failed to exec() latex
and make sure to push 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.410925 2024] [:error] [pid 6911] failed to exec() latex
to one of the sets: 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.423541 2024] [:error] [pid 6912] failed to exec() latex
, i.e. don't violate the first
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.435645 2024] [:error] [pid 6913] failed to exec() latex
constraints, since update on 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.447383 2024] [:error] [pid 6914] failed to exec() latex
might break the first 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.459198 2024] [:error] [pid 6915] failed to exec() latex
constraint satisfaction.

Once we finish a complete loop on 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.473018 2024] [:error] [pid 6916] 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:

[Sat Apr 20 07:52:25.487929 2024] [:error] [pid 6917] 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:

[Sat Apr 20 07:52:25.501192 2024] [:error] [pid 6918] failed to exec() latex

and 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.514058 2024] [:error] [pid 6919] failed to exec() latex

If we solve for 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.526249 2024] [:error] [pid 6920] failed to exec() latex
in delta-form using Gauss-Seidel, i.e. 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.539537 2024] [:error] [pid 6921] failed to exec() latex

then it follows that 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.553767 2024] [:error] [pid 6922] failed to exec() latex

for 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.567710 2024] [:error] [pid 6923] failed to exec() latex

Formulate the desired solution in the form of acceleration1 (inverse mass matrix times constraint forces), denoted by 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.582478 2024] [:error] [pid 6924] failed to exec() latex

then 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.595368 2024] [:error] [pid 6925] failed to exec() latex
update becomes 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.608860 2024] [:error] [pid 6926] failed to exec() latex

and 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.622687 2024] [:error] [pid 6927] failed to exec() latex

for 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.634983 2024] [:error] [pid 6928] failed to exec() latex
, where 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.647172 2024] [:error] [pid 6929] failed to exec() latex
is the relaxation parameter.

where each 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.661146 2024] [:error] [pid 6930] failed to exec() latex
is 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:

[Sat Apr 20 07:52:25.674971 2024] [:error] [pid 6931] failed to exec() latex
update above, constraint accelerations 
latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.687803 2024] [:error] [pid 6932] failed to exec() latex
are updated in the following manner: 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.701409 2024] [:error] [pid 6933] failed to exec() latex

for 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.717584 2024] [:error] [pid 6934] failed to exec() latex

where 

latex error! exitcode was 2 (signal 0), transscript follows:

[Sat Apr 20 07:52:25.731757 2024] [:error] [pid 6935] failed to exec() latex

For more details please see the list of references.

  1. to clarify, in quickstep.cpp, $$\bar{a}_c$$ is denoted by variable fc as of svn revision 1675 (1)

Wiki: physics_ode/ODE (last edited 2015-02-25 03:59:31 by Ying)

Except where otherwise noted, the ROS wiki is licensed under the
Creative Commons Attribution 3.0