Improve integrator documentation.

PiperOrigin-RevId: 729710349
Change-Id: I8bf1727b8cd3ea45bb16e3607c9d8f58e4a69007
This commit is contained in:
Yuval Tassa
2025-02-21 17:28:57 -08:00
committed by Copybara-Service
parent f554b38c36
commit b90ff3f0b1
+30 -16
View File
@@ -484,24 +484,27 @@ acceleration as a function of velocity: :math:`a_t = a(v_t)`, the velocity updat
This is a non-linear equation in the unknown vector :math:`v_{t+h}` and can be solved numerically at each time step This is a non-linear equation in the unknown vector :math:`v_{t+h}` and can be solved numerically at each time step
using a first-order expansion of :math:`a(v_{t+h})` around :math:`v_t`. Recall that the forward dynamics are using a first-order expansion of :math:`a(v_{t+h})` around :math:`v_t`. Recall that the forward dynamics are
.. math:: a(v) = M^{-1} \big(\tau(v) - c(v) + J^T f(v)\big) .. math::
:label: eq_forward
a(v) = M^{-1} \big(\tau(v) - c(v) + J^T f(v)\big)
Thus we define the derivative Thus we define the derivative
.. math:: .. math::
\begin{aligned} \begin{aligned}
{\partial a(v) \over \partial v} &= M^{-1} D \\ {\partial a(v) \over \partial v} &= M^{-1} D \\
D &\equiv {\partial \over \partial v} \Big(\tau(v) - c (v) + J^T f(v)\Big) D &\equiv {\partial \over \partial v} \Big(\tau(v) - c (v) + J^T f(v)\Big)
\end{aligned} \end{aligned}
The velocity update corresponding to Newton's method is as follows. First, we expand the right hand side to first order The velocity update corresponding to Newton's method is as follows. First, we expand the right hand side to first order
.. math:: .. math::
\begin{aligned} \begin{aligned}
v_{t+h} &= v_t + h a(v_{t+h}) \\ v_{t+h} &= v_t + h a(v_{t+h}) \\
&\approx v_t + h \big( a(v_t) + {\partial a(v) \over \partial v} \cdot (v_{t+h}-v_t) \big) \\ &\approx v_t + h \big( a(v_t) + {\partial a(v) \over \partial v} \cdot (v_{t+h}-v_t) \big) \\
&= v_t + h a(v_t) + h M^{-1} D \cdot (v_{t+h}-v_t) &= v_t + h a(v_t) + h M^{-1} D \cdot (v_{t+h}-v_t)
\end{aligned} \end{aligned}
Premultiplying by :math:`M` and rearranging yields Premultiplying by :math:`M` and rearranging yields
@@ -512,16 +515,27 @@ Solving for :math:`v_{t+h}`, we obtain the implicit-in-velocity update
.. math:: .. math::
:label: eq_implicit_update :label: eq_implicit_update
v_{t+h} = v_t + h (M-h D)^{-1} M a(v_t) \begin{aligned}
v_{t+h} &= v_t + h \widehat{M}^{-1} M a(v_t) \\
\widehat{M} &\equiv M-h D
\end{aligned}
.. _geIntergrators:
Intergrators
^^^^^^^^^^^^
MuJoCo supports four integrators: three single-step integrators and the multi-step 4th order Runge-Kutta integrator.
All three single-step integrators in MuJoCo use the update :eq:`eq_implicit_update`, with different definitions of the All three single-step integrators in MuJoCo use the update :eq:`eq_implicit_update`, with different definitions of the
:math:`D` matrix, which is always computed analytically. :math:`D` matrix, which is always computed analytically.
Semi-implicit with implicit joint damping (``Euler``) Semi-implicit with implicit joint damping (``Euler``)
For this method, :math:`D` only includes derivatives of joint damping. Note that in this case :math:`D` is diagonal For this method, :math:`D` only includes derivatives of joint damping. Note that in this case :math:`D` is diagonal
and :math:`M-h D` is symmetric, so Cholesky decomposition can be used. If the model has no joint damping or the and :math:`\widehat{M}` is symmetric, so :math:`L^TL` decomposition (a variant of Cholesky) can be used. This
factorization is stored ``mjData.qLD``. If the model has no joint damping or the
:ref:`eulerdamp<option-flag-eulerdamp>` disable-flag is set, implicit damping is disabled and the semi-implicit :ref:`eulerdamp<option-flag-eulerdamp>` disable-flag is set, implicit damping is disabled and the semi-implicit
update :eq:`eq_semimplicit` is used, rather than :eq:`eq_implicit_update`. update :eq:`eq_semimplicit` is used, rather than :eq:`eq_implicit_update`, avoiding the additional factorization of
:math:`\widehat{M}` (*additional* because :math:`M` is already factorized for the acceleration update
:eq:`eq_forward`).
Implicit-in-velocity (``implicit``) Implicit-in-velocity (``implicit``)
For this method, :math:`D` includes derivatives of all forces except the constraint forces :math:`J^T f(v)`. These For this method, :math:`D` includes derivatives of all forces except the constraint forces :math:`J^T f(v)`. These
@@ -530,8 +544,8 @@ Implicit-in-velocity (``implicit``)
future version. Additionally, we restrict :math:`D` to have the same sparsity pattern as :math:`M`, for computational future version. Additionally, we restrict :math:`D` to have the same sparsity pattern as :math:`M`, for computational
efficiency. This restriction will exclude damping in tendons which connect bodies that are on different branches of efficiency. This restriction will exclude damping in tendons which connect bodies that are on different branches of
the kinematic tree. Since :math:`D` is not symmetric, we cannot use Cholesky factorization, but because :math:`D` and the kinematic tree. Since :math:`D` is not symmetric, we cannot use Cholesky factorization, but because :math:`D` and
:math:`M` have the same sparsity pattern corresponding to the topology of the kinematic tree, reverse-order LU :math:`M` have the same sparsity pattern corresponding to the topology of the kinematic tree, reverse-order
factorization of :math:`M-h D` is `guaranteed to have no fill-in :math:`LU` factorization of :math:`\widehat{M}` is guaranteed to have `no fill-in
<https://link.springer.com/book/10.1007/978-1-4899-7560-7>`_. This factorization is stored ``mjData.qLU``. <https://link.springer.com/book/10.1007/978-1-4899-7560-7>`_. This factorization is stored ``mjData.qLU``.
Fast implicit-in-velocity (``implicitfast``) Fast implicit-in-velocity (``implicitfast``)
@@ -541,7 +555,7 @@ Fast implicit-in-velocity (``implicitfast``)
Second, these forces change rapidly only at high rotational velocities of complex pendula and spinning bodies, Second, these forces change rapidly only at high rotational velocities of complex pendula and spinning bodies,
scenarios which are not common and already well-handled by the Runge-Kutta integrator (see below). Because the RNE 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 derivatives are also the main source of asymmetry of :math:`D`, by dropping them and symmetrizing, we can use the
faster Cholesky rather than LU decomposition. faster :math:`L^TL` rather than :math:`LU` decomposition.
4th-order Runge-Kutta (``RK4``) 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 One advantage of our continuous-time formulation is that we can use higher order integrators such as Runge-Kutta or