Add double underscore suffix to all links in the changelog.

PiperOrigin-RevId: 571304907
Change-Id: Ie726f3d703421f9d13a5d16f2282123c71c2b530

doc/APIreference/functions.rst

@@ -3446,6 +3446,15 @@ mju_threadPoolCreate
 
 Create a thread pool with the specified number of threads running.
 
+.. _mju_bindThreadPool:
+
+mju_bindThreadPool
+~~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mju_bindThreadPool
+
+Adds a thread pool to :ref:`mjData` and configures it for multi-threaded use.
+
 .. _mju_threadPoolEnqueue:
 
 mju_threadPoolEnqueue

doc/XMLreference.rst

@@ -3854,8 +3854,8 @@ geoms volumes of either body. This constraint can be used to define ball joints
 
 :at:`active`: :at-val:`[false, true], "true"`
    If this attribute is set to "true", the constraint is active and the constraint solver will try to enforce it. The
-   corresponding field in mjModel is mjData.eq_active. This field can be used at runtime to turn specific constraints on
-   an off.
+   field :ref:`mjModel.eq_active0<mjModel>` corresponds to this value, and is used to initialize
+   :ref:`mjData.eq_active<mjData>`, which is user-settable at runtime.
 
 .. _equality-connect-solref:
 
@@ -3920,7 +3920,8 @@ of the other body, without any joint elements in the child body.
 
 :at:`body2`: :at-val:`string, optional`
    Name of the second body. If this attribute is omitted, the second body is the world body. Welding a body to the world
-   and changing the corresponding component of mjModel.eq_active at runtime can be used to fix the body temporarily.
+   and changing the corresponding component of :ref:`mjData.eq_active<mjData>` at runtime can be used to fix the body
+   temporarily.
 
 .. _equality-weld-relpose:
 
@@ -6751,8 +6752,6 @@ if omitted.
 
 .. _default-camera-user:
 
-.. _default-camera-resolution:
-
 .. _default-camera-focal:
 
 .. _default-camera-focalpixel:

doc/changelog.rst

@@ -37,7 +37,7 @@ New features
 
 4. Added capability to initialize :ref:`composite<body-composite>` particles with arbitrary positions.
 
-5. Added `shell <https://github.com/deepmind/mujoco/blob/main/plugin/elasticity/shell.cc>`_ passive force plugin:
+5. Added `shell <https://github.com/deepmind/mujoco/blob/main/plugin/elasticity/shell.cc>`__ passive force plugin:
 
    - Collisions use spheres located at mesh vertices.
    - Stretching as tendon constraints and bending using a constant precomputed Hessian (cotangent operator).
@@ -88,25 +88,32 @@ General
 
        **Migration:** Users should use the :at:`cable` and :at:`shell` elasticity plugins.
 
-12. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
+   12. Added :ref:`mjData.eq_active<mjData>` user input variable, for enabling/disabling the state of equality
+       constraints. Renamed ``mjModel.eq_active`` to :ref:`mjModel.eq_active0<mjModel>`, which now has the semantic of
+       "initial value of ``mjData.eq_active``".
+       Fixes `#876 <https://github.com/google-deepmind/mujoco/discussions/876>`__
+
+       **Migration:** Replace uses of ``mjModel.eq_active`` with ``mjData.eq_active``.
+
+13. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
     the exact formula rather than with Euler integration.
-13. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
+14. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
     actuator forces: using the next step's actuator state to compute the current actuator forces at the current timestep.
-14. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
+15. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
     :ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
     :ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
-15. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
+16. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
     Euler integrator. See the :ref:`Numerical Integration<geIntegration>` section for more details.
-16. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
+17. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
     :ref:`integrators<option-integrator>` other than ``RK4``. See the flag documentation for more details.
-17. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
+18. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
     linesearch stopping criteria in CG and Newton solvers. These can be useful for performance tuning.
-18. Added ``mesh_pos`` and ``mesh_quat`` fields to :ref:`mjModel` to store the normalizing transformation applied to
+19. Added ``mesh_pos`` and ``mesh_quat`` fields to :ref:`mjModel` to store the normalizing transformation applied to
     mesh assets. Fixes `#409 <https://github.com/google-deepmind/mujoco/issues/409>`__ .
-19. Added camera :ref:`resolution<body-camera-resolution>` attribute and :ref:`camprojection<sensor-camprojection>`
+20. Added camera :ref:`resolution<body-camera-resolution>` attribute and :ref:`camprojection<sensor-camprojection>`
     sensor. If camera resolution is set to positive values, the camera projection sensor will report the location of a
     target site, projected onto the camera image, in pixel coordinates.
-20. Added :ref:`camera<body-camera>` calibration attributes:
+21. Added :ref:`camera<body-camera>` calibration attributes:
 
     - The new attributes are :ref:`resolution<body-camera-resolution>`, :ref:`focal<body-camera-focal>`,
       :ref:`focalpixel<body-camera-focalpixel>`, :ref:`principal<body-camera-principal>`,
@@ -115,20 +122,36 @@ General
       attributes are specified. See the following
       `example model <https://github.com/deepmind/mujoco/blob/main/test/engine/testdata/vis_visualize/frustum.xml>`__.
     - Note that these attributes only take effect for offline rendering and do not affect interactive visualisation.
-
+22. Added multi-threaded constraint solving via :ref:`mj_island` and :ref:`mjThreadPool` to :ref:`testspeed<saTestspeed>`
+    exposed via npoolthread flag. The `22 humanoids <https://github.com/deepmind/mujoco/blob/main/model/humanoid/22_humanoids.xml>`__
+    model shows a 3x speedup compared to the single threaded simulation.
 Python bindings
 ^^^^^^^^^^^^^^^
 
-21. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
+23. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
     camera name used the default camera.
 
+Simulate
+^^^^^^^^
+
+.. youtube:: YSvWn_poqWs
+   :align: right
+   :width: 240px
+
+24. Added **state history** mechanism to :ref:`simulate<saSimulate>` and the managed
+    :ref:`Python viewer<PyViewerManaged>`. State history can be viewed by scrubbing the History slider and (more
+    precisely) with the left and right arrow keys. See screen capture:
+
+25. The ``LOADING...`` label is now shown correctly.
+    `Contribution <https://github.com/google-deepmind/mujoco/pull/1070>`__ by
+    `Levi Burner <https://github.com/aftersomemath>`__.
+
 Bug fixes
 ^^^^^^^^^
 
-22. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
+26. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
     midphase collision trees.
 
-
 Version 2.3.7 (July 20, 2023)
 -----------------------------
 
@@ -206,9 +229,9 @@ Bug fixes
 Plugins
 ^^^^^^^
 
-5. Added touch-grid sensor plugin. See `documentation <https://github.com/google-deepmind/mujoco/blob/main/plugin/sensor/README.md>`_
-   for details, and associated `touch_grid.xml <https://github.com/google-deepmind/mujoco/blob/main/model/plugin/sensor/touch_grid.xml>`_
-   example model. The plugin includes `in-scene visualisation <https://youtu.be/0LOJ3WMnqeA>`_.
+5. Added touch-grid sensor plugin. See `documentation <https://github.com/google-deepmind/mujoco/blob/main/plugin/sensor/README.md>`__
+   for details, and associated `touch_grid.xml <https://github.com/google-deepmind/mujoco/blob/main/model/plugin/sensor/touch_grid.xml>`__
+   example model. The plugin includes `in-scene visualisation <https://youtu.be/0LOJ3WMnqeA>`__.
 
 Simulate
 ^^^^^^^^
@@ -331,8 +354,8 @@ Python bindings
 #. Fixed a race condition in ``viewer.launch_passive`` and  ``viewer.launch_repl``. These functions could previously
    return before an internal call to ``mj_forward``. This allows user code to continue and potentially modify physics
    state concurrently with the internal ``mj_forward``, resulting in e.g.
-   `MuJoCo stack overflow error <https://github.com/google-deepmind/mujoco/issues/783>`_
-   or `segmentation fault <https://github.com/google-deepmind/mujoco/issues/790>`_.
+   `MuJoCo stack overflow error <https://github.com/google-deepmind/mujoco/issues/783>`__
+   or `segmentation fault <https://github.com/google-deepmind/mujoco/issues/790>`__.
 #. The ``viewer.launch_passive`` function now returns a handle which can be used to interact with the viewer. The
    passive viewer now also requires an explicit call to ``sync`` on its handle to pick up any update to the physics
    state. This is to avoid race conditions that can result in visual artifacts. See
@@ -345,9 +368,9 @@ Bug fixes
 
 12. Fixed bug in the handling of ellipsoid-based fluid model forces in the new implicitfast integrator.
 #.  Removed spurious whole-arena copying in `mj_copyData`, which can considerably
-    `slow down <https://github.com/google-deepmind/mujoco/issues/568>`_ the copying operation.
+    `slow down <https://github.com/google-deepmind/mujoco/issues/568>`__ the copying operation.
 #.  Make :ref:`shellinertia<body-geom-shellinertia>` ignore :ref:`exactmeshinertia<compiler-exactmeshinertia>`, which is
-    only used for legacy volume computations (`#759 <https://github.com/google-deepmind/mujoco/issues/759>`_).
+    only used for legacy volume computations (`#759 <https://github.com/google-deepmind/mujoco/issues/759>`__).
 
 
 Version 2.3.3 (March 20, 2023)
@@ -366,7 +389,7 @@ General
      become the default integrator in a future version.
 
    The table below shows the compute cost of the 627-DoF `humanoid100
-   <https://github.com/google-deepmind/mujoco/blob/main/model/humanoid100/humanoid100.xml>`_ model using different
+   <https://github.com/google-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.
 
@@ -384,14 +407,14 @@ General
 
 2. Added a collision mid-phase for pruning geoms in body pairs, see :ref:`documentation<coSelection>` for more details.
    This is based on static AABB bounding volume hierarchy (a BVH binary tree) in the body inertial frame. The GIF on
-   the right is cut from `this longer video <https://youtu.be/e0babIM8hBo>`_.
+   the right is cut from `this longer video <https://youtu.be/e0babIM8hBo>`__.
 #. 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
-   <https://github.com/google-deepmind/mujoco/blob/main/model/humanoid100/humanoid100.xml>`_ model, which previously
+   <https://github.com/google-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.
@@ -418,7 +441,7 @@ Python bindings
 Simulate
 ^^^^^^^^
 
-15. Implemented a workaround for `broken VSync <https://github.com/glfw/glfw/issues/2249>`_ on macOS so that the frame
+15. Implemented a workaround for `broken VSync <https://github.com/glfw/glfw/issues/2249>`__ on macOS so that the frame
     rate is correctly capped when the Vertical Sync toggle is enabled.
 
 .. image:: images/changelog/contactlabel.png
@@ -439,7 +462,7 @@ General
 
 1. A more performant mju_transposeSparse has been implemented that doesn't require dense memory allocation.
    For a constraint Jacobian matrix from the
-   `humanoid100.xml <https://github.com/google-deepmind/mujoco/blob/main/model/humanoid100/humanoid100.xml>`_ model,
+   `humanoid100.xml <https://github.com/google-deepmind/mujoco/blob/main/model/humanoid100/humanoid100.xml>`__ model,
    this function is 35% faster.
 #. The function :ref:`mj_name2id` is now implemented using a hash function instead of a linear search for better
    performance.
@@ -489,18 +512,18 @@ Python bindings
 ^^^^^^^^^^^^^^^
 
 1. The ``simulate`` GUI is now available through the ``mujoco`` Python package as ``mujoco.viewer``.
-   See :ref:`documentation<PyViewer>` for details. (Contribution by `Levi Burner <https://github.com/aftersomemath>`_.)
+   See :ref:`documentation<PyViewer>` for details. (Contribution by `Levi Burner <https://github.com/aftersomemath>`__.)
 #. The ``Renderer`` class from the MuJoCo tutorial Colab is now available directly in the native Python bindings.
 
 General
 ^^^^^^^
 
 3. The tendon :at:`springlength` attribute can now take two values. Given two non-decreasing values, `springlength`
-   specifies a `deadband  <https://en.wikipedia.org/wiki/Deadband>`_ range for spring stiffness. If the tendon length is
+   specifies a `deadband  <https://en.wikipedia.org/wiki/Deadband>`__ range for spring stiffness. If the tendon length is
    between the two values, the force is 0. If length is outside this range, the force behaves like a regular spring, with
    the spring resting length corresponding to the nearest :at:`springlength` value. This can be used to create tendons
    whose limits are enforced by springs rather than constraints, which are cheaper and easier to analyse. See
-   `tendon_springlength.xml <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/tendon_springlength.xml>`_
+   `tendon_springlength.xml <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/tendon_springlength.xml>`__
    example model.
 
    .. attention::
@@ -514,22 +537,22 @@ General
 #. Removed the requirement that stateless actuators come before stateful actuators.
 #. Added :ref:`mju_fill`, :ref:`mju_symmetrize` and :ref:`mju_eye` utility functions.
 #. Added :at:`gravcomp` attribute to :ref:`body<body>`, implementing gravity compensation and buoyancy.
-   See `balloons.xml <https://github.com/google-deepmind/mujoco/blob/main/model/balloons/balloons.xml>`_ example model.
+   See `balloons.xml <https://github.com/google-deepmind/mujoco/blob/main/model/balloons/balloons.xml>`__ example model.
 #. Renamed the ``cable`` plugin library to ``elasticity``.
 #. Added :at:`actdim` attribute to :ref:`general actuators<actuator-general>`. Values greater than 1 are only allowed
    for dyntype :at-val:`user`, as native activation dynamics are all scalar. Added example test implementing 2nd-order
    activation dynamics to
-   `engine_forward_test.cc <https://github.com/google-deepmind/mujoco/blob/main/test/engine/engine_forward_test.cc>`_.
+   `engine_forward_test.cc <https://github.com/google-deepmind/mujoco/blob/main/test/engine/engine_forward_test.cc>`__.
 #. Improved particle :ref:`composite<body-composite>` type, which now permits a user-specified geometry and multiple
    joints. See the two new examples:
-   `particle_free.xml <https://github.com/google-deepmind/mujoco/blob/main/model/composite/particle_free.xml>`_ and
-   `particle_free2d.xml <https://github.com/google-deepmind/mujoco/blob/main/model/composite/particle_free2d.xml>`_.
+   `particle_free.xml <https://github.com/google-deepmind/mujoco/blob/main/model/composite/particle_free.xml>`__ and
+   `particle_free2d.xml <https://github.com/google-deepmind/mujoco/blob/main/model/composite/particle_free2d.xml>`__.
 #. Performance improvements for non-AVX configurations:
 
-   - 14% faster ``mj_solveLD`` using `restrict <https://en.wikipedia.org/wiki/Restrict>`_. See `engine_core_smooth_benchmark_test
-     <https://github.com/google-deepmind/mujoco/blob/main/test/benchmark/engine_core_smooth_benchmark_test.cc>`_.
+   - 14% faster ``mj_solveLD`` using `restrict <https://en.wikipedia.org/wiki/Restrict>`__. See `engine_core_smooth_benchmark_test
+     <https://github.com/google-deepmind/mujoco/blob/main/test/benchmark/engine_core_smooth_benchmark_test.cc>`__.
    - 50% faster ``mju_dotSparse`` using manual loop unroll. See `engine_util_sparse_benchmark_test
-     <https://github.com/google-deepmind/mujoco/blob/main/test/benchmark/engine_util_sparse_benchmark_test.cc>`_.
+     <https://github.com/google-deepmind/mujoco/blob/main/test/benchmark/engine_util_sparse_benchmark_test.cc>`__.
 #. Added new :at:`solid` passive force plugin:
 
    .. youtube:: AGcTGHbbze4
@@ -553,9 +576,9 @@ General
    unspecified, the objtype is now :ref:`mjOBJ_UNKNOWN<mjtObj>`. ``user`` sensors :at:`datatype` default is now
    :at-val:`"real"`, :at:`needstage` default is now :at-val:`"acc"`.
 #. Added support for capsules in URDF import.
-#. On macOS, issue an informative error message when run under `Rosetta 2 <https://support.apple.com/en-gb/HT211861>`_
+#. On macOS, issue an informative error message when run under `Rosetta 2 <https://support.apple.com/en-gb/HT211861>`__
    translation on an Apple Silicon machine. Pre-built MuJoCo binaries make use of
-   `AVX <https://en.wikipedia.org/wiki/Advanced_Vector_Extensions>`_ instructions on x86-64 machines, which is not
+   `AVX <https://en.wikipedia.org/wiki/Advanced_Vector_Extensions>`__ instructions on x86-64 machines, which is not
    supported by Rosetta 2. (Before this version, users only get a cryptic "Illegal instruction" message.)
 
 Bug fixes
@@ -608,7 +631,7 @@ General
 
       x^* = \text{argmin} \; \tfrac{1}{2} x^T H x + x^T g \quad \text{s.t.} \quad l \le x \le u
 
-   The algorithm, introduced in `Tassa et al. 2014 <https://doi.org/10.1109/ICRA.2014.6907001>`_,
+   The algorithm, introduced in `Tassa et al. 2014 <https://doi.org/10.1109/ICRA.2014.6907001>`__,
    converges after 2-5 Cholesky factorisations, independent of problem size.
 
 #. Added :ref:`mju_mulVecMatVec` to multiply a square matrix :math:`M` with vectors :math:`x` and :math:`y` on both
@@ -638,11 +661,11 @@ General
 
    - Twist and bending stiffness can be set separately with the parameters :at:`twist` and :at:`bend`.
    - The stress-free configuration can be set to be the initial one or flat with the flag :at:`flat`.
-   - New `cable.xml <https://github.com/google-deepmind/mujoco/blob/main/model/plugin/elasticity/cable.xml>`_ example
+   - New `cable.xml <https://github.com/google-deepmind/mujoco/blob/main/model/plugin/elasticity/cable.xml>`__ example
      showing the formation of plectoneme.
-   - New `coil.xml <https://github.com/google-deepmind/mujoco/blob/main/model/plugin/elasticity/coil.xml>`_  example
+   - New `coil.xml <https://github.com/google-deepmind/mujoco/blob/main/model/plugin/elasticity/coil.xml>`__  example
      showing a curved equilibrium configuration.
-   - New `belt.xml <https://github.com/google-deepmind/mujoco/blob/main/model/plugin/elasticity/belt.xml>`_  example
+   - New `belt.xml <https://github.com/google-deepmind/mujoco/blob/main/model/plugin/elasticity/belt.xml>`__  example
      showing interaction between twist and anisotropy.
    - Added test using cantilever exact solution.
 
@@ -655,14 +678,14 @@ General
 Python bindings
 ^^^^^^^^^^^^^^^
 11. Added ``id`` and ``name`` properties to
-    `named accessor <https://mujoco.readthedocs.io/en/latest/python.html#named-access>`_ objects.
+    `named accessor <https://mujoco.readthedocs.io/en/latest/python.html#named-access>`__ objects.
     These provide more Pythonic API access to ``mj_name2id`` and ``mj_id2name`` respectively.
 
 #. The length of ``MjData.contact`` is now ``ncon`` rather than ``nconmax``, allowing it to be straightforwardly used as
    an iterator without needing to check ``ncon``.
 
 #. Fix a memory leak when a Python callable is installed as callback
-   (`#527 <https://github.com/google-deepmind/mujoco/issues/527>`_).
+   (`#527 <https://github.com/google-deepmind/mujoco/issues/527>`__).
 
 
 Version 2.2.2 (September 7, 2022)
@@ -676,7 +699,7 @@ General
    :height: 150px
 
 1. Added :ref:`adhesion actuators<actuator-adhesion>` mimicking vacuum grippers and adhesive biomechanical appendages.
-#. Added related `example model <https://github.com/google-deepmind/mujoco/tree/main/model/adhesion>`_ and video:
+#. Added related `example model <https://github.com/google-deepmind/mujoco/tree/main/model/adhesion>`__ and video:
 #. Added :ref:`mj_jacSubtreeCom` for computing the translational Jacobian of the center-of-mass of a subtree.
 #. Added :at:`torquescale` and :at:`anchor` attributes to :el:`weld` constraints. :at:`torquescale` sets the
    torque-to-force ratio exerted by the constraint, :at:`anchor` sets the point at which the weld wrench is
@@ -684,7 +707,7 @@ General
 #. Increased ``mjNEQDATA``, the row length of equality constraint parameters in ``mjModel.eq_data``, from 7 to 11.
 #. Added visualisation of anchor points for both :el:`connect` and :el:`weld` constraints (activated by the 'N' key in
    ``simulate``).
-#. Added `weld.xml <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/weld.xml>`_ showing
+#. Added `weld.xml <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/weld.xml>`__ showing
    different uses of new weld attributes.
 
    .. youtube:: s-0JHanqV1A
@@ -693,7 +716,7 @@ General
 
 #. Cartesian 6D end-effector control is now possible by adding a reference site to actuators with :at:`site`
    transmission. See description of new :at:`refsite` attribute in the :ref:`actuator<actuator-general>` documentation
-   and `refsite.xml <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/actuation/refsite.xml>`_
+   and `refsite.xml <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/actuation/refsite.xml>`__
    example model.
 
 #. Added :at:`autolimits` compiler option. If ``true``, joint and tendon :at:`limited` attributes and actuator
@@ -720,7 +743,7 @@ General
       :height: 150px
 
 #. Added catenary visualisation for hanging tendons. The model seen in the video can be found
-   `here <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/catenary.xml>`_.
+   `here <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/catenary.xml>`__.
 #. Added ``azimuth`` and ``elevation`` attributes to :ref:`visual/global<visual-global>`, defining the initial
    orientation of the free camera at model load time.
 #. Added ``mjv_defaultFreeCamera`` which sets the default free camera, respecting the above attributes.
@@ -768,7 +791,7 @@ General
    and `Memory Sanitizer (MSAN) <https://clang.llvm.org/docs/MemorySanitizer.html>`__. This allows ASAN to
    detect reads and writes to regions in ``mjModel.buffer`` and ``mjData.buffer`` that do not lie within an array, and
    for MSAN to detect reads from uninitialised fields in ``mjData`` following ``mj_resetData``.
-#. Added a `slider-crank example model <https://github.com/google-deepmind/mujoco/tree/main/model/slider_crank>`_.
+#. Added a `slider-crank example model <https://github.com/google-deepmind/mujoco/tree/main/model/slider_crank>`__.
 
 Bug fixes
 ^^^^^^^^^
@@ -814,8 +837,8 @@ Open Sourcing
 
    c. ``dist/``: Files related to packaging and binary distribution.
 
-#. Added `contributor's guide <https://github.com/google-deepmind/mujoco/blob/main/CONTRIBUTING.md>`_ and
-   `style guide <https://github.com/google-deepmind/mujoco/blob/main/STYLEGUIDE.md>`_.
+#. Added `contributor's guide <https://github.com/google-deepmind/mujoco/blob/main/CONTRIBUTING.md>`__ and
+   `style guide <https://github.com/google-deepmind/mujoco/blob/main/STYLEGUIDE.md>`__.
 
 General
 ^^^^^^^
@@ -946,7 +969,7 @@ Bug fixes
 ^^^^^^^^^
 
 5. Fixed rendering bug for planes, introduced in 2.1.2. This broke maze environments in
-   `dm_control <https://github.com/google-deepmind/dm_control>`_.
+   `dm_control <https://github.com/google-deepmind/dm_control>`__.
 
 
 Version 2.1.2 (Mar. 15, 2022)
@@ -967,7 +990,7 @@ API changes
 ^^^^^^^^^^^
 
 4. Moved definition of ``mjtNum`` floating point type into a new header
-   `mjtnum.h <https://github.com/google-deepmind/mujoco/blob/3577e2cf8bf841475b489aefff52276a39f24d51/include/mjtnum.h>`_.
+   `mjtnum.h <https://github.com/google-deepmind/mujoco/blob/3577e2cf8bf841475b489aefff52276a39f24d51/include/mjtnum.h>`__.
 #. Renamed header `mujoco_export.h` to :ref:`mjexport.h<inHeader>`.
 #. Added ``mj_printFormattedData``, which accepts a format string for floating point numbers, for example to increase
    precision.
@@ -975,12 +998,12 @@ API changes
 General
 ^^^^^^^
 
-7. MuJoCo can load `OBJ <https://en.wikipedia.org/wiki/Wavefront_.obj_file>`_ mesh files.
+7. MuJoCo can load `OBJ <https://en.wikipedia.org/wiki/Wavefront_.obj_file>`__ mesh files.
 
    a. Meshes containing polygons with more than 4 vertices are not supported.
    #. In OBJ files containing multiple object groups, any groups after the first one will be ignored.
    #. Added (post-release, not included in the 2.1.2 archive) textured
-      `mug <https://github.com/google-deepmind/mujoco/blob/main/model/mug/mug.xml>`_ example model:
+      `mug <https://github.com/google-deepmind/mujoco/blob/main/model/mug/mug.xml>`__ example model:
 
       .. image:: images/changelog/mug.png
          :width: 300px
@@ -1070,7 +1093,7 @@ Bug Fixes
 #. Computed capsule inertias are now exact. Until this change, capsule masses and inertias computed by the
    :ref:`compiler <compiler>`'s :at:`inertiafromgeom` mechanism were approximated by a cylinder, formed by the
    capsule's cylindrical middle section, extended on both ends by half the capsule radius. Capsule inertias are now
-   computed with the `Parallel Axis theorem <https://en.wikipedia.org/wiki/Parallel_axis_theorem>`_, applied to the two
+   computed with the `Parallel Axis theorem <https://en.wikipedia.org/wiki/Parallel_axis_theorem>`__, applied to the two
    hemispherical end-caps.
 
    .. attention::
@@ -1113,7 +1136,7 @@ Binary build
 
     a. MuJoCo library is now packaged as a `Framework Bundle
        <https://developer.apple.com/library/archive/documentation/MacOSX/Conceptual/BPFrameworks/Concepts/FrameworkAnato
-       my.html>`_, allowing it to be incorporated more easily into Xcode projects (including Swift projects). Developers
+       my.html>`__, allowing it to be incorporated more easily into Xcode projects (including Swift projects). Developers
        are encouraged to compile and link against MuJoCo using the ``-framework mujoco`` flag, however all header files
        and the ``libmujoco.2.1.1.dylib`` library can still be directly accessed inside the framework.
     #. Sample applications are now packaged into an Application Bundle called ``MuJoCo.app``. When launched via GUI,
@@ -1161,7 +1184,7 @@ General
 #. Fixed: The array of sensors in ``mjCModel`` was not cleared.
 #. Cleaned up cross-platform code (internal changes, not visible via the API).
 #. Fixed a bug in parsing of XML ``texcoord`` data (related to number of vertices).
-#. Fixed a bug in `simulate.cc <https://github.com/google-deepmind/mujoco/blob/main/simulate/simulate.cc>`_ related to
+#. Fixed a bug in `simulate.cc <https://github.com/google-deepmind/mujoco/blob/main/simulate/simulate.cc>`__ related to
    ``nkey`` (the number of keyframes).
 #. Accelerated collision detection in the presence of large numbers of non-colliding geoms (with ``contype==0 and
    conaffinity==0``).
@@ -1198,7 +1221,7 @@ License manager
 Earlier versions
 ----------------
 
-For changelogs of earlier versions please see `roboti.us <https://www.roboti.us/download.html>`_.
+For changelogs of earlier versions please see `roboti.us <https://www.roboti.us/download.html>`__.
 
 .. |humanoid| image:: images/models/humanoid.gif
    :width: 270px

doc/css/theme_overrides.css

@@ -18,6 +18,11 @@ body {
   color: var(--body-color);
 }
 
+/* Make the search bar query text readable in dark theme. */
+body[data-theme=dark] {
+  --color-sidebar-search-foreground: white;
+}
+
 .schema-small {
   font-size: 93%;
 }

doc/includes/references.h

@@ -25,18 +25,20 @@ typedef enum mjtState_ {          // state elements
   mjSTATE_CTRL          = 1<<5,   // control
   mjSTATE_QFRC_APPLIED  = 1<<6,   // applied generalized force
   mjSTATE_XFRC_APPLIED  = 1<<7,   // applied Cartesian force/torque
-  mjSTATE_MOCAP_POS     = 1<<8,   // positions of mocap bodies
-  mjSTATE_MOCAP_QUAT    = 1<<9,   // orientations of mocap bodies
-  mjSTATE_USERDATA      = 1<<10,  // user data
-  mjSTATE_PLUGIN        = 1<<11,  // plugin state
+  mjSTATE_EQ_ACTIVE     = 1<<8,   // enable/disable constraints
+  mjSTATE_MOCAP_POS     = 1<<9,   // positions of mocap bodies
+  mjSTATE_MOCAP_QUAT    = 1<<10,  // orientations of mocap bodies
+  mjSTATE_USERDATA      = 1<<11,  // user data
+  mjSTATE_PLUGIN        = 1<<12,  // plugin state
 
-  mjNSTATE              = 12,     // number of state elements
+  mjNSTATE              = 13,     // number of state elements
 
   // convenience values for commonly used state specifications
   mjSTATE_PHYSICS       = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT,
   mjSTATE_FULLPHYSICS   = mjSTATE_PHYSICS | mjSTATE_TIME | mjSTATE_PLUGIN,
   mjSTATE_USER          = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED |
-                          mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT | mjSTATE_USERDATA,
+                          mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT |
+                          mjSTATE_USERDATA,
   mjSTATE_INTEGRATION   = mjSTATE_FULLPHYSICS | mjSTATE_USER | mjSTATE_WARMSTART
 } mjtState;
 typedef enum mjtWarning_ {   // warning types
@@ -191,6 +193,7 @@ struct mjData_ {
   mjtNum* ctrl;              // control                                          (nu x 1)
   mjtNum* qfrc_applied;      // applied generalized force                        (nv x 1)
   mjtNum* xfrc_applied;      // applied Cartesian force/torque                   (nbody x 6)
+  mjtByte* eq_active;        // enable/disable constraints                       (neq x 1)
 
   // mocap data
   mjtNum* mocap_pos;         // positions of mocap bodies                        (nmocap x 3)
@@ -207,8 +210,8 @@ struct mjData_ {
   mjtNum* sensordata;        // sensor data array                                (nsensordata x 1)
 
   // plugins
-  int*        plugin;        // copy of m->plugin, required for deletion         (nplugin x 1)
-  uintptr_t*  plugin_data;   // pointer to plugin-managed data structure         (nplugin x 1)
+  int*       plugin;         // copy of m->plugin, required for deletion         (nplugin x 1)
+  uintptr_t* plugin_data;    // pointer to plugin-managed data structure         (nplugin x 1)
 
   //-------------------- POSITION dependent
 
@@ -1124,7 +1127,7 @@ struct mjModel_ {
   int*      eq_type;              // constraint type (mjtEq)                  (neq x 1)
   int*      eq_obj1id;            // id of object 1                           (neq x 1)
   int*      eq_obj2id;            // id of object 2                           (neq x 1)
-  mjtByte*  eq_active;            // enable/disable constraint                (neq x 1)
+  mjtByte*  eq_active0;           // initial enable/disable constraint state  (neq x 1)
   mjtNum*   eq_solref;            // constraint solver reference              (neq x mjNREF)
   mjtNum*   eq_solimp;            // constraint solver impedance              (neq x mjNIMP)
   mjtNum*   eq_data;              // numeric data for constraint              (neq x mjNEQDATA)
@@ -2112,7 +2115,6 @@ struct mjvSceneState_ {
     int* eq_type;
     int* eq_obj1id;
     int* eq_obj2id;
-    mjtByte* eq_active;
     mjtNum* eq_data;
 
     int* tendon_num;
@@ -2170,6 +2172,7 @@ struct mjvSceneState_ {
 
     mjtNum* ctrl;
     mjtNum* xfrc_applied;
+    mjtByte* eq_active;
 
     mjtNum* sensordata;
 
@@ -2610,6 +2613,7 @@ int mjp_resourceProviderCount(void);
 const mjpResourceProvider* mjp_getResourceProvider(const char* resource_name);
 const mjpResourceProvider* mjp_getResourceProviderAtSlot(int slot);
 mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
+void mju_bindThreadPool(mjData* d, void* thread_pool);
 void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task);
 void mju_threadPoolDestroy(mjThreadPool* thread_pool);
 void mju_defaultTask(mjTask* task);

include/mujoco/mjdata.h

@@ -33,18 +33,20 @@ typedef enum mjtState_ {          // state elements
   mjSTATE_CTRL          = 1<<5,   // control
   mjSTATE_QFRC_APPLIED  = 1<<6,   // applied generalized force
   mjSTATE_XFRC_APPLIED  = 1<<7,   // applied Cartesian force/torque
-  mjSTATE_MOCAP_POS     = 1<<8,   // positions of mocap bodies
-  mjSTATE_MOCAP_QUAT    = 1<<9,   // orientations of mocap bodies
-  mjSTATE_USERDATA      = 1<<10,  // user data
-  mjSTATE_PLUGIN        = 1<<11,  // plugin state
+  mjSTATE_EQ_ACTIVE     = 1<<8,   // enable/disable constraints
+  mjSTATE_MOCAP_POS     = 1<<9,   // positions of mocap bodies
+  mjSTATE_MOCAP_QUAT    = 1<<10,  // orientations of mocap bodies
+  mjSTATE_USERDATA      = 1<<11,  // user data
+  mjSTATE_PLUGIN        = 1<<12,  // plugin state
 
-  mjNSTATE              = 12,     // number of state elements
+  mjNSTATE              = 13,     // number of state elements
 
   // convenience values for commonly used state specifications
   mjSTATE_PHYSICS       = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT,
   mjSTATE_FULLPHYSICS   = mjSTATE_PHYSICS | mjSTATE_TIME | mjSTATE_PLUGIN,
   mjSTATE_USER          = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED |
-                          mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT | mjSTATE_USERDATA,
+                          mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT |
+                          mjSTATE_USERDATA,
   mjSTATE_INTEGRATION   = mjSTATE_FULLPHYSICS | mjSTATE_USER | mjSTATE_WARMSTART
 } mjtState;
 
@@ -219,6 +221,7 @@ struct mjData_ {
   mjtNum* ctrl;              // control                                          (nu x 1)
   mjtNum* qfrc_applied;      // applied generalized force                        (nv x 1)
   mjtNum* xfrc_applied;      // applied Cartesian force/torque                   (nbody x 6)
+  mjtByte* eq_active;        // enable/disable constraints                       (neq x 1)
 
   // mocap data
   mjtNum* mocap_pos;         // positions of mocap bodies                        (nmocap x 3)
@@ -235,8 +238,8 @@ struct mjData_ {
   mjtNum* sensordata;        // sensor data array                                (nsensordata x 1)
 
   // plugins
-  int*        plugin;        // copy of m->plugin, required for deletion         (nplugin x 1)
-  uintptr_t*  plugin_data;   // pointer to plugin-managed data structure         (nplugin x 1)
+  int*       plugin;         // copy of m->plugin, required for deletion         (nplugin x 1)
+  uintptr_t* plugin_data;    // pointer to plugin-managed data structure         (nplugin x 1)
 
   //-------------------- POSITION dependent
 

include/mujoco/mjmodel.h

@@ -856,7 +856,7 @@ struct mjModel_ {
   int*      eq_type;              // constraint type (mjtEq)                  (neq x 1)
   int*      eq_obj1id;            // id of object 1                           (neq x 1)
   int*      eq_obj2id;            // id of object 2                           (neq x 1)
-  mjtByte*  eq_active;            // enable/disable constraint                (neq x 1)
+  mjtByte*  eq_active0;           // initial enable/disable constraint state  (neq x 1)
   mjtNum*   eq_solref;            // constraint solver reference              (neq x mjNREF)
   mjtNum*   eq_solimp;            // constraint solver impedance              (neq x mjNIMP)
   mjtNum*   eq_data;              // numeric data for constraint              (neq x mjNEQDATA)

include/mujoco/mjvisualize.h

@@ -506,7 +506,6 @@ struct mjvSceneState_ {
     int* eq_type;
     int* eq_obj1id;
     int* eq_obj2id;
-    mjtByte* eq_active;
     mjtNum* eq_data;
 
     int* tendon_num;
@@ -564,6 +563,7 @@ struct mjvSceneState_ {
 
     mjtNum* ctrl;
     mjtNum* xfrc_applied;
+    mjtByte* eq_active;
 
     mjtNum* sensordata;
 

include/mujoco/mjxmacro.h

@@ -362,7 +362,7 @@
     XMJV( int,     eq_type,               neq,           1                    ) \
     XMJV( int,     eq_obj1id,             neq,           1                    ) \
     XMJV( int,     eq_obj2id,             neq,           1                    ) \
-    XMJV( mjtByte, eq_active,             neq,           1                    ) \
+    X   ( mjtByte, eq_active0,            neq,           1                    ) \
     X   ( mjtNum,  eq_solref,             neq,           mjNREF               ) \
     X   ( mjtNum,  eq_solimp,             neq,           mjNIMP               ) \
     XMJV( mjtNum,  eq_data,               neq,           mjNEQDATA            ) \
@@ -495,6 +495,7 @@
     XMJV( mjtNum,    ctrl,              nu,          1           ) \
     X   ( mjtNum,    qfrc_applied,      nv,          1           ) \
     XMJV( mjtNum,    xfrc_applied,      nbody,       6           ) \
+    XMJV( mjtByte,   eq_active,         neq,         1           ) \
     X   ( mjtNum,    mocap_pos,         nmocap,      3           ) \
     X   ( mjtNum,    mocap_quat,        nmocap,      4           ) \
     X   ( mjtNum,    qacc,              nv,          1           ) \

include/mujoco/mujoco.h

@@ -1310,6 +1310,9 @@ MJAPI const mjpResourceProvider* mjp_getResourceProviderAtSlot(int slot);
 // Create a thread pool with the specified number of threads running.
 MJAPI mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
 
+// Adds a thread pool to mjData and configures it for multi-threaded use.
+MJAPI void mju_bindThreadPool(mjData* d, void* thread_pool);
+
 // Enqueue a task in a thread pool.
 MJAPI void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task);
 

introspect/enums.py

@@ -394,15 +394,16 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjSTATE_CTRL', 32),
              ('mjSTATE_QFRC_APPLIED', 64),
              ('mjSTATE_XFRC_APPLIED', 128),
-             ('mjSTATE_MOCAP_POS', 256),
-             ('mjSTATE_MOCAP_QUAT', 512),
-             ('mjSTATE_USERDATA', 1024),
-             ('mjSTATE_PLUGIN', 2048),
-             ('mjNSTATE', 12),
+             ('mjSTATE_EQ_ACTIVE', 256),
+             ('mjSTATE_MOCAP_POS', 512),
+             ('mjSTATE_MOCAP_QUAT', 1024),
+             ('mjSTATE_USERDATA', 2048),
+             ('mjSTATE_PLUGIN', 4096),
+             ('mjNSTATE', 13),
              ('mjSTATE_PHYSICS', 14),
-             ('mjSTATE_FULLPHYSICS', 2063),
-             ('mjSTATE_USER', 2016),
-             ('mjSTATE_INTEGRATION', 4095),
+             ('mjSTATE_FULLPHYSICS', 4111),
+             ('mjSTATE_USER', 4064),
+             ('mjSTATE_INTEGRATION', 8191),
          ]),
      )),
     ('mjtWarning',

introspect/functions.py

@@ -8271,6 +8271,26 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Create a thread pool with the specified number of threads running.',  # pylint: disable=line-too-long
      )),
+    ('mju_bindThreadPool',
+     FunctionDecl(
+         name='mju_bindThreadPool',
+         return_type=ValueType(name='void'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='d',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjData'),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='thread_pool',
+                 type=PointerType(
+                     inner_type=ValueType(name='void'),
+                 ),
+             ),
+         ),
+         doc='Adds a thread pool to mjData and configures it for multi-threaded use.',  # pylint: disable=line-too-long
+     )),
     ('mju_threadPoolEnqueue',
      FunctionDecl(
          name='mju_threadPoolEnqueue',

introspect/structs.py

@@ -2565,11 +2565,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  doc='id of object 2                           (neq x 1)',
              ),
              StructFieldDecl(
-                 name='eq_active',
+                 name='eq_active0',
                  type=PointerType(
                      inner_type=ValueType(name='mjtByte'),
                  ),
-                 doc='enable/disable constraint                (neq x 1)',
+                 doc='initial enable/disable constraint state  (neq x 1)',
              ),
              StructFieldDecl(
                  name='eq_solref',
@@ -3826,6 +3826,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc='applied Cartesian force/torque                   (nbody x 6)',  # pylint: disable=line-too-long
              ),
+             StructFieldDecl(
+                 name='eq_active',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtByte'),
+                 ),
+                 doc='enable/disable constraints                       (neq x 1)',  # pylint: disable=line-too-long
+             ),
              StructFieldDecl(
                  name='mocap_pos',
                  type=PointerType(
@@ -6226,13 +6233,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                              ),
                              doc='',
                          ),
-                         StructFieldDecl(
-                             name='eq_active',
-                             type=PointerType(
-                                 inner_type=ValueType(name='mjtByte'),
-                             ),
-                             doc='',
-                         ),
                          StructFieldDecl(
                              name='eq_data',
                              type=PointerType(
@@ -6549,6 +6549,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                              ),
                              doc='',
                          ),
+                         StructFieldDecl(
+                             name='eq_active',
+                             type=PointerType(
+                                 inner_type=ValueType(name='mjtByte'),
+                             ),
+                             doc='',
+                         ),
                          StructFieldDecl(
                              name='sensordata',
                              type=PointerType(

python/mujoco/indexer_xmacro.h

@@ -78,14 +78,14 @@
   X( mjtNum, cam_, ipd,          ncam, 1               ) \
   X( mjtNum, cam_, user,         ncam, MJ_M(nuser_cam) )
 
-#define MJMODEL_EQUALITY                    \
-  X( int,     eq_, type,   neq, 1         ) \
-  X( int,     eq_, obj1id, neq, 1         ) \
-  X( int,     eq_, obj2id, neq, 1         ) \
-  X( mjtByte, eq_, active, neq, 1         ) \
-  X( mjtNum,  eq_, solref, neq, mjNREF    ) \
-  X( mjtNum,  eq_, solimp, neq, mjNIMP    ) \
-  X( mjtNum,  eq_, data,   neq, mjNEQDATA )
+#define MJMODEL_EQUALITY                     \
+  X( int,     eq_, type,    neq, 1         ) \
+  X( int,     eq_, obj1id,  neq, 1         ) \
+  X( int,     eq_, obj2id,  neq, 1         ) \
+  X( mjtByte, eq_, active0, neq, 1         ) \
+  X( mjtNum,  eq_, solref,  neq, mjNREF    ) \
+  X( mjtNum,  eq_, solimp,  neq, mjNIMP    ) \
+  X( mjtNum,  eq_, data,    neq, mjNEQDATA )
 
 #define MJMODEL_EXCLUDE                      \
   X( int, exclude_, signature, nexclude, 1 )

python/mujoco/simulate.cc

@@ -70,12 +70,12 @@ class SimulateWrapper {
  public:
   SimulateWrapper(std::unique_ptr<PlatformUIAdapter> platform_ui_adapter,
                   py::object scn, py::object cam, py::object opt,
-                  py::object pert, bool fully_managed)
+                  py::object pert, bool is_passive)
       : simulate_(new mujoco::Simulate(
             std::move(platform_ui_adapter), scn.cast<MjvSceneWrapper&>().get(),
             cam.cast<MjvCameraWrapper&>().get(),
             opt.cast<MjvOptionWrapper&>().get(),
-            pert.cast<MjvPerturbWrapper&>().get(), fully_managed)),
+            pert.cast<MjvPerturbWrapper&>().get(), is_passive)),
         m_(py::none()),
         d_(py::none()),
         scn_(scn),
@@ -194,12 +194,13 @@ PYBIND11_MODULE(_simulate, pymodule) {
   py::class_<SimulateWrapper>(pymodule, "Simulate")
       .def_readonly_static("MAX_GEOM", &mujoco::Simulate::kMaxGeom)
       .def(py::init([](py::object scn, py::object cam, py::object opt,
-                       py::object pert, bool fully_managed,
+                       py::object pert, bool run_physics_thread,
                        py::object key_callback) {
+        bool is_passive = !run_physics_thread;
         return std::make_unique<SimulateWrapper>(
             std::make_unique<UIAdapterWithPyCallback<UIAdapter>>(
                 key_callback),
-            scn, cam, opt, pert, fully_managed);
+            scn, cam, opt, pert, is_passive);
       }))
       .def("destroy", &SimulateWrapper::Destroy,
            py::call_guard<py::gil_scoped_release>())
@@ -210,7 +211,8 @@ PYBIND11_MODULE(_simulate, pymodule) {
            py::call_guard<py::gil_scoped_release>())
       .def("sync", CallIfNotNull(&mujoco::Simulate::Sync),
            py::call_guard<py::gil_scoped_release>())
-
+      .def("add_to_history", CallIfNotNull(&mujoco::Simulate::AddToHistory),
+           py::call_guard<py::gil_scoped_release>())
       .def("render_loop", CallIfNotNull(&mujoco::Simulate::RenderLoop),
            py::call_guard<py::gil_scoped_release>())
       .def("lock", GetIfNotNull(&mujoco::Simulate::mtx),

python/mujoco/viewer.py

@@ -237,6 +237,7 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
       if m is not None:
         assert d is not None
         if simulate.run:
+          stepped = False
           # Record CPU time at start of iteration.
           startcpu = time.time()
 
@@ -275,6 +276,7 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
 
             # Run single step, let next iteration deal with timing.
             mujoco.mj_step(m, d)
+            stepped = True
 
           # In-sync: step until ahead of cpu.
           else:
@@ -292,10 +294,16 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
 
               # Call mj_step.
               mujoco.mj_step(m, d)
+              stepped = True
 
               # Break if reset.
               if d.time < prevsim:
                 break
+
+          # save current state to history buffer
+          if (stepped):
+            simulate.add_to_history()
+
         else:  # simulate.run is False: GUI is paused.
 
           # Run mj_forward, to update rendering and joint sliders.

sample/testspeed.cc

@@ -119,12 +119,12 @@ void simulate(int id, int nstep, mjtNum* ctrl) {
 int main(int argc, char** argv) {
 
   // print help if arguments are missing
-  if (argc < 2 || argc > 6) {
-    return finish("\n Usage:  testspeed modelfile [nstep nthread ctrlnoise profile]\n");
+  if (argc < 2 || argc > 7) {
+    return finish("\n Usage:  testspeed modelfile [nstep nthread ctrlnoise profile npoolthread]\n");
   }
 
   // read arguments
-  int nstep = 10000, nthread = 0, profile = 1;
+  int nstep = 10000, nthread = 0, profile = 1, npoolthread = 0;
   // inject small noise by default, to avoid fixed contact state
   mjtNum ctrlnoise = 0.01;
   if (argc > 2 && (std::sscanf(argv[2], "%d", &nstep) != 1 || nstep <= 0)) {
@@ -139,12 +139,16 @@ int main(int argc, char** argv) {
   if (argc > 5 && std::sscanf(argv[5], "%d", &profile) != 1) {
     return finish("Invalid profile argument");
   }
+  if (argc > 6 && std::sscanf(argv[6], "%d", &npoolthread) != 1) {
+    return finish("Invalid npoolthread argument");
+  }
 
   // clamp ctrlnoise to [0.0, 1.0]
   ctrlnoise = mjMAX(0.0, mjMIN(ctrlnoise, 1.0));
 
   // clamp nthread to [1, maxthread]
   nthread = mjMAX(1, mjMIN(maxthread, nthread));
+  npoolthread = mjMAX(0, mjMIN(maxthread, npoolthread));
 
   // get filename, determine file type
   std::string filename(argv[1]);
@@ -169,6 +173,10 @@ int main(int argc, char** argv) {
       return finish("Could not allocate mjData", m);
     }
 
+    if (npoolthread > 0) {
+      mjThreadPool* threadpool = mju_threadPoolCreate(npoolthread);
+      mju_bindThreadPool(d[id], threadpool);
+    }
     // init to keyframe "test" if present
     if (testkey>=0) {
       mju_copy(d[id]->qpos, m->key_qpos + testkey*m->nq, m->nq);
@@ -254,7 +262,11 @@ int main(int argc, char** argv) {
 
   // free per-thread data
   for (int id=0; id<nthread; id++) {
+    mjThreadPool* threadpool = (mjThreadPool*) d[id]->threadpool;
     mj_deleteData(d[id]);
+    if (threadpool) {
+      mju_threadPoolDestroy(threadpool);
+    }
   }
 
   // finalize

simulate/main.cc

@@ -15,6 +15,7 @@
 #include <chrono>
 #include <cstdint>
 #include <cstdio>
+#include <cstdlib>
 #include <cstring>
 #include <iostream>
 #include <memory>
@@ -22,8 +23,6 @@
 #include <new>
 #include <string>
 #include <thread>
-#include <type_traits>
-#include <vector>
 
 #include <mujoco/mujoco.h>
 #include "glfw_adapter.h"
@@ -279,8 +278,7 @@ void PhysicsLoop(mj::Simulate& sim) {
         free(ctrlnoise);
         ctrlnoise = (mjtNum*) malloc(sizeof(mjtNum)*m->nu);
         mju_zero(ctrlnoise, m->nu);
-      }
-      else {
+      } else {
         sim.LoadMessageClear();
       }
     }
@@ -305,8 +303,7 @@ void PhysicsLoop(mj::Simulate& sim) {
         free(ctrlnoise);
         ctrlnoise = static_cast<mjtNum*>(malloc(sizeof(mjtNum)*m->nu));
         mju_zero(ctrlnoise, m->nu);
-      }
-      else {
+      } else {
         sim.LoadMessageClear();
       }
     }
@@ -327,6 +324,8 @@ void PhysicsLoop(mj::Simulate& sim) {
       if (m) {
         // running
         if (sim.run) {
+          bool stepped = false;
+
           // record cpu time at start of iteration
           const auto startCPU = mj::Simulate::Clock::now();
 
@@ -366,6 +365,7 @@ void PhysicsLoop(mj::Simulate& sim) {
 
             // run single step, let next iteration deal with timing
             mj_step(m, d);
+            stepped = true;
           }
 
           // in-sync: step until ahead of cpu
@@ -387,6 +387,7 @@ void PhysicsLoop(mj::Simulate& sim) {
 
               // call mj_step
               mj_step(m, d);
+              stepped = true;
 
               // break if reset
               if (d->time < prevSim) {
@@ -394,6 +395,11 @@ void PhysicsLoop(mj::Simulate& sim) {
               }
             }
           }
+
+          // save current state to history buffer
+          if (stepped) {
+            sim.AddToHistory();
+          }
         }
 
         // paused
@@ -483,7 +489,7 @@ int main(int argc, char** argv) {
   // simulate object encapsulates the UI
   auto sim = std::make_unique<mj::Simulate>(
       std::make_unique<mj::GlfwAdapter>(),
-      &scn, &cam, &opt, &pert, /* fully_managed = */ true
+      &scn, &cam, &opt, &pert, /* is_passive = */ false
   );
 
   const char* filename = nullptr;

simulate/simulate.cc

@@ -131,6 +131,7 @@ const char help_content[] =
   "Space\n"
   "+  -\n"
   "Right arrow\n"
+  "Left arrow\n"
   "[  ]\n"
   "Esc\n"
   "Double-click\n"
@@ -153,7 +154,8 @@ const char help_content[] =
 const char help_title[] =
   "Play / Pause\n"
   "Speed up / down\n"
-  "Step\n"
+  "Step forward\n"
+  "Step back\n"
   "Cycle cameras\n"
   "Free camera\n"
   "Select\n"
@@ -539,6 +541,25 @@ void ShowSensor(mj::Simulate* sim, mjrRect rect) {
   mjr_figure(viewport, &sim->figsensor, &sim->platform_ui->mjr_context());
 }
 
+// load state from history buffer
+static void LoadScrubState(mj::Simulate* sim) {
+  // get index into circular buffer
+  int i = (sim->scrub_index + sim->history_cursor_) % sim->nhistory_;
+  i = (i + sim->nhistory_) % sim->nhistory_;
+
+  // load state
+  mjtNum* state = &sim->history_[i * sim->state_size_];
+  mj_setState(sim->m_, sim->d_, state, mjSTATE_INTEGRATION);
+
+  // call forward dynamics
+  mj_forward(sim->m_, sim->d_);
+}
+
+// update an entire section of ui0
+static void mjui0_update_section(mj::Simulate* sim, int section) {
+  mjui_update(section, -1, &sim->ui0, &sim->uistate, &sim->platform_ui->mjr_context());
+}
+
 // prepare info text
 void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
               char (&title)[mj::Simulate::kMaxFilenameLength],
@@ -653,7 +674,7 @@ void UpdateWatch(mj::Simulate* sim, const mjModel* m, const mjData* d) {
 
 // make physics section of UI
 void MakePhysicsSection(mj::Simulate* sim, int oldstate) {
-  mjOption* opt = sim->fully_managed_ ? &sim->m_->opt : &sim->scnstate_.model.opt;
+  mjOption* opt = sim->is_passive_ ? &sim->scnstate_.model.opt : &sim->m_->opt;
   mjuiDef defPhysics[] = {
     {mjITEM_SECTION,   "Physics",       oldstate, nullptr,           "AP"},
     {mjITEM_SELECT,    "Integrator",    2, &(opt->integrator),        "Euler\nRK4\nimplicit\nimplicitfast"},
@@ -845,8 +866,8 @@ void MakeRenderingSection(mj::Simulate* sim, const mjModel* m, int oldstate) {
 
 // make visualization section of UI
 void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m, int oldstate) {
-  mjStatistic* stat = sim->fully_managed_ ? &sim->m_->stat : &sim->scnstate_.model.stat;
-  mjVisual* vis = sim->fully_managed_ ? &sim->m_->vis : &sim->scnstate_.model.vis;
+  mjStatistic* stat = sim->is_passive_ ? &sim->scnstate_.model.stat : &sim->m_->stat;
+  mjVisual* vis = sim->is_passive_ ? &sim->scnstate_.model.vis : &sim->m_->vis;
 
   mjuiDef defVisualization[] = {
     {mjITEM_SECTION,   "Visualization", oldstate, nullptr, "AV"},
@@ -980,7 +1001,7 @@ void MakeJointSection(mj::Simulate* sim, int oldstate) {
       }
 
       // set data and name
-      if (sim->fully_managed_) {
+      if (!sim->is_passive_) {
         defSlider[0].pdata = &sim->d_->qpos[sim->m_->jnt_qposadr[i]];
       } else {
         defSlider[0].pdata = &sim->qpos_[sim->jnt_qposadr_[i]];
@@ -1032,7 +1053,7 @@ void MakeControlSection(mj::Simulate* sim, int oldstate) {
     }
 
     // set data and name
-    if (sim->fully_managed_) {
+    if (!sim->is_passive_) {
       defSlider[0].pdata = &sim->d_->ctrl[i];
     } else {
       defSlider[0].pdata = &sim->ctrl_[i];
@@ -1221,16 +1242,16 @@ int UiPredicate(int category, void* userdata) {
 
   switch (category) {
   case 2:                 // require model
-    return sim->m_ || !sim->fully_managed_;
+    return sim->m_ || sim->is_passive_;
 
   case 3:                 // require model and nkey
-    return sim->fully_managed_ && sim->nkey_;
+    return !sim->is_passive_ && sim->nkey_;
 
   case 4:                 // require model and paused
     return sim->m_ && !sim->run;
 
   case 5:                 // require model and fully managed mode
-    return sim->fully_managed_ && sim->m_;
+    return !sim->is_passive_ && sim->m_;
 
   default:
     return 1;
@@ -1369,12 +1390,18 @@ void UiEvent(mjuiState* state) {
       case 7:             // Save key
         sim->pending_.save_key = true;
         break;
+
+      case 11:            // History scrubber
+        sim->run = 0;
+        sim->pending_.load_from_history = true;
+        mjui0_update_section(sim, SECT_SIMULATION);
+        break;
       }
     }
 
     // physics section
     else if (it && it->sectionid==SECT_PHYSICS && sim->m_) {
-      mjOption* opt = sim->fully_managed_ ? &sim->m_->opt : &sim->scnstate_.model.opt;
+      mjOption* opt = sim->is_passive_ ? &sim->scnstate_.model.opt : &sim->m_->opt;
 
       // update disable flags in mjOption
       opt->disableflags = 0;
@@ -1404,8 +1431,7 @@ void UiEvent(mjuiState* state) {
         } else {
           sim->cam.type = mjCAMERA_FREE;
           sim->camera = 0;
-          mjui_update(SECT_RENDERING, -1, &sim->ui0, &sim->uistate,
-                      &sim->platform_ui->mjr_context());
+          mjui0_update_section(sim, SECT_RENDERING);
         }
       } else {
         sim->cam.type = mjCAMERA_FIXED;
@@ -1474,25 +1500,56 @@ void UiEvent(mjuiState* state) {
   if (state->type==mjEVENT_KEY && state->key!=0) {
     switch (state->key) {
     case ' ':                   // Mode
-      if (sim->fully_managed_ && sim->m_) {
+      if (!sim->is_passive_ && sim->m_) {
         sim->run = 1 - sim->run;
         sim->pert.active = 0;
-        mjui_update(-1, -1, &sim->ui0, state, &sim->platform_ui->mjr_context());
+
+        if (sim->run) sim->scrub_index = 0;  // reset scrubber
+
+        mjui0_update_section(sim, -1);
       }
       break;
 
     case mjKEY_RIGHT:           // step forward
-      if (sim->fully_managed_ && sim->m_ && !sim->run) {
+      if (!sim->is_passive_ && sim->m_ && !sim->run) {
         ClearTimers(sim->d_);
-        mj_step(sim->m_, sim->d_);
+
+        // currently in scrubber: increment scrub, load state, update slider UI
+        if (sim->scrub_index < 0) {
+          sim->scrub_index++;
+          sim->pending_.load_from_history = true;
+          mjui0_update_section(sim, SECT_SIMULATION);
+        }
+
+        // not in scrubber: step
+        else {
+          mj_step(sim->m_, sim->d_);
+        }
+
         UpdateProfiler(sim, sim->m_, sim->d_);
         UpdateSensor(sim, sim->m_, sim->d_);
         UpdateSettings(sim, sim->m_);
       }
       break;
 
+    case mjKEY_LEFT:           // step backward
+      if (!sim->is_passive_ && sim->m_) {
+        sim->run = 0;
+        ClearTimers(sim->d_);
+
+        // decrement scrub, load state
+        sim->scrub_index = mjMAX(sim->scrub_index - 1, 1 - sim->nhistory_);
+        sim->pending_.load_from_history = true;
+
+        // update slider UI, profiler, sensor
+        mjui0_update_section(sim, SECT_SIMULATION);
+        UpdateProfiler(sim, sim->m_, sim->d_);
+        UpdateSensor(sim, sim->m_, sim->d_);
+      }
+      break;
+
     case mjKEY_PAGE_UP:         // select parent body
-      if ((sim->m_ || !sim->fully_managed_) && sim->pert.select > 0) {
+      if ((sim->m_ || sim->is_passive_) && sim->pert.select > 0) {
         sim->pert.select = sim->body_parentid_[sim->pert.select];
         sim->pert.skinselect = -1;
 
@@ -1505,7 +1562,7 @@ void UiEvent(mjuiState* state) {
       break;
 
     case ']':                   // cycle up fixed cameras
-      if ((sim->m_ || !sim->fully_managed_) && sim->ncam_) {
+      if ((sim->m_ || !sim->is_passive_) && sim->ncam_) {
         sim->cam.type = mjCAMERA_FIXED;
         // camera = {0 or 1} are reserved for the free and tracking cameras
         if (sim->camera < 2 || sim->camera == 2 + sim->ncam_ - 1) {
@@ -1514,12 +1571,12 @@ void UiEvent(mjuiState* state) {
           sim->camera += 1;
         }
         sim->cam.fixedcamid = sim->camera - 2;
-        mjui_update(SECT_RENDERING, -1, &sim->ui0, &sim->uistate, &sim->platform_ui->mjr_context());
+        mjui0_update_section(sim, SECT_RENDERING);
       }
       break;
 
     case '[':                   // cycle down fixed cameras
-      if ((sim->m_ || !sim->fully_managed_) && sim->ncam_) {
+      if ((sim->m_ || sim->is_passive_) && sim->ncam_) {
         sim->cam.type = mjCAMERA_FIXED;
         // camera = {0 or 1} are reserved for the free and tracking cameras
         if (sim->camera <= 2) {
@@ -1528,32 +1585,32 @@ void UiEvent(mjuiState* state) {
           sim->camera -= 1;
         }
         sim->cam.fixedcamid = sim->camera - 2;
-        mjui_update(SECT_RENDERING, -1, &sim->ui0, &sim->uistate, &sim->platform_ui->mjr_context());
+        mjui0_update_section(sim, SECT_RENDERING);
       }
       break;
 
     case mjKEY_F6:                   // cycle frame visualisation
-      if (sim->m_ || !sim->fully_managed_) {
+      if (sim->m_ || sim->is_passive_) {
         sim->opt.frame = (sim->opt.frame + 1) % mjNFRAME;
-        mjui_update(SECT_RENDERING, -1, &sim->ui0, &sim->uistate, &sim->platform_ui->mjr_context());
+        mjui0_update_section(sim, SECT_RENDERING);
       }
       break;
 
     case mjKEY_F7:                   // cycle label visualisation
-      if (sim->m_ || !sim->fully_managed_) {
+      if (sim->m_ || sim->is_passive_) {
         sim->opt.label = (sim->opt.label + 1) % mjNLABEL;
-        mjui_update(SECT_RENDERING, -1, &sim->ui0, &sim->uistate, &sim->platform_ui->mjr_context());
+        mjui0_update_section(sim, SECT_RENDERING);
       }
       break;
 
     case mjKEY_ESCAPE:          // free camera
       sim->cam.type = mjCAMERA_FREE;
       sim->camera = 0;
-      mjui_update(SECT_RENDERING, -1, &sim->ui0, &sim->uistate, &sim->platform_ui->mjr_context());
+      mjui0_update_section(sim, SECT_RENDERING);
       break;
 
     case '-':                   // slow down
-      if (sim->fully_managed_) {
+      if (!sim->is_passive_) {
         int numclicks = sizeof(sim->percentRealTime) / sizeof(sim->percentRealTime[0]);
         if (sim->real_time_index < numclicks-1 && !state->shift) {
           sim->real_time_index++;
@@ -1563,7 +1620,7 @@ void UiEvent(mjuiState* state) {
       break;
 
     case '=':                   // speed up
-      if (sim->fully_managed_ && sim->real_time_index > 0 && !state->shift) {
+      if (!sim->is_passive_ && sim->real_time_index > 0 && !state->shift) {
         sim->real_time_index--;
         sim->speed_changed = true;
       }
@@ -1576,7 +1633,7 @@ void UiEvent(mjuiState* state) {
   // 3D scroll
   if (state->type==mjEVENT_SCROLL && state->mouserect==3) {
     // emulate vertical mouse motion = 2% of window height
-    if (sim->fully_managed_) {
+    if (!sim->is_passive_) {
       mjv_moveCamera(sim->m_, mjMOUSE_ZOOM, 0, -zoom_increment*state->sy, &sim->scn, &sim->cam);
     } else {
       mjv_moveCameraFromState(
@@ -1589,7 +1646,7 @@ void UiEvent(mjuiState* state) {
   if (state->type==mjEVENT_PRESS && state->mouserect==3) {
     // set perturbation
     int newperturb = 0;
-    if (state->control && sim->pert.select>0 && (sim->m_ || !sim->fully_managed_)) {
+    if (state->control && sim->pert.select>0 && (sim->m_ || sim->is_passive_)) {
       // right: translate;  left: rotate
       if (state->right) {
         newperturb = mjPERT_TRANSLATE;
@@ -1602,7 +1659,7 @@ void UiEvent(mjuiState* state) {
     }
 
     // handle double-click
-    if (state->doubleclick && (sim->m_ || !sim->fully_managed_)) {
+    if (state->doubleclick && (sim->m_ || sim->is_passive_)) {
       sim->pending_.select = true;
       std::memcpy(&sim->pending_.select_state, state, sizeof(sim->pending_.select_state));
 
@@ -1615,7 +1672,7 @@ void UiEvent(mjuiState* state) {
   }
 
   // 3D release
-  if (state->type==mjEVENT_RELEASE && state->dragrect==3 && (sim->m_ || !sim->fully_managed_)) {
+  if (state->type==mjEVENT_RELEASE && state->dragrect==3 && (sim->m_ || sim->is_passive_)) {
     // stop perturbation
     sim->pert.active = 0;
     sim->pending_.newperturb = 0;
@@ -1623,7 +1680,7 @@ void UiEvent(mjuiState* state) {
   }
 
   // 3D move
-  if (state->type==mjEVENT_MOVE && state->dragrect==3 && (sim->m_ || !sim->fully_managed_)) {
+  if (state->type==mjEVENT_MOVE && state->dragrect==3 && (sim->m_ || sim->is_passive_)) {
     // determine action based on mouse button
     mjtMouse action;
     if (state->right) {
@@ -1637,7 +1694,7 @@ void UiEvent(mjuiState* state) {
     // move perturb or camera
     mjrRect r = state->rect[3];
     if (sim->pert.active) {
-      if (sim->fully_managed_) {
+      if (!sim->is_passive_) {
         mjv_movePerturb(
             sim->m_, sim->d_, action, state->dx / r.height, -state->dy / r.height,
             &sim->scn, &sim->pert);
@@ -1647,7 +1704,7 @@ void UiEvent(mjuiState* state) {
             &sim->scn, &sim->pert);
       }
     } else {
-      if (sim->fully_managed_) {
+      if (!sim->is_passive_) {
         mjv_moveCamera(
             sim->m_, action, state->dx / r.height, -state->dy / r.height,
             &sim->scn, &sim->cam);
@@ -1661,7 +1718,7 @@ void UiEvent(mjuiState* state) {
   }
 
   // Dropped files
-  if (state->type == mjEVENT_FILESDROP && state->dropcount > 0 && sim->fully_managed_) {
+  if (state->type == mjEVENT_FILESDROP && state->dropcount > 0 && !sim->is_passive_) {
     while (sim->droploadrequest.load()) {}
     mju::strcpy_arr(sim->dropfilename, state->droppaths[0]);
     sim->droploadrequest.store(true);
@@ -1682,8 +1739,8 @@ namespace mju = ::mujoco::sample_util;
 Simulate::Simulate(std::unique_ptr<PlatformUIAdapter> platform_ui,
                    mjvScene* scn, mjvCamera* cam,
                    mjvOption* opt, mjvPerturb* pert,
-                   bool fully_managed)
-    : fully_managed_(fully_managed),
+                   bool is_passive)
+    : is_passive_(is_passive),
       scn(*scn),
       cam(*cam),
       opt(*opt),
@@ -1693,6 +1750,8 @@ Simulate::Simulate(std::unique_ptr<PlatformUIAdapter> platform_ui,
   mjv_defaultSceneState(&scnstate_);
 }
 
+// synchronize model and data
+// operations which require holding the mutex, prevents racing with physics thread
 void Simulate::Sync() {
   MutexLock lock(this->mtx);
 
@@ -1749,7 +1808,7 @@ void Simulate::Sync() {
     }
   }
 
-  if (!fully_managed_) {
+  if (is_passive_) {
     // synchronize m_->opt with changes made via the UI
 #define X(name)                                                  \
   if (IsDifferent(scnstate_.model.opt.name, mjopt_prev_.name)) { \
@@ -1826,6 +1885,8 @@ void Simulate::Sync() {
     mj_forward(m_, d_);
     update_profiler = true;
     update_sensor = true;
+    scrub_index = 0;
+    mjui0_update_section(this, SECT_SIMULATION);
     pending_.reset = false;
   }
 
@@ -1839,6 +1900,13 @@ void Simulate::Sync() {
     pending_.copy_pose = false;
   }
 
+  if (pending_.load_from_history) {
+    LoadScrubState(this);
+    update_profiler = true;
+    update_sensor = true;
+    pending_.load_from_history = false;
+  }
+
   if (pending_.load_key) {
     int i = this->key;
     d_->time = m_->key_time[i];
@@ -1846,8 +1914,7 @@ void Simulate::Sync() {
     mju_copy(d_->qvel, m_->key_qvel + i*m_->nv, m_->nv);
     mju_copy(d_->act, m_->key_act + i*m_->na, m_->na);
     mju_copy(d_->mocap_pos, m_->key_mpos + i*3*m_->nmocap, 3*m_->nmocap);
-    mju_copy(d_->mocap_quat, m_->key_mquat + i*4*m_->nmocap,
-              4*m_->nmocap);
+    mju_copy(d_->mocap_quat, m_->key_mquat + i*4*m_->nmocap, 4*m_->nmocap);
     mju_copy(d_->ctrl, m_->key_ctrl + i*m_->nu, m_->nu);
     mj_forward(m_, d_);
     update_profiler = true;
@@ -1940,7 +2007,7 @@ void Simulate::Sync() {
   }
 
   // update scene
-  if (fully_managed_) {
+  if (!is_passive_) {
     mjv_updateScene(m_, d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn);
   } else {
     mjv_updateSceneState(m_, d_, &this->opt, &scnstate_);
@@ -1966,7 +2033,7 @@ void Simulate::Sync() {
   // clear timers once profiler info has been copied
   ClearTimers(d_);
 
-  if (this->run || !this->fully_managed_) {
+  if (this->run || this->is_passive_) {
     // clear old perturbations, apply new
     mju_zero(d_->xfrc_applied, 6*m_->nbody);
     mjv_applyPerturbPose(m_, d_, &this->pert, 0);  // mocap bodies only
@@ -2080,8 +2147,32 @@ void Simulate::LoadOnRenderThread() {
   std::memcpy(ctrl_.data(), this->d_->ctrl, sizeof(this->d_->ctrl[0]) * this->m_->nu);
   ctrl_prev_ = ctrl_;
 
+  // allocate history buffer: smaller of {2000 states, 100 MB}
+  if (!this->is_passive_) {
+    constexpr int kHistoryLength = 2000;
+    constexpr int kMaxHistoryBytes = 1e8;
+
+    // get state size, size of history buffer
+    state_size_ = mj_stateSize(this->m_, mjSTATE_INTEGRATION);
+    int state_bytes = state_size_ * sizeof(mjtNum);
+    int history_bytes = mjMIN(state_bytes * kHistoryLength, kMaxHistoryBytes);
+    nhistory_ = history_bytes / state_bytes;
+
+    // allocate history buffer, reset cursor and UI slider
+    history_.clear();
+    history_.resize(nhistory_ * state_size_);
+    history_cursor_ = 0;
+    scrub_index = 0;
+
+    // fill buffer with initial state
+    mj_getState(this->m_, this->d_, history_.data(), mjSTATE_INTEGRATION);
+    for (int i = 1; i < nhistory_; ++i) {
+      mju_copy(&history_[i * state_size_], history_.data(), state_size_);
+    }
+  }
+
   // re-create scene and context
-  if (this->fully_managed_) {
+  if (!this->is_passive_) {
     mjv_makeScene(this->m_, &this->scn, kMaxGeom);
   } else {
     mjopt_prev_ = m_->opt;
@@ -2113,7 +2204,7 @@ void Simulate::LoadOnRenderThread() {
   }
 
   // update scene
-  if (fully_managed_) {
+  if (!is_passive_) {
     mjv_updateScene(this->m_, this->d_,
                     &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn);
   } else {
@@ -2132,6 +2223,10 @@ void Simulate::LoadOnRenderThread() {
   this->ui0.sect[SECT_SIMULATION].item[5].slider.range[1] = mjMAX(0, this->m_->nkey - 1);
   this->ui0.sect[SECT_SIMULATION].item[5].slider.divisions = mjMAX(1, this->m_->nkey - 1);
 
+  // set scrubber range and divisions
+  this->ui0.sect[SECT_SIMULATION].item[11].slider.range[0] = 1 - nhistory_;
+  this->ui0.sect[SECT_SIMULATION].item[11].slider.divisions = nhistory_;
+
   // rebuild UI sections
   MakeUiSections(this, this->m_, this->d_);
 
@@ -2179,7 +2274,7 @@ void Simulate::Render() {
   }
 
   // no model
-  if (this->fully_managed_ && !this->m_) {
+  if (!this->is_passive_ && !this->m_) {
     // blank screen
     mjr_rectangle(rect, 0.2f, 0.3f, 0.4f, 1);
 
@@ -2217,17 +2312,17 @@ void Simulate::Render() {
 
   // update UI sections from last sync
   if (this->ui0_enable && this->ui0.sect[SECT_WATCH].state) {
-    mjui_update(SECT_WATCH, -1, &this->ui0, &this->uistate, &this->platform_ui->mjr_context());
+    mjui0_update_section(this, SECT_WATCH);
   }
 
   if (pending_.ui_update_physics) {
     if (this->ui0_enable && this->ui0.sect[SECT_PHYSICS].state) {
-      mjui_update(SECT_PHYSICS, -1, &this->ui0, &this->uistate, &this->platform_ui->mjr_context());
+      mjui0_update_section(this, SECT_PHYSICS);
     }
     pending_.ui_update_physics = false;
   }
 
-  if (!fully_managed_) {
+  if (is_passive_) {
     if (this->ui0_enable && this->ui0.sect[SECT_RENDERING].state &&
         (cam_prev_.type != cam.type ||
          cam_prev_.fixedcamid != cam.fixedcamid ||
@@ -2244,8 +2339,7 @@ void Simulate::Render() {
          IsDifferent(opt_prev_.tendongroup, opt.tendongroup) ||
          IsDifferent(opt_prev_.actuatorgroup, opt.actuatorgroup) ||
          IsDifferent(opt_prev_.skingroup, opt.skingroup))) {
-      mjui_update(SECT_GROUP, -1, &this->ui0, &this->uistate,
-                  &this->platform_ui->mjr_context());
+      mjui0_update_section(this, SECT_GROUP);
     }
 
     opt_prev_ = opt;
@@ -2254,8 +2348,7 @@ void Simulate::Render() {
 
   if (pending_.ui_update_rendering) {
     if (this->ui0_enable && this->ui0.sect[SECT_RENDERING].state) {
-      mjui_update(SECT_RENDERING, -1, &this->ui0, &this->uistate,
-                  &this->platform_ui->mjr_context());
+      mjui0_update_section(this, SECT_RENDERING);
     }
     pending_.ui_update_rendering = false;
   }
@@ -2397,7 +2490,7 @@ void Simulate::RenderLoop() {
   InitializeSensor(this);
 
   // make empty scene
-  if (fully_managed_) {
+  if (!is_passive_) {
     mjv_defaultScene(&this->scn);
     mjv_makeScene(nullptr, &this->scn, kMaxGeom);
   }
@@ -2494,7 +2587,7 @@ void Simulate::RenderLoop() {
       }
 
       // update scene, doing a full sync if in fully managed mode
-      if (this->fully_managed_) {
+      if (!this->is_passive_) {
         Sync();
       } else {
         scnstate_.data.warning[mjWARN_VGEOMFULL].number += mjv_updateSceneFromState(
@@ -2516,7 +2609,7 @@ void Simulate::RenderLoop() {
     }
   }
 
-  if (fully_managed_){
+  if (!is_passive_){
     mjv_freeScene(&this->scn);
   } else {
     mjv_freeSceneState(&scnstate_);
@@ -2525,6 +2618,20 @@ void Simulate::RenderLoop() {
   this->exitrequest.store(2);
 }
 
+// add state to history buffer
+void Simulate::AddToHistory() {
+  if (history_.empty()) {
+    return;
+  }
+
+  // circular increment of cursor
+  history_cursor_ = (history_cursor_ + 1) % nhistory_;
+
+  // add state at cursor
+  mjtNum* state = &history_[state_size_ * history_cursor_];
+  mj_getState(m_, d_, state, mjSTATE_INTEGRATION);
+}
+
 void Simulate::UpdateHField(int hfieldid) {
   MutexLock lock(this->mtx);
   if (!m_ || hfieldid < 0 || hfieldid >= m_->nhfield) {

simulate/simulate.h

@@ -52,7 +52,7 @@ class Simulate {
   Simulate(
       std::unique_ptr<PlatformUIAdapter> platform_ui_adapter,
       mjvScene* scn, mjvCamera* cam,
-      mjvOption* opt, mjvPerturb* pert, bool fully_managed);
+      mjvOption* opt, mjvPerturb* pert, bool is_passive);
 
   // Synchronize mjModel and mjData state with UI inputs, and update
   // visualization.
@@ -84,12 +84,15 @@ class Simulate {
   // loop to render the UI (must be called from main thread because of MacOS)
   void RenderLoop();
 
+  // add state to history buffer
+  void AddToHistory();
+
   // constants
   static constexpr int kMaxFilenameLength = 1000;
 
-  // whether the viewer is operating in fully managed mode, where it can assume
+  // whether the viewer is operating in passive mode, where it cannot assume
   // that it has exclusive access to mjModel, mjData, and various mjv objects
-  bool fully_managed_ = true;
+  bool is_passive_ = false;
 
   // model and data to be visualized
   mjModel* mnew_ = nullptr;
@@ -100,6 +103,9 @@ class Simulate {
 
   int ncam_ = 0;
   int nkey_ = 0;
+  int state_size_ = 0;      // number of mjtNums in a history buffer state
+  int nhistory_ = 0;        // number of states saved in history buffer
+  int history_cursor_ = 0;  // cursor pointing at last saved state
 
   std::vector<int> body_parentid_;
 
@@ -113,6 +119,8 @@ class Simulate {
   std::vector<std::optional<std::pair<mjtNum, mjtNum>>> actuator_ctrlrange_;
   std::vector<std::string> actuator_names_;
 
+  std::vector<mjtNum> history_;  // history buffer (nhistory x state_size)
+
   // mjModel and mjData fields that can be modified by the user through the GUI
   std::vector<mjtNum> qpos_;
   std::vector<mjtNum> qpos_prev_;
@@ -134,6 +142,7 @@ class Simulate {
     bool reset;
     bool align;
     bool copy_pose;
+    bool load_from_history;
     bool load_key;
     bool save_key;
     bool zero_ctrl;
@@ -171,6 +180,9 @@ class Simulate {
   // keyframe index
   int key = 0;
 
+  // index of history-scrubber slider
+  int scrub_index = 0;
+
   // simulation
   int run = 1;
 
@@ -266,7 +278,7 @@ class Simulate {
 
 
   // simulation section of UI
-  const mjuiDef def_simulation[12] = {
+  const mjuiDef def_simulation[14] = {
     {mjITEM_SECTION,   "Simulation",    1, nullptr,              "AS"},
     {mjITEM_RADIO,     "",              5, &this->run,           "Pause\nRun"},
     {mjITEM_BUTTON,    "Reset",         2, nullptr,              " #259"},
@@ -278,6 +290,8 @@ class Simulate {
     {mjITEM_BUTTON,    "Save key",      3},
     {mjITEM_SLIDERNUM, "Noise scale",   5, &this->ctrl_noise_std,  "0 2"},
     {mjITEM_SLIDERNUM, "Noise rate",    5, &this->ctrl_noise_rate, "0 2"},
+    {mjITEM_SEPARATOR, "History",       1},
+    {mjITEM_SLIDERINT, "",              5, &this->scrub_index,     "0 0"},
     {mjITEM_END}
   };
 

src/engine/engine_collision_driver.c

@@ -14,6 +14,7 @@
 
 #include "engine/engine_collision_driver.h"
 
+#include <math.h>
 #include <stddef.h>
 #include <string.h>
 
@@ -61,8 +62,10 @@ static void collideGeoms(const mjModel* m, mjData* d,
 static inline void resetArena(mjData* d) {
   d->parena = d->ncon * sizeof(mjContact);
 #ifdef ADDRESS_SANITIZER
-  ASAN_POISON_MEMORY_REGION(
-      (char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
+  if (!d->threadpool) {
+    ASAN_POISON_MEMORY_REGION(
+        (char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
+  }
 #endif
 }
 

src/engine/engine_core_constraint.c

@@ -521,7 +521,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
 
   // find active equality constraints
   for (int i=0; i < m->neq; i++) {
-    if (m->eq_active[i]) {
+    if (d->eq_active[i]) {
       // get constraint data
       data = m->eq_data + mjNEQDATA*i;
       id[0] = m->eq_obj1id[i];
@@ -1481,7 +1481,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
 
   // find active equality constraints
   for (int i=0; i < neq; i++) {
-    if (m->eq_active[i]) {
+    if (d->eq_active[i]) {
       id[0] = m->eq_obj1id[i];
       id[1] = m->eq_obj2id[i];
       size = 0;

src/engine/engine_forward.c

@@ -40,6 +40,8 @@
 #include "engine/engine_util_misc.h"
 #include "engine/engine_util_solve.h"
 #include "engine/engine_util_sparse.h"
+#include "thread/thread_pool.h"
+#include "thread/thread_task.h"
 
 
 
@@ -492,6 +494,52 @@ static void warmstart(const mjModel* m, mjData* d) {
 
 
 
+// struct encapsulating arguments to thread task
+struct mjSolIslandArgs_ {
+  const mjModel* m;
+  mjData* d;
+  int island;
+};
+typedef struct mjSolIslandArgs_ mjSolIslandArgs;
+
+// extract arguments, pass to solver
+void* mj_solCG_island_wrapper(void* args) {
+  mjSolIslandArgs* solargs = (mjSolIslandArgs*) args;
+  mj_solCG_island(solargs->m, solargs->d, solargs->island, solargs->m->opt.iterations);
+  return NULL;
+}
+
+
+
+
+// CG solver, multi-threaded over islands
+void mj_solCG_island_multithreaded(const mjModel* m, mjData* d) {
+  mj_markStack(d);
+  // allocate array of arguments to be passed to threads
+  mjSolIslandArgs* sol_cg_island_args =
+      mj_stackAllocByte(d, sizeof(mjSolIslandArgs) * d->nisland, _Alignof(mjSolIslandArgs));
+  mjTask* tasks = mj_stackAllocByte(d, sizeof(mjTask)  * d->nisland, _Alignof(mjTask));
+
+  for (int island = 0; island < d->nisland; ++island) {
+    sol_cg_island_args[island].m = m;
+    sol_cg_island_args[island].d = d;
+    sol_cg_island_args[island].island = island;
+
+    mju_defaultTask(&tasks[island]);
+    tasks[island].func = mj_solCG_island_wrapper;
+    tasks[island].args = &sol_cg_island_args[island];
+    mju_threadPoolEnqueue((mjThreadPool*)d->threadpool, &tasks[island]);
+  }
+
+  for (int island = 0; island < d->nisland; ++island) {
+    mju_taskJoin(&tasks[island]);
+  }
+
+  mj_freeStack(d);
+}
+
+
+
 // compute efc_b, efc_force, qfrc_constraint; update qacc
 void mj_fwdConstraint(const mjModel* m, mjData* d) {
   TM_START;
@@ -524,9 +572,15 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
 
   // run solver over constraint islands
   if (islands_supported) {
-    // loop over islands
-    for (int island=0; island < nisland; island++) {
-      mj_solCG_island(m, d, island, m->opt.iterations);
+    // no threadpool, loop over islands
+    if (!d->threadpool) {
+      for (int island=0; island < nisland; island++) {
+        mj_solCG_island(m, d, island, m->opt.iterations);
+      }
+    }
+    else {
+      // solve using threads
+      mj_solCG_island_multithreaded(m, d);
     }
     d->solver_nisland = nisland;
   }

src/engine/engine_io.c

@@ -1470,7 +1470,9 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
   //------------------------------ clear header
 
   // clear stack pointer
-  d->pstack = 0;
+  if (!d->threadpool) {
+    d->pstack = 0;
+  }
   d->pbase = 0;
 
   // clear arena pointers
@@ -1549,6 +1551,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
   mju_zero(d->qvel, m->nv);
   mju_zero(d->act, m->na);
   mju_zero(d->ctrl, m->nu);
+  for (int i=0; i<m->neq; i++) d->eq_active[i] = m->eq_active0[i];
   mju_zero(d->qfrc_applied, m->nv);
   mju_zero(d->xfrc_applied, 6*m->nbody);
   mju_zero(d->qacc, m->nv);

src/engine/engine_print.c

@@ -880,6 +880,13 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
   printArray("CTRL", m->nu, 1, d->ctrl, fp, float_format);
   printArray("QFRC_APPLIED", m->nv, 1, d->qfrc_applied, fp, float_format);
   printArray("XFRC_APPLIED", m->nbody, 6, d->xfrc_applied, fp, float_format);
+  if (m->neq) {
+    fprintf(fp, NAME_FORMAT, "EQ_ACTIVE");
+    for (int c=0; c < m->neq; c++) {
+      fprintf(fp, " %d", d->eq_active[c]);
+    }
+    fprintf(fp, "\n\n");
+  }
   printArray("MOCAP_POS", m->nmocap, 3, d->mocap_pos, fp, float_format);
   printArray("MOCAP_QUAT", m->nmocap, 4, d->mocap_quat, fp, float_format);
   printArray("QACC", m->nv, 1, d->qacc, fp, float_format);

src/engine/engine_support.c

@@ -105,6 +105,7 @@ static inline int mj_stateElemSize(const mjModel* m, mjtState spec) {
     case mjSTATE_CTRL:          return m->nu;
     case mjSTATE_QFRC_APPLIED:  return m->nv;
     case mjSTATE_XFRC_APPLIED:  return 6*m->nbody;
+    case mjSTATE_EQ_ACTIVE:     return m->neq;  // mjtByte, stored as mjtNum in state vector
     case mjSTATE_MOCAP_POS:     return 3*m->nmocap;
     case mjSTATE_MOCAP_QUAT:    return 4*m->nmocap;
     case mjSTATE_USERDATA:      return m->nuserdata;
@@ -176,9 +177,21 @@ void mj_getState(const mjModel* m, const mjData* d, mjtNum* state, unsigned int
     mjtState element = 1<<i;
     if (element & spec) {
       int size = mj_stateElemSize(m, element);
-      const mjtNum* ptr = mj_stateElemConstPtr(m, d, element);
-      mju_copy(state + adr, ptr, size);
-      adr += size;
+
+      // special handling of eq_active (mjtByte)
+      if (element == mjSTATE_EQ_ACTIVE) {
+        int neq = m->neq;
+        for (int j=0; j < neq; j++) {
+          state[adr++] = d->eq_active[j];
+        }
+      }
+
+      // regular state components (mjtNum)
+      else {
+        const mjtNum* ptr = mj_stateElemConstPtr(m, d, element);
+        mju_copy(state + adr, ptr, size);
+        adr += size;
+      }
     }
   }
 }
@@ -196,9 +209,21 @@ void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, unsigned int
     mjtState element = 1<<i;
     if (element & spec) {
       int size = mj_stateElemSize(m, element);
-      mjtNum* ptr = mj_stateElemPtr(m, d, element);
-      mju_copy(ptr, state + adr, size);
-      adr += size;
+
+      // special handling of eq_active (mjtByte)
+      if (element == mjSTATE_EQ_ACTIVE) {
+        int neq = m->neq;
+        for (int j=0; j < neq; j++) {
+          d->eq_active[j] = state[adr++];
+        }
+      }
+
+      // regular state components (mjtNum)
+      else {
+        mjtNum* ptr = mj_stateElemPtr(m, d, element);
+        mju_copy(ptr, state + adr, size);
+        adr += size;
+      }
     }
   }
 }

src/engine/engine_util_solve.c

@@ -708,7 +708,7 @@ void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n,
 
 // eigenvalue decomposition of symmetric 3x3 matrix
 static const mjtNum eigEPS = 1E-12;
-int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]) {
+int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum mat[9]) {
   mjtNum D[9], tmp[9];
   mjtNum tau, t, c;
   int iter, rk, ck, rotk;

src/engine/engine_util_solve.h

@@ -84,7 +84,7 @@ void mju_solveLUSparse(mjtNum *res, const mjtNum *LU, const mjtNum* vec, int n,
                        const int *rownnz, const int *rowadr, const int *colind);
 
 // eigenvalue decomposition of symmetric 3x3 matrix
-MJAPI int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]);
+MJAPI int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum mat[9]);
 
 // solve QCQP in 2 dimensions:
 //  min  0.5*x'*A*x + x'*b  s.t.  sum (xi/di)^2 <= r^2

src/engine/engine_vis_visualize.c

@@ -1908,7 +1908,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
   if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) {
     // connect or weld
     for (int i=0; i < m->neq; i++) {
-      if (m->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) {
+      if (d->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) {
         // compute endpoints in global coordinates
         int j = m->eq_obj1id[i], k = m->eq_obj2id[i];
         mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_WELD), d->xmat+9*j);

src/thread/thread_pool.cc

@@ -206,7 +206,10 @@ mjStackInfo* mju_getStackInfoForThread(mjData* d, size_t thread_id) {
   // align the end of the shard to be mjStackInfo.
   misalignment = result % alignof(mjStackInfo);
   result -= misalignment;
-
+#ifdef ADDRESS_SANITIZER
+  // Ensure StackInfo is always accessible
+  ASAN_UNPOISON_MEMORY_REGION((void*)result, sizeof(mjStackInfo));
+#endif
   return (mjStackInfo*) result;
 }
 
@@ -253,7 +256,7 @@ static void  ConfigureMultiThreadedStack(mjData* d) {
 }
 
 // adds a thread pool to mjData and configures it for multi-threaded use.
-void mju_bindThreadPool(mjData* d, mjThreadPool* thread_pool) {
+void mju_bindThreadPool(mjData* d, void* thread_pool) {
   if (d->threadpool) {
     mju_error("Thread Pool already bound to mjData");
   }

src/thread/thread_pool.h

@@ -54,7 +54,7 @@ MJAPI mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
 mjStackInfo* mju_getStackInfoForThread(mjData* d, size_t thread_id);
 
 // Adds a thread pool to mjData and configures it for multi-threaded use.
-MJAPI void mju_bindThreadPool(mjData* d, mjThreadPool* thread_pool);
+MJAPI void mju_bindThreadPool(mjData* d, void* thread_pool);
 
 // Gets the number of running threads in the thread pool.
 MJAPI size_t mju_threadPoolNumberOfThreads(mjThreadPool* thread_pool);

src/thread/thread_task.h

@@ -15,14 +15,13 @@
 #ifndef MUJOCO_SRC_THREAD_THREAD_TASK_H_
 #define MUJOCO_SRC_THREAD_THREAD_TASK_H_
 
-#include <atomic>
-#include <new>
-#include <type_traits>
-
 #include <mujoco/mjexport.h>
 #include <mujoco/mjthread.h>
 
 #ifdef __cplusplus
+#include <atomic>
+#include <new>
+#include <type_traits>
 namespace mujoco {
 extern "C" {
 #endif

src/user/user_model.cc

@@ -1950,7 +1950,7 @@ void mjCModel::CopyObjects(mjModel* m) {
     m->eq_type[i] = peq->type;
     m->eq_obj1id[i] = peq->obj1id;
     m->eq_obj2id[i] = peq->obj2id;
-    m->eq_active[i] = peq->active;
+    m->eq_active0[i] = peq->active;
     copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF);
     copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP);
     copyvec(m->eq_data+mjNEQDATA*i, peq->data, mjNEQDATA);

test/benchmark/CMakeLists.txt

@@ -48,6 +48,14 @@ target_link_libraries(
   benchmark::benchmark
 )
 
+mujoco_benchmark_test(thread_performance_test)
+target_link_libraries(
+  thread_performance_test
+  fixture
+  gmock
+  benchmark::benchmark
+)
+
 mujoco_benchmark_test(parse_benchmark_test)
 target_link_libraries(
   parse_benchmark_test

test/benchmark/step_benchmark_test.cc

@@ -46,23 +46,21 @@ static void run_step_benchmark(const mjModel* model, benchmark::State& state) {
   int nsteps = kNumWarmupSteps+kNumBenchmarkSteps;
   std::vector<mjtNum> ctrl = GetCtrlNoise(model, nsteps);
 
-  // warm-up rollout to get a typcal state
+  // warm-up rollout to get a typical state
   for (int i=0; i < kNumWarmupSteps; i++) {
     mju_copy(data->ctrl, ctrl.data()+model->nu*i, model->nu);
     mj_step(model, data);
   }
+
   // save state
-  std::vector<mjtNum> qpos = AsVector(data->qpos, model->nq);
-  std::vector<mjtNum> qvel = AsVector(data->qvel, model->nv);
-  std::vector<mjtNum> act = AsVector(data->act, model->na);
-  std::vector<mjtNum> warmstart = AsVector(data->qacc_warmstart, model->nv);
+  int spec = mjSTATE_INTEGRATION;
+  int size = mj_stateSize(model, spec);
+  std::vector<mjtNum> initial_state(size);
+  mj_getState(model, data, initial_state.data(), spec);
 
   // reset state, benchmark subsequent kNumBenchmarkSteps steps
   while (state.KeepRunningBatch(kNumBenchmarkSteps)) {
-    mju_copy(data->qpos, qpos.data(), model->nq);
-    mju_copy(data->qvel, qvel.data(), model->nv);
-    mju_copy(data->act, act.data(), model->na);
-    mju_copy(data->qacc_warmstart, warmstart.data(), model->nv);
+    mj_setState(model, data, initial_state.data(), spec);
 
     for (int i=kNumWarmupSteps; i < nsteps; i++) {
       mju_copy(data->ctrl, ctrl.data()+model->nu*i, model->nu);

test/benchmark/testdata/humanoid200.xml

@@ -0,0 +1,913 @@
+<!-- Copyright 2023 DeepMind Technologies Limited
+
+     Licensed under the Apache License, Version 2.0 (the "License");
+     you may not use this file except in compliance with the License.
+     You may obtain a copy of the License at
+
+         http://www.apache.org/licenses/LICENSE-2.0
+
+     Unless required by applicable law or agreed to in writing, software
+     distributed under the License is distributed on an "AS IS" BASIS,
+     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+     See the License for the specific language governing permissions and
+     limitations under the License.
+-->
+
+<mujoco model="Humanoid and 200 objects">
+<!-- Degree of Freedom: 627
+     Actuators: 21
+
+     Humanoid and 200 free primitive objects.
+-->
+  <include file="humanoid_body.xml"/>
+
+  <option timestep="0.005"/>
+
+  <size memory="1000M"/>
+
+  <default>
+    <geom solimp=".9 .9 .01"/>
+    <default class="object1">
+      <geom type="capsule" material="object1" size="0.1 0.05"/>
+    </default>
+    <default class="object2">
+      <geom type="ellipsoid" material="object2" size="0.15 0.1 0.07"/>
+    </default>
+    <default class="object3">
+      <geom type="box" material="object3" size="0.15 0.1 0.05"/>
+    </default>
+    <default class="object4">
+      <geom type="cylinder" material="object4" size="0.1 0.05" condim="4" friction="1 .01 .01"/>
+    </default>
+    <default class="object5">
+      <geom type="sphere" material="object5" size="0.1"/>
+    </default>
+    <default class="border">
+      <geom type="capsule" size="0.4" rgba=".4 .4 .4 1"/>
+    </default>
+    <default class="borderpost">
+      <geom type="box" size="0.41 0.41 0.41" rgba=".55 .55 .55 1"/>
+    </default>
+  </default>
+
+  <asset>
+    <texture type="skybox" builtin="gradient" width="512" height="512" rgb1=".4 .6 .8" rgb2="0 0 0"/>
+    <texture name="texgeom" type="cube" builtin="flat" mark="cross" width="128" height="128"
+             rgb1="0.6 0.6 0.6" rgb2="0.6 0.6 0.6" markrgb="1 1 1"/>
+    <texture name="texplane" type="2d" builtin="checker" rgb1=".4 .4 .4" rgb2=".6 .6 .6"
+             width="512" height="512"/>
+    <material name='MatPlane' reflectance='0.3' texture="texplane" texrepeat="1 1" texuniform="true"
+              rgba=".7 .7 .7 1"/>
+    <material name='object1' texture="texgeom" texuniform="true" rgba=".4 .9 .6 1" />
+    <material name='object2' texture="texgeom" texuniform="true" rgba=".4 .6 .9 1" />
+    <material name='object3' texture="texgeom" texuniform="true" rgba=".4 .9 .9 1" />
+    <material name='object4' texture="texgeom" texuniform="true" rgba=".8 .6 .8 1" />
+    <material name='object5' texture="texgeom" texuniform="true" rgba=".9 .1 .1 1" />
+  </asset>
+
+  <visual>
+    <quality shadowsize="4096" offsamples="8"/>
+    <map znear="0.1" force="0.05"/>
+  </visual>
+
+  <statistic extent="4"/>
+
+  <worldbody>
+    <light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
+    <geom name="floor" type="plane" size="3 3 .5" material="MatPlane"/>
+    <geom class="border" fromto="-3 3 0 3 3 0"  />
+    <geom class="border" fromto="-3 -3 0 3 -3 0"  />
+    <geom class="border" fromto="3 3 0 3 -3 0"  />
+    <geom class="border" fromto="-3 3 0 -3 -3 0"  />
+    <geom class="borderpost" pos="3 3 0"/>
+    <geom class="borderpost" pos="-3 3 0"/>
+    <geom class="borderpost" pos="3 -3 0"/>
+    <geom class="borderpost" pos="-3 -3 0"/>
+
+    <body pos="-2 -2 1" quat="-0.632456 -0.632456 0.316228 0.316228">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -2 2" quat="-0.5547 -0.5547 0.5547 0.27735">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -2 3" quat="-0.471405 -0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -2 4" quat="-0.471405 -0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -2 5" quat="-0.471405 -0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -2 6" quat="-0.471405 -0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -2 7" quat="-0.471405 -0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -2 8" quat="-0.471405 -0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+
+    <body pos="-2 -1 1" quat="-0.755929 -0.377964 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -1 2" quat="-0.632456 -0.316228 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -1 3" quat="-0.516398 -0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -1 4" quat="-0.516398 -0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -1 5" quat="-0.755929 -0.377964 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -1 6" quat="-0.632456 -0.316228 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -1 7" quat="-0.516398 -0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 -1 8" quat="-0.516398 -0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+
+    <body pos="-2 0 1" quat="-0.816497 0 0.408248 0.408248">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 0 2" quat="-0.666667 0 0.666667 0.333333">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 0 3" quat="-0.534522 0 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 0 4" quat="-0.534522 0 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 0 5" quat="-0.816497 0 0.408248 0.408248">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 0 6" quat="-0.666667 0 0.666667 0.333333">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 0 7" quat="-0.534522 0 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 0 8" quat="-0.534522 0 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+
+    <body pos="-2 1 1" quat="-0.755929 0.377964 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 1 2" quat="-0.632456 0.316228 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 1 3" quat="-0.516398 0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 1 4" quat="-0.516398 0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 1 5" quat="-0.755929 0.377964 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 1 6" quat="-0.632456 0.316228 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 1 7" quat="-0.516398 0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 1 8" quat="-0.516398 0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+
+    <body pos="-2 2 1" quat="-0.632456 0.632456 0.316228 0.316228">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 2 2" quat="-0.5547 0.5547 0.5547 0.27735">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 2 3" quat="-0.471405 0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 2 4" quat="-0.471405 0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 2 5" quat="-0.632456 0.632456 0.316228 0.316228">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 2 6" quat="-0.5547 0.5547 0.5547 0.27735">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 2 7" quat="-0.471405 0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+    <body pos="-2 2 8" quat="-0.471405 0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object1"/>
+    </body>
+
+    <body pos="-1 -2 1" quat="-0.377964 -0.755929 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -2 2" quat="-0.316228 -0.632456 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -2 3" quat="-0.258199 -0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -2 4" quat="-0.258199 -0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -2 5" quat="-0.377964 -0.755929 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -2 6" quat="-0.316228 -0.632456 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -2 7" quat="-0.258199 -0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -2 8" quat="-0.258199 -0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+
+    <body pos="-1 -1 1" quat="-0.5 -0.5 0.5 0.5">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -1 2" quat="-0.377964 -0.377964 0.755929 0.377964">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -1 3" quat="-0.288675 -0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -1 4" quat="-0.288675 -0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -1 5" quat="-0.5 -0.5 0.5 0.5">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -1 6" quat="-0.377964 -0.377964 0.755929 0.377964">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -1 7" quat="-0.288675 -0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 -1 8" quat="-0.288675 -0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+
+    <body pos="-1 0 1" quat="-0.57735 0 0.57735 0.57735">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 0 2" quat="-0.408248 0 0.816497 0.408248">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 0 3" quat="-0.301511 0 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 0 4" quat="-0.301511 0 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 0 5" quat="-0.57735 0 0.57735 0.57735">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 0 6" quat="-0.408248 0 0.816497 0.408248">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 0 7" quat="-0.301511 0 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 0 8" quat="-0.301511 0 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+
+    <body pos="-1 1 1" quat="-0.5 0.5 0.5 0.5">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 1 2" quat="-0.377964 0.377964 0.755929 0.377964">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 1 3" quat="-0.288675 0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 1 4" quat="-0.288675 0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 1 5" quat="-0.5 0.5 0.5 0.5">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 1 6" quat="-0.377964 0.377964 0.755929 0.377964">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 1 7" quat="-0.288675 0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 1 8" quat="-0.288675 0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+
+    <body pos="-1 2 1" quat="-0.377964 0.755929 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 2 2" quat="-0.316228 0.632456 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 2 3" quat="-0.258199 0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 2 4" quat="-0.258199 0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 2 5" quat="-0.377964 0.755929 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 2 6" quat="-0.316228 0.632456 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 2 7" quat="-0.258199 0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+    <body pos="-1 2 8" quat="-0.258199 0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object2"/>
+    </body>
+
+    <body pos="0 -2 1" quat="0 -0.816497 0.408248 0.408248">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -2 2" quat="0 -0.666667 0.666667 0.333333">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -2 3" quat="0 -0.534522 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -2 4" quat="0 -0.534522 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -2 5" quat="0 -0.816497 0.408248 0.408248">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -2 6" quat="0 -0.666667 0.666667 0.333333">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -2 7" quat="0 -0.534522 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -2 8" quat="0 -0.534522 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+
+    <body pos="0 -1 1" quat="0 -0.57735 0.57735 0.57735">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -1 2" quat="0 -0.408248 0.816497 0.408248">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -1 3" quat="0 -0.301511 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -1 4" quat="0 -0.301511 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -1 5" quat="0 -0.57735 0.57735 0.57735">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -1 6" quat="0 -0.408248 0.816497 0.408248">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -1 7" quat="0 -0.301511 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 -1 8" quat="0 -0.301511 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+
+    <body pos="0.3 0 1" quat="0 0 0.707107 0.707107">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 0 2" quat="0 0 0.894427 0.447214">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 0 3" quat="0 0 0.948683 0.316228">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 0 4" quat="0 0 0.948683 0.316228">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0.3 0 5" quat="0 0 0.707107 0.707107">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 0 6" quat="0 0 0.894427 0.447214">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 0 7" quat="0 0 0.948683 0.316228">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 0 8" quat="0 0 0.948683 0.316228">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+
+    <body pos="0 1 1" quat="0 0.57735 0.57735 0.57735">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 1 2" quat="0 0.408248 0.816497 0.408248">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 1 3" quat="0 0.301511 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 1 4" quat="0 0.301511 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 1 5" quat="0 0.57735 0.57735 0.57735">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 1 6" quat="0 0.408248 0.816497 0.408248">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 1 7" quat="0 0.301511 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 1 8" quat="0 0.301511 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+
+    <body pos="0 2 1" quat="0 0.816497 0.408248 0.408248">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 2 2" quat="0 0.666667 0.666667 0.333333">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 2 3" quat="0 0.534522 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 2 4" quat="0 0.534522 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 2 5" quat="0 0.816497 0.408248 0.408248">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 2 6" quat="0 0.666667 0.666667 0.333333">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 2 7" quat="0 0.534522 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+    <body pos="0 2 8" quat="0 0.534522 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object3"/>
+    </body>
+
+    <body pos="1 -2 1" quat="0.377964 -0.755929 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -2 2" quat="0.316228 -0.632456 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -2 3" quat="0.258199 -0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -2 4" quat="0.258199 -0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -2 5" quat="0.377964 -0.755929 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -2 6" quat="0.316228 -0.632456 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -2 7" quat="0.258199 -0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -2 8" quat="0.258199 -0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+
+    <body pos="1 -1 1" quat="0.5 -0.5 0.5 0.5">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -1 2" quat="0.377964 -0.377964 0.755929 0.377964">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -1 3" quat="0.288675 -0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -1 4" quat="0.288675 -0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -1 5" quat="0.5 -0.5 0.5 0.5">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -1 6" quat="0.377964 -0.377964 0.755929 0.377964">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -1 7" quat="0.288675 -0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 -1 8" quat="0.288675 -0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+
+    <body pos="1 0 1" quat="0.57735 0 0.57735 0.57735">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 0 2" quat="0.408248 0 0.816497 0.408248">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 0 3" quat="0.301511 0 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 0 4" quat="0.301511 0 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 0 5" quat="0.57735 0 0.57735 0.57735">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 0 6" quat="0.408248 0 0.816497 0.408248">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 0 7" quat="0.301511 0 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 0 8" quat="0.301511 0 0.904534 0.301511">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+
+    <body pos="1 1 1" quat="0.5 0.5 0.5 0.5">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 1 2" quat="0.377964 0.377964 0.755929 0.377964">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 1 3" quat="0.288675 0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 1 4" quat="0.288675 0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 1 5" quat="0.5 0.5 0.5 0.5">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 1 6" quat="0.377964 0.377964 0.755929 0.377964">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 1 7" quat="0.288675 0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 1 8" quat="0.288675 0.288675 0.866025 0.288675">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+
+    <body pos="1 2 1" quat="0.377964 0.755929 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 2 2" quat="0.316228 0.632456 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 2 3" quat="0.258199 0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 2 4" quat="0.258199 0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 2 5" quat="0.377964 0.755929 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 2 6" quat="0.316228 0.632456 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 2 7" quat="0.258199 0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+    <body pos="1 2 8" quat="0.258199 0.516398 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object4"/>
+    </body>
+
+    <body pos="2 -2 1" quat="0.632456 -0.632456 0.316228 0.316228">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -2 2" quat="0.5547 -0.5547 0.5547 0.27735">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -2 3" quat="0.471405 -0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -2 4" quat="0.471405 -0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -2 5" quat="0.632456 -0.632456 0.316228 0.316228">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -2 6" quat="0.5547 -0.5547 0.5547 0.27735">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -2 7" quat="0.471405 -0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -2 8" quat="0.471405 -0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+
+    <body pos="2 -1 1" quat="0.755929 -0.377964 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -1 2" quat="0.632456 -0.316228 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -1 3" quat="0.516398 -0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -1 4" quat="0.516398 -0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -1 5" quat="0.755929 -0.377964 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -1 6" quat="0.632456 -0.316228 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -1 7" quat="0.516398 -0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 -1 8" quat="0.516398 -0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+
+    <body pos="2 0 1" quat="0.816497 0 0.408248 0.408248">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 0 2" quat="0.666667 0 0.666667 0.333333">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 0 3" quat="0.534522 0 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 0 4" quat="0.534522 0 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 0 5" quat="0.816497 0 0.408248 0.408248">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 0 6" quat="0.666667 0 0.666667 0.333333">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 0 7" quat="0.534522 0 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 0 8" quat="0.534522 0 0.801784 0.267261">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+
+    <body pos="2 1 1" quat="0.755929 0.377964 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 1 2" quat="0.632456 0.316228 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 1 3" quat="0.516398 0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 1 4" quat="0.516398 0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 1 5" quat="0.755929 0.377964 0.377964 0.377964">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 1 6" quat="0.632456 0.316228 0.632456 0.316228">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 1 7" quat="0.516398 0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 1 8" quat="0.516398 0.258199 0.774597 0.258199">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+
+    <body pos="2 2 1" quat="0.632456 0.632456 0.316228 0.316228">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 2 2" quat="0.5547 0.5547 0.5547 0.27735">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 2 3" quat="0.471405 0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 2 4" quat="0.471405 0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 2 5" quat="0.632456 0.632456 0.316228 0.316228">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 2 6" quat="0.5547 0.5547 0.5547 0.27735">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 2 7" quat="0.471405 0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+    <body pos="2 2 8" quat="0.471405 0.471405 0.707107 0.235702">
+      <freejoint/>
+      <geom class="object5"/>
+    </body>
+
+  </worldbody>
+</mujoco>

test/benchmark/testdata/humanoid_body.xml

@@ -0,0 +1,158 @@
+<!-- Copyright 2023 DeepMind Technologies Limited
+     Licensed under the Apache License, Version 2.0 (the "License");
+     you may not use this file except in compliance with the License.
+     You may obtain a copy of the License at
+         http://www.apache.org/licenses/LICENSE-2.0
+     Unless required by applicable law or agreed to in writing, software
+     distributed under the License is distributed on an "AS IS" BASIS,
+     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+     See the License for the specific language governing permissions and
+     limitations under the License.
+-->
+
+<mujoco model="Humanoid body">
+<!-- Degree of Freedom: 27
+     Actuators: 21
+
+     This simplified humanoid model, introduced in [1], is designed for bipedal locomotion
+     behaviours. While several variants of it exist in the wild, this version is based on the model
+     in the DeepMind Control Suite [2], which has fairly realistic actuator gains.
+     [1] Synthesis and Stabilization of Complex Behaviors through Online Trajectory Optimization.
+           https://doi.org/10.1109/IROS.2012.6386025
+     [2] DeepMind Control Suite, Tassa et al. https://arxiv.org/abs/1801.00690
+-->
+
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
+    <texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128"
+             rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
+    <material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
+    <texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
+    <material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
+  </asset>
+
+  <default>
+    <motor ctrlrange="-1 1" ctrllimited="true"/>
+    <default class="body">
+      <geom type="capsule" condim="1" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body"/>
+      <joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
+      <default class="big_joint">
+        <joint damping="5" stiffness="10"/>
+        <default class="big_stiff_joint">
+          <joint stiffness="20"/>
+        </default>
+      </default>
+    </default>
+  </default>
+
+  <visual>
+    <map force="0.1" zfar="30"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <quality shadowsize="4096"/>
+    <global offwidth="800" offheight="800"/>
+  </visual>
+
+  <worldbody>
+    <body name="torso" pos="0 0 1.5" childclass="body">
+      <light name="top" pos="0 0 2" mode="trackcom"/>
+      <camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
+      <camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
+      <freejoint name="root"/>
+      <geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
+      <geom name="upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
+      <body name="head" pos="0 0 .19">
+        <geom name="head" type="sphere" size=".09"/>
+        <camera name="egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
+      </body>
+      <body name="lower_waist" pos="-.01 0 -.26">
+        <geom name="lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
+        <joint name="abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
+        <joint name="abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
+        <body name="pelvis" pos="0 0 -.165">
+          <joint name="abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
+          <geom name="butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
+          <body name="right_thigh" pos="0 -.1 -.04">
+            <joint name="right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
+            <joint name="right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
+            <joint name="right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
+            <geom name="right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
+            <body name="right_shin" pos="0 .01 -.403">
+              <joint name="right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
+              <geom name="right_shin" fromto="0 0 0 0 0 -.3"  size=".049"/>
+              <body name="right_foot" pos="0 0 -.39">
+                <joint name="right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
+                <joint name="right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
+                <geom name="right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
+                <geom name="left_right_foot" fromto="-.07 0 0 .14  .02 0" size=".027"/>
+              </body>
+            </body>
+          </body>
+          <body name="left_thigh" pos="0 .1 -.04">
+            <joint name="left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
+            <joint name="left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
+            <joint name="left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
+            <geom name="left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
+            <body name="left_shin" pos="0 -.01 -.403">
+              <joint name="left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
+              <geom name="left_shin" fromto="0 0 0 0 0 -.3"  size=".049"/>
+              <body name="left_foot" pos="0 0 -.39">
+                <joint name="left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
+                <joint name="left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
+                <geom name="left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
+                <geom name="right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="right_upper_arm" pos="0 -.17 .06">
+        <joint name="right_shoulder1" axis="2 1 1"  range="-85 60"/>
+        <joint name="right_shoulder2" axis="0 -1 1" range="-85 60"/>
+        <geom name="right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
+        <body name="right_lower_arm" pos=".18 -.18 -.18">
+          <joint name="right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
+          <geom name="right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
+          <body name="right_hand" pos=".18 .18 .18">
+            <geom name="right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
+          </body>
+        </body>
+      </body>
+      <body name="left_upper_arm" pos="0 .17 .06">
+        <joint name="left_shoulder1" axis="2 -1 1" range="-60 85"/>
+        <joint name="left_shoulder2" axis="0 1 1"  range="-60 85"/>
+        <geom name="left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
+        <body name="left_lower_arm" pos=".18 .18 -.18">
+          <joint name="left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
+          <geom name="left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
+          <body name="left_hand" pos=".18 -.18 .18">
+            <geom name="left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <motor name="abdomen_y"       gear="40"  joint="abdomen_y"/>
+    <motor name="abdomen_z"       gear="40"  joint="abdomen_z"/>
+    <motor name="abdomen_x"       gear="40"  joint="abdomen_x"/>
+    <motor name="right_hip_x"     gear="40"  joint="right_hip_x"/>
+    <motor name="right_hip_z"     gear="40"  joint="right_hip_z"/>
+    <motor name="right_hip_y"     gear="120" joint="right_hip_y"/>
+    <motor name="right_knee"      gear="80"  joint="right_knee"/>
+    <motor name="right_ankle_x"   gear="20"  joint="right_ankle_x"/>
+    <motor name="right_ankle_y"   gear="20"  joint="right_ankle_y"/>
+    <motor name="left_hip_x"      gear="40"  joint="left_hip_x"/>
+    <motor name="left_hip_z"      gear="40"  joint="left_hip_z"/>
+    <motor name="left_hip_y"      gear="120" joint="left_hip_y"/>
+    <motor name="left_knee"       gear="80"  joint="left_knee"/>
+    <motor name="left_ankle_x"    gear="20"  joint="left_ankle_x"/>
+    <motor name="left_ankle_y"    gear="20"  joint="left_ankle_y"/>
+    <motor name="right_shoulder1" gear="20"  joint="right_shoulder1"/>
+    <motor name="right_shoulder2" gear="20"  joint="right_shoulder2"/>
+    <motor name="right_elbow"     gear="40"  joint="right_elbow"/>
+    <motor name="left_shoulder1"  gear="20"  joint="left_shoulder1"/>
+    <motor name="left_shoulder2"  gear="20"  joint="left_shoulder2"/>
+    <motor name="left_elbow"      gear="40"  joint="left_elbow"/>
+  </actuator>
+</mujoco>

test/benchmark/thread_performance_test.cc

@@ -0,0 +1,110 @@
+// Copyright 2021 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include <array>
+#include <vector>
+
+#include <benchmark/benchmark.h>
+#include <mujoco/mjmodel.h>
+#include <mujoco/mjthread.h>
+#include <mujoco/mujoco.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+// number of steps to roll out before benchmarking
+static const int kNumWarmupSteps = 500;
+
+void BM_StepHumanoid200(benchmark::State& state) {
+  auto model_path = GetTestDataFilePath("benchmark/testdata/humanoid200.xml");
+  std::array<char, 1024> error;
+  mjModel* model =
+      mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
+
+  model->opt.solver = mjSOL_CG;             // use CG solver
+  model->opt.enableflags |= mjENBL_ISLAND;  // enable islands
+
+  mjData* data = mj_makeData(model);
+  mjThreadPool* threadpool = mju_threadPoolCreate(10);
+  mju_bindThreadPool(data, threadpool);
+
+  // warm-up rollout to get a steady state
+  for (int i = 0; i < kNumWarmupSteps; i++) {
+    mj_step(model, data);
+  }
+
+  // save the initial state and step
+  int spec = mjSTATE_INTEGRATION;
+  int size = mj_stateSize(model, spec);
+  std::vector<mjtNum> initial_state(size);
+  mj_getState(model, data, initial_state.data(), spec);
+
+  // note: this tests resetting and stepping
+  for (int i = 0; i < 10; ++i) {
+    // reset to the saved state, step again, get the resulting state
+    mj_setState(model, data, initial_state.data(), spec);
+    for (int i = 0; i < kNumWarmupSteps; i++) {
+      mj_step(model, data);
+    }
+  }
+
+  state.SetItemsProcessed(state.iterations());
+  mj_deleteData(data);
+  mju_threadPoolDestroy(threadpool);
+}
+
+void BM_Step22Humanoids(benchmark::State& state) {
+  auto model_path = GetTestDataFilePath("benchmark/testdata/22_humanoids.xml");
+  std::array<char, 1024> error;
+  mjModel* model =
+      mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
+  model->opt.solver = mjSOL_CG;             // use CG solver
+  model->opt.enableflags |= mjENBL_ISLAND;  // enable islands
+
+  mjData* data = mj_makeData(model);
+  mjThreadPool* threadpool = mju_threadPoolCreate(10);
+  mju_bindThreadPool(data, threadpool);
+
+  // warm-up rollout to get a steady state
+  for (int i = 0; i < kNumWarmupSteps; i++) {
+    mj_step(model, data);
+  }
+
+  // save the initial state and step
+  int spec = mjSTATE_INTEGRATION;
+  int size = mj_stateSize(model, spec);
+  std::vector<mjtNum> initial_state(size);
+  mj_getState(model, data, initial_state.data(), spec);
+
+  std::vector<mjtNum> ctrl = GetCtrlNoise(model, kNumWarmupSteps);
+
+  // note: this tests resetting and stepping
+  for (int i = 0; i < 10; ++i) {
+    // reset to the saved state, step again, get the resulting state
+    mj_setState(model, data, initial_state.data(), spec);
+    for (int i = 0; i < kNumWarmupSteps; i++) {
+      mju_copy(data->ctrl, ctrl.data()+model->nu*i, model->nu);
+      mj_step(model, data);
+    }
+  }
+
+  state.SetItemsProcessed(state.iterations());
+  mj_deleteData(data);
+  mju_threadPoolDestroy(threadpool);
+}
+
+BENCHMARK(BM_StepHumanoid200);
+BENCHMARK(BM_Step22Humanoids);
+}  // namespace
+}  // namespace mujoco

test/engine/engine_forward_test.cc

@@ -555,6 +555,57 @@ TEST_F(ForwardTest, gravcomp) {
   mj_deleteModel(model);
 }
 
+// test disabling of equality constraints
+TEST_F(ForwardTest, eq_active) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint name="vertical" type="slide" axis="0 0 1"/>
+        <geom size="1"/>
+      </body>
+    </worldbody>
+    <equality>
+      <joint joint1="vertical"/>
+    </equality>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml);
+  ASSERT_THAT(model, NotNull());
+
+  mjData* data = mj_makeData(model);
+
+  // simulate for 1 second
+  while (data->time < 1) {
+    mj_step(model, data);
+  }
+
+  // expect that the body has barely moved
+  EXPECT_LT(mju_abs(data->qpos[0]), 0.001);
+
+  // turn the equality off, simulate for another second
+  data->eq_active[0] = 0;
+  while (data->time < 2) {
+    mj_step(model, data);
+  }
+
+  // expect that the body has fallen about 5m
+  EXPECT_LT(data->qpos[0], -4.5);
+  EXPECT_GT(data->qpos[0], -5.5);
+
+  // turn the equality back on, simulate for another second
+  data->eq_active[0] = 1;
+  while (data->time < 3) {
+    mj_step(model, data);
+  }
+
+  // expect that the body has snapped back
+  EXPECT_LT(mju_abs(data->qpos[0]), 0.001);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 // user defined 2nd-order activation dynamics: frequency-controlled oscillator
 //  note that scalar mjcb_act_dyn callbacks are expected to return act_dot, but
 //  since we have a vector output we write into act_dot directly

test/engine/engine_island_test.cc

@@ -14,15 +14,20 @@
 
 // Tests for engine/engine_island.c.
 
+#include <array>
+#include <cstring>
 #include <string>
 #include <vector>
 
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 #include <mujoco/mjmodel.h>
+#include <mujoco/mjthread.h>
+#include <mujoco/mjxmacro.h>
 #include <mujoco/mujoco.h>
 #include "src/engine/engine_island.h"
 #include "src/engine/engine_util_sparse.h"
+#include "src/thread/thread_pool.h"
 #include "test/fixture.h"
 
 namespace mujoco {
@@ -382,5 +387,61 @@ TEST_F(IslandTest, IslandEfcElliptic) {
   mj_deleteModel(model);
 }
 
+TEST_F(IslandTest, IslandSingleAndMultiThreadedMatch) {
+  auto model_path = GetTestDataFilePath("benchmark/testdata/22_humanoids.xml");
+  std::array<char, 1024> error;
+  mjModel* model =
+      mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
+  model->opt.solver = mjSOL_CG;             // use CG solver
+  model->opt.enableflags |= mjENBL_ISLAND;  // enable islands
+
+  mjModel* model_threaded =
+      mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
+  model_threaded->opt.solver = mjSOL_CG;             // use CG solver
+  model_threaded->opt.enableflags |= mjENBL_ISLAND;  // enable islands
+
+  mjData* data = mj_makeData(model);
+  mjData* data_threaded = mj_makeData(model_threaded);
+
+  // warm-up rollout to get a steady state
+  static constexpr int kNumWarmupSteps = 100;
+  for (int i = 0; i < kNumWarmupSteps; i++) {
+    mj_step(model, data);
+  }
+
+  // sync the models and data
+  int spec = mjSTATE_INTEGRATION;
+  int size = mj_stateSize(model, spec);
+  std::vector<mjtNum> initial_state(size);
+  mj_getState(model, data, initial_state.data(), spec);
+  mj_setState(model_threaded, data_threaded, initial_state.data(), spec);
+
+  // bind a threadpool to the data_threaded
+  mjThreadPool* threadpool = mju_threadPoolCreate(10);
+  mju_bindThreadPool(data_threaded, threadpool);
+
+  for (int i = 0; i < 10; ++i) {
+    mj_step(model, data);
+    mj_step(model_threaded, data_threaded);
+  }
+
+  // compare the mjData's.
+  {
+    MJDATA_POINTERS_PREAMBLE((model))
+    #define X(type, name, nr, nc)                                     \
+        EXPECT_EQ(std::memcmp(data->name, data_threaded->name,        \
+                              sizeof(type)*(model->nr)*(nc)),         \
+                  0) << "mjData::" #name " differs";
+    MJDATA_POINTERS
+    #undef X
+  }
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+  mj_deleteData(data_threaded);
+  mj_deleteModel(model_threaded);
+  mju_threadPoolDestroy(threadpool);
+}
+
 }  // namespace
 }  // namespace mujoco

test/engine/engine_support_test.cc

@@ -369,6 +369,7 @@ TEST_F(SupportTest, GetSetStateStepEqual) {
   // set controls and applied joint forces to random values
   for (int i=0; i < model->nu; i++) data->ctrl[i] = dist(rng);
   for (int i=0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng);
+  for (int i=0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0;
 
   // take one step
   mj_step(model, data);

test/testdata/model.xml

@@ -153,4 +153,11 @@
     <accelerometer site="head"/>
     <subtreeangmom body="head"/>
   </sensor>
+
+  <keyframe>
+    <key name="start"
+         time="0.128"
+         qpos="0.0133625 -0.013799 0.659429 0.999776 0.019159 0.00902704 0.000572574 -0.00776575 0.0983289 0.999861 -0.00869796 -0.0141805 0.000912197 0.00700633 0.0930296 0.00637469 -0.00313231 0.119692 0.0051478 -0.0469067 0 -0.500224 0.300659 0.69913 1 -0.000367766 -0.000265698 -0.000109715 -0.319851 0.525628 0.702735 0.29858 0.698263 -0.0490604 -0.648746 -0.579432 0.4206 0.749601 0.900145 -0.208463 0.346664 -0.161576 -0.200033 0.300084 0.976421 0.878429 -0.00171382 0.00127086 -0.477867"
+         qvel="0.508861 -0.243772 -0.327162 0.549686 1.13746 0.0282475 -0.0865243 1.06414 -0.239958 -0.532111 -0.00133789 -0.0654491 0.533148 -0.253161 0.122149 0.824764 0.723529 -0.674175 0 -0.00154509 -0.00568917 0.00479921 0.00541817 0.00115895 0.00317796 -0.160161 -0.599488 -1.38103 14.9789 -1.05454 -13.9252 -0.31223 -0.0102486 -0.405851 5.43992 -4.71514 0.114928 -0.00123758 0.000187845 -0.244109 -0.190199 0.0697975 -16.3286"/>
+  </keyframe>
 </mujoco>

unity/Runtime/Bindings/MjBindings.cs

@@ -4765,6 +4765,7 @@ public unsafe struct mjData_ {
   public double* ctrl;
   public double* qfrc_applied;
   public double* xfrc_applied;
+  public byte* eq_active;
   public double* mocap_pos;
   public double* mocap_quat;
   public double* qacc;
@@ -5318,7 +5319,7 @@ public unsafe struct mjModel_ {
   public int* eq_type;
   public int* eq_obj1id;
   public int* eq_obj2id;
-  public byte* eq_active;
+  public byte* eq_active0;
   public double* eq_solref;
   public double* eq_solimp;
   public double* eq_data;
@@ -6047,7 +6048,6 @@ public unsafe struct model {
   public int* eq_type;
   public int* eq_obj1id;
   public int* eq_obj2id;
-  public byte* eq_active;
   public double* eq_data;
   public int* tendon_num;
   public int* tendon_matid;
@@ -6104,6 +6104,7 @@ public unsafe struct data {
   public double* act;
   public double* ctrl;
   public double* xfrc_applied;
+  public byte* eq_active;
   public double* sensordata;
   public double* xpos;
   public double* xquat;
@@ -7087,5 +7088,8 @@ public static unsafe extern void mjd_subQuat(double* qa, double* qb, double* Da,
 
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mjd_quatIntegrate(double* vel, double scale, double* Dquat, double* Dvel, double* Dscale);
+
+[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
+public static unsafe extern void mju_bindThreadPool(mjData_* d, void* thread_pool);
 }
 }