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
ODE/latex_09e4deef6c97d8c40727528e24342d89c7d4817f_p1.png "$$\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
ODE/latex_b0a76f020550c0a7eb0c1e2ce80c8c1501b43bb6_p1.png "$$
\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 ODE/latex_3be43c4d4e302bffb57ff49811e4915966e5c584_p1.png "$$J$$")
given ODE/latex_425ca5808a9e4c7d6a162318d1c833b96baf5d19_p1.png "$$\overline{\overline{J}} \dot{\overline{v}} = -\overline{k}$$")
or ODE/latex_45e677358049f8b58ced4889865ef4cfc293267d_p1.png "$$\overline{\overline{J}} \overline{v} = \overline{c} $$")
where ODE/latex_7485fa0d648d9bfed4139dc2cd60ff40858e455d_p1.png "$$c_i = 0$$")
for fixed joints and ODE/latex_edcdf034a600bb212f42a5fa6753f3184c91a08f_p1.png "$$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]$](attachments/physics_ode(2f)ODE/latex_f9da05dc7c2df04495cc486e01069888514d9605_p1.png "$\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 ODE/latex_f8f28b22e25b55b1a64f3fa388f9fcb37fddd5be_p1.png "$$\dot{v_i}=\frac{v_i^{n+1}-v_i^{n}}{h}$$")
and rearrange to get:
![$\Biggl[$
\begin{tabular}{c c}
$\frac{1}{h}\overline{\overline{M}}$ & $-\overline{\overline{J}}^{t}$ \\
$\overline{\overline{J}}$ & $0$ \\
\end{tabular}$\Biggr]$
$\Biggl[$
\begin{tabular}{c}
$\overline{v}^{n+1}$ \\
$\overline\lambda$
\end{tabular}$\Biggr]$
$= $
$\Biggl[$
\begin{tabular}{c}
$\overline{f}_{ext} + \frac{\overline{\overline{M}}\overline{v}^{n}}{h}$ \\
$-\overline{c}$
\end{tabular}
$\Biggr]$](attachments/physics_ode(2f)ODE/latex_503b911edb736023dd0159f26a9ce28b30c45a69_p1.png "$\Biggl[$
\begin{tabular}{c c}
$\frac{1}{h}\overline{\overline{M}}$ & $-\overline{\overline{J}}^{t}$ \\
$\overline{\overline{J}}$ & $0$ \\
\end{tabular}$\Biggr]$
$\Biggl[$
\begin{tabular}{c}
$\overline{v}^{n+1}$ \\
$\overline\lambda$
\end{tabular}$\Biggr]$
$= $
$\Biggl[$
\begin{tabular}{c}
$\overline{f}_{ext} + \frac{\overline{\overline{M}}\overline{v}^{n}}{h}$ \\
$-\overline{c}$
\end{tabular}
$\Biggr]$")
Left multiply top row of the matrix equation by ODE/latex_8fad322eb658eb92d7aa9fc476b2d26077ec80a5_p1.png "$$\overline{\overline{J}} \overline{\overline{M}}^{-1}$$")
, then eliminate ODE/latex_6b858403b4de8f98caa9d54adef3096755458ab9_p1.png "$$\overline{v}^{n+1}$$")
from the top row using the equality in the second row (ODE/latex_0e7da4ff8626af78210ddcf5ce4810400aa081db_p1.png "$$\overline{\overline{j}} \overline{v}^{n+1} = \overline{c}$$")
) and arrive at:
![$$ [\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} ]$$](attachments/physics_ode(2f)ODE/latex_5dc2423b85861868767910c264f641ead02b6843_p1.png "$$ [\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 and external forces ODE/latex_101fade72d3072f6e1dba2168012d5630981019a_p1.png "$$f_{ext}$$")
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:
ODE/latex_2e358aeff9e765ef40d0710449ff3921a8db70c5_p1.png "$$ \overline{\overline{A}} \overline{\lambda} = \overline{b} + \overline{\omega} $$")
such that:
ODE/latex_748d2a9a4233ae6f3d044d6cb3c4d9a09b432999_p1.png "$$ \lambda_i = lo, \ \ \ \omega_i > 0$$")
ODE/latex_a6c17133fa94e8ea4c279c6d4a349f36770fa4e9_p1.png "$$ \lambda_i = hi, \ \ \ \omega_i < 0$$")
ODE/latex_8ecf7c45852b2039ee00061fd45fff8984ed3bb1_p1.png "$$ lo < \lambda_i < hi, \ \ \ \omega_i = 0$$")
In ODE's step.cpp, ODE/latex_176a2a1306704e92aa3da792738c9d3f3d8e0d01_p1.png "$$\overline{\omega}$$")
is set to ODE/latex_7d0624feaf01e0c50432836ebbcba7580238956d_p1.png "$$\overline{0}$$")
, then it has consistent form with SOR PGS LCP:
ODE/latex_2567484c3dc82462b7101ed9138fd51c901db5b0_p1.png "$$ \overline{\overline{A}} \overline{\lambda} = \overline{b} $$")
The Dantzig algorithm applies to more general BLCP. It incrementally computes intermediate solutions for each entry in the unknown vector: ODE/latex_9496946609e1e56bc4837387c608fc66b05abd42_p1.png "$$\overline{\lambda}$$")
. It compute the ODE/latex_24b135c98fd4657a48dcd4eabd77c07d2f142797_p1.png "$$i^{th}$$")
unknown ODE/latex_ff9b0c30625901dd6374c237c5781afd97d7a55c_p1.png "$$\overline{\lambda}_i$$")
without violating the non-interpenetration or box friction conditions for the previous ODE/latex_3125792c612cd4635558bf2e0657774b65037b5d_p1.png "$$i$$")
rows that already resolved. Suppose the length of is ODE/latex_4bb143e858094b0b1fefe92ffbf393a3682de741_p1.png "$$m$$")
, the solution should be obtained after we solve the ODE/latex_63388d326c516c05b76868ab623043d5b2f5ec2f_p1.png "$$m^{th}$$")
unknown ODE/latex_9bc3cc8ce9dfb60e1b5972ea22c78b8a621b4bc1_p1.png "$$\overline{\lambda}_m$$")
.
We first define the different sets based on properties of unknowns: Clamped Set ODE/latex_4eebccd17f5f092d515f75809e0a7e7f8eea732e_p1.png "$$C$$")
is a set of index , with size ODE/latex_ea9056ceff6edcd92fe68c183c9bd5ab3720a52f_p1.png "$$nC$$")
that satisfies:
Similarly, Non-Clamped Set ODE/latex_8c452f44b945b80d778d61f5bbbfe89a62927ccb_p1.png "$$N$$")
is a set of index that satisfies:
or
Do-not-care Set ODE/latex_37b97f6469183833edacfaba5e6bd36b0a8c1525_p1.png "$$R$$")
is a set of index that satisfies:
ODE/latex_c0ab97aea7a920d821b2df9dc48c19e32c912394_p1.png "$$ \lambda_i = 0$$")
where ODE/latex_a9237168c263bc683c41d34bfa0a30e65a36a962_p1.png "$$\omega_i$$")
could be any value. The permuted index is in the order of: ![$$[C; N; R]$$](attachments/physics_ode(2f)ODE/latex_0a60cf38f630dafb8c47532eb737efbb82a70c35_p1.png "$$[C; N; R]$$")
.
During execution of Dantzig's algorithm, the left top ODE/latex_7e15a83840cf8ff8d9ee63db1d8b852c73e2135e_p1.png "$$nC \times nC$$")
clamped matrix of ODE/latex_28f3ee56458e23ed85a976f907114fb2a38ac98f_p1.png "$$\overline{\overline{A}}$$")
, we denote as ODE/latex_a26604de3e08eb6ccf1fc7f3744a822e5490d661_p1.png "$$\overline{\overline{AC}}$$")
, always maintains with an ODE/latex_e83c2c1adfd9225a5bab94894259fdaf417b0e41_p1.png "$$L*D*L^{\prime}$$")
(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 , we do an LDLT factorization of matrix , 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 unknowns.
When we hit the first friction constraint, compute the corresponding lower and upper bound, using normal force at the same contact.
Assume ODE/latex_cc78ea15f2924e5f84f20d9c8cc8120f5dd6c78f_p1.png "$$\overline{\lambda}_{i+1, ..., m} = 0$$")
, update ODE/latex_69132dc3e35d7c6a7a6555c77acaacda231005bc_p1.png "$$\overline{\lambda}_{i}$$")
and make sure to push ODE/latex_bd506988a3cd7fecfcbc56768a440f177eb678df_p1.png "$$\overline{\lambda}_{i=0,1,2, ... i-1}$$")
to one of the sets: , i.e. don't violate the firstODE/latex_11e070789db569efc6d9e392d845432d3ebe106d_p1.png "$$(i-1)$$")
constraints, since update on might break the first constraint satisfaction.
Once we finish a complete loop on ODE/latex_d130bc125e8ff917dc8ba23d1982003c027d8c04_p1.png "$$\overline{\lambda}_{i=0,1,2, ..., m}$$")
, 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.
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} ]$$](attachments/physics_ode(2f)ODE/latex_e1c18962d36e1eb4efcea14e1a08fb0d7ab70e6f_p1.png "$$ \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}] $$](attachments/physics_ode(2f)ODE/latex_de977ea417248626368f7928dccc2f8bb7578c90_p1.png "$$ \overline{\overline{A}} = [\overline{\overline{J}}\overline{\overline{M}}^{-1}\overline{\overline{J}}^{T}] $$")
If we solve for in delta-form using Gauss-Seidel, i.e.
ODE/latex_7cc2c23829951b40b707e79642b6353bc7909608_p1.png "$$\delta_i = \lambda_i^{n ew} - \lambda_i^{old}$$")
then it follows that
ODE/latex_febf94b68e2a8360dc46404ae0076a71807b6f2d_p1.png "$$ \delta_i = \frac{b_i}{A_{ii}} - \sum_{j=1}^{N_{constrai nts}}{\frac{A_{ij}}{A_{ii}} \lambda_j }$$")
for ODE/latex_3e970c2ae79b9708d571307edd699187a687dbb9_p1.png "$$ 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
ODE/latex_765db6a39f1eb273b30dd85d8f907c750cb14c86_p1.png "$$ \overline{a}_c = \overline{\overline{M}}^{-1}\overline{f}_c = \overline{\overline{M}}^{-1}\overline{\overline{J}}^{T} \overline\lambda $$")
then ODE/latex_c51299184668c2e5d260a613e80b4c8aae2b867d_p1.png "$$\overline\lambda$$")
update becomes
ODE/latex_b79656be843004b49e4b85688cca87b17c3cc46d_p1.png "$$ \delta_i = \frac{b_i}{A_{ii}} - \frac{ \sum_{k=1}^{ N_{D O F}} J_{ik} a_{c_{k}} }{A_{ii}} $$")
and
ODE/latex_bdebd0fa93660068f5ad01eb60861e253a462078_p1.png "$$ \lambda_i^{n ew} = \lambda_i^{old} + \omega \delta_i $$")
for , where ODE/latex_f41333b40a60dd354691c82a71f2ddf729e379f3_p1.png "$$\omega$$")
is the relaxation parameter.
where each ODE/latex_144cd4c4160bc2080636b32013ec4b5e13309ba2_p1.png "$$\lambda_i^{n e w}$$")
is projected into its corresponding solution space depending on the type of constraint specified.
At every iteration, for each update above, constraint accelerations ODE/latex_051d5d14060cdd92e8c6c98c7d8ef60a63550936_p1.png "$$a_{c_{k}}$$")
are updated in the following manner:
ODE/latex_257d68b41fcdd91aee5e911726a5d84e3e9e7f28_p1.png "$$a_{c_{k}}^{n+1} = a_{c_{k}}^{n} + G_{ki} (\omega \delta_{i})$$")
for ODE/latex_13dbe511a289a5ac7d5e9524ca0169e0b076fa48_p1.png "$$ k = 1 , ... , N_{DOF}$$")
where
ODE/latex_ebe2778b38f48a4f7a4e745741fb66f30f9b6ebb_p1.png "$$ \overline{\overline{G}} \equiv \overline{\overline{M}}^{-1} \overline{\overline{J}}^{T}$$")
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)