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:

[Wed Apr 24 21:08:37.660991 2024] [:error] [pid 14488] 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:

[Wed Apr 24 21:08:37.679370 2024] [:error] [pid 14489] failed to exec() latex

Constraints are described by the constraint Jacobian 

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

[Wed Apr 24 21:08:37.694589 2024] [:error] [pid 14490] failed to exec() latex
given 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:37.709631 2024] [:error] [pid 14491] failed to exec() latex
or 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:37.723669 2024] [:error] [pid 14492] failed to exec() latex
where 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:37.737803 2024] [:error] [pid 14493] failed to exec() latex
for fixed joints and 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:37.752133 2024] [:error] [pid 14494] failed to exec() latex
for contact joints.

If we rewrite in matrix form we have: 

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

[Wed Apr 24 21:08:37.770890 2024] [:error] [pid 14495] failed to exec() latex

Substitute 

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

[Wed Apr 24 21:08:37.785700 2024] [:error] [pid 14496] failed to exec() latex
and rearrange to get: 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:37.803112 2024] [:error] [pid 14499] failed to exec() latex

Left multiply top row of the matrix equation by 

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

[Wed Apr 24 21:08:37.817966 2024] [:error] [pid 14500] failed to exec() latex
, then eliminate 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:37.831437 2024] [:error] [pid 14501] failed to exec() latex
from the top row using the equality in the second row (
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:37.845996 2024] [:error] [pid 14502] failed to exec() latex
) and arrive at: 

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

[Wed Apr 24 21:08:37.861050 2024] [:error] [pid 14504] failed to exec() latex

ODE is semi-implicit in that the Jacobians 

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

[Wed Apr 24 21:08:37.876369 2024] [:error] [pid 14511] failed to exec() latex
and external forces 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:37.890829 2024] [:error] [pid 14512] 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:

[Wed Apr 24 21:08:37.915565 2024] [:error] [pid 14513] failed to exec() latex

such that: 

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

[Wed Apr 24 21:08:37.931665 2024] [:error] [pid 14514] failed to exec() latex


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

[Wed Apr 24 21:08:37.946837 2024] [:error] [pid 14515] failed to exec() latex


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

[Wed Apr 24 21:08:37.959600 2024] [:error] [pid 14516] failed to exec() latex

In ODE's step.cpp, 

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

[Wed Apr 24 21:08:37.973818 2024] [:error] [pid 14517] failed to exec() latex
is set to 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:37.985958 2024] [:error] [pid 14518] failed to exec() latex
, then it has consistent form with SOR PGS LCP: 

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

[Wed Apr 24 21:08:38.000664 2024] [:error] [pid 14519] 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:

[Wed Apr 24 21:08:38.014555 2024] [:error] [pid 14520] failed to exec() latex
. It compute the 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.026305 2024] [:error] [pid 14521] failed to exec() latex
unknown 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.038805 2024] [:error] [pid 14522] 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:

[Wed Apr 24 21:08:38.050607 2024] [:error] [pid 14523] failed to exec() latex
rows that already resolved. Suppose the length of 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.061725 2024] [:error] [pid 14524] failed to exec() latex
is 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.072735 2024] [:error] [pid 14525] failed to exec() latex
, the solution should be obtained after we solve the 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.083898 2024] [:error] [pid 14526] failed to exec() latex
unknown 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.095098 2024] [:error] [pid 14527] 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:

[Wed Apr 24 21:08:38.107821 2024] [:error] [pid 14528] failed to exec() latex
is a set of index 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.118542 2024] [:error] [pid 14529] failed to exec() latex
, with size 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.136505 2024] [:error] [pid 14530] failed to exec() latex
that satisfies: 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.148202 2024] [:error] [pid 14531] failed to exec() latex

Similarly, Non-Clamped Set 

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

[Wed Apr 24 21:08:38.160014 2024] [:error] [pid 14532] failed to exec() latex
is a set of index 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.171125 2024] [:error] [pid 14533] failed to exec() latex
that satisfies: 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.183285 2024] [:error] [pid 14534] failed to exec() latex

or 

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

[Wed Apr 24 21:08:38.195690 2024] [:error] [pid 14535] failed to exec() latex

Do-not-care Set 

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

[Wed Apr 24 21:08:38.207297 2024] [:error] [pid 14536] failed to exec() latex
is a set of index 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.218099 2024] [:error] [pid 14537] failed to exec() latex
that satisfies: 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.230788 2024] [:error] [pid 14538] failed to exec() latex

where 

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

[Wed Apr 24 21:08:38.243171 2024] [:error] [pid 14539] 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:

[Wed Apr 24 21:08:38.254757 2024] [:error] [pid 14540] failed to exec() latex
.

During execution of Dantzig's algorithm, the left top 

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

[Wed Apr 24 21:08:38.266769 2024] [:error] [pid 14541] failed to exec() latex
clamped matrix of 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.277500 2024] [:error] [pid 14542] failed to exec() latex
, we denote as 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.288193 2024] [:error] [pid 14543] failed to exec() latex
, always maintains with an 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.299344 2024] [:error] [pid 14545] 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:

[Wed Apr 24 21:08:38.372115 2024] [:error] [pid 14546] failed to exec() latex
, we do an LDLT factorization of matrix 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.384181 2024] [:error] [pid 14547] 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:

[Wed Apr 24 21:08:38.396846 2024] [:error] [pid 14548] 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:

[Wed Apr 24 21:08:38.409679 2024] [:error] [pid 14549] failed to exec() latex
, update 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.420837 2024] [:error] [pid 14550] failed to exec() latex
and make sure to push 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.433999 2024] [:error] [pid 14551] failed to exec() latex
to one of the sets: 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.446386 2024] [:error] [pid 14552] failed to exec() latex
, i.e. don't violate the first
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.459406 2024] [:error] [pid 14553] failed to exec() latex
constraints, since update on 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.472498 2024] [:error] [pid 14554] failed to exec() latex
might break the first 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.485068 2024] [:error] [pid 14555] failed to exec() latex
constraint satisfaction.

Once we finish a complete loop on 

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

[Wed Apr 24 21:08:38.498606 2024] [:error] [pid 14556] 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:

[Wed Apr 24 21:08:38.514764 2024] [:error] [pid 14557] 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:

[Wed Apr 24 21:08:38.529686 2024] [:error] [pid 14558] failed to exec() latex

and 

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

[Wed Apr 24 21:08:38.546835 2024] [:error] [pid 14559] failed to exec() latex

If we solve for 

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

[Wed Apr 24 21:08:38.562803 2024] [:error] [pid 14560] failed to exec() latex
in delta-form using Gauss-Seidel, i.e. 

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

[Wed Apr 24 21:08:38.576788 2024] [:error] [pid 14561] failed to exec() latex

then it follows that 

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

[Wed Apr 24 21:08:38.589561 2024] [:error] [pid 14562] failed to exec() latex

for 

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

[Wed Apr 24 21:08:38.601195 2024] [:error] [pid 14563] 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:

[Wed Apr 24 21:08:38.615118 2024] [:error] [pid 14564] failed to exec() latex

then 

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

[Wed Apr 24 21:08:38.626577 2024] [:error] [pid 14565] failed to exec() latex
update becomes 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.638392 2024] [:error] [pid 14566] failed to exec() latex

and 

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

[Wed Apr 24 21:08:38.649978 2024] [:error] [pid 14567] failed to exec() latex

for 

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

[Wed Apr 24 21:08:38.661245 2024] [:error] [pid 14568] failed to exec() latex
, where 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.671606 2024] [:error] [pid 14569] failed to exec() latex
is the relaxation parameter.

where each 

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

[Wed Apr 24 21:08:38.683137 2024] [:error] [pid 14570] 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:

[Wed Apr 24 21:08:38.695047 2024] [:error] [pid 14571] failed to exec() latex
update above, constraint accelerations 
latex error! exitcode was 2 (signal 0), transscript follows:

[Wed Apr 24 21:08:38.706117 2024] [:error] [pid 14572] failed to exec() latex
are updated in the following manner: 

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

[Wed Apr 24 21:08:38.717728 2024] [:error] [pid 14573] failed to exec() latex

for 

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

[Wed Apr 24 21:08:38.728506 2024] [:error] [pid 14574] failed to exec() latex

where 

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

[Wed Apr 24 21:08:38.740085 2024] [:error] [pid 14575] 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