Remove midpoint integration, superseded by free-body gyroscopic derivatives.
The gyroscopic (bias) derivatives applied to standalone free bodies by the implicitfast integrator provide comparable stability for spinning bodies, with none of midpoint's restrictions: they apply under contacts, fluid forces and constraints, and preserve the linear force-velocity relation required by discrete-time inverse dynamics. The invdiscrete flag reverts to its original single meaning and no longer affects forward dynamics. Restore implicitfast coverage in the DiscreteInverseMatch test, removed when midpoint made discrete inverse dynamics untestable. Add implicit gyroscopic (bias) derivatives for free bodies in implicitfast. The implicitfast integrator drops the RNE (bias) derivative to stay on the symmetric Cholesky path, so fast-spinning free bodies integrate gyroscopic forces explicitly and can gain energy. Symmetrizing the gyroscopic Jacobian is not an option: its stabilizing content is the antisymmetric part, and adding only the symmetric part is destabilizing. Instead, exploit the fact that for a standalone free body the 6x6 block of M - h*D is decoupled from the rest of the system (qDeriv sparsity is tree-local): after the global solve, rebuild the block with the exact bias derivative in closed form (mjd_freeBias_vel) and re-solve it with dense unsymmetric LU, overwriting the block's rows of qacc. For lone spinning bodies this makes implicitfast match implicit to rounding, at ~150ns per eligible body: cheaper than the midpoint machinery it will replace. Eligibility is structural only; contacts, fluid and constraints need no gating. The same block is mirrored in discrete inverse dynamics (mj_discreteAcc), making invdiscrete exact for spinning free bodies. PiperOrigin-RevId: 948472495 Change-Id: I813ef3d98c7b399881bc8603b9f9208cfb02eb58
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
b2106db52f
commit
f0fa3d8260
+7
-16
@@ -661,22 +661,13 @@ from its default.
|
||||
.. _option-flag-invdiscrete:
|
||||
|
||||
:at:`invdiscrete`: :at-val:`[disable, enable], "disable"`
|
||||
This dual-purpose flag enables discrete-time inverse dynamics and disables :ref:`midpoint integration<geMidpoint>`.
|
||||
|
||||
Enable discrete-time inverse dynamics
|
||||
This flag **enables** discrete-time inverse dynamics with :ref:`mj_inverse` for all
|
||||
:ref:`integrators<option-integrator>` other than ``RK4``. Recall from the :ref:`numerical
|
||||
integration<geIntegration>` section that the one-step integrators (``Euler``, ``implicit`` and ``implicitfast``),
|
||||
modify the mass matrix :math:`M \rightarrow M-hD`. This implies that finite-differenced accelerations
|
||||
:math:`(v_{t+h} - v_t)/h` will not correspond to the continuous-time acceleration ``mjData.qacc``. When this flag
|
||||
is enabled, :ref:`mj_inverse` will interpret ``qacc`` as having been computed from the difference of two sequential
|
||||
velocities, and undo the above modification.
|
||||
|
||||
Disable midpoint integration
|
||||
Additionally and relatedly, this flag **disables** :ref:`midpoint integration<geMidpoint>` for free bodies, which
|
||||
would otherwise break the linear relationship between finite-differenced velocities and forces assumed by discrete
|
||||
inverse dynamics. Note that disabling midpoint integration might be useful for debugging or for other reasons,
|
||||
regardless or whether inverse dynamics are used.
|
||||
This flag enables discrete-time inverse dynamics with :ref:`mj_inverse` for all
|
||||
:ref:`integrators<option-integrator>` other than ``RK4``. Recall from the
|
||||
:ref:`numerical integration<geIntegration>` section that the one-step integrators (``Euler``, ``implicit`` and
|
||||
``implicitfast``), modify the mass matrix :math:`M \rightarrow M-hD`. This implies that finite-differenced
|
||||
accelerations :math:`(v_{t+h} - v_t)/h` will not correspond to the continuous-time acceleration ``mjData.qacc``.
|
||||
When this flag is enabled, :ref:`mj_inverse` will interpret ``qacc`` as having been computed from the difference of
|
||||
two sequential velocities, and undo the above modification.
|
||||
|
||||
|
||||
.. _option-flag-multiccd:
|
||||
|
||||
+10
-2
@@ -7,6 +7,14 @@ Upcoming version (not yet released)
|
||||
|
||||
General
|
||||
^^^^^^^
|
||||
- Replaced midpoint integration of free bodies with :ref:`gyroscopic derivatives<geFreeBody>` in the ``implicitfast``
|
||||
:ref:`integrator<geIntegrators>`: the bias-force derivative of every standalone free body is applied via a local
|
||||
unsymmetric solve of its decoupled block, making ``implicitfast`` identical to ``implicit`` for such bodies.
|
||||
Unlike midpoint integration, which required vacuum and no constraints, this applies in all environments (contacts,
|
||||
fluid, constraints), and is compatible with discrete-time inverse dynamics. Spinning free bodies no
|
||||
longer gain energy, but tumbling motion is now mildly damped; models requiring long-horizon energy conservation of
|
||||
tumbling bodies in vacuum should use ``RK4``. The :ref:`invdiscrete<option-flag-invdiscrete>` flag no longer has any
|
||||
effect on forward dynamics.
|
||||
- Added Nesterov momentum extrapolation with adaptive gradient restart (O'Donoghue-Candès) to the PGS solver,
|
||||
significantly improving convergence. Overall PGS now requires ~2x fewer iterations.
|
||||
- Added the Newton decrement -- the quadratic model's predicted cost improvement of the next iteration -- as a third
|
||||
@@ -233,7 +241,7 @@ General
|
||||
improving performance by ~20%.
|
||||
3. :commit:`b9c1877e` Added support for :ref:`elastic2d<flex-elasticity-elastic2d>` for trilinear and quadratic flex
|
||||
:ref:`dofs<body-flexcomp-dof>`.
|
||||
4. :commit:`910b3336` :ref:`Midpoint integration<geMidpoint>` is now restricted to the ``implicitfast``
|
||||
4. :commit:`910b3336` Midpoint integration is now restricted to the ``implicitfast``
|
||||
:ref:`integrator<geIntegrators>` and is disabled when fluid forces are active
|
||||
(nonzero :ref:`density<option-density>` or :ref:`viscosity<option-viscosity>`).
|
||||
Midpoint integration treats external forces as zero-order-hold constants, which causes
|
||||
@@ -335,7 +343,7 @@ General
|
||||
scalar arrays (``jnt_stiffness``, ``dof_damping``, etc.) continue to hold the linear coefficient and are unchanged.
|
||||
The polynomial order is defined by the new constant :ref:`mjNPOLY<glNumericSizes>`. A future breaking C-API change
|
||||
may unify the linear and higher-order coefficients into a single array.
|
||||
4. :commit:`0c337799` Added :ref:`midpoint integration<geMidpoint>` for standalone free bodies in ``implicit`` and
|
||||
4. :commit:`0c337799` Added midpoint integration for standalone free bodies in ``implicit`` and
|
||||
``implicitfast`` :ref:`integrators<geIntegrators>`. This applies the implicit midpoint rule to the rotational
|
||||
dynamics of free bodies with no children, conserving kinetic energy to machine precision in the absence of external
|
||||
torques. The :ref:`invdiscrete<option-flag-invdiscrete>` flag now also disables midpoint integration, providing an
|
||||
|
||||
+21
-48
@@ -580,50 +580,24 @@ Solving for :math:`v_{t+h}`, we obtain the implicit-in-velocity update
|
||||
\widehat{M} &\equiv M-h D
|
||||
\end{aligned}
|
||||
|
||||
.. _geMidpoint:
|
||||
.. _geFreeBody:
|
||||
|
||||
Midpoint integration for free bodies in vacuum
|
||||
The implicit-in-velocity update :eq:`eq_implicit_update` treats the acceleration as a function of velocity and
|
||||
linearizes. While effective for damping-like forces, it is sub-optimal for rotational dynamics, where
|
||||
Coriolis and gyroscopic forces are *quadratic* in angular velocity. For this case, a better approach is to directly
|
||||
discretize the rotational equations of motion using the *midpoint method*.
|
||||
Gyroscopic derivatives for free bodies
|
||||
The ``implicitfast`` integrator described :ref:`below<geIntegrators>` excludes the derivatives of centripetal,
|
||||
Coriolis and gyroscopic forces from :math:`D`, so that :math:`\widehat M` remains symmetric and can be factorized
|
||||
with the faster Cholesky decomposition. However integrating gyroscopic forces explicitly can lead to
|
||||
energy gain and divergence of fast-spinning free bodies with asymmetric inertia.
|
||||
|
||||
Consider a rigid body rotating in its principal-axis frame with angular velocity
|
||||
:math:`\omega \in \mathbb{R}^3` and diagonal inertia tensor :math:`I = \text{diag}(I_1, I_2, I_3)`. The rotational
|
||||
dynamics are given by `Euler's rotation equation
|
||||
<https://en.wikipedia.org/wiki/Euler%27s_equations_(rigid_body_dynamics)>`__:
|
||||
Therefore for *standalone free bodies* (free joints whose body has no children), these derivatives are reinstated.
|
||||
The rows of :math:`\widehat M` corresponding to such a body form a :math:`6\times 6` block which is decoupled from
|
||||
the rest of the system. After the global Cholesky solve, this block is re-assembled with the exact derivative of the
|
||||
body's bias force and re-solved with an optimized :math:`6\times 6` LU routine. For standalone free bodies,
|
||||
``implicitfast`` and ``implicit`` therefore compute identical updates.
|
||||
|
||||
.. math::
|
||||
I \dot{\omega} + \omega \times I\omega = \tau
|
||||
|
||||
where :math:`\tau` is the external torque in the principal-axis frame.
|
||||
Evaluating the velocities at the midpoint, :math:`\omega_\text{mid} = (\omega_t + \omega_{t+h})/2`, gives:
|
||||
|
||||
.. math::
|
||||
\frac{2}{h} I (\omega_\text{mid} - \omega_t) + \omega_\text{mid} \times I \omega_\text{mid} = \tau
|
||||
|
||||
This is a system of 3 nonlinear equations in 3 unknowns :math:`\omega_\text{mid}`, solved at each timestep using
|
||||
Newton's method with a backtracking line search. After solving, the new velocity is recovered as
|
||||
:math:`\omega_{t+h} = 2\omega_\text{mid} - \omega_t`.
|
||||
|
||||
**Properties.** The midpoint method preserves all `quadratic first integrals
|
||||
<https://doi.org/10.1007/3-540-30666-8>`__ of the ODE. For Euler's equations, these are the
|
||||
kinetic energy :math:`H = \frac{1}{2}\omega^T I\omega` and the squared angular momentum
|
||||
:math:`C = \frac{1}{2}|I\omega|^2`, both conserved exactly in the absence of external torque. Since :math:`C` is the
|
||||
Casimir function of the `Lie-Poisson <https://en.wikipedia.org/wiki/Poisson_bracket>`__ structure, the midpoint
|
||||
method is a symmetric (time-reversible) and second-order accurate *Poisson integrator*.
|
||||
|
||||
**Eligibility.** Midpoint integration is only applied when using the ``implicitfast`` integrator, to
|
||||
free bodies with no child bodies, and only when the medium has zero :ref:`density<option-density>` and
|
||||
:ref:`viscosity<option-viscosity>`.
|
||||
|
||||
**Performance.** While the midpoint method carries computational overhead, we've found it to be
|
||||
negligible compared to the rest of the pipeline, on the order of 1% in the worst case.
|
||||
|
||||
**Disabling.** Because midpoint integration solves a nonlinear equation for the next velocity, it breaks the linear
|
||||
relationship between finite-differenced velocities and forces assumed by discrete inverse dynamics. Therefore,
|
||||
setting the :ref:`invdiscrete<option-flag-invdiscrete>` flag disables midpoint integration, and also provides a
|
||||
general opt-out mechanism for this integrator.
|
||||
**Properties.** The kinetic energy of a spinning free body is non-increasing in the absence of applied force.
|
||||
Steady spins about principal axes are conserved almost exactly; tumbling motion is mildly damped, at a rate
|
||||
scaling like :math:`(h|\omega|)^2` per step. Systems requiring long-horizon energy conservation of tumbling
|
||||
bodies should use the ``RK4`` integrator.
|
||||
|
||||
.. _geIntegrators:
|
||||
|
||||
@@ -661,8 +635,8 @@ Fast implicit-in-velocity (``implicitfast``)
|
||||
scenarios which are not common and already well-handled by the Runge-Kutta integrator (see below). Because the RNE
|
||||
derivatives are also the main source of asymmetry of :math:`D`, by dropping them and symmetrizing, we can use the
|
||||
faster :math:`L^TL` rather than :math:`LU` decomposition.
|
||||
The ``implicitfast`` integrator applies :ref:`midpoint integration<geMidpoint>` to eligible free bodies in vacuum,
|
||||
providing exact energy conservation for spinning objects at negligible additional cost.
|
||||
For standalone free bodies, the dropped :ref:`gyroscopic derivatives<geFreeBody>` are reinstated with a local
|
||||
unsymmetric solve, preventing energy gain of spinning bodies at negligible additional cost.
|
||||
|
||||
4th-order Runge-Kutta (``RK4``)
|
||||
One advantage of our continuous-time formulation is that we can use higher order integrators such as Runge-Kutta or
|
||||
@@ -696,11 +670,10 @@ Fast implicit-in-velocity (``implicitfast``)
|
||||
increased stability, and is therefore a strict improvement. It is the recommended integrator for most models.
|
||||
**implicit**:
|
||||
The benefit over ``implicitfast`` is the implicit integration of Coriolis and centripetal forces for *coupled*
|
||||
rotational systems such as multi-link pendula. Note that ``implicit`` does not apply :ref:`midpoint
|
||||
integration<geMidpoint>` (only ``implicitfast`` does), but its RNE derivatives provide comparable stability
|
||||
for free-body rotation. For example, `gyroscopic.xml <../_static/gyroscopic.xml>`__ shows an ellipsoid rolling
|
||||
on an inclined plane; both ``implicitfast`` and ``implicit`` handle this case well, while ``Euler`` quickly
|
||||
diverges.
|
||||
rotational systems such as multi-link pendula. For standalone free bodies the two integrators coincide, since
|
||||
``implicitfast`` applies the :ref:`gyroscopic derivatives<geFreeBody>` to such bodies. For example,
|
||||
`gyroscopic.xml <../_static/gyroscopic.xml>`__ shows an ellipsoid rolling on an inclined plane; both
|
||||
``implicitfast`` and ``implicit`` handle this case well, while ``Euler`` quickly diverges.
|
||||
**RK4**:
|
||||
This integrator is best for systems which are energy conserving, or almost energy-conserving. `pendulum.xml
|
||||
<../_static/pendulum.xml>`__ shows a complicated pendulum mechanism which diverges quickly using ``Euler`` or
|
||||
|
||||
Reference in New Issue
Block a user