Contents

  1. ODE Solvers
    1. Basic Governing Equations of Constrained Dynamics
    2. Solvers
      1. Lemke'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 $$\overline{f}=\overline{\overline{M}}\overline{a}$$

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

$$ \overline{\overline{M}} \left( \overline{v}^{n+1} - \overline{v}^{n} \right) - h \overline{\overline{J}}^{T} \overline{\lambda} = h \overline{f}_{ext} $$

Constraints are described by the constraint Jacobian $$J$$ given $$\overline{\overline{J}} \dot{\overline{v}} = -\overline{k}$$ or $$\overline{\overline{J}} \overline{v} = \overline{c} $$ where $$c_i = 0$$ for fixed joints and $$c_i >=0$$ for contact joints.

If we rewrite in matrix form we have:

$\Biggl[$ \begin{tabular}{c c} $\overline{\overline{M}}$ & $-\overline{\overline{J}}^{t}$ \\ $\overline{\overline{J}}$ & $0$ \\ \end{tabular}$\Biggr]$ $\Biggl[$ \begin{tabular}{c} $\dot{\overline{v}}$ \\ $\overline\lambda$ \end{tabular}$\Biggr]$ $= $ $\Biggl[$ \begin{tabular}{c} $\overline{f}_{ext}$ \\ $-\overline{k}$ \end{tabular} $\Biggr]$

Substitute $$\dot{v_i}=\frac{v_i^{n+1}-v_i^{n}}{h}$$ and rearrange to get:

$$ [\overline{\overline{J}}\overline{\overline{M}}^{-1}\overline{\overline{J}}^{T}] \overline\lambda = \frac{\overline{c}}{h} - \overline{\overline{J}}[ \frac{\overline{v}^{n}}{h} + \overline{\overline{M}}^{-1}\overline{f}_{ext} ]$$

ODE is semi-implicit in that the Jacobians $$J$$ and external forces $$f_{ext}$$ from the previous time step are used throughout the iterations.

Solvers

ODE ships with two default solvers

  • Lemke'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).

Lemke'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.

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.

$$ \overline{\overline{A}} \overline{\lambda} = \overline{b} $$

where based on the derivations from governing equations in the previous section,

$$ \overline{b} = \frac{\overline{c}}{h} - \overline{\overline{J}}[ \frac{\overline{v}^{n}}{h} + \overline{\overline{M}}^{-1}\overline{f}_{ext} ]$$

and

$$ \overline{\overline{A}} = [\overline{\overline{J}}\overline{\overline{M}}^{-1}\overline{\overline{J}}^{T}] $$

If we solve for $$\overline{\lambda}$$ in delta-form using Gauss-Seidel, i.e.

$$\delta_i = \lambda_i^{n ew} - \lambda_i^{old}$$

then it follows that

$$ \delta_i = \frac{b_i}{A_{ii}} - \sum_{j=1}^{N_{constrai nts}}{\frac{A_{ij}}{A_{ii}} \lambda_j }$$

for $$ i = 1,...,N_{constra i n t s} $$

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

$$ \overline{a}_c = \overline{\overline{M}}^{-1}\overline{f}_c = \overline{\overline{M}}^{-1}\overline{\overline{J}}^{T} \overline\lambda $$

then $$\overline\lambda$$ update becomes

$$ \delta_i = \frac{b_i}{A_{ii}} - \frac{ \sum_{k=1}^{ N_{D O F}} J_{ik} a_{c_{k}} }{A_{ii}} $$

and

$$ \lambda_i^{n ew} = \lambda_i^{old} + \delta_i $$

for $$ i = 1,...,N_{constra i n t s} $$

where each $$\lambda_i^{n e w}$$ is projected into its corresponding solution space depending on the type of constraint specified.

At every iteration, for each $$i$$ update above, constraint accelerations $$a_{c_{k}}$$ are updated in the following manner:

$$a_{c_{k}}^{n+1} = a_{c_{k}}^{n} + G_{ki} \delta_{i}$$

for $$ k = 1 , ... , N_{DOF}$$

where

$$ \overline{\overline{G}} \equiv \overline{\overline{M}}^{-1} \overline{\overline{J}}^{T}$$

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 2010-10-16 19:44:33 by hsu)

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