chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

New implicitfast integrator and sparse RNE derivatives for implicit.

Browse Source 
PiperOrigin-RevId: 516910733
Change-Id: I29a0465c0f0b1749a73e3d7e01925200d025ddd0
This commit is contained in:
Yuval Tassa
2023-03-15 13:16:45 -07:00
committed by Copybara-Service
parent 056e849273
commit 8c7f6ce5a0
23 changed files with 961 additions and 332 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+25 -3
View File
@@ -8,17 +8,39 @@ Upcoming version (not yet released)
General
^^^^^^^
- Improvements to implicit integration:
- The derivatives of the RNE algorithm are now computed using sparse math, leading to significant speed
improvements for large models when using the :ref:`implicit integrator<geIntegration>`.
- A new integrator called ``implicitfast`` was added. It is similar to the existing implicit integrator, but skips the
derivatives of Coriolis and centripetal forces. See the :ref:`numerical integration<geIntegration>` section for a
detailed motivation and discussion. The implicitfast integrator is recommended for all new models and will
become the default integrator in a future version.
The table below shows the compute cost of the 627-DoF `humanoid100
<https://github.com/deepmind/mujoco/blob/main/model/humanoid100/humanoid100.xml>`_ model using different integrators.
"implicit (old)" uses dense RNE derivatives, "implicit (new)" is after the sparsification mentioned above.
Timings were measured on a single core of an AMD 3995WX CPU.
.. csv-table::
:header: "timing", "Euler", "implicitfast", "implicit (new)", "implicit (old)"
:widths: auto
:align: left
one step (ms), 0.5, 0.53, 0.77, 5.0
steps/second, 2000, 1900, 1300, 200
- The ``mjd_transitionFD`` function no longer triggers sensor calculation unless explicitly requested.
- Corrected the spelling of the ``inteval`` attribute to ``interval`` in the :ref:`mjLROpt` struct.
- Mesh texture and normal mappings are now 3-per-triangle rather than 1-per-vertex. Mesh vertices are no longer
duplicated in order to circumvent this limitation as they previously were.
- The non-zeros for the sparse constraint Jacobian matrix are now precounted and used for matrix memory allocation.
For instance, the constraint Jacobian matrix from the `humanoid100.xml
For instance, the constraint Jacobian matrix from the `humanoid100
<https://github.com/deepmind/mujoco/blob/main/model/humanoid100/humanoid100.xml>`_ model, which previously required
~500,000 ``mjtNum``'s, now only requires ~6000. Very large models can now load and run with the CG solver.
- Modified :ref:`mju_error` and :ref:`mju_warning` to be variadic functions (support for printf-like arguments). The
functions :ref:`mju_error_i`, :ref:`mju_error_s`, :ref:`mju_warning_i`, and :ref:`mju_warning_s` are now deprecated.
- Implemented a performant :ref:`mju_sqrMatTDSparse` function that doesn't require dense memory allocation.
- Implemented a performant ``mju_sqrMatTDSparse`` function that doesn't require dense memory allocation.
Python bindings
@@ -28,7 +50,7 @@ Python bindings
continuation of an IPython interactive shell session, and is no longer considered experimental feature.
- Remove ``efc_`` fields from joint indexers. Since the introduction of arena memory, these fields now have dynamic
sizes that change between time steps depending on the number of active constraints, breaking strict correspondence
between joints and `efc_` rows.
between joints and ``efc_`` rows.
Simulate
^^^^^^^^
doc/computation.rst
+102 -89
View File
@@ -421,118 +421,131 @@ Numerical integration
MuJoCo computes forward and inverse dynamics in continuous time. The end result of forward dynamics is the joint
acceleration :math:`a=\dot{v}` as well as the actuator activations :math:`\dot{w}` when present in the model. These are
used to advance the simulation time from :math:`t` to :math:`t+h`, and to update the state variables :math:`q, v, w`.
Three numerical integrators are available:
Semi-implicit Euler method (Euler)
This method updates the activation and velocity with the usual Euler method, however the position is updated using
the *new* velocity; this is known as a "semi-implicit" update:
Four numerical integrators are available, three single-step integrators and the multi-step 4th order Runge-Kutta
integrator. Before describing the integrators, we begin with a general description of single-step Euler integrators:
*explicit*, *semi-implicit* and *implicit-in-velocity*. The *explicit* Euler method is not supported by MuJoCo but has
pedagogical value. It can be written as:
.. math::
\begin{aligned}
\textrm{activation: }w_{t+h} &= w_t + h \dot{w}_t \\
\textrm{velocity: }v_{t+h} &= v_t + h a_t \\
\textrm{position: }q_{t+h} &= q_t + h v_{t+h}
\end{aligned}
.. math::
:label: eq_explicit
Using the new velocity in the position update improves stability, and is standard in physics simulation. The
summation in the position update generally involves vectors with different dimensionality, and is done by taking into
account the properties of quaternions.
\begin{aligned}
\textrm{activation:}\quad w_{t+h} &= w_t + h \dot{w}_t \\
\textrm{velocity:}\quad v_{t+h} &= v_t + h a_t \\
\textrm{position:}\quad q_{t+h} &= q_t + h v_t
\end{aligned}
When joint damping is defined in the model, the Euler method applies a correction to the inertia matrix that
corresponds to implicit integration of damping forces. Let :math:`B` be the diagonal matrix of negative joint damping
coefficients. Letting :math:`\widehat{M} = M-h B`, we then update the velocity using a corrected acceleration as
follows:
Note that in the presence of quaternions, the operation :math:`q_t + h v_t` is more involved than a simple summation, as
the dimensionalities of :math:`q` and :math:`v` are different. The reason explicit Euler is not implemented is that the
following formulation, known as *semi-implicit* Euler is `strictly better <https://en.wikipedia.org/wiki/Semi-
implicit_Euler_method>`_, and standard in physics simulation:
.. math::
\begin{aligned}
v_{t+h} &= v_t + h \widehat{M}^{-1} M a_t
\end{aligned}
.. math::
:label: eq_semimplicit
This correction is temporary, and the original acceleration computed by forward dynamics and saved in ``mjData.qacc``
is not modified. We explain below how this correction is derived, and why it is a special case of implicit
integration.
\begin{aligned}
v_{t+h} &= v_t + h a_t \\
q_{t+h} &= q_t + h v_{\color{red}t+h}
\end{aligned}
Implicit-in-velocity Euler method (implicit)
This method approximates the following discrete-time update:
Comparing :eq:`eq_explicit` and :eq:`eq_semimplicit`, we see that in semi-implicit Euler, the position is updated using
the *new* velocity. *Implicit* Euler means:
.. math::
\begin{aligned}
w_{t+h} &= w_t + h \dot{w}_t \\
v_{t+h} &= v_t + h a_{t+h} \\
q_{t+h} &= q_t + h v_{t+h}
\end{aligned}
.. math::
:label: eq_implicit
Note the acceleration :math:`a_{t+h}=\dot{v}_{t+h}` on the right hand side of the velocity update is evaluated at
:math:`t+h`, making the integrator fully implicit in velocity. When applied to Hamiltonian systems, such integrators
are called symplectic, and there is a large mathematical literature on them. Hamiltonian systems have the property
that certain quantities (symplectic forms) remain constant over time in the true continuous-time system. Symplectic
integrators have the unique property that they preserve the same quantities exactly, despite using a discrete-time
approximation. Few MuJoCo models used in practice correspond to Hamiltonian systems; these are systems without
activation dynamics, contacts, friction, driving forces etc. Nevertheless this type of integrator has appealing
properties in terms of accuracy and stability, achieved at the expense of added computation. It is particularly
effective in systems where instabilities (of the regular Euler integrator) are caused by velocity-dependent forces:
multi-joint pendulums, bodies tumbling through space, systems with substantial lift and drag forces, systems with
substantial damping in tendons and actuators (as well as joints, but joint damping is already handled implicitly in
the regular Euler integrator).
\begin{aligned}
v_{t+h} &= v_t + h a_{\color{red}t+h} \\
q_{t+h} &= q_t + h v_{t+h}
\end{aligned}
Writing the acceleration (i.e. the forward dynamics) more explicitly as a function of velocity: :math:`a_t=\dot
{v}_t = a(v_t)`, the velocity update we aim to approximate is
Comparing :eq:`eq_semimplicit` and :eq:`eq_implicit`, we see that the acceleration :math:`a_{t+h}=\dot{v}_{t+h}` on the
right hand side of the velocity update is evaluated at the *next time step*. While evaluating the next acceleration
is not possible without stepping, we can use a first-order Taylor expansion to approximate this quantity, and
take a single step of Newton's method. When the expansion is only with respect to velocity (and not position), the
integrator is known as *implicit-in-velocity* Euler. This approach is particularly effective in systems where
instabilities are caused by velocity-dependent forces: multi-joint pendulums, bodies tumbling through space, systems
with lift and drag forces, and systems with substantial damping in tendons and actuators. Writing the
acceleration as a function of velocity: :math:`a_t = a(v_t)`, the velocity update we aim to approximate is
.. math:: v_{t+h} = v_t + h a(v_{t+h})
.. math:: v_{t+h} = v_t + h a(v_{t+h})
This is a non-linear equation in the unknown vector :math:`v_{t+h}`. It must be solved numerically at each time step
in order to implement an implicit integrator. We approximate the solution using a single step of Newton's method,
based on the first-order Taylor series expansion of :math:`a(v_{t+h})` around :math:`v_t`. Recall that the forward
dynamics are
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
.. math:: a(v) = M^{-1} \big(\tau(v) - c(v) + J^T f(v)\big)
.. math:: a(v) = M^{-1} \big(\tau(v) - c(v) + J^T f(v)\big)
Here we will ignore the velocity dependence of the constraint forces :math:`J^T f(v)`, because differentiating them
is complicated, and furthermore MuJoCo uses soft constraints whose integration is very stable even without implicit
integration. Thus we define the approximate derivative
Thus we define the derivative
.. math::
\begin{aligned}
{\partial a(v) \over \partial v} &\approx M^{-1} D(v) \\
D(v) &= {\partial\big(\tau(v) - c (v)\big) \over \partial v}
\end{aligned}
.. math::
\begin{aligned}
{\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)
\end{aligned}
MuJoCo computes :math:`D(v)` analytically, by differentating the Recursive Newton-Euler algorithm as well as the code
that computes applied and bias forces. A further approximation is that we restrict :math:`D` to have the same
sparsity pattern as :math:`M`, for computational efficiency (see below). This restriction will exclude damping in
tendons which connect bodies that are on different branches of the kinematic tree. 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::
\begin{aligned}
v_{t+h} &= v_t + h a(v_{t+h}) \\
&= v_t + h a(v_t + v_{t+h}-v_t) \\
&\approx v_t + h a(v_t) + h M^{-1} D \cdot (v_{t+h}-v_t)
\end{aligned}
.. math::
\begin{aligned}
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) \\
&= v_t + h a(v_t) + h M^{-1} D \cdot (v_{t+h}-v_t)
\end{aligned}
Premultiplying by :math:`M` and rearranging yields
Premultiplying by :math:`M` and rearranging yields
.. math:: (M-h D) v_{t+h} = (M-h D) v_t + h M a(v_t)
.. math:: (M-h D) v_{t+h} = (M-h D) v_t + h M a(v_t)
Now letting :math:`\widehat{M} = M-h D`, we obtain the implicit update
Solving for :math:`v_{t+h}`, we obtain the implicit-in-velocity update
.. math:: v_{t+h} = v_t + h \widehat{M}^{-1} M a(v_t)
.. math::
:label: eq_implicit_update
Comparing to the regular Euler method with damping correction, we see that the matrix :math:`B` in the regular Euler
method corresponds to :math:`D` used here, but restricted to joint damping. Since :math:`D` and :math:`M` have the
same sparsity pattern corresponding to the topology of the kinematic tree, reverse-order LU factorization of
:math:`\widehat{M}` is guaranteed to have no fill-in, which is the computational speed-up mentioned above. This
factorization is stored ``mjData.qLU``. It is now also clear why the Euler method uses :math:`B` rather than
:math:`D`: since :math:`B` is diagonal, :math:`\widehat{M}` remains symmetric and can be factorized with Cholesky
rather than LU.
v_{t+h} = v_t + h (M-h D)^{-1} M a(v_t)
4th-order Runge-Kutta method (RK4)
One advantage of our continuous-time formulation is that we can use more advanced integrators such as Runge-Kutta or
multistep methods. The only such integrator currently implemented is the fixed-step 4th-order Runge-Kutta method. We
have observed that for energy-conserving systems it is qualitatively better than the Euler method, both in terms of
stability and accuracy, even when the timestep of the Euler method is decreased by a factor of 4 (so the
computational effort is identical). In the presence of contacts we have not observed significant benefits, although a
more systematic investigation remains to be performed.
All three single-step integrators in MuJoCo use the update :eq:`eq_implicit_update`, with different definitions of the
:math:`D` matrix, which is computed analytically.
Semi-implicit Euler 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
and :math:`M-h D` is symmetric, so Cholesky decomposition can be used.
Implicit-in-velocity Euler (``implicit``)
For this method :math:`D` includes derivatives of all forces except the constraint forces :math:`J^T f(v)`. These are
currently ignored since even though computing them is possible, it is complicated, and numerical tests show that
including them does not confer much benefit. That said, analytical derivatives of constraint forces are planned for a
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
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
factorization of :math:`M-h D` 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``.
Fast implicit-in-velocity (``implicitfast``)
For this method :math:`D` includes derivatives of all forces used in the implicit method, with the exception of the
centripetal and Coriolis forces :math:`c (v)` computed by the RNE algorithm. Additionally, it is symmetrized :math:`D
\leftarrow (D + D^T)/2`. One reason for dropping the RNE derivatives is that they are the most expensive to compute.
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
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.
.. tip::
The implicitfast integrator has similar computational cost to Euler, yet provides increased stability, and is
therefore a strict improvement. It is the recommended integrator and will become the default in a future version.
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
multistep methods. The only such integrator currently implemented is the fixed-step `4th-order Runge-Kutta method
<https://en.wikipedia.org/wiki/Runge–Kutta_methods#Derivation_of_the_Runge–Kutta_fourth-order_method>`_, though users
can easily implement other integrators by calling :ref:`mj_forward` and integrating accelerations themselves. We have
observed that for energy-conserving systems (`example
<https://github.com/deepmind/mujoco/blob/main/test/engine/testdata/derivative/energy_conserving_pendulum.xml>`_) RK4
is qualitatively better than the single-step methods, both in terms of stability and accuracy, even when the timestep
is decreased by a factor of 4 (so the computational effort is identical). In the presence of large velocity-
dependent forces, if the chosen single-step method integrates those forces implicitly, single-step methods can be
significantly more stable than RK4.
.. note::
The accuracy and stability of all integrators can be improved by reducing the time step :math:`h` which is stored in
doc/includes/references.h
+9 -4
View File
@@ -245,14 +245,17 @@ struct mjData_ {
mjtNum* subtree_linvel; // linear velocity of subtree com (nbody x 3)
mjtNum* subtree_angmom; // angular momentum about subtree com (nbody x 3)
// computed by mj_Euler
// computed by mj_Euler or mj_implicit
mjtNum* qH; // L'*D*L factorization of modified M (nM x 1)
mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1)
// computed by mj_implicit
// computed by mj_resetData
int* D_rownnz; // non-zeros in each row (nv x 1)
int* D_rowadr; // address of each row in D_colind (nv x 1)
int* D_colind; // column indices of non-zeros (nD x 1)
int* B_rownnz; // non-zeros in each row (nbody x 1)
int* B_rowadr; // address of each row in B_colind (nbody x 1)
int* B_colind; // column indices of non-zeros (nB x 1)
// computed by mj_implicit/mj_derivative
mjtNum* qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1)
@@ -391,7 +394,8 @@ typedef enum mjtTexture_ { // type of texture
typedef enum mjtIntegrator_ { // integrator mode
mjINT_EULER = 0, // semi-implicit Euler
mjINT_RK4, // 4th-order Runge Kutta
mjINT_IMPLICIT // implicit in velocity
mjINT_IMPLICIT, // implicit in velocity
mjINT_IMPLICITFAST // implicit in velocity, no rne derivative
} mjtIntegrator;
typedef enum mjtCollision_ { // collision mode for selecting geom pairs
mjCOL_ALL = 0, // test precomputed and dynamic pairs
@@ -791,7 +795,8 @@ struct mjModel_ {
// sizes set after mjModel construction (only affect mjData)
int nM; // number of non-zeros in sparse inertia matrix
int nD; // number of non-zeros in sparse derivative matrix
int nD; // number of non-zeros in sparse dof-dof matrix
int nB; // number of non-zeros in sparse body-dof matrix
int nemax; // number of potential equality-constraint rows
int njmax; // number of available rows in constraint Jacobian
int nconmax; // number of potential contacts in contact list