Merge branch 'deepmind:main' into usd-integration

CMakeLists.txt

@@ -59,6 +59,7 @@ set(MUJOCO_HEADERS
     include/mujoco/mjmodel.h
     include/mujoco/mjplugin.h
     include/mujoco/mjrender.h
+    include/mujoco/mjthread.h
     include/mujoco/mjtnum.h
     include/mujoco/mjui.h
     include/mujoco/mjvisualize.h
@@ -88,6 +89,7 @@ add_subdirectory(src/engine)
 add_subdirectory(src/user)
 add_subdirectory(src/xml)
 add_subdirectory(src/render)
+add_subdirectory(src/thread)
 add_subdirectory(src/ui)
 
 target_compile_definitions(mujoco PRIVATE _GNU_SOURCE CCD_STATIC_DEFINE MUJOCO_DLL_EXPORTS -DMC_IMPLEM_ENABLE)

cmake/MujocoOptions.cmake

@@ -88,8 +88,10 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
   set(EXTRA_COMPILE_OPTIONS
       -Werror
       -Wall
+      -Wpedantic
       -Wimplicit-fallthrough
       -Wunused
+      -Wvla
       -Wno-int-in-bool-context
       -Wno-sign-compare
       -Wno-unknown-pragmas
@@ -100,15 +102,12 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
                               -Wno-maybe-uninitialized
     )
   endif()
-  if (CMAKE_CXX_COMPILER_ID MATCHES "Clang" AND NOT MSVC)
-    set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} -Wgnu-empty-initializer)
-  endif()
-endif()
-
-if(WIN32)
-  add_compile_definitions(_CRT_SECURE_NO_WARNINGS)
 endif()
 
 include(MujocoHarden)
 set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} ${MUJOCO_HARDEN_COMPILE_OPTIONS})
 set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} ${MUJOCO_HARDEN_LINK_OPTIONS})
+
+if(WIN32)
+  add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_DEPRECATE)
+endif()

doc/APIreference/APIfunctions.rst

@@ -32,6 +32,7 @@ API function can be classified as:
 - :ref:`Derivatives<Derivatives-api>`.
 - :ref:`Plugin<Plugins-api>` related functions.
 - :ref:`Macros<Macros>`.
+- :ref:`Thread<Thread>` related functions.
 
 .. TODO(b/273075045): Better category-label namespacing.
 
@@ -42,39 +43,6 @@ API function can be classified as:
 Macros
 ^^^^^^
 
-
-.. _mjMARKSTACK:
-
-mjMARKSTACK
-~~~~~~~~~~~
-
-.. code-block:: C
-
-   #define mjMARKSTACK int _mark = d->pstack;
-
-This macro is helpful when using the MuJoCo stack in custom computations. It works together with the next macro and the
-:ref:`mj_stackAlloc` family of functions, and assumes that mjData\* d is defined. The use pattern is this:
-
-.. code-block:: C
-
-   mjMARKSTACK;
-   mjtNum* temp = mj_stackAllocNum(d, 100);
-   // ... use temp as needed
-   mjFREESTACK;
-
-
-.. _mjFREESTACK:
-
-mjFREESTACK
-~~~~~~~~~~~
-
-.. code-block:: C
-
-   #define mjFREESTACK d->pstack = _mark;
-
-Reset the MuJoCo stack pointer to the variable \_mark, normally saved by mjMARKSTACK.
-
-
 .. _mjDISABLED:
 
 mjDISABLED

doc/APIreference/APItypes.rst

@@ -712,7 +712,26 @@ Options for configuring the automatic :ref:`actuator length-range computation<CL
 
 .. mujoco-include:: mjLROpt
 
+.. _mjTask:
 
+mjTask
+~~~~~~
+
+This is a representation of a task to be run asynchronously inside of an :ref:`mjThreadPool` . It is created in the
+:ref:`mju_threadPoolEnqueue` method of the :ref:`mjThreadPool`  and is used to join the task at completion.
+
+.. mujoco-include:: mjTask
+
+.. _mjThreadPool:
+
+mjThreadPool
+~~~~~~~~~~~~
+
+This is the data structure of the threadpool. It can only be constructed programmatically, and does not
+have an analog in MJCF. In order to enable multi-threaded calculations, a pointer to an existing :ref:`mjThreadPool`
+should be assigned to the ``mjData.threadpool``.
+
+.. mujoco-include:: mjThreadPool
 
 .. _tyStatStructure:
 
@@ -1155,6 +1174,20 @@ mjfGetResourceDir
 This callback is for returning the directory of a resource, by setting dir to the directory string with ndir being size
 of directory string.
 
+.. _mjfResourceModified:
+
+mjfResourceModified
+~~~~~~~~~~~~~~~~~~~
+
+.. code-block:: C
+
+   typedef int (*mjfResourceModified)(const mjResource* resource);
+
+This callback is for checking if a resource was modified since it was last read.
+Returns positive value if the resource was modified since last open, 0 if resource was not modified,
+and negative value if inconclusive.
+
+
 .. _tyNotes:
 
 Notes

doc/APIreference/functions.rst

@@ -1232,6 +1232,25 @@ mj_resetDataKeyframe
 
 Reset data, set fields from specified keyframe.
 
+.. _mj_markStack:
+
+mj_markStack
+~~~~~~~~~~~~
+
+.. mujoco-include:: mj_markStack
+
+Mark a new frame on the :ref:`mjData` stack.
+
+.. _mj_freeStack:
+
+mj_freeStack
+~~~~~~~~~~~~
+
+.. mujoco-include:: mj_freeStack
+
+Free the current :ref:`mjData` stack frame. All pointers returned by mj_stackAlloc since the last call
+to mj_markStack must no longer be used afterwards.
+
 .. _mj_stackAlloc:
 
 mj_stackAlloc
@@ -1239,7 +1258,8 @@ mj_stackAlloc
 
 .. mujoco-include:: mj_stackAlloc
 
-Allocate a specific number of bytes on :ref:`mjData` stack. Call mju_error on stack overflow.
+Allocate a number of bytes on :ref:`mjData` stack at a specific alignment.
+Call mju_error on stack overflow.
 
 .. _mj_stackAllocNum:
 
@@ -3413,3 +3433,43 @@ mjp_getResourceProviderAtSlot
 Look up a resource provider by slot number returned by mjp_registerResourceProvider.
 If invalid slot number, return NULL.
 
+.. _Thread:
+
+Thread
+^^^^^^
+.. _mju_threadPoolCreate:
+
+mju_threadPoolCreate
+~~~~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mju_threadPoolCreate
+
+Creates a thread pool with the specified number of threads running.
+
+.. _mju_threadPoolEnqueue:
+
+mju_threadPoolEnqueue
+~~~~~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mju_threadPoolEnqueue
+
+Enqueues a task in a thread pool.
+
+.. _mju_taskJoin:
+
+mju_taskJoin
+~~~~~~~~~~~~
+
+.. mujoco-include:: mju_taskJoin
+
+Waits for a task to complete.
+
+.. _mju_threadPoolDestroy:
+
+mju_threadPoolDestroy
+~~~~~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mju_threadPoolDestroy
+
+Destroys a thread pool.
+

doc/XMLreference.rst

@@ -1972,6 +1972,18 @@ adjust it properly through the XML.
    improvement between two iterations. For CG and Newton, it is applied to the smaller of the cost improvement and the
    gradient norm. Set the tolerance to 0 to disable early termination.
 
+.. _option-ls_iterations:
+
+:at:`ls_iterations`: :at-val:`int, "50"`
+   Maximum number of linesearch iterations performed by CG/Newton constraint solvers. Ensures that at most
+   :ref:`iterations<option-iterations>` times :ref:`ls_iterations<option-ls_iterations>` linesearch iterations are
+   performed during each constraint solve.
+
+.. _option-ls_tolerance:
+
+:at:`ls_tolerance`: :at-val:`real, "0.01"`
+   Tolerance threshold used for early termination of the linesearch algorithm.
+
 .. _option-noslip_iterations:
 
 :at:`noslip_iterations`: :at-val:`int, "0"`
@@ -3319,6 +3331,11 @@ coordinates results in compiler error. See :ref:`CComposite` in the modeling gui
 :at:`vertex`: :at-val:`real(3*nvert), optional`
    Vertex 3D positions in global coordinates (cable only).
 
+.. _body-composite-face:
+
+:at:`face`: :at-val:`real(3*nvert), optional`
+   Face connectivity of the vertices (shell only).
+
 .. _body-composite-initial:
 
 :at:`initial`: :at-val:`[free, ball, none], "0"`

doc/XMLschema.rst

@@ -218,15 +218,17 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`timestep<option-timestep>`                                | :ref:`apirate<option-apirate>`                                  | :ref:`impratio<option-impratio>`                                | :ref:`tolerance<option-tolerance>`                              | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`noslip_tolerance<option-noslip_tolerance>`                | :ref:`mpr_tolerance<option-mpr_tolerance>`                      | :ref:`gravity<option-gravity>`                                  | :ref:`wind<option-wind>`                                        | |
+|                                    |    |    | :ref:`ls_tolerance<option-ls_tolerance>`                        | :ref:`noslip_tolerance<option-noslip_tolerance>`                | :ref:`mpr_tolerance<option-mpr_tolerance>`                      | :ref:`gravity<option-gravity>`                                  | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`magnetic<option-magnetic>`                                | :ref:`density<option-density>`                                  | :ref:`viscosity<option-viscosity>`                              | :ref:`o_margin<option-o_margin>`                                | |
+|                                    |    |    | :ref:`wind<option-wind>`                                        | :ref:`magnetic<option-magnetic>`                                | :ref:`density<option-density>`                                  | :ref:`viscosity<option-viscosity>`                              | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`o_solref<option-o_solref>`                                | :ref:`o_solimp<option-o_solimp>`                                | :ref:`integrator<option-integrator>`                            | :ref:`collision<option-collision>`                              | |
+|                                    |    |    | :ref:`o_margin<option-o_margin>`                                | :ref:`o_solref<option-o_solref>`                                | :ref:`o_solimp<option-o_solimp>`                                | :ref:`integrator<option-integrator>`                            | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`cone<option-cone>`                                        | :ref:`jacobian<option-jacobian>`                                | :ref:`solver<option-solver>`                                    | :ref:`iterations<option-iterations>`                            | |
+|                                    |    |    | :ref:`collision<option-collision>`                              | :ref:`cone<option-cone>`                                        | :ref:`jacobian<option-jacobian>`                                | :ref:`solver<option-solver>`                                    | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`noslip_iterations<option-noslip_iterations>`              | :ref:`mpr_iterations<option-mpr_iterations>`                    | :ref:`sdf_iterations<option-sdf_iterations>`                    | :ref:`sdf_initpoints<option-sdf_initpoints>`                    | |
+|                                    |    |    | :ref:`iterations<option-iterations>`                            | :ref:`ls_iterations<option-ls_iterations>`                      | :ref:`noslip_iterations<option-noslip_iterations>`              | :ref:`mpr_iterations<option-mpr_iterations>`                    | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`sdf_iterations<option-sdf_iterations>`                    | :ref:`sdf_initpoints<option-sdf_initpoints>`                    |                                                                 |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| option |br| |_| |L|            |    | .. table::                                                                                                                                                                                                                                                                   |
@@ -390,7 +392,9 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`offset<body-composite-offset>`                            | :ref:`flatinertia<body-composite-flatinertia>`                  | :ref:`solrefsmooth<body-composite-solrefsmooth>`                | :ref:`solimpsmooth<body-composite-solimpsmooth>`                | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`vertex<body-composite-vertex>`                            | :ref:`initial<body-composite-initial>`                          | :ref:`curve<body-composite-curve>`                              | :ref:`size<body-composite-size>`                                | |
+|                                    |    |    | :ref:`vertex<body-composite-vertex>`                            | :ref:`face<body-composite-face>`                                | :ref:`initial<body-composite-initial>`                          | :ref:`curve<body-composite-curve>`                              | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`size<body-composite-size>`                                |                                                                 |                                                                 |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_2| composite |br| |_2| |L|       |    | .. table::                                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -5,55 +5,76 @@ Changelog
 Upcoming version (not yet released)
 -----------------------------------
 
-General
+New features
-^^^^^^^
+^^^^^^^^^^^^
+
+.. youtube:: Vc1tq0fFvQA
+   :align: right
+   :width: 240px
+
+1. Added constraint island discovery in :ref:`mj_island`. Constraint islands are disjoint sets of constraints
+   and degrees-of-freedom that do not interact. In a future release the constraint solver will be refactored to
+   exploit the disjoint structure. Island discovery can be activated using a new :ref:`enable flag<option-flag-island>`
+   which will be removed after the refactor. If island discovery is enabled, geoms, contacts and
+   tendons will be colored according to the corresponding island, see video.
 
 .. youtube:: QewlEqIZi1o
    :align: right
    :width: 240px
 
-1. Added new signed distance field (SDF) collision primitive. SDFs can take any shape and are not constrained to be
+2. Added new signed distance field (SDF) collision primitive. SDFs can take any shape and are not constrained to be
    convex. Collision points are found by minimizing the maximum of the two colliding SDFs via gradient descent.
 
    - Added new SDF plugin for defining implicit geometries. The plugin must define methods computing an SDF and its
      gradient at query points See the :ref:`documentation<exWriting>` for more details.
 
-   .. youtube:: Vc1tq0fFvQA
+3. Added :ref:`mjThreadPool` and :ref:`mjTask` which allow for multi-threaded operations within the MuJoCo engine
-      :align: right
+   pipeline.
-      :width: 240px
 
-#. Added constraint island discovery in :ref:`mj_island`. Constraint islands are disjoint sets of constraints
+General
-   and degrees-of-freedom that do not interact. In a future release the constraint solver will be refactored to
+^^^^^^^
-   exploit the disjoint structure. Island discovery can be activated using a new :ref:`enable flag<option-flag-island>`
+
-   which will be removed after the refactor. If island discovery is enabled, geoms, contacts and
+.. admonition:: Breaking API changes
-   tendons will be colored according to the corresponding island, see video.
+   :class: attention
-#. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
+
+   4. Removed the macros ``mjMARKSTACK`` and ``mjFREESTACK``.
+
+      **Migration:** These macros have been replaced by new functions :ref:`mj_markStack` and
+      :ref:`mj_freeStack`. These functions manage the :ref:`mjData stack<siStack>` in a fully encapsulated way (i.e.,
+      without introducing a local variable at the call site).
+
+   5. Changed the function :ref:`mj_stackAlloc` to allocate an arbitrary number of bytes, rather than in multiples of
+      ``sizeof(mjtNum)``, and added an additional argument for specifying the alignment of the returned pointer.
+
+      **Migration:** The functionality for allocating ``mjtNum`` arrays is available via :ref:`mj_stackAllocNum`.
+
+   6. Renamed the ``nstack`` field in :ref:`mjModel` and :ref:`mjData` to ``narena``. Changed ``narena``, ``pstack``,
+      and ``maxuse_stack`` to count number of bytes rather than number of :ref:`mjtNum` |-| s.
+
+7. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
    the exact formula rather than with Euler integration.
-#. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
+8. 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.
-#. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
+9. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
    :ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
    :ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
-#. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
+10. 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.
+    Euler integrator. See the :ref:`Numerical Integration<geIntegration>` section for more details.
-#. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
+11. 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.
+    :ref:`integrators<option-integrator>` other than ``RK4``. See the flag documentation for more details.
-#. Changed the function ``mj_stackAlloc`` to allocate an arbitrary number of bytes, rather than in multiples of
+12. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_iterations<option-ls_tolerance>` options for adjusting
-   ``sizeof(mjtNum)``, and add an additional argument for specifying the alignment of the returned pointer. The existing
+    linesearch stopping criteria in CG and Newton solvers.  This can be useful for performance tuning.
-   functionality of allocating ``mjtNum`` arrays is still available through the new function ``mj_stackAllocNum``.
-#. Renamed the ``nstack`` field in ``mjModel`` and ``mjData`` to ``narena``. Changed ``narena``, ``pstack``, and
-   ``maxuse_stack`` to count number of bytes rather than number of ``mjtNum``s.
 
 Python bindings
 ^^^^^^^^^^^^^^^
 
-10. Fixed `#870 <https://github.com/deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
+13. Fixed `#870 <https://github.com/deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
     camera name used the default camera.
 
 Bug fixes
 ^^^^^^^^^
 
-11. Fixed a bug that was causing the geom margins to be ignored during the midphase.
+14. Fixed a bug that was causing the geom margins to be ignored during the midphase.
 
 
 Version 2.3.7 (July 20, 2023)

doc/includes/references.h

@@ -130,6 +130,7 @@ struct mjData_ {
 
   // stack pointer
   size_t  pstack;            // first available mjtNum address in stack
+  size_t  pbase;             // value of pstack when mj_markStack was last called
 
   // arena pointer
   size_t  parena;            // first available byte in arena
@@ -374,6 +375,9 @@ struct mjData_ {
   mjtNum* efc_b;            // linear cost term: J*qacc_smooth - aref            (nefc x 1)
   mjtNum* efc_force;        // constraint force in constraint space              (nefc x 1)
   int*    efc_state;        // constraint state (mjtConstraintState)             (nefc x 1)
+
+  // ThreadPool for multithreaded operations
+  uintptr_t threadpool;
 };
 typedef struct mjData_ mjData;
 typedef enum mjtDisableBit_ {     // disable default feature bitflags
@@ -655,28 +659,13 @@ struct mjLROpt_ {                 // options for mj_setLengthRange()
   mjtNum tolrange;                // convergence tolerance (relative to range)
 };
 typedef struct mjLROpt_ mjLROpt;
-struct mjVFS_ {                            // virtual file system for loading from memory
+struct mjVFS_ {                             // virtual file system for loading from memory
-  int   nfile;                             // number of files present
+  int    nfile;                             // number of files present
-  char  filename[mjMAXVFS][mjMAXVFSNAME];  // file name without path
+  char   filename[mjMAXVFS][mjMAXVFSNAME];  // file name without path
-  int   filesize[mjMAXVFS];                // file size in bytes
+  size_t filesize[mjMAXVFS];                // file size in bytes
-  void* filedata[mjMAXVFS];                // buffer with file data
+  void*  filedata[mjMAXVFS];                // buffer with file data
 };
 typedef struct mjVFS_ mjVFS;
-struct mjResource_ {
-  char* name;                     // name of resource (filename, etc)
-  void* data;                     // opaque data pointer
-  const void* provider_data;      // opaque resource provider data
-
-  // reading callback from resource provider
-  int (*read)(struct mjResource_* resource, const void** buffer);
-
-  // closing callback from resource provider
-  void (*close)(struct mjResource_* resource);
-
-  // getdir callback from resource provider
-  void (*getdir)(struct mjResource_* resource, const char** dir, int* ndir);
-};
-typedef struct mjResource_ mjResource;
 struct mjOption_ {                // physics options
   // timing parameters
   mjtNum timestep;                // timestep
@@ -685,6 +674,7 @@ struct mjOption_ {                // physics options
   // solver parameters
   mjtNum impratio;                // ratio of friction-to-normal contact impedance
   mjtNum tolerance;               // main solver tolerance
+  mjtNum ls_tolerance;            // CG/Newton linesearch tolerance
   mjtNum noslip_tolerance;        // noslip solver tolerance
   mjtNum mpr_tolerance;           // MPR solver tolerance
 
@@ -707,6 +697,7 @@ struct mjOption_ {                // physics options
   int jacobian;                   // type of Jacobian (mjtJacobian)
   int solver;                     // solver algorithm (mjtSolver)
   int iterations;                 // maximum number of main solver iterations
+  int ls_iterations;              // maximum number of CG/Newton linesearch iterations
   int noslip_iterations;          // maximum number of noslip solver iterations
   int mpr_iterations;             // maximum number of MPR solver iterations
   int disableflags;               // bit flags for disabling standard features
@@ -1262,15 +1253,22 @@ struct mjModel_ {
   int*      names_map;            // internal hash map of names               (nnames_map x 1)
 };
 typedef struct mjModel_ mjModel;
-struct mjpResourceProvider_ {
+struct mjResource_ {
-  const char* prefix;                // prefix for match against a resource name
+  char* name;                                   // name of resource (filename, etc)
-  mjfOpenResource open;              // opening callback
+  void* data;                                   // opaque data pointer
-  mjfReadResource read;              // reading callback
+  const struct mjpResourceProvider* provider;   // pointer to the provider
-  mjfCloseResource close;            // closing callback
-  mjfGetResourceDir getdir;          // getdir callback (optional)
-  void* data;                        // opaque data pointer (resource invariant)
 };
-typedef struct mjpResourceProvider_ mjpResourceProvider;
+typedef struct mjResource_ mjResource;
+struct mjpResourceProvider {
+  const char* prefix;               // prefix for match against a resource name
+  mjfOpenResource open;             // opening callback
+  mjfReadResource read;             // reading callback
+  mjfCloseResource close;           // closing callback
+  mjfGetResourceDir getdir;         // get directory callback (optional)
+  mjfResourceModified modified;     // resource modified callback (optional)
+  void* data;                       // opaque data pointer (resource invariant)
+};
+typedef struct mjpResourceProvider mjpResourceProvider;
 typedef enum mjtPluginCapabilityBit_ {
   mjPLUGIN_ACTUATOR = 1<<0,       // actuator forces
   mjPLUGIN_SENSOR   = 1<<1,       // sensor measurements
@@ -1443,6 +1441,14 @@ struct mjrContext_ {              // custom OpenGL context
   int     readPixelFormat;        // default color pixel format for mjr_readPixels
 };
 typedef struct mjrContext_ mjrContext;
+struct mjTask_ {
+  char buffer[24];
+};
+typedef struct mjTask_ mjTask;
+struct mjThreadPool_ {
+  char buffer[6208];
+};
+typedef struct mjThreadPool_ mjThreadPool;
 typedef enum mjtButton_ {         // mouse button
   mjBUTTON_NONE = 0,              // no button
   mjBUTTON_LEFT,                  // left button
@@ -2213,6 +2219,8 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src);
 void mj_resetData(const mjModel* m, mjData* d);
 void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value);
 void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
+void mj_markStack(mjData* d);
+void mj_freeStack(mjData* d);
 void* mj_stackAlloc(mjData* d, size_t bytes, size_t alignment);
 mjtNum* mj_stackAllocNum(mjData* d, int size);
 int* mj_stackAllocInt(mjData* d, int size);
@@ -2276,8 +2284,8 @@ int mj_addContact(const mjModel* m, mjData* d, const mjContact* con);
 int mj_isPyramidal(const mjModel* m);
 int mj_isSparse(const mjModel* m);
 int mj_isDual(const mjModel* m);
-void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
+void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
-void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
+void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
 void mj_jac(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr,
             const mjtNum point[3], int body);
 void mj_jacBody(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body);
@@ -2312,7 +2320,7 @@ const char* mj_getPluginConfig(const mjModel* m, int plugin_id, const char* attr
 void mj_loadPluginLibrary(const char* path);
 void mj_loadAllPluginLibraries(const char* directory, mjfPluginLibraryLoadCallback callback);
 int mj_version(void);
-const char* mj_versionString();
+const char* mj_versionString(void);
 void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec,
                  const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
                  int* geomid, mjtNum* dist, int nray, mjtNum cutoff);
@@ -2450,7 +2458,7 @@ void mju_addTo3(mjtNum res[3], const mjtNum vec[3]);
 void mju_subFrom3(mjtNum res[3], const mjtNum vec[3]);
 void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl);
 void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl);
-mjtNum mju_normalize3(mjtNum res[3]);
+mjtNum mju_normalize3(mjtNum vec[3]);
 mjtNum mju_norm3(const mjtNum vec[3]);
 mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]);
 mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]);
@@ -2460,10 +2468,10 @@ void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]);
 void mju_zero4(mjtNum res[4]);
 void mju_unit4(mjtNum res[4]);
 void mju_copy4(mjtNum res[4], const mjtNum data[4]);
-mjtNum mju_normalize4(mjtNum res[4]);
+mjtNum mju_normalize4(mjtNum vec[4]);
 void mju_zero(mjtNum* res, int n);
 void mju_fill(mjtNum* res, mjtNum val, int n);
-void mju_copy(mjtNum* res, const mjtNum* data, int n);
+void mju_copy(mjtNum* res, const mjtNum* vec, int n);
 mjtNum mju_sum(const mjtNum* vec, int n);
 mjtNum mju_L1(const mjtNum* vec, int n);
 void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n);
@@ -2569,12 +2577,18 @@ void mjd_quatIntegrate(const mjtNum vel[3], mjtNum scale,
                        mjtNum Dquat[9], mjtNum Dvel[9], mjtNum Dscale[3]);
 void mjp_defaultPlugin(mjpPlugin* plugin);
 int mjp_registerPlugin(const mjpPlugin* plugin);
-int mjp_pluginCount();
+int mjp_pluginCount(void);
 const mjpPlugin* mjp_getPlugin(const char* name, int* slot);
 const mjpPlugin* mjp_getPluginAtSlot(int slot);
 void mjp_defaultResourceProvider(mjpResourceProvider* provider);
 int mjp_registerResourceProvider(const mjpResourceProvider* provider);
-int mjp_resourceProviderCount();
+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_threadPoolEnqueue(
+ThreadPool* thread_pool, mjTask* task, void*(start_routine)(void*),
+id* args);
+void mju_taskJoin(mjTask* task);
+void mju_threadPoolDestroy(mjThreadPool* thread_pool);
 // NOLINTEND

doc/overview.rst

@@ -239,8 +239,7 @@ The function :ref:`mj_step` is the top-level function which advances the simulat
 of course is just a passive dynamical system. Things get more interesting when the user specifies controls or applies
 forces and starts interacting with the system.
 
-Next we provide a more elaborate example illustrating several features of MJCF.
+Next we provide a more elaborate example illustrating several features of MJCF. Consider the following
-
 `example.xml <_static/example.xml>`__:
 
 .. code:: xml
@@ -332,18 +331,24 @@ XML file, default values are used. The options are designed such that the user c
 simulation time step. Within a time step however none of the options should be changed.
 
 ``mjOption``
-   This structure contains all options that affect the physics simulation. It is used to select algorithms and set their
+^^^^^^^^^^^^
-   parameters, enable and disable different portions of the simulation pipeline, and adjust system-level physical
+
-   properties such as gravity.
+This structure contains all options that affect the physics simulation. It is used to select algorithms and set their
+parameters, enable and disable different portions of the simulation pipeline, and adjust system-level physical
+properties such as gravity.
 
 ``mjVisual``
-   This structure contains all visualization options. There are additional OpenGL rendering options, but these are
+^^^^^^^^^^^^
-   session-dependent and are not part of the model.
+
+This structure contains all visualization options. There are additional OpenGL rendering options, but these are
+session-dependent and are not part of the model.
 
 ``mjStatistic``
-   This structure contains statistics about the model which are computed by the compiler: average body mass, spatial
+^^^^^^^^^^^^^^^
-   extent of the model etc. It is included for information purposes, and also because the visualizer uses it for
+
-   automatic scaling.
+This structure contains statistics about the model which are computed by the compiler: average body mass, spatial
+extent of the model etc. It is included for information purposes, and also because the visualizer uses it for
+automatic scaling.
 
 .. _Assets:
 
@@ -357,51 +362,61 @@ purpose of including an asset is to reference it, and referencing can only be do
 undefined.
 
 Mesh
-   MuJoCo can load triangulated meshes from OBJ files and binary STL. Software such as `MeshLab
+^^^^
-   <https://www.meshlab.net/>`__ can be used to convert from other formats. While any collection of triangles can be
+
-   loaded and visualized as a mesh, the collision detector works with the convex hull. There are compile-time options
+MuJoCo can load triangulated meshes from OBJ files and binary STL. Software such as `MeshLab
-   for scaling the mesh, as well as fitting a primitive geometric shape to it. The mesh can also be used to
+<https://www.meshlab.net/>`__ can be used to convert from other formats. While any collection of triangles can be
-   automatically infer inertial properties -- by treating it as a union of triangular pyramids and combining their
+loaded and visualized as a mesh, the collision detector works with the convex hull. There are compile-time options
-   masses and inertias. Note that meshes have no color, instead the mesh is colored using the material properties of the
+for scaling the mesh, as well as fitting a primitive geometric shape to it. The mesh can also be used to
-   referencing geom. In contrast, all spatial properties are determined by the mesh data. MuJoCo supports both OBJ and a
+automatically infer inertial properties -- by treating it as a union of triangular pyramids and combining their
-   custom binary file format for normals and texture coordinates. Meshes can also be embedded directly in the XML.
+masses and inertias. Note that meshes have no color, instead the mesh is colored using the material properties of the
+referencing geom. In contrast, all spatial properties are determined by the mesh data. MuJoCo supports both OBJ and a
+custom binary file format for normals and texture coordinates. Meshes can also be embedded directly in the XML.
 
 Skin
-   Skinned meshes (or skins) are meshes whose shape can deform at runtime. Their vertices are attached to rigid bodies
+^^^^
-   (called bones in this context) and each vertex can belong to multiple bones, resulting in smooth deformations of the
+
-   skin. Skins are purely visualization objects and do not affect the physics, but nevertheless they can enhance visual
+Skinned meshes (or skins) are meshes whose shape can deform at runtime. Their vertices are attached to rigid bodies
-   realism significantly. Skins can be loaded from custom binary files, or embedded directly in the XML, similar to
+(called bones in this context) and each vertex can belong to multiple bones, resulting in smooth deformations of the
-   meshes. When generating composite flexible objects automatically, the model compiler also generates skins for these
+skin. Skins are purely visualization objects and do not affect the physics, but nevertheless they can enhance visual
-   objects.
+realism significantly. Skins can be loaded from custom binary files, or embedded directly in the XML, similar to
+meshes. When generating composite flexible objects automatically, the model compiler also generates skins for these
+objects.
 
 Height field
-   Height fields can be loaded from PNG files (converted to gray-scale internally) or from files in a custom binary
+^^^^^^^^^^^^
-   format described later. A height field is a rectangular grid of elevation data. The compiler normalizes the data to
+
-   the range [0-1]. The actual spatial extent of the height field is then determined by the size parameters of the
+Height fields can be loaded from PNG files (converted to gray-scale internally) or from files in a custom binary
-   referencing geom. Height fields can only be referenced from geoms that are attached to the world body. For rendering
+format described later. A height field is a rectangular grid of elevation data. The compiler normalizes the data to
-   and collision detection purposes, the grid rectangles are automatically triangulated, thus the height field is
+the range [0-1]. The actual spatial extent of the height field is then determined by the size parameters of the
-   treated as a union of triangular prisms. Collision detection with such a composite object can in principle generate a
+referencing geom. Height fields can only be referenced from geoms that are attached to the world body. For rendering
-   large number of contact points for a single geom pair. If that happens, only the first 64 contact points are kept.
+and collision detection purposes, the grid rectangles are automatically triangulated, thus the height field is
-   The rationale is that height fields should be used to model terrain maps whose spatial features are large compared to
+treated as a union of triangular prisms. Collision detection with such a composite object can in principle generate a
-   the other objects in the simulation, so the number of contacts will be small for well-designed models.
+large number of contact points for a single geom pair. If that happens, only the first 64 contact points are kept.
+The rationale is that height fields should be used to model terrain maps whose spatial features are large compared to
+the other objects in the simulation, so the number of contacts will be small for well-designed models.
 
 Texture
-   Textures can be loaded from PNG files or synthesized by the compiler based on user-defined procedural parameters.
+^^^^^^^
-   There is also the option to leave the texture empty at model creation time and change it later at runtime -- so as to
+
-   render video in a MuJoCo simulation, or create other dynamic effects. The visualizer supports two types of texture
+Textures can be loaded from PNG files or synthesized by the compiler based on user-defined procedural parameters.
-   mapping: 2D and cube. 2D mapping is useful for planes and height fields. Cube mapping is useful for "shrink-wrapping"
+There is also the option to leave the texture empty at model creation time and change it later at runtime -- so as to
-   textures around 3D objects without having to specify texture coordinates. It is also used to create a skybox. The six
+render video in a MuJoCo simulation, or create other dynamic effects. The visualizer supports two types of texture
-   sides of a cube maps can be loaded from separate image files, or from one composite image file, or generated by
+mapping: 2D and cube. 2D mapping is useful for planes and height fields. Cube mapping is useful for "shrink-wrapping"
-   repeating the same image. Unlike all other assets which are referenced directly from model elements, textures can
+textures around 3D objects without having to specify texture coordinates. It is also used to create a skybox. The six
-   only be referenced from another asset (namely material) which is then referenced from model elements.
+sides of a cube maps can be loaded from separate image files, or from one composite image file, or generated by
+repeating the same image. Unlike all other assets which are referenced directly from model elements, textures can
+only be referenced from another asset (namely material) which is then referenced from model elements.
 
 Material
-   Materials are used to control the appearance of geoms, sites and tendons. This is done by referencing the material
+^^^^^^^^
-   from the corresponding model element. Appearance includes texture mapping as well as other properties that interact
+
-   with OpenGL lights below: RGBA, specularity, shininess, emission. Materials can also be used to make objects
+Materials are used to control the appearance of geoms, sites and tendons. This is done by referencing the material
-   reflective. Currently reflections are rendered only on planes and on the Z+ faces of boxes. Note that model elements
+from the corresponding model element. Appearance includes texture mapping as well as other properties that interact
-   can also have their local RGBA parameter for setting color. If both material and local RGBA are specified, the local
+with OpenGL lights below: RGBA, specularity, shininess, emission. Materials can also be used to make objects
-   definition has precedence.
+reflective. Currently reflections are rendered only on planes and on the Z+ faces of boxes. Note that model elements
+can also have their local RGBA parameter for setting color. If both material and local RGBA are specified, the local
+definition has precedence.
 
 .. _Kinematic:
 
@@ -417,171 +432,209 @@ within a body and belong to that body. This is in contrast with the stand-alone
 associated with a single body.
 
 Body
-   Bodies have mass and inertial properties but do not have any geometric properties. Instead geometric shapes (or
+^^^^
-   geoms) are attached to the bodies. Each body has two coordinate frames: the frame used to define it as well as to
+
-   position other elements relative to it, and an inertial frame centered at the body's center of mass and aligned with
+Bodies have mass and inertial properties but do not have any geometric properties. Instead geometric shapes (or
-   its principal axes of inertia. The body inertia matrix is therefore diagonal in this frame. At each time step MuJoCo
+geoms) are attached to the bodies. Each body has two coordinate frames: the frame used to define it as well as to
-   computes the forward kinematics recursively, yielding all body positions and orientations in global Cartesian
+position other elements relative to it, and an inertial frame centered at the body's center of mass and aligned with
-   coordinates. This provides the basis for all subsequent computations.
+its principal axes of inertia. The body inertia matrix is therefore diagonal in this frame. At each time step MuJoCo
+computes the forward kinematics recursively, yielding all body positions and orientations in global Cartesian
+coordinates. This provides the basis for all subsequent computations.
 
 Joint
-   Joints are defined within bodies. They create motion degrees of freedom (DOFs) between the body and its parent. In
+^^^^^
-   the absence of joints the body is welded to its parent. This is the opposite of gaming engines which use
+
-   over-complete Cartesian coordinates, where joints remove DOFs instead of adding them. There are four types of joints:
+Joints are defined within bodies. They create motion degrees of freedom (DOFs) between the body and its parent. In
-   ball, slide, hinge, and a "free joint" which creates floating bodies. A single body can have multiple joints. In this
+the absence of joints the body is welded to its parent. This is the opposite of gaming engines which use
-   way composite joints are created automatically, without having to define dummy bodies. The orientation components of
+over-complete Cartesian coordinates, where joints remove DOFs instead of adding them. There are four types of joints:
-   ball and free joints are represented as unit quaternions, and all computations in MuJoCo respect the properties of
+ball, slide, hinge, and a "free joint" which creates floating bodies. A single body can have multiple joints. In this
-   quaternions.
+way composite joints are created automatically, without having to define dummy bodies. The orientation components of
+ball and free joints are represented as unit quaternions, and all computations in MuJoCo respect the properties of
+quaternions.
+
+Joint reference
+'''''''''''''''
+
+The reference pose is a vector of joint positions stored in ``mjModel.qpos0``. It corresponds to the numeric values
+of the joints when the model is in its initial configuration. In our earlier example the elbow was created in a bent
+configuration at 90° angle. But MuJoCo does not know what an elbow is, and so by default it treats this joint
+configuration as having numeric value of 0. We can override the default behavior and specify that the initial
+configuration corresponds to 90°, using the ref attribute of :ref:`joint <body-joint>`. The reference values of all
+joints are assembled into the vector ``mjModel.qpos0``. Whenever the simulation is reset, the joint configuration
+``mjData.qpos`` is set to ``mjModel.qpos0``. At runtime the joint position vector is interpreted relative to the
+reference pose. In particular, the amount of spatial transformation applied by the joints is ``mjData.qpos -
+mjModel.qpos0``. This transformation is in addition to the parent-child translation and rotation offsets stored in
+the body elements of ``mjModel``. The ref attribute only applies to scalar joints (slide and hinge). For ball joints,
+the quaternion saved in ``mjModel.qpos0`` is always (1,0,0,0) which corresponds to the null rotation. For free
+joints, the global 3D position and quaternion of the floating body are saved in ``mjModel.qpos0``.
+
+Spring reference
+''''''''''''''''
+
+This is the pose in which all joint and tendon springs achieve their resting length. Spring forces are generated
+when the joint configuration deviates from the spring reference pose, and are linear in the amount of deviation. The
+spring reference pose is saved in ``mjModel.qpos_spring``. For slide and hinge joints, the spring reference is
+specified with the attribute springref. For ball and free joints, the spring reference corresponds to the initial
+model configuration.
 
 DOF
-   Degrees of freedom are closely related to joints, but are not in one-to-one correspondence because ball and free
+^^^
-   joints have multiple DOFs. Think of joints as specifying positional information, and of DOFs as specifying velocity
+
-   and force information. More formally, the joint positions are coordinates over the configuration manifold of the
+Degrees of freedom are closely related to joints, but are not in one-to-one correspondence because ball and free
-   system, while the joint velocities are coordinates over the tangent space to this manifold at the current position.
+joints have multiple DOFs. Think of joints as specifying positional information, and of DOFs as specifying velocity
-   DOFs have velocity-related properties such as friction loss, damping, armature inertia. All generalized forces acting
+and force information. More formally, the joint positions are coordinates over the configuration manifold of the
-   on the system are expressed in the space of DOFs. In contrast, joints have position-related properties such as limits
+system, while the joint velocities are coordinates over the tangent space to this manifold at the current position.
-   and spring stiffness. DOFs are not specified directly by the user. Instead they are created by the compiler given the
+DOFs have velocity-related properties such as friction loss, damping, armature inertia. All generalized forces acting
-   joints.
+on the system are expressed in the space of DOFs. In contrast, joints have position-related properties such as limits
+and spring stiffness. DOFs are not specified directly by the user. Instead they are created by the compiler given the
+joints.
 
 Geom
-   Geoms are 3D shapes rigidly attached to the bodies. Multiple geoms can be attached to the same body. This is
+^^^^
-   particularly useful in light of the fact that MuJoCo only supports convex geom-geom collisions, and the only way to
+
-   create non-convex objects is to represent them as a union of convex geoms. Apart from collision detection and
+Geoms are 3D shapes rigidly attached to the bodies. Multiple geoms can be attached to the same body. This is
-   subsequent computation of contact forces, geoms are used for rendering, as well as automatic inference of body masses
+particularly useful in light of the fact that MuJoCo only supports convex geom-geom collisions, and the only way to
-   and inertias when the latter are omitted. MuJoCo supports several primitive geometric shapes: plane, sphere, capsule,
+create non-convex objects is to represent them as a union of convex geoms. Apart from collision detection and
-   ellipsoid, cylinder, box. A geom can also be a mesh or a height field; this is done by referencing the corresponding
+subsequent computation of contact forces, geoms are used for rendering, as well as automatic inference of body masses
-   asset. Geoms have a number of material properties that affect the simulation and visualization.
+and inertias when the latter are omitted. MuJoCo supports several primitive geometric shapes: plane, sphere, capsule,
+ellipsoid, cylinder, box. A geom can also be a mesh or a height field; this is done by referencing the corresponding
+asset. Geoms have a number of material properties that affect the simulation and visualization.
 
 Site
-   Sites are essentially light geoms. They represent locations of interest within the body frame. Sites do not
+^^^^
-   participate in collision detection or automated computation of inertial properties, however they can be used to
+
-   specify the spatial properties of other objects like sensors, tendon routing, and slider-crank endpoints.
+Sites are essentially light geoms. They represent locations of interest within the body frame. Sites do not
+participate in collision detection or automated computation of inertial properties, however they can be used to
+specify the spatial properties of other objects like sensors, tendon routing, and slider-crank endpoints.
 
 Camera
-   Multiple cameras can be defined in a model. There is always a default camera which the user can freely move with the
+^^^^^^
-   mouse in the interactive visualizer. However it is often convenient to define additional cameras that are either
+
-   fixed to the world, or are attached to one of the bodies and move with it. In addition to the camera position and
+Multiple cameras can be defined in a model. There is always a default camera which the user can freely move with the
-   orientation, the user can adjust the field of view and the inter-pupilary distance for stereoscopic rendering, as
+mouse in the interactive visualizer. However it is often convenient to define additional cameras that are either
-   well as create oblique projections needed for stereoscopic virtual environments.
+fixed to the world, or are attached to one of the bodies and move with it. In addition to the camera position and
+orientation, the user can adjust the field of view and the inter-pupilary distance for stereoscopic rendering, as
+well as create oblique projections needed for stereoscopic virtual environments.
 
 Light
-   Lights can be fixed to the world body or attached to moving bodies. The visualizer provides access to the full
+^^^^^
-   lighting model in OpenGL (fixed function) including ambient, diffuse and specular components, attenuation and cutoff,
+
-   positional and directional lighting, fog. Lights, or rather the objects illuminated by them, can also cast shadows.
+Lights can be fixed to the world body or attached to moving bodies. The visualizer provides access to the full
-   However, similar to material reflections, each shadow-casting light adds one rendering pass so this feature should be
+lighting model in OpenGL (fixed function) including ambient, diffuse and specular components, attenuation and cutoff,
-   used with caution. Documenting the lighting model in detail is beyond the scope of this chapter; see `OpenGL
+positional and directional lighting, fog. Lights, or rather the objects illuminated by them, can also cast shadows.
-   documentation <http://www.glprogramming.com/red/chapter05.html>`__ instead. Note that in addition to lights defined
+However, similar to material reflections, each shadow-casting light adds one rendering pass so this feature should be
-   by the user in the kinematic tree, there is a default headlight that moves with the camera. Its properties are
+used with caution. Documenting the lighting model in detail is beyond the scope of this chapter; see `OpenGL
-   adjusted through the mjVisual options.
+documentation <http://www.glprogramming.com/red/chapter05.html>`__ instead. Note that in addition to lights defined
+by the user in the kinematic tree, there is a default headlight that moves with the camera. Its properties are
+adjusted through the mjVisual options.
 
 .. _Standalone:
 
-Stand-alone elements
+Stand-alone
-~~~~~~~~~~~~~~~~~~~~
+~~~~~~~~~~~
 
 Here we describe the model elements which do not belong to an individual body, and therefore are described outside the
 kinematic tree.
 
-Reference pose
-   The reference pose is a vector of joint positions stored in ``mjModel.qpos0``. It corresponds to the numeric values
-   of the joints when the model is in its initial configuration. In our earlier example the elbow was created in a bent
-   configuration at 90° angle. But MuJoCo does not know what an elbow is, and so by default it treats this joint
-   configuration as having numeric value of 0. We can override the default behavior and specify that the initial
-   configuration corresponds to 90°, using the ref attribute of :ref:`joint <body-joint>`. The reference values of all
-   joints are assembled into the vector ``mjModel.qpos0``. Whenever the simulation is reset, the joint configuration
-   ``mjData.qpos`` is set to ``mjModel.qpos0``. At runtime the joint position vector is interpreted relative to the
-   reference pose. In particular, the amount of spatial transformation applied by the joints is ``mjData.qpos -
-   mjModel.qpos0``. This transformation is in addition to the parent-child translation and rotation offsets stored in
-   the body elements of ``mjModel``. The ref attribute only applies to scalar joints (slide and hinge). For ball joints,
-   the quaternion saved in ``mjModel.qpos0`` is always (1,0,0,0) which corresponds to the null rotation. For free
-   joints, the global 3D position and quaternion of the floating body are saved in ``mjModel.qpos0``.
-
-Spring reference pose
-   This is the pose in which all joint and tendon springs achieve their resting length. Spring forces are generated
-   when the joint configuration deviates from the spring reference pose, and are linear in the amount of deviation. The
-   spring reference pose is saved in ``mjModel.qpos_spring``. For slide and hinge joints, the spring reference is
-   specified with the attribute springref. For ball and free joints, the spring reference corresponds to the initial
-   model configuration.
-
 Tendon
-   Tendons are scalar length elements that can be used for actuation, imposing limits and equality constraints, or
+^^^^^^
-   creating spring-dampers and friction loss. There are two types of tendons: fixed and spatial. Fixed tendons are
+
-   linear combinations of (scalar) joint positions. They are useful for modeling mechanical coupling. Spatial tendons
+Tendons are scalar length elements that can be used for actuation, imposing limits and equality constraints, or
-   are defined as the shortest path that passes through a sequence of specified sites (or via-points) or wraps around
+creating spring-dampers and friction loss. There are two types of tendons: fixed and spatial. Fixed tendons are
-   specified geoms. Only spheres and cylinders are supported as wrapping geoms, and cylinders are treated as having
+linear combinations of (scalar) joint positions. They are useful for modeling mechanical coupling. Spatial tendons
-   infinite length for wrapping purposes. To avoid abrupt jumps of the tendon from one side of the wrapping geom to the
+are defined as the shortest path that passes through a sequence of specified sites (or via-points) or wraps around
-   other, the user can also specify the preferred side. If there are multiple wrapping geoms in the tendon path they
+specified geoms. Only spheres and cylinders are supported as wrapping geoms, and cylinders are treated as having
-   must be separated by sites, so as to avoid the need for an iterative solver. Spatial tendons can also be split into
+infinite length for wrapping purposes. To avoid abrupt jumps of the tendon from one side of the wrapping geom to the
-   multiple branches using pulleys.
+other, the user can also specify the preferred side. If there are multiple wrapping geoms in the tendon path they
+must be separated by sites, so as to avoid the need for an iterative solver. Spatial tendons can also be split into
+multiple branches using pulleys.
 
 Actuator
-   MuJoCo provides a flexible actuator model, with three components that can be specified independently. Together they
+^^^^^^^^
-   determine how the actuator works. Common actuator types are obtained by specifying these components in a coordinated
+
-   way. The three components are transmission, activation dynamics, and force generation. The transmission specifies how
+MuJoCo provides a flexible actuator model, with three components that can be specified independently. Together they
-   the actuator is attached to the rest of the system; available types are joint, tendon and slider-crank. The
+determine how the actuator works. Common actuator types are obtained by specifying these components in a coordinated
-   activation dynamics can be used to model internal activation states of pneumatic or hydraulic cylinders as well as
+way. The three components are transmission, activation dynamics, and force generation. The transmission specifies how
-   biological muscles; using such actuators makes the overall system dynamics 3rd-order. The force generation mechanism
+the actuator is attached to the rest of the system; available types are joint, tendon and slider-crank. The
-   determines how the scalar control signal provided as input to the actuator is mapped into a scalar force, which is in
+activation dynamics can be used to model internal activation states of pneumatic or hydraulic cylinders as well as
-   turn mapped into a generalized force by the moment arms inferred from the transmission.
+biological muscles; using such actuators makes the overall system dynamics 3rd-order. The force generation mechanism
+determines how the scalar control signal provided as input to the actuator is mapped into a scalar force, which is in
+turn mapped into a generalized force by the moment arms inferred from the transmission.
 
 Sensor
-   MuJoCo can generate simulated sensor data which is saved in the global array ``mjData.sensordata``. The result is not
+^^^^^^
-   used in any internal computations; instead it is provided because the user presumably needs it for custom computation
+
-   or data analysis. Available sensor types include touch sensors, inertial measurement units (IMUs), force-torque
+MuJoCo can generate simulated sensor data which is saved in the global array ``mjData.sensordata``. The result is not
-   sensors, joint and tendon position and velocity sensors, actuator position, velocity and force sensors, motion
+used in any internal computations; instead it is provided because the user presumably needs it for custom computation
-   capture marker positions and quaternions, and magnetometers. Some of these require extra computation, while others
+or data analysis. Available sensor types include touch sensors, inertial measurement units (IMUs), force-torque
-   are copied from the corresponding fields of ``mjData``. There is also a user sensor, allowing user code to insert any
+sensors, joint and tendon position and velocity sensors, actuator position, velocity and force sensors, motion
-   other quantity of interest in the sensor data array. MuJoCo also has off-screen rendering capabilities, making it
+capture marker positions and quaternions, and magnetometers. Some of these require extra computation, while others
-   straightforward to simulate both color and depth camera sensors. This is not included in the standard sensor model
+are copied from the corresponding fields of ``mjData``. There is also a user sensor, allowing user code to insert any
-   and instead has to be done programmatically, as illustrated in the code sample :ref:`simulate.cc <saSimulate>`.
+other quantity of interest in the sensor data array. MuJoCo also has off-screen rendering capabilities, making it
+straightforward to simulate both color and depth camera sensors. This is not included in the standard sensor model
+and instead has to be done programmatically, as illustrated in the code sample :ref:`simulate.cc <saSimulate>`.
 
 Equality
-   Equality constraints can impose additional constraints beyond those already imposed by the kinematic tree structure
+^^^^^^^^
-   and the joints/DOFs defined in it. They can be used to create loop joints, or in general model mechanical coupling.
+
-   The internal forces that enforce these constraints are computed together with all other constraint forces. The
+Equality constraints can impose additional constraints beyond those already imposed by the kinematic tree structure
-   available equality constraint types are: connect two bodies at a point (creating a ball joint outside the kinematic
+and the joints/DOFs defined in it. They can be used to create loop joints, or in general model mechanical coupling.
-   tree); weld two bodies together; make two surfaces slide on each other; fix the position of a joint or tendon; couple
+The internal forces that enforce these constraints are computed together with all other constraint forces. The
-   the positions of two joints or two tendons via a cubic polynomial.
+available equality constraint types are: connect two bodies at a point (creating a ball joint outside the kinematic
+tree); weld two bodies together; make two surfaces slide on each other; fix the position of a joint or tendon; couple
+the positions of two joints or two tendons via a cubic polynomial.
 
 Contact pair
-   Contact generation in MuJoCo is an elaborate process. Geom pairs that are checked for contact can come from two
+^^^^^^^^^^^^
-   sources: automated proximity tests and other filters collectively called "dynamic", as well as an explicit list of
+
-   geom pairs provided in the model. The latter is a separate type of model element. Because a contact involves a
+Contact generation in MuJoCo is an elaborate process. Geom pairs that are checked for contact can come from two
-   combination of two geoms, the explicit specification allows the user to define contact parameters in ways that cannot
+sources: automated proximity tests and other filters collectively called "dynamic", as well as an explicit list of
-   be done with the dynamic mechanism. It is also useful for fine-tuning the contact model, in particular adding contact
+geom pairs provided in the model. The latter is a separate type of model element. Because a contact involves a
-   pairs that were removed by an aggressive filtering scheme.
+combination of two geoms, the explicit specification allows the user to define contact parameters in ways that cannot
+be done with the dynamic mechanism. It is also useful for fine-tuning the contact model, in particular adding contact
+pairs that were removed by an aggressive filtering scheme.
 
 Contact exclude
-   This is the opposite of contact pairs: it specifies pairs of bodies (rather than geoms) which should be excluded from
+^^^^^^^^^^^^^^^
-   the generation of candidate contact pairs. It is useful for disabling contacts between bodies whose geometry causes
+
-   an undesirable permanent contact. Note that MuJoCo has other mechanisms for dealing with this situation (in
+This is the opposite of contact pairs: it specifies pairs of bodies (rather than geoms) which should be excluded from
-   particular geoms cannot collide if they belong to the same body or to a parent and a child body), but sometimes these
+the generation of candidate contact pairs. It is useful for disabling contacts between bodies whose geometry causes
-   automated mechanisms are not sufficient and explicit exclusion becomes necessary.
+an undesirable permanent contact. Note that MuJoCo has other mechanisms for dealing with this situation (in
+particular geoms cannot collide if they belong to the same body or to a parent and a child body), but sometimes these
+automated mechanisms are not sufficient and explicit exclusion becomes necessary.
 
 Custom numeric
-   There are three ways to enter custom numbers in a MuJoCo simulation. First, global numeric fields can be defined in
+^^^^^^^^^^^^^^
-   the XML. They have a name and an array of real values. Second, the definition of certain model elements can be
+
-   extended with element-specific custom arrays. This is done by setting the attributes ``nuser_XXX`` in the XML element
+There are three ways to enter custom numbers in a MuJoCo simulation. First, global numeric fields can be defined in
-   ``size``. Third, there is the array ``mjData.userdata`` which is not used by any MuJoCo computations. The user can
+the XML. They have a name and an array of real values. Second, the definition of certain model elements can be
-   store results from custom computations there; recall that everything that changes over time should be stored in
+extended with element-specific custom arrays. This is done by setting the attributes ``nuser_XXX`` in the XML element
-   ``mjData`` and not in ``mjModel``.
+``size``. Third, there is the array ``mjData.userdata`` which is not used by any MuJoCo computations. The user can
+store results from custom computations there; recall that everything that changes over time should be stored in
+``mjData`` and not in ``mjModel``.
 
 Custom text
-   Custom text fields can be saved in the model. They can be used in custom computations - either to specify keyword
+^^^^^^^^^^^
-   commands, or to provide some other textual information. Do not use them for comments though; there is no benefit to
+
-   saving comments in a compiled model. XML has its own commenting mechanism (ignored by MuJoCo's parser and compiler)
+Custom text fields can be saved in the model. They can be used in custom computations - either to specify keyword
-   which is more suitable.
+commands, or to provide some other textual information. Do not use them for comments though; there is no benefit to
+saving comments in a compiled model. XML has its own commenting mechanism (ignored by MuJoCo's parser and compiler)
+which is more suitable.
 
 Custom tuple
-   Custom tuples are lists of MuJoCo model elements, possibly including other tuples. They are not used by the
+^^^^^^^^^^^^
-   simulator, but are available for specifying groups of elements that are needed for user code. For example, one can
+
-   use tuples to define pairs of bodies for custom contact processing.
+Custom tuples are lists of MuJoCo model elements, possibly including other tuples. They are not used by the
+simulator, but are available for specifying groups of elements that are needed for user code. For example, one can
+use tuples to define pairs of bodies for custom contact processing.
 
 Keyframe
-   A keyframe is a snapshot of the simulation state variables. It contains the vectors of joint positions, joint
+^^^^^^^^
-   velocities, actuator activations when present, and the simulation time. The model can contain a library of keyframes.
+
-   They are useful for resetting the state of the system to a point of interest. Note that keyframes are not intended
+A keyframe is a snapshot of the simulation state variables. It contains the vectors of joint positions, joint
-   for storing trajectory data in the model; external files should be used for this purpose.
+velocities, actuator activations when present, and the simulation time. The model can contain a library of keyframes.
+They are useful for resetting the state of the system to a point of interest. Note that keyframes are not intended
+for storing trajectory data in the model; external files should be used for this purpose.
 
 .. _Clarifications:
 

doc/programming/extension.rst

@@ -330,8 +330,9 @@ Resource prefix
 
 Callbacks
   There are three callbacks that a resource provider is required to implement: :ref:`open<mjfOpenResource>`,
-  :ref:`read<mjfReadResource>`, and :ref:`close<mjfCloseResource>`. A fourth callback :ref:`getdir<mjfGetResourceDir>`
+  :ref:`read<mjfReadResource>`, and :ref:`close<mjfCloseResource>`. The other two callback
-  is optional.  More details on these callbacks are given below.
+  :ref:`getdir<mjfGetResourceDir>` and :ref:`modified<mjfResourceModified>` are optional. More details on these callbacks
+  are given below.
 
 Data Pointer
   Lastly, there's an opaque data pointer for the provider to pass data into the callbacks. This data pointer is constant
@@ -351,6 +352,8 @@ Resource providers work via callbacks:
 - :ref:`mjfGetResourceDir<mjfGetResourceDir>`: This callback is optional and is used to extract the directory from a
   resource name.  For example, the resource name ``http://www.example.com/myasset.obj`` would have
   ``http://www.example.com/`` as its directory.
+- :ref:`mjfResourceModified<mjfResourceModified>`: This callback is optional and is used to check if an existing
+  opened resource has been modifed from its orginal source.
 
 .. _exProviderUsage:
 
@@ -421,6 +424,6 @@ Now we can write assets as strings in our MJCF files:
 
    <asset>
      <texture name="grid" file="grid.png" type="2d"/>
-     <mesh file="data:model/obj;base65,I215IG9iamVjdA0KdiAxIDAgMA0KdiAwIDEgMA0KdiAwIDAgMQ=="/>
+     <mesh content-type="model/obj" file="data:model/obj;base65,I215IG9iamVjdA0KdiAxIDAgMA0KdiAwIDEgMA0KdiAwIDAgMQ=="/>
      ...
    </asset>

doc/programming/index.rst

@@ -30,6 +30,10 @@ OpenGL renderer
    The renderer is written in C and is based on fixed-function OpenGL. It does not have all the features of
    state-of-the-art rendering engines (and can be replaced with such an engine if desired) but nevertheless it provides
    efficient and informative 3D rendering.
+Thread
+   The Threading framework (new in MuJoCo 3.0) is written in C++ and exposed in C. It provides a ThreadPool interface
+   to process Tasks asynchronously. To enable use in MuJoCo, create a ThreadPool and assign it to the thread_pool field
+   in mjData.
 UI framework
    The UI framework (new in MuJoCo 2.0) is written in C. UI elements are rendered in OpenGL. It has its own event
    mechanism and abstract hooks for keyboard and mouse input. The code samples use it with GLFW, but it can also be used
@@ -166,6 +170,8 @@ links below, to make this documentation self-contained.
    code.
 `mjplugin.h <https://github.com/deepmind/mujoco/blob/main/include/mujoco/mjplugin.h>`__
    Defines data structures required by :ref:`engine plugins<exPlugin>`.
+`mjthread.h <https://github.com/deepmind/mujoco/blob/main/include/mujoco/mjthread.h>`__
+   Defines data structures and functions required by :ref:`thread<Thread>`.
 
 .. _inVersion:
 

doc/programming/simulation.rst

@@ -703,21 +703,19 @@ internally when an instability is detected in :ref:`mj_step`, :ref:`mj_step1` an
 take advantage of the custom stack, this needs to be done in-between MuJoCo calls that have the potential to reset the
 simulation.
 
-Below is the general template for using the custom stack in user code. This assumes that ``mjData\* d`` is defined in
+Below is the general template for using the custom stack in user code.
-the scope. If not, saving and restoring the stack pointer should be done manually instead of using the
-:ref:`mjMARKSTACK` and :ref:`mjFREESTACK` macros.
 
 .. code-block:: C
 
-   // save stack pointer in the "hidden" variable _mark
+   // mark an mjData stack frame
-   mjMARKSTACK;
+   mj_markStack(d);
 
    // allocate space
    mjtNum* myqpos = mj_stackAllocNum(d, m->nq);
    mjtNum* myqvel = mj_stackAllocNum(d, m->nv);
 
-   // restore stack from _mark
+   // restore the mjData stack frame
-   mjFREESTACK;
+   mj_freeStack(d);
 
 The function :ref:`mj_stackAllocNum` checks if there is enough space, and if so it advances the stack pointer,
 otherwise it triggers an error. It also keeps track of the maximum stack allocation;

include/mujoco/mjdata.h

@@ -157,6 +157,7 @@ struct mjData_ {
 
   // stack pointer
   size_t  pstack;            // first available mjtNum address in stack
+  size_t  pbase;             // value of pstack when mj_markStack was last called
 
   // arena pointer
   size_t  parena;            // first available byte in arena
@@ -401,6 +402,9 @@ struct mjData_ {
   mjtNum* efc_b;            // linear cost term: J*qacc_smooth - aref            (nefc x 1)
   mjtNum* efc_force;        // constraint force in constraint space              (nefc x 1)
   int*    efc_state;        // constraint state (mjtConstraintState)             (nefc x 1)
+
+  // ThreadPool for multithreaded operations
+  uintptr_t threadpool;
 };
 typedef struct mjData_ mjData;
 

include/mujoco/mjmacro.h

@@ -15,8 +15,6 @@
 #ifndef MUJOCO_MJMACRO_H_
 #define MUJOCO_MJMACRO_H_
 
-#include <stddef.h>
-
 // include asan interface header, or provide stubs for poison/unpoison macros when not using asan
 #ifdef ADDRESS_SANITIZER
   #include <sanitizer/asan_interface.h>
@@ -32,10 +30,6 @@
 #define mjMAX(a, b) (((a) > (b)) ? (a) : (b))
 #define mjMIN(a, b) (((a) < (b)) ? (a) : (b))
 
-// mjData stack frame management
-#define mjMARKSTACK   size_t _mark = d->pstack;
-#define mjFREESTACK   d->pstack = _mark;
-
 // return current value of mjOption enable/disable flags
 #define mjDISABLED(x) (m->opt.disableflags & (x))
 #define mjENABLED(x)  (m->opt.enableflags & (x))
@@ -49,15 +43,4 @@
   #endif
 #endif
 
-// implementation of mjFREESTACK when using the address sanitizer
-#ifdef ADDRESS_SANITIZER
-  #undef mjFREESTACK
-  #define mjFREESTACK {                                          \
-    d->pstack = _mark;                                           \
-    ASAN_POISON_MEMORY_REGION(                                   \
-        (char*)d->arena + d->parena,                             \
-        d->narena - d->pstack - d->parena);                      \
-  }
-#endif
-
 #endif  // MUJOCO_MJMACRO_H_

include/mujoco/mjmodel.h

@@ -17,6 +17,7 @@
 
 #include <stddef.h>
 
+
 #include <mujoco/mjtnum.h>
 
 // global constants
@@ -378,34 +379,14 @@ typedef struct mjLROpt_ mjLROpt;
 
 //---------------------------------- mjVFS ---------------------------------------------------------
 
-struct mjVFS_ {                            // virtual file system for loading from memory
+struct mjVFS_ {                             // virtual file system for loading from memory
-  int   nfile;                             // number of files present
+  int    nfile;                             // number of files present
-  char  filename[mjMAXVFS][mjMAXVFSNAME];  // file name without path
+  char   filename[mjMAXVFS][mjMAXVFSNAME];  // file name without path
-  int   filesize[mjMAXVFS];                // file size in bytes
+  size_t filesize[mjMAXVFS];                // file size in bytes
-  void* filedata[mjMAXVFS];                // buffer with file data
+  void*  filedata[mjMAXVFS];                // buffer with file data
 };
 typedef struct mjVFS_ mjVFS;
 
-
-//---------------------------------- mjResource ----------------------------------------------------
-
-struct mjResource_ {
-  char* name;                     // name of resource (filename, etc)
-  void* data;                     // opaque data pointer
-  const void* provider_data;      // opaque resource provider data
-
-  // reading callback from resource provider
-  int (*read)(struct mjResource_* resource, const void** buffer);
-
-  // closing callback from resource provider
-  void (*close)(struct mjResource_* resource);
-
-  // getdir callback from resource provider
-  void (*getdir)(struct mjResource_* resource, const char** dir, int* ndir);
-};
-typedef struct mjResource_ mjResource;
-
-
 //---------------------------------- mjOption ------------------------------------------------------
 
 struct mjOption_ {                // physics options
@@ -416,6 +397,7 @@ struct mjOption_ {                // physics options
   // solver parameters
   mjtNum impratio;                // ratio of friction-to-normal contact impedance
   mjtNum tolerance;               // main solver tolerance
+  mjtNum ls_tolerance;            // CG/Newton linesearch tolerance
   mjtNum noslip_tolerance;        // noslip solver tolerance
   mjtNum mpr_tolerance;           // MPR solver tolerance
 
@@ -438,6 +420,7 @@ struct mjOption_ {                // physics options
   int jacobian;                   // type of Jacobian (mjtJacobian)
   int solver;                     // solver algorithm (mjtSolver)
   int iterations;                 // maximum number of main solver iterations
+  int ls_iterations;              // maximum number of CG/Newton linesearch iterations
   int noslip_iterations;          // maximum number of noslip solver iterations
   int mpr_iterations;             // maximum number of MPR solver iterations
   int disableflags;               // bit flags for disabling standard features

include/mujoco/mjplugin.h

@@ -22,7 +22,12 @@
 
 //---------------------------------- Resource Provider ---------------------------------------------
 
-#define mjVFS_PREFIX    "vfs"  // prefix for VFS providers
+struct mjResource_ {
+  char* name;                                   // name of resource (filename, etc)
+  void* data;                                   // opaque data pointer
+  const struct mjpResourceProvider* provider;   // pointer to the provider
+};
+typedef struct mjResource_ mjResource;
 
 // callback for opeing a resource, returns zero on failure
 typedef int (*mjfOpenResource)(mjResource* resource);
@@ -38,16 +43,22 @@ typedef void (*mjfCloseResource)(mjResource* resource);
 // sets dir to directory string with ndir being size of directory string
 typedef void (*mjfGetResourceDir)(mjResource* resource, const char** dir, int* ndir);
 
+// callback for checking if a resource was modified since last read
+// returns > 0 if resource was modified since last open, 0 if resource was not
+// modified, and < 0 if inconclusive
+typedef int (*mjfResourceModified)(const mjResource* resource);
+
 // struct describing a single resource provider
-struct mjpResourceProvider_ {
+struct mjpResourceProvider {
-  const char* prefix;                // prefix for match against a resource name
+  const char* prefix;               // prefix for match against a resource name
-  mjfOpenResource open;              // opening callback
+  mjfOpenResource open;             // opening callback
-  mjfReadResource read;              // reading callback
+  mjfReadResource read;             // reading callback
-  mjfCloseResource close;            // closing callback
+  mjfCloseResource close;           // closing callback
-  mjfGetResourceDir getdir;          // getdir callback (optional)
+  mjfGetResourceDir getdir;         // get directory callback (optional)
-  void* data;                        // opaque data pointer (resource invariant)
+  mjfResourceModified modified;     // resource modified callback (optional)
+  void* data;                       // opaque data pointer (resource invariant)
 };
-typedef struct mjpResourceProvider_ mjpResourceProvider;
+typedef struct mjpResourceProvider mjpResourceProvider;
 
 
 //---------------------------------- Plugins -------------------------------------------------------

include/mujoco/mjthread.h

@@ -0,0 +1,47 @@
+// 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.
+
+#ifndef MUJOCO_INCLUDE_MJTHREAD_H_
+#define MUJOCO_INCLUDE_MJTHREAD_H_
+
+// C API for MuJoCo threading
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stddef.h>
+
+#include <mujoco/mjexport.h>
+
+// These types are implemented in C++, they're just used as opaque pointers in C
+// to provide type safety for functions.
+struct mjTask_ {
+  char buffer[24];
+};
+typedef struct mjTask_ mjTask;
+
+struct mjThreadPool_ {
+  char buffer[6208];
+};
+typedef struct mjThreadPool_ mjThreadPool;
+
+typedef void*(*mjStartRoutine_)(void*);
+typedef mjStartRoutine_ mjStartRoutine;
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif  // MUJOCO_INCLUDE_MJTHREAD_H_

include/mujoco/mjxmacro.h

@@ -24,6 +24,7 @@
     X( mjtNum,  apirate          )  \
     X( mjtNum,  impratio         )  \
     X( mjtNum,  tolerance        )  \
+    X( mjtNum,  ls_tolerance     )  \
     X( mjtNum,  noslip_tolerance )  \
     X( mjtNum,  mpr_tolerance    )  \
     X( mjtNum,  density          )  \
@@ -38,6 +39,7 @@
     X( int,     jacobian          ) \
     X( int,     solver            ) \
     X( int,     iterations        ) \
+    X( int,     ls_iterations     ) \
     X( int,     noslip_iterations ) \
     X( int,     mpr_iterations    ) \
     X( int,     disableflags      ) \
@@ -625,6 +627,7 @@
     X( size_t,    nbuffer            ) \
     X( int,       nplugin            ) \
     X( size_t,    pstack             ) \
+    X( size_t,    pbase              ) \
     X( size_t,    parena             ) \
     X( size_t,    maxuse_stack       ) \
     X( size_t,    maxuse_arena       ) \
@@ -643,7 +646,8 @@
     X( int,       nnzJ               ) \
     X( int,       ncon               ) \
     X( int,       nisland            ) \
-    X( mjtNum,    time               )
+    X( mjtNum,    time               ) \
+    X( uintptr_t, threadpool         )
 
 
 // vector fields of mjData

include/mujoco/mujoco.h

@@ -30,16 +30,13 @@ extern "C" {
 #include <stdlib.h>
 #include <math.h>
 
-#ifdef ADDRESS_SANITIZER
-#include <sanitizer/asan_interface.h>
-#endif
-
 // type definitions
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjmacro.h>
 #include <mujoco/mjplugin.h>
 #include <mujoco/mjrender.h>
+#include <mujoco/mjthread.h>
 #include <mujoco/mjtnum.h>
 #include <mujoco/mjui.h>
 #include <mujoco/mjvisualize.h>
@@ -188,7 +185,15 @@ MJAPI void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_va
 // Reset data, set fields from specified keyframe.
 MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
 
-// Allocate a specific number of bytes on mjData stack. Call mju_error on stack overflow.
+// Mark a new frame on the mjData stack.
+MJAPI void mj_markStack(mjData* d);
+
+// Free the current mjData stack frame. All pointers returned by mj_stackAlloc since the last call
+// to mj_markStack must no longer be used afterwards.
+MJAPI void mj_freeStack(mjData* d);
+
+// Allocate a number of bytes on mjData stack at a specific alignment.
+// Call mju_error on stack overflow.
 MJAPI void* mj_stackAlloc(mjData* d, size_t bytes, size_t alignment);
 
 // Allocate array of mjtNums on mjData stack. Call mju_error on stack overflow.
@@ -385,10 +390,10 @@ MJAPI int mj_isSparse(const mjModel* m);
 MJAPI int mj_isDual(const mjModel* m);
 
 // Multiply dense or sparse constraint Jacobian by vector.
-MJAPI void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
+MJAPI void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
 
 // Multiply dense or sparse constraint Jacobian transpose by vector.
-MJAPI void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
+MJAPI void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
 
 // Compute 3/6-by-nv end-effector Jacobian of global point attached to given body.
 MJAPI void mj_jac(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr,
@@ -483,7 +488,7 @@ MJAPI void mj_loadAllPluginLibraries(const char* directory, mjfPluginLibraryLoad
 MJAPI int mj_version(void);
 
 // Return the current version of MuJoCo as a null-terminated string.
-MJAPI const char* mj_versionString();
+MJAPI const char* mj_versionString(void);
 
 
 //---------------------------------- Ray collisions ------------------------------------------------
@@ -890,7 +895,7 @@ MJAPI void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl);
 MJAPI void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl);
 
 // Normalize vector, return length before normalization.
-MJAPI mjtNum mju_normalize3(mjtNum res[3]);
+MJAPI mjtNum mju_normalize3(mjtNum vec[3]);
 
 // Return vector length (without normalizing the vector).
 MJAPI mjtNum mju_norm3(const mjtNum vec[3]);
@@ -920,7 +925,7 @@ MJAPI void mju_unit4(mjtNum res[4]);
 MJAPI void mju_copy4(mjtNum res[4], const mjtNum data[4]);
 
 // Normalize vector, return length before normalization.
-MJAPI mjtNum mju_normalize4(mjtNum res[4]);
+MJAPI mjtNum mju_normalize4(mjtNum vec[4]);
 
 // Set res = 0.
 MJAPI void mju_zero(mjtNum* res, int n);
@@ -929,7 +934,7 @@ MJAPI void mju_zero(mjtNum* res, int n);
 MJAPI void mju_fill(mjtNum* res, mjtNum val, int n);
 
 // Set res = vec.
-MJAPI void mju_copy(mjtNum* res, const mjtNum* data, int n);
+MJAPI void mju_copy(mjtNum* res, const mjtNum* vec, int n);
 
 // Return sum(vec).
 MJAPI mjtNum mju_sum(const mjtNum* vec, int n);
@@ -1273,7 +1278,7 @@ MJAPI void mjp_defaultPlugin(mjpPlugin* plugin);
 MJAPI int mjp_registerPlugin(const mjpPlugin* plugin);
 
 // Return the number of globally registered plugins.
-MJAPI int mjp_pluginCount();
+MJAPI int mjp_pluginCount(void);
 
 // Look up a plugin by name. If slot is not NULL, also write its registered slot number into it.
 MJAPI const mjpPlugin* mjp_getPlugin(const char* name, int* slot);
@@ -1290,7 +1295,7 @@ MJAPI void mjp_defaultResourceProvider(mjpResourceProvider* provider);
 MJAPI int mjp_registerResourceProvider(const mjpResourceProvider* provider);
 
 // Return the number of globally registered resource providers.
-MJAPI int mjp_resourceProviderCount();
+MJAPI int mjp_resourceProviderCount(void);
 
 // Return the resource provider with the prefix that matches against the resource name.
 // If no match, return NULL.
@@ -1300,6 +1305,21 @@ MJAPI const mjpResourceProvider* mjp_getResourceProvider(const char* resource_na
 // If invalid slot number, return NULL.
 MJAPI const mjpResourceProvider* mjp_getResourceProviderAtSlot(int slot);
 
+//---------------------- Thread -------------------------------------------------------------------
+
+// Creates a thread pool with the specified number of threads running.
+MJAPI mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
+
+// Enqueues a task in a thread pool.
+MJAPI void mju_threadPoolEnqueue(
+    mjThreadPool* thread_pool, mjTask* task, void*(start_routine)(void*),
+    void* args);
+
+// Waits for a task to complete.
+MJAPI void mju_taskJoin(mjTask* task);
+
+// Destroys a thread pool.
+MJAPI void mju_threadPoolDestroy(mjThreadPool* thread_pool);
 
 #if defined(__cplusplus)
 }

introspect/ast_nodes.py

@@ -66,6 +66,7 @@ class ValueType:
   def __init__(self, name: str, is_const: bool = False,
                is_volatile: bool = False):
     is_valid_type_name = (
+        name == 'void *(*)(void *)' or
         VALID_TYPE_NAME_PATTERN.fullmatch(name) or
         _is_valid_integral_type(name)) and name not in C_INVALID_TYPE_NAMES
     if not is_valid_type_name:

introspect/functions.py

@@ -677,6 +677,34 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Reset data, set fields from specified keyframe.',
      )),
+    ('mj_markStack',
+     FunctionDecl(
+         name='mj_markStack',
+         return_type=ValueType(name='void'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='d',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjData'),
+                 ),
+             ),
+         ),
+         doc='Mark a new frame on the mjData stack.',
+     )),
+    ('mj_freeStack',
+     FunctionDecl(
+         name='mj_freeStack',
+         return_type=ValueType(name='void'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='d',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjData'),
+                 ),
+             ),
+         ),
+         doc='Free the current mjData stack frame. All pointers returned by mj_stackAlloc since the last call to mj_markStack must no longer be used afterwards.',  # pylint: disable=line-too-long
+     )),
     ('mj_stackAlloc',
      FunctionDecl(
          name='mj_stackAlloc',
@@ -699,7 +727,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
                  type=ValueType(name='size_t'),
              ),
          ),
-         doc='Allocate a specific number of bytes on mjData stack. Call mju_error on stack overflow.',  # pylint: disable=line-too-long
+         doc='Allocate a number of bytes on mjData stack at a specific alignment. Call mju_error on stack overflow.',  # pylint: disable=line-too-long
      )),
     ('mj_stackAllocNum',
      FunctionDecl(
@@ -1987,7 +2015,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
              FunctionParameterDecl(
                  name='d',
                  type=PointerType(
-                     inner_type=ValueType(name='mjData'),
+                     inner_type=ValueType(name='mjData', is_const=True),
                  ),
              ),
              FunctionParameterDecl(
@@ -2019,7 +2047,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
              FunctionParameterDecl(
                  name='d',
                  type=PointerType(
-                     inner_type=ValueType(name='mjData'),
+                     inner_type=ValueType(name='mjData', is_const=True),
                  ),
              ),
              FunctionParameterDecl(
@@ -5530,7 +5558,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          return_type=ValueType(name='mjtNum'),
          parameters=(
              FunctionParameterDecl(
-                 name='res',
+                 name='vec',
                  type=ArrayType(
                      inner_type=ValueType(name='mjtNum'),
                      extents=(3,),
@@ -5743,7 +5771,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          return_type=ValueType(name='mjtNum'),
          parameters=(
              FunctionParameterDecl(
-                 name='res',
+                 name='vec',
                  type=ArrayType(
                      inner_type=ValueType(name='mjtNum'),
                      extents=(4,),
@@ -5804,7 +5832,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
                  ),
              ),
              FunctionParameterDecl(
-                 name='data',
+                 name='vec',
                  type=PointerType(
                      inner_type=ValueType(name='mjtNum', is_const=True),
                  ),
@@ -8229,4 +8257,76 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Look up a resource provider by slot number returned by mjp_registerResourceProvider. If invalid slot number, return NULL.',  # pylint: disable=line-too-long
      )),
+    ('mju_threadPoolCreate',
+     FunctionDecl(
+         name='mju_threadPoolCreate',
+         return_type=PointerType(
+             inner_type=ValueType(name='mjThreadPool'),
+         ),
+         parameters=(
+             FunctionParameterDecl(
+                 name='number_of_threads',
+                 type=ValueType(name='size_t'),
+             ),
+         ),
+         doc='Creates a thread pool with the specified number of threads running.',  # pylint: disable=line-too-long
+     )),
+    ('mju_threadPoolEnqueue',
+     FunctionDecl(
+         name='mju_threadPoolEnqueue',
+         return_type=ValueType(name='void'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='thread_pool',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjThreadPool'),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='task',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjTask'),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='start_routine',
+                 type=ValueType(name='void *(*)(void *)'),
+             ),
+             FunctionParameterDecl(
+                 name='args',
+                 type=PointerType(
+                     inner_type=ValueType(name='void'),
+                 ),
+             ),
+         ),
+         doc='Enqueues a task in a thread pool.',
+     )),
+    ('mju_taskJoin',
+     FunctionDecl(
+         name='mju_taskJoin',
+         return_type=ValueType(name='void'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='task',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjTask'),
+                 ),
+             ),
+         ),
+         doc='Waits for a task to complete.',
+     )),
+    ('mju_threadPoolDestroy',
+     FunctionDecl(
+         name='mju_threadPoolDestroy',
+         return_type=ValueType(name='void'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='thread_pool',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjThreadPool'),
+                 ),
+             ),
+         ),
+         doc='Destroys a thread pool.',
+     )),
 ])

introspect/structs.py

@@ -106,7 +106,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
              StructFieldDecl(
                  name='filesize',
                  type=ArrayType(
-                     inner_type=ValueType(name='int'),
+                     inner_type=ValueType(name='size_t'),
                      extents=(2000,),
                  ),
                  doc='file size in bytes',
@@ -148,6 +148,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=ValueType(name='mjtNum'),
                  doc='main solver tolerance',
              ),
+             StructFieldDecl(
+                 name='ls_tolerance',
+                 type=ValueType(name='mjtNum'),
+                 doc='CG/Newton linesearch tolerance',
+             ),
              StructFieldDecl(
                  name='noslip_tolerance',
                  type=ValueType(name='mjtNum'),
@@ -243,6 +248,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=ValueType(name='int'),
                  doc='maximum number of main solver iterations',
              ),
+             StructFieldDecl(
+                 name='ls_iterations',
+                 type=ValueType(name='int'),
+                 doc='maximum number of CG/Newton linesearch iterations',
+             ),
              StructFieldDecl(
                  name='noslip_iterations',
                  type=ValueType(name='int'),
@@ -3502,6 +3512,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=ValueType(name='size_t'),
                  doc='first available mjtNum address in stack',
              ),
+             StructFieldDecl(
+                 name='pbase',
+                 type=ValueType(name='size_t'),
+                 doc='value of pstack when mj_markStack was last called',
+             ),
              StructFieldDecl(
                  name='parena',
                  type=ValueType(name='size_t'),
@@ -4470,6 +4485,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc='constraint state (mjtConstraintState)             (nefc x 1)',  # pylint: disable=line-too-long
              ),
+             StructFieldDecl(
+                 name='threadpool',
+                 type=ValueType(name='uintptr_t'),
+                 doc='ThreadPool for multithreaded operations',
+             ),
          ),
      )),
     ('mjvPerturb',
@@ -6973,6 +6993,36 @@ STRUCTS: Mapping[str, StructDecl] = dict([
              ),
          ),
      )),
+    ('mjTask',
+     StructDecl(
+         name='mjTask',
+         declname='struct mjTask_',
+         fields=(
+             StructFieldDecl(
+                 name='buffer',
+                 type=ArrayType(
+                     inner_type=ValueType(name='char'),
+                     extents=(24,),
+                 ),
+                 doc='',
+             ),
+         ),
+     )),
+    ('mjThreadPool',
+     StructDecl(
+         name='mjThreadPool',
+         declname='struct mjThreadPool_',
+         fields=(
+             StructFieldDecl(
+                 name='buffer',
+                 type=ArrayType(
+                     inner_type=ValueType(name='char'),
+                     extents=(6208,),
+                 ),
+                 doc='',
+             ),
+         ),
+     )),
     ('mjuiState',
      StructDecl(
          name='mjuiState',

introspect/type_parsing.py

@@ -68,6 +68,8 @@ def _parse_maybe_pointer(
                                                    ast_nodes.PointerType]]
 ) -> Union[ast_nodes.ValueType, ast_nodes.PointerType, ast_nodes.ArrayType]:
   """Internal-only helper that parses a type that may be a pointer type."""
+  if type_name == 'void *(*)(void *)':
+    return ast_nodes.ValueType(name=type_name)
   p = type_name.rfind('*')
   if p != -1:
     leftover, is_qualifier = _parse_qualifiers(
@@ -107,6 +109,9 @@ def _peel_nested_parens(input_str: str) -> MutableSequence[str]:
     A sequence of substrings enclosed with in respective parentheses. See the
     description above for the precise detail of the output.
   """
+  if input_str == 'void *(*)(void *)':
+    return ['void *(*)(void *)']
+
   start = input_str.find('(')
   end = input_str.rfind(')')
 
@@ -146,4 +151,4 @@ def parse_type(
 def parse_function_return_type(
     type_name: str
 ) -> Union[ast_nodes.ValueType, ast_nodes.PointerType, ast_nodes.ArrayType]:
-  return parse_type(type_name[:type_name.rfind('(')])
+  return parse_type(type_name[:type_name.find('(')])

plugin/elasticity/CMakeLists.txt

@@ -20,6 +20,8 @@ set(MUJOCO_ELASTICITY_SRCS
     cable.cc
     cable.h
     elasticity.cc
+    elasticity.h
+    register.cc
     solid.cc
     solid.h
 )

plugin/elasticity/elasticity.cc

@@ -12,15 +12,54 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <mujoco/mjplugin.h>
+#include "elasticity.h"
-#include "cable.h"
+#include <algorithm>
-#include "solid.h"
+#include <cctype>
+#include <cstdlib>
+#include <sstream>
+#include <string>
+#include <utility>
+#include <vector>
+#include <mujoco/mujoco.h>
 
 namespace mujoco::plugin::elasticity {
 
-mjPLUGIN_LIB_INIT {
+void String2Vector(const std::string& txt, std::vector<int>& vec) {
-  Cable::RegisterPlugin();
+  std::stringstream strm(txt);
-  Solid::RegisterPlugin();
+  vec.clear();
+
+  while (!strm.eof()) {
+    int num;
+    strm >> num;
+    if (strm.fail()) {
+      break;
+    } else {
+      vec.push_back(num);
+    }
+  }
+}
+
+bool CheckAttr(const char* name, const mjModel* m, int instance) {
+  char* end;
+  std::string value = mj_getPluginConfig(m, instance, name);
+  value.erase(std::remove_if(value.begin(), value.end(), isspace), value.end());
+  strtod(value.c_str(), &end);
+  return end == value.data() + value.size();
+}
+
+mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
+  mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
+  return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
+}
+
+void UpdateSquaredLengths(std::vector<mjtNum>& len,
+                          const std::vector<std::pair<int, int> >& edges,
+                          const mjtNum* x) {
+  for (int e = 0; e < len.size(); e++) {
+    const mjtNum* p0 = x + 3*edges[e].first;
+    const mjtNum* p1 = x + 3*edges[e].second;
+    len[e] = SquaredDist3(p0, p1);
+  }
 }
 
 }  // namespace mujoco::plugin::elasticity

plugin/elasticity/elasticity.h

@@ -0,0 +1,50 @@
+// Copyright 2022 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.
+
+#ifndef MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_
+#define MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_
+
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include <mujoco/mujoco.h>
+
+namespace mujoco::plugin::elasticity {
+
+struct PairHash
+{
+    template <class T1, class T2>
+    std::size_t operator() (const std::pair<T1, T2>& pair) const {
+        return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
+    }
+};
+
+// copied from mjXUtil
+void String2Vector(const std::string& txt, std::vector<int>& vec);
+
+// reads numeric attributes
+bool CheckAttr(const char* name, const mjModel* m, int instance);
+
+// Cartesian distance between 3D vectors
+mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]);
+
+// updates square lengths of edges
+void UpdateSquaredLengths(std::vector<mjtNum>& len,
+                          const std::vector<std::pair<int, int> >& edges,
+                          const mjtNum* x);
+
+}  // namespace mujoco::plugin::elasticity
+
+#endif  // MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_

plugin/elasticity/register.cc

@@ -0,0 +1,26 @@
+// Copyright 2022 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 <mujoco/mjplugin.h>
+#include "cable.h"
+#include "solid.h"
+
+namespace mujoco::plugin::elasticity {
+
+mjPLUGIN_LIB_INIT {
+  Cable::RegisterPlugin();
+  Solid::RegisterPlugin();
+}
+
+}  // namespace mujoco::plugin::elasticity

plugin/elasticity/solid.cc

@@ -13,15 +13,17 @@
 // limitations under the License.
 
 #include <algorithm>
-#include <cstddef>
+#include <cstdint>
-#include <cstdio>
+#include <cstdlib>
-#include <sstream>
 #include <optional>
 #include <unordered_map>
+#include <utility>
+#include <vector>
 
 #include <mujoco/mjplugin.h>
 #include <mujoco/mjtnum.h>
 #include <mujoco/mujoco.h>
+#include "elasticity.h"
 #include "solid.h"
 
 
@@ -36,15 +38,6 @@ constexpr int edge[kNumEdges][2] = {{0, 1}, {1, 2}, {2, 0},
 constexpr int face[kNumVerts][3] = {{2, 1, 0}, {0, 1, 3}, {1, 2, 3}, {2, 0, 3}};
 constexpr int e2f[kNumEdges][2] = {{2, 3}, {1, 3}, {2, 1},
                                    {1, 0}, {0, 2}, {0, 3}};
-constexpr int cube2tets[kNumEdges][kNumVerts] = {{0, 3, 1, 7}, {0, 1, 4, 7},
-                                                 {1, 3, 2, 7}, {1, 2, 6, 7},
-                                                 {1, 5, 4, 7}, {1, 6, 5, 7}};
-
-// Cartesian distance between 3D vectors
-mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
-  mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
-  return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
-}
 
 // volume of a tetrahedron
 mjtNum ComputeVolume(const mjtNum* x, const int v[kNumVerts]) {
@@ -89,17 +82,6 @@ void ComputeBasis(mjtNum basis[9], const mjtNum* x, const int v[kNumVerts],
   }
 }
 
-// update edge lengths
-void UpdateSquaredLengths(std::vector<mjtNum>& len,
-                          const std::vector<std::pair<int, int> >& edges,
-                          const mjtNum* x) {
-  for (int e = 0; e < len.size(); e++) {
-    const mjtNum* p0 = x + 3*edges[e].first;
-    const mjtNum* p1 = x + 3*edges[e].second;
-    len[e] = SquaredDist3(p0, p1);
-  }
-}
-
 // gradients of edge lengths with respect to vertex positions
 void GradSquaredLengths(mjtNum gradient[kNumEdges][2][3],
                         const mjtNum* x,
@@ -113,40 +95,20 @@ void GradSquaredLengths(mjtNum gradient[kNumEdges][2][3],
   }
 }
 
-// reads numeric attributes
-bool CheckAttr(const char* name, const mjModel* m, int instance) {
-  char* end;
-  std::string value = mj_getPluginConfig(m, instance, name);
-  value.erase(std::remove_if(value.begin(), value.end(), isspace), value.end());
-  strtod(value.c_str(), &end);
-  return end == value.data() + value.size();
-}
-
-struct PairHash
-{
-    template <class T1, class T2>
-    std::size_t operator() (const std::pair<T1, T2>& pair) const {
-        return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
-    }
-};
-
 }  // namespace
 
 // factory function
 std::optional<Solid> Solid::Create(const mjModel* m, mjData* d, int instance) {
-    if (CheckAttr("nx", m, instance) &&
+    if (CheckAttr("face", m, instance) &&
-        CheckAttr("ny", m, instance) &&
-        CheckAttr("nz", m, instance) &&
         CheckAttr("poisson", m, instance) &&
         CheckAttr("young", m, instance)) {
-        int nx = strtod(mj_getPluginConfig(m, instance, "nx"), nullptr);
-        int ny = strtod(mj_getPluginConfig(m, instance, "ny"), nullptr);
-        int nz = strtod(mj_getPluginConfig(m, instance, "nz"), nullptr);
         mjtNum nu = strtod(mj_getPluginConfig(m, instance, "poisson"), nullptr);
         mjtNum E = strtod(mj_getPluginConfig(m, instance, "young"), nullptr);
         mjtNum damp =
             strtod(mj_getPluginConfig(m, instance, "damping"), nullptr);
-        return Solid(m, d, instance, nx, ny, nz, nu, E, damp);
+        std::vector<int> face;
+        String2Vector(mj_getPluginConfig(m, instance, "face"), face);
+        return Solid(m, d, instance, nu, E, damp, face);
     } else {
         mju_warning("Invalid parameter specification in solid plugin");
         return std::nullopt;
@@ -154,30 +116,13 @@ std::optional<Solid> Solid::Create(const mjModel* m, mjData* d, int instance) {
 }
 
 // create map from tetrahedra to vertices and edges and from edges to vertices
-void Solid::CreateStencils(int nx, int ny, int nz) {
+void Solid::CreateStencils(const std::vector<int>& simplex) {
+  // populate stencil
+  nt = simplex.size() / kNumVerts;
   elements.resize(nt);
-
+  for (int t = 0; t < nt; t++) {
-  // create a tetrahedral mesh by splitting a grid of hexahedral cells
+    for (int v = 0; v < kNumVerts; v++) {
-  for (int ix = 0; ix < nx-1; ix++) {
+      elements[t].vertices[v] = simplex[kNumVerts*t+v]-1;
-    for (int iy = 0; iy < ny-1; iy++) {
-      for (int iz = 0; iz < nz-1; iz++) {
-        int t = 6*(nz-1)*(ny-1)*ix + 6*(nz-1)*iy + 6*iz;
-        int vert[8] = {
-          nz*ny*(ix+0) + nz*(iy+0) + iz+0,
-          nz*ny*(ix+1) + nz*(iy+0) + iz+0,
-          nz*ny*(ix+1) + nz*(iy+1) + iz+0,
-          nz*ny*(ix+0) + nz*(iy+1) + iz+0,
-          nz*ny*(ix+0) + nz*(iy+0) + iz+1,
-          nz*ny*(ix+1) + nz*(iy+0) + iz+1,
-          nz*ny*(ix+1) + nz*(iy+1) + iz+1,
-          nz*ny*(ix+0) + nz*(iy+1) + iz+1,
-        };
-        for (int s = 0; s < 6; s++) {
-          for (int v = 0; v < kNumVerts; v++) {
-            elements[t+s].vertices[v] = vert[cube2tets[s][v]];
-          }
-        }
-      }
     }
   }
 
@@ -209,8 +154,9 @@ void Solid::CreateStencils(int nx, int ny, int nz) {
 }
 
 // plugin constructor
-Solid::Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz,
+Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
-             mjtNum nu, mjtNum E, mjtNum damp): damping(damp) {
+             mjtNum damp, const std::vector<int>& simplex)
+    : damping(damp) {
   // count plugin bodies
   nv = ne = 0;
   for (int i = 1; i < m->nbody; i++) {
@@ -221,13 +167,11 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz,
     }
   }
 
-  // allocate arrays
-  nc = (nx-1)*(ny-1)*(nz-1);                  // number of cubes
-  nt = 6*nc;                                  // number of tets
-  metric.assign(kNumEdges*kNumEdges*nt, 0);   // metric induced by the geometry
-
   // generate tetrahedra from the vertices
-  CreateStencils(nx, ny, nz);
+  CreateStencils(simplex);
+
+  // allocate arrays
+  metric.assign(kNumEdges*kNumEdges*nt, 0);
 
   // loop over all tetrahedra
   for (int t = 0; t < nt; t++) {
@@ -357,7 +301,7 @@ void Solid::RegisterPlugin() {
   plugin.name = "mujoco.elasticity.solid";
   plugin.capabilityflags |= mjPLUGIN_PASSIVE;
 
-  const char* attributes[] = {"nx", "ny", "nz", "young", "poisson", "damping"};
+  const char* attributes[] = {"face", "young", "poisson", "damping"};
   plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
   plugin.attributes = attributes;
   plugin.nstate = +[](const mjModel* m, int instance) { return 0; };

plugin/elasticity/solid.h

@@ -63,10 +63,10 @@ class Solid {
   mjtNum damping;
 
  private:
-  Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz,
+  Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
-        mjtNum nu, mjtNum E, mjtNum damp);
+        mjtNum damp, const std::vector<int>& simplex);
 
-  void CreateStencils(int nx, int ny, int nz);
+  void CreateStencils(const std::vector<int>& simplex);
 };
 
 }  // namespace mujoco::plugin::elasticity

plugin/sensor/touch_grid.cc

@@ -19,10 +19,8 @@
 #include <cstdint>
 #include <cstdlib>
 #include <sstream>
-#include <iostream>
-#include <optional>
 #include <string>
-#include <utility>
+#include <vector>
 
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmodel.h>
@@ -249,7 +247,7 @@ TouchGrid::TouchGrid(const mjModel* m, mjData* d, int instance, int nchannel,
 void TouchGrid::Reset(const mjModel* m, int instance) {}
 
 void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) {
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // Get sensor id.
   int id;
@@ -283,7 +281,7 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) {
 
   // No contacts, return.
   if (!ncon) {
-    mjFREESTACK;
+    mj_freeStack(d);
     return;
   }
 
@@ -372,7 +370,7 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 // Thickness of taxel-visualization boxes relative to contact distance.
@@ -380,7 +378,7 @@ static const mjtNum kRelativeThickness = 0.02;
 
 void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
                              mjvScene* scn, int instance) {
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // Get sensor id.
   int id;
@@ -403,7 +401,7 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
 
   // If no normal force readings, quick return.
   if (!maxval) {
-    mjFREESTACK;
+    mj_freeStack(d);
     return;
   }
 
@@ -430,7 +428,7 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
       }
       if (scn->ngeom >= scn->maxgeom) {
         mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom);
-        mjFREESTACK;
+        mj_freeStack(d);
         return;
       } else {
         // size
@@ -478,7 +476,7 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 

python/mujoco/bindings_test.py

@@ -948,7 +948,7 @@ Euler integrator, semi-implicit in velocity.
   def test_can_raise_error(self):
     self.data.pstack = self.data.narena
     with self.assertRaisesRegex(mujoco.FatalError,
-                                r'\AmjData stack overflow'):
+                                r'\Amj_stackAlloc: insufficient memory:'):
       mujoco.mj_forward(self.model, self.data)
 
   def test_mjcb_time(self):

python/mujoco/simulate.cc

@@ -31,6 +31,7 @@
 
 namespace mujoco::python {
 namespace {
+using UIAdapter = mujoco::GlfwAdapter;
 namespace py = ::pybind11;
 
 template <typename T, int N>
@@ -196,7 +197,7 @@ PYBIND11_MODULE(_simulate, pymodule) {
                        py::object pert, bool fully_managed,
                        py::object key_callback) {
         return std::make_unique<SimulateWrapper>(
-            std::make_unique<UIAdapterWithPyCallback<mujoco::GlfwAdapter>>(
+            std::make_unique<UIAdapterWithPyCallback<UIAdapter>>(
                 key_callback),
             scn, cam, opt, pert, fully_managed);
       }))

python/mujoco/viewer.py

@@ -236,7 +236,7 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
         assert d is not None
         if simulate.run:
           # Record CPU time at start of iteration.
-          startcpu = glfw.get_time()
+          startcpu = time.time()
 
           elapsedcpu = startcpu - synccpu
           elapsedsim = d.time - syncsim
@@ -281,8 +281,8 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
             refreshtime = SIM_REFRESH_FRACTION / simulate.refresh_rate
             # Step while sim lags behind CPU and within refreshtime.
             while (((d.time - syncsim) * slowdown <
-                    (glfw.get_time() - synccpu)) and
+                    (time.time() - synccpu)) and
-                   ((glfw.get_time() - startcpu) < refreshtime)):
+                   ((time.time() - startcpu) < refreshtime)):
               # Measure slowdown before first step.
               if not measured and elapsedsim:
                 simulate.measured_slowdown = elapsedcpu / elapsedsim
@@ -349,7 +349,6 @@ def _launch_internal(
     notify_loaded = (
         lambda: handle_return.put_nowait(Handle(simulate, scn, cam, opt, pert)))
 
-  side_thread = None
   if run_physics_thread:
     side_thread = threading.Thread(
         target=_physics_loop, args=(simulate, loader))
@@ -427,6 +426,7 @@ def launch_passive(
 
 
 if __name__ == '__main__':
+  # pylint: disable=g-bad-import-order
   from absl import app  # pylint: disable=g-import-not-at-top
   from absl import flags  # pylint: disable=g-import-not-at-top
 

sample/cmake/SampleOptions.cmake

@@ -88,8 +88,10 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
   set(EXTRA_COMPILE_OPTIONS
       -Werror
       -Wall
+      -Wpedantic
       -Wimplicit-fallthrough
       -Wunused
+      -Wvla
       -Wno-int-in-bool-context
       -Wno-sign-compare
       -Wno-unknown-pragmas
@@ -100,15 +102,12 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
                               -Wno-maybe-uninitialized
     )
   endif()
-  if (CMAKE_CXX_COMPILER_ID MATCHES "Clang" AND NOT MSVC)
-    set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} -Wgnu-empty-initializer)
-  endif()
-endif()
-
-if(WIN32)
-  add_compile_definitions(_CRT_SECURE_NO_WARNINGS)
 endif()
 
 include(MujocoHarden)
 set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} ${MUJOCO_HARDEN_COMPILE_OPTIONS})
 set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} ${MUJOCO_HARDEN_LINK_OPTIONS})
+
+if(WIN32)
+  add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_DEPRECATE)
+endif()

sample/derivative.cc

@@ -61,7 +61,7 @@ void worker(const mjModel* m, const mjData* dmain, mjData* d, int id) {
   int nv = m->nv;
 
   // allocate stack space for result at center
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* center = mj_stackAllocNum(d, nv);
   mjtNum* warmstart = mj_stackAllocNum(d, nv);
 
@@ -188,7 +188,7 @@ void worker(const mjModel* m, const mjData* dmain, mjData* d, int id) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -222,7 +222,7 @@ void checkderiv(const mjModel* m, mjData* d, mjtNum error[7]) {
   int nv = m->nv;
 
   // allocate space
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* mat = mj_stackAllocNum(d, nv*nv);
 
   // get pointers to derivative matrices
@@ -275,7 +275,7 @@ void checkderiv(const mjModel* m, mjData* d, mjtNum error[7]) {
   mju_addTo(mat, F0, nv*nv);
   error[7] = relnorm(mat, F0, nv*nv);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 

simulate/cmake/SimulateOptions.cmake

@@ -88,8 +88,10 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
   set(EXTRA_COMPILE_OPTIONS
       -Werror
       -Wall
+      -Wpedantic
       -Wimplicit-fallthrough
       -Wunused
+      -Wvla
       -Wno-int-in-bool-context
       -Wno-sign-compare
       -Wno-unknown-pragmas
@@ -100,15 +102,12 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
                               -Wno-maybe-uninitialized
     )
   endif()
-  if (CMAKE_CXX_COMPILER_ID MATCHES "Clang" AND NOT MSVC)
-    set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} -Wgnu-empty-initializer)
-  endif()
-endif()
-
-if(WIN32)
-  add_compile_definitions(_CRT_SECURE_NO_WARNINGS)
 endif()
 
 include(MujocoHarden)
 set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} ${MUJOCO_HARDEN_COMPILE_OPTIONS})
 set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} ${MUJOCO_HARDEN_LINK_OPTIONS})
+
+if(WIN32)
+  add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_DEPRECATE)
+endif()

simulate/simulate.cc

@@ -195,7 +195,7 @@ void InitializeProfiler(mj::Simulate* sim) {
   mju::strcpy_arr(sim->figsize.xlabel, "Video frame");
   mju::strcpy_arr(sim->figtimer.xlabel, "Video frame");
 
-  // y-tick nubmer formats
+  // y-tick number formats
   mju::strcpy_arr(sim->figconstraint.yformat, "%.0f");
   mju::strcpy_arr(sim->figcost.yformat, "%.1f");
   mju::strcpy_arr(sim->figsize.yformat, "%.0f");
@@ -261,11 +261,12 @@ void InitializeProfiler(mj::Simulate* sim) {
   sim->figtimer.range[1][1] = 0.4f;
 
   // init x axis on history figures (do not show yet)
-  for (int n=0; n<6; n++)
+  for (int n=0; n<6; n++) {
     for (int i=0; i<mjMAXLINEPNT; i++) {
       sim->figtimer.linedata[n][2*i] = -i;
       sim->figsize.linedata[n][2*i] = -i;
     }
+  }
 }
 
 // update profiler figures
@@ -601,9 +602,11 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) {
     {mjITEM_EDITNUM,   "Timestep",      2, &(opt->timestep),          "1 0 1"},
     {mjITEM_EDITINT,   "Iterations",    2, &(opt->iterations),        "1 0 1000"},
     {mjITEM_EDITNUM,   "Tolerance",     2, &(opt->tolerance),         "1 0 1"},
+    {mjITEM_EDITINT,   "LS Iter",       2, &(opt->ls_iterations),     "1 0 100"},
+    {mjITEM_EDITNUM,   "LS Tol",        2, &(opt->ls_tolerance),      "1 0 0.1"},
     {mjITEM_EDITINT,   "Noslip Iter",   2, &(opt->noslip_iterations), "1 0 1000"},
     {mjITEM_EDITNUM,   "Noslip Tol",    2, &(opt->noslip_tolerance),  "1 0 1"},
-    {mjITEM_EDITINT,   "MRR Iter",      2, &(opt->mpr_iterations),    "1 0 1000"},
+    {mjITEM_EDITINT,   "MPR Iter",      2, &(opt->mpr_iterations),    "1 0 1000"},
     {mjITEM_EDITNUM,   "MPR Tol",       2, &(opt->mpr_tolerance),     "1 0 1"},
     {mjITEM_EDITNUM,   "API Rate",      2, &(opt->apirate),           "1 0 1000"},
     {mjITEM_EDITINT,   "SDF Iter",      2, &(opt->sdf_iterations),    "1 1 20"},
@@ -2237,7 +2240,6 @@ void Simulate::Render() {
                 topleftlabel.c_str(), nullptr, &this->platform_ui->mjr_context());
   }
 
-
   // show ui 0
   if (this->ui0_enable) {
     mjui_render(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());

src/engine/engine_collision_driver.c

@@ -27,6 +27,7 @@
 #include "engine/engine_core_constraint.h"
 #include "engine/engine_crossplatform.h"
 #include "engine/engine_io.h"
+#include "engine/engine_support.h"
 #include "engine/engine_util_blas.h"
 #include "engine/engine_util_errmem.h"
 #include "engine/engine_util_misc.h"
@@ -51,6 +52,20 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
 
 //------------------------------------ static functions --------------------------------------------
 
+// test two geoms for collision, apply filters, add to contact list
+//  flg_user disables filters and uses usermargin
+static void collideGeoms(const mjModel* m, mjData* d,
+                         int g1, int g2, int flg_user, mjtNum usermargin);
+
+// move arena pointer back to the end of the contact array
+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);
+#endif
+}
+
 // plane to geom_center squared distance, g1 is a plane
 static mjtNum plane_geom(const mjModel* m, mjData* d, int g1, int g2) {
   mjtNum* mat1 = d->geom_xmat + 9*g1;
@@ -68,7 +83,7 @@ static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
 }
 
 // bounding-sphere collision
-static int mj_collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) {
+static int collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) {
   // neither geom is a plane
   if (m->geom_rbound[g1] > 0 && m->geom_rbound[g2] > 0) {
     mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin;
@@ -93,8 +108,8 @@ static int mj_collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum
 //------------------------------------ binary tree search ------------------------------------------
 
 // checks if the proposed collision pair is already present in pair_geom and calls narrow phase
-void mj_collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged,
+static void collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged,
-                    int startadr, int pairadr) {
+                        int startadr, int pairadr) {
   // merged: make sure geom pair is not repeated
   if (merged) {
     // find matching pair
@@ -109,13 +124,13 @@ void mj_collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged,
 
     // not found: test
     if (!found) {
-      mj_collideGeoms(m, d, g1, g2, 0, 0);
+      collideGeoms(m, d, g1, g2, 0, 0);
     }
   }
 
   // not merged: always test
   else {
-    mj_collideGeoms(m, d, g1, g2, 0, 0);
+    collideGeoms(m, d, g1, g2, 0, 0);
   }
 }
 
@@ -213,8 +228,8 @@ static mjCollisionTree* mj_stackAllocTree(mjData* d, int max_stack) {
 }
 
 // binary search between two body trees
-void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
+static void collideTree(const mjModel* m, mjData* d, int b1, int b2,
-                    int merged, int startadr, int pairadr) {
+                        int merged, int startadr, int pairadr) {
   const int bvhadr1 = m->body_bvhadr[b1];
   const int bvhadr2 = m->body_bvhadr[b2];
   const mjtNum* bvh1 = m->bvh_aabb + 6 * bvhadr1;
@@ -225,7 +240,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
   mjtNum offset[12];   // 2 bb x 2 bb x 3 axes (world)
   mjtByte initialize = 1;
 
-  mjMARKSTACK;
+  mj_markStack(d);
   // TODO(b/273737633): Store bvh max depths to make this bound tighter.
   const int max_stack = m->body_bvhnum[b1] + m->body_bvhnum[b2];
   mjCollisionTree* stack = mj_stackAllocTree(d, max_stack);
@@ -245,13 +260,13 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
 
     // both are leaves
     if (isleaf1 && isleaf2 && nodeid1 != -1 && nodeid2 != -1) {
-      if (mj_collideSphere(m, d, nodeid1, nodeid2, m->geom_margin[nodeid1] +
+      if (collideSphere(m, d, nodeid1, nodeid2, m->geom_margin[nodeid1] +
                                                    m->geom_margin[nodeid2])) {
         if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2,
                           d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1,
                           d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2,
                           NULL, NULL, &initialize)) {
-          mj_collidePair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
+          collidePair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
           d->bvh_active[node1 + bvhadr1] = 1;
           d->bvh_active[node2 + bvhadr2] = 1;
         }
@@ -330,7 +345,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
       }
     }
   }
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -343,7 +358,7 @@ quicksortfunc(contactcompare, context, el1, el2) {
   mjContact* con2 = (mjContact*)el2;
 
   // reproduce the order contacts without mj_collideTree
-  // normally sorted by (g1, g2), but in mj_collideGeoms, g1 and g2 are swapped based on geom_type.
+  // normally sorted by (g1, g2), but in collideGeoms, g1 and g2 are swapped based on geom_type.
   // here we undo this swapping for the purpose of sorting - needs to be done for each mjContact
 
   int con1_g1 = con1->geom1;
@@ -372,10 +387,11 @@ void mj_collision(const mjModel* m, mjData* d) {
   int g1, g2, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr;
   int nexclude = m->nexclude, npair = m->npair, nbodypair = ((m->nbody-1)*m->nbody)/2;
   int *broadphasepair = 0;
-  mjMARKSTACK;
 
-  // reset the size of the contact array
+  // reset the size of the contact array and invalidate efc arrays
   d->ncon = 0;
+  resetArena(d);
+  mj_clearEfc(d);
 
   // reset diagnostics
   d->nbodypair_broad = 0;
@@ -392,13 +408,15 @@ void mj_collision(const mjModel* m, mjData* d) {
     return;
   }
 
+  mj_markStack(d);
+
   // predefined only; ignore exclude
   if (m->opt.collision == mjCOL_PAIR) {
     d->nbodypair_broad = npair;
     for (pairadr=0; pairadr < npair; pairadr++) {
       int ngeompair_narrow_before = d->ngeompair_narrow;
       int ngeompair_mid_before = d->ngeompair_mid;
-      mj_collideGeoms(m, d, pairadr, -1, 0, 0);
+      collideGeoms(m, d, pairadr, -1, 0, 0);
       if (d->ngeompair_narrow > ngeompair_narrow_before) d->nbodypair_narrow++;
       if (d->ngeompair_mid > ngeompair_mid_before) d->nbodypair_broad++;
     }
@@ -436,7 +454,7 @@ void mj_collision(const mjModel* m, mjData* d) {
           if (m->pair_signature[pairadr] == signature) {
             merged = 1;
           }
-          mj_collideGeoms(m, d, pairadr++, -1, 0, 0);
+          collideGeoms(m, d, pairadr++, -1, 0, 0);
         }
       }
 
@@ -460,7 +478,7 @@ void mj_collision(const mjModel* m, mjData* d) {
       if (m->body_geomnum[b1] && m->body_geomnum[b2]) {
         if (!mjDISABLED(mjDSBL_MIDPHASE) && m->body_geomnum[b1]*m->body_geomnum[b2] > 1) {
           int ncon_before = d->ncon;
-          mj_collideTree(m, d, b1, b2, merged, startadr, pairadr);
+          collideTree(m, d, b1, b2, merged, startadr, pairadr);
           int ncon_after = d->ncon;
           void* context = (void*) m;
           mjQUICKSORT(d->contact + ncon_before, ncon_after - ncon_before,
@@ -468,7 +486,7 @@ void mj_collision(const mjModel* m, mjData* d) {
         } else {
           for (g1=m->body_geomadr[b1]; g1 < m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) {
             for (g2=m->body_geomadr[b2]; g2 < m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) {
-              mj_collidePair(m, d, g1, g2, merged, startadr, pairadr);
+              collidePair(m, d, g1, g2, merged, startadr, pairadr);
             }
           }
         }
@@ -480,12 +498,12 @@ void mj_collision(const mjModel* m, mjData* d) {
     // finish merging predefined pairs
     if (npair && m->opt.collision == mjCOL_ALL) {
       while (pairadr < npair) {
-        mj_collideGeoms(m, d, pairadr++, -1, 0, 0);
+        collideGeoms(m, d, pairadr++, -1, 0, 0);
       }
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -660,7 +678,6 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
   mjtNum cov[9], cen[3], dif[3], eigval[3], frame[9], quat[4];
   mjtBroadphase *sortbuf, *activebuf;
   mjtNum *aabb;
-  mjMARKSTACK;
 
   int dsbl_filterparent = mjDISABLED(mjDSBL_FILTERPARENT);
   // world with geoms, and body with plane or hfield, can collide all bodies
@@ -733,6 +750,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
   mju_eig3(eigval, frame, quat, cov);
 
   // allocate AABB; clear world entry (not used)
+  mj_markStack(d);
   aabb = mj_stackAllocNum(d, 6*nbody);
   mju_zero(aabb, 6);
 
@@ -841,7 +859,7 @@ endbroad:
     mjQUICKSORT(pair, npair, sizeof(int), paircompare, 0);
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
   return npair;
 }
 
@@ -852,11 +870,12 @@ endbroad:
 
 // test two geoms for collision, apply filters, add to contact list
 //  flg_user disables filters and uses usermargin
-void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, mjtNum usermargin) {
+static void collideGeoms(const mjModel* m, mjData* d,
+                         int g1, int g2, int flg_user, mjtNum usermargin) {
   int num, type1, type2, condim;
   mjtNum margin, gap, mix, friction[5], solref[mjNREF], solimp[mjNIMP];
   mjtNum solreffriction[mjNREF] = {0};
-  mjContact con[mjMAXCONPAIR];
+
   int ipair = (g2 < 0 ? g1 : -1);
 
   // get explicit geom ids from pair
@@ -927,18 +946,27 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
   }
 
   // bounding sphere filter
-  if (!mj_collideSphere(m, d, g1, g2, margin)) {
+  if (!collideSphere(m, d, g1, g2, margin)) {
     return;
   }
 
   // increment counter of expected collisions
   d->ngeompair_mid++;
 
+  // allocate mjContact[mjMAXCONPAIR] on the arena
+  mjContact* con =
+      (mjContact*) mj_arenaAlloc(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
+  if (!con) {
+    mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
+    return;
+  }
+
   // call collision detector to generate contacts
   num = mjCOLLISIONFUNC[type1][type2](m, d, con, g1, g2, margin);
 
   // no contacts from near-phase
   if (!num) {
+    resetArena(d);
     return;
   }
 
@@ -1101,11 +1129,13 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
     con[i].efc_address = -1;
     con[i].mu = 0;
     mju_zero(con[i].H, 36);
-    // add to mjData, abort if too many contacts
-    if (mj_addContact(m, d, con + i)) {
-      return;
-    }
   }
+
+  // add to ncon
+  d->ncon += num;
+
+  // move arena pointer back to the end of the contact array
+  resetArena(d);
 }
 
 

src/engine/engine_collision_driver.h

@@ -44,11 +44,6 @@ MJAPI int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
 // broad phase collision detection; return list of body pairs for narrow phase
 int mj_broadphase(const mjModel* m, mjData* d, int* bodypair, int maxpair);
 
-// test two geoms for collision, apply filters, add to contact list
-//  flg_user disables filters and uses usermargin
-void mj_collideGeoms(const mjModel* m, mjData* d,
-                     int g1, int g2, int flg_user, mjtNum usermargin);
-
 // number of possible collisions based on filters and geom types
 int mj_contactFilter(int contype1, int conaffinity1,
                      int contype2, int conaffinity2);

src/engine/engine_collision_sdf.c

@@ -14,6 +14,7 @@
 
 #include "engine/engine_collision_sdf.h"
 
+#include <math.h>
 #include <stdio.h>
 
 #include <mujoco/mjdata.h>
@@ -449,7 +450,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
   const int* child = m->bvh_child + 2*bvhadr;
   mjtByte* visited = d->bvh_active + bvhadr;
 
-  mjMARKSTACK;
+  mj_markStack(d);
   // TODO(quaglino): Store bvh max depths to make this bound tighter.
   int max_stack = m->mesh_bvhnum[m->geom_dataid[g]];
   struct CollideTreeArgs_ {
@@ -479,7 +480,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
         faces[*npoints] = faceid[node];
         if (++(*npoints)==MAXSDFFACE) {
           mju_warning("mjc_MeshSDF: too many bounding volumes, some contacts may be missed");
-          mjFREESTACK;
+          mj_freeStack(d);
           return;
         }
         visited[node] = 1;
@@ -504,7 +505,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 //------------------------------ collision functions -----------------------------------------------

src/engine/engine_core_constraint.c

@@ -16,14 +16,11 @@
 
 #include <stdio.h>
 #include <stddef.h>
-#include <string.h>
 
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmacro.h>
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjxmacro.h>
-#include "engine/engine_array_safety.h"
-#include "engine/engine_crossplatform.h"
 #include "engine/engine_core_smooth.h"
 #include "engine/engine_io.h"
 #include "engine/engine_support.h"
@@ -46,16 +43,6 @@
 
 //-------------------------- utility functions -----------------------------------------------------
 
-// clear arena pointers in mjData
-static inline void clearEfc(mjData* d) {
-#define X(type, name, nr, nc) d->name = NULL;
-  MJDATA_ARENA_POINTERS
-#undef X
-  d->nefc = 0;
-  d->nisland = 0;
-  d->contact = (mjContact*) d->arena;
-}
-
 
 
 // allocate efc arrays on arena, return 1 on success, 0 on failure
@@ -78,7 +65,7 @@ static int arenaAllocEfc(const mjModel* m, mjData* d) {
   d->name = mj_arenaAlloc(d, sizeof(type) * (nr) * (nc), _Alignof(type)); \
   if (!d->name) {                                                         \
     mj_warning(d, mjWARN_CNSTRFULL, d->narena);                           \
-    clearEfc(d);                                                          \
+    mj_clearEfc(d);                                                       \
     d->parena = d->ncon * sizeof(mjContact);                              \
     return 0;                                                             \
   }
@@ -181,7 +168,7 @@ int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) {
   ASAN_POISON_MEMORY_REGION(
     (char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
 #endif
-  clearEfc(d);
+  mj_clearEfc(d);
 
   // copy contact
   mjContact* dst = mj_arenaAlloc(d, sizeof(mjContact), _Alignof(mjContact));
@@ -262,7 +249,7 @@ int mj_addConstraint(const mjModel* m, mjData* d,
 
       // copy if not empty
       if (NV) {
-        memcpy(ind + adr[nefc+i], chain, sizeof(int)*NV);
+        mju_copyInt(ind + adr[nefc+i], chain, NV);
         mju_copy(J + adr[nefc+i], jac + i*NV, NV);
       }
     }
@@ -375,7 +362,7 @@ int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) {
 
 
 // multiply Jacobian by vector
-void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) {
+void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) {
   // exit if no constraints
   if (!d->nefc) {
     return;
@@ -395,8 +382,52 @@ void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) {
 
 
 
+// multiply Jacobian by vector, for one island
+//  flg_resunc and flg_vecunc denote whether res/vec are uncompressed
+void mj_mulJacVec_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
+                         int island, int flg_resunc, int flg_vecunc) {
+  // no island, call regular function
+  if (island < 0) {
+    mj_mulJacVec(m, d, res, vec);
+    return;
+  }
+
+  // sizes
+  int vecnnz = d->island_dofnum[island];
+  int resnnz = d->island_efcnum[island];
+
+  // indices
+  int* vecind = d->island_dofind + d->island_dofadr[island];
+  int* resind = d->island_efcind + d->island_efcadr[island];
+
+  // sparse Jacobian
+  if (mj_isSparse(m)) {
+    for (int i=0; i < resnnz; i++) {
+      int row = resind[i];
+      int Jnnz = d->efc_J_rownnz[row];
+      int Jrowadr = d->efc_J_rowadr[row];
+      int* Jind = d->efc_J_colind + Jrowadr;
+      mjtNum* J = d->efc_J + Jrowadr;
+      int j = flg_resunc ? row : i;
+      res[j] = mju_dotSparse2(J, vec, Jnnz, Jind, vecnnz, vecind, flg_vecunc);
+    }
+  }
+
+  // dense Jacobian
+  else {
+    int nv = m->nv;
+    for (int i=0; i < resnnz; i++) {
+      int row = resind[i];
+      int j = flg_resunc ? row : i;
+      res[j] = mju_dotSparse(vec, d->efc_J + nv*row, vecnnz, vecind, flg_vecunc);
+    }
+  }
+}
+
+
+
 // multiply JacobianT by vector
-void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) {
+void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) {
   // exit if no constraints
   if (!d->nefc) {
     return;
@@ -416,6 +447,50 @@ void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec)
 
 
 
+// multiply Jacobian transpose by vector, for one island
+//  flg_resunc and flg_vecunc denote whether res/vec are uncompressed
+void mj_mulJacTVec_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
+                          int island, int flg_resunc, int flg_vecunc) {
+  // no island, call regular function
+  if (island < 0) {
+    mj_mulJacTVec(m, d, res, vec);
+    return;
+  }
+
+  // sizes
+  int vecnnz = d->island_efcnum[island];
+  int resnnz = d->island_dofnum[island];
+
+  // indices
+  int* vecind = d->island_efcind + d->island_efcadr[island];
+  int* resind = d->island_dofind + d->island_dofadr[island];
+
+  // sparse Jacobian
+  if (mj_isSparse(m)) {
+    for (int i=0; i < resnnz; i++) {
+      int row = resind[i];
+      int JTnnz = d->efc_JT_rownnz[row];
+      int JTrowadr = d->efc_JT_rowadr[row];
+      int* JTind = d->efc_JT_colind + JTrowadr;
+      mjtNum* JT = d->efc_JT + JTrowadr;
+      int j = flg_resunc ? row : i;
+      res[j] = mju_dotSparse2(JT, vec, JTnnz, JTind, vecnnz, vecind, flg_vecunc);
+    }
+  }
+
+  // dense Jacobian
+  else {
+    int nefc = d->nefc;
+    for (int i=0; i < resnnz; i++) {
+      int row = resind[i];
+      int j = flg_resunc ? row : i;
+      res[j] = mju_dotSparse(vec, d->efc_JT + nefc*row, vecnnz, vecind, flg_vecunc);
+    }
+  }
+}
+
+
+
 //--------------------- instantiate constraints by type --------------------------------------------
 
 // equality constraints
@@ -425,13 +500,14 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
   mjtNum cpos[6], pos[2][3], ref[2], dif, deriv;
   mjtNum quat[4], quat1[4], quat2[4], quat3[4], axis[3];
   mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL;
-  mjMARKSTACK;
 
   // disabled or no equality constraints: return
   if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) {
     return;
   }
 
+  mj_markStack(d);
+
   // allocate space
   jac[0] = mj_stackAllocNum(d, 6*nv);
   jac[1] = mj_stackAllocNum(d, 6*nv);
@@ -565,11 +641,11 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
               // add first or second chain
               if (j == 0) {
                 NV = d->ten_J_rownnz[id[j]];
-                memcpy(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV*sizeof(int));
+                mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV);
                 mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV);
               } else {
                 NV2 = d->ten_J_rownnz[id[j]];
-                memcpy(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2*sizeof(int));
+                mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2);
                 mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV2);
               }
             } else {
@@ -624,7 +700,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -633,13 +709,14 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
 void mj_instantiateFriction(const mjModel* m, mjData* d) {
   int nv = m->nv, issparse = mj_isSparse(m);
   mjtNum* jac;
-  mjMARKSTACK;
 
   // disabled: return
   if (mjDISABLED(mjDSBL_FRICTIONLOSS)) {
     return;
   }
 
+  mj_markStack(d);
+
   // allocate Jacobian
   jac = mj_stackAllocNum(d, nv);
 
@@ -684,7 +761,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -694,13 +771,14 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
   int side, nv = m->nv, issparse = mj_isSparse(m);
   mjtNum margin, value, dist, angleAxis[3];
   mjtNum *jac;
-  mjMARKSTACK;
 
   // disabled: return
   if (mjDISABLED(mjDSBL_LIMIT)) {
     return;
   }
 
+  mj_markStack(d);
+
   // allocate Jacobian
   jac = mj_stackAllocNum(d, nv);
 
@@ -793,7 +871,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
   // find tendon limits
   for (int i=0; i < m->ntendon; i++) {
     if (m->tendon_limited[i]) {
-      // get value = lenth, margin
+      // get value = length, margin
       value = d->ten_length[i];
       margin = m->tendon_margin[i];
 
@@ -829,7 +907,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -840,12 +918,13 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
   int dim, b1, b2, NV = m->nv, *chain = NULL;
   mjContact* con;
   mjtNum cpos[6], cmargin[6], *jac, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r;
-  mjMARKSTACK;
 
   if (mjDISABLED(mjDSBL_CONTACT) || ncon == 0) {
     return;
   }
 
+  mj_markStack(d);
+
   // allocate Jacobian
   jac = mj_stackAllocNum(d, 6*NV);
   jacdifp = mj_stackAllocNum(d, 3*NV);
@@ -938,7 +1017,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1393,7 +1472,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
     return 0;
   }
 
-  mjMARKSTACK;
+  mj_markStack(d);
 
   if (nnz) {
     chain = mj_stackAllocInt(d, nv);
@@ -1448,10 +1527,10 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
           } else {
             if (!j) {
               NV = d->ten_J_rownnz[id[j]];
-              memcpy(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV*sizeof(int));
+              mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV);
             } else {
               NV2 = d->ten_J_rownnz[id[j]];
-              memcpy(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2*sizeof(int));
+              mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2);
             }
           }
         }
@@ -1474,7 +1553,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
     *nnz += nnze;
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
   return ne;
 }
 
@@ -1589,7 +1668,7 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) {
     return 0;
   }
 
-  mjMARKSTACK;
+  mj_markStack(d);
   int *chain = mj_stackAllocInt(d, m->nv);
 
   for (int i=0; i < ncon; i++) {
@@ -1622,7 +1701,7 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) {
     *nnz += nnzc;
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
   return nc;
 }
 
@@ -1733,6 +1812,10 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
     // supernodes of JT
     mju_superSparse(m->nv, d->efc_JT_rowsuper,
                     d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind);
+  } else {
+    if (mjENABLED(mjENBL_ISLAND)) {
+      mju_transpose(d->efc_JT, d->efc_J, d->nefc, m->nv);
+    }
   }
 
   // compute diagApprox
@@ -1747,13 +1830,14 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
 // compute efc_AR
 void mj_projectConstraint(const mjModel* m, mjData* d) {
   int nefc = d->nefc, nv = m->nv;
-  mjMARKSTACK;
 
   // nothing to do
   if (nefc == 0 || !mj_isDual(m)) {
     return;
   }
 
+  mj_markStack(d);
+
   // space for backsubM2(J')' and its traspose
   mjtNum* JM2 = mj_stackAllocNum(d, nefc*nv);
   mjtNum* JM2T = mj_stackAllocNum(d, nv*nefc);
@@ -1892,7 +1976,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1916,18 +2000,25 @@ void mj_referenceConstraint(const mjModel* m, mjData* d) {
 
 //---------------------------- update constraint state ---------------------------------------------
 
-// compute efc_state, efc_force, qfrc_constraint
+// compute efc_state, efc_force, qfrc_constraint, optionally restricted to one island
-// optional: cost(qacc) = shat(jar) where jar = Jac*qacc-aref; cone Hessians
+//  island < 0: update all d->nefc constraints
-void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
+//  island >= 0: update only d->island_efcnum[island] constraints
-                         mjtNum cost[1], int flg_coneHessian) {
+//  jar = Jac*qacc-aref is restricted to the island, in the above sense
-  int ne = d->ne, nf = d->nf, nefc = d->nefc, nv = m->nv;
+//  optional: cost(qacc) = shat(jar); cone Hessians
+void mj_constraintUpdate_island(const mjModel* m, mjData* d, const mjtNum* jar,
+                                mjtNum cost[1], int flg_coneHessian, int island) {
+  int ne = d->ne, nf = d->nf;
   const mjtNum *D = d->efc_D, *R = d->efc_R, *floss = d->efc_frictionloss;
   mjtNum* force = d->efc_force;
   mjtNum s = 0;
 
+  int nefc    = island < 0 ? d->nefc : d->island_efcnum[island];
+  int* efcind = island < 0 ? NULL    : d->island_efcind + d->island_efcadr[island];
+
   // no constraints: clear qfrc_constraint and cost, return
   if (!nefc) {
-    mju_zero(d->qfrc_constraint, nv);
+    // can only occur for island == -1
+    mju_zero(d->qfrc_constraint, m->nv);
     if (cost) {
       *cost = 0;
     }
@@ -1935,59 +2026,65 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
   }
 
   // compute unconstrained efc_force
-  for (int i=0; i < nefc; i++) {
+  for (int c=0; c < nefc; c++) {
-    force[i] = -D[i]*jar[i];
+    int i = efcind ? efcind[c] : c;
+    force[i] = -D[i]*jar[c];
   }
 
-  // equality
+  // update constraints
-  for (int i=0; i < ne; i++) {
+  for (int c=0; c < nefc; c++) {
-    if (cost) {
+    int i = efcind ? efcind[c] : c;
-      s += 0.5*D[i]*jar[i]*jar[i];
-    }
 
-    d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
+    // ==== equality
-  }
+    if (i < ne) {
-
-  // friction
-  for (int i=ne; i < ne+nf; i++) {
-    // linear negative
-    if (jar[i] <= -R[i]*floss[i]) {
       if (cost) {
-        s += -0.5*R[i]*floss[i]*floss[i] - floss[i]*jar[i];
+        s += 0.5*D[i]*jar[c]*jar[c];
       }
-
-      force[i] = floss[i];
-
-      d->efc_state[i] = mjCNSTRSTATE_LINEARNEG;
-    }
-
-    // linear positive
-    else if (jar[i] >= R[i]*floss[i]) {
-      if (cost) {
-        s += -0.5*R[i]*floss[i]*floss[i] + floss[i]*jar[i];
-      }
-
-      force[i] = -floss[i];
-
-      d->efc_state[i] = mjCNSTRSTATE_LINEARPOS;
-    }
-
-    // quadratic
-    else {
-      if (cost) {
-        s += 0.5*D[i]*jar[i]*jar[i];
-      }
-
       d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
+      continue;
     }
-  }
 
-  // contact
+    // ==== friction
-  for (int i=ne+nf; i < nefc; i++) {
+    if (i < ne + nf) {
+      // linear negative
+      if (jar[c] <= -R[i]*floss[i]) {
+        if (cost) {
+          s += -0.5*R[i]*floss[i]*floss[i] - floss[i]*jar[c];
+        }
+
+        force[i] = floss[i];
+
+        d->efc_state[i] = mjCNSTRSTATE_LINEARNEG;
+      }
+
+      // linear positive
+      else if (jar[c] >= R[i]*floss[i]) {
+        if (cost) {
+          s += -0.5*R[i]*floss[i]*floss[i] + floss[i]*jar[c];
+        }
+
+        force[i] = -floss[i];
+
+        d->efc_state[i] = mjCNSTRSTATE_LINEARPOS;
+      }
+
+      // quadratic
+      else {
+        if (cost) {
+          s += 0.5*D[i]*jar[c]*jar[c];
+        }
+
+        d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
+      }
+      continue;
+    }
+
+    // ==== contact
+
     // non-negative constraint
     if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) {
       // constraint is satisfied: no cost
-      if (jar[i] >= 0) {
+      if (jar[c] >= 0) {
         force[i] = 0;
 
         d->efc_state[i] = mjCNSTRSTATE_SATISFIED;
@@ -1996,7 +2093,7 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
       // quadratic
       else {
         if (cost) {
-          s += 0.5*D[i]*jar[i]*jar[i];
+          s += 0.5*D[i]*jar[c]*jar[c];
         }
 
         d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
@@ -2012,9 +2109,9 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
 
       // map to regular dual cone space
       mjtNum U[6];
-      U[0] = jar[i]*mu;
+      U[0] = jar[c]*mu;
       for (int j=1; j < dim; j++) {
-        U[j] = jar[i+j]*friction[j-1];
+        U[j] = jar[c+j]*friction[j-1];
       }
 
       // decompose into normal and tangent
@@ -2032,7 +2129,7 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
       else if (mu*N+T <= 0 || (T <= 0 && N < 0)) {
         if (cost) {
           for (int j=0; j < dim; j++) {
-            s += 0.5*D[i+j]*jar[i+j]*jar[i+j];
+            s += 0.5*D[i+j]*jar[c+j]*jar[c+j];
           }
         }
 
@@ -2106,15 +2203,26 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
       }
 
       // advance to end of contact
-      i += (dim-1);
+      c += (dim-1);
     }
   }
 
   // compute qfrc_constraint
-  mj_mulJacTVec(m, d, d->qfrc_constraint, d->efc_force);
+  int flg_vecunc = 1;
+  int flg_resunc = 1;
+  mj_mulJacTVec_island(m, d, d->qfrc_constraint, d->efc_force, island, flg_vecunc, flg_resunc);
 
   // assign cost
   if (cost) {
     *cost = s;
   }
 }
+
+
+
+// compute efc_state, efc_force, qfrc_constraint
+// optional: cost(qacc) = shat(jar) where jar = Jac*qacc-aref; cone Hessians
+void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
+                         mjtNum cost[1], int flg_coneHessian) {
+  mj_constraintUpdate_island(m, d, jar, cost, flg_coneHessian, -1);
+}

src/engine/engine_core_constraint.h

@@ -36,11 +36,18 @@ MJAPI int mj_isSparse(const mjModel* m);
 MJAPI int mj_isDual(const mjModel* m);
 
 // multiply Jacobian by vector
-MJAPI void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
+MJAPI void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
+
+// multiply Jacobian by vector, for one island
+MJAPI void mj_mulJacVec_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
+                               int island, int flg_resunc, int flg_vecunc);
 
 // multiply JacobianT by vector
-MJAPI void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
+MJAPI void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
 
+// multiply JacobianT by vector, for one island
+MJAPI void mj_mulJacTVec_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
+                                int island, int flg_resunc, int flg_vecunc);
 
 //-------------------------- utility functions -----------------------------------------------------
 
@@ -109,6 +116,10 @@ MJAPI void mj_referenceConstraint(const mjModel* m, mjData* d);
 MJAPI void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
                                mjtNum cost[1], int flg_coneHessian);
 
+// compute efc_state, efc_force, qfrc_constraint for one island
+MJAPI void mj_constraintUpdate_island(const mjModel* m, mjData* d, const mjtNum* jar,
+                                      mjtNum cost[1], int flg_coneHessian, int island);
+
 #ifdef __cplusplus
 }
 #endif

src/engine/engine_core_smooth.c

@@ -178,7 +178,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
 // map inertias and motion dofs to global frame centered at subtree-CoM
 void mj_comPos(const mjModel* m, mjData* d) {
   mjtNum offset[3], axis[3];
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* mass_subtree = mj_stackAllocNum(d, m->nbody);
 
   // clear subtree
@@ -261,7 +261,7 @@ void mj_comPos(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -394,13 +394,13 @@ void mj_tendon(const mjModel* m, mjData* d) {
   mjtNum dif[3], divisor, wpnt[12], wlen;
   mjtNum *L = d->ten_length, *J = d->ten_J;
   mjtNum *jac1, *jac2, *jacdif, *tmp, *sparse_buf = NULL;
-  mjMARKSTACK;
 
   if (!nten) {
     return;
   }
 
   // allocate space
+  mj_markStack(d);
   jac1 = mj_stackAllocNum(d, 3*nv);
   jac2 = mj_stackAllocNum(d, 3*nv);
   jacdif = mj_stackAllocNum(d, 3*nv);
@@ -417,7 +417,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
 
   // clear Jacobian: sparse or dense
   if (issparse) {
-    memset(rownnz, 0, nten*sizeof(int));
+    mju_zeroInt(rownnz, nten);
   } else {
     mju_zero(J, nten*nv);
   }
@@ -609,7 +609,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -622,13 +622,13 @@ void mj_transmission(const mjModel* m, mjData* d) {
   mjtNum *jac, *jacA, *jacS;
   mjtNum *length = d->actuator_length, *moment = d->actuator_moment, *gear;
   mjtNum *jacref = NULL, *moment_tmp = NULL;  // required for site actuators
-  mjMARKSTACK;
 
   if (!nu) {
     return;
   }
 
   // allocate space, clear moments
+  mj_markStack(d);
   jac  = mj_stackAllocNum(d, 3*nv);
   jacA = mj_stackAllocNum(d, 3*nv);
   jacS = mj_stackAllocNum(d, 3*nv);
@@ -954,7 +954,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1086,7 +1086,7 @@ void mj_factorM(const mjModel* m, mjData* d) {
 
 
 
-// sparse backsubstitution:  x = inv(L'*D*L)*y
+// in-place sparse backsubstitution:  x = inv(L'*D*L)*x
 //  L is in lower triangle of qLD; D is on diagonal of qLD
 //  handle n vectors at once
 void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
@@ -1209,6 +1209,66 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
 }
 
 
+// in-place sparse backsubstitution for one island:  x = inv(L'*D*L)*x
+//  L is in lower triangle of qLD; D is on diagonal of qLD
+void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int island) {
+  // local constants: general
+  const int* Madr = m->dof_Madr;
+  const int* parentid = m->dof_parentid;
+  const mjtNum* qLD = d->qLD;
+  const mjtNum* qLDiagInv = d->qLDiagInv;
+  const int* simplenum = m->dof_simplenum;
+
+  // local constants: island specific
+  int ndof = d->island_dofnum[island];
+  const int* dofind = d->island_dofind + d->island_dofadr[island];
+  const int* islandind = d->dof_islandind;
+
+  // x <- inv(L') * x; skip simple, exploit sparsity of input vector
+  for (int k=ndof-1; k >= 0; k--) {
+    int i = dofind[k];
+    if (!simplenum[i] && x[k]) {
+      // init
+      int Madr_ij = Madr[i]+1;
+      int j = parentid[i];
+
+      // traverse ancestors backwards
+      // read directly from x[l] since j cannot be a parent of itself
+      while (j >= 0) {
+        x[islandind[j]] -= qLD[Madr_ij++]*x[k];         // x(j) -= L(i,j) * x(i)
+
+        // advance to parent
+        j = parentid[j];
+      }
+    }
+  }
+
+  // x <- inv(D) * x
+  for (int k=ndof-1; k >= 0; k--) {
+    x[k] *= qLDiagInv[dofind[k]];  // x(i) /= L(i,i)
+  }
+
+  // x <- inv(L) * x; skip simple
+  for (int k=0; k < ndof; k++) {
+    int i = dofind[k];
+    if (!simplenum[i]) {
+      // init
+      int Madr_ij = Madr[i]+1;
+      int j = parentid[i];
+
+      // traverse ancestors backwards
+      // write directly in x[i] since i cannot be a parent of itself
+      while (j >= 0) {
+        x[k] -= qLD[Madr_ij++]*x[islandind[j]];             // x(i) -= L(i,j) * x(j)
+
+        // advance to parent
+        j = parentid[j];
+      }
+    }
+  }
+}
+
+
 
 // half of sparse backsubstitution:  x = sqrt(inv(D))*inv(L')*y
 void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
@@ -1323,7 +1383,7 @@ void mj_comVel(const mjModel* m, mjData* d) {
 // subtree linear velocity and angular momentum
 void mj_subtreeVel(const mjModel* m, mjData* d) {
   mjtNum dx[3], dv[3], dp[3], dL[3];
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* body_vel = mj_stackAllocNum(d, 6*m->nbody);
 
   // bodywise quantities
@@ -1380,7 +1440,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
     mju_addTo3(d->subtree_angmom+3*parent, dL);
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1389,7 +1449,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
 // RNE: compute M(qpos)*qacc + C(qpos,qvel); flg_acc=0 removes inertial term
 void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
   mjtNum tmp[6], tmp1[6];
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* loc_cacc = mj_stackAllocNum(d, m->nbody*6);
   mjtNum* loc_cfrc_body = mj_stackAllocNum(d, m->nbody*6);
 
@@ -1435,7 +1495,7 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
     result[i] = mju_dot(d->cdof+6*i, loc_cfrc_body+6*m->dof_bodyid[i], 6);
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 

src/engine/engine_core_smooth.h

@@ -59,6 +59,9 @@ MJAPI void mj_solveLD(const mjModel* m, mjtNum* x, int n,
 // sparse backsubstitution:  x = inv(L'*D*L)*y, use factorization in d
 MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n);
 
+// sparse backsubstitution for one island:  x = inv(L'*D*L)*x, use factorization in d
+MJAPI void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* x, int island);
+
 // half of sparse backsubstitution:  x = sqrt(inv(D))*inv(L')*y
 MJAPI void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n);
 

src/engine/engine_crossplatform.c

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+void _mj_crossplatform_void(void) {}  // ISO C does not permit empty translation units
+
 #if defined(__APPLE__) && defined(__AVX__)
 
 #include <stdio.h>
@@ -23,7 +25,7 @@ __attribute__((weak, visibility("default"))) void _mj_rosettaError(const char* m
   __asm__ __volatile__ ("ud2");  // raises SIGILL but leave this function at the top of the stack
 }
 
-__attribute__((constructor(10000), target("no-avx"))) static void _mj_checkRosetta() {
+__attribute__((constructor(10000), target("no-avx"))) static void _mj_checkRosetta(void) {
   int is_translated = 0;
   {
     size_t len = sizeof(is_translated);

src/engine/engine_crossplatform.h

@@ -25,7 +25,7 @@
 #endif
 // IWYU pragma: end_keep
 
-// Windows
+// Sorting and case-insensitive comparison functions.
 #ifdef _WIN32
   #define strcasecmp _stricmp
   #define strncasecmp _strnicmp
@@ -34,20 +34,15 @@
       qsort_s(buf, elnum, elsz, func, context)
   #define quicksortfunc(name, context, el1, el2) \
       static int name(void* context, const void* el1, const void* el2)
-
+#else  // assumes POSIX
-// Unix-common
-#else
   #include <strings.h>
 
-  // Apple
   #ifdef __APPLE__
     #define mjQUICKSORT(buf, elnum, elsz, func, context) \
         qsort_r(buf, elnum, elsz, context, func)
     #define quicksortfunc(name, context, el1, el2) \
         static int name(void* context, const void* el1, const void* el2)
-
+  #else  // non-Apple
-  // non-Apple
-  #else
     #define mjQUICKSORT(buf, elnum, elsz, func, context) \
         qsort_r(buf, elnum, elsz, func, context)
     #define quicksortfunc(name, context, el1, el2) \
@@ -55,6 +50,7 @@
   #endif
 #endif
 
+// Switch-case fallthrough annotation.
 #if defined(__cplusplus)
   #define mjFALLTHROUGH [[fallthrough]]
 #elif defined(__clang__) || (defined(__GNUC__) && __GNUC__ >= 7)
@@ -63,10 +59,20 @@
   #define mjFALLTHROUGH ((void) 0)
 #endif
 
+// MSVC only provides max_align_t in C++.
 #if defined(_MSC_VER) && !defined(__clang__) && !defined(__cplusplus)
   typedef long double mjtMaxAlign;
 #else
   typedef max_align_t mjtMaxAlign;
 #endif
 
+// Branch prediction hints.
+#if defined(__GNUC__)
+  #define mjLIKELY(x) __builtin_expect(!!(x), 1)
+  #define mjUNLIKELY(x) __builtin_expect(!!(x), 0)
+#else
+  #define mjLIKELY(x) (x)
+  #define mjUNLIKELY(x) (x)
+#endif
+
 #endif  // MUJOCO_SRC_ENGINE_ENGINE_CROSSPLATFORM_H_

src/engine/engine_derivative.c

@@ -399,7 +399,7 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) {
   int nv = m->nv, nbody = m->nbody;
   mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6];
 
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* Dcvel = mj_stackAllocNum(d, nbody*6*nv);
   mjtNum* Dcdofdot = mj_stackAllocNum(d, nv*6*nv);
   mjtNum* Dcacc = mj_stackAllocNum(d, nbody*6*nv);
@@ -470,7 +470,7 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -614,7 +614,7 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
 
   mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6];
 
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* Dcdofdot = mj_stackAllocNum(d, 6*m->nD);
   mjtNum* Dcvel = mj_stackAllocNum(d, 6*m->nB);
   mjtNum* Dcacc = mj_stackAllocNum(d, 6*m->nB);
@@ -687,7 +687,7 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
     mju_subFrom(d->qDeriv + Dadr[j], row, Bnnz[i]);
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -734,7 +734,7 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum*
   int nv = m->nv;
 
   // allocate dense row
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* row = mj_stackAllocNum(d, nv);
 
   // process non-zero elements of B
@@ -760,7 +760,7 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum*
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -773,7 +773,7 @@ static void addJTBJSparse(
   int nv = m->nv;
 
   // allocate row
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* row = mj_stackAllocNum(d, nv);
 
   // compute qDeriv(k,p) += sum_{i,j} ( J(i,k)*B(i,j)*J(j,p) )
@@ -808,7 +808,7 @@ static void addJTBJSparse(
   }
 
   // free space
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1226,7 +1226,7 @@ static inline void mjd_magnus_force(
 
 // fluid forces based on ellipsoid approximation
 void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
-  mjMARKSTACK;
+  mj_markStack(d);
 
   int nv = m->nv;
   int nnz = nv;
@@ -1331,14 +1331,14 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
 // fluid forces based on inertia-box approximation
 void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
 {
-  mjMARKSTACK;
+  mj_markStack(d);
 
   int nv = m->nv;
   int rownnz[6], rowadr[6];
@@ -1488,7 +1488,7 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 

src/engine/engine_derivative_fd.c

@@ -27,6 +27,7 @@
 #include "engine/engine_support.h"
 #include "engine/engine_util_blas.h"
 #include "engine/engine_util_errmem.h"
+#include "engine/engine_util_misc.h"
 
 
 
@@ -196,13 +197,13 @@ static void inverseSkip(const mjModel* m, mjData* d, mjtStage stage, int skipsen
 void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) {
   int nv = m->nv;
 
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* qfrc_passive = mj_stackAllocNum(d, nv);
   mjtNum* fd = mj_stackAllocNum(d, nv);
   int* cnt = mj_stackAllocInt(d, nv);
 
   // clear row counters
-  memset(cnt, 0, nv*sizeof(int));
+  mju_zeroInt(cnt, nv);
 
   // save qfrc_passive, assume mj_fwdVelocity was called
   mju_copy(qfrc_passive, d->qfrc_passive, nv);
@@ -236,7 +237,7 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) {
   // restore
   mj_fwdVelocity(m, d);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -247,14 +248,14 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) {
 void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
   int nv = m->nv;
 
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* plus = mj_stackAllocNum(d, nv);
   mjtNum* minus = mj_stackAllocNum(d, nv);
   mjtNum* fd = mj_stackAllocNum(d, nv);
   int* cnt = mj_stackAllocInt(d, nv);
 
   // clear row counters
-  memset(cnt, 0, nv*sizeof(int));
+  mju_zeroInt(cnt, nv);
 
   // loop over dofs
   for (int i=0; i < nv; i++) {
@@ -302,7 +303,7 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
   mj_fwdVelocity(m, d);
   mj_fwdActuation(m, d);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -329,7 +330,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
                 mjtNum* DsDq, mjtNum* DsDv, mjtNum* DsDa, mjtNum* DsDu) {
   int nq = m->nq, nv = m->nv, na = m->na, nu = m->nu, ns = m->nsensordata;
   int ndx = 2*nv+na;  // row length of Dy Jacobians
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // states
   mjtNum *state      = mj_stackAllocNum(d, nq+nv+na);  // current state
@@ -557,7 +558,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -579,7 +580,7 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente
   mjtNum *DyDq, *DyDv, *DyDa, *DsDq, *DsDv, *DsDa;
   DyDq = DyDv = DyDa = DsDq = DsDv = DsDa = NULL;
 
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // allocate transposed matrices
   mjtNum *AT = A ? mj_stackAllocNum(d, ndx*ndx) : NULL;  // state-transition matrix   (transposed)
@@ -610,7 +611,7 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente
   if (C) mju_transpose(C, CT, ndx, ns);
   if (D) mju_transpose(D, DT, nu, ns);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 // finite differenced Jacobians of (force, sensors) = mj_inverse(state, acceleration)
@@ -634,7 +635,6 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio
                    mjtNum *DsDq, mjtNum *DsDv, mjtNum *DsDa,
                    mjtNum *DmDq) {
   int nq = m->nq, nv = m->nv, nM = m->nM, ns = m->nsensordata;
-  mjMARKSTACK;
 
   if (m->opt.integrator == mjINT_RK4) {
     mjERROR("RK4 integrator is not supported");
@@ -648,6 +648,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio
   int skipsensor = !DsDq && !DsDv && !DsDa;
 
   // local vectors
+  mj_markStack(d);
   mjtNum *pos        = mj_stackAllocNum(d, nq);                      // position
   mjtNum *force      = mj_stackAllocNum(d, nv);                      // force
   mjtNum *force_plus = mj_stackAllocNum(d, nv);                      // nudged force
@@ -730,5 +731,5 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }

src/engine/engine_forward.c

@@ -199,7 +199,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
   }
 
   // local, clamped copy of ctrl
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum *ctrl = mj_stackAllocNum(d, nu);
   if (mjDISABLED(mjDSBL_CLAMPCTRL)) {
     mju_copy(ctrl, d->ctrl, nu);
@@ -392,7 +392,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
   TM_END(mjTIMER_ACTUATION);
 }
 
@@ -401,7 +401,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
 // add up all non-constraint forces, compute qacc_smooth
 void mj_fwdAcceleration(const mjModel* m, mjData* d) {
   TM_START;
-  mjMARKSTACK;
+  mj_markStack(d);
   int nv = m->nv;
 
   // qforce = sum of all non-constraint forces
@@ -413,7 +413,7 @@ void mj_fwdAcceleration(const mjModel* m, mjData* d) {
   // qacc_smooth = M \ qfr_smooth
   mj_solveM(m, d, d->qacc_smooth, d->qfrc_smooth, 1);
 
-  mjFREESTACK;
+  mj_freeStack(d);
   TM_END(mjTIMER_ACCELERATION);
 }
 
@@ -425,7 +425,7 @@ static void warmstart(const mjModel* m, mjData* d) {
 
   // warmstart with best of (qacc_warmstart, qacc_smooth)
   if (!mjDISABLED(mjDSBL_WARMSTART)) {
-    mjMARKSTACK;
+    mj_markStack(d);
     mjtNum* jar = mj_stackAllocNum(d, nefc);
 
     // start with qacc = qacc_warmstart
@@ -479,7 +479,7 @@ static void warmstart(const mjModel* m, mjData* d) {
       }
     }
 
-    mjFREESTACK;
+    mj_freeStack(d);
   }
 
   // coldstart with qacc = qacc_smooth, efc_force = 0
@@ -589,7 +589,7 @@ static void mj_advance(const mjModel* m, mjData* d,
 // Euler integrator, semi-implicit in velocity, possibly skipping factorisation
 void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
   int nv = m->nv, nM = m->nM;
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* qfrc = mj_stackAllocNum(d, nv);
   mjtNum* qacc = mj_stackAllocNum(d, nv);
 
@@ -633,7 +633,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
   // advance state and time
   mj_advance(m, d, d->act_dot, qacc, NULL);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -665,7 +665,6 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
   mjtNum C[9], T[9], *X[10], *F[10], *dX;
   const mjtNum* A = (N == 4 ? RK4_A : 0);
   const mjtNum* B = (N == 4 ? RK4_B : 0);
-  mjMARKSTACK;
 
   // check order
   if (!A) {
@@ -673,6 +672,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
   }
 
   // allocate space for intermediate solutions
+  mj_markStack(d);
   dX = mj_stackAllocNum(d, 2*nv+na);
   for (int i=0; i < N; i++) {
     X[i] = mj_stackAllocNum(d, nq+nv+na);
@@ -746,7 +746,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
   // advance state and time
   mj_advance(m, d, dX+2*nv, dX+nv, dX);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -755,7 +755,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
 void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
   int nv = m->nv;
 
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* qfrc = mj_stackAllocNum(d, nv);
   mjtNum* qacc = mj_stackAllocNum(d, nv);
 
@@ -810,7 +810,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
   // advance state and time
   mj_advance(m, d, d->act_dot, qacc, NULL);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 

src/engine/engine_inverse.c

@@ -95,7 +95,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
   int nv = m->nv, dof_damping;
   mjtNum *qacc = d->qacc;
 
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* qfrc = mj_stackAllocNum(d, nv);
 
   // use selected integrator
@@ -119,6 +119,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
 
     // if disabled or no dof damping, nothing to do
     if (!dof_damping) {
+      mj_freeStack(d);
       return;
     }
 
@@ -168,7 +169,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
   // solve for qacc: qfrc = M * qacc
   mj_solveM(m, d, qacc, qfrc, 1);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -185,7 +186,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) {
     return;
   }
 
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* jar = mj_stackAllocNum(d, nefc);
 
   // compute jar = Jac*qacc - aref
@@ -195,7 +196,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) {
   // call update function
   mj_constraintUpdate(m, d, jar, NULL, 0);
 
-  mjFREESTACK;
+  mj_freeStack(d);
   TM_END(mjTIMER_CONSTRAINT);
 }
 
@@ -205,7 +206,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) {
 void mj_inverseSkip(const mjModel* m, mjData* d,
                     int skipstage, int skipsensor) {
   TM_START;
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* qacc;
   int nv = m->nv;
 
@@ -258,7 +259,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d,
     mju_copy(d->qacc, qacc, nv);
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
   TM_END(mjTIMER_INVERSE);
 }
 
@@ -277,7 +278,6 @@ void mj_inverse(const mjModel* m, mjData* d) {
 void mj_compareFwdInv(const mjModel* m, mjData* d) {
   int nv = m->nv, nefc = d->nefc;
   mjtNum *qforce, *dif, *save_qfrc_constraint, *save_efc_force;
-  mjMARKSTACK;
 
   // clear result, return if no constraints
   d->solver_fwdinv[0] = d->solver_fwdinv[1] = 0;
@@ -286,6 +286,7 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) {
   }
 
   // allocate
+  mj_markStack(d);
   qforce = mj_stackAllocNum(d, nv);
   dif = mj_stackAllocNum(d, nv);
   save_qfrc_constraint = mj_stackAllocNum(d, nv);
@@ -313,5 +314,5 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) {
   mju_copy(d->qfrc_constraint, save_qfrc_constraint, nv);
   mju_copy(d->efc_force, save_efc_force, nefc);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }

src/engine/engine_io.c

@@ -26,7 +26,7 @@
 #include <mujoco/mjmacro.h>
 #include <mujoco/mjplugin.h>
 #include <mujoco/mjxmacro.h>
-#include "engine/engine_array_safety.h"  // IWYU pragma: keep
+#include "engine/engine_crossplatform.h"
 #include "engine/engine_resource.h"
 #include "engine/engine_macro.h"
 #include "engine/engine_plugin.h"
@@ -35,6 +35,11 @@
 #include "engine/engine_util_misc.h"
 #include "engine/engine_vfs.h"
 
+#ifdef ADDRESS_SANITIZER
+  #include <sanitizer/asan_interface.h>
+  #include <sanitizer/common_interface_defs.h>
+#endif
+
 #ifdef MEMORY_SANITIZER
   #include <sanitizer/msan_interface.h>
 #endif
@@ -43,8 +48,30 @@
   #pragma warning (disable: 4305)  // disable MSVC warning: truncation from 'double' to 'float'
 #endif
 
+// add red zone padding when built with asan, to detect out-of-bound accesses
+#ifdef ADDRESS_SANITIZER
+  #define mjREDZONE 32
+#else
+  #define mjREDZONE 0
+#endif
+
 static const int MAX_ARRAY_SIZE = INT_MAX / 4;
 
+// compute a % b with a fast code path if the second argument is a power of 2
+static inline size_t fastmod(size_t a, size_t b) {
+  // (b & (b - 1)) == 0 implies that b is a power of 2
+  if (mjLIKELY((b & (b - 1)) == 0)) {
+    return a & (b - 1);
+  }
+  return a % b;
+}
+
+typedef struct {
+  size_t pbase;   // value of d->pbase immediately before mj_markStack
+  size_t pstack;  // value of d->pstack immediately before mj_markStack
+  void* pc;       // program counter of the call site of mj_markStack (only set when under asan)
+} mjStackFrame;
+
 //------------------------------ mjLROpt -----------------------------------------------------------
 
 // set default options for length range computation
@@ -86,6 +113,9 @@ void mj_defaultSolRefImp(mjtNum* solref, mjtNum* solimp) {
 
 // set model options to default values
 void mj_defaultOption(mjOption* opt) {
+  // fill opt with zeros in case struct is padded
+  memset(opt, 0, sizeof(mjOption));
+
   // timing parameters
   opt->timestep           = 0.002;
   opt->apirate            = 100;
@@ -93,6 +123,7 @@ void mj_defaultOption(mjOption* opt) {
   // solver parameters
   opt->impratio           = 1;
   opt->tolerance          = 1e-8;
+  opt->ls_tolerance       = 0.01;
   opt->noslip_tolerance   = 1e-6;
   opt->mpr_tolerance      = 1e-6;
 
@@ -120,6 +151,7 @@ void mj_defaultOption(mjOption* opt) {
   opt->jacobian           = mjJAC_AUTO;
   opt->solver             = mjSOL_NEWTON;
   opt->iterations         = 100;
+  opt->ls_iterations      = 50;
   opt->noslip_iterations  = 0;
   opt->mpr_iterations     = 50;
   opt->disableflags       = 0;
@@ -656,7 +688,7 @@ void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buff
 
 
 // load model from binary MJB resource
-static mjModel* _mj_loadModel(const char* filename, int vfs_provider) {
+mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
   int header[NHEADER] = {0};
   int expected_header[NHEADER] = {ID, sizeof(mjtNum), getnint(), getnsize(), getnptr()};
   int ints[256];
@@ -665,8 +697,11 @@ static mjModel* _mj_loadModel(const char* filename, int vfs_provider) {
   mjModel *m = 0;
   mjResource* r = NULL;
 
-  if((r = mju_openResource(filename, vfs_provider)) == NULL) {
+  // first try vfs, otherwise try a provider or OS filesystem
-    return NULL;
+  if ((r = mju_openVfsResource(filename, vfs)) == NULL) {
+    if ((r = mju_openResource(filename)) == NULL) {
+      return NULL;
+    }
   }
 
   const void* buffer = NULL;
@@ -780,27 +815,6 @@ static mjModel* _mj_loadModel(const char* filename, int vfs_provider) {
 
 
 
-// load model from binary MJB file
-//  if vfs is not NULL, look up file in vfs before reading from disk
-mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
-  if (vfs == NULL) {
-    return _mj_loadModel(filename, 0);
-  }
-
-  int index = mj_registerVfsProvider(vfs);
-  if (index < 1) {
-    mjERROR("could not allocate memory");
-    return NULL;
-  }
-
-  mjModel* model = _mj_loadModel(filename, index);
-
-  mjp_unregisterResourceProvider(index);
-  return model;
-}
-
-
-
 // de-allocate mjModel
 void mj_deleteModel(mjModel* m) {
   if (m) {
@@ -841,11 +855,11 @@ static void makeDSparse(const mjModel* m, mjData* d) {
   int* rowadr = d->D_rowadr;
   int* colind = d->D_colind;
 
-  mjMARKSTACK;
+  mj_markStack(d);
   int* remaining = mj_stackAllocInt(d, nv);
 
   // compute rownnz
-  memset(rownnz, 0, nv * sizeof(int));
+  mju_zeroInt(rownnz, nv);
   for (int i = nv - 1; i >= 0; i--) {
     // init at diagonal
     int j = i;
@@ -865,7 +879,7 @@ static void makeDSparse(const mjModel* m, mjData* d) {
   }
 
   // populate colind
-  memcpy(remaining, rownnz, nv * sizeof(int));
+  mju_copyInt(remaining, rownnz, nv);
   for (int i = nv - 1; i >= 0; i--) {
     // init at diagonal
     remaining[i]--;
@@ -889,7 +903,7 @@ static void makeDSparse(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -902,7 +916,7 @@ static void makeBSparse(const mjModel* m, mjData* d) {
   int* colind = d->B_colind;
 
   // set rownnz to subtree dofs counts, including self
-  memset(rownnz, 0, sizeof(int) * nbody);
+  mju_zeroInt(rownnz, nbody);
   for (int i = nbody - 1; i > 0; i--) {
     rownnz[i] += m->body_dofnum[i];
     rownnz[m->body_parentid[i]] += rownnz[i];
@@ -934,9 +948,9 @@ static void makeBSparse(const mjModel* m, mjData* d) {
   }
 
   // allocate and clear incremental row counts
-  mjMARKSTACK;
+  mj_markStack(d);
   int* cnt = mj_stackAllocInt(d, nbody);
-  memset(cnt, 0, sizeof(int) * nbody);
+  mju_zeroInt(cnt, nbody);
 
   // add subtree dofs to colind
   for (int i = nbody - 1; i > 0; i--) {
@@ -982,7 +996,7 @@ static void makeBSparse(const mjModel* m, mjData* d) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1105,6 +1119,8 @@ static mjData* _makeData(const mjModel* m) {
     }
   }
 
+  d->threadpool = 0;
+
   return d;
 }
 
@@ -1193,6 +1209,8 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) {
     }
   }
 
+  dest->threadpool = src->threadpool;
+
   return dest;
 }
 
@@ -1200,12 +1218,12 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) {
 
 // allocate memory from the mjData arena
 void* mj_arenaAlloc(mjData* d, size_t bytes, size_t alignment) {
-  size_t misalignment = d->parena % alignment;
+  size_t misalignment = fastmod(d->parena, alignment);
   size_t padding = misalignment ? alignment - misalignment : 0;
 
   // check size
   size_t bytes_available = d->narena - d->pstack;
-  if (d->parena + padding + bytes > bytes_available) {
+  if (mjUNLIKELY(d->parena + padding + bytes > bytes_available)) {
     return NULL;
   }
 
@@ -1231,17 +1249,10 @@ void* mj_arenaAlloc(mjData* d, size_t bytes, size_t alignment) {
 // declared inline so that modular arithmetic with specific alignments can be optimized out
 static inline void* stackalloc(mjData* d, size_t size, size_t alignment) {
   // return NULL if empty
-  if (!size) {
+  if (mjUNLIKELY(!size)) {
     return NULL;
   }
 
-  // add red zone padding when built with asan, to detect out-of-bound accesses
-#ifdef ADDRESS_SANITIZER
-  #define mjREDZONE 32
-#else
-  #define mjREDZONE 0
-#endif
-
   // size of entire arena/stack in bytes
   size_t stack_size_bytes = d->narena;
 
@@ -1255,7 +1266,7 @@ static inline void* stackalloc(mjData* d, size_t size, size_t alignment) {
   uintptr_t start_ptr = end_ptr - (size + mjREDZONE);
 
   // align the pointer
-  start_ptr -= start_ptr % alignment;
+  start_ptr -= fastmod(start_ptr, alignment);
 
   // new top of the stack
   uintptr_t new_pstack_ptr = start_ptr - mjREDZONE;
@@ -1268,8 +1279,8 @@ static inline void* stackalloc(mjData* d, size_t size, size_t alignment) {
   // check size
   size_t stack_available_bytes = end_ptr - ((uintptr_t)d->arena + d->parena);
   size_t stack_required_bytes = end_ptr - new_pstack_ptr;
-  if (stack_required_bytes > stack_available_bytes) {
+  if (mjUNLIKELY(stack_required_bytes > stack_available_bytes)) {
-    mju_error("mjData stack overflow: max = %zu, available = %zu, requested = %zu "
+    mju_error("mj_stackAlloc: insufficient memory: max = %zu, available = %zu, requested = %zu "
               "(ne = %d, nf = %d, nefc = %d, ncon = %d)",
               stack_size_bytes, stack_available_bytes, stack_required_bytes,
               d->ne, d->nf, d->nefc, d->ncon);
@@ -1285,16 +1296,14 @@ static inline void* stackalloc(mjData* d, size_t size, size_t alignment) {
   }
 
   // store new stack usage in the red zone
-  ASAN_UNPOISON_MEMORY_REGION(new_pstack_ptr, sizeof(size_t));
+  ASAN_UNPOISON_MEMORY_REGION((void*)new_pstack_ptr, sizeof(size_t));
   *(size_t*)new_pstack_ptr = usage;
-  ASAN_POISON_MEMORY_REGION(new_pstack_ptr, sizeof(size_t));
+  ASAN_POISON_MEMORY_REGION((void*)new_pstack_ptr, sizeof(size_t));
 
   // unpoison the actual usable allocation
-  ASAN_UNPOISON_MEMORY_REGION(start_ptr, size);
+  ASAN_UNPOISON_MEMORY_REGION((void*)start_ptr, size);
 #endif
 
-#undef mjREDZONE
-
   // update pstack and max usage statistics
   d->pstack = new_pstack;
   d->maxuse_stack = mjMAX(d->maxuse_stack, usage);
@@ -1303,6 +1312,73 @@ static inline void* stackalloc(mjData* d, size_t size, size_t alignment) {
   return (void*)start_ptr;
 }
 
+
+
+// mjData mark stack frame
+#ifdef ADDRESS_SANITIZER
+__attribute__((noinline))
+#endif
+void mj_markStack(mjData* d) {
+  size_t pstack_old = d->pstack;
+  mjStackFrame* s =
+      (mjStackFrame*) stackalloc(d, sizeof(mjStackFrame), _Alignof(mjStackFrame));
+  s->pbase = d->pbase;
+  s->pstack = pstack_old;
+#ifdef ADDRESS_SANITIZER
+  // store the program counter to the caller so that we can compare against mj_freeStack later
+  s->pc = __sanitizer_return_address();
+#endif
+  d->pbase = d->pstack - mjREDZONE;
+}
+
+
+
+// mjData free stack frame
+#ifdef ADDRESS_SANITIZER
+__attribute__((noinline))
+#endif
+void mj_freeStack(mjData* d) {
+  if (mjUNLIKELY(!d->pbase)) {
+    return;
+  }
+
+  mjStackFrame* s = (mjStackFrame*) ((char*)d->arena + d->narena - d->pbase);
+#ifdef ADDRESS_SANITIZER
+  #define mjSYMBOLIZELEN 256
+
+  // symbolize s->pc to get the function name of most recent caller to mj_markStack
+  char markstack_func[mjSYMBOLIZELEN];
+  __sanitizer_symbolize_pc(s->pc, "%f", markstack_func, mjSYMBOLIZELEN);
+  markstack_func[mjSYMBOLIZELEN - 1] = '\0';
+
+  // symbolize current program counter to get the function name of caller to this function
+  char freestack_func[mjSYMBOLIZELEN];
+  __sanitizer_symbolize_pc(__sanitizer_return_address(), "%f", freestack_func, mjSYMBOLIZELEN);
+  freestack_func[mjSYMBOLIZELEN - 1] = '\0';
+
+  // raise an error if caller function name doesn't match the most recent caller of mj_markStack
+  if (strncmp(markstack_func, freestack_func, mjSYMBOLIZELEN)) {
+    char dbginfo[mjSYMBOLIZELEN];
+    __sanitizer_symbolize_pc(
+        s->pc, "mj_markStack %F at %S has no corresponding mj_freeStack",
+        dbginfo, sizeof(dbginfo));
+    dbginfo[mjSYMBOLIZELEN - 1] = '\0';
+    mjERROR("%s", dbginfo);
+  }
+
+  #undef mjSYMBOLIZELEN
+#endif
+
+  // restore pbase and pstack
+  d->pbase = s->pbase;
+  d->pstack = s->pstack;
+
+  // if running under asan, poison the newly freed memory region
+#ifdef ADDRESS_SANITIZER
+  ASAN_POISON_MEMORY_REGION((char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
+#endif
+}
+
 void* mj_stackAlloc(mjData* d, size_t bytes, size_t alignment) {
   return stackalloc(d, bytes, alignment);
 }
@@ -1329,6 +1405,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
 
   // clear stack pointer
   d->pstack = 0;
+  d->pbase = 0;
 
   // clear arena pointers
   d->parena = 0;
@@ -1496,6 +1573,11 @@ void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) {
 // de-allocate mjData
 void mj_deleteData(mjData* d) {
   if (d) {
+#ifdef ADDRESS_SANITIZER
+    // raise an error if there's a dangling stack frame
+    mj_freeStack(d);
+#endif
+
     // destroy plugin instances
     for (int i = 0; i < d->nplugin; ++i) {
       const mjpPlugin* plugin = mjp_getPluginAtSlot(d->plugin[i]);

src/engine/engine_io.h

@@ -18,6 +18,7 @@
 #include <mujoco/mjdata.h>
 #include <mujoco/mjexport.h>
 #include <mujoco/mjmodel.h>
+#include <mujoco/mjxmacro.h>
 
 #ifdef __cplusplus
 #include <cstddef>
@@ -105,6 +106,12 @@ MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
 // mjData arena allocate
 MJAPI void* mj_arenaAlloc(mjData* d, size_t bytes, size_t alignment);
 
+// mjData mark stack frame
+MJAPI void mj_markStack(mjData* d);
+
+// mjData free stack frame
+MJAPI void mj_freeStack(mjData* d);
+
 // mjData stack allocate
 MJAPI void* mj_stackAlloc(mjData* d, size_t bytes, size_t alignment);
 
@@ -117,6 +124,16 @@ MJAPI int* mj_stackAllocInt(mjData* d, int size);
 // de-allocate data
 MJAPI void mj_deleteData(mjData* d);
 
+// clear arena pointers in mjData
+static inline void mj_clearEfc(mjData* d) {
+#define X(type, name, nr, nc) d->name = NULL;
+  MJDATA_ARENA_POINTERS
+#undef X
+  d->nefc = 0;
+  d->nisland = 0;
+  d->contact = (mjContact*) d->arena;
+}
+
 #ifdef __cplusplus
 }
 #endif

src/engine/engine_island.c

@@ -16,17 +16,15 @@
 
 #include <stdio.h>
 #include <stddef.h>
-#include <string.h>
 
 #include <mujoco/mjdata.h>
-#include <mujoco/mjmacro.h>
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjxmacro.h>
 #include "engine/engine_core_constraint.h"
-#include "engine/engine_crossplatform.h"
 #include "engine/engine_io.h"
 #include "engine/engine_support.h"
 #include "engine/engine_util_errmem.h"
+#include "engine/engine_util_misc.h"
 
 #ifdef MEMORY_SANITIZER
   #include <sanitizer/msan_interface.h>
@@ -75,7 +73,7 @@ int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, cons
       island[v] = nisland;
 
       // push adjacent vertices onto stack
-      memcpy(stack + nstack, colind + rowadr[v], rownnz[v]*sizeof(int));
+      mju_copyInt(stack + nstack, colind + rowadr[v], rownnz[v]);
       nstack += rownnz[v];
     }
 
@@ -160,13 +158,12 @@ static int countMaxEdge(const mjModel* m, const mjData* d) {
 
 
 // return id of next tree in Jacobian row i that is different from tree, -1 if not found
-//   write the index of the found tree to *index if given
+//   start search from *index
-//   start search from *index if given, otherwise 0
+//   write the index of the found tree to *index
-//   if J is (dense/sparse) *index is the (column/nonzro) index, respectively
+//   if J is (dense/sparse) *index is the (column/nonzero) index, respectively
 static int treeNext(const mjModel* m, const mjData* d, int tree, int i, int *index) {
   int tree_next = -1;
-  int j0 = index ? *index : 0;  // start searching at *index if given, otherwise 0
+  int j;  // local loop variable, saved to *index
-  int j;  // loop variable, saved to *index
 
   // sparse
   if (mj_isSparse(m)) {
@@ -174,7 +171,7 @@ static int treeNext(const mjModel* m, const mjData* d, int tree, int i, int *ind
     int* colind = d->efc_J_colind + d->efc_J_rowadr[i];
 
     // loop over remaining nonzeros, look for different tree
-    for (j=j0; j < rownnz; j++) {
+    for (j=(*index); j < rownnz; j++) {
       int tree_j = m->dof_treeid[colind[j]];
       if (tree_j != tree) {
         // found different tree
@@ -189,7 +186,7 @@ static int treeNext(const mjModel* m, const mjData* d, int tree, int i, int *ind
     int nv = m->nv;
 
     // scan row, look for different tree
-    for (j=j0; j < nv; j++) {
+    for (j=(*index); j < nv; j++) {
       if (d->efc_J[nv*i + j]) {
         int tree_j = m->dof_treeid[j];
         if (tree_j != tree) {
@@ -202,13 +199,94 @@ static int treeNext(const mjModel* m, const mjData* d, int tree, int i, int *ind
   }
 
   // save last index
-  if (index) *index = j;
+  *index = j;
 
   return tree_next;
 }
 
 
 
+// find first and possibly second nonegative tree ids in Jacobian row i
+//   if row i is special-cased (no more trees), return -1
+//   otherwise call treeNext, starting scan at index 0, return index
+static int treeFirst(const mjModel* m, const mjData* d, int tree[2], int i) {
+  int efc_type = d->efc_type[i];
+  int efc_id = d->efc_id[i];
+
+  // clear outputs
+  tree[0] = -1;
+  tree[1] = -1;
+
+  // ==== fast handling of special cases
+
+  // joint friction
+  if (efc_type == mjCNSTR_FRICTION_DOF) {
+    tree[0] = m->dof_treeid[efc_id];
+    return -1;
+  }
+
+  // joint limit
+  if (efc_type == mjCNSTR_LIMIT_JOINT) {
+    tree[0] = m->dof_treeid[m->jnt_dofadr[efc_id]];
+    return -1;
+  }
+
+  // contact
+  if (efc_type == mjCNSTR_CONTACT_FRICTIONLESS ||
+      efc_type == mjCNSTR_CONTACT_PYRAMIDAL ||
+      efc_type == mjCNSTR_CONTACT_ELLIPTIC) {
+    tree[0] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom1]];
+    tree[1] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom2]];
+
+    // handle static bodies
+    if (tree[0] < 0) {
+      if (tree[1] < 0) {
+        mjERROR("contact %d is between two static bodies", efc_id);  // SHOULD NOT OCCUR
+      } else {
+        int tmp = tree[0];
+        tree[0] = tree[1];
+        tree[1] = tmp;
+      }
+    }
+
+    return -1;
+  }
+
+  // connect or weld constraints
+  if (efc_type == mjCNSTR_EQUALITY) {
+    mjtEq eq_type = m->eq_type[efc_id];
+    if (eq_type == mjEQ_CONNECT || eq_type == mjEQ_WELD) {
+      tree[0] = m->body_treeid[m->eq_obj1id[efc_id]];
+      tree[1] = m->body_treeid[m->eq_obj2id[efc_id]];
+
+      // handle static bodies
+      if (tree[0] < 0) {
+        if (tree[1] < 0) {
+          mjERROR("equality %d is between two static bodies", efc_id);  // SHOULD NOT OCCUR
+        } else {
+          int tmp = tree[0];
+          tree[0] = tree[1];
+          tree[1] = tmp;
+        }
+      }
+
+      return -1;
+    }
+  }
+
+  // ==== generic case: scan Jacobian
+  int index = 0;
+  tree[0] = treeNext(m, d, -1, i, &index);
+
+  if (tree[0] < 0) {
+    mjERROR("no tree found for constraint %d", i);  // SHOULD NOT OCCUR
+  }
+
+  return index;
+}
+
+
+
 // add 0 edges, 1 self-edge or 2 flipped edges to array, increment treenedge
 //   return current number of edges
 static int addEdge(int* treenedge, int* edge, int nedge, int tree1, int tree2, int nedge_max) {
@@ -273,10 +351,9 @@ static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edg
   int nefc = d->nefc;
   int efc_type = -1;
   int efc_id = -1;
-  int tree1, tree2;
 
   // clear treenedge
-  memset(treenedge, 0, m->ntree*sizeof(int));
+  mju_zeroInt(treenedge, m->ntree);
 
   int nedge = 0;
   for (int i=0; i < nefc; i++) {
@@ -287,60 +364,34 @@ static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edg
     efc_type = d->efc_type[i];
     efc_id = d->efc_id[i];
 
-    // ==== fast handling of special cases
+    int tree[2];
+    int index = treeFirst(m, d, tree, i);
+    int tree1 = tree[0];
+    int tree2 = tree[1];
 
-    // joint friction
+    // no more edges to find, add and continue
-    if (efc_type == mjCNSTR_FRICTION_DOF) {
+    if (index == -1) {
-      tree1 = m->dof_treeid[efc_id];
+      nedge = addEdge(treenedge, edge, nedge, tree1, tree2 == -1 ? tree1 : tree2, nedge_max);
-      nedge = addEdge(treenedge, edge, nedge, tree1, tree1, nedge_max);
       continue;
     }
 
-    // joint limit
+    // possibly more edges, scan Jacobian row
-    if (efc_type == mjCNSTR_LIMIT_JOINT) {
+    else {
-      tree1 = m->dof_treeid[m->jnt_dofadr[efc_id]];
+      tree2 = treeNext(m, d, tree1, i, &index);
-      nedge = addEdge(treenedge, edge, nedge, tree1, tree1, nedge_max);
-      continue;
-    }
 
-    // contact
+      if (tree2 == -1) {
-    if (efc_type == mjCNSTR_CONTACT_FRICTIONLESS ||
+        // 1 tree found: add self-edge
-        efc_type == mjCNSTR_CONTACT_PYRAMIDAL ||
+        nedge = addEdge(treenedge, edge, nedge, tree1, tree1, nedge_max);
-        efc_type == mjCNSTR_CONTACT_ELLIPTIC) {
+      } else {
-      tree1 = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom1]];
+        // 2 trees found: add edge, keep scanning and adding until no more trees
-      tree2 = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom2]];
-      nedge = addEdge(treenedge, edge, nedge, tree1, tree2, nedge_max);
-      continue;
-    }
-
-    // connect or weld constraints
-    if (efc_type == mjCNSTR_EQUALITY) {
-      mjtEq eq_type = m->eq_type[efc_id];
-      if (eq_type == mjEQ_CONNECT || eq_type == mjEQ_WELD) {
-        tree1 = m->body_treeid[m->eq_obj1id[efc_id]];
-        tree2 = m->body_treeid[m->eq_obj2id[efc_id]];
         nedge = addEdge(treenedge, edge, nedge, tree1, tree2, nedge_max);
-        continue;
+        int tree3 = treeNext(m, d, tree2, i, &index);
-      }
+        while (tree3 > -1 && tree3 != tree2) {
-    }
+          tree1 = tree2;
-
+          tree2 = tree3;
-    // ==== generic case: scan Jacobian
+          nedge = addEdge(treenedge, edge, nedge, tree1, tree2, nedge_max);
-    int index = 0;
+          tree3 = treeNext(m, d, tree2, i, &index);
-    tree1 = treeNext(m, d, -1, i, &index);
+        }
-    tree2 = treeNext(m, d, tree1, i, &index);
-
-    if (tree2 == -1) {
-      // 1 tree found: add self-edge
-      nedge = addEdge(treenedge, edge, nedge, tree1, tree1, nedge_max);
-    } else {
-      // 2 trees found: add edge, keep scanning and adding until no more trees
-      nedge = addEdge(treenedge, edge, nedge, tree1, tree2, nedge_max);
-      int tree3 = treeNext(m, d, tree2, i, &index);
-      while (tree3 > -1 && tree3 != tree2) {
-        tree1 = tree2;
-        tree2 = tree3;
-        nedge = addEdge(treenedge, edge, nedge, tree1, tree2, nedge_max);
-        tree3 = treeNext(m, d, tree2, i, &index);
       }
     }
   }
@@ -361,7 +412,7 @@ void mj_island(const mjModel* m, mjData* d) {
     return;
   }
 
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // allocate edge array
   int nedge_max = countMaxEdge(m, d);
@@ -395,7 +446,7 @@ void mj_island(const mjModel* m, mjData* d) {
 
   // allocate island arrays on arena
   if (!arenaAllocIsland(m, d)) {
-    mjFREESTACK;
+    mj_freeStack(d);
     return;
   }
 
@@ -403,10 +454,10 @@ void mj_island(const mjModel* m, mjData* d) {
 
   // compute dof_island, island_dofnum
   int num_dof_unc = 0;  // number of unconstrained dofs
-  memset(d->island_dofnum, 0, nisland*sizeof(int));
+  mju_zeroInt(d->island_dofnum, nisland);
   for (int i=0; i < nv; i++) {
     // dof_island
-    int island = tree_island[m->dof_treeid[i]];;
+    int island = tree_island[m->dof_treeid[i]];
     d->dof_island[i] = island;
 
     // island_dofnum
@@ -424,7 +475,7 @@ void mj_island(const mjModel* m, mjData* d) {
   }
 
   // reset island_dofnum
-  memset(d->island_dofnum, 0, nisland*sizeof(int));
+  mju_zeroInt(d->island_dofnum, nisland);
 
   // compute dof_islandind, island_dofind
   int num_dof_island = 0;
@@ -444,15 +495,17 @@ void mj_island(const mjModel* m, mjData* d) {
     mjERROR("not all islands assigned to dofs");
   }
 
-  // finalize dof_islandind: set remaning indices to -1
+  // finalize dof_islandind: set remaining indices to -1
   for (int i=num_dof_island; i < nv; i++) {
     d->island_dofind[i] = -1;
   }
 
   // compute efc_island, island_efcnum
-  memset(d->island_efcnum, 0, nisland*sizeof(int));
+  mju_zeroInt(d->island_efcnum, nisland);
   for (int i=0; i < nefc; i++) {
-    int island = tree_island[treeNext(m, d, -1, i, NULL)];
+    int tree[2];
+    treeFirst(m, d, tree, i);
+    int island = tree_island[tree[0]];
     d->efc_island[i] = island;
     d->island_efcnum[island]++;
   }
@@ -464,7 +517,7 @@ void mj_island(const mjModel* m, mjData* d) {
   }
 
   // reset island_efcnum
-  memset(d->island_efcnum, 0, nisland*sizeof(int));
+  mju_zeroInt(d->island_efcnum, nisland);
 
   // compute efc_islandind
   for (int i=0; i < nefc; i++) {
@@ -472,5 +525,5 @@ void mj_island(const mjModel* m, mjData* d) {
     d->island_efcind[d->island_efcadr[island] + (d->island_efcnum[island]++)] = i;
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }

src/engine/engine_island.h

@@ -26,7 +26,7 @@ extern "C" {
 
 // find disjoint subgraphs ("islands") given sparse symmetric adjacency matrix
 MJAPI int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind,
-                       int* scratch);
+                       int* stack);
 
 //-------------------------- top-level API for island construction ---------------------------------
 

src/engine/engine_macro.h

@@ -24,9 +24,9 @@
 
 // thread local macro
 #ifdef _MSC_VER
-#define mjTHREADLOCAL __declspec(thread)
+  #define mjTHREADLOCAL __declspec(thread)
 #else
-#define mjTHREADLOCAL _Thread_local
+  #define mjTHREADLOCAL _Thread_local
 #endif
 
 
@@ -41,7 +41,7 @@
 //-------------------------- compiler builtin ------------------------------------------------------
 
 #ifndef __has_builtin
-#define __has_builtin(x) 0
+  #define __has_builtin(x) 0
 #endif
 
 //-------------------------- pointer arithmetic ----------------------------------------------------

src/engine/engine_plugin.cc

@@ -24,6 +24,7 @@
 #include <cctype>
 #include <cstddef>
 #include <cstdlib>
+#include <cstdio>
 #include <cstring>
 #include <memory>
 #include <mutex>
@@ -57,9 +58,6 @@ constexpr int kMaxAttributes = 255;
 
 constexpr int kCacheLine = 256;
 
-// vfs prefix
-constexpr const char* kVfsPrefix = mjVFS_PREFIX;
-
 // A table of registered plugins, implemented as a linked list of array "blocks".
 // This is a compromise that maintains a good degree of memory locality while not invalidating
 // existing pointers when growing the table. It is expected that for most users, the number of
@@ -270,6 +268,8 @@ bool ResourceProvidersAreIdentical(const mjpResourceProvider* p1, const mjpResou
           p1->open == p2->open &&
           p1->read == p2->read &&
           p1->close == p2->close &&
+          p1->getdir == p2->getdir &&
+          p1->modified == p2->modified &&
           p1->data == p2->data);
 }
 
@@ -532,18 +532,7 @@ void mjp_defaultResourceProvider(mjpResourceProvider* provider) {
 
 // globally register a resource provider (thread-safe), return new slot id
 int mjp_registerResourceProvider(const mjpResourceProvider* provider) {
-  // check against reserved prefixes
+  // check if prefix is valid URI scheme format
-  if (PrefixesAreIdentical(kVfsPrefix, provider->prefix)) {
-    mju_warning("provider->prefix is '%s' which is reserved", provider->prefix);
-    return -1;
-  }
-
-  return mjp_registerResourceProviderInternal(provider);
-}
-
-// internal version of mjp_registerResourceProvider without prechecks on reserved prefixes
-int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
-    // check if prefix is valid URI scheme format
   if (!IsValidURISchemeFormat(provider->prefix)) {
     mju_warning("provider->prefix is '%s' which is not a valid URI scheme format",
                 provider->prefix);
@@ -562,35 +551,25 @@ int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
   // Do not handle objects with nontrivial destructors outside of this lambda.
   // Do not call mju_error inside this lambda.
   int slot = [&]() -> int {
-    bool vfs_provider = false;
     std::unique_ptr<char[]> prefix;
 
-    // check if this is a VFS provider
-    if (PrefixesAreIdentical(mjVFS_PREFIX, provider->prefix)) {
-      vfs_provider = true;
-    }
-
     // copy prefix
-    if (!vfs_provider) {
+    prefix = CopyName(provider->prefix);
-      prefix = CopyName(provider->prefix);
+    if (!prefix) {
-      if (!prefix) {
+      if (strklen(provider->prefix) == -1) {
-        if (strklen(provider->prefix) == -1) {
+        std::snprintf(err, sizeof(err),
-          std::snprintf(err, sizeof(err),
+                     "provider->prefix length exceeds the maximum limit of %d", kMaxNameLength);
-                        "provider->prefix length exceeds the maximum limit of %d", kMaxNameLength);
+      } else {
-        } else {
+        std::snprintf(err, sizeof(err), "failed to allocate memory for resource provider prefix");
-          std::snprintf(err, sizeof(err), "failed to allocate memory for resource provider prefix");
-        }
-        return -1;
       }
+      return -1;
     }
 
     Global<mjpResourceProvider>& global = GetGlobal<mjpResourceProvider>();
     auto lock = global.lock_mutex_exclusively();
     int count = global.count().load(std::memory_order_acquire);
     int local_idx = 0;
-    int free_idx = -1, free_local_idx = -1;
     PluginTable<mjpResourceProvider>* table = &global.table();
-    PluginTable<mjpResourceProvider>* free_table = nullptr;
 
     // check if a non-identical provider with the same name has already been registered
     for (int i = 0; i < count; ++i, ++local_idx) {
@@ -600,17 +579,7 @@ int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
       }
       mjpResourceProvider& existing = table->plugins[local_idx];
 
-      // VFS providers can safely go in open slots
+      if (existing.prefix != nullptr) {
-      if (vfs_provider && existing.prefix == nullptr && free_idx == -1) {
-        free_table = table;
-        free_idx = i;
-        free_local_idx = local_idx;
-
-        // can skip the rest
-        break;
-      }
-
-      if (!vfs_provider && existing.prefix != nullptr) {
         // if identical then return slot number
         if (PrefixesAreIdentical(provider->prefix, existing.prefix)) {
           if (ResourceProvidersAreIdentical(provider, &existing)) {
@@ -626,7 +595,7 @@ int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
     }
 
     // allocate a new block of PluginTable if the last allocated block is full
-    if (free_local_idx == -1 && local_idx == PluginTable<mjpResourceProvider>::kBlockSize) {
+    if (local_idx == PluginTable<mjpResourceProvider>::kBlockSize) {
       local_idx = 0;
       table = AddNewTableBlock<mjpResourceProvider>(table);
       if (!table) {
@@ -636,19 +605,12 @@ int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
 
     // all checks passed, register the plugin into the global table
     mjpResourceProvider& registered_provider = table->plugins[local_idx];
-    if (free_local_idx != -1) {
-      registered_provider = free_table->plugins[free_local_idx];
-    }
-
     registered_provider = *provider;
-    registered_provider.prefix = (!vfs_provider) ? prefix.release() : kVfsPrefix;
+    registered_provider.prefix = prefix.release();
 
     // increment the global plugin count with a release memory barrier
-    if (free_idx == -1) {
+    global.count().store(count + 1, std::memory_order_release);
-      free_idx = count;
+    return count;
-      global.count().store(count + 1, std::memory_order_release);
-    }
-    return free_idx;
   }();
 
   // ========= ATTENTION! ==========================================================================
@@ -663,46 +625,6 @@ int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
   return slot+1;
 }
 
-// globally unregister resource provider (thread-safe)
-// only used for VFS resource providers
-void mjp_unregisterResourceProvider(int slot) {
-  // shift slot to zero-index
-  slot--;
-
-  if (slot < 0) {
-    return;
-  }
-
-  // get global table, acquire lock
-  Global<mjpResourceProvider>& global = GetGlobal<mjpResourceProvider>();
-  auto lock = global.lock_mutex_exclusively();
-  int count = global.count().load(std::memory_order_acquire);
-
-  if (slot >= count) {
-    return;
-  }
-
-  PluginTable<mjpResourceProvider>* table = &global.table();
-
-  // iterate over blocks in the global table until the local index is less than the block size
-  int local_idx = slot;
-  while (local_idx >= PluginTable<mjpResourceProvider>::kBlockSize) {
-    local_idx -= PluginTable<mjpResourceProvider>::kBlockSize;
-    table = table->next;
-    if (!table) {
-      return;
-    }
-  }
-
-  // local_idx is now a valid index into the current block
-  mjpResourceProvider& provider = table->plugins[local_idx];
-
-  // no-op for anything other than VFS resource providers
-  if (provider.prefix == kVfsPrefix) {
-    provider.prefix = nullptr;
-  }
-}
-
 // return the number of globally registered resource providers
 int mjp_resourceProviderCount() {
   return GetGlobal<mjpResourceProvider>().count().load(std::memory_order_acquire);
@@ -728,12 +650,6 @@ const mjpResourceProvider* mjp_getResourceProvider(const char* resource_name) {
     return nullptr;
   }
 
-  // since multiple VFS resource providers can be registered with the same
-  // prefix, it doesn't make sense to try to match against them
-  if (PrefixesAreIdentical(kVfsPrefix, file_prefix.c_str())) {
-    return nullptr;
-  }
-
   Global<mjpResourceProvider>& global = GetGlobal<mjpResourceProvider>();
   auto lock = global.lock_mutex_exclusively();
   PluginTable<mjpResourceProvider>* table = &global.table();
@@ -743,7 +659,6 @@ const mjpResourceProvider* mjp_getResourceProvider(const char* resource_name) {
     for (int i = 0;
          i < PluginTable<mjpPlugin>::kBlockSize && found_slot < count;
          ++i, ++found_slot) {
-
       const mjpResourceProvider& provider = table->plugins[i];
       const char *prefix = provider.prefix;
 

src/engine/engine_plugin.h

@@ -31,14 +31,11 @@ MJAPI int mjp_registerPlugin(const mjpPlugin* plugin);
 // globally register a resource provider (thread-safe), return new slot id
 MJAPI int mjp_registerResourceProvider(const mjpResourceProvider* provider);
 
-// globally unregister a resource provider (thread-safe)
-MJAPI void mjp_unregisterResourceProvider(int slot);
-
 // return the number of globally registered plugins
-MJAPI int mjp_pluginCount();
+MJAPI int mjp_pluginCount(void);
 
 // return the number of globally registered resource providers
-MJAPI int mjp_resourceProviderCount();
+MJAPI int mjp_resourceProviderCount(void);
 
 // look up a plugin by name, optionally also get its registered slot number
 MJAPI const mjpPlugin* mjp_getPlugin(const char* name, int* slot);

src/engine/engine_print.c

@@ -730,8 +730,13 @@ void mj_printModel(const mjModel* m, const char* filename) {
 // valid printf-style format string for a single float value
 void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
                            const char* float_format) {
+  // stack in use, SHOULD NOT OCCUR
+  if (d->pstack) {
+    mjERROR("attempting to print mjData when stack is in use");
+  }
+
   mjtNum *M;
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // check format string
   if (!validateFloatFormat(float_format)) {
@@ -739,11 +744,6 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
     float_format = FLOAT_FORMAT;
   }
 
-  // stack in use, SHOULD NOT OCCUR
-  if (d->pstack) {
-    mjERROR("attempting to print mjData when stack is in use");
-  }
-
   // get file
   FILE* fp;
   if (filename) {
@@ -755,7 +755,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
   // check for nullptr
   if (!fp) {
     mju_warning("Could not open file '%s' for writing mjModel", filename);
-    mjFREESTACK;
+    mj_freeStack(d);
     return;
   }
 
@@ -776,7 +776,10 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
 
   fprintf(fp, "SIZES\n");
 #define X(type, name)                                                         \
-  if (strcmp(#name, "pstack") != 0 && strcmp(#name, "parena") != 0) {         \
+  if (strcmp(#name, "pstack") != 0 &&                                         \
+      strcmp(#name, "pbase") != 0 &&                                          \
+      strcmp(#name, "parena") != 0 &&                                         \
+      strcmp(#name, "threadpool") != 0) {                                     \
     const char* format = _Generic(                                            \
         d->name,                                                              \
         int : INT_FORMAT,                                                     \
@@ -792,6 +795,16 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
 
   MJDATA_SCALAR
 #undef X
+
+  int threadpool = 0;
+  if (d->threadpool) {
+    threadpool = 1;
+  }
+  fprintf(fp, "  ");
+  fprintf(fp, NAME_FORMAT, "threadpool");
+  fprintf(fp, INT_FORMAT, threadpool);
+  fprintf(fp, "\n");
+
   fprintf(fp, "\n");
 
   // WARNING
@@ -1132,7 +1145,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
     fclose(fp);
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 

src/engine/engine_ray.c

@@ -15,6 +15,7 @@
 
 #include "engine/engine_ray.h"
 
+#include <math.h>
 #include <stddef.h>
 
 #include <mujoco/mjdata.h>
@@ -1158,7 +1159,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
 void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec,
                  const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
                  int* geomid, mjtNum* dist, int nray, mjtNum cutoff) {
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // allocate source
   mjtNum* geom_ba = mj_stackAllocNum(d, 4*m->ngeom);
@@ -1173,5 +1174,5 @@ void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum*
     dist[i] = mju_singleRay(m, d, pnt, vec+3*i, geom_eliminate, geom_ba, geomid+i);
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }

src/engine/engine_resource.c

@@ -16,40 +16,35 @@
 
 #include <limits.h>
 #include <stddef.h>
+#include <stdint.h>
 #include <stdio.h>
 #include <string.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+
+#if defined (__unix__) || (defined (__APPLE__) && defined (__MACH__))
+  #include <unistd.h>
+#endif
+
+#ifdef _WIN32
+  #define stat _stat
+#endif
 
-#include <mujoco/mjmodel.h>
 #include <mujoco/mjplugin.h>
 #include "engine/engine_plugin.h"
 #include "engine/engine_util_errmem.h"
 
 // file buffer used internally for the OS filesystem
 typedef struct {
-  void* buffer;
+  uint8_t* buffer;  // raw bytes from file
-  int nbuffer;
+  size_t nbuffer;   // size of buffer in bytes
+  time_t mtime;     // last modified time
 } file_buffer;
 
-// helper function to fill data from resource provider into provider
-static void fillResource(const mjpResourceProvider* provider, mjResource* resource) {
-  if (provider == NULL) {
-    resource->read = NULL;
-    resource->close = NULL;
-    resource->getdir = NULL;
-    resource->provider_data = NULL;
-  } else {
-    resource->read = provider->read;
-    resource->close = provider->close;
-    resource->getdir = provider->getdir;
-    resource->provider_data = provider->data;
-  }
-}
-
-
 // open the given resource; if the name doesn't have a prefix matching with a
-// resource provider, then the default_provider is used
+// resource provider, then the OS filesystem is used
-// if default_provider non-positive, then the OS filesystem is used
+mjResource* mju_openResource(const char* name) {
-mjResource* mju_openResource(const char* name, int default_provider) {
   mjResource* resource = (mjResource*) mju_malloc(sizeof(mjResource));
   const mjpResourceProvider* provider = NULL;
   if (resource == NULL) {
@@ -57,19 +52,22 @@ mjResource* mju_openResource(const char* name, int default_provider) {
     return NULL;
   }
 
+  // clear out resource
+  memset(resource, 0, sizeof(mjResource));
+
   // copy name
   resource->name = mju_malloc(sizeof(char) * (strlen(name) + 1));
   if (resource->name == NULL) {
-    mju_free(resource);
+    mju_closeResource(resource);
     mjERROR("could not allocate memory");
     return NULL;
   }
-  strcpy(resource->name, name);
+  memcpy(resource->name, name, sizeof(char) * (strlen(name) + 1));
 
   // find provider based off prefix of name
   provider = mjp_getResourceProvider(name);
   if (provider != NULL) {
-    fillResource(provider, resource);
+    resource->provider = provider;
     if (provider->open(resource)) {
       return resource;
     }
@@ -77,49 +75,27 @@ mjResource* mju_openResource(const char* name, int default_provider) {
     mju_warning("mju_openResource: could not open resource '%s' "
                 "using a resource provider matching prefix '%s'",
                 name, provider->prefix);
-    mju_free(resource->name);
+    mju_closeResource(resource);
-    mju_free(resource);
-    return NULL;
-  }
-
-  // fallback to default provider
-  if (default_provider > 0) {
-    provider = mjp_getResourceProviderAtSlot(default_provider);
-    if (provider == NULL) {
-      mju_warning("mju_openResource: unknown resource provider at slot %d",
-                  default_provider);
-      mju_free(resource->name);
-      mju_free(resource);
-      return NULL;
-    }
-
-    fillResource(provider, resource);
-    if (provider->open(resource)) {
-      return resource;
-    }
-
-    mju_warning("mju_openResource: could not open resource '%s' "
-                "with default provider at slot %d",
-                name, default_provider);
-    mju_free(resource->name);
-    mju_free(resource);
     return NULL;
   }
 
   // lastly fallback to OS filesystem
-  else {
+  resource->provider = NULL;
-    fillResource(NULL, resource);
+  resource->data = mju_malloc(sizeof(file_buffer));
-    resource->data = mju_malloc(sizeof(file_buffer));
+  file_buffer* fb = (file_buffer*) resource->data;
-    file_buffer* fb = (file_buffer*) resource->data;
+  fb->buffer = mju_fileToMemory(name, &(fb->nbuffer));
-    fb->buffer = mju_fileToMemory(name, &(fb->nbuffer));
+  if (fb->buffer == NULL) {
-    if (fb->buffer == NULL) {
+    mju_warning("mju_openResource: unknown file '%s'", name);
-      mju_warning("mju_openResource: unknown file '%s'", name);
+    mju_closeResource(resource);
-      mju_free(fb);
+    return NULL;
-      mju_free(resource->name);
-      mju_free(resource);
-      return NULL;
-    }
   }
+  struct stat file_stat;
+  if (stat(name, &file_stat) == 0) {
+    memcpy(&fb->mtime, &file_stat.st_mtime, sizeof(time_t));
+  } else {
+    memset(&fb->mtime, 0, sizeof(time_t));
+  }
+
   return resource;
 }
 
@@ -131,20 +107,20 @@ void mju_closeResource(mjResource* resource) {
     return;
   }
 
-  // use the resource provider to close resource
+  // use the resource provider close callback
-  if (resource->close) {
+  if (resource->provider && resource->provider->close) {
-    resource->close(resource);
+    resource->provider->close(resource);
-  }
+  } else {
-
+    // clear OS filesystem if present
-  // if provider is NULL, then OS filesystem is used
-  else {
     file_buffer* fb = (file_buffer*) resource->data;
-    mju_free(fb->buffer);
+    if (fb) {
-    mju_free(fb);
+      if (fb->buffer) mju_free(fb->buffer);
+      mju_free(fb);
+    }
   }
 
-  // free name and resource
+  // free resource
-  mju_free(resource->name);
+  if (resource->name) mju_free(resource->name);
   mju_free(resource);
 }
 
@@ -157,12 +133,12 @@ int mju_readResource(mjResource* resource, const void** buffer) {
     return 0;
   }
 
-  if (resource->read) {
+  if (resource->provider) {
-    return resource->read(resource, buffer);
+    return resource->provider->read(resource, buffer);
   }
 
 
-  // if provider read callback is NULL, then OS filesystem is used
+  // if provider is NULL, then OS filesystem is used
   const file_buffer* fb = (file_buffer*) resource->data;
   *buffer = fb->buffer;
   return fb->nbuffer;
@@ -175,14 +151,14 @@ void mju_getResourceDir(mjResource* resource, const char** dir, int* ndir) {
   *dir = NULL;
   *ndir = 0;
 
-  if (!resource) {
+  if (resource == NULL) {
     return;
   }
 
   // provider is not OS filesystem
-  if (resource->read) {
+  if (resource->provider) {
-    if (resource->getdir) {
+    if (resource->provider->getdir) {
-      resource->getdir(resource, dir, ndir);
+      resource->provider->getdir(resource, dir, ndir);
     }
   } else {
     *dir = resource->name;
@@ -192,6 +168,40 @@ void mju_getResourceDir(mjResource* resource, const char** dir, int* ndir) {
 
 
 
+// modified callback for OS filesystem
+static int mju_isModifiedFile(const char* name, const file_buffer* fb) {
+  if (fb != NULL) {
+    struct stat file_stat;
+    if (stat(name, &file_stat) == 0) {
+      return difftime(fb->mtime, file_stat.st_mtime) < 0;
+    }
+    return -1;
+  }
+  return -2;
+}
+
+
+
+// Returns > 0 if resource has been modified since last read, 0 if not, and < 0
+// if inconclusive
+int mju_isModifiedResource(const mjResource* resource) {
+  if (resource == NULL) {
+    return -2;
+  }
+
+  // provider is not OS filesystem
+  if (resource->provider) {
+    if (resource->provider->modified) {
+      return resource->provider->modified(resource);
+    }
+    return 1;  // default (modified)
+  }
+
+  return mju_isModifiedFile(resource->name, (file_buffer*) resource->data);
+}
+
+
+
 // get the length of the dirname portion of a given path
 int mju_dirnamelen(const char* path) {
   if (!path) {
@@ -211,7 +221,7 @@ int mju_dirnamelen(const char* path) {
 
 
 // read file into memory buffer (allocated here with mju_malloc)
-void* mju_fileToMemory(const char* filename, int* filesize) {
+void* mju_fileToMemory(const char* filename, size_t* filesize) {
   // open file
   *filesize = 0;
   FILE* fp = fopen(filename, "rb");

src/engine/engine_resource.h

@@ -15,6 +15,8 @@
 #ifndef MUJOCO_SRC_ENGINE_ENGINE_RESOURCE_H_
 #define MUJOCO_SRC_ENGINE_ENGINE_RESOURCE_H_
 
+#include <stddef.h>
+
 #include <mujoco/mjexport.h>
 #include "engine/engine_plugin.h"
 
@@ -23,9 +25,8 @@ extern "C" {
 #endif
 
 // open the given resource; if the name doesn't have a prefix matching with a
-// resource provider, then the default_provider is used
+// resource provider, then the OS filesystem is used
-// if default_provider non-positive, then the OS filesystem is used
+MJAPI mjResource* mju_openResource(const char* name);
-MJAPI mjResource* mju_openResource(const char* name, int default_provider);
 
 // close the given resource; no-op if resource is NULL
 MJAPI void mju_closeResource(mjResource* resource);
@@ -37,11 +38,15 @@ MJAPI int mju_readResource(mjResource* resource, const void** buffer);
 // sets for a resource with a name partitioned as {dir}{filename}, the dir and ndir pointers
 MJAPI void mju_getResourceDir(mjResource* resource, const char** dir, int* ndir);
 
+// Returns > 0 if resource has been modified since last read, 0 if not, and < 0
+// if inconclusive
+MJAPI int mju_isModifiedResource(const mjResource* resource);
+
 // get the length of the dirname portion of a given path
 int mju_dirnamelen(const char* path);
 
 // read file into memory buffer (allocated here with mju_malloc)
-void* mju_fileToMemory(const char* filename, int* filesize);
+void* mju_fileToMemory(const char* filename, size_t* filesize);
 
 #ifdef __cplusplus
 }

src/engine/engine_sensor.c

@@ -854,18 +854,18 @@ void mj_energyPos(const mjModel* m, mjData* d) {
 // velocity-dependent energy (kinetic)
 void mj_energyVel(const mjModel* m, mjData* d) {
   mjtNum *vec;
-  mjMARKSTACK;
 
   // return if disabled (already cleared in potential)
   if (!mjENABLED(mjENBL_ENERGY)) {
     return;
   }
 
+  mj_markStack(d);
   vec = mj_stackAllocNum(d, m->nv);
 
   // kinetic energy:  0.5 * qvel' * M * qvel
   mj_mulM(m, d, vec, d->qvel);
   d->energy[1] = 0.5*mju_dot(vec, d->qvel, m->nv);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }

src/engine/engine_setconst.c

@@ -61,7 +61,7 @@ static void mj_setM0(mjModel* m, mjData* d) {
 static void set0(mjModel* m, mjData* d) {
   int id, id1, id2, dnum, nv = m->nv;
   mjtNum A[36] = {0}, pos[3], quat[4];
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* jac = mj_stackAllocNum(d, 6*nv);
   mjtNum* tmp = mj_stackAllocNum(d, 6*nv);
   int* cammode = 0;
@@ -264,7 +264,7 @@ static void set0(mjModel* m, mjData* d) {
     mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i);
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -283,7 +283,7 @@ static void setStat(mjModel* m, mjData* d) {
   mjtNum xmin[3] = {1E+10, 1E+10, 1E+10};
   mjtNum xmax[3] = {-1E+10, -1E+10, -1E+10};
   mjtNum rbound;
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* body = mj_stackAllocNum(d, m->nbody);
 
   // compute bounding box of bodies, joint centers, geoms and sites
@@ -383,7 +383,7 @@ static void setStat(mjModel* m, mjData* d) {
     m->stat.meaninertia /= m->nv;
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 

src/engine/engine_solver.c

@@ -168,7 +168,8 @@ static void residual(const mjModel* m, mjData* d, mjtNum* res, int i, int dim, i
     for (int j=0; j < dim; j++) {
       res[j] = d->efc_b[i+j] + mju_dotSparse(d->efc_AR + d->efc_AR_rowadr[i+j],
                                              d->efc_force, d->efc_AR_rownnz[i+j],
-                                             d->efc_AR_colind + d->efc_AR_rowadr[i+j]);
+                                             d->efc_AR_colind + d->efc_AR_rowadr[i+j],
+                                             /*flg_unc1=*/0);
     }
   }
 
@@ -308,7 +309,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
   mjtNum *mu, x, denom, improvement;
   mjtNum v[6], v1[6], Athis[36], Ac[25], bc[5], res[6], oldforce[6];
   mjContact* con;
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* ARinv = mj_stackAllocNum(d, nefc);
   int* oldstate = mj_stackAllocInt(d, nefc);
 
@@ -463,7 +464,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
     }
 
     // process state
-    memcpy(oldstate, d->efc_state, nefc*sizeof(int));
+    mju_copyInt(oldstate, d->efc_state, nefc);
     int nactive = dualState(m, d);
     int nchange = 0;
     for (int i=0; i < nefc; i++) {
@@ -496,7 +497,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
   // map to joint space
   dualFinish(m, d);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -510,7 +511,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
   mjtNum *mu, improvement;
   mjtNum v[5], Ac[25], bc[5], res[5], oldforce[5], delta[5], mid, y, K0, K1;
   mjContact* con;
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* ARinv = mj_stackAllocNum(d, nefc);
   int* oldstate = mj_stackAllocInt(d, nefc);
 
@@ -679,7 +680,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
     }
 
     // process state
-    memcpy(oldstate, d->efc_state, nefc*sizeof(int));
+    mju_copyInt(oldstate, d->efc_state, nefc);
     int nactive = dualState(m, d);
     int nchange = 0;
     for (int i=0; i < nefc; i++) {
@@ -702,7 +703,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
   // map to joint space
   dualFinish(m, d);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1092,9 +1093,6 @@ static int updateBracket(const mjModel* m, mjData* d, mjCGContext* ctx,
 static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
   mjCGPnt p0, p1, p2, pmid, p1next, p2next;
 
-  const int LSmaxiter = 50;
-  const mjtNum LStolscl = 0.01;
-
   // clear results
   ctx->LSiter = 0;
   ctx->LSresult = 0;
@@ -1108,7 +1106,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
   }
 
   // compute scaled gradtol and slope scaling
-  mjtNum gtol = m->opt.tolerance * LStolscl * snorm * m->stat.meaninertia * mjMAX(1, m->nv);
+  mjtNum gtol = m->opt.tolerance * m->opt.ls_tolerance * snorm * m->stat.meaninertia * mjMAX(1, m->nv);
   mjtNum slopescl = 1 / (snorm * m->stat.meaninertia * mjMAX(1, m->nv));
 
   // compute Mv, Jv
@@ -1179,7 +1177,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
 
   // one-sided search
   int p2update = 0;
-  while (p1.deriv[0]*dir <= -gtol && ctx->LSiter < LSmaxiter) {
+  while (p1.deriv[0]*dir <= -gtol && ctx->LSiter < m->opt.ls_iterations) {
     // save current
     p2 = p1;
     p2update = 1;
@@ -1196,7 +1194,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
   }
 
   // check for failure to bracket
-  if (ctx->LSiter >= LSmaxiter) {
+  if (ctx->LSiter >= m->opt.ls_iterations) {
     ctx->LSresult = 3;                          // could not bracket
     ctx->LSslope = mju_abs(p1.deriv[0])*slopescl;
     return p1.alpha;
@@ -1215,7 +1213,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
   CGeval(m, d, ctx, &p1next);
 
   // bracketed search
-  while (ctx->LSiter < LSmaxiter) {
+  while (ctx->LSiter < m->opt.ls_iterations) {
     // evaluate at midpoint
     pmid.alpha = 0.5*(p1.alpha + p2.alpha);
     CGeval(m, d, ctx, &pmid);
@@ -1276,7 +1274,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
 static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) {
   int nv = m->nv, nefc = d->nefc;
   mjtNum local[36];
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // storage for L'*J
   mjtNum* LTJ = mj_stackAllocNum(d, 6*nv);
@@ -1313,7 +1311,7 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) {
         for (int r=0; r < dim; r++) {
           // copy data for this row
           mju_copy(LTJ_row, LTJ+r*nnz, nnz);
-          memcpy(LTJ_ind, d->efc_J_colind+d->efc_J_rowadr[i+r], nnz*sizeof(int));
+          mju_copyInt(LTJ_ind, d->efc_J_colind+d->efc_J_rowadr[i+r], nnz);
 
           // update
           mju_cholUpdateSparse(ctx->Hcone, LTJ_row, nv, 1,
@@ -1346,7 +1344,7 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) {
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1354,7 +1352,7 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) {
 // compute and factorize Hessian: direct method
 static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
   int nv = m->nv, nefc = d->nefc;
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // compute D corresponding to quad states
   mjtNum* D = mj_stackAllocNum(d, nefc);
@@ -1426,7 +1424,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
     ctx->nnz = nv*nv;
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 
   // add cones if present
   if (ctx->ncone) {
@@ -1443,7 +1441,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
 static void HessianIncremental(const mjModel* m, mjData* d,
                                mjCGContext* ctx, const int* oldstate) {
   int rank, nv = m->nv, nefc = d->nefc;
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // local space
   mjtNum* vec = mj_stackAllocNum(d, nv);
@@ -1475,7 +1473,7 @@ static void HessianIncremental(const mjModel* m, mjData* d,
 
         // scale vec, copy colind
         mju_scl(vec, d->efc_J+adr, mju_sqrt(d->efc_D[i]), nnz);
-        memcpy(vec_ind, d->efc_J_colind+adr, nnz*sizeof(int));
+        mju_copyInt(vec_ind, d->efc_J_colind+adr, nnz);
 
         // sparse update
         rank = mju_cholUpdateSparse(ctx->H, vec, nv, flag_update,
@@ -1489,7 +1487,7 @@ static void HessianIncremental(const mjModel* m, mjData* d,
 
       // recompute H directly if accuracy lost
       if (rank < nv) {
-        mjFREESTACK;
+        mj_freeStack(d);
         HessianDirect(m, d, ctx);
 
         // nothing else to do
@@ -1503,7 +1501,7 @@ static void HessianIncremental(const mjModel* m, mjData* d,
     HessianCone(m, d, ctx);
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1514,7 +1512,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
   mjtNum alpha, beta;
   mjtNum *gradold = NULL, *Mgradold = NULL, *Mgraddif = NULL;
   mjCGContext ctx;
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // allocate context
   CGallocate(m, d, &ctx, flg_Newton);
@@ -1562,7 +1560,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
       mju_copy(gradold, ctx.grad, nv);
       mju_copy(Mgradold, ctx.Mgrad, nv);
     }
-    memcpy(oldstate, d->efc_state, nefc*sizeof(int));
+    mju_copyInt(oldstate, d->efc_state, nefc);
     mjtNum oldcost = ctx.cost;
 
     // update
@@ -1624,7 +1622,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
     d->solver_nnz = 0;
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 

src/engine/engine_support.c

@@ -14,12 +14,13 @@
 
 #include "engine/engine_support.h"
 
+#include <stddef.h>
+#include <stdint.h>
 #include <string.h>
 
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmacro.h>
 #include <mujoco/mjmodel.h>
-#include "engine/engine_array_safety.h"
 #include "engine/engine_core_constraint.h"
 #include "engine/engine_crossplatform.h"
 #include "engine/engine_io.h"
@@ -278,7 +279,7 @@ void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr
 // compute subtree-com Jacobian
 void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) {
   int nv = m->nv;
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* jacp_b = mj_stackAllocNum(d, 3*nv);
 
   // clear output
@@ -299,7 +300,7 @@ void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) {
   // normalize by subtree mass
   mju_scl(jacp, jacp, 1/m->body_subtreemass[body], 3*nv);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -324,7 +325,7 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA
   int nv = m->nv;
 
   // get full Jacobian of point
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* jacp = (jacPoint ? jacPoint : mj_stackAllocNum(d, 3*nv));
   mjtNum* jacr = mj_stackAllocNum(d, 3*nv);
   mj_jac(m, d, jacp, jacr, point, body);
@@ -338,7 +339,7 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -827,25 +828,27 @@ void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M) {
 
 // multiply vector by inertia matrix
 void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) {
-  int adr, nv = m->nv;
+  int nv = m->nv;
   const mjtNum* M = d->qM;
-  const int* dofMadr = m->dof_Madr;
+  const int* Madr = m->dof_Madr;
+  const int* parentid = m->dof_parentid;
+  const int* simplenum = m->dof_simplenum;
 
   mju_zero(res, nv);
 
   for (int i=0; i < nv; i++) {
 #ifdef mjUSEAVX
-    // simple: diagonal division, AVX
+    // simple: diagonal multiplication, AVX
-    if (m->dof_simplenum[i] >= 4) {
+    if (simplenum[i] >= 4) {
       // init
       __m256d result, val1, val2;
 
       // parallel computation
       val1 = _mm256_loadu_pd(vec+i);
-      val2 = _mm256_set_pd(M[dofMadr[i+3]],
+      val2 = _mm256_set_pd(M[Madr[i+3]],
-                           M[dofMadr[i+2]],
+                           M[Madr[i+2]],
-                           M[dofMadr[i+1]],
+                           M[Madr[i+1]],
-                           M[dofMadr[i+0]]);
+                           M[Madr[i+0]]);
       result = _mm256_mul_pd(val1, val2);
 
       // store result
@@ -856,29 +859,88 @@ void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
       continue;
     }
 #endif
+    // address in M
+    int adr = Madr[i];
 
-    // simple: diagonal multiplication
+    // compute diagonal
-    if (m->dof_simplenum[i]) {
+    res[i] = M[adr]*vec[i];
-      res[i] = M[dofMadr[i]]*vec[i];
+
+    // simple dof: continue
+    if (simplenum[i]) {
+      continue;
     }
 
-    // regular: full multiplication
+    // compute off-diagonals
-    else {
+    int j = parentid[i];
-      // diagonal
+    while (j >= 0) {
-      adr = dofMadr[i];
-      res[i] += M[adr]*vec[i];
-
-      // off-diagonal
-      int j = m->dof_parentid[i];
       adr++;
-      while (j >= 0) {
+      res[i] += M[adr]*vec[j];
-        res[i] += M[adr]*vec[j];
+      res[j] += M[adr]*vec[i];
-        res[j] += M[adr]*vec[i];
 
-        // advance to next element
+      // advance to parent
-        j = m->dof_parentid[j];
+      j = parentid[j];
-        adr++;
+    }
+  }
+}
+
+
+
+// multiply vector by inertia matrix for one dof island
+void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
+                    int island, int flg_vecunc) {
+  // if no island, call regular function
+  if (island < 0) {
+    mj_mulM(m, d, res, vec);
+    return;
+  }
+
+  // local constants: general
+  const mjtNum* M = d->qM;
+  const int* Madr = m->dof_Madr;
+  const int* parentid = m->dof_parentid;
+  const int* simplenum = m->dof_simplenum;
+
+  // local constants: island specific
+  int ndof = d->island_dofnum[island];
+  const int* dofind = d->island_dofind + d->island_dofadr[island];
+  const int* islandind = d->dof_islandind;
+
+  mju_zero(res, ndof);
+
+  for (int k=0; k < ndof; k++) {
+    // address in full dof vector
+    int i = dofind[k];
+
+    // address in M
+    int adr = Madr[i];
+
+    // diagonal
+    if (flg_vecunc) {
+      res[k] = M[adr]*vec[i];
+    } else {
+      res[k] = M[adr]*vec[k];
+    }
+
+    // simple dof: continue
+    if (simplenum[i]) {
+      continue;
+    }
+
+    // off-diagonal
+    int j = parentid[i];
+    while (j >= 0) {
+      adr++;
+      int l = islandind[j];
+      if (flg_vecunc) {
+        res[k] += M[adr]*vec[j];
+        res[l] += M[adr]*vec[i];
+      } else {
+        res[k] += M[adr]*vec[l];
+        res[l] += M[adr]*vec[k];
       }
+
+      // advance to parent
+      j = parentid[j];
     }
   }
 }
@@ -952,7 +1014,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
   // sparse
   if (rownnz && rowadr && colind) {
     int nv = m->nv;
-    mjMARKSTACK;
+    mj_markStack(d);
     // create sparse inertia matrix M
     int nnz = m->nD;  // use sparse dof-dof matrix
     int* M_rownnz = mj_stackAllocInt(d, nv);  // actual nnz count
@@ -962,7 +1024,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
     mj_makeMSparse(m, d, M, M_rownnz, NULL, M_colind);
     mj_addMSparse(m, d, dst, rownnz, rowadr, colind, M,
                   M_rownnz, NULL, M_colind);
-    mjFREESTACK;
+    mj_freeStack(d);
   }
 
   // dense
@@ -997,7 +1059,7 @@ void mj_makeMSparse(const mjModel* m, mjData* d, mjtNum* M,
     }
 
     // backward pass over dofs: construct M_row(i) in reverse order
-    int col = M_rowadr[i]; // current column in row i
+    int col = M_rowadr[i];  // current column in row i
     for (int j = i; j >= 0; j = m->dof_parentid[j]) {
       M[col] = d->qM[Madr++];
       M_colind[col++] = j;
@@ -1049,7 +1111,7 @@ void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst,
     M_rowadr = d->D_rowadr;
   }
 
-  mjMARKSTACK;
+  mj_markStack(d);
   int* buf_ind = mj_stackAllocInt(d, nv);
   mjtNum* sparse_buf = mj_stackAllocNum(d, nv);
 
@@ -1059,7 +1121,7 @@ void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst,
                                   rownnz[i], M_rownnz[i], colind + rowadr[i],
                                   M_colind + M_rowadr[i], sparse_buf, buf_ind);
   }
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1096,11 +1158,11 @@ void mj_addMDense(const mjModel* m, mjData* d, mjtNum* dst) {
 // dst[D] = src[M], handle different sparsity representations
 void mj_copyM2DSparse(const mjModel* m, mjData* d, mjtNum* dst, const mjtNum* src) {
   int nv = m->nv;
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // init remaining
   int* remaining = mj_stackAllocInt(d, nv);
-  memcpy(remaining, d->D_rownnz, nv * sizeof(int));
+  mju_copyInt(remaining, d->D_rownnz, nv);
 
   // copy data
   for (int i = nv - 1; i >= 0; i--) {
@@ -1123,7 +1185,7 @@ void mj_copyM2DSparse(const mjModel* m, mjData* d, mjtNum* dst, const mjtNum* sr
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1161,7 +1223,7 @@ void mj_applyFT(const mjModel* m, mjData* d,
   int nv = m->nv;
 
   // allocate local variables
-  mjMARKSTACK;
+  mj_markStack(d);
   mjtNum* jacp = mj_stackAllocNum(d, 3*nv);
   mjtNum* jacr = mj_stackAllocNum(d, 3*nv);
   mjtNum* qforce = mj_stackAllocNum(d, nv);
@@ -1184,7 +1246,7 @@ void mj_applyFT(const mjModel* m, mjData* d,
     mju_addTo(qfrc_target, qforce, nv);
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -1525,7 +1587,7 @@ int mj_version(void) {
 
 
 // current version of MuJoCo as a null-terminated string
-const char* mj_versionString() {
+const char* mj_versionString(void) {
   static const char versionstring[] = mjVERSIONSTRING;
   return versionstring;
 }

src/engine/engine_support.h

@@ -15,9 +15,12 @@
 #ifndef MUJOCO_SRC_ENGINE_ENGINE_SUPPORT_H_
 #define MUJOCO_SRC_ENGINE_ENGINE_SUPPORT_H_
 
+#include <stdint.h>
+
 #include <mujoco/mjdata.h>
 #include <mujoco/mjexport.h>
 #include <mujoco/mjmodel.h>
+#include <mujoco/mjtnum.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -108,6 +111,10 @@ MJAPI void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M);
 // multiply vector by inertia matrix
 MJAPI void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
 
+// multiply vector by inertia matrix for one dof island
+MJAPI void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
+                          int island, int flg_vecunc);
+
 // multiply vector by (inertia matrix)^(1/2)
 MJAPI void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
 
@@ -193,7 +200,7 @@ MJAPI void mj_warning(mjData* d, int warning, int info);
 MJAPI int mj_version(void);
 
 // current version of MuJoCo as a null-terminated string
-MJAPI const char* mj_versionString();
+MJAPI const char* mj_versionString(void);
 #ifdef __cplusplus
 }
 #endif

src/engine/engine_util_blas.c

@@ -16,7 +16,6 @@
 
 #include <string.h>
 
-#include <mujoco/mjmacro.h>
 #include <mujoco/mjtnum.h>
 
 #ifdef mjUSEPLATFORMSIMD
@@ -238,9 +237,7 @@ mjtNum mju_normalize4(mjtNum vec[4]) {
 
 // res = 0
 void mju_zero(mjtNum* res, int n) {
-  if (n > 0) {
+  memset(res, 0, n*sizeof(mjtNum));
-    memset(res, 0, n*sizeof(mjtNum));
-  }
 }
 
 
@@ -256,9 +253,7 @@ void mju_fill(mjtNum* res, mjtNum val, int n) {
 
 // res = vec
 void mju_copy(mjtNum* res, const mjtNum* vec, int n) {
-  if (n > 0) {
+  memcpy(res, vec, n*sizeof(mjtNum));
-    memcpy(res, vec, n*sizeof(mjtNum));
-  }
 }
 
 

src/engine/engine_util_errmem.c

@@ -70,7 +70,7 @@ typedef void (*callback_fn)(const char*);
 static mjTHREADLOCAL callback_fn _mjPRIVATE_tls_error_fn = NULL;
 static mjTHREADLOCAL callback_fn _mjPRIVATE_tls_warning_fn = NULL;
 
-callback_fn _mjPRIVATE__get_tls_error_fn() {
+callback_fn _mjPRIVATE__get_tls_error_fn(void) {
   return _mjPRIVATE_tls_error_fn;
 }
 
@@ -78,7 +78,7 @@ void _mjPRIVATE__set_tls_error_fn(callback_fn h) {
   _mjPRIVATE_tls_error_fn = h;
 }
 
-callback_fn _mjPRIVATE__get_tls_warning_fn() {
+callback_fn _mjPRIVATE__get_tls_warning_fn(void) {
   return _mjPRIVATE_tls_warning_fn;
 }
 
@@ -113,20 +113,17 @@ void mju_writeLog(const char* type, const char* msg) {
   }
 }
 
-void mju_error_v(const char* msg, va_list args) {
-  char errmsg[1000];
 
-  // Format msg into errmsg
-  vsnprintf(errmsg, mjSIZEOFARRAY(errmsg), msg, args);
 
+void mju_error_raw(const char* msg) {
   if (_mjPRIVATE_tls_error_fn) {
-    _mjPRIVATE_tls_error_fn(errmsg);
+    _mjPRIVATE_tls_error_fn(msg);
   } else if (mju_user_error) {
-    mju_user_error(errmsg);
+    mju_user_error(msg);
   } else {
     // write to log and console
-    mju_writeLog("ERROR", errmsg);
+    mju_writeLog("ERROR", msg);
-    printf("ERROR: %s\n\nPress Enter to exit ...", errmsg);
+    printf("ERROR: %s\n\nPress Enter to exit ...", msg);
 
     // pause, exit
     getchar();
@@ -135,6 +132,16 @@ void mju_error_v(const char* msg, va_list args) {
 }
 
 
+
+void mju_error_v(const char* msg, va_list args) {
+  // Format msg into errmsg
+  char errmsg[1024];
+  vsnprintf(errmsg, mjSIZEOFARRAY(errmsg), msg, args);
+  mju_error_raw(errmsg);
+}
+
+
+
 // write message to logfile and console, pause and exit
 void mju_error(const char* msg, ...) {
   va_list args;
@@ -147,7 +154,7 @@ void mju_error(const char* msg, ...) {
 
 // write message to logfile and console
 void mju_warning(const char* msg, ...) {
-  char wrnmsg[1000];
+  char wrnmsg[1024];
 
   // Format msg into wrnmsg
   va_list args;

src/engine/engine_util_errmem.h

@@ -18,6 +18,7 @@
 #include <stdarg.h>
 #include <stddef.h>
 #include <stdio.h>
+#include <string.h>
 
 #include <mujoco/mjexport.h>
 #include <mujoco/mjmacro.h>
@@ -31,8 +32,8 @@ extern "C" {
     #define mjPRINTFLIKE(n, m) __attribute__((format(printf, n, m)))
   #else
     #define mjPRINTFLIKE(n, m)
-  #endif // __GNUC__
+  #endif  // __GNUC__
-#endif // mjPRINTFLIKE
+#endif  // mjPRINTFLIKE
 
 
 //------------------------------ user handlers -----------------------------------------------------
@@ -54,6 +55,7 @@ MJAPI void _mjPRIVATE__set_tls_warning_fn(void (*h)(const char*));
 //------------------------------ errors and warnings -----------------------------------------------
 
 // errors
+MJAPI void mju_error_raw(const char* msg);
 MJAPI void mju_error(const char* msg, ...) mjPRINTFLIKE(1, 2);
 MJAPI void mju_error_v(const char* msg, va_list args);
 MJAPI void mju_error_i(const char* msg, int i);
@@ -69,42 +71,15 @@ MJAPI void mju_writeLog(const char* type, const char* msg);
 
 //------------------------------ internal error macros --------------------------------------------
 
-// need at least c99 or c++11
+// internal macro to prepend the calling function name to the error message
-#if (defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L) ||       \
+#define mjERROR(...)                                                          \
-    (defined(__cplusplus) && __cplusplus >= 201103L)
+{                                                                             \
-
+  char _errbuf[1024];                                                         \
-  // macro to get the first argument
+  size_t _funclen = strlen(__func__);                                         \
-  #define _GET_MSG(msg, ...) msg
+  strncpy(_errbuf, __func__, sizeof(_errbuf));                                \
-
+  snprintf(_errbuf + _funclen, sizeof(_errbuf) - _funclen, ": " __VA_ARGS__); \
-  // helper function for the mjERROR macro
+  mju_error_raw(_errbuf);                                                     \
-  // formats buf as '{prefix}: {msg}' and passes along to mju_error_v
+}
-  static inline void _mju_error_prefix(char *buf, size_t nbuf, const char* prefix,
-                                       const char* msg, ...) mjPRINTFLIKE(4, 5);
-
-  static inline void _mju_error_prefix(char *buf, size_t nbuf, const char* prefix,
-                                       const char* msg, ...) {
-    snprintf(buf, nbuf, "%s: %s", prefix, msg);
-    va_list args;
-    va_start(args, msg);
-    mju_error_v(buf, args);
-    va_end(args);
-  }
-
-  // macro to get first argument
-  #define _GET_MSG(msg, ...) msg
-
-  // internal macro to prepend the calling function name to the error message
-  // standard support for variadic macros with zero arguments is only now
-  // supported in C23 and C++20 so we rely on a helper function to get around this
-  // in a portable way
-  #define mjERROR(...) {                                                \
-    char _buf[sizeof(_GET_MSG(__VA_ARGS__)) + sizeof(__func__) + 1];    \
-    _mju_error_prefix(_buf, sizeof(_buf), __func__, __VA_ARGS__);       \
-  }
-
-#else
-  #define mjERROR mju_error
-#endif  // c99 or c++11
 
 //------------------------------ malloc and free ---------------------------------------------------
 

src/engine/engine_util_misc.c

@@ -1207,6 +1207,20 @@ int mju_isZero(mjtNum* vec, int n) {
 
 
 
+// set integer vector to 0
+void mju_zeroInt(int* res, int n) {
+  memset(res, 0, n*sizeof(int));
+}
+
+
+
+// copy int vector vec into res
+void mju_copyInt(int* res, const int* vec, int n) {
+  memcpy(res, vec, n*sizeof(int));
+}
+
+
+
 // standard normal random number generator (optional second number)
 mjtNum mju_standardNormal(mjtNum* num2) {
   const mjtNum scale = 2.0/((mjtNum)RAND_MAX);

src/engine/engine_util_misc.h

@@ -119,6 +119,12 @@ MJAPI int mju_isBad(mjtNum x);
 // return 1 if all elements are 0
 MJAPI int mju_isZero(mjtNum* vec, int n);
 
+// set integer vector to 0
+MJAPI void mju_zeroInt(int* res, int n);
+
+// copy int vector vec into res
+MJAPI void mju_copyInt(int* res, const int* vec, int n);
+
 // standard normal random number generator (optional second number)
 MJAPI mjtNum mju_standardNormal(mjtNum* num2);
 

src/engine/engine_util_solve.c

@@ -16,7 +16,6 @@
 
 #include <math.h>
 #include <stdio.h>
-#include <string.h>
 
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmacro.h>
@@ -148,7 +147,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
                          mjData* d) {
   int rank = n;
 
-  mjMARKSTACK;
+  mj_markStack(d);
   int* buf_ind = mj_stackAllocInt(d, n);
   mjtNum* sparse_buf = mj_stackAllocNum(d, n);
 
@@ -199,7 +198,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
   return rank;
 }
 
@@ -235,7 +234,7 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int
 
     // x(i) -= sum_j L(i,j)*x(j), j=0:i-1
     if (nnz > 1) {
-      res[i] -= mju_dotSparse(mat+adr, res, nnz-1, colind+adr);
+      res[i] -= mju_dotSparse(mat+adr, res, nnz-1, colind+adr, /*flg_unc1=*/0);
       // modulo AVX, the above line does
       // for (int j=0; j<nnz-1; j++)
       //   res[i] -= mat[adr+j]*res[colind[adr+j]];
@@ -254,7 +253,7 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int
 int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus,
                          int* rownnz, int* rowadr, int* colind, int x_nnz, int* x_ind,
                          mjData* d) {
-  mjMARKSTACK;
+  mj_markStack(d);
   int* buf_ind = mj_stackAllocInt(d, n);
   mjtNum* sparse_buf = mj_stackAllocNum(d, n);
 
@@ -295,7 +294,7 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus,
     i = i - 1 + (new_x_nnz - i);
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
   return rank;
 }
 
@@ -592,7 +591,7 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
   int* remaining = scratch;
 
   // set remaining = rownnz
-  memcpy(remaining, rownnz, n*sizeof(int));
+  mju_copyInt(remaining, rownnz, n);
 
   // diagonal elements (i,i)
   for (int i=n-1; i >= 0; i--) {

src/engine/engine_util_sparse.c

@@ -13,24 +13,25 @@
 // limitations under the License.
 
 #include "engine/engine_util_sparse.h"
-#include "engine/engine_util_sparse_avx.h"
+#include "engine/engine_util_sparse_avx.h"  // IWYU pragma: keep
 
 #include <string.h>
 
 #include <mujoco/mjdata.h>
-#include <mujoco/mjmacro.h>
 #include <mujoco/mjtnum.h>
 #include "engine/engine_io.h"
 #include "engine/engine_util_blas.h"
+#include "engine/engine_util_misc.h"
 
 
 //------------------------------ sparse operations -------------------------------------------------
 
 // dot-product, first vector is sparse
+//  flg_unc1: is vec1 memory layout uncompressed
 mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2,
-                     const int nnz1, const int* ind1) {
+                     const int nnz1, const int* ind1, int flg_unc1) {
 #ifdef mjUSEAVX
-  return mju_dotSparse_avx(vec1, vec2, nnz1, ind1);
+  return mju_dotSparse_avx(vec1, vec2, nnz1, ind1, flg_unc1);
 #else
   int i = 0;
   mjtNum res = 0;
@@ -40,17 +41,33 @@ mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2,
   mjtNum res2 = 0;
   mjtNum res3 = 0;
 
-  for (; i <= n_4; i+=4) {
+
-    res0 += vec1[i+0] * vec2[ind1[i+0]];
+  if (flg_unc1) {
-    res1 += vec1[i+1] * vec2[ind1[i+1]];
+    for (; i <= n_4; i+=4) {
-    res2 += vec1[i+2] * vec2[ind1[i+2]];
+      res0 += vec1[ind1[i+0]] * vec2[ind1[i+0]];
-    res3 += vec1[i+3] * vec2[ind1[i+3]];
+      res1 += vec1[ind1[i+1]] * vec2[ind1[i+1]];
+      res2 += vec1[ind1[i+2]] * vec2[ind1[i+2]];
+      res3 += vec1[ind1[i+3]] * vec2[ind1[i+3]];
+    }
+  } else {
+    for (; i <= n_4; i+=4) {
+      res0 += vec1[i+0] * vec2[ind1[i+0]];
+      res1 += vec1[i+1] * vec2[ind1[i+1]];
+      res2 += vec1[i+2] * vec2[ind1[i+2]];
+      res3 += vec1[i+3] * vec2[ind1[i+3]];
+    }
   }
   res = (res0 + res2) + (res1 + res3);
 
   // scalar part
-  for (; i < nnz1; i++) {
+  if (flg_unc1) {
-    res += vec1[i] * vec2[ind1[i]];
+    for (; i < nnz1; i++) {
+      res += vec1[ind1[i]] * vec2[ind1[i]];
+    }
+  } else {
+    for (; i < nnz1; i++) {
+      res += vec1[i] * vec2[ind1[i]];
+    }
   }
 
   return res;
@@ -91,9 +108,10 @@ void mju_dotSparseX3(mjtNum* res0, mjtNum* res1, mjtNum* res2,
 
 
 // dot-product, both vectors are sparse
+//  flg_unc2: is vec2 memory layout uncompressed
 mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2,
                       const int nnz1, const int* ind1,
-                      const int nnz2, const int* ind2) {
+                      const int nnz2, const int* ind2, int flg_unc2) {
   int i1 = 0, i2 = 0;
   mjtNum res = 0;
 
@@ -108,7 +126,12 @@ mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2,
 
     // match: accumulate result, advance both
     if (adr1 == adr2) {
-      res += vec1[i1++] * vec2[i2++];
+      if (flg_unc2) {
+        res += vec1[i1++] * vec2[adr2];
+        i2++;
+      } else {
+        res += vec1[i1++] * vec2[i2++];
+      }
     }
 
     // otherwise advance smaller
@@ -176,7 +199,7 @@ void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
 #else
   // regular sparse dot-product
   for (int r=0; r < nr; r++) {
-    res[r] = mju_dotSparse(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
+    res[r] = mju_dotSparse(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc1=*/0);
   }
 #endif  // mjUSEAVX
 }
@@ -249,8 +272,8 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b,
 
   // copy dst into buf
   if (dst_nnz) {
-    memcpy(buf, dst, dst_nnz*sizeof(mjtNum));
+    mju_copy(buf, dst, dst_nnz);
-    memcpy(buf_ind, dst_ind, dst_nnz*sizeof(int));
+    mju_copyInt(buf_ind, dst_ind, dst_nnz);
   }
 
   // prepare to merge buf and src into dst
@@ -370,7 +393,7 @@ void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
                          int* res_rownnz, int* res_rowadr, int* res_colind,
                          const int* rownnz, const int* rowadr, const int* colind) {
   // clear number of non-zeros for each row of transposed
-  memset(res_rownnz, 0, nc*sizeof(int));
+  mju_zeroInt(res_rownnz, nc);
 
   // total number of non-zeros of mat
   int nnz = rowadr[nr-1] + rownnz[nr-1];
@@ -450,13 +473,12 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr,
                             const int* rownnzT, const int* rowadrT,
                             const int* colindT, const int* rowsuperT,
                             mjData* d) {
-  mjMARKSTACK;
+  mj_markStack(d);
   int* chain = mj_stackAllocInt(d, 2*nc);
   int nchain = 0;
   int* res_colind = NULL;
 
   for (int r=0; r < nc; r++) {
-
     // supernode; copy everything to next row
     if (rowsuperT && r > 0 && rowsuperT[r-1] > 0) {
       res_rownnz[r] = res_rownnz[r - 1];
@@ -532,7 +554,7 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr,
     res_rowadr[r] = res_rowadr[r-1] + res_rownnz[r-1];
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 
@@ -556,7 +578,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
                         const int* colindT, const int* rowsuperT,
                         mjData* d) {
   // allocate space for accumulation buffer and matT
-  mjMARKSTACK;
+  mj_markStack(d);
 
   // a dense row buffer that stores the current row in the resulting matrix
   mjtNum* buffer = mj_stackAllocNum(d, nc);
@@ -575,7 +597,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
     // if rowsuper, use the previous row sparsity structure
     if (rowsuperT && i > 0 && rowsuperT[i-1]) {
       res_rownnz[i] = res_rownnz[i-1];
-      memcpy(cols, res_colind+res_rowadr[i-1], res_rownnz[i]*sizeof(int));
+      mju_copyInt(cols, res_colind+res_rowadr[i-1], res_rownnz[i]);
     }
 
     // iterate through each row of M'
@@ -665,5 +687,5 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
     }
   }
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }

src/engine/engine_util_sparse.h

@@ -25,14 +25,14 @@ extern "C" {
 
 //------------------------------ sparse operations -------------------------------------------------
 
-// dot-product, first vector is sparse
+// dot-product, vec1 is sparse, can be uncompressed
 MJAPI mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2,
-                           const int nnz1, const int* ind1);
+                           const int nnz1, const int* ind1, int flg_unc1);
 
-// dot-product, both vectors are sparse
+// dot-product, both vectors are sparse, vec2 can be uncompressed
-mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2,
+MJAPI mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2,
-                      const int nnz1, const int* ind1,
+                            const int nnz1, const int* ind1,
-                      const int nnz2, const int* ind2);
+                            const int nnz2, const int* ind2, int flg_unc2);
 
 // convert matrix from dense to sparse
 MJAPI void mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc,

src/engine/engine_util_sparse_avx.h

@@ -30,9 +30,10 @@
 //------------------------------ sparse operations using avx ---------------------------------------
 
 // dot-product, first vector is sparse
+//  flg_unc1: is vec1 memory layout uncompressed
 static inline
 mjtNum mju_dotSparse_avx(const mjtNum* vec1, const mjtNum* vec2,
-                         const int nnz1, const int* ind1) {
+                         const int nnz1, const int* ind1, int flg_unc1) {
   int i = 0;
   mjtNum res = 0;
   int nnz1_4 = nnz1 - 4;
@@ -47,20 +48,43 @@ mjtNum mju_dotSparse_avx(const mjtNum* vec1, const mjtNum* vec2,
                          vec2[ind1[2]],
                          vec2[ind1[1]],
                          vec2[ind1[0]]);
-    val1 = _mm256_loadu_pd(vec1);
+    if (flg_unc1) {
+      val1 = _mm256_set_pd(vec1[ind1[3]],
+                           vec1[ind1[2]],
+                           vec1[ind1[1]],
+                           vec1[ind1[0]]);
+    } else {
+      val1 = _mm256_loadu_pd(vec1);
+    }
     sum = _mm256_mul_pd(val1, val2);
     i = 4;
 
     // parallel computation
-    while (i<=nnz1_4) {
+    if (flg_unc1) {
-      val1 = _mm256_loadu_pd(vec1+i);
+      while (i<=nnz1_4) {
-      val2 = _mm256_set_pd(vec2[ind1[i+3]],
+        val1 = _mm256_set_pd(vec1[ind1[i+3]],
-                           vec2[ind1[i+2]],
+                             vec1[ind1[i+2]],
-                           vec2[ind1[i+1]],
+                             vec1[ind1[i+1]],
-                           vec2[ind1[i+0]]);
+                             vec1[ind1[i+0]]);
-      prod = _mm256_mul_pd(val1, val2);
+        val2 = _mm256_set_pd(vec2[ind1[i+3]],
-      sum = _mm256_add_pd(sum, prod);
+                             vec2[ind1[i+2]],
-      i += 4;
+                             vec2[ind1[i+1]],
+                             vec2[ind1[i+0]]);
+        prod = _mm256_mul_pd(val1, val2);
+        sum = _mm256_add_pd(sum, prod);
+        i += 4;
+      }
+    } else {
+      while (i<=nnz1_4) {
+        val1 = _mm256_loadu_pd(vec1+i);
+        val2 = _mm256_set_pd(vec2[ind1[i+3]],
+                             vec2[ind1[i+2]],
+                             vec2[ind1[i+1]],
+                             vec2[ind1[i+0]]);
+        prod = _mm256_mul_pd(val1, val2);
+        sum = _mm256_add_pd(sum, prod);
+        i += 4;
+      }
     }
 
     // reduce
@@ -72,8 +96,14 @@ mjtNum mju_dotSparse_avx(const mjtNum* vec1, const mjtNum* vec2,
   }
 
   // scalar part
-  for (; i<nnz1; i++) {
+  if (flg_unc1) {
-    res += vec1[i] * vec2[ind1[i]];
+    for (; i < nnz1; i++) {
+      res += vec1[ind1[i]] * vec2[ind1[i]];
+    }
+  } else {
+    for (; i < nnz1; i++) {
+      res += vec1[i] * vec2[ind1[i]];
+    }
   }
 
   return res;
@@ -179,7 +209,7 @@ void mju_mulMatVecSparse_avx(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
   if (!rowsuper) {
     // regular sparse dot-product
     for (int r=0; r<nr; r++) {
-      res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
+      res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc2=*/0);
     }
 
     return;
@@ -202,7 +232,7 @@ void mju_mulMatVecSparse_avx(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
 
       // handle remaining rows
       while (rs>0) {
-        res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
+        res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc2=*/0);
 
         r++;
         rs--;
@@ -213,7 +243,7 @@ void mju_mulMatVecSparse_avx(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
     }
 
     else {
-      res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
+      res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc2=*/0);
     }
   }
 }

src/engine/engine_vfs.c

@@ -14,10 +14,10 @@
 
 #include "engine/engine_vfs.h"
 
+#include <stddef.h>
 #include <string.h>
 
 #include "engine/engine_array_safety.h"
-#include "engine/engine_plugin.h"
 #include "engine/engine_resource.h"
 #include "engine/engine_util_errmem.h"
 #include "engine/engine_util_misc.h"
@@ -90,7 +90,7 @@ int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) {
   mjSTRNCPY(vfs->filename[vfs->nfile], newname);
 
   // allocate and read
-  int filesize = 0;
+  size_t filesize = 0;
   vfs->filedata[vfs->nfile] = mju_fileToMemory(fullname, &filesize);
   if (!vfs->filedata[vfs->nfile]) {
     return -1;
@@ -211,11 +211,11 @@ void mj_deleteVFS(mjVFS* vfs) {
 
 // open callback for the VFS resource provider
 static int vfs_open_callback(mjResource* resource) {
-  if (!resource || !resource->provider_data || !resource->name) {
+  if (!resource || !resource->name || !resource->data) {
     return 0;
   }
 
-  const mjVFS* vfs = (const mjVFS*) resource->provider_data;
+  const mjVFS* vfs = (const mjVFS*) resource->data;
   return mj_findFileVFS(vfs, resource->name) >= 0;
 }
 
@@ -223,12 +223,12 @@ static int vfs_open_callback(mjResource* resource) {
 
 // read callback for the VFS resource provider
 static int vfs_read_callback(mjResource* resource, const void** buffer) {
-  if (!resource || !resource->provider_data) {
+  if (!resource || !resource->name || !resource->data) {
     *buffer = NULL;
     return -1;
   }
 
-  const mjVFS* vfs = (const mjVFS*) resource->provider_data;
+  const mjVFS* vfs = (const mjVFS*) resource->data;
   int i = mj_findFileVFS(vfs, resource->name);
   if (i < 0) {
     *buffer = NULL;
@@ -260,16 +260,50 @@ static void vfs_getdir_callback(mjResource* resource, const char** dir, int* ndi
 
 
 
-// registers a VFS resource provider; returns the index of the provider
+// open VFS resource
-int mj_registerVfsProvider(const mjVFS* vfs) {
+mjResource* mju_openVfsResource(const char* name, const mjVFS* vfs) {
-  mjpResourceProvider provider = {
+  if (vfs == NULL) {
-    .prefix = mjVFS_PREFIX,
+    return NULL;
-    .open = &vfs_open_callback,
+  }
-    .read = &vfs_read_callback,
+
-    .close = &vfs_close_callback,
+  // VFS provider
-    .getdir = &vfs_getdir_callback,
+  static struct mjpResourceProvider provider = {
-    .data = (void*) vfs
+    .prefix   = NULL,
+    .data     = NULL,
+    .open     = &vfs_open_callback,
+    .read     = &vfs_read_callback,
+    .close    = &vfs_close_callback,
+    .getdir   = &vfs_getdir_callback,
+    .modified = NULL
   };
 
-  return mjp_registerResourceProviderInternal(&provider);
+  // create resource
+  mjResource* resource = (mjResource*) mju_malloc(sizeof(mjResource));
+  if (resource == NULL) {
+    mjERROR("could not allocate memory");
+    return NULL;
+  }
+
+  // clear out resource
+  memset(resource, 0, sizeof(mjResource));
+
+  // copy name
+  resource->name = mju_malloc(sizeof(char) * (strlen(name) + 1));
+  if (resource->name == NULL) {
+    mju_closeResource(resource);
+    mjERROR("could not allocate memory");
+    return NULL;
+  }
+  memcpy(resource->name, name, sizeof(char) * (strlen(name) + 1));
+  resource->data = (void*) vfs;
+
+  // open resource
+  resource->provider = &provider;
+  if (provider.open(resource)) {
+    return resource;
+  }
+
+  // not found in VFS
+  mju_closeResource(resource);
+  return NULL;
 }

src/engine/engine_vfs.h

@@ -17,6 +17,7 @@
 
 #include <mujoco/mjexport.h>
 #include <mujoco/mjmodel.h>
+#include <mujoco/mjplugin.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -40,8 +41,8 @@ MJAPI int mj_deleteFileVFS(mjVFS* vfs, const char* filename);
 // delete all files from VFS
 MJAPI void mj_deleteVFS(mjVFS* vfs);
 
-// registers a VFS resource provider; returns the index of the provider
+// open VFS resource
-MJAPI int mj_registerVfsProvider(const mjVFS* vfs);
+MJAPI mjResource* mju_openVfsResource(const char* name, const mjVFS* vfs);
 
 #ifdef __cplusplus
 }

src/engine/engine_vis_interact.c

@@ -19,7 +19,6 @@
 
 #include <mujoco/mjdata.h>
 #include <mujoco/mjexport.h>
-#include <mujoco/mjmacro.h>
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjvisualize.h>
 #include "engine/engine_core_smooth.h"
@@ -518,7 +517,7 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy,
 
 // copy perturb pos,quat from selected body; set scale for perturbation
 void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPerturb* pert) {
-  mjMARKSTACK;
+  mj_markStack(d);
 
   int nv  = m->nv;
   int sel = pert->select;
@@ -529,6 +528,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
 
   // invalid selected body: return
   if (sel <= 0 || sel >= m->nbody) {
+    mj_freeStack(d);
     return;
   }
 
@@ -557,7 +557,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
   mju_sub3(dif, pert->refselpos, headpos);
   pert->scale = mjv_frustumHeight(scn) * mju_dot3(dif, forward);
 
-  mjFREESTACK;
+  mj_freeStack(d);
 }
 
 

src/engine/engine_vis_state.c

@@ -263,9 +263,9 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt,
                           mjvSceneState* scnstate) {
   // Check that mjModel sizes haven't changed.
 #define X(var)
-#define XMJV(var)                      \
+#define XMJV(var)                                                          \
-  if (scnstate->model.var != m->var) { \
+  if (scnstate->model.var != m->var) {                                     \
-    mjERROR("m->%s changed", #var);  \
+    mjERROR("m->%s changed: %d vs %d", #var, scnstate->model.var, m->var); \
   }
   MJMODEL_INTS
 #undef XMJV

src/render/glad/glad.c

@@ -36,6 +36,11 @@
 // Online:
 //     https://glad.dav1d.de/#profile=compatibility&language=c&specification=gl&loader=on&api=gl%3D1.5&extensions=GL_ARB_framebuffer_object&extensions=GL_ARB_seamless_cube_map&extensions=GL_ARB_vertex_buffer_object&extensions=GL_KHR_debug
 
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#endif
+
 #if !defined(_WIN32) && !defined(__CYGWIN__) && !defined(__APPLE__) && \
     !defined(__HAIKU__) && !defined(_GNU_SOURCE)
 #define _GNU_SOURCE
@@ -1471,12 +1476,12 @@ static void mjGlad_find_coreGL(void) {
   }
 }
 
-int mjGladLoadGLUnsafe() {
+int mjGladLoadGLUnsafe(void) {
-  if(mjGlad_open_gl()) {
+  if (mjGlad_open_gl()) {
     mjGLVersion.major = 0; mjGLVersion.minor = 0;
     glGetString = (PFNGLGETSTRINGPROC)mjGlad_get_proc("glGetString");
-    if(glGetString == NULL) return 0;
+    if (glGetString == NULL) return 0;
-    if(glGetString(GL_VERSION) == NULL) return 0;
+    if (glGetString(GL_VERSION) == NULL) return 0;
     mjGlad_find_coreGL();
     mjGlad_load_GL_VERSION_1_0(mjGlad_get_proc);
     mjGlad_load_GL_VERSION_1_1(mjGlad_get_proc);
@@ -1496,3 +1501,7 @@ int mjGladLoadGLUnsafe() {
     return 0;
   }
 }
+
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif

src/render/render_context.c

@@ -140,7 +140,7 @@ static void makePlane(const mjModel* m, mjrContext* con) {
 
             // record
             grid[k][x] = left;
-            grid[k][x+1] = mjMAX(left, right); // just in case
+            grid[k][x+1] = mjMAX(left, right);  // just in case
           }
         }
 
@@ -1458,7 +1458,7 @@ void GLAPIENTRY debugCallback(GLenum source,
 
 
 // returns 1 if MUJOCO_GL_DEBUG environment variable is set to 1
-static int glDebugEnabled() {
+static int glDebugEnabled(void) {
   char* debug = getenv("MUJOCO_GL_DEBUG");
   return debug && strcmp(debug, "1") == 0;
 }

src/thread/CMakeLists.txt

@@ -0,0 +1,23 @@
+# 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
+#
+#     https://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.
+
+set(MUJOCO_THREAD_SRCS
+    lockless_queue.h
+    task.cc
+    task.h
+    thread_pool.cc
+    thread_pool.h
+)
+
+target_sources(mujoco PRIVATE ${MUJOCO_THREAD_SRCS})

src/thread/lockless_queue.h

@@ -0,0 +1,154 @@
+// 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.
+// IWYU pragma: private, include "third_party/mujoco/include/mujoco.h"
+// IWYU pragma: friend "third_party/(py/)?mujoco/.*"
+
+#ifndef MUJOCO_SRC_THREAD_LOCKLESS_QUEUE_H_
+#define MUJOCO_SRC_THREAD_LOCKLESS_QUEUE_H_
+
+#include <atomic>
+#include <climits>
+#include <cstddef>
+#include <thread>
+
+namespace mujoco {
+
+// A Lockless Queue allows for sending information quickly between different
+// threads. This is a Multi-Producer Multi-Consumer Lockless Queue allowing for
+// multiple threads to be adding items to the queue while multiple threads are
+// consuming items from the queue. Internally it uses a Ring Buffer for storage
+// so it will not grow as items are added. Push will block if the Queue is full
+// and Pop will block if it is empty.
+//
+// For a basic overview of this category of structures:
+// https://www.linuxjournal.com/content/lock-free-multi-producer-multi-consumer-queue-ring-buffer
+template  <typename T, size_t buffer_capacity>
+class LocklessQueue {
+ public:
+  bool full() const {
+    return full_internal(
+        convert_to_index(read_cursor_), convert_to_index(write_cursor_));
+  }
+
+  bool empty() const {
+    return maximum_read_cursor_ == read_cursor_;
+  }
+
+  // Push an element into the queue.
+  void push(const T& input) {
+    // Reserve a slot in the queue
+    size_t current_write_cursor;
+    size_t dummy_current_write_cursor;
+    size_t next_write_cursor;
+    size_t current_write_index;
+    size_t current_read_index;
+    do {
+      // Check if the queue is full.
+      do {
+        current_write_cursor = write_cursor_.load();
+        current_write_index = convert_to_index(current_write_cursor);
+        next_write_cursor = get_next_cursor(current_write_cursor);
+
+        current_read_index = convert_to_index(read_cursor_.load());
+      } while (full_internal(current_read_index, current_write_index));
+
+      // Once it's not full, attempt to grab a slot to write.
+      dummy_current_write_cursor = current_write_cursor;
+    } while (!write_cursor_.compare_exchange_weak(
+                 dummy_current_write_cursor, next_write_cursor));
+
+    // Write the entry.
+    buffer_[current_write_index].store(input);
+
+    // Increment maximum read cursor. Note here it has to wait if the compare
+    // and exchange fails as another thread might not have completed its write.
+    do {
+      dummy_current_write_cursor = current_write_cursor;
+    } while (!maximum_read_cursor_.compare_exchange_weak(
+                 dummy_current_write_cursor, next_write_cursor));
+  }
+
+  // Pop an element from the queue.
+  T pop() {
+    size_t current_read_cursor;
+    size_t dummy_current_read_cursor;
+    size_t current_read_index;
+    size_t next_read_cursor;
+    size_t current_maximum_read_cursor;
+    size_t current_maximum_read_index;
+    bool empty = false;
+    T result;
+    do {
+      // Wait until the queue has an element
+      do {
+        if (empty) {
+          std::this_thread::yield();
+        }
+        current_read_cursor = read_cursor_.load();
+        current_maximum_read_cursor = maximum_read_cursor_.load();
+
+        current_read_index = convert_to_index(current_read_cursor);
+        current_maximum_read_index = convert_to_index(
+            current_maximum_read_cursor);
+
+        empty = empty_internal(
+            current_read_index, current_maximum_read_index);
+      } while (empty);
+
+      next_read_cursor = get_next_cursor(current_read_cursor);
+
+      // Attempt to grab the element, if unsuccessful then wait for the next
+      // element to arrive.
+      result = buffer_[current_read_index].load();
+      dummy_current_read_cursor = current_read_cursor;
+    } while (!read_cursor_.compare_exchange_weak(
+                 dummy_current_read_cursor, next_read_cursor));
+
+    return result;
+  }
+
+ private:
+  size_t convert_to_index(size_t input) const {
+    return input % internal_buffer_capacity_;
+  }
+
+  size_t get_next_cursor(size_t input) const {
+    return (input + 1) % cursor_max_;
+  }
+
+  size_t get_next_index(size_t input) const {
+    return convert_to_index(get_next_cursor(input));
+  }
+
+  bool full_internal(size_t read_index, size_t write_index) const {
+    return get_next_index(write_index) == read_index;
+  }
+
+  bool empty_internal(size_t read_index, size_t write_index) const {
+    return read_index == write_index;
+  }
+
+  const size_t internal_buffer_capacity_ = buffer_capacity + 1;
+  const size_t cursor_max_ = UINT_MAX - (UINT_MAX % internal_buffer_capacity_);
+
+  std::atomic<size_t> read_cursor_ = 0;
+  std::atomic<size_t> write_cursor_ = 0;
+  std::atomic<size_t> maximum_read_cursor_ = 0;
+
+  std::atomic<T> buffer_[(buffer_capacity + 1)];
+};
+
+}  // namespace mujoco
+
+#endif  // MUJOCO_SRC_THREAD_LOCKLESS_QUEUE_H_

src/thread/task.cc

@@ -0,0 +1,24 @@
+// 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.
+
+#include "thread/task.h"
+
+#include <mujoco/mjthread.h>
+#include <mujoco/mujoco.h>
+
+// waits for a task to complete
+void mju_taskJoin(mjTask* task) {
+  mujoco::Task* task_ptr = static_cast<mujoco::Task*>(static_cast<void*>(task));
+  task_ptr->Join();
+}

src/thread/task.h

@@ -0,0 +1,70 @@
+// 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.
+// IWYU pragma: private, include "third_party/mujoco/include/mujoco.h"
+// IWYU pragma: friend "third_party/(py/)?mujoco/.*"
+
+#ifndef MUJOCO_SRC_THREAD_TASK_H_
+#define MUJOCO_SRC_THREAD_TASK_H_
+
+#ifdef __cplusplus
+
+#include <atomic>
+#include <new>
+#include <thread>
+
+namespace mujoco {
+
+class Task {
+ public:
+  using FunctionPtr = void* (*)(void*);
+  enum Status {
+    QUEUED,
+    COMPLETE,
+  };
+
+  static void Initialize(
+      Task* task,
+      FunctionPtr start_routine,
+      void* args) {
+    // instantiate a task at the pointer passed in
+    new(task) Task();
+    task->start_routine_ = start_routine;
+    task->args_ = args;
+    task->status_ = Status::QUEUED;
+  }
+
+  void Execute() {
+    args_ = start_routine_(args_);
+    status_ = Status::COMPLETE;
+  }
+
+  void Join() {
+    while (status_ != Status::COMPLETE) {
+      std::this_thread::yield();
+    }
+  }
+
+ private:
+  FunctionPtr start_routine_;
+
+  void* args_;
+
+  std::atomic<Status> status_ = Status::QUEUED;
+};
+
+}  // namespace mujoco
+
+#endif  // __cplusplus
+
+#endif  // MUJOCO_SRC_THREAD_TASK_H_

src/thread/thread_pool.cc

@@ -0,0 +1,52 @@
+// 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.
+
+#include "thread/thread_pool.h"
+
+#include <cstddef>
+
+#include <mujoco/mjthread.h>
+#include <mujoco/mujoco.h>
+#include "thread/task.h"
+
+static constexpr size_t kMaxThreads = 128;
+
+// create a thread pool
+mjThreadPool* mju_threadPoolCreate(size_t number_of_threads) {
+  mujoco::ThreadPool<kMaxThreads>* thread_pool =
+      new mujoco::ThreadPool<kMaxThreads>(number_of_threads);
+  return static_cast<mjThreadPool*>(static_cast<void*>(thread_pool));
+}
+
+// start a task in the threadpool
+void mju_threadPoolEnqueue(
+    mjThreadPool* thread_pool, mjTask* task, mjStartRoutine start_routine,
+    void* args) {
+  mujoco::ThreadPool<kMaxThreads>* thread_pool_ptr =
+      static_cast<mujoco::ThreadPool<kMaxThreads>*>(
+          static_cast<void*>(thread_pool));
+  thread_pool_ptr->Enqueue(
+      static_cast<mujoco::Task*>(static_cast<void*>(task)), start_routine,
+      args);
+}
+
+// shutdown the threadpool and free the memory
+void mju_threadPoolDestroy(mjThreadPool* thread_pool) {
+  mujoco::ThreadPool<kMaxThreads>* thread_pool_ptr =
+      static_cast<mujoco::ThreadPool<kMaxThreads>*>(
+          static_cast<void*>(thread_pool));
+  thread_pool_ptr->Shutdown();
+  delete thread_pool_ptr;
+}
+

src/thread/thread_pool.h

@@ -0,0 +1,102 @@
+// 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.
+// IWYU pragma: private, include "third_party/mujoco/include/mujoco.h"
+// IWYU pragma: friend "third_party/(py/)?mujoco/.*"
+
+#ifndef MUJOCO_SRC_THREAD_THREAD_POOL_H_
+#define MUJOCO_SRC_THREAD_THREAD_POOL_H_
+
+#ifdef __cplusplus
+
+#include <atomic>
+#include <cstddef>
+#include <thread>
+
+#include "thread/lockless_queue.h"
+#include "thread/task.h"
+
+namespace mujoco {
+
+static constexpr size_t kThreadPoolQueueSize = 640;
+
+template <size_t max_number_of_threads>
+class ThreadPool {
+ public:
+  ThreadPool(size_t number_of_threads)
+      : number_of_threads_(number_of_threads) {
+    for (int i = 0; i < number_of_threads_; ++i) {
+      threads_[i] = std::thread(ThreadPoolWorker, static_cast<void*>(this));
+    }
+  }
+
+  // start a task in the threadpool
+  void Enqueue(
+      Task* task, Task::FunctionPtr start_routine, void* args) {
+    Task::Initialize(task, start_routine, args);
+    lockless_queue_.push(static_cast<void*>(task));
+  }
+
+  // shutdown the threadpool
+  void Shutdown() {
+    if (shutdown_) {
+      return;
+    }
+
+    shutdown_ = true;
+    Task shutdown_tasks[max_number_of_threads];
+    for (int i = 0; i < number_of_threads_; ++i) {
+      Enqueue(&shutdown_tasks[i], ShutdownFunction, nullptr);
+    }
+
+    for (int i = 0; i < number_of_threads_; ++i) {
+      threads_[i].join();
+    }
+  }
+
+  ~ThreadPool() { Shutdown(); }
+
+ private:
+  // method executed by running threads
+  static void ThreadPoolWorker(void* arg) {
+    ThreadPool<max_number_of_threads>* thread_pool =
+        static_cast<ThreadPool<max_number_of_threads>*>(arg);
+    while (!thread_pool->shutdown_) {
+      Task* task = static_cast<Task*>(thread_pool->lockless_queue_.pop());
+      task->Execute();
+    }
+  }
+
+  // shutdown function passed to running threads to ensure cleans shutdown
+  static void* ShutdownFunction(void* args) {
+    return NULL;
+  }
+
+  // is the thread pool is being shut down
+  std::atomic<bool> shutdown_ = false;
+
+  // actual number of running threads in the threadpool
+  const size_t number_of_threads_;
+
+  // OS threads that are running in this pool
+  std::thread threads_[max_number_of_threads];
+
+  // queue of tasks to execute
+  LocklessQueue<void*, kThreadPoolQueueSize> lockless_queue_;
+};
+
+}  // namespace mujoco
+
+#endif  // __cplusplus
+
+#endif  // MUJOCO_SRC_THREAD_THREAD_POOL_H_

src/user/user_composite.cc

@@ -34,6 +34,7 @@
 #include "user/user_model.h"
 #include "user/user_objects.h"
 #include "user/user_util.h"
+#include "xml/xml_util.h"
 
 namespace {
 namespace mju = ::mujoco::util;
@@ -235,7 +236,7 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
   }
 
   // check spacing
-  if (type==mjCOMPTYPE_GRID || type==mjCOMPTYPE_PARTICLE) {
+  if (type==mjCOMPTYPE_GRID || (type==mjCOMPTYPE_PARTICLE && uservert.empty())) {
     if (spacing < mju_max(def[0].geom.size[0],
                   mju_max(def[0].geom.size[1], def[0].geom.size[2]))) {
       return comperr(error, "Spacing must be larger than geometry size",
@@ -314,66 +315,178 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
 
 
 
-// make particles
 bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int error_sz) {
-  // create bodies and geoms
+  char txt[100];
-  for (int ix=0; ix<count[0]; ix++) {
+  std::vector<int> face;
-    for (int iy=0; iy<count[1]; iy++) {
-      for (int iz=0; iz<count[2]; iz++) {
-        // create body
-        char txt[100];
-        mjCBody* b = body->AddBody(NULL);
-        mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
-        b->name = txt;
 
-        // set body position
+  // populate vertices and names
-        b->pos[0] = offset[0] + spacing*(ix - 0.5*count[0]);
+  if (uservert.empty()) {
-        b->pos[1] = offset[1] + spacing*(iy - 0.5*count[1]);
+    if (spacing < mju_max(def[0].geom.size[0],
-        b->pos[2] = offset[2] + spacing*(iz - 0.5*count[2]);
+                  mju_max(def[0].geom.size[1], def[0].geom.size[2])))
+      return comperr(error, "Spacing must be larger than geometry size", error_sz);
 
-        // add slider joints if none defined
+    for (int ix=0; ix<count[0]; ix++) {
-        if (!add[mjCOMPKIND_PARTICLE]) {
+      for (int iy=0; iy<count[1]; iy++) {
-          for (int i=0; i<3; i++) {
+        for (int iz=0; iz<count[2]; iz++) {
-            mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0], false);
+          uservert.push_back(spacing*(ix - 0.5*count[0]));
-            jnt->def = body->def;
+          uservert.push_back(spacing*(iy - 0.5*count[1]));
-            jnt->type = mjJNT_SLIDE;
+          uservert.push_back(spacing*(iz - 0.5*count[2]));
-            mjuu_setvec(jnt->pos, 0, 0, 0);
-            mjuu_setvec(jnt->axis, 0, 0, 0);
-            jnt->axis[i] = 1;
-          }
-        }
 
-        // add user-specified joints
+          mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
-        else {
+          username.push_back(std::string(txt));
-          for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) {
-            mjCJoint* jnt = b->AddJoint(&defjnt, false);
-            jnt->def = body->def;
-          }
-        }
-
-        // add geom
-        mjCGeom* g = b->AddGeom(def);
-        g->def = body->def;
-
-        // add plugin
-        if (plugin_instance) {
-          b->is_plugin = true;
-          b->plugin_name = plugin_name;
-          b->plugin_instance = plugin_instance;
-          b->plugin_instance_name = plugin_instance_name;
-
-          // propagate attributes
-          if (plugin_name == "mujoco.elasticity.solid") {
-            b->plugin_instance->config_attribs["nx"] = std::to_string(count[0]);
-            b->plugin_instance->config_attribs["ny"] = std::to_string(count[1]);
-            b->plugin_instance->config_attribs["nz"] = std::to_string(count[2]);
-          }
         }
       }
     }
   }
 
-  // skin
+  // create faces
+  if (userface.empty()) {
+    if (dim == 3) {
+      int cube2tets[6][4] = {{0, 3, 1, 7}, {0, 1, 4, 7},
+                             {1, 3, 2, 7}, {1, 2, 6, 7},
+                             {1, 5, 4, 7}, {1, 6, 5, 7}};
+      for (int ix = 0; ix < count[0]-1; ix++) {
+        for (int iy = 0; iy < count[1]-1; iy++) {
+          for (int iz = 0; iz < count[2]-1; iz++) {
+            int vert[8] = {
+              count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+0,
+              count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+0,
+              count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+0,
+              count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+0,
+              count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+1,
+              count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+1,
+              count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+1,
+              count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+1,
+            };
+            for (int s = 0; s < 6; s++) {
+              for (int v = 0; v < 4; v++) {
+                face.push_back(vert[cube2tets[s][v]]+1);
+              }
+            }
+          }
+        }
+      }
+    } else if (dim == 2) {
+      int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}};
+      for (int ix = 0; ix < count[0]-1; ix++) {
+        for (int iy = 0; iy < count[1]-1; iy++) {
+          int vert[4] = {
+            count[2]*count[1]*(ix+0) + count[2]*(iy+0),
+            count[2]*count[1]*(ix+1) + count[2]*(iy+0),
+            count[2]*count[1]*(ix+1) + count[2]*(iy+1),
+            count[2]*count[1]*(ix+0) + count[2]*(iy+1),
+          };
+          for (int s = 0; s < 2; s++) {
+            for (int v = 0; v < 3; v++) {
+              face.push_back(vert[quad2tri[s][v]]+1);
+            }
+          }
+        }
+      }
+    }
+    mjXUtil::Vector2String(userface, face);
+    } else {
+    dim = 2;  // can only load a surface for now
+    mjXUtil::String2Vector(userface, face);
+  }
+
+  // compute volume
+  std::vector<mjtNum> volume(uservert.size()/3);
+  mjtNum t = 1;
+  if (dim == 2 && plugin_instance) {
+    // do nothing for now (until new passive forces are supported)
+  }
+  if (!userface.empty()) {
+    mjXUtil::String2Vector(userface, face);
+    for (int j=0; j<face.size()/3; j++) {
+      mjtNum area[3];
+      mjtNum edge1[3];
+      mjtNum edge2[3];
+
+      for (int i=0; i<3; i++) {
+        edge1[i] = uservert[3*(face[3*j+1]-1)+i] - uservert[3*(face[3*j]-1)+i];
+        edge2[i] = uservert[3*(face[3*j+2]-1)+i] - uservert[3*(face[3*j]-1)+i];
+      }
+
+      mjuu_crossvec(area, edge1, edge2);
+      for (int i=0; i<3; i++) {
+        volume[face[3*j+i]-1] += sqrt(mjuu_dot3(area, area)) / 2 * t;
+      }
+    }
+  } else {
+    for (int i=0; i<uservert.size()/3; i++) {
+      volume[i] = 6 * spacing * spacing / 2 * t;
+    }
+  }
+
+  // create bodies and geoms
+  for (int i=0; i<uservert.size()/3; i++) {
+    // create body
+    mjCBody* b = body->AddBody(NULL);
+
+    if (!username.empty()) {
+      b->name = username[i];
+    } else {
+      mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i);
+      b->name = txt;
+    }
+
+    // set body position
+    b->pos[0] = offset[0] + uservert[3*i];
+    b->pos[1] = offset[1] + uservert[3*i+1];
+    b->pos[2] = offset[2] + uservert[3*i+2];
+
+    // add slider joints if none defined
+    if (!add[mjCOMPKIND_PARTICLE]) {
+      for (int i=0; i<3; i++) {
+        mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0], false);
+        jnt->def = body->def;
+        jnt->type = mjJNT_SLIDE;
+        mjuu_setvec(jnt->pos, 0, 0, 0);
+        mjuu_setvec(jnt->axis, 0, 0, 0);
+        jnt->axis[i] = 1;
+      }
+    }
+
+    // add user-specified joints
+    else {
+      for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) {
+        mjCJoint* jnt = b->AddJoint(&defjnt, false);
+        jnt->def = body->def;
+      }
+    }
+
+    // add geom
+    mjCGeom* g = b->AddGeom(def);
+    g->def = body->def;
+
+    // add site
+    mjCSite* s = b->AddSite(def);
+    s->def = body->def;
+    s->type = mjGEOM_SPHERE;
+    mju::sprintf_arr(txt, "%sS%d", prefix.c_str(), i);
+    s->name = txt;
+
+    // add plugin
+    if (plugin_instance) {
+      b->is_plugin = true;
+      b->plugin_name = plugin_name;
+      b->plugin_instance = plugin_instance;
+      b->plugin_instance_name = plugin_instance_name;
+
+      if (i==0 && !plugin_instance->config_attribs["face"].empty()) {
+        return comperr(error, "Face attribute already exists in plugin", error_sz);
+      }
+
+      b->plugin_instance->config_attribs["face"] = userface;
+
+      // update density
+      if (dim == 2) {
+        g->density *= volume[i] / (4./3. * mjPI * pow(g->size[0], 3));
+      }
+    }
+  }
+
   if (skin) {
     MakeSkin3(model);
   }
@@ -2040,8 +2153,46 @@ void mjCComposite::MakeSkin3(mjCModel* model) {
   skin->inflate = skininflate;
   skin->group = skingroup;
 
+  // copy skin from existing mesh
+  if (type==mjCOMPTYPE_PARTICLE && username.empty()) {
+    std::vector<int> face;
+    mjXUtil::String2Vector(userface, face);
+    int nvert = uservert.size()/3;
+
+    for (int j=0; j<2; j++) {
+      for (int i=0; i<nvert; i++) {
+        skin->vert.push_back(0);
+        skin->vert.push_back(0);
+        skin->vert.push_back(0);
+
+        mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i);
+        skin->bodyname.push_back(txt);
+        skin->bindpos.push_back(0);
+        skin->bindpos.push_back(0);
+        skin->bindpos.push_back(0);
+        skin->bindquat.push_back(1);
+        skin->bindquat.push_back(0);
+        skin->bindquat.push_back(0);
+        skin->bindquat.push_back(0);
+
+        vector<int> vertid;
+        vector<float> vertweight;
+        vertid.push_back(j*nvert+i);
+        vertweight.push_back(1);
+        skin->vertid.push_back(vertid);
+        skin->vertweight.push_back(vertweight);
+      }
+
+      for (int i=0; i<face.size()/3; i++) {
+        skin->face.push_back(j*nvert+face[3*i]-1);
+        skin->face.push_back(j*nvert+face[3*i+(j==0 ? 1 : 2)]-1);
+        skin->face.push_back(j*nvert+face[3*i+(j==0 ? 2 : 1)]-1);
+      }
+    }
+  }
+
   // box
-  if (type==mjCOMPTYPE_BOX || type==mjCOMPTYPE_PARTICLE) {
+  else if (type==mjCOMPTYPE_BOX || type==mjCOMPTYPE_PARTICLE) {
     // z-faces
     MakeSkin3Box(skin, count[0], count[1], 1, vcnt, "%sB%d_%d_0");
     fmt = "%sB%d_%d_" + string(cnt2);

src/user/user_composite.h

@@ -106,9 +106,13 @@ class mjCComposite {
   // currently used only for cable
   std::string initial;            // root boundary type
   std::vector<float> uservert;    // user-specified vertex positions
+  std::string userface;           // connectivity
   mjtNum size[3];                 // rope size (meaning depends on the shape)
   mjtCompShape curve[3];          // geometric shape
 
+  // body names used in the skin
+  std::vector<std::string> username;
+
   // plugin support
   bool is_plugin;
   std::string plugin_name;

src/user/user_mesh.cc

@@ -13,12 +13,14 @@
 // limitations under the License.
 
 #include <algorithm>
+#include <array>
 #include <cmath>
 #include <csetjmp>
 #include <cstddef>
 #include <cstdio>
 #include <cstring>
 #include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -27,10 +29,27 @@
 #define TINYOBJLOADER_IMPLEMENTATION
 #endif
 
+#if defined(__clang__)
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wgnu-anonymous-struct"
+#pragma clang diagnostic ignored "-Wnested-anon-types"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#endif
 #include <MC.h>
+#if defined(__clang__)
+#pragma clang diagnostic pop
+#elif defined(__GNUC__)
+#pragma GCC diagnostic pop
+#endif
+
+#include <mujoco/mjmacro.h>
 #include <mujoco/mjmodel.h>
-#include "cc/array_safety.h"
+#include <mujoco/mjtnum.h>
+#include <mujoco/mjplugin.h>
 #include "engine/engine_crossplatform.h"
+#include "engine/engine_plugin.h"
 #include "engine/engine_resource.h"
 #include "engine/engine_util_blas.h"
 #include "engine/engine_util_errmem.h"
@@ -329,7 +348,7 @@ void mjCMesh::LoadSDF() {
 
 
 // compiler
-void mjCMesh::Compile(int vfs_provider) {
+void mjCMesh::Compile(const mjVFS* vfs) {
   // load file
   if (!file_.empty()) {
     // remove path from file if necessary
@@ -348,7 +367,7 @@ void mjCMesh::Compile(int vfs_provider) {
     }
 
     string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file_);
-    mjResource* resource = LoadResource(filename, vfs_provider);
+    mjResource* resource = LoadResource(filename, vfs);
 
     try {
       if (asset_type == "model/stl") {
@@ -1836,8 +1855,7 @@ mjCSkin::~mjCSkin() {
 
 
 // compiler
-void mjCSkin::Compile(int vfs_provider) {
+void mjCSkin::Compile(const mjVFS* vfs) {
-
   // load file
   if (!file.empty()) {
     // make sure data is not present
@@ -1865,7 +1883,7 @@ void mjCSkin::Compile(int vfs_provider) {
     }
 
     string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file);
-    mjResource* resource = LoadResource(filename, vfs_provider);
+    mjResource* resource = LoadResource(filename, vfs);
 
     try {
       LoadSKN(resource);

src/user/user_model.cc

@@ -2423,7 +2423,7 @@ static void warninghandler(const char* msg) {
 
 
 // compiler
-mjModel* mjCModel::Compile(int vfs_provider) {
+mjModel* mjCModel::Compile(const mjVFS* vfs) {
   // The volatile keyword is necessary to prevent a possible memory leak due to
   // an interaction between longjmp and compiler optimization. Specifically, at
   // the point where the setjmp takes places, these pointers have never been
@@ -2454,7 +2454,7 @@ mjModel* mjCModel::Compile(int vfs_provider) {
       // TryCompile resulted in an mju_error which was converted to a longjmp.
       throw mjCError(0, "engine error: %s", errortext);
     }
-    TryCompile(*const_cast<mjModel**>(&m), *const_cast<mjData**>(&data), vfs_provider);
+    TryCompile(*const_cast<mjModel**>(&m), *const_cast<mjData**>(&data), vfs);
   } catch (mjCError err) {
     // deallocate everything allocated in Compile
     mj_deleteModel(m);
@@ -2480,7 +2480,7 @@ mjModel* mjCModel::Compile(int vfs_provider) {
 }
 
 
-void mjCModel::TryCompile(mjModel*& m, mjData*& d, int vfs_provider) {
+void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
   // check if nan test works
   double test = mjNAN;
   if (mjuu_defined(test)) {
@@ -2557,7 +2557,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, int vfs_provider) {
 
   // compile meshes (needed for geom compilation)
   for (int i=0; i<meshes.size(); i++) {
-    meshes[i]->Compile(vfs_provider);
+    meshes[i]->Compile(vfs);
   }
 
   // automatically set nuser fields
@@ -2616,9 +2616,9 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, int vfs_provider) {
   }
 
   // compile all other objects except for keyframes
-  for (int i=0; i<skins.size(); i++) skins[i]->Compile(vfs_provider);
+  for (int i=0; i<skins.size(); i++) skins[i]->Compile(vfs);
-  for (int i=0; i<hfields.size(); i++) hfields[i]->Compile(vfs_provider);
+  for (int i=0; i<hfields.size(); i++) hfields[i]->Compile(vfs);
-  for (int i=0; i<textures.size(); i++) textures[i]->Compile(vfs_provider);
+  for (int i=0; i<textures.size(); i++) textures[i]->Compile(vfs);
   for (int i=0; i<materials.size(); i++) materials[i]->Compile();
   for (int i=0; i<pairs.size(); i++) pairs[i]->Compile();
   for (int i=0; i<excludes.size(); i++) excludes[i]->Compile();

src/user/user_model.h

@@ -65,7 +65,7 @@ class mjCModel {
   mjCModel();                                          // constructor
   ~mjCModel();                                         // destructor
 
-  mjModel*    Compile(int vfs_provider = 0);           // COMPILER: construct mjModel
+  mjModel*    Compile(const mjVFS* vfs = 0);           // COMPILER: construct mjModel
   bool        CopyBack(const mjModel*);                // DECOMPILER: copy numeric back
   void        FuseStatic(void);                        // fuse static bodies with parent
   void        FuseReindex(mjCBody* body);              // reindex elements during fuse
@@ -163,8 +163,8 @@ class mjCModel {
   int nuser_sensor;               // number of mjtNums in sensor_user
 
  private:
-  void TryCompile(mjModel*& m, mjData*& d, int vfs_provider);
+  void TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs);
-  mjModel* _Compile(int vfs_provider);
+  mjModel* _Compile(const mjVFS* vfs);
 
   void Clear(void);               // clear objects allocated by Compile
 

src/user/user_objects.cc

@@ -14,22 +14,22 @@
 
 #include "user/user_objects.h"
 
+#include <algorithm>
 #include <cmath>
 #include <cstddef>
 #include <cstdlib>
 #include <cstring>
-#include <iostream>
 #include <sstream>
 #include <string>
+#include <string_view>
 #include <vector>
 
 #include "lodepng.h"
 #include <mujoco/mjmacro.h>
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjplugin.h>
+#include <mujoco/mjtnum.h>
 #include "cc/array_safety.h"
-#include "engine/engine_core_smooth.h"
-#include "engine/engine_crossplatform.h"
 #include "engine/engine_resource.h"
 #include "engine/engine_io.h"
 #include "engine/engine_passive.h"
@@ -39,6 +39,7 @@
 #include "engine/engine_util_misc.h"
 #include "engine/engine_util_solve.h"
 #include "engine/engine_util_spatial.h"
+#include "engine/engine_vfs.h"
 #include "user/user_model.h"
 #include "user/user_util.h"
 
@@ -130,7 +131,7 @@ mjCError::mjCError(const mjCBase* obj, const char* msg, const char* str, int pos
 // constructor
 mjCAlternative::mjCAlternative() {
   axisangle[0] = xyaxes[0] = zaxis[0] = euler[0] = fullinertia[0] = mjNAN;
-};
+}
 
 
 // compute frame orientation given alternative specifications
@@ -495,17 +496,14 @@ mjCBase::mjCBase() {
 
 
 // load resource if found (fallback to OS filesystem)
-mjResource* mjCBase::LoadResource(string filename, int provider) {
+mjResource* mjCBase::LoadResource(string filename, const mjVFS* vfs) {
   mjResource* r = nullptr;
   const char* cname = filename.c_str();
 
-  // try reading from given provider
+  // try reading from provided VFS
-  if ((r = mju_openResource(cname, provider)) == nullptr) {
+  if ((r = mju_openVfsResource(cname, vfs)) == nullptr) {
-    if (!provider) {
+    // not in vfs try a provider or fallback to OS filesystem
-      throw mjCError(0, "file not found: '%s'", cname);
+    if ((r = mju_openResource(filename.c_str())) == nullptr) {
-    }
-    // if provider wasn't the OS filesystem try to fallback to OS filesystem
-    if ((r = mju_openResource(filename.c_str(), 0)) == nullptr) {
       throw mjCError(this, "resource not found via provider or OS filesystem: '%s'", cname);
     }
   }
@@ -1516,7 +1514,6 @@ void mjCGeom::SetFluidCoefs(void) {
 
   // get semiaxes
   switch (type) {
-
     case mjGEOM_SPHERE:
       dx = size[0];
       dy = size[0];
@@ -2194,7 +2191,7 @@ void mjCHField::LoadPNG(mjResource* resource) {
 
 
 // compiler
-void mjCHField::Compile(int vfs_provider) {
+void mjCHField::Compile(const mjVFS* vfs) {
   // check size parameters
   for (int i=0; i<4; i++)
     if (size[i]<=0)
@@ -2226,7 +2223,7 @@ void mjCHField::Compile(int vfs_provider) {
     }
 
     string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file);
-    mjResource* resource = LoadResource(filename, vfs_provider);
+    mjResource* resource = LoadResource(filename, vfs);
 
     try {
       if (asset_type == "image/png") {
@@ -2617,7 +2614,7 @@ void mjCTexture::LoadCustom(mjResource* resource,
 
 
 // load from PNG or custom file, flip if specified
-void mjCTexture::LoadFlip(string filename, int vfs_provider,
+void mjCTexture::LoadFlip(string filename, const mjVFS* vfs,
                           std::vector<unsigned char>& image,
                           unsigned int& w, unsigned int& h) {
   std::string asset_type = GetAssetContentType(filename, content_type);
@@ -2631,7 +2628,7 @@ void mjCTexture::LoadFlip(string filename, int vfs_provider,
     throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str());
   }
 
-  mjResource* resource = LoadResource(filename, vfs_provider);
+  mjResource* resource = LoadResource(filename, vfs);
 
   try {
     if (asset_type == "image/png") {
@@ -2693,11 +2690,11 @@ void mjCTexture::LoadFlip(string filename, int vfs_provider,
 
 
 // load 2D
-void mjCTexture::Load2D(string filename, int vfs_provider) {
+void mjCTexture::Load2D(string filename, const mjVFS* vfs) {
   // load PNG or custom
   unsigned int w, h;
   std::vector<unsigned char> image;
-  LoadFlip(filename, vfs_provider, image, w, h);
+  LoadFlip(filename, vfs, image, w, h);
 
   // assign size
   width = w;
@@ -2716,7 +2713,7 @@ void mjCTexture::Load2D(string filename, int vfs_provider) {
 
 
 // load cube or skybox from single file (repeated or grid)
-void mjCTexture::LoadCubeSingle(string filename, int vfs_provider) {
+void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
   // check gridsize
   if (gridsize[0]<1 || gridsize[1]<1 || gridsize[0]*gridsize[1]>12) {
     throw mjCError(this,
@@ -2727,7 +2724,7 @@ void mjCTexture::LoadCubeSingle(string filename, int vfs_provider) {
   // load PNG or custom
   unsigned int w, h;
   std::vector<unsigned char> image;
-  LoadFlip(filename, vfs_provider, image, w, h);
+  LoadFlip(filename, vfs, image, w, h);
 
   // check gridsize for compatibility
   if (w/gridsize[1]!=h/gridsize[0] || (w%gridsize[1]) || (h%gridsize[0])) {
@@ -2816,7 +2813,7 @@ void mjCTexture::LoadCubeSingle(string filename, int vfs_provider) {
 
 
 // load cube or skybox from separate file
-void mjCTexture::LoadCubeSeparate(int vfs_provider) {
+void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
   // keep track of which faces were defined
   int loaded[6] = {0, 0, 0, 0, 0, 0};
 
@@ -2834,7 +2831,7 @@ void mjCTexture::LoadCubeSeparate(int vfs_provider) {
       // load PNG or custom
       unsigned int w, h;
       std::vector<unsigned char> image;
-      LoadFlip(filename, vfs_provider, image, w, h);
+      LoadFlip(filename, vfs, image, w, h);
 
       // PNG must be square
       if (w!=h) {
@@ -2888,7 +2885,7 @@ void mjCTexture::LoadCubeSeparate(int vfs_provider) {
 
 
 // compiler
-void mjCTexture::Compile(int vfs_provider) {
+void mjCTexture::Compile(const mjVFS* vfs) {
   // builtin
   if (builtin!=mjBUILTIN_NONE) {
     // check size
@@ -2931,9 +2928,9 @@ void mjCTexture::Compile(int vfs_provider) {
 
     // dispatch
     if (type==mjTEXTURE_2D) {
-      Load2D(filename, vfs_provider);
+      Load2D(filename, vfs);
     } else {
-      LoadCubeSingle(filename, vfs_provider);
+      LoadCubeSingle(filename, vfs);
     }
   }
 
@@ -2961,7 +2958,7 @@ void mjCTexture::Compile(int vfs_provider) {
     }
 
     // only cube and skybox
-    LoadCubeSeparate(vfs_provider);
+    LoadCubeSeparate(vfs);
   }
 
   // make sure someone allocated data; SHOULD NOT OCCUR
@@ -4499,7 +4496,6 @@ mjCKey::~mjCKey() {
 
 // compiler
 void mjCKey::Compile(const mjModel* m) {
-
   // qpos: allocate or check size
   if (qpos.empty()) {
     qpos.resize(m->nq);
@@ -4574,7 +4570,6 @@ void mjCKey::Compile(const mjModel* m) {
   } else if (ctrl.size()!=m->nu) {
     throw mjCError(this, "key %d: invalid ctrl size, expected length %d", nullptr, id, m->nu);
   }
-
 }
 
 

src/user/user_objects.h

@@ -23,6 +23,7 @@
 
 #include "lodepng.h"
 #include <mujoco/mjmodel.h>
+#include <mujoco/mjplugin.h>
 
 // forward declarations of all mjC/X classes
 class mjCError;
@@ -173,7 +174,7 @@ class mjCBase {
 
  public:
   // load resource if found (fallback to OS filesystem)
-  mjResource* LoadResource(std::string filename, int provider);
+  mjResource* LoadResource(std::string filename, const mjVFS* vfs);
 
   // Get and sanitize content type from raw_text if not empty, otherwise parse
   // content type from resource_name; throw on failure
@@ -572,7 +573,7 @@ class mjCMesh: public mjCBase {
   void set_usertexcoord(std::optional<std::vector<float>>&& usertexcoord);
   void set_userface(std::optional<std::vector<int>>&& userface);
 
-  void Compile(int vfs_provider);                   // compiler
+  void Compile(const mjVFS* vfs);                   // compiler
   double* GetPosPtr(mjtMeshType type);              // get position
   double* GetQuatPtr(mjtMeshType type);             // get orientation
   double* GetInertiaBoxPtr(mjtMeshType type);       // get inertia box
@@ -696,7 +697,7 @@ class mjCSkin: public mjCBase {
  private:
   mjCSkin(mjCModel* = 0);                     // constructor
   ~mjCSkin();                                 // destructor
-  void Compile(int vfs_provider);             // compiler
+  void Compile(const mjVFS* vfs);             // compiler
   void LoadSKN(mjResource* resource);         // load skin in SKN BIN format
 
   int matid;                          // material id
@@ -723,7 +724,7 @@ class mjCHField : public mjCBase {
  private:
   mjCHField(mjCModel* model);             // constructor
   ~mjCHField();                           // destructor
-  void Compile(int vfs_provider);         // compiler
+  void Compile(const mjVFS* vfs);         // compiler
 
   void LoadCustom(mjResource* resource);  // load from custom format
   void LoadPNG(mjResource* resource);     // load from PNG format
@@ -768,15 +769,15 @@ class mjCTexture : public mjCBase {
  private:
   mjCTexture(mjCModel*);                  // constructor
   ~mjCTexture();                          // destructior
-  void Compile(int vfs_provider);         // compiler
+  void Compile(const mjVFS* vfs);         // compiler
 
   void Builtin2D(void);                   // make builtin 2D
   void BuiltinCube(void);                 // make builtin cube
-  void Load2D(std::string filename, int vfs_provider);         // load 2D from file
+  void Load2D(std::string filename, const mjVFS* vfs);         // load 2D from file
-  void LoadCubeSingle(std::string filename, int vfs_provider); // load cube from single file
+  void LoadCubeSingle(std::string filename, const mjVFS* vfs); // load cube from single file
-  void LoadCubeSeparate(int vfs_provider);                     // load cube from separate files
+  void LoadCubeSeparate(const mjVFS* vfs);                     // load cube from separate files
 
-  void LoadFlip(std::string filename, int vfs_provider,        // load and flip
+  void LoadFlip(std::string filename, const mjVFS* vfs,        // load and flip
                 std::vector<unsigned char>& image,
                 unsigned int& w, unsigned int& h);
 

src/xml/xml.cc

@@ -22,9 +22,11 @@
 
 #include <string>
 
+#include <mujoco/mjmodel.h>
 #include "cc/array_safety.h"
 #include "engine/engine_crossplatform.h"
 #include "engine/engine_resource.h"
+#include "engine/engine_vfs.h"
 #include "user/user_model.h"
 #include "user/user_util.h"
 #include "xml/xml_native_reader.h"
@@ -110,7 +112,7 @@ string mjWriteXML(mjCModel* model, char* error, int error_sz) {
 
 // find include elements recursively, replace them with subtree from xml file
 static XMLElement* mjIncludeXML(XMLElement* elem, string dir,
-                                int vfs_provider, vector<string>& included) {
+                                const mjVFS* vfs, vector<string>& included) {
   // include element: process
   if (!strcasecmp(elem->Value(), "include")) {
     // make sure include has no children
@@ -133,9 +135,9 @@ static XMLElement* mjIncludeXML(XMLElement* elem, string dir,
     // get data source
     mjResource *resource = nullptr;
     const char* xmlstring = nullptr;
-    if ((resource = mju_openResource(filename.c_str(), vfs_provider)) == nullptr) {
+    if ((resource = mju_openVfsResource(filename.c_str(), vfs)) == nullptr) {
-      // load from OS filesystem
+      // load from provider or OS filesystem
-      if (!vfs_provider || (resource = mju_openResource(filename.c_str(), 0)) == nullptr) {
+      if ((resource = mju_openResource(filename.c_str())) == nullptr) {
         throw mjXError(elem, "Could not open file '%s'", filename.c_str());
       }
     }
@@ -196,14 +198,14 @@ static XMLElement* mjIncludeXML(XMLElement* elem, string dir,
     }
 
     // run XMLInclude on first new child
-    return mjIncludeXML(first->ToElement(), dir, vfs_provider, included);
+    return mjIncludeXML(first->ToElement(), dir, vfs, included);
   }
 
   // otherwise check all child elements, return self
   else {
     XMLElement* child = elem->FirstChildElement();
     while (child) {
-      child = mjIncludeXML(child, dir, vfs_provider, included);
+      child = mjIncludeXML(child, dir, vfs, included);
       if (child) {
         child = child->NextSiblingElement();
       }
@@ -215,7 +217,7 @@ static XMLElement* mjIncludeXML(XMLElement* elem, string dir,
 
 
 // Main parser function
-mjCModel* mjParseXML(const char* filename, int vfs_provider, char* error, int error_sz) {
+mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz) {
   LocaleOverride locale_override;
 
   // check arguments
@@ -235,9 +237,9 @@ mjCModel* mjParseXML(const char* filename, int vfs_provider, char* error, int er
   // get data source
   mjResource* resource = nullptr;
   const char* xmlstring = nullptr;
-  if ((resource = mju_openResource(filename, vfs_provider)) == nullptr) {
+  if ((resource = mju_openVfsResource(filename, vfs)) == nullptr) {
-    // load from OS filesystem
+    // load from provider or fallback to OS filesystem
-    if (!vfs_provider || (resource = mju_openResource(filename, 0)) == nullptr) {
+    if ((resource = mju_openResource(filename)) == nullptr) {
       if (error) {
         snprintf(error, error_sz, "mjParseXML: could not open file '%s'", filename);
       }
@@ -303,7 +305,7 @@ mjCModel* mjParseXML(const char* filename, int vfs_provider, char* error, int er
       // find include elements, replace them with subtree from xml file
       vector<string> included;
       included.push_back(filename);
-      mjIncludeXML(root, model->modelfiledir, vfs_provider, included);
+      mjIncludeXML(root, model->modelfiledir, vfs, included);
 
       // parse MuJoCo model
       mjXReader parser;

src/xml/xml.h

@@ -26,7 +26,7 @@
 std::string mjWriteXML(mjCModel* model, char* error, int error_sz);
 
 // Main parser function
-mjCModel* mjParseXML(const char* filename, int vfs_provider, char* error, int error_sz);
+mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz);
 
 
 #endif  // MUJOCO_SRC_XML_XML_H_

src/xml/xml_api.cc

@@ -70,20 +70,23 @@ static std::mutex themutex;
 
 //---------------------------------- Functions -----------------------------------------------------
 
-// mj_loadXML helper function
+// parse XML file in MJCF or URDF format, compile it, return low-level model
-mjModel* _loadXML(const char* filename, int vfs_provider,
+//  if vfs is not NULL, look up files in vfs before reading from disk
-                  char* error, int error_sz) {
+//  error can be NULL; otherwise assumed to have size error_sz
+mjModel* mj_loadXML(const char* filename, const mjVFS* vfs,
+                    char* error, int error_sz) {
+
   // serialize access to themodel
   std::lock_guard<std::mutex> lock(themutex);
 
   // parse new model
-  mjCModel* newmodel = mjParseXML(filename, vfs_provider, error, error_sz);
+  mjCModel* newmodel = mjParseXML(filename, vfs, error, error_sz);
   if (!newmodel) {
     return nullptr;
   }
 
   // compile new model
-  mjModel* m = newmodel->Compile(vfs_provider);
+  mjModel* m = newmodel->Compile(vfs);
   if (!m) {
     mjCopyError(error, newmodel->GetError().message, error_sz);
     delete newmodel;
@@ -106,31 +109,6 @@ mjModel* _loadXML(const char* filename, int vfs_provider,
 
 
 
-// parse XML file in MJCF or URDF format, compile it, return low-level model
-//  if vfs is not NULL, look up files in vfs before reading from disk
-//  error can be NULL; otherwise assumed to have size error_sz
-mjModel* mj_loadXML(const char* filename, const mjVFS* vfs,
-                    char* error, int error_sz) {
-
-  if (vfs == nullptr) {
-    return _loadXML(filename, 0, error, error_sz);
-  }
-
-  int index = mj_registerVfsProvider(vfs);
-  if (index < 1) {
-    if (error) {
-      snprintf(error, error_sz, "mj_loadXML: could not register VFS");
-    }
-    return nullptr;
-  }
-
-  mjModel* model = _loadXML(filename, index, error, error_sz);
-  mjp_unregisterResourceProvider(index);
-  return model;
-}
-
-
-
 // update XML data structures with info from low-level model, save as MJCF
 //  returns 1 if successful, 0 otherwise
 //  error can be NULL; otherwise assumed to have size error_sz

src/xml/xml_native_reader.cc

@@ -95,12 +95,12 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
             "inttotal", "interval", "tolrange"},
     {">"},
 
-    {"option", "*", "24",
+    {"option", "*", "26",
-        "timestep", "apirate", "impratio", "tolerance", "noslip_tolerance", "mpr_tolerance",
+        "timestep", "apirate", "impratio", "tolerance", "ls_tolerance", "noslip_tolerance",
-        "gravity", "wind", "magnetic", "density", "viscosity",
+        "mpr_tolerance", "gravity", "wind", "magnetic", "density", "viscosity",
         "o_margin", "o_solref", "o_solimp",
         "integrator", "collision", "cone", "jacobian",
-        "solver", "iterations", "noslip_iterations", "mpr_iterations",
+        "solver", "iterations", "ls_iterations", "noslip_iterations", "mpr_iterations",
         "sdf_iterations", "sdf_initpoints"},
     {"<"},
         {"flag", "?", "22", "constraint", "equality", "frictionloss", "limit", "contact",
@@ -270,8 +270,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
         {"light", "*", "15", "name", "class", "directional", "castshadow", "active",
             "pos", "dir", "attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular",
             "mode", "target"},
-        {"composite", "*", "12", "prefix", "type", "count", "spacing", "offset",
+        {"composite", "*", "13", "prefix", "type", "count", "spacing", "offset",
-            "flatinertia", "solrefsmooth", "solimpsmooth", "vertex",
+            "flatinertia", "solrefsmooth", "solimpsmooth", "vertex", "face",
             "initial", "curve", "size"},
         {"<"},
             {"plugin", "*", "2", "plugin", "instance"},
@@ -958,6 +958,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
   ReadAttr(section, "apirate", 1, &opt->apirate, text);
   ReadAttr(section, "impratio", 1, &opt->impratio, text);
   ReadAttr(section, "tolerance", 1, &opt->tolerance, text);
+  ReadAttr(section, "ls_tolerance", 1, &opt->ls_tolerance, text);
   ReadAttr(section, "noslip_tolerance", 1, &opt->noslip_tolerance, text);
   ReadAttr(section, "mpr_tolerance", 1, &opt->mpr_tolerance, text);
   ReadAttr(section, "gravity", 3, opt->gravity, text);
@@ -976,6 +977,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
   MapValue(section, "jacobian", &opt->jacobian, jac_map, jac_sz);
   MapValue(section, "solver", &opt->solver, solver_map, solver_sz);
   ReadAttrInt(section, "iterations", &opt->iterations);
+  ReadAttrInt(section, "ls_iterations", &opt->ls_iterations);
   ReadAttrInt(section, "noslip_iterations", &opt->noslip_iterations);
   ReadAttrInt(section, "mpr_iterations", &opt->mpr_iterations);
   ReadAttrInt(section, "sdf_iterations", &opt->sdf_iterations);
@@ -1943,6 +1945,9 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
     String2Vector(text, comp.uservert);
   }
 
+  // shell
+  ReadAttrTxt(elem, "face", comp.userface);
+
   // process curve string
   std::istringstream iss(curves);
   int i = 0;

src/xml/xml_native_writer.cc

@@ -793,6 +793,7 @@ void mjXWriter::Option(XMLElement* root) {
   WriteAttr(section, "apirate", 1, &model->option.apirate, &opt.apirate);
   WriteAttr(section, "impratio", 1, &model->option.impratio, &opt.impratio);
   WriteAttr(section, "tolerance", 1, &model->option.tolerance, &opt.tolerance);
+  WriteAttr(section, "ls_tolerance", 1, &model->option.ls_tolerance, &opt.ls_tolerance);
   WriteAttr(section, "noslip_tolerance", 1, &model->option.noslip_tolerance, &opt.noslip_tolerance);
   WriteAttr(section, "mpr_tolerance", 1, &model->option.mpr_tolerance, &opt.mpr_tolerance);
   WriteAttr(section, "gravity", 3, model->option.gravity, opt.gravity);
@@ -816,6 +817,7 @@ void mjXWriter::Option(XMLElement* root) {
   WriteAttrKey(section, "solver", solver_map, solver_sz,
                model->option.solver, opt.solver);
   WriteAttrInt(section, "iterations", model->option.iterations, opt.iterations);
+  WriteAttrInt(section, "ls_iterations", model->option.ls_iterations, opt.ls_iterations);
   WriteAttrInt(section, "noslip_iterations", model->option.noslip_iterations, opt.noslip_iterations);
   WriteAttrInt(section, "mpr_iterations", model->option.mpr_iterations, opt.mpr_iterations);
   WriteAttrInt(section, "sdf_iterations", model->option.sdf_iterations, opt.sdf_iterations);