Merge branch 'google-deepmind:main' into tendons

CMakeLists.txt

@@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON)
 
 project(
   mujoco
-  VERSION 3.1.5
+  VERSION 3.1.6
   DESCRIPTION "MuJoCo Physics Simulator"
   HOMEPAGE_URL "https://mujoco.org"
 )

README.md

@@ -50,16 +50,17 @@ your machine.
 If you are a Python user, you might want to start with our tutorial notebooks
 running on Google Colab:
 
- - The **introductory tutorial** teaches MuJoCo basics:
+ - The **introductory** tutorial teaches MuJoCo basics:
    [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/tutorial.ipynb)
  - The **LQR** tutorial synthesizes a linear-quadratic controller, balancing a humanoid on one leg:
    [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/LQR.ipynb)
  - The **least-squares** tutorial explains how to use the Python-based nonlinear least-squares solver:
    [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/least_squares.ipynb)
  - The **MJX** tutorial provides usage examples of
-   [MuJoCo XLA](https://mujoco.readthedocs.io/en/stable/mjx.html), a branch of MuJoCo written in
+   [MuJoCo XLA](https://mujoco.readthedocs.io/en/stable/mjx.html), a branch of MuJoCo written in JAX:
-   JAX:
    [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/mjx/tutorial.ipynb)
+ - The **differentiable physics** tutorial trains locomotion policies with analytical gradients automatically derived from MuJoCo's physics step:
+   [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/mjx/training_apg.ipynb)
 
 ## Installation
 

cmake/MujocoDependencies.cmake

@@ -27,7 +27,7 @@ set(MUJOCO_DEP_VERSION_tinyobjloader
     CACHE STRING "Version of `tinyobjloader` to be fetched."
 )
 set(MUJOCO_DEP_VERSION_MarchingCubeCpp
-    5b79e5d6bded086a0abe276a4b5a69fc17ae9bf1
+    f03a1b3ec29b1d7d865691ca8aea4f1eb2c2873d
     CACHE STRING "Version of `MarchingCubeCpp` to be fetched."
 )
 set(MUJOCO_DEP_VERSION_ccd
@@ -44,7 +44,7 @@ set(MUJOCO_DEP_VERSION_Eigen3
 )
 
 set(MUJOCO_DEP_VERSION_abseil
-    2f9e432cce407ce0ae50676696666f33a77d42ac # LTS 20240116.1
+    d7aaad83b488fd62bd51c81ecf16cd938532cc0a # LTS 20240116.2
     CACHE STRING "Version of `abseil` to be fetched."
 )
 

dist/mujoco.rc

@@ -1,6 +1,6 @@
 1 VERSIONINFO
-FILEVERSION 3,1,5,0
+FILEVERSION 3,1,6,0
-PRODUCTVERSION 3,1,5,0
+PRODUCTVERSION 3,1,6,0
 FILEOS 0x4
 FILETYPE 0x1
 {
@@ -9,9 +9,9 @@ FILETYPE 0x1
     BLOCK "040904b0"
     {
       VALUE "ProductName", "MuJoCo"
-      VALUE "ProductVersion", "3.1.5"
+      VALUE "ProductVersion", "3.1.6"
       VALUE "FileDescription", "MuJoCo"
-      VALUE "FileVersion", "3.1.5"
+      VALUE "FileVersion", "3.1.6"
       VALUE "InternalName", "mujoco.dll"
       VALUE "OriginalFilename", "mujoco.dll"
       VALUE "CompanyName", "Google DeepMind"

dist/simulate.rc

@@ -1,8 +1,8 @@
 MUJOCO ICON "mujoco.ico"
 
 1 VERSIONINFO
-FILEVERSION 3,1,5,0
+FILEVERSION 3,1,6,0
-PRODUCTVERSION 3,1,5,0
+PRODUCTVERSION 3,1,6,0
 FILEOS 0x4
 FILETYPE 0x1
 {
@@ -11,9 +11,9 @@ FILETYPE 0x1
     BLOCK "040904b0"
     {
       VALUE "ProductName", "MuJoCo"
-      VALUE "ProductVersion", "3.1.5"
+      VALUE "ProductVersion", "3.1.6"
       VALUE "FileDescription", "MuJoCo"
-      VALUE "FileVersion", "3.1.5"
+      VALUE "FileVersion", "3.1.6"
       VALUE "InternalName", "simulate.exe"
       VALUE "OriginalFilename", "simulate.exe"
       VALUE "CompanyName", "Google DeepMind"

doc/APIreference/APIglobals.rst

@@ -522,7 +522,7 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr
      - Maximum number of UI rectangles.
        Defined in `mjui.h <https://github.com/google-deepmind/mujoco/blob/main/include/mujoco/mjui.h>`_.
    * - ``mjVERSION_HEADER``
-     - 315
+     - 316
      - The version of the MuJoCo headers; changes with every release. This is an integer equal to 100x the software
        version, so 210 corresponds to version 2.1. Defined in  mujoco.h. The API function :ref:`mj_version` returns a
        number with the same meaning but for the compiled library.

doc/APIreference/functions.rst

@@ -57,27 +57,27 @@ Main simulation
 
 These are the main entry points to the simulator. Most users will only need to call :ref:`mj_step`, which computes
 everything and advanced the simulation state by one time step. Controls and applied forces must either be set in advance
-(in mjData.{ctrl, qfrc_applied, xfrc_applied}), or a control callback :ref:`mjcb_control` must be installed which will be
+(in ``mjData.{ctrl, qfrc_applied, xfrc_applied}``), or a control callback :ref:`mjcb_control` must be installed which
-called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and
+will be called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and
 :ref:`mj_step2` which break down the simulation pipeline into computations that are executed before and after the
 controls are needed; in this way one can set controls that depend on the results from :ref:`mj_step1`. Keep in mind
-though that the RK4 solver does not work with mj_step1/2.
+though that the RK4 solver does not work with mj_step1/2. See :ref:`Pipeline` for a more detailed description.
 
 mj_forward performs the same computations as :ref:`mj_step` but without the integration. It is useful after loading or
 resetting a model (to put the entire mjData in a valid state), and also for out-of-order computations that involve
 sampling or finite-difference approximations.
 
-mj_inverse runs the inverse dynamics, and writes its output in ``mjData.qfrc_inverse``. Note that ``mjData.qacc`` must
+:ref:`mj_inverse` runs the inverse dynamics, and writes its output in ``mjData.qfrc_inverse``. Note that ``mjData.qacc``
-be set before calling this function. Given the state (qpos, qvel, act), mj_forward maps from force to acceleration,
+must be set before calling this function. Given the state (qpos, qvel, act), mj_forward maps from force to acceleration,
 while mj_inverse maps from acceleration to force. Mathematically these functions are inverse of each other, but
 numerically this may not always be the case because the forward dynamics rely on a constraint optimization algorithm
 which is usually terminated early. The difference between the results of forward and inverse dynamics can be computed
-with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a general
+with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a
-sanity check).
+general sanity check).
 
 The skip version of :ref:`mj_forward` and :ref:`mj_inverse` are useful for example when qpos was unchanged but qvel was
 changed (usually in the context of finite differencing). Then there is no point repeating the computations that only
-depend on qpos. Calling the dynamics with skipstage = mjSTAGE_POS will achieve these savings.
+depend on qpos. Calling the dynamics with skipstage = :ref:`mjSTAGE_POS<mjtStage>` will achieve these savings.
 
 .. _mj_step:
 
@@ -249,8 +249,8 @@ degrees-of-freedom and a given point. Given a body specified by its integer id (
 frame (``point``) treated as attached to the body, the Jacobian has both translational (``jacp``) and rotational
 (``jacr``) components. Passing ``NULL`` for either pointer will skip that part of the computation. Each component is a
 3-by-nv matrix. Each row of this matrix is the gradient of the corresponding coordinate of the specified point with
-respect to the degrees-of-freedom. The ability to compute end-effector Jacobians efficiently and analytically is one of
+respect to the degrees-of-freedom. The :ref:`pipeline stages<piStages>` required for Jacobian computations to be
-the advantages of working in minimal coordinates.
+consistenst with the current generalized positions ``mjData.qpos`` are :ref:`mj_kinematics` and :ref:`mj_comPos`.
 
 .. _mj_jacBody:
 
@@ -408,6 +408,28 @@ Compute object 6D acceleration (rot:lin) in object-centered frame, world/local o
 sensors are not present in the model, :ref:`mj_rnePostConstraint` must be manually called in order to calculate
 mjData.cacc -- the total body acceleration, including contributions from the constraint solver.
 
+.. _mj_geomDistance:
+
+mj_geomDistance
+~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mj_geomDistance
+
+Returns the smallest signed distance between two geoms and optionally the segment from ``geom1`` to ``geom2``.
+Returned distances are bounded from above by ``distmax``. |br| If no collision of distance smaller than ``distmax`` is
+found, the function will return ``distmax`` and ``fromto``, if given, will be set to (0, 0, 0, 0, 0, 0).
+
+.. admonition:: Positive ``distmax`` values
+   :class: note
+
+   .. TODO: b/339596989 - Improve mjc_Convex.
+
+   For some colliders, a large, positive ``distmax`` will result in an accurate measurement. However, for collision
+   pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate.
+   This is considered a bug to be fixed in a future release.
+   In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of
+   `engine_collision_driver.c <https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c>`__.
+
 .. _mj_contactForce:
 
 mj_contactForce

doc/APIreference/functions_override.rst

@@ -33,27 +33,27 @@ The model and all files referenced in it can be loaded from disk or from a VFS w
 
 These are the main entry points to the simulator. Most users will only need to call :ref:`mj_step`, which computes
 everything and advanced the simulation state by one time step. Controls and applied forces must either be set in advance
-(in mjData.{ctrl, qfrc_applied, xfrc_applied}), or a control callback :ref:`mjcb_control` must be installed which will be
+(in ``mjData.{ctrl, qfrc_applied, xfrc_applied}``), or a control callback :ref:`mjcb_control` must be installed which
-called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and
+will be called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and
 :ref:`mj_step2` which break down the simulation pipeline into computations that are executed before and after the
 controls are needed; in this way one can set controls that depend on the results from :ref:`mj_step1`. Keep in mind
-though that the RK4 solver does not work with mj_step1/2.
+though that the RK4 solver does not work with mj_step1/2. See :ref:`Pipeline` for a more detailed description.
 
 mj_forward performs the same computations as :ref:`mj_step` but without the integration. It is useful after loading or
 resetting a model (to put the entire mjData in a valid state), and also for out-of-order computations that involve
 sampling or finite-difference approximations.
 
-mj_inverse runs the inverse dynamics, and writes its output in ``mjData.qfrc_inverse``. Note that ``mjData.qacc`` must
+:ref:`mj_inverse` runs the inverse dynamics, and writes its output in ``mjData.qfrc_inverse``. Note that ``mjData.qacc``
-be set before calling this function. Given the state (qpos, qvel, act), mj_forward maps from force to acceleration,
+must be set before calling this function. Given the state (qpos, qvel, act), mj_forward maps from force to acceleration,
 while mj_inverse maps from acceleration to force. Mathematically these functions are inverse of each other, but
 numerically this may not always be the case because the forward dynamics rely on a constraint optimization algorithm
 which is usually terminated early. The difference between the results of forward and inverse dynamics can be computed
-with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a general
+with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a
-sanity check).
+general sanity check).
 
 The skip version of :ref:`mj_forward` and :ref:`mj_inverse` are useful for example when qpos was unchanged but qvel was
 changed (usually in the context of finite differencing). Then there is no point repeating the computations that only
-depend on qpos. Calling the dynamics with skipstage = mjSTAGE_POS will achieve these savings.
+depend on qpos. Calling the dynamics with skipstage = :ref:`mjSTAGE_POS<mjtStage>` will achieve these savings.
 
 .. _Initialization:
 
@@ -165,8 +165,8 @@ degrees-of-freedom and a given point. Given a body specified by its integer id (
 frame (``point``) treated as attached to the body, the Jacobian has both translational (``jacp``) and rotational
 (``jacr``) components. Passing ``NULL`` for either pointer will skip that part of the computation. Each component is a
 3-by-nv matrix. Each row of this matrix is the gradient of the corresponding coordinate of the specified point with
-respect to the degrees-of-freedom. The ability to compute end-effector Jacobians efficiently and analytically is one of
+respect to the degrees-of-freedom. The :ref:`pipeline stages<piStages>` required for Jacobian computations to be
-the advantages of working in minimal coordinates.
+consistenst with the current generalized positions ``mjData.qpos`` are :ref:`mj_kinematics` and :ref:`mj_comPos`.
 
 .. _mj_jacBody:
 
@@ -180,6 +180,23 @@ generalized velocities to subtree angular momentum. More precisely if :math:`h`
 body index ``body`` in ``mjData.subtree_angmom`` (reported by the :ref:`subtreeangmom<sensor-subtreeangmom>` sensor)
 and :math:`\dot q` is the generalized velocity ``mjData.qvel``, then :math:`h = H \dot q`.
 
+.. _mj_geomDistance:
+
+Returns the smallest signed distance between two geoms and optionally the segment from ``geom1`` to ``geom2``.
+Returned distances are bounded from above by ``distmax``. |br| If no collision of distance smaller than ``distmax`` is
+found, the function will return ``distmax`` and ``fromto``, if given, will be set to (0, 0, 0, 0, 0, 0).
+
+.. admonition:: Positive ``distmax`` values
+   :class: note
+
+   .. TODO: b/339596989 - Improve mjc_Convex.
+
+   For some colliders, a large, positive ``distmax`` will result in an accurate measurement. However, for collision
+   pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate.
+   This is considered a bug to be fixed in a future release.
+   In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of
+   `engine_collision_driver.c <https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c>`__.
+
 .. _mj_mulM:
 
 This function multiplies the joint-space inertia matrix stored in mjData.qM by a vector. qM has a custom sparse format

doc/XMLreference.rst

@@ -214,6 +214,10 @@ and namespaced appropriately. Detailed examples of models using replicate can be
             </replicate>
           </replicate>
         </worldbody>
+
+        <sensor>
+          <accelerometer name="Bob" site="Alice"/>
+        </sensor>
       </mujoco>
 
    Results in this model:
@@ -227,6 +231,13 @@ and namespaced appropriately. Detailed examples of models using replicate can be
           <geom name="Alice-01" size="0.1" pos="0 1 0" quat="1 1 0 0"/>
           <geom name="Alice-11" size="0.1" pos="1 1 0" quat="0.5 0.5 0.5 0.5"/>
         </worldbody>
+
+        <sensor>
+          <accelerometer name="Bob-00" site="Alice-00"/>
+          <accelerometer name="Bob-10" site="Alice-10"/>
+          <accelerometer name="Bob-01" site="Alice-01"/>
+          <accelerometer name="Bob-11" site="Alice-11"/>
+        </sensor>
       </mujoco>
 
 .. _include:
@@ -301,8 +312,8 @@ adjust it properly through the XML.
    setting of solimp determines a single impedance value for all contact dimensions, which is then modulated by this
    attribute. Settings larger than 1 cause friction forces to be "harder" than normal forces, having the general effect
    of preventing slip, without increasing the actual friction coefficient. For pyramidal friction cones the situation is
-   more complex because the pyramidal approximation mixes normal and frictional dimensions within each basis vector; but
+   more complex because the pyramidal approximation mixes normal and frictional dimensions within each basis vector; it
-   the overall effect of this attribute is qualitatively similar.
+   is not recommended to use high impratio values with pyramidal cones.
 
 .. _option-gravity:
 
@@ -1088,6 +1099,8 @@ specified with OBJ files and MSH files, as well as explicitly in the XML with th
 STL files. These mechanism cannot be mixed. So if you have an STL mesh, the only way to add texture coordinates to it is
 to convert to one of the other supported formats.
 
+.. _legacy-msh-docs:
+
 MSH file format
    The binary MSH file starts with 4 integers specifying the number of vertex positions (nvertex), vertex normals
    (nnormal), vertex texture coordinates (ntexcoord), and vertex indices making up the faces (nface), followed by the
@@ -2274,7 +2287,8 @@ helps clarify the role of bodies and geoms in MuJoCo.
    along both axes of the tangent plane. The second number is the torsional friction, acting around the contact normal.
    The third number is the rolling friction, acting around both axes of the tangent plane. The friction parameters for
    the contact pair are combined depending on the solmix and priority attributes, as explained in :ref:`Contact
-   parameters <CContact>`.
+   parameters <CContact>`. See the general :ref:`Contact<coContact>` section for descriptions of the semantics of this
+   attribute.
 
 .. _body-geom-mass:
 
@@ -4261,11 +4275,15 @@ joint types (slide and hinge) can be used.
 .. _equality-joint-polycoef:
 
 :at:`polycoef`: :at-val:`real(5), "0 1 0 0 0"`
-   Coefficients a0 ... a4 of the quartic polynomial. If the two joint values are y and x, and their reference positions
+   Coefficients :math:`a_0 \ldots a_4` of the quartic polynomial. If the joint values of :at:`joint1` and :at:`joint2`
-   (corresponding to the joint values in the initial model configuration) are y0 and x0, the constraint is:
+   are respectively :math:`y` and :math:`x`, and their reference positions (corresponding to the joint values in the
-   y-y0 = a0 + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4.
+   initial model configuration) are :math:`y_0` and :math:`x_0`, the constraint is:
-   Omitting the second joint is equivalent to setting x = x0, in which case the constraint is y = y0 + a0.
 
+   .. math::
+      y-y_0 = a_0 + a_1(x-x_0) + a_2(x-x_0)^2 + a_3(x-x_0)^3 + a_4(x-x_0)^4
+
+   Omitting :at:`joint2` is equivalent to setting :math:`x = x_0`, in which case the constraint is
+   :math:`y = y_0 + a_0`.
 
 .. _equality-tendon:
 
@@ -4301,7 +4319,7 @@ This element constrains the length of one tendon to be a quartic polynomial of a
 .. _equality-tendon-polycoef:
 
 :at:`polycoef`: :at-val:`real(5), "0 1 0 0 0"`
-   Same as in the equality/ :ref:`joint <equality-joint>` element above, but applied to tendon lengths instead of joint
+   Same as in the :ref:`equality/joint <equality-joint>` element above, but applied to tendon lengths instead of joint
    positions.
 
 
@@ -4958,15 +4976,16 @@ This element does not have custom attributes. It only has common attributes, whi
 :el-prefix:`actuator/` |-| **position** (*)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-This element creates a position servo. The underlying :el:`general` attributes are set as follows:
+This element creates a position servo with an optional first-order filter. The underlying :el:`general` attributes are
+set as follows:
 
-========= ======= ========= =========
+========= =================== ========= =============
-Attribute Setting Attribute Setting
+Attribute Setting             Attribute Setting
-========= ======= ========= =========
+========= =================== ========= =============
-dyntype   none    dynprm    1 0 0
+dyntype   none or filterexact dynprm    timeconst 0 0
-gaintype  fixed   gainprm   kp 0 0
+gaintype  fixed               gainprm   kp 0 0
-biastype  affine  biasprm   0 -kp -kv
+biastype  affine              biasprm   0 -kp -kv
-========= ======= ========= =========
+========= =================== ========= =============
 
 
 This element has one custom attribute in addition to the common attributes:
@@ -5026,6 +5045,29 @@ This element has one custom attribute in addition to the common attributes:
    Damping applied by the actuator.
    When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
 
+.. _actuator-position-dampratio:
+
+:at:`dampratio`: :at-val:`real, "0"`
+   Damping applied by the actuator, using damping ratio units.
+   This attribute is exclusive with :at:`kv` and has similar meaning, but instead of units of force/velocity, the units
+   are :math:`2 \sqrt{k_p \cdot m}`, corresponding to a harmonic oscillator's
+   `damping ratio <https://en.wikipedia.org/wiki/Damping#Damping_ratio_definition>`__.
+   A value of 1 corresponds to a *critically damped* oscillator, which often produces desirable behavior.
+   Values smaller or larger than 1 correspond to underdamped and overdamped oscillations, respectively.
+   The mass :math:`m` is computed at the reference configuration ``mjModel.qpos0``, taking into account joint
+   :ref:`armature <body-joint-armature>`.
+   However, passive :ref:`damping <body-joint-damping>` or :ref:`frictionloss <body-joint-frictionloss>` in the affected
+   joints are not taken into account; if they are non-negligible, :at:`dampratio` values smaller than 1 might be
+   required to achieve desirable motion.
+   When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
+
+.. _actuator-position-timeconst:
+
+:at:`timeconst`: :at-val:`real, "0"`
+   Time-constant of optional first-order filter.  If larger than zero, the actuator uses the :at:`filterexact`
+   :ref:`dynamics type<actuator-general-dyntype>`, if zero (the default) no filter is used.
+
+
 .. _actuator-position-inheritrange:
 
 :at:`inheritrange`: :at-val:`real, "0"`
@@ -5188,6 +5230,11 @@ This element has one custom attribute in addition to the common attributes:
    Damping applied by the actuator.
    When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
 
+.. _actuator-intvelocity-dampratio:
+
+:at:`dampratio`: :at-val:`real, "0"`
+   See :ref:`position/dampratio<actuator-position-dampratio>`.
+
 .. _actuator-intvelocity-inheritrange:
 
 :at:`inheritrange`: :at-val:`real, "0"`
@@ -6720,6 +6767,186 @@ The presence of this sensor in a model triggers a call to :ref:`mj_subtreeVel` d
 :at:`body`: :at-val:`string, required`
    Name of the body where the kinematic subtree is rooted.
 
+.. _collision-sensors:
+
+collision sensors
+^^^^^^^^^^^^^^^^^
+
+The following 3 sensor types, :ref:`sensor/distance<sensor-distance>`, :ref:`sensor/normal<sensor-normal>` and
+:ref:`sensor/fromto<sensor-fromto>`, respectively measure the distance, normal direction and line segment of the
+smallest signed distance between the surfaces of two geoms using the narrow-phase geom-geom colliders. The collision
+computation is always performed, independently of the standard collision :ref:`selection and filtering<coSelection>`
+pipeline. These 3 sensors share some common properties:
+
+.. _collision-sensors-cutoff:
+
+:at:`cutoff`
+   For most sensors, the :at:`cutoff` attribute simply defines a clipping operation on sensor values. For collision
+   sensors, it defines the maximum distance at which collisions will be detected, corresponding to the ``dismax``
+   argument of :ref:`mj_geomDistance`. For example, at the default value of 0, only negative distances (corresponding
+   to geom-geom penetration) will be reported by :ref:`sensor/distance<sensor-distance>`.
+   In order to determine collision properties of non-penetrating geom pairs, a positive :at:`cutoff` is required.
+
+   .. admonition:: Positive cutoff values
+      :class: note
+
+      .. TODO: b/339596989 - Improve mjc_Convex.
+
+      For some colliders, a positive :at:`cutoff` will result in an accurate measurement. However, for collision
+      pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate.
+      This is considered a bug to be fixed in a future release.
+      In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of
+      `engine_collision_driver.c <https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c>`__.
+
+:at:`geom1`, :at:`geom2`, :at:`body1`, :at:`body2`
+   For all 3 collision sensor types, the two colliding geoms can be specified explicitly using the :at:`geom1` and
+   :at:`geom2` attributes or implicitly, using :at:`body1`, :at:`body2`. In the latter case the sensor will iterate over
+   all geoms of the specified body or bodies (mixed specification like :at:`geom1`, :at:`body2` are allowed), and
+   select the collision with the smallest signed distance.
+
+sequential sensors
+   When multiple collision sensors are defined sequentially and have identical attributes (:at:`geom1`, :at:`body1`,
+   :at:`geom2`, :at:`body2`, :at:`cutoff`), for example when both distance and normal are queried for the same geom
+   pair, the collision functions will be called once for the whole sensor block, avoiding repeated computation.
+
+.. _sensor-distance:
+
+:el-prefix:`sensor/` |-| **distance** (*)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This element creates a sensor that returns the smallest signed distance between the surfaces of two geoms.
+See :ref:`collision-sensors` for more details about sensors of this type.
+
+.. _sensor-distance-cutoff:
+
+:at:`cutoff`
+   See :ref:`collision-sensors` for the sematics of this attribute, which is different than for other sensor categories.
+   If no collision is detected, the distance sensor returns the :at:`cutoff` value, so in this case
+   :at:`cutoff` acts as a maximum clipping value, in addition to the special semantics.
+
+.. _sensor-distance-geom1:
+
+:at:`geom1`: :at-val:`string, optional`
+   Name of the first geom. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
+
+.. _sensor-distance-geom2:
+
+:at:`geom2`: :at-val:`string, optional`
+   Name of the second geom. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
+
+.. _sensor-distance-body1:
+
+:at:`body1`: :at-val:`string, optional`
+   Name of the first body. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
+
+.. _sensor-distance-body2:
+
+:at:`body2`: :at-val:`string, optional`
+   Name of the second body. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
+
+.. _sensor-distance-name:
+
+.. _sensor-distance-noise:
+
+.. _sensor-distance-user:
+
+:at:`name`, :at:`noise`, :at:`user`
+   See :ref:`CSensor`.
+
+
+.. _sensor-normal:
+
+:el-prefix:`sensor/` |-| **normal** (*)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This element creates a sensor that returns the normal direction of the smallest signed distance between the surfaces of
+two geoms. It is guaranteed to point from the surface of geom1 to the surface of geom2, though note that in the case of
+penetration, this direction is generally in the opposite direction to that of the centroids.
+See :ref:`collision-sensors` for more details about sensors of this type.
+
+.. _sensor-normal-cutoff:
+
+:at:`cutoff`
+   See :ref:`collision-sensors` for the sematics of this attribute, which is different than for other sensor categories.
+   If no collision is detected, the :ref:`normal<sensor-normal>` sensor returns (0, 0, 0), otherwise it returns a
+   normalized direction vector. For this sensor, :at:`cutoff` does not lead to any clamping.
+
+.. _sensor-normal-geom1:
+
+:at:`geom1`: :at-val:`string, optional`
+   Name of the first geom. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
+
+.. _sensor-normal-geom2:
+
+:at:`geom2`: :at-val:`string, optional`
+   Name of the second geom. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
+
+.. _sensor-normal-body1:
+
+:at:`body1`: :at-val:`string, optional`
+   Name of the first body. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
+
+.. _sensor-normal-body2:
+
+:at:`body2`: :at-val:`string, optional`
+   Name of the second body. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
+
+.. _sensor-normal-name:
+
+.. _sensor-normal-noise:
+
+.. _sensor-normal-user:
+
+:at:`name`, :at:`noise`, :at:`user`
+   See :ref:`CSensor`.
+
+
+.. _sensor-fromto:
+
+:el-prefix:`sensor/` |-| **fromto** (*)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This element creates a sensor that returns the segment defining the smallest signed distance between the surfaces of two
+geoms. The segment is defined by 6 numbers (x1, y1, z1, x2, y2, z2) corresponding to two points in the world frame.
+(x1, y1, z1) is on the surface of geom1, (x2, y2, z2) is on the surface of geom2. When this sensor is present and the
+:ref:`mjVIS_RANGEFINDER<mjtVisFlag>` visualization flag is set, segments will be visualized as rangefinder rays.
+See :ref:`collision-sensors` for more details about sensors of this type.
+
+.. _sensor-fromto-cutoff:
+
+:at:`cutoff`
+   See :ref:`collision-sensors` for the sematics of this attribute, which is different than for other sensor categories.
+   If no collision is detected, the :ref:`fromto<sensor-fromto>` sensor returns 6 zeros.
+   For this sensor, :at:`cutoff` does not lead to any clamping.
+
+.. _sensor-fromto-geom1:
+
+:at:`geom1`: :at-val:`string, optional`
+   Name of the first geom. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
+
+.. _sensor-fromto-geom2:
+
+:at:`geom2`: :at-val:`string, optional`
+   Name of the second geom. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
+
+.. _sensor-fromto-body1:
+
+:at:`body1`: :at-val:`string, optional`
+   Name of the first body. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
+
+.. _sensor-fromto-body2:
+
+:at:`body2`: :at-val:`string, optional`
+   Name of the second body. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
+
+.. _sensor-fromto-name:
+
+.. _sensor-fromto-noise:
+
+.. _sensor-fromto-user:
+
+:at:`name`, :at:`noise`, :at:`user`
+   See :ref:`CSensor`.
 
 .. _sensor-clock:
 
@@ -7891,6 +8118,10 @@ tendon, slidersite, cranksite.
 
 .. _default-position-kv:
 
+.. _default-position-dampratio:
+
+.. _default-position-timeconst:
+
 :el-prefix:`default/` |-| **position** (?)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
@@ -7951,6 +8182,8 @@ refsite, tendon, slidersite, cranksite.
 
 .. _default-intvelocity-kv:
 
+.. _default-intvelocity-dampratio:
+
 :el-prefix:`default/` |-| **intvelocity** (?)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 

doc/XMLschema.rst

@@ -705,7 +705,7 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`cranksite<actuator-position-cranksite>`                   | :ref:`site<actuator-position-site>`                             | :ref:`refsite<actuator-position-refsite>`                       | :ref:`kp<actuator-position-kp>`                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`kv<actuator-position-kv>`                                 |                                                                 |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`kv<actuator-position-kv>`                                 | :ref:`dampratio<actuator-position-dampratio>`                   | :ref:`timeconst<actuator-position-timeconst>`                   |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| actuator |br| |_| |L|          |    | .. table::                                                                                                                                                                                                                                                                   |
@@ -737,7 +737,7 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`slidersite<actuator-intvelocity-slidersite>`              | :ref:`cranksite<actuator-intvelocity-cranksite>`                | :ref:`site<actuator-intvelocity-site>`                          | :ref:`refsite<actuator-intvelocity-refsite>`                    | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`kp<actuator-intvelocity-kp>`                              | :ref:`kv<actuator-intvelocity-kv>`                              |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`kp<actuator-intvelocity-kp>`                              | :ref:`kv<actuator-intvelocity-kv>`                              | :ref:`dampratio<actuator-intvelocity-dampratio>`                |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| actuator |br| |_| |L|          |    | .. table::                                                                                                                                                                                                                                                                   |
@@ -1163,6 +1163,33 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| sensor |br| |_| |L|            |    | .. table::                                                                                                                                                                                                                                                                   |
+| :ref:`distance                     | \* |    :class: mjcf-attributes                                                                                                                                                                                                                                                   |
+| <sensor-distance>`                 |    |                                                                                                                                                                                                                                                                              |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`name<sensor-distance-name>`                               | :ref:`geom1<sensor-distance-geom1>`                             | :ref:`geom2<sensor-distance-geom2>`                             | :ref:`body1<sensor-distance-body1>`                             | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`body2<sensor-distance-body2>`                             | :ref:`cutoff<sensor-distance-cutoff>`                           | :ref:`noise<sensor-distance-noise>`                             | :ref:`user<sensor-distance-user>`                               | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| |_| sensor |br| |_| |L|            |    | .. table::                                                                                                                                                                                                                                                                   |
+| :ref:`normal                       | \* |    :class: mjcf-attributes                                                                                                                                                                                                                                                   |
+| <sensor-normal>`                   |    |                                                                                                                                                                                                                                                                              |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`name<sensor-normal-name>`                                 | :ref:`geom1<sensor-normal-geom1>`                               | :ref:`geom2<sensor-normal-geom2>`                               | :ref:`body1<sensor-normal-body1>`                               | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`body2<sensor-normal-body2>`                               | :ref:`cutoff<sensor-normal-cutoff>`                             | :ref:`noise<sensor-normal-noise>`                               | :ref:`user<sensor-normal-user>`                                 | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| |_| sensor |br| |_| |L|            |    | .. table::                                                                                                                                                                                                                                                                   |
+| :ref:`fromto                       | \* |    :class: mjcf-attributes                                                                                                                                                                                                                                                   |
+| <sensor-fromto>`                   |    |                                                                                                                                                                                                                                                                              |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`name<sensor-fromto-name>`                                 | :ref:`geom1<sensor-fromto-geom1>`                               | :ref:`geom2<sensor-fromto-geom2>`                               | :ref:`body1<sensor-fromto-body1>`                               | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`body2<sensor-fromto-body2>`                               | :ref:`cutoff<sensor-fromto-cutoff>`                             | :ref:`noise<sensor-fromto-noise>`                               | :ref:`user<sensor-fromto-user>`                                 | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| |_| sensor |br| |_| |L|            |    | .. table::                                                                                                                                                                                                                                                                   |
 | :ref:`clock                        | \* |    :class: mjcf-attributes                                                                                                                                                                                                                                                   |
 | <sensor-clock>`                    |    |                                                                                                                                                                                                                                                                              |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
@@ -1450,7 +1477,9 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`forcerange<default-position-forcerange>`                  | :ref:`gear<default-position-gear>`                              | :ref:`cranklength<default-position-cranklength>`                | :ref:`user<default-position-user>`                              | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`group<default-position-group>`                            | :ref:`kp<default-position-kp>`                                  | :ref:`kv<default-position-kv>`                                  |                                                                 | |
+|                                    |    |    | :ref:`group<default-position-group>`                            | :ref:`kp<default-position-kp>`                                  | :ref:`kv<default-position-kv>`                                  | :ref:`dampratio<default-position-dampratio>`                    | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`timeconst<default-position-timeconst>`                    |                                                                 |                                                                 |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| default |br| |_| |L|           |    | .. table::                                                                                                                                                                                                                                                                   |
@@ -1474,6 +1503,8 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`user<default-intvelocity-user>`                           | :ref:`group<default-intvelocity-group>`                         | :ref:`kp<default-intvelocity-kp>`                               | :ref:`kv<default-intvelocity-kv>`                               | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`dampratio<default-intvelocity-dampratio>`                 |                                                                 |                                                                 |                                                                 | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| default |br| |_| |L|           |    | .. table::                                                                                                                                                                                                                                                                   |
 | :ref:`damper                       | ?  |    :class: mjcf-attributes                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -8,56 +8,94 @@ Upcoming version (not yet released)
 General
 ^^^^^^^
 
+1. Added :ref:`mj_geomDistance` for computing the shortest signed distance between two geoms and optionally a segment
+   connecting them. Relatedly, added the 3 sensors: :ref:`distance<sensor-distance>`, :ref:`normal<sensor-normal>`,
+   :ref:`fromto<sensor-fromto>`. See the function and sensor documentation for details. Fixes :github:issue:`51`.
+2. Improvements to position actuators:
+
+   - Added :ref:`timeconst<actuator-position-timeconst>` attribute to the :ref:`position actuator<actuator-position>`.
+     When set to a positive value, the actuator is made stateful with :at:`filterexact` dynamics.
+   - Added :ref:`dampratio<actuator-position-dampratio>` to both :el:`position` and :el:`intvelocity` actuators. An
+     alternative to the :at:`kv` attribute, it provides a convenient way to set actuator damping using natural units.
+     See attribute documentation for details.
+
+MJX
+^^^
+
+3. Add height-field collision support. Fixes :github:issue:`1491`.
+4. Add a pre-compiled field ``mesh_convex`` to ``mjx.Model`` so that mesh properties can be vmapped over.
+   Fixes :github:issue:`1655`.
+5. Fix a bug in convex mesh collisions, where erroneous edge contacts were being created even though face
+   separating axes were found. Fixes :github:issue:`1695`.
+
+Bug fixes
+^^^^^^^^^
+
+6. Fixed a bug the could cause collisions to be missed when :ref:`fusestatic<compiler-fusestatic>` is enabled, as is
+   often the case for URDF imports. Fixes :github:issue:`1069`, :github:issue:`1577`.
+7. Fixed a bug that was causing the visualization of SDF iterations to write outside the size of the vector storing
+   them. Fixes :github:issue:`1539`.
+
+Version 3.1.5 (May 7, 2024)
+---------------------------
+
+General
+^^^^^^^
+
 .. youtube:: 5k0_wsIRAFc
    :align: right
    :width: 240px
 
 1. Added the :ref:`replicate<replicate>` to MJCF, a :ref:`meta-element<meta-element>` which permits to repeat a subtree
    with incremental translational and rotational offsets.
-2. Added ``mjModel.mesh_scale``: the scaling applied to asset vertices, as specified in the
+2. Enabled an internal cache in the MuJoCo compiler resulting in recompilation speedup. Currently, processed
+   textures, hfields, and OBJ meshes are cached. Support for Unity environments is not yet available.
+3. Added ``mjModel.mesh_scale``: the scaling applied to asset vertices, as specified in the
    :ref:`scale<asset-mesh-scale>` attribute.
-3. Added visual properties which are ignored by the native renderer, but can be used by external renderers:
+4. Added visual properties which are ignored by the native renderer, but can be used by external renderers:
 
    - :ref:`light/bulbradius<body-light-bulbradius>` attribute and corresponding ``mjModel.light_bulbradius`` field.
    - :ref:`material/metallic<asset-material-metallic>` attribute and corresponding ``mjModel.material_metallic`` field.
    - :ref:`material/roughness<asset-material-roughness>` attribute and corresponding ``mjModel.material_roughness``
      field.
-4. The type of the ``size`` argument of :ref:`mj_stackAllocNum` and :ref:`mj_stackAllocInt` was changed from ``int``
+5. The type of the ``size`` argument of :ref:`mj_stackAllocNum` and :ref:`mj_stackAllocInt` was changed from ``int``
    to ``size_t``.
-5. Added support for gmsh format version 2.2 surface meshes in :ref:`flexcomp<body-flexcomp-file>`.
+6. Added support for gmsh format version 2.2 surface meshes in :ref:`flexcomp<body-flexcomp-file>`.
 
 MJX
 ^^^
 .. admonition:: Breaking API changes
    :class: attention
 
-   6. Removed deprecated ``mjx.device_get_into`` and ``mjx.device_put`` functions as they lack critical new
+   7. Removed deprecated ``mjx.device_get_into`` and ``mjx.device_put`` functions as they lack critical new
       functionality.
 
       **Migration:** Use ``mjx.get_data_into`` instead of ``mjx.device_get_into``, and ``mjx.put_data`` instead of
       ``mjx.device_put``.
 
-7. Added cylinder plane collisions.
+8. Added cylinder plane collisions.
-8. Added ``efc_type`` to ``mjx.Data`` and ``dim``, ``efc_address`` to ``mjx.Contact``.
+9. Added ``efc_type`` to ``mjx.Data`` and ``dim``, ``efc_address`` to ``mjx.Contact``.
-9. Added ``geom`` to ``mjx.Contact`` and marked ``geom1``, ``geom2`` deprecated.
+10. Added ``geom`` to ``mjx.Contact`` and marked ``geom1``, ``geom2`` deprecated.
-10. Added ``ne``, ``nf``, ``nl``, ``nefc``, and ``ncon`` to ``mjx.Data`` to match ``mujoco.MjData``.
+11. Added ``ne``, ``nf``, ``nl``, ``nefc``, and ``ncon`` to ``mjx.Data`` to match ``mujoco.MjData``.
-11. Given the above added fields, removed ``mjx.get_params``, ``mjx.ncon``, and ``mjx.count_constraints``.
+12. Given the above added fields, removed ``mjx.get_params``, ``mjx.ncon``, and ``mjx.count_constraints``.
-12. Changed the way meshes are organized on device to speed up collision detection when a mesh is replicated for many
+13. Changed the way meshes are organized on device to speed up collision detection when a mesh is replicated for many
     geoms.
-13. Fixed a bug where capsules might be ignored in broadphase colliision checking.
+14. Fixed a bug where capsules might be ignored in broadphase colliision checking.
-14. Added cylinder collisions using SDFs.
+15. Added cylinder collisions using SDFs.
-15. Added support for all :ref:`condim <coContact>`: 1, 3, 4, 6.
+16. Added support for all :ref:`condim <coContact>`: 1, 3, 4, 6.
-16. Add support functions for ``id2name`` and ``name2id``, MJX versions of :ref:`mj_id2name` and :ref:`mj_name2id`.
+17. Add support functions for ``id2name`` and ``name2id``, MJX versions of :ref:`mj_id2name` and :ref:`mj_name2id`.
-17. Added support for :ref:`gravcomp<body-gravcomp>` and :ref:`actuatorgravcomp<body-joint-actuatorgravcomp>`.
+18. Added support for :ref:`gravcomp<body-gravcomp>` and :ref:`actuatorgravcomp<body-joint-actuatorgravcomp>`.
-18. Fixed a bug in ``mjx.ray`` for sometimes allowed negative distances for ray-mesh tests.
+19. Fixed a bug in ``mjx.ray`` for sometimes allowed negative distances for ray-mesh tests.
+20. Added a new `differentiable physics tutorial <https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/mjx/training_apg.ipynb>`__ that demonstrates training locomotion policies with analytical gradients
+    automatically derived from the MJX physics step.  Contribution by :github:user:`Andrew-Luo1`.
 
 Bug fixes
 ^^^^^^^^^
-19. Defaults of lights were not being saved, now fixed.
+21. Defaults of lights were not being saved, now fixed.
-20. Prevent overwriting of frame names by body names when saving an XML. Bug introduced in 3.1.4.
+22. Prevent overwriting of frame names by body names when saving an XML. Bug introduced in 3.1.4.
-21. Fixed bug in Python binding of :ref:`mj_saveModel`: ``buffer`` argument was documented as optional but was actually
+23. Fixed bug in Python binding of :ref:`mj_saveModel`: ``buffer`` argument was documented as optional but was actually
     not optional.
-22. Fixed bug that prevented memory allocations larger than 2.15 GB.
+24. Fixed bug that prevented memory allocations larger than 2.15 GB. Fixes :github:issue:`1606`.
 
 
 Version 3.1.4 (April 10th, 2024)
@@ -193,7 +231,7 @@ MJX
 
 Python bindings
 ^^^^^^^^^^^^^^^
-12. Improved the implementation of the :ref:`rollout<PySample>` module. Note the changes below are breaking, dependent
+12. Improved the implementation of the :ref:`rollout<PyRollout>` module. Note the changes below are breaking, dependent
     code will require modification.
 
     - Uses :ref:`mjSTATE_FULLPHYSICS<geFullPhysics>` as state spec, enabling divergence detection by inspecting time.

doc/computation/index.rst

@@ -29,7 +29,7 @@ Robots as well as humans interact with their environment primarily through physi
 importance of physics modeling in robotics, machine learning, animation, virtual reality, biomechanics and other fields,
 there is need for simulation models of contact dynamics that are both physically accurate and computationally efficient.
 One application of simulation models is to assess candidate estimation and control strategies before deploying them on
-physical systems. Another application is to automate the design of those strategies - usually through numerical
+physical systems. Another application is to automate the design of those strategies -- usually through numerical
 optimization that uses simulation in an inner loop. The latter application imposes an additional constraint: the
 objective function defined with respect to the contact dynamics should be amenable to numerical optimization. The
 contact model underlying MuJoCo has benefits along these and other relevant dimensions. In the following sections we
@@ -50,7 +50,7 @@ for frictional contacts there are differences.
 If one sees convex models as approximations to LCP, the logical question to ask is how good that approximation is.
 However we do not see it that way. Instead, we see both LCP models and convex models as different approximations to
 physical reality, each with its strengths and weaknesses. The immediate consequence of dropping strict complementarity
-and replacing it with a cost is that complementarity can be violated - meaning that force and velocity in the contact
+and replacing it with a cost is that complementarity can be violated -- meaning that force and velocity in the contact
 normal direction can be simultaneously positive, and frictional forces may not be maximally dissipative. A related
 phenomenon is that the only way to initiate slip is to generate some motion in the normal direction. These effects are
 numerically small yet undesirable. This shortcoming however has little practical relevance, because it is premised on
@@ -117,7 +117,7 @@ Inverse dynamics and optimization
 The objective of inverse dynamics is to recover the applied force and contact force given the position, velocity and
 acceleration of the multi-joint system. With hard contacts this computation is impossible. Consider pushing against a
 wall without moving. The contact force cannot be recovered from the kinematics, unless of course we consider the
-material deformations - in which case we need a soft contact model. Inverse dynamics are trivial to compute with
+material deformations -- in which case we need a soft contact model. Inverse dynamics are trivial to compute with
 spring-damper models of contact, because in that case the contact force is only a function of position and velocity and
 does not depend on applied force. But this is also the reason why spring-damper models are undesirable: ignoring the
 applied force means that an error is introduced at each time step, and so the simulator is perpetually in
@@ -125,7 +125,7 @@ error-correction mode, in turn causing instabilities. In contrast, modern contac
 well as all internal forces) into account when computing the contact force/impulse. But this complicates inversion. The
 present contact model has a uniquely-defined inverse. The inverse dynamics are in fact easier to compute than the
 forward dynamics, because the optimization problem becomes diagonal and decomposes into independent optimization
-problems over individual contacts - which can be solved analytically.
+problems over individual contacts -- which can be solved analytically.
 
 Inverse dynamics play a key role in optimization algorithms arising in system identification, estimation and control.
 They make it possible to treat the sequence of positions (or a parametric representation thereof) as the object being
@@ -269,7 +269,7 @@ activation state :math:`w_i` with its own dynamics. The control inputs for all a
 the force outputs are stored in ``mjData.actuator_force``, and the activation states (if any) are stored in
 ``mjData.act``.
 
-These three components of an actuator - transmission, activation dynamics, and force generation - determine how the
+These three components of an actuator -- transmission, activation dynamics, and force generation -- determine how the
 actuator works. The user can set them independently for maximum flexibility, or use :ref:`Actuator shortcuts
 <CActShortcuts>` which instantiate common actuator types.
 
@@ -736,8 +736,8 @@ properties of quaternions, differentiation with respect to :math:`q` produces ve
 
 Among other applications, equality constraints can be used to create "loop joints", i.e., joints that cannot be modeled
 via the kinematic tree. Gaming engines represent all joints in this way. The same can be done in MuJoCo but is not
-recommended - because it leads to both slower and less accurate simulation, effectively turning MuJoCo into a gaming
+recommended -- because it leads to both slower and less accurate simulation, effectively turning MuJoCo into a gaming
-engine. The only reason to represent joints with equality constraints would be to model soft joints - which can be done
+engine. The only reason to represent joints with equality constraints would be to model soft joints -- which can be done
 via the constraint solver but not via the kinematic tree.
 
 There are five types of equality constraints described next. The numbers in the headings correspond to the
@@ -846,7 +846,7 @@ at the limit, and negative if the limit is violated. The constraint becomes acti
 constraint force depends on distance through the solver :ref:`parameters <soParameters>` described later.
 
 It is possible that both the lower and the upper limits for a given joint or tendon become active. In that case they are
-both included in the list of scalar constraints, however this situation should be avoided - by increasing the range or
+both included in the list of scalar constraints, however this situation should be avoided -- by increasing the range or
 decreasing the margin. In particular, avoid using narrow ranges to approximate an equality constraint. Instead use an
 explicit equality constraint, and if some slack is desired make the constraint soft by adjusting the solver parameters.
 This is more efficient computationally, not only because it involves one scalar constraint instead of two, but also
@@ -900,7 +900,8 @@ model definition.
    * - ``condim``
      - Dimensionality of the contact force/torque in the contact frame. |br| It can be 1, 3, 4 or 6.
    * - ``friction``
-     - Vector of friction coefficients with dimensionality ``condim-1``.
+     - Vector of friction coefficients with dimensionality ``condim-1``. See below for semantics of the specific
+       coefficients.
    * - ``margin``
      - The distance margin used to determine if the contact should be included in the global contact array
        ``mjData.contact``.
@@ -921,19 +922,23 @@ as defined later. The ``condim`` parameter determines the contact type, and has
    similar to a joint or tendon limit, but is applied to the distance between two geoms.
 
 ``condim = 3`` : 3 for elliptic, 4 for pyramidal
-   This is a regular frictional contact, which can generate normal force as well as tangential friction force opposing
+   This is a regular frictional contact, which can generate normal force as well as a tangential friction force opposing
-   slip.
+   slip. An interpertation of this number is the slope of a surface above which a flat object will begin to slip
+   under gravity.
 
 ``condim = 4`` : 4 for elliptic, 6 for pyramidal
    In addition to normal and tangential force, this contact can generate torsional friction torque opposing rotation
-   around the contact normal. This is useful for modeling soft fingers, and can substantially improve the stability of
+   around the contact normal, corresponding to a torque generated by a contacting surface patch. This is useful for
-   simulated grasping. Keep in mind that the torsional (as well as rolling) friction coefficients have different units
+   modeling soft fingers, and can substantially improve the stability of simulated grasping. Torsional friction
-   from the tangential friction coefficients.
+   coefficients have **units of length** which can be interperted as the diameter of the surface contact patch.
 
 ``condim = 6`` : 6 for elliptic, 10 for pyramidal
    This contact can oppose motion in all relative degrees of freedom between the two geoms. In particular it adds
-   rolling friction, which can be used for example to stop a ball from rolling indefinitely on a plane. It can also be
+   rolling friction, which can be used for example to stop a ball from rolling indefinitely on a plane. Rolling friction
-   used to model rolling friction between tires and a road, and in general to stabilize contacts.
+   in the real world results from energy dissipated by local deformations near the contact point. It can be
+   used to model rolling friction between tires and a road, and in general to stabilize contacts. Rolling friction
+   coefficients also have **units of length** which can be interperted as the depth of the local deformation within
+   which energy is dissipated.
 
 Note that condim cannot be 2 or 5. This is because the two tangential directions and the two rolling directions are
 treated as pairs. The friction coefficients within a pair can be different though, which can be used to model skating
@@ -1109,7 +1114,7 @@ and the dual of the dual of a cone is the cone itself. The pyramidal friction co
 self-dual, but the elliptic one is not.
 
 The Huber "norm" is based on the Huber function from robust statistics: it is a quadratic around zero, and transitions
-smoothly to a linear function when the absolute value of the argument crosses a threshold - in this case given by the
+smoothly to a linear function when the absolute value of the argument crosses a threshold -- in this case given by the
 friction loss parameters. Setting :math:`\eta = \infty` recovers the quadratic norm; we use this convention for all
 constraint forces that are not due to friction loss. This is another instance of reverse engineering: we want to obtain
 interval constraints on the friction loss forces, which is non-trivial because Lagrange duality usually yields
@@ -1190,7 +1195,7 @@ the constraints. We are then left with an unconstrained optimization problem ove
 more efficient algorithms.
 
 The reduction is based on the fact that minimization over :math:`y` in :eq:`eq:primal` comes down to finding the nearest
-point on the constraint set - which is either a plane or a cone, and can be done analytically. Substituting the result,
+point on the constraint set -- which is either a plane or a cone, and can be done analytically. Substituting the result,
 we obtain the unconstrained problem
 
 .. math::
@@ -1296,7 +1301,7 @@ because the forward dynamics need all the quantities that enter into the inverse
 the analytical formula. This makes it possible to implement an automatic correctness check in MuJoCo. When the flag
 ``fwdinv`` in ``mjModel.opt.enableflags`` is on, the forward and inverse dynamics are automatically compared at the end
 of each time step, and the difference is recorded in ``mjData.solver_fwdinv``. Discrepancies indicate that the forward
-solver - which is numerical and is usually terminated early - is not converging well. Of course the inverse dynamics are
+solver---which is numerical and is usually terminated early---is not converging well. Of course the inverse dynamics are
 also useful on their own, without computing the forward dynamics first.
 
 .. _soAlgorithms:
@@ -1348,7 +1353,7 @@ representations of the constraint Jacobian and related matrices.
    can be up to 5-dimensional given our contact model. Now we optimize the quadratic cost within this ellipsoid. This is
    an instance of quadratically constrained quadratic programming (QCQP). Since there is only one scalar constraint
    (however nonlinear it may be), the dual is a scalar optimization problem over the unknown Lagrange multiplier. We
-   solve this problem with Newton's method applied until convergence - which in practice takes less than 10 iterations,
+   solve this problem with Newton's method applied until convergence -- which in practice takes less than 10 iterations,
    and involves small matrices. Overall this algorithm has similar behavior to PGS for pyramidal cones, but it can
    handle elliptic cones without approximating them. It does more work per contact, however the contact dimensionality
    is smaller, and these two factors roughly balance each other.
@@ -1570,11 +1575,23 @@ Top level
   pipeline does not support the Runge Kutta integrator.
 - :ref:`mj_fwdPosition` invokes stages **2-11**, the position-dependent part of the pipeline.
 
+.. _piStages:
+
 Stages
 ^^^^^^
+Below we describe the pipeline stages and API functions corresponding to each stage. All functions write their outputs
+into attributes of :ref:`mjData`. It is informative to compare the list below with the :ref:`mjData` struct definition,
+wherein comments above blocks of attributes specify the function that computes them. Note that each stage depends on
+the values computed in some or all of the previous stages.
+
 1. Check the positions and velocities for invalid or unacceptably large real values indicating divergence. If divergence
    is detected, the state is automatically reset and the corresponding warning is raised:
    :ref:`mj_checkPos`, :ref:`mj_checkVel`
+
+Position
+''''''''
+The stages below compute quantities that depend on the generalized positions ``mjData.qpos``.
+
 2. Compute the forward kinematics. This yields the global positions and orientations of all bodies, geoms, sites,
    cameras and lights. It also normalizes all quaternions: :ref:`mj_kinematics`, :ref:`mj_camLight`
 3. Compute the body inertias and joint axes, in global frames centered at the centers of mass of the corresponding
@@ -1591,6 +1608,12 @@ Stages
 11. Compute the matrices and vectors needed by the constraint solvers: :ref:`mj_projectConstraint`
 12. Compute sensor data that only depends on position, and the potential energy if enabled: :ref:`mj_sensorPos`,
     :ref:`mj_energyPos`
+
+Velocity
+''''''''
+The stages below compute quantities that depend on the generalized velocity ``mjData.qvel``. Due to the sequential
+dependence structure of the pipeline, the actual dependence is on both ``qpos`` and ``qvel``.
+
 13. Compute the tendon, flex edge and actuator velocities: :ref:`mj_fwdVelocity`
 14. Compute the body velocities and rates of change of the joint axes, again in the global coordinate frames centered at
     the subtree centers of mass: :ref:`mj_comVel`
@@ -1599,6 +1622,12 @@ Stages
     (if required by sensors, call :ref:`mj_subtreeVel`): :ref:`mj_sensorVel`
 17. Compute the reference constraint acceleration: :ref:`mj_referenceConstraint`
 18. Compute the vector of Coriolis, centrifugal and gravitational forces: :ref:`mj_rne`
+
+Force/acceleration
+''''''''''''''''''
+The stages below compute quantities that depend on :ref:`user inputs<geInput>`. Due to the sequential nature
+of the pipeline, the actual dependence is on the entire :ref:`integration state<geIntegrationState>`.
+
 19. Compute the actuator forces and activation dynamics if defined: :ref:`mj_fwdActuation`
 20. Compute the joint acceleration resulting from all forces except for the (still unknown) constraint forces:
     :ref:`mj_fwdAcceleration`
@@ -1615,12 +1644,53 @@ Stages
     repeats the above sequence three more times, except for the optional computations which are performed only once:
     one of :ref:`mj_Euler`, :ref:`mj_RungeKutta`, :ref:`mj_implicit`
 
+.. _piConsistency:
+
+Consistency in ``mjData``
+~~~~~~~~~~~~~~~~~~~~~~~~~
+The MuJoCo computation pipeline is entirely imperative, nothing happens automatically. This leads to behavior which
+can seem unexpected to users more familiar with other paradigms. Here are two examples of intended behaviors which can
+sometimes be surprising:
+
+- After setting :ref:`the state<geState>`, state-derived quantities do not automatically correspond to the new state.
+  The required stage or stages must be manually invoked. For example after setting the generalized positions
+  ``mjData.qpos``, Cartesian positions and orientations will not be consistent with ``qpos`` without first calling
+  :ref:`mj_kinematics`.
+- After an :ref:`mj_step`, which terminates immediately after updating the state, quantities in ``mjData`` correspond
+  to the *previous* state (or more precisely, the *transition* between the previous and current state).
+  In particular, all position-dependent sensor values and position-dependent computations like kinematic
+  :ref:`Jacobians<mj_jac>`, will be with respect to the *previous positions*.
+
+
+
+.. _piReproducibility:
+
+Reproducibility
+~~~~~~~~~~~~~~~
+
+MuJoCo's simulation pipeline is entirely deterministic and reproducible -- if a :ref:`state<geState>` in a trajectory is
+saved and reloaded and :ref:`mj_step` called again, the resulting next state will be identical. However, there are some
+important caveats:
+
+- Save all the required :ref:`integration state<geIntegrationState>` components. In particular :ref:`warmstart
+  accelerations<geWarmstart>` have only a very small effect on the next state, but should be saved if bit-wise equality
+  is required.
+- Any numerical difference between states, no matter how small, will become significant upon integration, especially for
+  systems with contact. Contact events have high `Lyapunov exponents
+  <https://en.wikipedia.org/wiki/Lyapunov_exponent>`__; this is a property of any rigid-body simulator (and indeed of
+  `real-world physics <https://en.wikipedia.org/wiki/Roulette>`__) and is not MuJoCo-specific.
+- Exact reproducibility is only guaranteed within a **single version**, on the **same architecture**. Small numerical
+  differences are quite common between versioned releases, for example due to code optimizations. This means that when
+  saving an initial state and an open-loop control sequence, the resulting rolled-out trajectory will be identical
+  within the same version, but will likely be different between MuJoCo versions or different operating systems.
+
 .. _piInverse:
 
 Inverse dynamics
 ~~~~~~~~~~~~~~~~
 
-The top-level function :ref:`mj_inverse` invokes the following sequence of computations.
+The top-level function :ref:`mj_inverse` invokes the following sequence of computations. The notes above regarding
+:ref:`consistency<piConsistency>` and :ref:`reproducibility<piReproducibility>` apply here as well.
 
 #. Compute the forward kinematics.
 #. Compute the body inertias and joint axes.
@@ -1647,27 +1717,6 @@ The top-level function :ref:`mj_inverse` invokes the following sequence of compu
    equals the sum of external and actuation forces.
 
 
-.. _piReproducibility:
-
-Reproducibility
-~~~~~~~~~~~~~~~
-
-MuJoCo's simulation pipeline is entirely deterministic and reproducible -- if a :ref:`state<geState>` in a trajectory is
-saved and reloaded and :ref:`mj_step` called again, the resulting next state will be identical. However, there are some
-important caveats:
-
-- Save all the required :ref:`integration state<geIntegrationState>` components. In particular :ref:`warmstart
-  accelerations<geWarmstart>` have only a very small effect on the next state, but should be saved if bit-wise equality
-  is required.
-- Any numerical difference between states, no matter how small, will become significant upon integration, especially for
-  systems with contact. Contact events have high `Lyapunov exponents
-  <https://en.wikipedia.org/wiki/Lyapunov_exponent>`__; this is a property of any rigid-body simulator (and indeed of
-  `real-world physics <https://en.wikipedia.org/wiki/Roulette>`__) and is not MuJoCo-specific.
-- Exact reproducibility is only guaranteed within a **single version**, on the **same architecture**. Small numerical
-  differences are quite common between versioned releases, for example due to code optimizations. This means that when
-  saving an initial state and an open-loop control sequence, the resulting rolled-out trajectory will be identical
-  within the same version, but will likely be different between MuJoCo versions or different operating systems.
-
 .. _derivatives:
 
 Derivatives

doc/conf.py

@@ -111,7 +111,7 @@ html_logo = 'images/banner.svg'
 SHARED_CSS_VARIABLES = {
     'admonition-font-size': '1rem',
     'admonition-title-font-size': '1rem',
-    'sidebar-item-font-size': '115%',
+    'sidebar-item-font-size': '130%',
 }
 
 # font-stack--monospace used in code blocks, Inconsolata fits in 100 chars.

doc/css/theme_overrides.css

@@ -260,6 +260,12 @@ dt .at {
   margin-left: 0.5em;
 }
 
+/* Reduce top and bottom margins around displayed KaTeX equations */
+.katex-display {
+  margin-top: 0.3em;
+  margin-bottom: 0.3em;
+}
+
 details summary {
   font-weight: 600;
 }

doc/includes/references.h

@@ -94,7 +94,7 @@ struct mjContact_ {                // result of collision detection functions
 
   // internal storage used by solver
   mjtNum  mu;                      // friction of regularized cone, set by mj_makeConstraint
-  mjtNum  H[36];                   // cone Hessian, set by mj_updateConstraint
+  mjtNum  H[36];                   // cone Hessian, set by mj_constraintUpdate
 
   // contact descriptors set by mj_collideXXX
   int     dim;                     // contact space dimensionality: 1, 3, 4 or 6
@@ -643,6 +643,11 @@ typedef enum mjtSensor_ {         // type of sensor
   mjSENS_SUBTREELINVEL,           // 3D linear velocity of subtree
   mjSENS_SUBTREEANGMOM,           // 3D angular momentum of subtree
 
+  // sensors for geometric distance; attached to geoms or bodies
+  mjSENS_GEOMDIST,                // signed distance between two geoms
+  mjSENS_GEOMNORMAL,              // normal direction between two geoms
+  mjSENS_GEOMFROMTO,              // segment between two geoms
+
   // global sensors
   mjSENS_CLOCK,                   // simulation time
 
@@ -2542,6 +2547,8 @@ void mj_objectVelocity(const mjModel* m, const mjData* d,
                        int objtype, int objid, mjtNum res[6], int flg_local);
 void mj_objectAcceleration(const mjModel* m, const mjData* d,
                            int objtype, int objid, mjtNum res[6], int flg_local);
+mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2,
+                       mjtNum distmax, mjtNum fromto[6]);
 void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]);
 void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
                          const mjtNum* qpos1, const mjtNum* qpos2);
@@ -2632,9 +2639,9 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt,
                           mjvSceneState* scnstate);
 void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* opt,
                   const mjvPerturb* pert, int catmask, mjvScene* scn);
-void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn);
+void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn);
-void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn);
+void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScene* scn);
-void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn);
+void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn);
 void mjr_defaultContext(mjrContext* con);
 void mjr_makeContext(const mjModel* m, mjrContext* con, int fontscale);
 void mjr_changeFont(int fontscale, mjrContext* con);

doc/mjx.rst

@@ -75,7 +75,7 @@ an ``mjx.Data``:
    model = mujoco.MjModel.from_xml_string("...")
    data = mujoco.MjData(model)
    mjx_model = mjx.put_model(model)
-   mjx_data = mjx.put_data(data)
+   mjx_data = mjx.put_data(model, data)
 
 These MJX variants mirror their MuJoCo counterparts but have a few key differences:
 
@@ -100,7 +100,7 @@ Neither ``mjx.Model`` nor ``mjx.Data`` are meant to be constructed manually.  An
 .. code-block:: python
 
    model = mujoco.MjModel.from_xml_string("...")
-   mjx_model = mjx.device_put(model)
+   mjx_model = mjx.put_model(model)
    mjx_data = mjx.make_data(model)
 
 Using ``mjx.make_data`` may be preferable when constructing batched ``mjx.Data`` structures inside of a ``vmap``.
@@ -151,7 +151,7 @@ Minimal example
    """
 
    model = mujoco.MjModel.from_xml_string(XML)
-   mjx_model = mjx.device_put(model)
+   mjx_model = mjx.put_model(model)
 
    @jax.vmap
    def batched_step(vel):
@@ -196,7 +196,7 @@ The following features are **fully supported** in MJX:
    * - :ref:`Actuator Bias <mjtBias>`
      - ``NONE``, ``AFFINE``
    * - :ref:`Geom <mjtGeom>`
-     - ``PLANE``, ``SPHERE``, ``CAPSULE``, ``BOX``, ``MESH``
+     - ``PLANE``, ``HFIELD``, ``SPHERE``, ``CAPSULE``, ``BOX``, ``MESH`` are fully implemented. ``ELLIPSOID`` and ``CYLINDER`` are implemented but only collide with other primitives.
    * - :ref:`Constraint <mjtConstraint>`
      - ``EQUALITY``, ``LIMIT_JOINT``, ``CONTACT_FRICTIONLESS``, ``CONTACT_PYRAMIDAL``
    * - :ref:`Equality <mjtEq>`
@@ -223,7 +223,7 @@ The following features are **in development** and coming soon:
    * - Category
      - Feature
    * - :ref:`Geom <mjtGeom>`
-     - ``SDF``, ``HFIELD``, ``ELLIPSOID``, ``CYLINDER``
+     - ``SDF``. Collisions between (``SPHERE``, ``BOX``, ``MESH``, ``HFIELD``) and ``CYLINDER``. Collisions between (``BOX``, ``MESH``, ``HFIELD``) and ``ELLIPSOID``.
    * - :ref:`Constraint <mjtConstraint>`
      - :ref:`Frictionloss <coFriction>`, ``CONTACT_ELLIPTIC``, ``FRICTION_DOF``
    * - :ref:`Integrator <mjtIntegrator>`

doc/modeling.rst

@@ -31,7 +31,7 @@ Loading models
 
 As explained in :ref:`Model instances <Instance>` in the Overview chapter, MuJoCo models can be loaded from plain-text
 XML files in the MJCF or URDF formats, and then compiled into a low-level mjModel. Alternatively a previously saved
-mjModel can be loaded directly from a binary MJB file - whose format is not documented but is essentially a copy of the
+mjModel can be loaded directly from a binary MJB file -- whose format is not documented but is essentially a copy of the
 mjModel memory buffer. MJCF and URDF files are loaded with :ref:`mj_loadXML` while MJB files are loaded with
 :ref:`mj_loadModel`.
 
@@ -45,8 +45,8 @@ XML file has a unique top-level element. This element must be :el:`mujoco` for M
 Compiling models
 ~~~~~~~~~~~~~~~~
 
-Once a high-level mjCModel is created - by loading an MJCF file or a URDF file, or programmatically when such
+Once a high-level mjCModel is created---by loading an MJCF file or a URDF file, or programmatically when such
-functionality becomes available - it is compiled into mjModel. Even though loading and compilation are presently
+functionality becomes available---it is compiled into mjModel. Even though loading and compilation are presently
 combined in one step, compilation is independent of loading, meaning that the compiler works in the same way
 regardless of how mjCModel was created. Both the parser and the compiler perform extensive error checking, and abort
 when the first error is encountered. The resulting error messages contain the row and column number in the XML file,
@@ -56,7 +56,7 @@ simulation step of the compiled model is performed and any runtime errors are in
 (temporarily) setting :ref:`mju_user_error` to point to a function that throws C++
 exceptions; the user can implement similar error-interception functionality at runtime if desired.
 
-The entire process of parsing and compilation is very fast - less than a second if the model does not contain large
+The entire process of parsing and compilation is very fast -- less than a second if the model does not contain large
 meshes or actuator lengthranges that need to be computed via simulation. This makes it possible to design models
 interactively, by re-loading often and visualizing the changes. Note that the :ref:`simulate.cc <saSimulate>` code
 sample has a keyboard shortcut for re-loading the current model (Ctrl+L).
@@ -166,7 +166,7 @@ which overrides the default settings. The cylinder specifies defaults class "mai
 "sub", even though the latter was specified in the childclass attribute of the body containing the geom.
 
 Now we describe the general rules. MuJoCo supports unlimited number of defaults classes, created by possibly nested
-:ref:`default <default>` elements in the XML. Each class has a unique name - which is a required
+:ref:`default <default>` elements in the XML. Each class has a unique name -- which is a required
 attribute except for the top-level class whose name is "main" if left undefined. Each class also has a complete
 collection of dummy model elements, with their attributes set as follows. When a defaults class is defined within
 another defaults class, the child automatically inherits all attribute values from the parent. It can then override
@@ -248,7 +248,7 @@ specified by the user, the frame is not rotated.
 :at:`zaxis`: :at-val:`real(3), optional`
    The Z axis of the frame. The compiler finds the minimal rotation that maps the vector :math:`(0, 0, 1)` into the
    vector specified here. This determines the X and Y axes of the frame implicitly. This is useful for geoms with
-   rotational symmetry around the Z axis, as well as lights - which are oriented along the Z axis of their frame.
+   rotational symmetry around the Z axis, as well as lights -- which are oriented along the Z axis of their frame.
 
 .. _CSolver:
 
@@ -448,33 +448,36 @@ there is nothing to do, but what if their parameters are different? In that case
 :at:`solmix` and :at:`priority` to decide how to combine them. The combination rules for each contact parameter
 are as follows:
 
-condim
+**condim**
    If one of the two geoms has higher priority, its condim is used. If both geoms have the same priority, the maximum of
    the two condims is used. In this way a frictionless geom and a frictional geom form a frictional contact, unless the
    frictionless geom has higher priority. The latter is desirable in particle systems for example, where we may not want
    the particles to stick to any objects.
-friction
+**friction**
    Recall that contacts can have up to 5 friction coefficients: two tangential, one torsional, two rolling. Each contact
    in mjData.contact actually has all 5 of them, even if condim is less than 6 and not all coefficients are used. In
    contrast, geoms have only 3 friction coefficients: tangential (same for both axes), torsional, rolling (same for both
    axes). Each of these 3D vectors of friction coefficients is expanded into a 5D vector of friction coefficients by
-   replicating the tangetial and rolling components. The contact friction coefficients are then computed according to
+   replicating the tangetial and rolling components. See the :ref:`Contact<coContact>` section in the Computation
-   the following rule: if one of the two geoms has higher priority, its friction coefficients are used. Otherwise the
+   chapter for an intuitive description of the semantics of tangential, torsional and rolling coefficients.
-   element-wise maximum of each friction coefficient over the two geoms is used. The rationale is similar to taking the
+
-   maximum over condim: we want the more frictional geom to win.
+   The contact friction coefficients are then computed according to the following rule: if one of the two geoms has
+   higher priority, its friction coefficients are used. Otherwise the **element-wise maximum** of each friction
+   coefficient over the two geoms is used.
+
    The reason for having 5 coefficients per contact and only 3 per geom is as follows. For a contact pair, we want to
    allow the most flexible model our solver can handle. As mentioned earlier, anisotropic friction can be exploited to
    model effects such as skating. This however requires knowing how the two axes of the contact tangent plane are
    oriented. For a predefined contact pair we know the two geom types in advance, and the corresponding collision
-   function always generates contact frames oriented in the same way - which we do not describe here but it can be seen
+   function always generates contact frames oriented in the same way -- which we do not describe here but it can be seen
    in the visualizer. For individual geoms however, we do not know which other geoms they might collide with and what
    their geom types might be, so there is no way to know how the contact tangent plane will be oriented when specifying
    an individual geom. This is why MuJoCo does not allow anisotropic friction in the individual geom specifications, but
    only in the explicit contact pair specifications.
-margin, gap
+**margin**, **gap**
    The maximum of the two geom margins (or gaps respectively) is used. The geom priority is ignored here, because the
    margin and gap are distance properties and a one-sided specification makes little sense.
-solref, solimp
+**solref**, **solimp**
    If one of the two geoms has higher priority, its solref and solimp parameters are used. If both geoms have the same
    priority, the weighted average is used. The weights are proportional to the solmix attributes, i.e., weight1 =
    solmix1 / (solmix1 + solmix2) and similarly for weight2. There is one important exception to this weighted averaging
@@ -493,7 +496,7 @@ facilitate this process, we provide a mechanism to override some of the solver p
 the actual model. Once the override is disabled, the simulation reverts to the parameters specified in the model. This
 mechanism can also be used to implement continuation methods in the context of numerical optimization (such as optimal
 control or state estimation). This is done by allowing contacts to act from a distance in the early phases of
-optimization - so as to help the optimizer find a gradient and get close to a good solution - and reducing this effect
+optimization---so as to help the optimizer find a gradient and get close to a good solution---and reducing this effect
 later to make the final solution physically realistic.
 
 The relevant settings here are the :at:`override` attribute of :ref:`flag <option-flag>` which
@@ -556,12 +559,12 @@ general guidelines and observations:
    threshold is better defined in terms of number of active constraints, which is model and behavior dependent.
 -  The choice between pyramidal and elliptic friction cones is a modeling choice rather than an algorithmic choice,
    i.e., it leads to a different optimization problem solved with the same algorithms. Elliptic cones correspond more
-   closely to physical reality. However pyramidal cones can improve the performance of the algorithms - but not
+   closely to physical reality. However pyramidal cones can improve the performance of the algorithms -- but not
    necessarily. While the default is pyramidal, we recommend trying the elliptic cones. When contact slip is a problem,
    the best way to suppress it is to use elliptic cones, large impratio, and the Newton algorithm with very small
    tolerance. If that is not sufficient, enable the Noslip solver.
 -  The Newton algorithm is the best choice for most models. It has quadratic convergence near the global minimum and
-   gets there in surprisingly few iterations - usually around 5, and rarely more than 20. It should be used with
+   gets there in surprisingly few iterations -- usually around 5, and rarely more than 20. It should be used with
    aggressive tolerance values, say 1e-10, because it is capable of achieving high accuracy without added delay (due to
    quadratic convergence at the end). The only situation where we have seen it slow down are large models with elliptic
    cones and many slipping contacts. In that regime the Hessian factorization needs a lot of updates. It may also slow
@@ -1314,7 +1317,7 @@ probe away (which is possible because the probe is a mocap body which can move i
 we can see the indentation made by the probe, and the resulting deformation in the rest of the body. By changing the
 solref and solimp attributes of the equality constraints that hold the soft object together, one can adjust the behavior
 of the system making it softer or harder, damped or springy, etc. Note that box, cylinder and ellipsoid objects do not
-involve long kinematic chains, and can be simulated at large timesteps - similar to particle and grid, and unlike rope
+involve long kinematic chains, and can be simulated at large timesteps -- similar to particle and grid, and unlike rope
 and cloth.
 
 .. _CDeformable:
@@ -1403,7 +1406,7 @@ self-collisions; see XML reference.
 In case of 3D flexes made of tetrahedra, it may be useful to examine how the flex is "triangulated" internally. We have
 a special visualization mode that peels off the outer layers. Below is an example with the Stanford Bunny. Note how it
 has smaller tetrahedra on the outside and larger ones on the inside. This mesh design makes sense, because we want the
-collision surface to be accurate, but on the inside we just need soft material properties - which require less spatial
+collision surface to be accurate, but on the inside we just need soft material properties -- which require less spatial
 resolution. In order to convert a surface mesh to a tetrahedral mesh, we recommend open tools like the
 `fTetWild library <https://github.com/wildmeshing/fTetWild>`__.
 
@@ -1436,7 +1439,7 @@ The flexibility of repeated MCJF sections comes at a price: global settings that
 the :at:`angle` attribute of :ref:`compiler <compiler>` for example, can be defined multiple times.
 MuJoCo allows this, and uses the last definition encountered in the composite model, after all include elements have
 been processed. So if model A is defined in degrees and model B is defined in radians, and A is included in B after
-the :el:`compiler` element in B, the entire composite model will be treated as if it was defined in degrees - leading
+the :el:`compiler` element in B, the entire composite model will be treated as if it was defined in degrees -- leading
 to undesirable consequences in this case. The user has to make sure that models included in each other are compatible
 in this sense; local vs. global coordinates is another compatibility requirement.
 
@@ -1469,7 +1472,7 @@ unless there is a specific reason to define it. There can be several such reason
    is useful for custom computations involving a model element that is identified by its name in the XML (as opposed to
    relying on a fixed index which can change when the model is edited).
 -  The model file could in principle become more readable by naming certain elements. Keep in mind however that XML
-   itself has a commenting mechanism, and that mechanism is more suitable for achieving readability - especially since
+   itself has a commenting mechanism, and that mechanism is more suitable for achieving readability -- especially since
    most text editors provide syntax highlighting which detects XML comments.
 
 .. _CURDF:
@@ -1486,7 +1489,9 @@ sub-elements :ref:`compiler <compiler>`, :ref:`option <option>`,
 are modified so as to accomodate the URDF modeling convention. The :ref:`compiler <compiler>` extension
 in particular has proven very useful, and indeed several of its attributes were introduced because a number of
 existing URDF models have non-physical dynamics parameters which MuJoCo's built-in compiler will reject if left
-unmodified. This extension is also needed to specify mesh directories.
+unmodified. This extension is also needed to specify mesh directories. Also note that the compiler attributes
+:ref:`strippath<compiler-strippath>`, :ref:`angle<compiler-angle>`, :ref:`fusestatic<compiler-fusestatic>` and
+:ref:`discardvisual<compiler-discardvisual>` have different default values for URDF and MJCF.
 
 Note that the while MJCF models are checked against a custom XML schema by the parser, URDF models are not. Even the
 MuJoCo-specific elements embedded in the URDF file are not checked. As a result, mis-typed attribute names are
@@ -1620,8 +1625,8 @@ is not always obvious, so it may be useful to have it spelled out.
 Performance tuning
 ~~~~~~~~~~~~~~~~~~
 
-Here is a list of steps one can take in order to maximize simulation throughput. All of the recommendations below
+Below is a list of steps one can take in order to maximize simulation throughput. All of the recommendations
-involve some tweaking. It is recommended that these be carried out in interactive fashion while looking at the
+involve parameter tweaking. It is recommended that these be carried out in interactive fashion while looking at the
 :ref:`simulate<saSimulate>` utility's built-in profiler. A detailed and sometimes more useful profile is also reported
 by the :ref:`testspeed<saTestspeed>` utility. When embarking on the more elaborate steps below, target the most
 expensive pipeline component reported by the profiler. Note that some of these are subtly different for MJX, see
@@ -1663,6 +1668,61 @@ dedicated section :ref:`therein<MjxPerformance>`.
    - If replacing collision meshes with primitives is not feasible, decimate the meshes as much as possible. Open source
      tools like trimesh, Blender, MeshLab and CoACD are very useful in this regard.
 
+.. _CSlippage:
+
+Preventing slip
+~~~~~~~~~~~~~~~
+
+Below is a list of steps one can take in order to diagnose and solve contact slippage, which is especially problematic
+in manipulation tasks. In order to diagnose slippage, it is recommended to use the :ref:`simulate<saSimulate>` utility's
+built in visualization options to inspect contacts and contact forces. It is often helpful to tweak the visual size of
+contacts and forces (using the global :ref:`meansize<statistic-meansize>` or the specific
+:ref:`contactwidth<visual-scale-contactwidth>`, :ref:`contactheight<visual-scale-contactheight>` and
+:ref:`forcewidth<visual-scale-forcewidth>` attributes) and the :ref:`force scaling<visual-map-force>` attribute, to
+better visualize and understand the contact configuration and resulting forces.
+
+**Slip-preventing contact forces are outside the friction cone**
+  This implies that the physics cannot prevent slip, even in principle. This occurs when:
+
+  a. *The normal force is too small.* Ensure that the maximum force that can be applied by the gripper mutiplied by
+     the sliding friction coefficient is significantly greater than the weight of the object.
+  b. *The sliding friction coefficient is too low.* Increase the sliding :ref:`friction<body-geom-friction>`
+     coefficient.
+  c. *Torsional friction is insufficient to apply the required torques.* Increase :ref:`condim<body-geom-condim>` to
+     4 or 6 and choose appropriate friction coefficients.
+     **condim 4** enables torsional friction, preventing rotation around the normal.
+     **condim 6** also enables rolling friction, preventing rotation around the tangential directions.
+     See the :ref:`Contact<coContact>` section for details and the specifc semantics of these coefficients.
+
+**The geometry does not support the required forces or torques**
+  This is a common real-world problem, solved by improved design of grippers and handles.
+
+  a. Improve the geometry of the contacting geoms in order to add more contact points, possibly with non-flat
+     geometry (e.g., bumps), so slippage is prevented by the normal force and not only frictional components.
+  b. If contacts are between flat surfaces, try enabling the :ref:`multiccd<option-flag-multiccd>` flag, which allows
+     the detector to find more contacts than the single contact returned by the convex-convex collider.
+
+**High-frequency vibration**
+  High-frequency, low-amplitude vibrations are also a real-world problem in many industrial settings, but unlike in
+  simulation, in the real world they are audible. Such vibration is often caused by controllers with very
+  high gains and sometimes by stick-slip feedback from contacts or joints, resonating with the eigen-modes of the
+  mechanism. The easist way to diagnose such vibration is to visualize contact forces in
+  :ref:`simulate<saSimulate>`. The solution is usually to reduce the :ref:`timestep<option-timestep>` and/or add
+  some :ref:`armature<body-joint-armature>` to the relevant joints. Another reason for vibration is feedback from
+  explicit damping. Use the implicit or implicitfast integrators, as documented in the
+  :ref:`Numerical Integration<geIntegration>` section.
+
+**Slow slippage**
+  Unlike the above problems which lead to fast slippage, slow, gradual slippage is a property of MuJoCo's contact
+  model by design, since without it the inverse dynamics are not defined. This is discussed in detail in the
+  :ref:`softness and slip<Soft>` clarification. This type of slippage can be addressed in two ways.
+
+  a. Increase the :ref:`impration<option-impratio>` parameter. This will reduce (but not entirely prevent) slow
+     slippage. Note that high impratio values work well only with :ref:`elliptic cones<option-cone>`.
+  b. Enable the noslip solver by increasing :ref:`noslip_iterations<option-noslip_iterations>` to a positive integer.
+     A small number (1, 2 or 3) is usually sufficient. The noslip post-processing solver will entirely prevent slip,
+     at the cost of making inverse dynamics ill-defined and additional computational cost.
+
 .. _CBacklash:
 
 Backlash
@@ -1710,7 +1770,7 @@ destabilize the system because of integration errors. This was already discussed
 
 The standard approach to reducing integration errors is to reduce the timestep or use the Runge-Kutta integrator, both
 of which are effective but slow down the simulation. An alternative approach is to put all damping in the joints and
-use the Euler integrator. In that case damping forces are integrated implicitly - meaning that the inertia matrix is
+use the Euler integrator. In that case damping forces are integrated implicitly -- meaning that the inertia matrix is
 adjusted and re-factorized internally as part of the velocity update, in a way transparent to the user. Implicit
 integration is much more stable than explicit integration, allowing substantially larger time steps. Note that the
 Runge-Kutta integrator is explicit, and so is Euler except for the way it treats damping forces. Ideally we would have

doc/models.rst

@@ -34,6 +34,8 @@ Humanoids
 
    * - Model
      - Preview
+   * - `Unitree G1 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/unitree_g1>`_
+     - .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/unitree_g1/g1.png
    * - `Unitree H1 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/unitree_h1>`_
      - .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/unitree_h1/h1.png
    * - `Robotis OP3 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/robotis_op3>`_
@@ -74,6 +76,8 @@ Quadrupeds
      - .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/google_barkour_v0/barkour_v0.png
    * - `Google Barkour vB <https://github.com/google-deepmind/mujoco_menagerie/tree/main/google_barkour_vb>`_
      - .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/google_barkour_vb/barkour_vb.png
+   * - `Boston Dynamics Spot <https://github.com/google-deepmind/mujoco_menagerie/tree/main/boston_dynamics_spot>`_
+     - .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/boston_dynamics_spot/spot.png
 
 Grippers & Hands
 ^^^^^^^^^^^^^^^^
@@ -100,6 +104,8 @@ Arms
      - Preview
    * - `Franka Emika Panda <https://github.com/google-deepmind/mujoco_menagerie/tree/main/franka_emika_panda>`_
      - .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/franka_emika_panda/panda.png
+   * - `Franka FR3 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/franka_fr3>`_
+     - .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/franka_fr3/fr3.png
    * - `Universal Robots UR5e <https://github.com/google-deepmind/mujoco_menagerie/tree/main/universal_robots_ur5e>`_
      - .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/universal_robots_ur5e/ur5e.png
    * - `Universal Robots UR10e <https://github.com/google-deepmind/mujoco_menagerie/tree/main/universal_robots_ur10e>`_

doc/overview.rst

@@ -56,7 +56,7 @@ Soft, convex and analytically-invertible contact dynamics
    equality constraints.
 
 Tendon geometry
-   MuJoCo can model the 3D geometry of tendons - which are minimum-path-length strings obeying wrapping and via-point
+   MuJoCo can model the 3D geometry of tendons -- which are minimum-path-length strings obeying wrapping and via-point
    constraints. The mechanism is similar to the one in OpenSim but implements a more restricted, closed-form set of
    wrapping options to speed up computation. It also offers robotics-specific structures such as pulleys and coupled
    degrees of freedom. Tendons can be used for actuation as well as to impose inequality or equality constraints on the
@@ -635,7 +635,7 @@ store results from custom computations there; recall that everything that change
 Custom text
 ^^^^^^^^^^^
 
-Custom text fields can be saved in the model. They can be used in custom computations - either to specify keyword
+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)
 which is more suitable.
@@ -788,7 +788,8 @@ For situations where it is desirable to suppress slip completely, there is a sec
 the main solver. It updates the contact forces in friction dimensions by disregarding constraint softness. When this
 option is used however, MuJoCo is no longer solving the convex optimization problem it was designed to solve, and the
 simulation may become less robust. Thus using the Newton solver with elliptic friction cones and large value of
-``impratio`` is the recommended way of reducing slip.
+``impratio`` is the recommended way of reducing slip. For more detailed recommendations, see
+:ref:`preventing slip<CSlippage>` in the Modeling chapter.
 
 .. _TypeNameId:
 
@@ -847,7 +848,7 @@ Now the differences. Bodies are used to construct the kinematic tree and are con
 geoms and sites. Bodies have a spatial frame, inertial properties, but no properties related to appearance or collision
 geometry. This is because such properties do not affect the physics (except for contacts of course, but these are
 handled separately). If you have seen diagrams of kinematic trees in robotics textbooks, the bodies are usually drawn as
-amorphous shapes - to make the point that their actual shape is irrelevant to the physics.
+amorphous shapes -- to make the point that their actual shape is irrelevant to the physics.
 
 Geoms (short for geometric primitive) are used to specify appearance and collision geometry. Each geom belongs to a body
 and is rigidly attached to that body. Multiple geoms can be attached to the same body. This is particularly useful in

doc/programming/simulation.rst

@@ -150,10 +150,10 @@ continuous time, and then integrate over a time period specified by ``mjModel.op
 the acceleration ``mjData.qacc`` at time ``mjData.time``, given the :ref:`state and control <siStateControl>` at time
 ``mjData.time``. The numerical integrator then advances the state and time to ``mjData.time + mjModel.opt.timestep``.
 Now, the control is required to be a function of the state at time ``mjData.time``. However a general feedback
-controller can be a very complex function, depending on various features of the state - in particular all the features
+controller can be a very complex function, depending on various features of the state -- in particular all the features
 computed by MuJoCo as intermediate results of the simulation. These may include contacts, Jacobians, passive forces.
-None of these quantities are available before ``mj_step`` is called (or rather, they are available but outdated by one
+None of these quantities are available before ``mj_step`` is called (or rather, they are available but *outdated by one
-time step). In contrast, when ``mj_step`` calls our control callback, it does so as late in the computation as possible
+time step*). In contrast, when ``mj_step`` calls our control callback, it does so as late in the computation as possible
 - namely after all the intermediate results dependent on the state but not on the control have been computed.
 
 The same effect can be achieved without using a control callback. This is done by breaking ``mj_step`` in two parts:
@@ -253,7 +253,7 @@ notion of state of a dynamical system. Dynamical systems are usually described i
 where ``t`` is the time, ``x`` is the state vector, ``u`` is the control vector, and ``f`` is the function that
 computes the time-derivative of the state. This is a continuous-time formulation, and indeed the physics model
 simulated by MuJoCo is defined in continuous time. Even though the numerical integrator operates in discrete time, the
-main part of the computation - namely the function :ref:`mj_forward` - corresponds to the
+main part of the computation---namely the function :ref:`mj_forward`---corresponds to the
 continuous-time dynamics function ``f(t,x,u)`` above. Here we explain this correspondence.
 
 The state vector in MuJoCo is:
@@ -426,7 +426,7 @@ the first and second sections (position and velocity dependent) have already bee
 When can we use the above machinery and skip some of the computations? In a regular simulation this is not possible.
 However, MuJoCo is designed not only for simulation but also for more advanced applications such as model-based
 optimization, machine learning etc. In such settings one often needs to sample the dynamics at a cloud of nearby
-states, or approximate derivatives via finite differences - which is another form of sampling. If the samples are
+states, or approximate derivatives via finite differences -- which is another form of sampling. If the samples are
 arranged on a grid, where only the position or only the velocity or only the control is different from the center
 point, then the above mechanism can improve performance by about a factor of 2.
 
@@ -591,7 +591,7 @@ The only way to guarantee that all changes are saved is to save the model as a b
 changes need to be made programmatically, as in system identification for example, and this can only be done with the
 compiled model. So in summary, we have reasonable but not perfect mechanisms for saving model changes. The reason for
 this lack of perfection is that we are working with a compiled model, so this is like changing a binary executable and
-asking a "decompiler" to make corresponding changes to the C code - it is just not possible in general.
+asking a "decompiler" to make corresponding changes to the C code -- it is just not possible in general.
 
 .. _siLayout:
 
@@ -642,7 +642,7 @@ spread out the data so that we do not have to perform many memory moves when ins
 layout "uncompressed". It is still a valid layout, but instead of A_rowadr[r] = A_rowadr[r-1] + A_rownnz[r] which is
 the standard convention, we set A_rowadr[r] = r*n. MuJoCo uses sparse matrices internally
 
-To represent 3D orientations and rotations, MuJoCo uses unit quaternions - namely 4D unit vectors arranged as q = (w,
+To represent 3D orientations and rotations, MuJoCo uses unit quaternions -- namely 4D unit vectors arranged as q = (w,
 x, y, z). Here (x, y, z) is the rotation axis unit vector scaled by sin(a/2), where a is the rotation angle in
 radians, and w = cos(a/2). Thus the quaternion corresponding to a null rotation is (1, 0, 0, 0). This is the default
 setting of all quaternions in MJCF.
@@ -841,7 +841,7 @@ Jacobians
 The derivative of any vector function with respect to its vector argument is called Jacobian. When this term is used
 in multi-joint kinematics and dynamics, it refers to the derivative of some spatial quantity as a function of the
 system configuration. In that case the Jacobian is also a linear map that operates on vectors in the (co)tangent space
-to the configuration manifold - such as velocities, momenta, accelerations, forces. One caveat here is that the system
+to the configuration manifold -- such as velocities, momenta, accelerations, forces. One caveat here is that the system
 configuration encoded in ``mjData.qpos`` has dimensionality ``mjModel.nq``, while the tangent space has dimensionality
 ``mjModel.nv``, and the latter is smaller when quaternion joints are present. So the size of the Jacobian matrix is
 N-by-``mjModel.nv`` where N is the dimensionality of the spatial quantity being differentiated.
@@ -861,7 +861,7 @@ Jacobians, which tell us how a spatial frame anchored at the given point will tr
 change to the kinematic configuration. More precisely, the Jacobian maps joint velocities to end-effector velocities,
 while the transpose of the Jacobian maps end-effector forces to joint forces. There are also several other
 ``mj_jacXXX`` functions; these are convenience functions that call the main ``mj_jac`` function with different points
-of interest - such as a body center of mass, geom center etc.
+of interest -- such as a body center of mass, geom center etc.
 
 The ability to compute end-effector Jacobians exactly and efficiently is a key advantage of working in joint
 coordinates. Such Jacobians are the foundation of many control schemes that map end-effector errors to actuator

doc/python.rst

@@ -1,8 +1,8 @@
-===============
+======
-Python Bindings
+Python
-===============
+======
 
-Starting with version 2.1.2, MuJoCo comes with native Python bindings that are developed in C++ using
+MuJoCo comes with native Python bindings that are developed in C++ using
 `pybind11 <https://pybind11.readthedocs.io/>`__. The Python API is consistent with the underlying C API. This leads to
 some non-Pythonic code structure (e.g. order of function arguments), but it has the benefit that the
 :doc:`API documentation<APIreference/index>` is applicable to both languages.
@@ -12,23 +12,22 @@ low-level bindings that are meant to give as close to a direct access to the MuJ
 order to provide an API and semantics that developers would expect in a typical Python library, the bindings
 deliberately diverge from the raw MuJoCo API in a number of places, which are documented throughout this page.
 
-Google DeepMind’s `dm_control <https://github.com/google-deepmind/dm_control>`__ reinforcement learning library (which
+Google DeepMind’s `dm_control <https://github.com/google-deepmind/dm_control>`__ reinforcement learning library depends
-prior to version 1.0.0 implemented its own MuJoCo bindings based on ``ctypes``) has been updated to depend on the
+on the ``mujoco`` package and continues to be supported by Google DeepMind. For code that depends on dm_control versions
-``mujoco`` package and continues to be supported by Google DeepMind. Changes in dm_control should be largely transparent
+prior to 1.0.0, consult the
-to users of previous versions, however code that depended directly on its low-level API may need to be updated. Consult
+`migration guide <https://github.com/google-deepmind/dm_control/blob/main/migration_guide_1.0.md>`__.
-the `migration guide <https://github.com/google-deepmind/dm_control/blob/main/migration_guide_1.0.md>`__ for detail.
 
-For mujoco-py users, we include :ref:`notes <PyMjpy_migration>` below to aid migration.
+For mujoco-py users, we include :ref:`migration notes <PyMjpy_migration>` below.
 
 .. _PyNotebook:
 
 Tutorial notebook
 =================
 
-A MuJoCo tutorial using the Python bindings is available here: |colab|
+A MuJoCo tutorial using the Python bindings is available here: |mjcolab|
 
-.. |colab| image:: https://colab.research.google.com/assets/colab-badge.svg
+.. |mjcolab| image:: https://colab.research.google.com/assets/colab-badge.svg
-           :target: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/tutorial.ipynb
+             :target: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/tutorial.ipynb
 
 .. _PyInstallation:
 
@@ -55,8 +54,8 @@ use cases are supported:
 
 .. _PyViewerApp:
 
-Standalone application
+Standalone app
-----------------------
+--------------
 
 - ``python -m mujoco.viewer`` launches an empty visualization session, where a model can be loaded by drag-and-drop.
 - ``python -m mujoco.viewer --mjcf=/path/to/some/mjcf.xml`` launches a visualization session for the specified
@@ -466,90 +465,6 @@ Alternatively, if a callback is implemented in a native dynamic library, users c
 it to ``mujoco.set_mjcb_foo``. The bindings will then retrieve the underlying function pointer and assign it directly to
 the raw callback pointer, and the GIL will **not** be acquired each time the callback is entered.
 
-.. _PySample:
-
-Open-loop rollouts
-==================
-
-We include a code sample showing how to add additional C/C++ functionality, exposed as a Python module via pybind11. The
-sample, implemented in `rollout.cc <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout.cc>`__
-and wrapped in `rollout.py <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout.py>`__,
-implements a common use case where tight loops implemented outside of Python are beneficial: rolling out a trajectory
-(i.e., calling ``mj_step()`` in a loop), given an intial state and sequence of controls, and returning subsequent states
-and sensor values. The basic usage form is
-
-.. code-block:: python
-
-   state, sensordata = rollout.rollout(model, data, initial_state, control)
-
-``initial_state`` is a ``nroll x nstate`` array, with ``nroll`` initial states of size ``nstate``, where
-``nstate = mj_stateSize(model, mjtState.mjSTATE_FULLPHYSICS)`` is the size of the
-:ref:`full physics state<geFullPhysics>`. ``control`` is a ``nroll x nstep x ncontrol`` array of controls. Controls are
-by default the ``mjModel.nu`` standard actuators, but any combination of :ref:`user input<geInput>` arrays can be
-specified by passing an optional ``control_spec`` bitflag.
-
-If a rollout diverges, the current state and sensor values are used to fill the remainder of the trajectory.
-Therefore, non-increasing time values can be used to detect diverged rollouts.
-
-The ``rollout`` function is designed to be completely stateless, so all inputs of the stepping pipeline are set and any
-values already present in the given ``MjData`` instance will have no effect on the output.
-
-Since the Global Interpreter Lock can be released, this function can be efficiently threaded using Python threads. See
-the ``test_threading`` function in
-`rollout_test.py <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout_test.py>`__ for an example
-of threaded operation (and more generally for usage examples).
-
-.. _PyMjpy_migration:
-
-Migration from mujoco-py
-========================
-
-In mujoco-py, the main entry point is the `MjSim <https://github.com/openai/mujoco-py/blob/master/mujoco_py/mjsim.pyx>`_
-class.  Users construct a stateful ``MjSim`` instance from an MJCF model (similar to ``dm_control.Physics``), and this
-instance holds references to an ``mjModel`` instance and its associated ``mjData``.  In contrast, the MuJoCo Python
-bindings (``mujoco``) take a more low-level approach, as explained above: following the design principle of the C
-library, the ``mujoco`` module itself is stateless, and merely wraps the underlying native structs and functions.
-
-While a complete survey of mujoco-py is beyond the scope of this document, we offer below implementation notes for a
-non-exhaustive list of specific mujoco-py features:
-
-``mujoco_py.load_model_from_xml(bstring)``
-   This factory function constructs a stateful ``MjSim`` instance. When using ``mujoco``, the user should call the
-   factory function ``mujoco.MjModel.from_xml_*`` as described :ref:`above <PyStructs>`. The user is then responsible
-   for holding the resulting ``MjModel`` struct instance and explicitly generating the corresponding ``MjData`` by
-   calling ``mujoco.MjData(model)``.
-
-``sim.reset()``, ``sim.forward()``, ``sim.step()``
-   Here as above, ``mujoco`` users needs to call the underlying library functions, passing instances of ``MjModel`` and
-   ``MjData``: :ref:`mujoco.mj_resetData(model, data) <mj_resetData>`, :ref:`mujoco.mj_forward(model, data)
-   <mj_forward>`, and :ref:`mujoco.mj_step(model, data) <mj_step>`.
-
-``sim.get_state()``, ``sim.set_state(state)``, ``sim.get_flattened_state()``, ``sim.set_state_from_flattened(state)``
-   The MuJoCo library’s computation is deterministic given a specific input, as explained in the :ref:`Programming
-   section <Simulation>`. mujoco-py implements methods for getting and setting some of the relevant fields (and
-   similarly ``dm_control.Physics`` offers methods that correspond to the flattened case). ``mujoco`` do not offer such
-   abstraction, and the user is expected to get/set the values of the relevant fields explicitly.
-
-``sim.model.get_joint_qvel_addr(joint_name)``
-   This is a convenience method in mujoco-py that returns a list of contiguous indices corresponding to this joint. The
-   list starts from ``model.jnt_qposadr[joint_index]``, and its length depends on the joint type. ``mujoco`` doesn't
-   offer this functionality, but this list can be easily constructed using ``model.jnt_qposadr[joint_index]`` and
-   ``xrange``.
-
-``sim.model.*_name2id(name)``
-   mujoco-py creates dicts in ``MjSim`` that allow for efficient lookup of indices for objects of different types:
-   ``site_name2id``, ``body_name2id`` etc. These functions replace the function :ref:`mujoco.mj_name2id(model,
-   type_enum, name) <mj_name2id>`. ``mujoco`` offers a different approach for using entity names – :ref:`named access
-   <PyNamed>`, as well as access to the native :ref:`mj_name2id`.
-
-``sim.save(fstream, format_name)``
-   This is the one context in which the MuJoCo library (and therefore also ``mujoco``) is stateful: it holds a copy in
-   memory of the last XML that was compiled, which is used in :ref:`mujoco.mj_saveLastXML(fname) <mj_saveLastXML>`. Note
-   that mujoco-py’s implementation has a convenient extra feature, whereby the pose (as determined by ``sim.data``’s
-   state) is transformed to a keyframe that’s added to the model before saving.  This extra feature is not currently
-   available in ``mujoco``.
-
-
 .. _PyBuild:
 
 Building from source
@@ -609,7 +524,9 @@ Building from source
    .. code-block:: shell
 
       cd dist
-      MUJOCO_PATH=/PATH/TO/MUJOCO MUJOCO_PLUGIN_PATH=/PATH/TO/MUJOCO_PLUGIN pip install mujoco-x.y.z.tar.gz
+      MUJOCO_PATH=/PATH/TO/MUJOCO
+      MUJOCO_PLUGIN_PATH=/PATH/TO/MUJOCO_PLUGIN
+      pip install mujoco-x.y.z.tar.gz
 
 The Python bindings should now be installed! To check that they've been
 successfully installed, ``cd`` outside of the ``mujoco`` directory and run
@@ -619,3 +536,131 @@ successfully installed, ``cd`` outside of the ``mujoco`` directory and run
    As a reference, a working build configuration can be found in MuJoCo's
    `continuous integration setup <https://github.com/google-deepmind/mujoco/blob/main/.github/workflows/build.yml>`_ on
    GitHub.
+
+
+.. _PyModule:
+
+Modules
+=======
+
+The ``mujoco`` package contains two sub-modules: ``mujoco.rollout`` and ``mujoco.minimize``
+
+.. _PyRollout:
+
+rollout
+-------
+
+``mujoco.rollout`` shows how to add additional C/C++ functionality, exposed as a Python module via pybind11. It is
+implemented in `rollout.cc <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout.cc>`__
+and wrapped in `rollout.py <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout.py>`__. The module
+performs a common functionality where tight loops implemented outside of Python are beneficial: rolling out a trajectory
+(i.e., calling :ref:`mj_step` in a loop), given an intial state and sequence of controls, and returning subsequent
+states and sensor values. The basic usage form is
+
+.. code-block:: python
+
+   state, sensordata = rollout.rollout(model, data, initial_state, control)
+
+``initial_state`` is an ``nroll x nstate`` array, with ``nroll`` initial states of size ``nstate``, where
+``nstate = mj_stateSize(model, mjtState.mjSTATE_FULLPHYSICS)`` is the size of the
+:ref:`full physics state<geFullPhysics>`. ``control`` is a ``nroll x nstep x ncontrol`` array of controls. Controls are
+by default the ``mjModel.nu`` standard actuators, but any combination of :ref:`user input<geInput>` arrays can be
+specified by passing an optional ``control_spec`` bitflag.
+
+If a rollout diverges, the current state and sensor values are used to fill the remainder of the trajectory.
+Therefore, non-increasing time values can be used to detect diverged rollouts.
+
+The ``rollout`` function is designed to be completely stateless, so all inputs of the stepping pipeline are set and any
+values already present in the given ``MjData`` instance will have no effect on the output.
+
+Since the Global Interpreter Lock can be released, this function can be efficiently threaded using Python threads. See
+the ``test_threading`` function in
+`rollout_test.py <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout_test.py>`__ for an example
+of threaded operation (and more generally for usage examples).
+
+.. _PyMinimize:
+
+minimize
+--------
+
+This module contains optimization-related utilities.
+
+The ``minimize.least_squares()`` function implements a nonlinear Least Squares optimizer solving sequential
+Quadratic Programs with :ref:`mju_boxQP`. It is documented in the associated notebook: |lscolab|
+
+.. |lscolab| image:: https://colab.research.google.com/assets/colab-badge.svg
+             :target: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/least_squares.ipynb
+
+
+
+
+.. _PyUtility:
+
+Utilities
+=========
+
+The `python/mujoco <https://github.com/google-deepmind/mujoco/tree/main/python/mujoco>`__ directory also contains
+utility scripts.
+
+
+.. _PyMsh2obj:
+
+msh2obj.py
+----------
+
+The `msh2obj.py <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/msh2obj.py>`__ script converts the
+:ref:`legacy .msh format<legacy-msh-docs>` for surface meshes (different from the possibly-volumetric
+:ref:`gmsh format<gmsh-file-docs>` also using .msh), to OBJ files. The legacy format is depricated and will be removed
+in a future release. Please convert all legacy files to OBJ.
+
+
+
+.. _PyMjpy_migration:
+
+mujoco-py migration
+===================
+
+In mujoco-py, the main entry point is the `MjSim <https://github.com/openai/mujoco-py/blob/master/mujoco_py/mjsim.pyx>`_
+class.  Users construct a stateful ``MjSim`` instance from an MJCF model (similar to ``dm_control.Physics``), and this
+instance holds references to an ``mjModel`` instance and its associated ``mjData``.  In contrast, the MuJoCo Python
+bindings (``mujoco``) take a more low-level approach, as explained above: following the design principle of the C
+library, the ``mujoco`` module itself is stateless, and merely wraps the underlying native structs and functions.
+
+While a complete survey of mujoco-py is beyond the scope of this document, we offer below implementation notes for a
+non-exhaustive list of specific mujoco-py features:
+
+``mujoco_py.load_model_from_xml(bstring)``
+   This factory function constructs a stateful ``MjSim`` instance. When using ``mujoco``, the user should call the
+   factory function ``mujoco.MjModel.from_xml_*`` as described :ref:`above <PyStructs>`. The user is then responsible
+   for holding the resulting ``MjModel`` struct instance and explicitly generating the corresponding ``MjData`` by
+   calling ``mujoco.MjData(model)``.
+
+``sim.reset()``, ``sim.forward()``, ``sim.step()``
+   Here as above, ``mujoco`` users needs to call the underlying library functions, passing instances of ``MjModel`` and
+   ``MjData``: :ref:`mujoco.mj_resetData(model, data) <mj_resetData>`, :ref:`mujoco.mj_forward(model, data)
+   <mj_forward>`, and :ref:`mujoco.mj_step(model, data) <mj_step>`.
+
+``sim.get_state()``, ``sim.set_state(state)``, ``sim.get_flattened_state()``, ``sim.set_state_from_flattened(state)``
+   The MuJoCo library’s computation is deterministic given a specific input, as explained in the :ref:`Programming
+   section <Simulation>`. mujoco-py implements methods for getting and setting some of the relevant fields (and
+   similarly ``dm_control.Physics`` offers methods that correspond to the flattened case). ``mujoco`` do not offer such
+   abstraction, and the user is expected to get/set the values of the relevant fields explicitly.
+
+``sim.model.get_joint_qvel_addr(joint_name)``
+   This is a convenience method in mujoco-py that returns a list of contiguous indices corresponding to this joint. The
+   list starts from ``model.jnt_qposadr[joint_index]``, and its length depends on the joint type. ``mujoco`` doesn't
+   offer this functionality, but this list can be easily constructed using ``model.jnt_qposadr[joint_index]`` and
+   ``xrange``.
+
+``sim.model.*_name2id(name)``
+   mujoco-py creates dicts in ``MjSim`` that allow for efficient lookup of indices for objects of different types:
+   ``site_name2id``, ``body_name2id`` etc. These functions replace the function :ref:`mujoco.mj_name2id(model,
+   type_enum, name) <mj_name2id>`. ``mujoco`` offers a different approach for using entity names – :ref:`named access
+   <PyNamed>`, as well as access to the native :ref:`mj_name2id`.
+
+``sim.save(fstream, format_name)``
+   This is the one context in which the MuJoCo library (and therefore also ``mujoco``) is stateful: it holds a copy in
+   memory of the last XML that was compiled, which is used in :ref:`mujoco.mj_saveLastXML(fname) <mj_saveLastXML>`. Note
+   that mujoco-py’s implementation has a convenient extra feature, whereby the pose (as determined by ``sim.data``’s
+   state) is transformed to a keyframe that’s added to the model before saving.  This extra feature is not currently
+   available in ``mujoco``.

doc/unity.rst

@@ -30,14 +30,14 @@ _____
 
 The MuJoCo app needs to be run at least once before the native library can be used, in order to register the library as
 a trusted binary. Then, copy the dynamic library file from
-``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.1.5.dylib`` (it can be
+``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.1.6.dylib`` (it can be
 found by browsing the contents of ``MuJoCo.app``) and rename it as ``mujoco.dylib``.
 
 Linux
 _____
 
 Expand the ``tar.gz`` archive to ``~/.mujoco``. Then copy the dynamic library from
-``~/.mujoco/mujoco-3.1.5/lib/libmujoco.so.3.1.5`` and rename it as ``libmujoco.so``.
+``~/.mujoco/mujoco-3.1.6/lib/libmujoco.so.3.1.6`` and rename it as ``libmujoco.so``.
 
 Windows
 _______

include/mujoco/mjdata.h

@@ -110,7 +110,7 @@ struct mjContact_ {                // result of collision detection functions
 
   // internal storage used by solver
   mjtNum  mu;                      // friction of regularized cone, set by mj_makeConstraint
-  mjtNum  H[36];                   // cone Hessian, set by mj_updateConstraint
+  mjtNum  H[36];                   // cone Hessian, set by mj_constraintUpdate
 
   // contact descriptors set by mj_collideXXX
   int     dim;                     // contact space dimensionality: 1, 3, 4 or 6

include/mujoco/mjmodel.h

@@ -327,6 +327,11 @@ typedef enum mjtSensor_ {         // type of sensor
   mjSENS_SUBTREELINVEL,           // 3D linear velocity of subtree
   mjSENS_SUBTREEANGMOM,           // 3D angular momentum of subtree
 
+  // sensors for geometric distance; attached to geoms or bodies
+  mjSENS_GEOMDIST,                // signed distance between two geoms
+  mjSENS_GEOMNORMAL,              // normal direction between two geoms
+  mjSENS_GEOMFROMTO,              // segment between two geoms
+
   // global sensors
   mjSENS_CLOCK,                   // simulation time
 

include/mujoco/mujoco.h

@@ -24,7 +24,7 @@ extern "C" {
 #endif
 
 // header version; should match the library version as returned by mj_version()
-#define mjVERSION_HEADER 315
+#define mjVERSION_HEADER 316
 
 // needed to define size_t, fabs and log10
 #include <stdlib.h>
@@ -464,6 +464,10 @@ MJAPI void mj_objectVelocity(const mjModel* m, const mjData* d,
 MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d,
                                  int objtype, int objid, mjtNum res[6], int flg_local);
 
+// Returns smallest signed distance between two geoms and optionally segment from geom1 to geom2.
+MJAPI mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2,
+                             mjtNum distmax, mjtNum fromto[6]);
+
 // Extract 6D force:torque given contact id, in the contact frame.
 MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]);
 
@@ -684,13 +688,13 @@ MJAPI void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* opt,
                         const mjvPerturb* pert, int catmask, mjvScene* scn);
 
 // Make list of lights.
-MJAPI void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn);
+MJAPI void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn);
 
 // Update camera.
-MJAPI void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn);
+MJAPI void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScene* scn);
 
 // Update skins.
-MJAPI void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn);
+MJAPI void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn);
 
 
 //---------------------------------- OpenGL rendering ----------------------------------------------

introspect/enums.py

@@ -338,9 +338,12 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjSENS_SUBTREECOM', 34),
              ('mjSENS_SUBTREELINVEL', 35),
              ('mjSENS_SUBTREEANGMOM', 36),
-             ('mjSENS_CLOCK', 37),
+             ('mjSENS_GEOMDIST', 37),
-             ('mjSENS_PLUGIN', 38),
+             ('mjSENS_GEOMNORMAL', 38),
-             ('mjSENS_USER', 39),
+             ('mjSENS_GEOMFROMTO', 39),
+             ('mjSENS_CLOCK', 40),
+             ('mjSENS_PLUGIN', 41),
+             ('mjSENS_USER', 42),
          ]),
      )),
     ('mjtStage',

introspect/functions.py

@@ -2743,6 +2743,45 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation.',  # pylint: disable=line-too-long
      )),
+    ('mj_geomDistance',
+     FunctionDecl(
+         name='mj_geomDistance',
+         return_type=ValueType(name='mjtNum'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='m',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjModel', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='d',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjData', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='geom1',
+                 type=ValueType(name='int'),
+             ),
+             FunctionParameterDecl(
+                 name='geom2',
+                 type=ValueType(name='int'),
+             ),
+             FunctionParameterDecl(
+                 name='distmax',
+                 type=ValueType(name='mjtNum'),
+             ),
+             FunctionParameterDecl(
+                 name='fromto',
+                 type=ArrayType(
+                     inner_type=ValueType(name='mjtNum'),
+                     extents=(6,),
+                 ),
+             ),
+         ),
+         doc='Returns smallest signed distance between two geoms and optionally segment from geom1 to geom2.',  # pylint: disable=line-too-long
+     )),
     ('mj_contactForce',
      FunctionDecl(
          name='mj_contactForce',
@@ -4439,7 +4478,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(
@@ -4465,7 +4504,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(
@@ -4497,7 +4536,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(

introspect/structs.py

@@ -4011,7 +4011,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                      inner_type=ValueType(name='mjtNum'),
                      extents=(36,),
                  ),
-                 doc='cone Hessian, set by mj_updateConstraint',
+                 doc='cone Hessian, set by mj_constraintUpdate',
              ),
              StructFieldDecl(
                  name='dim',

mjx/mujoco/mjx/_src/collision_convex.py

@@ -14,8 +14,9 @@
 # ==============================================================================
 """Convex collisions."""
 
+from collections.abc import Callable
 import functools
-from typing import Tuple
+from typing import Tuple, Union
 
 import jax
 from jax import numpy as jp
@@ -26,16 +27,19 @@ from mujoco.mjx._src.collision_types import Collision
 from mujoco.mjx._src.collision_types import ConvexInfo
 from mujoco.mjx._src.collision_types import FunctionKey
 from mujoco.mjx._src.collision_types import GeomInfo
+from mujoco.mjx._src.collision_types import HFieldInfo
 from mujoco.mjx._src.types import Data
 from mujoco.mjx._src.types import GeomType
 from mujoco.mjx._src.types import Model
 # pylint: enable=g-importing-member
 
+_GeomInfo = Union[GeomInfo, ConvexInfo]
+
 
 def collider(ncon: int):
   """Wraps collision functions for use by collision_driver."""
 
-  def wrapper(func):
+  def wrapper(collision_fn):
     def collide(
         m: Model, d: Data, key: FunctionKey, geom: jax.Array
     ) -> Collision:
@@ -45,18 +49,25 @@ def collider(ncon: int):
           GeomInfo(d.geom_xpos[g2], d.geom_xmat[g2], m.geom_size[g2]),
       ]
       in_axes = [0, 0]
+      fn = collision_fn
       for i in [0, 1]:
         if key.types[i] == GeomType.BOX:
           infos[i] = mesh.box(infos[i])
           in_axes[i] = jax.tree_util.tree_map(lambda x: None, infos[i]).replace(
-              pos=0, mat=0, face=0, vert=0
+              pos=0, mat=0, size=0, face=0, vert=0
           )
         elif key.types[i] == GeomType.MESH:
-          infos[i] = mesh.convex(m, key.data_ids[i], infos[i])
+          c, cm = infos[i], m.mesh_convex[key.data_ids[i]]
+          infos[i] = ConvexInfo(**vars(c), **vars(cm))
           in_axes[i] = jax.tree_util.tree_map(lambda x: None, infos[i]).replace(
-              pos=0, mat=0
+              pos=0, mat=0, size=0
           )
-      dist, pos, frame = jax.vmap(func, in_axes=in_axes)(*infos)
+        elif key.types[i] == GeomType.HFIELD:
+          hfield_info = mesh.hfield(m, key.data_ids[i])
+          infos[i] = hfield_info.replace(pos=infos[i].pos, mat=infos[i].mat)
+          in_axes[i] = hfield_info.replace(pos=0, mat=0, data=None)
+          fn = functools.partial(fn, subgrid_size=key.subgrid_size)
+      dist, pos, frame = jax.vmap(fn, in_axes=in_axes)(*infos)
       if ncon > 1:
         return jax.tree_util.tree_map(jp.concatenate, (dist, pos, frame))
       return dist, pos, frame
@@ -242,9 +253,8 @@ def plane_convex(plane: GeomInfo, convex: ConvexInfo) -> Collision:
   return dist, pos, frame
 
 
-@collider(ncon=1)
+def _sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
-def sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
+  """Calculates contact between a sphere and a convex mesh."""
-  """Calculates contact between a sphere and a convex object."""
   faces = convex.face
   normals = convex.face_normal
 
@@ -276,7 +286,7 @@ def sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
       face_normal,
   )
   edge_dist = jax.vmap(
-      lambda plane_pt, plane_norm: (pt - plane_pt).dot(plane_norm)
+      lambda plane_pt, plane_norm, pt=pt: (pt - plane_pt).dot(plane_norm)
   )(edge_p0, side_normals)
   pt_on_face = jp.all(edge_dist <= 0)  # lte to handle degenerate edges
 
@@ -291,7 +301,8 @@ def sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
 
   # Get the normal, dist, and contact position.
   pt_normal, d = math.normalize_with_norm(pt - sphere_pos)
-  # Ensure normal points towards convex centroid.
+  # Ensure normal points towards convex centroid. Assume convex centroid is at
+  # the origin.
   inside = jp.dot(pt, pt_normal) > 0
   sign = jp.where(inside, -1, 1)
   n = jp.where(pt_on_face | (d < 1e-6), -face_normal, sign * pt_normal)
@@ -305,11 +316,17 @@ def sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
   n = convex.mat @ n
   pos = convex.mat @ pos + convex.pos
 
+  return dist, pos, n
+
+
+@collider(ncon=1)
+def sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
+  """Calculates contact between a sphere and a convex mesh."""
+  dist, pos, n = _sphere_convex(sphere, convex)
   return dist, pos, math.make_frame(n)
 
 
-@collider(ncon=2)
+def _capsule_convex(cap: GeomInfo, convex: ConvexInfo) -> Collision:
-def capsule_convex(cap: GeomInfo, convex: ConvexInfo) -> Collision:
   """Calculates contacts between a capsule and a convex object."""
   # Get convex transformed normals, faces, and vertices.
   faces = convex.face
@@ -431,6 +448,13 @@ def capsule_convex(cap: GeomInfo, convex: ConvexInfo) -> Collision:
   dist = -jp.where(
       has_edge_contact, jp.array([edge_penetration, -1]), face_penetration
   )
+  return dist, pos, n
+
+
+@collider(ncon=2)
+def capsule_convex(cap: GeomInfo, convex: ConvexInfo) -> Collision:
+  """Calculates contacts between a capsule and a convex object."""
+  dist, pos, n = _capsule_convex(cap, convex)
   frame = jax.vmap(math.make_frame)(n)
   return dist, pos, frame
 
@@ -851,6 +875,7 @@ def _sat_gaussmap(
       incident_face_norm,
       -best_axis,
   )
+  dist = jp.where(is_face_separating, 1.0, dist)
 
   # Handle edge separating axes by checking all edge pairs.
   a_idx = jp.tile(jp.arange(edges_a.shape[0]), reps=edges_b.shape[0])
@@ -891,8 +916,9 @@ def _sat_gaussmap(
   best_edge_idx = edge_dist.argmax()
   best_edge_dist = edge_dist[best_edge_idx]
   is_edge_contact = jp.where(
-      dist.max() < 0, best_edge_dist > dist.max() - 1e-6,
+      dist.max() < 0.0,
-      (best_edge_dist < 0) & ~jp.isinf(best_edge_dist)
+      best_edge_dist > dist.max() - 1e-6,
+      (best_edge_dist < 0) & ~jp.isinf(best_edge_dist),
   )
   is_edge_contact = is_edge_contact & ~is_face_separating
   normal = jp.where(is_edge_contact, edge_axes[best_edge_idx], normal)
@@ -911,9 +937,45 @@ def _sat_gaussmap(
   return dist, pos, normal
 
 
-@collider(ncon=4)
+def _box_box(b1: ConvexInfo, b2: ConvexInfo) -> Collision:
-def convex_convex(c1: ConvexInfo, c2: ConvexInfo) -> Collision:
+  """Calculates contacts between two boxes."""
-  """Calculates contacts between two convex objects."""
+  faces1 = b1.face
+  faces2 = b2.face
+
+  to_local_pos = b2.mat.T @ (b1.pos - b2.pos)
+  to_local_mat = b2.mat.T @ b1.mat
+
+  faces1 = to_local_pos + faces1 @ to_local_mat.T
+  normals1 = b1.face_normal @ to_local_mat.T
+  normals2 = b2.face_normal
+
+  vertices1 = to_local_pos + b1.vert @ to_local_mat.T
+  vertices2 = b2.vert
+
+  unique_edges1 = jp.take(vertices1, b1.edge_dir, axis=0)
+  unique_edges2 = jp.take(vertices2, b2.edge_dir, axis=0)
+
+  # brute-force SAT is more performant for box-box
+  dist, pos, normal = _sat_bruteforce(
+      faces1,
+      faces2,
+      vertices1,
+      vertices2,
+      normals1,
+      normals2,
+      unique_edges1,
+      unique_edges2,
+  )
+
+  # Go back to world frame.
+  pos = b2.pos + pos @ b2.mat.T
+  n = normal @ b2.mat.T
+
+  return dist, pos, n
+
+
+def _convex_convex(c1: ConvexInfo, c2: ConvexInfo) -> Collision:
+  """Calculates contacts between two convex meshes."""
   # pad face vertices so that we can broadcast between geom1 and geom2
   # face has shape (n_face, n_vert, 3)
   nvert1, nvert2 = c1.face.shape[1], c2.face.shape[1]
@@ -932,6 +994,7 @@ def convex_convex(c1: ConvexInfo, c2: ConvexInfo) -> Collision:
   faces1 = c1.face
   faces2 = c2.face
 
+  # convert to c2 frame
   to_local_pos = c2.mat.T @ (c1.pos - c2.pos)
   to_local_mat = c2.mat.T @ c1.mat
 
@@ -942,49 +1005,209 @@ def convex_convex(c1: ConvexInfo, c2: ConvexInfo) -> Collision:
   vertices1 = to_local_pos + c1.vert @ to_local_mat.T
   vertices2 = c2.vert
 
-  unique_edges1 = jp.take(vertices1, c1.edge_dir, axis=0)
-  unique_edges2 = jp.take(vertices2, c2.edge_dir, axis=0)
-
   edges1 = jp.take(vertices1, c1.edge, axis=0)
   edges2 = jp.take(vertices2, c2.edge, axis=0)
 
   edge_face_normals1 = c1.edge_face_normal @ to_local_mat.T
   edge_face_normals2 = c2.edge_face_normal
 
-  enable_bruteforce = (
+  dist, pos, normal = _sat_gaussmap(
-      unique_edges1.shape[0] * unique_edges2.shape[0]
+      to_local_pos,
-      < edges1[0].shape[0] * edges2[0].shape[0]
+      faces1,
+      faces2,
+      vertices1,
+      vertices2,
+      normals1,
+      normals2,
+      edges1,
+      edges2,
+      edge_face_normals1,
+      edge_face_normals2,
   )
-  if enable_bruteforce:
-    dist, pos, normal = _sat_bruteforce(
-        faces1,
-        faces2,
-        vertices1,
-        vertices2,
-        normals1,
-        normals2,
-        unique_edges1,
-        unique_edges2,
-    )
-  else:
-    dist, pos, normal = _sat_gaussmap(
-        to_local_pos,
-        faces1,
-        faces2,
-        vertices1,
-        vertices2,
-        normals1,
-        normals2,
-        edges1,
-        edges2,
-        edge_face_normals1,
-        edge_face_normals2,
-    )
 
   # Go back to world frame.
   pos = c2.pos + pos @ c2.mat.T
-  normal = normal @ c2.mat.T
+  n = normal @ c2.mat.T
-  normal = -normal if swapped else normal
+  n = -n if swapped else n
-  frame = jax.vmap(math.make_frame)(normal)
 
+  return dist, pos, n
+
+
+@collider(ncon=4)
+def box_box(b1: ConvexInfo, b2: ConvexInfo) -> Collision:
+  """Calculates contacts between two boxes."""
+  dist, pos, n = _box_box(b1, b2)
+  frame = jax.vmap(math.make_frame)(n)
   return dist, pos, frame
+
+
+@collider(ncon=4)
+def convex_convex(c1: ConvexInfo, c2: ConvexInfo) -> Collision:
+  """Calculates contacts between two convex objects."""
+  dist, pos, n = _convex_convex(c1, c2)
+  frame = jax.vmap(math.make_frame)(n)
+  return dist, pos, frame
+
+
+def _hfield_collision(
+    collider_fn: Callable[[_GeomInfo, _GeomInfo], Collision],
+    h: HFieldInfo,
+    obj: _GeomInfo,
+    obj_rbound: jax.Array,
+    subgrid_size: Tuple[int, int],
+) -> Collision:
+  """Collides an object with prisms in a height field."""
+  # put obj in hfield frame
+  obj_pos = h.mat.T @ (obj.pos - h.pos)
+  obj_mat = h.mat.T @ obj.mat
+
+  xmin = obj_pos[0] - obj_rbound
+  ymin = obj_pos[1] - obj_rbound
+  cmin = jp.floor((xmin + h.size[0]) / (2 * h.size[0]) * (h.ncol - 1))
+  cmin = cmin.astype(int)
+  rmin = jp.floor((ymin + h.size[1]) / (2 * h.size[1]) * (h.nrow - 1))
+  rmin = rmin.astype(int)
+
+  # compute real-valued grid step
+  dx = 2.0 * h.size[0] / (h.ncol - 1)
+  dy = 2.0 * h.size[1] / (h.nrow - 1)
+
+  # set zbottom value using base size
+  bvert = jp.array([0.0, 0.0, -h.size[3]])
+  bmask = jp.array([True, True, False])
+
+  # process all prisms in sub-grid
+  prisms = []
+  for r in range(subgrid_size[1]):
+    for c in range(subgrid_size[0]):
+      ri, ci = rmin + r, cmin + c
+
+      # ensure ri, ci are in the bounds of the hfield
+      ri = jp.clip(ri, 0, h.nrow - 2)
+      ci = jp.clip(ci, 0, h.ncol - 2)
+
+      p1 = [
+          dx * ci - h.size[0],
+          dy * ri - h.size[1],
+          h.data[ci, ri] * h.size[2],
+      ]
+      p2 = [
+          dx * (ci + 1) - h.size[0],
+          dy * (ri + 1) - h.size[1],
+          h.data[ci + 1, ri + 1] * h.size[2],
+      ]
+      p3 = [
+          dx * ci - h.size[0],
+          dy * (ri + 1) - h.size[1],
+          h.data[ci, ri + 1] * h.size[2],
+      ]
+      top = jp.array([p1, p2, p3])
+      bottom = jp.array([p1, p3, p2]) * bmask + bvert
+      vert = jp.concatenate([bottom, top])
+      prisms.append(mesh.hfield_prism(vert))
+
+      p3 = p2
+      p2 = [
+          dx * (ci + 1) - h.size[0],
+          dy * ri - h.size[1],
+          h.data[ci + 1, ri] * h.size[2],
+      ]
+      top = jp.array([p1, p2, p3])
+      bottom = jp.array([p1, p3, p2]) * bmask + bvert
+      vert = jp.concatenate([bottom, top])
+      # NB: If the order of verts is updated above, the corresponding
+      # hfield_prism function must be updated to ensure that all faces have the
+      # correct winding order.
+      prisms.append(mesh.hfield_prism(vert))
+
+  n_prisms = len(prisms)
+  prisms = jax.tree_util.tree_map(lambda *x: jp.stack(x), *prisms)
+  dist, pos, n = jax.vmap(collider_fn, in_axes=[None, 0])(
+      obj.replace(pos=obj_pos, mat=obj_mat), prisms
+  )
+
+  dist = dist.flatten()
+  pos = pos.reshape((-1, 3))
+  n = n.reshape((-1, 3))
+  n *= -1  # flip the normal since we flipped args in the call to collider_fn
+
+  # Check that we're in the half-space of the hfield norm. If not, pick the top
+  # face norm. This resolves issues with cracks of doom.
+  n_repeats = dist.shape[0] // n_prisms
+  top_norm = jp.repeat(prisms.face_normal[:, 1], n_repeats, axis=0)
+  cond = jax.vmap(jp.dot, in_axes=[0, None])(n, h.mat[2]) < 1e-6
+  n = jp.where(cond[:, None], top_norm, n)
+
+  return dist, pos, n
+
+
+@collider(ncon=4)
+def hfield_sphere(
+    h: HFieldInfo, s: GeomInfo, subgrid_size: Tuple[int, int]
+) -> Collision:
+  """Calculates contacts between a hfield and a sphere."""
+  rbound = jp.max(s.size)
+  dist, pos, n = _hfield_collision(_sphere_convex, h, s, rbound, subgrid_size)
+
+  n_mean = jp.mean(n, axis=0)
+  mask = dist < jp.minimum(0, dist.min() + 1e-3)
+  idx = _manifold_points(pos, mask, n_mean)
+  dist, pos, n = dist[idx], pos[idx], n[idx]
+
+  # zero out non-unique contacts
+  unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1
+  dist = jp.where(unique, dist, 1)
+
+  # back to world frame, _hfield_collision returns collision in hfield frame
+  pos = jax.vmap(lambda p: h.mat @ p + h.pos)(pos)
+  n = jax.vmap(lambda n: h.mat @ n)(n)
+
+  return dist, pos, jax.vmap(math.make_frame)(n)
+
+
+@collider(ncon=4)
+def hfield_capsule(
+    h: HFieldInfo, c: GeomInfo, subgrid_size: Tuple[int, int]
+) -> Collision:
+  """Calculates contacts between a hfield and a capsule."""
+  rbound = c.size[0] + c.size[1]
+  dist, pos, n = _hfield_collision(_capsule_convex, h, c, rbound, subgrid_size)
+
+  n_mean = jp.mean(n, axis=0)
+  mask = dist < jp.minimum(0, dist.min() + 1e-3)
+  idx = _manifold_points(pos, mask, n_mean)
+  dist, pos, n = dist[idx], pos[idx], n[idx]
+
+  # zero out non-unique contacts
+  unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1
+  dist = jp.where(unique, dist, 1)
+
+  # back to world frame, _hfield_collision returns collision in hfield frame
+  pos = jax.vmap(lambda p: h.mat @ p + h.pos)(pos)
+  n = jax.vmap(lambda n: h.mat @ n)(n)
+
+  return dist, pos, jax.vmap(math.make_frame)(n)
+
+
+@collider(ncon=4)
+def hfield_convex(
+    h: HFieldInfo, c: ConvexInfo, subgrid_size: Tuple[int, int]
+) -> Collision:
+  """Calculates contacts between a hfield and a capsule."""
+  rbound = jp.max(c.size)
+  dist, pos, n = _hfield_collision(_convex_convex, h, c, rbound, subgrid_size)
+
+  n_mean = jp.mean(n, axis=0)
+  mask = dist < jp.minimum(0, dist.min() + 1e-3)
+  idx = _manifold_points(pos, mask, n_mean)
+  dist, pos, n = dist[idx], pos[idx], n[idx]
+
+  # zero out non-unique contacts
+  unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1
+  dist = jp.where(unique, dist, 1)
+
+  # back to world frame, _hfield_collision returns collision in hfield frame
+  pos = jax.vmap(lambda p: h.mat @ p + h.pos)(pos)
+  n = jax.vmap(lambda n: h.mat @ n)(n)
+
+  return dist, pos, jax.vmap(math.make_frame)(n)

mjx/mujoco/mjx/_src/collision_driver.py

@@ -45,8 +45,12 @@ from jax import numpy as jp
 import mujoco
 from mujoco.mjx._src import support
 # pylint: disable=g-importing-member
+from mujoco.mjx._src.collision_convex import box_box
 from mujoco.mjx._src.collision_convex import capsule_convex
 from mujoco.mjx._src.collision_convex import convex_convex
+from mujoco.mjx._src.collision_convex import hfield_capsule
+from mujoco.mjx._src.collision_convex import hfield_convex
+from mujoco.mjx._src.collision_convex import hfield_sphere
 from mujoco.mjx._src.collision_convex import plane_convex
 from mujoco.mjx._src.collision_convex import sphere_convex
 from mujoco.mjx._src.collision_primitive import capsule_capsule
@@ -78,6 +82,10 @@ _COLLISION_FUNC = {
     (GeomType.PLANE, GeomType.ELLIPSOID): plane_ellipsoid,
     (GeomType.PLANE, GeomType.CYLINDER): plane_cylinder,
     (GeomType.PLANE, GeomType.MESH): plane_convex,
+    (GeomType.HFIELD, GeomType.SPHERE): hfield_sphere,
+    (GeomType.HFIELD, GeomType.CAPSULE): hfield_capsule,
+    (GeomType.HFIELD, GeomType.BOX): hfield_convex,
+    (GeomType.HFIELD, GeomType.MESH): hfield_convex,
     (GeomType.SPHERE, GeomType.SPHERE): sphere_sphere,
     (GeomType.SPHERE, GeomType.CAPSULE): sphere_capsule,
     (GeomType.SPHERE, GeomType.BOX): sphere_convex,
@@ -90,7 +98,7 @@ _COLLISION_FUNC = {
     (GeomType.ELLIPSOID, GeomType.ELLIPSOID): ellipsoid_ellipsoid,
     (GeomType.ELLIPSOID, GeomType.CYLINDER): ellipsoid_cylinder,
     (GeomType.CYLINDER, GeomType.CYLINDER): cylinder_cylinder,
-    (GeomType.BOX, GeomType.BOX): convex_convex,
+    (GeomType.BOX, GeomType.BOX): box_box,
     (GeomType.BOX, GeomType.MESH): convex_convex,
     (GeomType.MESH, GeomType.MESH): convex_convex,
 }
@@ -210,6 +218,21 @@ def _geom_groups(
       condim = max(m.geom_condim[g1], m.geom_condim[g2])
 
     key = FunctionKey(types, data_ids, condim)
+
+    if types[0] == mujoco.mjtGeom.mjGEOM_HFIELD:
+      # add static grid bounds to the grouping key for hfield collisions
+      geom_rbound_hfield = (
+          m.geom_rbound_hfield if isinstance(m, Model) else m.geom_rbound
+      )
+      nrow, ncol = m.hfield_nrow[data_ids[0]], m.hfield_ncol[data_ids[0]]
+      xsize, ysize = m.hfield_size[data_ids[0]][:2]
+      xtick, ytick = (2 * xsize) / (ncol - 1), (2 * ysize) / (nrow - 1)
+      xbound = int(np.ceil(2 * geom_rbound_hfield[g2] / xtick)) + 1
+      xbound = min(xbound, ncol)
+      ybound = int(np.ceil(2 * geom_rbound_hfield[g2] / ytick)) + 1
+      ybound = min(ybound, nrow)
+      key = FunctionKey(types, data_ids, condim, (xbound, ybound))
+
     groups.setdefault(key, []).append((g1, g2, ip))
 
   return groups

mjx/mujoco/mjx/_src/collision_driver_test.py

@@ -721,6 +721,95 @@ class ConvexTest(absltest.TestCase):
     self.assertTrue((c.dist > 0).all())
 
 
+class HFieldTest(absltest.TestCase):
+  _HFIELD = """
+    <mujoco>
+      <asset>
+        <hfield name="J" size="0.9 1.1 .2 .1" nrow="8" ncol="10" elevation="
+                0 0 0 0 0 0 1 1 0 0
+                0 0 0 0 0 0 1 1 0 0
+                0 0 0 0 1 1 1 1 0 0
+                0 0 1 1 0 0 1 1 0 0
+                0 0 1 1 1 1 1 1 0 0
+                0 0 1 1 1 1 1 0 0 0
+                0 0 0 0 0 0 0 0 0 0
+                0 0 0 0 0 0 0 0 0 0"/>
+      </asset>
+      <worldbody>
+        <light pos="0 0 0.32"/>
+        <geom type="hfield" hfield="J"/>
+        <body pos="0 0 1">
+          <freejoint/>
+          <geom size="0.1" contype="0"/>
+        </body>
+        <body pos="0 0 0.2">
+          <freejoint/>
+          <geom type="capsule" size="0.01 0.1" contype="0"/>
+        </body>
+        <body pos="0 0 0.55">
+          <freejoint/>
+          <geom type="box" size="0.05 0.05 0.025" contype="0"/>
+        </body>
+      </worldbody>
+      <keyframe>
+        <key name="qpos1" qpos='-0.0127496 0.156995 0.118336 0.336325 -0.810241 0.442853 -0.185139 -0.19614 -0.000912274 0.112334 -0.455846 0.852431 -0.0871208 0.24078 0.124334 0.23346 0.100627 0.293376 -0.27087 0.855587 0.32944'/>
+        <key name="qpos2" qpos='0.0815885 -3.18397 -9.4802 0.57036 0.695092 -0.403995 -0.168298 -0.0156545 0.157173 -0.00734406 0.606091 -0.185052 0.759332 -0.147728 0.100088 -0.234066 0.224884 0.999906 0.000384 -9.62645e-05 -0.013677'/>
+      </keyframe>
+    </mujoco>
+  """
+
+  def test_sphere_hfield(self):
+    m = mujoco.MjModel.from_xml_string(self._HFIELD)
+    mx = mjx.put_model(m)
+
+    d = mujoco.MjData(m)
+    d.qpos[:] = m.keyframe('qpos1').qpos
+    dx = mjx.put_data(m, d)
+
+    collision_jit_fn = jax.jit(mjx.collision)
+    kinematics_jit_fn = jax.jit(mjx.kinematics)
+    dx = kinematics_jit_fn(mx, dx)
+    dx = collision_jit_fn(mx, dx)
+
+    # check that all geoms are colliding with the hfield
+    for geom_id in [1, 2, 3]:
+      mask = (dx.contact.geom == np.array([0, geom_id])).all(axis=1)
+      c = jax.tree_util.tree_map(lambda x, m=mask: x[m], dx.contact)
+      self.assertTrue((c.dist < 0).any())
+      self.assertTrue((c.dist > -1e-3).any())
+      # all contact normals are roughly pointing in the right direction
+      self.assertTrue((c.frame[:, 0].dot(np.array([0, 0, 1])) > 0.7).all())
+
+  def test_hfield_outside(self):
+    """Tests that objects outside of the hfield do not collide."""
+    positions = ['2.0 0', '-2.0 0', '0 -2.0', '0 2.0']
+    for p in positions:
+      xml = self._HFIELD.replace('<body pos="0 0', f'<body pos="{p}')
+      _, dx = _collide(xml)
+      self.assertTrue((dx.contact.dist >= 0).all())
+
+  def test_hfield_deep(self):
+    """Tests that objects with deep penetration do not get stuck."""
+    m = mujoco.MjModel.from_xml_string(self._HFIELD)
+    mx = mjx.put_model(m)
+
+    d = mujoco.MjData(m)
+    d.qpos[:] = m.keyframe('qpos2').qpos
+    dx = mjx.put_data(m, d)
+
+    collision_jit_fn = jax.jit(mjx.collision)
+    kinematics_jit_fn = jax.jit(mjx.kinematics)
+    dx = kinematics_jit_fn(mx, dx)
+    dx = collision_jit_fn(mx, dx)
+
+    # check that all geoms are colliding with the hfield
+    for geom_id in [1, 2, 3]:
+      mask = (dx.contact.geom == np.array([0, geom_id])).all(axis=1)
+      c = jax.tree_util.tree_map(lambda x, m=mask: x[m], dx.contact)
+      # all contact normals are in the top half-face of the hfield
+      self.assertTrue((c.frame[:, 0].dot(np.array([0, 0, 1])) > 0.7).all())
+
+
 class BodyPairFilterTest(absltest.TestCase):
   """Tests that certain body pairs get filtered."""
 

mjx/mujoco/mjx/_src/collision_sdf.py

@@ -122,8 +122,16 @@ def cylinder_jvp(primals, tangents):
   return primal_out, tangent_out
 
 
-def _to_local(f: SDFFn, pos: jax.Array, mat: jax.Array)-> SDFFn:
+def _from_to(
-  return lambda p: f(mat.T @ (p - pos))
+    f: SDFFn,
+    from_pos: jax.Array,
+    from_mat: jax.Array,
+    to_pos: jax.Array,
+    to_mat: jax.Array,
+) -> SDFFn:
+  relmat = math.matmul_unroll(to_mat.T, from_mat)
+  relpos = to_mat.T @ (from_pos - to_pos)
+  return lambda p: f(relmat @ p + relpos)
 
 
 def _intersect(d1: SDFFn, d2: SDFFn) -> SDFFn:
@@ -175,13 +183,16 @@ def _optim(
 ) -> Collision:
   """Optimizes the clearance function."""
   d1 = functools.partial(d1, size=info1.size)
-  d1 = _to_local(d1, info1.pos, info1.mat)
+  # evaluate d1 in d2 frame
+  d1 = _from_to(d1, info2.pos, info2.mat, info1.pos, info1.mat)
   d2 = functools.partial(d2, size=info2.size)
-  d2 = _to_local(d2, info2.pos, info2.mat)
+  x0 = info2.mat.T @ (x0 - info2.pos)
   fn = _clearance(d1, d2)
   _, pos = _gradient_descent(fn, x0, 10)
   dist = d1(pos) + d2(pos)
   n = jax.grad(d1)(pos) - jax.grad(d2)(pos)
+  pos = info2.mat @ pos + info2.pos  # d2 to global frame
+  n = info2.mat @ n
   return dist, pos, math.make_frame(n)
 
 
@@ -240,4 +251,3 @@ def cylinder_cylinder(c1: GeomInfo, c2: GeomInfo) -> Collision:
   ])
   optim_ = functools.partial(_optim, _cylinder, _cylinder, c1, c2)
   return jax.vmap(optim_)(x0)
-

mjx/mujoco/mjx/_src/collision_types.py

@@ -15,31 +15,10 @@
 """Collision base types."""
 
 import dataclasses
-from typing import Tuple
+from typing import Optional, Tuple
 import jax
-# pylint: disable=g-importing-member
+from mujoco.mjx._src.dataclasses import PyTreeNode  # pylint: disable=g-importing-member
-from mujoco.mjx._src.dataclasses import PyTreeNode
+import numpy as np
-# pylint: enable=g-importing-member
-
-
-class GeomInfo(PyTreeNode):
-  """Geom propertes of primitive and SDF shapes."""
-  pos: jax.Array
-  mat: jax.Array
-  size: jax.Array
-
-
-class ConvexInfo(PyTreeNode):
-  """Geom propertes of convex meshes."""
-  pos: jax.Array
-  mat: jax.Array
-  vert: jax.Array
-  face: jax.Array
-  face_normal: jax.Array
-  edge: jax.Array
-  edge_face_normal: jax.Array
-  edge_dir: jax.Array
-
 
 # Collision returned by collision functions:
 #  - distance          distance between nearest points; neg: penetration
@@ -48,18 +27,53 @@ class ConvexInfo(PyTreeNode):
 Collision = Tuple[jax.Array, jax.Array, jax.Array]
 
 
+class GeomInfo(PyTreeNode):
+  """Geom properties for primitive shapes."""
+
+  pos: jax.Array
+  mat: jax.Array
+  size: jax.Array
+
+
+class ConvexInfo(PyTreeNode):
+  """Geom properties for convex meshes."""
+
+  pos: jax.Array
+  mat: jax.Array
+  size: jax.Array
+  vert: jax.Array
+  face: jax.Array
+  face_normal: jax.Array
+  edge: jax.Array
+  edge_face_normal: jax.Array
+  edge_dir: Optional[jax.Array] = None
+
+
+class HFieldInfo(PyTreeNode):
+  """Geom properties for height fields."""
+
+  pos: jax.Array
+  mat: jax.Array
+  size: np.ndarray
+  nrow: int
+  ncol: int
+  data: jax.Array
+
+
 @dataclasses.dataclass(frozen=True)
 class FunctionKey:
   """Specifies how geom pairs group into collision_driver's function table.
 
   Attributes:
     types: geom type pair, which determines the collision function
-    data_ids: geom data id pair: mesh id for mesh geoms, otherwise -1.
+    data_ids: geom data id pair: mesh id for mesh geoms, otherwise -1. Meshes
-              Meshes have distinct face/vertex counts, so must occupy distinct
+      have distinct face/vertex counts, so must occupy distinct entries in the
-              entries in the collision function table.
+      collision function table.
     condim: grouping by condim of the colliision ensures that the size of the
-            resulting constraint jacobian is determined at compile time.
+      resulting constraint jacobian is determined at compile time.
+    subgrid_size: the size determines the hfield subgrid to collide with
   """
   types: Tuple[int, int]
   data_ids: Tuple[int, int]
   condim: int
+  subgrid_size: Tuple[int, int] = (-1, -1)

mjx/mujoco/mjx/_src/constraint.py

@@ -73,8 +73,8 @@ def _kbi(
   k = 1 / (dmax * dmax * timeconst * timeconst * dampratio * dampratio)
   b = 2 / (dmax * timeconst)
   # TODO(robotics-simulation): check various solparam settings in model gen test
-  k = jp.where(dampratio <= 0, -dampratio / (dmax * dmax), k)
+  k = jp.where(dampratio <= 0, -solref[0] / (dmax * dmax), k)
-  b = jp.where(timeconst <= 0, -timeconst / dmax, b)
+  b = jp.where(timeconst <= 0, -solref[1] / dmax, b)
 
   imp_x = jp.abs(pos) / width
   imp_a = (1.0 / jp.power(mid, power - 1)) * jp.power(imp_x, power)

mjx/mujoco/mjx/_src/constraint_test.py

@@ -104,10 +104,10 @@ class ConstraintTest(absltest.TestCase):
     ne, nf, nl, nc = constraint.counts(constraint.make_efc_type(m))
     self.assertEqual(ne, 0)
     self.assertEqual(nf, 0)
-    self.assertEqual(nl, 2)
+    self.assertEqual(nl, 3)
     self.assertEqual(nc, 148)
     dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m))
-    self.assertEqual(dx.efc_J.shape[0], 150)  # only joint range, contact
+    self.assertEqual(dx.efc_J.shape[0], 151)  # only joint range, contact
 
   def test_disable_contact(self):
     m = test_util.load_test_file('constraints.xml')
@@ -115,10 +115,10 @@ class ConstraintTest(absltest.TestCase):
     ne, nf, nl, nc = constraint.counts(constraint.make_efc_type(m))
     self.assertEqual(ne, 10)
     self.assertEqual(nf, 0)
-    self.assertEqual(nl, 2)
+    self.assertEqual(nl, 3)
     self.assertEqual(nc, 0)
     dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m))
-    self.assertEqual(dx.efc_J.shape[0], 12)  # only joint range, limit
+    self.assertEqual(dx.efc_J.shape[0], 13)  # only joint range, limit
 
 
 if __name__ == '__main__':

mjx/mujoco/mjx/_src/dataclasses.py

@@ -30,7 +30,7 @@ def _jax_in_args(typ) -> bool:
     return True
   if dataclasses.is_dataclass(typ):
     return any(_jax_in_args(f.type) for f in dataclasses.fields(typ))
-  if typing.get_origin(typ) in (list, dict, Union, set):
+  if typing.get_origin(typ) in (tuple, list, dict, Union, set):
     return any(_jax_in_args(t) for t in typing.get_args(typ))
   return False
 

mjx/mujoco/mjx/_src/io.py

@@ -22,6 +22,7 @@ from jax import numpy as jp
 import mujoco
 from mujoco.mjx._src import collision_driver
 from mujoco.mjx._src import constraint
+from mujoco.mjx._src import mesh
 from mujoco.mjx._src import support
 from mujoco.mjx._src import types
 import numpy as np
@@ -68,14 +69,16 @@ def put_model(m: mujoco.MjModel, device=None) -> types.Model:
   if m.ntendon:
     raise NotImplementedError('tendons are not supported')
 
+  mesh_geomid = set()
   for g1, g2, ip in collision_driver.geom_pairs(m):
     t1, t2 = m.geom_type[[g1, g2]]
     # check collision function exists for type pair
     if not collision_driver.has_collision_fn(t1, t2):
       t1, t2 = mujoco.mjtGeom(t1), mujoco.mjtGeom(t2)
       raise NotImplementedError(f'({t1}, {t2}) collisions not implemented.')
-    # margin/gap not supported for geoms
+    # margin/gap not supported for meshes and height fields
-    if mujoco.mjtGeom.mjGEOM_MESH in (t1, t2):
+    no_margin = {mujoco.mjtGeom.mjGEOM_MESH, mujoco.mjtGeom.mjGEOM_HFIELD}
+    if no_margin.intersection({t1, t2}):
       if ip != -1:
         margin = m.pair_margin[ip]
       else:
@@ -83,6 +86,9 @@ def put_model(m: mujoco.MjModel, device=None) -> types.Model:
       if margin.any():
         t1, t2 = mujoco.mjtGeom(t1), mujoco.mjtGeom(t2)
         raise NotImplementedError(f'({t1}, {t2}) margin/gap not implemented.')
+    for t, g in [(t1, g1), (t2, g2)]:
+      if t == mujoco.mjtGeom.mjGEOM_MESH:
+        mesh_geomid.add(g)
 
   for enum_field, enum_type, mj_type in (
       (m.actuator_biastype, types.BiasType, mujoco.mjtBias),
@@ -100,12 +106,24 @@ def put_model(m: mujoco.MjModel, device=None) -> types.Model:
   if not np.allclose(m.dof_frictionloss, 0):
     raise NotImplementedError('dof_frictionloss is not implemented.')
 
-  fields = {f.name: getattr(m, f.name) for f in types.Model.fields()}
+  mjx_only = {'mesh_convex', 'geom_rbound_hfield'}
+  mj_field_names = {f.name for f in types.Model.fields()} - mjx_only
+  fields = {f: getattr(m, f) for f in mj_field_names}
+  fields['geom_rbound_hfield'] = fields['geom_rbound']
   fields['geom_rgba'] = fields['geom_rgba'].reshape((-1, 4))
   fields['mat_rgba'] = fields['mat_rgba'].reshape((-1, 4))
   fields['cam_mat0'] = fields['cam_mat0'].reshape((-1, 3, 3))
   fields['opt'] = _make_option(m.opt)
   fields['stat'] = _make_statistic(m.stat)
+
+  # Pre-compile meshes for MJX collisions.
+  fields['mesh_convex'] = [None] * m.nmesh
+  for i in mesh_geomid:
+    dataid = m.geom_dataid[i]
+    if fields['mesh_convex'][dataid] is None:
+      fields['mesh_convex'][dataid] = mesh.convex(m, dataid)  # pytype: disable=unsupported-operands
+  fields['mesh_convex'] = tuple(fields['mesh_convex'])
+
   model = types.Model(**{k: copy.copy(v) for k, v in fields.items()})
 
   return jax.device_put(model, device=device)

mjx/mujoco/mjx/_src/mesh.py

@@ -16,14 +16,18 @@
 
 import collections
 import itertools
-from typing import Tuple
+from typing import Tuple, Union
 import warnings
 
 import jax
 from jax import numpy as jp
+import mujoco
+from mujoco.mjx._src import math
 # pylint: disable=g-importing-member
 from mujoco.mjx._src.collision_types import ConvexInfo
 from mujoco.mjx._src.collision_types import GeomInfo
+from mujoco.mjx._src.collision_types import HFieldInfo
+from mujoco.mjx._src.types import ConvexMesh
 from mujoco.mjx._src.types import Model
 # pylint: enable=g-importing-member
 import numpy as np
@@ -141,7 +145,9 @@ def _convex_hull_2d(points: np.ndarray, normal: np.ndarray) -> np.ndarray:
   return hull_point_idx
 
 
-def _merge_coplanar(m: Model, tm: trimesh.Trimesh, meshid: int) -> np.ndarray:
+def _merge_coplanar(
+    m: Union[mujoco.MjModel, Model], tm: trimesh.Trimesh, meshid: int
+) -> np.ndarray:
   """Merges coplanar facets."""
   if not tm.facets:
     return tm.faces.copy()  # no facets
@@ -221,6 +227,7 @@ def box(info: GeomInfo) -> ConvexInfo:
   c = ConvexInfo(
       info.pos,
       info.mat,
+      info.size,
       vert,
       face,
       face_normal,
@@ -236,22 +243,21 @@ def box(info: GeomInfo) -> ConvexInfo:
   return c
 
 
-def convex(m: Model, mesh_id: int, info: GeomInfo) -> ConvexInfo:
+def convex(m: Union[mujoco.MjModel, Model], data_id: int) -> ConvexMesh:
   """Processes a mesh for use in convex collision algorithms.
 
   Args:
     m: an MJX model
-    mesh_id: the mesh id to process
+    data_id: the mesh id to process
-    info: pos, mat, size of this geom
 
   Returns:
-    a convex mesh info
+    a convex mesh
   """
-  vert_beg = m.mesh_vertadr[mesh_id]
+  vert_beg = m.mesh_vertadr[data_id]
-  vert_end = m.mesh_vertadr[mesh_id + 1] if mesh_id < m.nmesh - 1 else None
+  vert_end = m.mesh_vertadr[data_id + 1] if data_id < m.nmesh - 1 else None
   vert = m.mesh_vert[vert_beg:vert_end]
 
-  graphadr = m.mesh_graphadr[mesh_id]
+  graphadr = m.mesh_graphadr[data_id]
   graph = m.mesh_graph[graphadr:]
   graph_idx = 0
 
@@ -273,21 +279,98 @@ def convex(m: Model, mesh_id: int, info: GeomInfo) -> ConvexInfo:
 
   tm_convex = trimesh.Trimesh(vertices=vert, faces=face)
   vert = np.array(tm_convex.vertices)
-  face = _merge_coplanar(m, tm_convex, mesh_id)
+  face = _merge_coplanar(m, tm_convex, data_id)
   face_normal = _get_face_norm(vert, face)
   edge, edge_face_normal = _get_edge_normals(face, face_normal)
-  edge_dir = _get_unique_edge_dir(vert, face)
   face = vert[face]  # materialize full nface x nvert matrix
 
-  c = ConvexInfo(
+  c = ConvexMesh(
-      info.pos,
-      info.mat,
       vert,
       face,
       face_normal,
       edge,
       edge_face_normal,
-      edge_dir,
   )
 
   return jax.tree_util.tree_map(jp.array, c)
+
+
+def hfield_prism(vert: jax.Array) -> ConvexInfo:
+  """Builds a hfield prism."""
+  # The first 3 vertices define the bottom triangle, and the next 3 vertices
+  # define the top triangle. The remaining triangles define the side of the
+  # prism.
+  face = np.array([
+      [0, 1, 2, 0],  # bottom
+      [3, 4, 5, 3],  # top
+      [0, 3, 5, 1],
+      [0, 2, 4, 3],
+      [2, 1, 5, 4],
+  ])
+  edges = np.array([
+      # bottom
+      [0, 1],
+      [1, 2],
+      [0, 2],
+      # top
+      [3, 4],
+      [3, 5],
+      [4, 5],
+      # sides
+      [0, 3],
+      [1, 5],
+      [2, 4],
+  ])
+  edge_face_norm = np.array([
+      # bottom
+      [0, 2],
+      [0, 4],
+      [0, 3],
+      # top
+      [1, 3],
+      [1, 2],
+      [1, 4],
+      # sides
+      [2, 3],
+      [2, 4],
+      [3, 4],
+  ])
+
+  def get_face_norm(face):
+    # use ccw winding order convention, and avoid using the last vertex
+    edge0 = face[2, :] - face[1, :]
+    edge1 = face[0, :] - face[1, :]
+    return math.normalize(jp.cross(edge0, edge1))
+
+  centroid = jp.mean(vert, axis=0)
+  vert = vert - centroid
+  face = vert[face]
+  face_norm = jax.vmap(get_face_norm)(face)
+
+  c = ConvexInfo(
+      centroid,
+      jp.eye(3, dtype=float),
+      jp.ones(3),
+      vert,
+      face,
+      face_norm,
+      edges,
+      face_norm[edge_face_norm],
+      None,
+  )
+
+  return jax.tree_util.tree_map(jp.array, c)
+
+
+def hfield(m: Union[mujoco.MjModel, Model], data_id: int) -> HFieldInfo:
+  adr = m.hfield_adr[data_id]
+  nrow, ncol = m.hfield_nrow[data_id], m.hfield_ncol[data_id]
+  h = HFieldInfo(
+      jp.zeros(3, dtype=float),
+      jp.eye(3, dtype=float),
+      m.hfield_size[data_id],
+      nrow,
+      ncol,
+      m.hfield_data[adr : adr + nrow * ncol].reshape((ncol, nrow), order='F'),
+  )
+  return h

mjx/mujoco/mjx/_src/support.py

@@ -197,6 +197,7 @@ def _getnum(m: Union[Model, mujoco.MjModel], obj: mujoco._enums.mjtObj) -> int:
       mujoco.mjtObj.mjOBJ_SITE: m.nsite,
       mujoco.mjtObj.mjOBJ_CAMERA: m.ncam,
       mujoco.mjtObj.mjOBJ_MESH: m.nmesh,
+      mujoco.mjtObj.mjOBJ_HFIELD: m.nhfield,
       mujoco.mjtObj.mjOBJ_PAIR: m.npair,
       mujoco.mjtObj.mjOBJ_EQUALITY: m.neq,
       mujoco.mjtObj.mjOBJ_ACTUATOR: m.nu,
@@ -218,6 +219,7 @@ def _getadr(
       mujoco.mjtObj.mjOBJ_SITE: m.name_siteadr,
       mujoco.mjtObj.mjOBJ_CAMERA: m.name_camadr,
       mujoco.mjtObj.mjOBJ_MESH: m.name_meshadr,
+      mujoco.mjtObj.mjOBJ_HFIELD: m.name_hfieldadr,
       mujoco.mjtObj.mjOBJ_PAIR: m.name_pairadr,
       mujoco.mjtObj.mjOBJ_EQUALITY: m.name_eqadr,
       mujoco.mjtObj.mjOBJ_ACTUATOR: m.name_actuatoradr,
@@ -277,3 +279,43 @@ def name2id(
   }
 
   return names_map.get(name, -1)
+
+
+def _decode_pyramid(
+    pyramid: jax.Array, mu: jax.Array, condim: int
+) -> jax.Array:
+  """Converts pyramid representation to contact force."""
+  force = jp.zeros(6, dtype=float)
+  if condim == 1:
+    return force.at[0].set(pyramid[0])
+
+  # force_normal = sum(pyramid0_i + pyramid1_i)
+  force = force.at[0].set(pyramid[0 : 2 * (condim - 1)].sum())
+
+  # force_tangent_i = (pyramid0_i - pyramid1_i) * mu_i
+  i = np.arange(0, condim)
+  force = force.at[i + 1].set((pyramid[2 * i] - pyramid[2 * i + 1]) * mu[i])
+
+  return force
+
+
+def contact_force(
+    m: Model, d: Data, contact_id: int, to_world_frame: bool = False
+) -> jax.Array:
+  """Extract 6D force:torque for one contact, in contact frame by default."""
+  efc_address = d.contact.efc_address[contact_id]
+  condim = d.contact.dim[contact_id]
+  if m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL:
+    force = _decode_pyramid(
+        d.efc_force[efc_address:], d.contact.friction[contact_id], condim
+    )
+  elif m.opt.cone == mujoco.mjtCone.mjCONE_ELLIPTIC:
+    raise NotImplementedError('Elliptic cone force is not implemented yet.')
+  else:
+    raise ValueError(f'Unknown cone type: {m.opt.cone}')
+
+  if to_world_frame:
+    force = force.reshape((-1, 3)) @ d.contact.frame[contact_id]
+    force = force.reshape(-1)
+
+  return force * (efc_address >= 0)

mjx/mujoco/mjx/_src/support_test.py

@@ -157,6 +157,65 @@ class SupportTest(parameterized.TestCase):
         i = i if n is not None else -1
         self.assertEqual(support.name2id(mx, obj, n), i)
 
+  _CONTACTS = """
+    <mujoco>
+      <worldbody>
+        <body pos="0 0 0.55" euler="1 0 0">
+          <joint axis="1 0 0" type="free"/>
+          <geom fromto="-0.4 0 0 0.4 0 0" size="0.05" type="capsule" condim="6"/>
+        </body>
+        <body pos="0 0 0.5" euler="0 1 0">
+          <joint axis="1 0 0" type="free"/>
+          <geom fromto="-0.4 0 0 0.4 0 0" size="0.05" type="capsule" condim="3"/>
+        </body>
+        <body pos="0 0 0.445" euler="0 90 0">
+          <joint axis="1 0 0" type="free"/>
+          <geom fromto="-0.4 0 0 0.4 0 0" size="0.05" type="capsule" condim="1"/>
+        </body>
+      </worldbody>
+    </mujoco>
+  """
+
+  def test_contact_force(self):
+    m = mujoco.MjModel.from_xml_string(self._CONTACTS)
+    d = mujoco.MjData(m)
+    mujoco.mj_step(m, d)
+    assert (
+        np.unique(d.contact.geom).shape[0] == 3
+    ), 'This test assumes all capsule are in contact.'
+    mx = mjx.put_model(m)
+    dx = mjx.put_data(m, d)
+    mujoco.mj_step(m, d)
+    dx = mjx.step(mx, dx)
+
+    # map MJX contacts to MJ ones
+    def _find(g):
+      val = (g == dx.contact.geom).sum(axis=1)
+      return np.where(val == 2)[0][0]
+
+    contact_id_map = {i: _find(d.contact.geom[i]) for i in range(d.ncon)}
+
+    for i in range(d.ncon):
+      result = np.zeros(6, dtype=float)
+      mujoco.mj_contactForce(m, d, i, result)
+
+      j = contact_id_map[i]
+      force = jax.jit(support.contact_force, static_argnums=(2,))(mx, dx, j)
+      np.testing.assert_allclose(result, force, rtol=1e-5, atol=2)
+
+      # test world conversion
+      force = jax.jit(
+          support.contact_force,
+          static_argnums=(
+              2,
+              3,
+          ),
+      )(mx, dx, j, True)
+      # back to contact frame
+      force = force.at[:3].set(dx.contact.frame[j] @ force[:3])
+      force = force.at[3:].set(dx.contact.frame[j] @ force[3:])
+      np.testing.assert_allclose(result, force, rtol=1e-5, atol=2)
+
 
 if __name__ == '__main__':
   absltest.main()

mjx/mujoco/mjx/_src/types.py

@@ -15,7 +15,7 @@
 """Base types used in MJX."""
 
 import enum
-
+from typing import Tuple
 import jax
 import mujoco
 from mujoco.mjx._src.dataclasses import PyTreeNode  # pylint: disable=g-importing-member
@@ -112,6 +112,24 @@ class GeomType(enum.IntEnum):
   # unsupported: NGEOMTYPES, ARROW*, LINE, SKIN, LABEL, NONE
 
 
+class ConvexMesh(PyTreeNode):
+  """Geom properties for convex meshes.
+
+  Attributes:
+    vert: vertices of the convex mesh
+    face: faces of the convex mesh
+    face_normal: normal vectors for the faces
+    edge: edge indexes for all edges in the convex mesh
+    edge_face_normal: indexes for face normals adjacent to edges in `edge`
+  """
+
+  vert: jax.Array
+  face: jax.Array
+  face_normal: jax.Array
+  edge: jax.Array
+  edge_face_normal: jax.Array
+
+
 class ConeType(enum.IntEnum):
   """Type of friction cone.
 
@@ -322,6 +340,7 @@ class Model(PyTreeNode):
     nmesh: number of meshes
     nmeshvert: number of vertices in all meshes
     nmeshface: number of triangular faces in all meshes
+    nhfield: number of heightfields
     nmat: number of materials
     npair: number of predefined geom pairs
     nexclude: number of excluded geom pairs
@@ -396,6 +415,7 @@ class Model(PyTreeNode):
     geom_solimp: constraint solver impedance: contact         (ngeom, mjNIMP)
     geom_size: geom-specific size parameters                  (ngeom, 3)
     geom_rbound: radius of bounding sphere                    (ngeom,)
+    geom_rbound_hfield: static rbound for hfield grid bounds  (ngeom,)
     geom_pos: local position offset rel. to body              (ngeom, 3)
     geom_quat: local orientation offset rel. to body          (ngeom, 4)
     geom_friction: friction for (slide, spin, roll)           (ngeom, 3)
@@ -419,6 +439,12 @@ class Model(PyTreeNode):
     mesh_vert: vertex positions for all meshes                (nmeshvert, 3)
     mesh_face: vertex face data                               (nmeshface, 3)
     mesh_graph: convex graph data                             (nmeshgraph,)
+    mesh_convex: pre-compiled convex mesh info for MJX        (nmesh,)
+    hfield_size: (x, y, z_top, z_bottom)                      (nhfield,)
+    hfield_nrow: number of rows in grid                       (nhfield,)
+    hfield_ncol: number of columns in grid                    (nhfield,)
+    hfield_adr: address in hfield_data                        (nhfield,)
+    hfield_data: elevation data                               (nhfielddata,)
     mat_rgba: rgba                                            (nmat, 4)
     pair_dim: contact dimensionality                          (npair,)
     pair_geom1: id of geom1                                   (npair,)
@@ -488,6 +514,7 @@ class Model(PyTreeNode):
   nmesh: int
   nmeshvert: int
   nmeshface: int
+  nhfield: int
   nmat: int
   npair: int
   nexclude: int
@@ -561,6 +588,7 @@ class Model(PyTreeNode):
   geom_solimp: jax.Array
   geom_size: jax.Array
   geom_rbound: jax.Array
+  geom_rbound_hfield: np.ndarray
   geom_pos: jax.Array
   geom_quat: jax.Array
   geom_friction: jax.Array
@@ -584,6 +612,12 @@ class Model(PyTreeNode):
   mesh_vert: np.ndarray
   mesh_face: np.ndarray
   mesh_graph: np.ndarray
+  mesh_convex: Tuple[ConvexMesh, ...]
+  hfield_size: np.ndarray
+  hfield_nrow: np.ndarray
+  hfield_ncol: np.ndarray
+  hfield_adr: np.ndarray
+  hfield_data: jax.Array
   mat_rgba: np.ndarray
   pair_dim: np.ndarray
   pair_geom1: np.ndarray
@@ -632,6 +666,7 @@ class Model(PyTreeNode):
   name_siteadr: np.ndarray
   name_camadr: np.ndarray
   name_meshadr: np.ndarray
+  name_hfieldadr: np.ndarray
   name_pairadr: np.ndarray
   name_eqadr: np.ndarray
   name_actuatoradr: np.ndarray

mjx/mujoco/mjx/test_data/barkour_v0/assets/barkour_v0_mjx.xml

@@ -77,6 +77,7 @@
   </custom>
 
   <asset>
+    <hfield name="rand_uniform" file="hfield_240_280.png" size="20.1 20.0 .05 .1"/>
     <mesh file="head.stl"/>
     <mesh file="powercable.stl"/>
     <mesh file="handle.stl"/>
@@ -101,7 +102,8 @@
     <site name="origin"/>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
     <camera name="default" pos="0.846 -1.465 0.916" xyaxes="0.866 0.500 0.000 -0.171 0.296 0.940"/>
-    <geom name="floor" size="0 0 0.05" type="plane" conaffinity="1" material="groundplane"/>
+    <geom name="hfield_floor" type="hfield" hfield="rand_uniform" conaffinity="1"/>
+    <!-- <geom name="floor" pos="0 0 -0.05" size="0 0 0.05" type="plane" conaffinity="1" material="groundplane"/> -->
     <body name="chassis">
       <camera name="track" pos="0.846 -1.465 0.916" xyaxes="0.866 0.500 0.000 -0.171 0.296 0.940" mode="trackcom"/>
       <freejoint/>

mjx/mujoco/mjx/test_data/constraints.xml

@@ -38,6 +38,11 @@
       <geom class="box"/>
     </body>
 
+    <body name="beam5" pos="-2 0 0" euler="45 45 0">
+      <joint name="joint5" type="ball" range="0 45" solreflimit="-100 -1"/> <!-- test negative solref -->
+      <geom class="box" contype="0" conaffinity="0"/>
+    </body>
+
     <body name="box_condim1" pos="4 0 0">
       <freejoint/>
       <geom class="box" condim="1"/>

mjx/mujoco/mjx/test_data/convex.xml

@@ -1,8 +1,4 @@
 <mujoco model="convex_convex">
-  <custom>
-    <numeric data="0.2" name="baumgarte_erp"/>
-    <numeric data="0.5" name="elasticity"/>
-  </custom>
   <default>
     <geom friction="0.5 0.0 0.0"/>
   </default>

mjx/pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name="mujoco-mjx"
-version = "3.1.5"
+version = "3.1.6"
 authors = [
     {name = "Google DeepMind", email = "mujoco@deepmind.com"},
 ]
@@ -31,7 +31,7 @@ dependencies = [
     "etils[epath]",
     "jax",
     "jaxlib",
-    "mujoco>=3.1.5.dev0",
+    "mujoco>=3.1.6.dev0",
     "scipy",
     "trimesh",
 ]
@@ -42,6 +42,6 @@ mjx-viewer = "mujoco.mjx.viewer:main"
 
 [project.urls]
 Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
-Documentation = "https://mujoco.readthedocs.io/en/3.1.5"
+Documentation = "https://mujoco.readthedocs.io/en/3.1.6"
 Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
-Changelog = "https://mujoco.readthedocs.io/en/3.1.5/changelog.html"
+Changelog = "https://mujoco.readthedocs.io/en/3.1.6/changelog.html"

mjx/requirements.txt

@@ -32,7 +32,8 @@ jaxlib==0.4.18; python_version >= '3.9' \
     --hash=sha256:f7787a5531d226d6cc9ec2baa7141260bb713435e1cfc053cb9f5cefa9756ac3 \
     --hash=sha256:6cb20bbbdafd90e71ad0deb9295519a0175c108c8c557b84fb9fe94f751daee4 \
     --hash=sha256:116a0d6aedd3e856b52493d7e392fb1b40952b84fb72448fde1c1ab5687db667 \
-    --hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652
+    --hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652 \
+    --hash=sha256:2b17b3f05b3bbf8e0ddb85fba339525ac03bac21c9f26d0f83dcea1b1654353e
 pip==23.3.1 \
     --hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b
 pytest==7.4.2 \
@@ -66,7 +67,8 @@ scipy==1.11.3; python_version >= '3.9' \
     --hash=sha256:9ea7f579182d83d00fed0e5c11a4aa5ffe01460444219dedc448a36adf0c3917 \
     --hash=sha256:5305792c7110e32ff155aed0df46aa60a60fc6e52cd4ee02cdeb67eaccd5356e \
     --hash=sha256:a63d1ec9cadecce838467ce0631c17c15c7197ae61e49429434ba01d618caa83 \
-    --hash=sha256:715c9966eb8906bc67e450e962bd07a5254420077178f98258904da4004a172f
+    --hash=sha256:715c9966eb8906bc67e450e962bd07a5254420077178f98258904da4004a172f \
+    --hash=sha256:d4d88a6fc091614b842a739b3db6ae15f95c77b308113bd6daefd4b05539b103
 setuptools==68.2.2 \
     --hash=sha256:b454a35605876da60632df1a60f736524eb73cc47bbc9f3f1ef1b644de74fd2a
 trimesh==4.0.0 \
@@ -135,7 +137,8 @@ numpy==1.26.0; python_version >= '3.9' \
     --hash=sha256:914b28d3215e0c721dc75db3ad6d62f51f630cb0c277e6b3bcb39519bed10bd8 \
     --hash=sha256:c78a22e95182fb2e7874712433eaa610478a3caf86f28c621708d35fa4fd6e7f \
     --hash=sha256:86f737708b366c36b76e953c46ba5827d8c27b7a8c9d0f471810728e5a2fe57c \
-    --hash=sha256:020cdbee66ed46b671429c7265cf00d8ac91c046901c55684954c3958525dab2
+    --hash=sha256:020cdbee66ed46b671429c7265cf00d8ac91c046901c55684954c3958525dab2 \
+    --hash=sha256:d6fa6d17727169ff1385ad3cb8f290bbcc3f2097322d90507c1956a4f9f870fc
 opt-einsum==3.3.0 \
     --hash=sha256:2455e59e3947d3c275477df7f5205b30635e266fe6dc300e3d9f9646bfcea147
 

mjx/training_apg.ipynb

@@ -1,6 +1,39 @@
 {
  "cells": [
   {
+    "cell_type": "markdown",
+    "metadata": {
+     "id": "MpkYHwCqk7W-"
+    },
+    "source": [
+     "![MuJoCo banner](https://raw.githubusercontent.com/google-deepmind/mujoco/main/banner.png)\n",
+     "\n",
+     "# <h1><center>Tutorial  <a href=\"https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/mjx/training_apg.ipynb\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" width=\"140\" align=\"center\"/></a></center></h1>\n",
+     "\n",
+     "This notebook provides a tutorial for differentiable physics for policy learning in [**MuJoCo XLA (MJX)**](https://github.com/google-deepmind/mujoco/blob/main/mjx), a JAX-based implementation of MuJoCo.\n",
+     "\n",
+     "**A Colab runtime with GPU acceleration is required.** If you're using a CPU-only runtime, you can switch using the menu \"Runtime > Change runtime type\".\n",
+     "\n",
+     "\n",
+     "This notebook was written by [Jing Yuan Luo](https://github.com/Andrew-Luo1).\n",
+     "\n",
+     "\u003c!-- Copyright 2021 DeepMind Technologies Limited\n",
+     "\n",
+     "     Licensed under the Apache License, Version 2.0 (the \"License\");\n",
+     "     you may not use this file except in compliance with the License.\n",
+     "     You may obtain a copy of the License at\n",
+     "\n",
+     "         http://www.apache.org/licenses/LICENSE-2.0\n",
+     "\n",
+     "     Unless required by applicable law or agreed to in writing, software\n",
+     "     distributed under the License is distributed on an \"AS IS\" BASIS,\n",
+     "     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n",
+     "     See the License for the specific language governing permissions and\n",
+     "     limitations under the License.\n",
+     "--\u003e"
+    ]
+   },
+   {
    "cell_type": "markdown",
    "metadata": {},
    "source": [

mjx/tutorial.ipynb

@@ -977,7 +977,7 @@
     "    path = epath.Path('mujoco_menagerie/google_barkour_vb/scene_mjx.xml')\n",
     "    sys = mjcf.load(path.as_posix())\n",
     "    self._dt = 0.02  # this environment is 50 fps\n",
-    "    sys = sys.tree_replace({'opt.timestep': 0.004, 'dt': 0.004})\n",
+    "    sys = sys.tree_replace({'opt.timestep': 0.004})\n",
     "\n",
     "    # override menagerie params for smoother policy\n",
     "    sys = sys.replace(\n",
@@ -1448,7 +1448,7 @@
    },
    "outputs": [],
    "source": [
-    "HTML(html.render(eval_env.sys.replace(dt=eval_env.dt), rollout))"
+    "HTML(html.render(eval_env.sys.tree_replace({'opt.timestep': eval_env.dt}), rollout))"
    ]
   }
  ],

plugin/elasticity/solid.cc

@@ -123,6 +123,9 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
       if (m->flex_vertbodyid[m->flex_vertadr[i]+j] == i0) {
         f0 = i;
         nv = m->flex_vertnum[f0];
+        if (m->flex_dim[i] != 3) {  // SHOULD NOT OCCUR
+          mju_error("mujoco.elasticity.solid requires a 3D mesh");
+        }
       }
     }
   }

plugin/sdf/sdf.cc

@@ -38,7 +38,7 @@ SdfVisualizer::SdfVisualizer() {
 }
 
 void SdfVisualizer::AddPoint(const mjtNum point[3]) {
-  if (!npoints_.empty()) {
+  if (!npoints_.empty() && npoints_.back() < points_.size()/3) {
     points_[3*npoints_.back()+0] = point[0];
     points_[3*npoints_.back()+1] = point[1];
     points_[3*npoints_.back()+2] = point[2];

python/LQR.ipynb

@@ -10,27 +10,22 @@
     "\n",
     "# <h1><center>LQR tutorial  <a href=\"https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/LQR.ipynb\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" width=\"140\" align=\"center\"/></a></center></h1>\n",
     "\n",
-    "This notebook provides an example of an LQR controller using [**MuJoCo** physics](https://github.com/google-deepmind/mujoco#readme)."
+    "This notebook provides an example of an LQR controller using [**MuJoCo** physics](https://github.com/google-deepmind/mujoco#readme).\n",
-   ]
+    "\n",
-  },
+    "<!-- Copyright 2021 DeepMind Technologies Limited\n",
-  {
+    "\n",
-   "cell_type": "markdown",
+    "     Licensed under the Apache License, Version 2.0 (the \"License\");\n",
-   "metadata": {
+    "     you may not use this file except in compliance with the License.\n",
-    "id": "LBAvTJ0xHKy7"
+    "     You may obtain a copy of the License at\n",
-   },
+    "\n",
-   "source": [
+    "         http://www.apache.org/licenses/LICENSE-2.0\n",
-    "### Copyright notice"
+    "\n",
-   ]
+    "     Unless required by applicable law or agreed to in writing, software\n",
-  },
+    "     distributed under the License is distributed on an \"AS IS\" BASIS,\n",
-  {
+    "     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n",
-   "cell_type": "markdown",
+    "     See the License for the specific language governing permissions and\n",
-   "metadata": {
+    "     limitations under the License.\n",
-    "id": "_UbO9uhtBSX5"
+    "-->"
-   },
-   "source": [
-    "> <p><small><small>Copyright 2022 DeepMind Technologies Limited</small></p>\n",
-    "> <p><small><small>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 <a href=\"http://www.apache.org/licenses/LICENSE-2.0\">http://www.apache.org/licenses/LICENSE-2.0</a>.</small></small></p>\n",
-    "> <p><small><small>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.</small></small></p>"
    ]
   },
   {
@@ -935,7 +930,6 @@
   "accelerator": "GPU",
   "colab": {
    "collapsed_sections": [
-    "LBAvTJ0xHKy7",
     "QPdJNe3k62mx"
    ],
    "private_outputs": true

python/build_requirements.txt

@@ -40,7 +40,9 @@ numpy==1.26.0; python_version >= '3.9' \
     --hash=sha256:914b28d3215e0c721dc75db3ad6d62f51f630cb0c277e6b3bcb39519bed10bd8 \
     --hash=sha256:c78a22e95182fb2e7874712433eaa610478a3caf86f28c621708d35fa4fd6e7f \
     --hash=sha256:86f737708b366c36b76e953c46ba5827d8c27b7a8c9d0f471810728e5a2fe57c \
-    --hash=sha256:020cdbee66ed46b671429c7265cf00d8ac91c046901c55684954c3958525dab2
+    --hash=sha256:020cdbee66ed46b671429c7265cf00d8ac91c046901c55684954c3958525dab2 \
+    --hash=sha256:d6fa6d17727169ff1385ad3cb8f290bbcc3f2097322d90507c1956a4f9f870fc \
+    --hash=sha256:79d9c3c363cbd919d879dd38aeb19d96be8699eda2af5f3f3c97bf774e1e6438
 pip==23.3.1 \
     --hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b
 PyOpenGL==3.1.7 \

python/mujoco/CMakeLists.txt

@@ -84,7 +84,7 @@ if(NOT TARGET mujoco)
     if(MUJOCO_FRAMEWORK)
       message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework")
       set(MUJOCO_LIBRARY
-          ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.1.5.dylib
+          ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.1.6.dylib
       )
       target_compile_options(mujoco INTERFACE -F${MUJOCO_FRAMEWORK})
     endif()
@@ -92,7 +92,7 @@ if(NOT TARGET mujoco)
 
   if(NOT MUJOCO_FRAMEWORK)
     find_library(
-      MUJOCO_LIBRARY mujoco mujoco.3.1.5 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED
+      MUJOCO_LIBRARY mujoco mujoco.3.1.6 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED
     )
     find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED)
     message("MuJoCo is at ${MUJOCO_LIBRARY}")
@@ -140,7 +140,7 @@ findorfetch(
   GIT_REPO
   https://github.com/abseil/abseil-cpp
   GIT_TAG
-  2f9e432cce407ce0ae50676696666f33a77d42ac # LTS 20240116.1
+  d7aaad83b488fd62bd51c81ecf16cd938532cc0a # LTS 20240116.2
   TARGETS
   ${MUJOCO_PYTHON_ABSL_TARGETS}
   EXCLUDE_FROM_ALL

python/mujoco/bindings_test.py

@@ -1194,6 +1194,14 @@ Euler integrator, semi-implicit in velocity.
     mujoco.mjd_inverseFD(self.model, self.data, eps, flg_centered,
                          None, None, None, None, None, None, None)
 
+  def test_geom_distance(self):
+    mujoco.mj_forward(self.model, self.data)
+    fromto = np.empty(6, np.float64)
+    dist = mujoco.mj_geomDistance(self.model, self.data, 0, 2, 200, fromto)
+    self.assertEqual(dist, 41.9)
+    np.testing.assert_array_equal(fromto,
+                                  np.array((42., 0., 0., 42., 0., 41.9)))
+
   def test_inverse_fd(self):
     eps = 1e-6
     flg_centered = 0

python/mujoco/functions.cc

@@ -531,6 +531,18 @@ PYBIND11_MODULE(_functions, pymodule) {
   Def<traits::mj_objectVelocity>(pymodule);
   Def<traits::mj_objectAcceleration>(pymodule);
   Def<traits::mj_contactForce>(pymodule);
+  Def<traits::mj_geomDistance>(
+      pymodule,
+      [](const raw::MjModel* m, const raw::MjData* d,
+         int geom1, int geom2, mjtNum distmax,
+         std::optional<Eigen::Ref<EigenArrayXX>> fromto) {
+        if (fromto.has_value() && fromto->size() != 6) {
+          throw py::type_error("fromto should be of size 6");
+        }
+        return InterceptMjErrors(::mj_geomDistance)(
+            m, d, geom1, geom2, distmax,
+            fromto.has_value() ? fromto->data() : nullptr);
+      });
   Def<traits::mj_differentiatePos>(
       pymodule,
       [](const raw::MjModel* m, Eigen::Ref<EigenVectorX> qvel,

python/mujoco/minimize.py

@@ -14,10 +14,11 @@
 # ==============================================================================
 """Nonlinear Least Squares minimization with box bounds."""
 
+import abc
 import dataclasses
 import enum
 import time
-from typing import Callable, List, Optional, TextIO, Tuple, Union
+from typing import Callable, List, Optional, Sequence, TextIO, Tuple, Union
 
 import mujoco
 import numpy as np
@@ -32,14 +33,14 @@ class Verbosity(enum.Enum):
 
 class Status(enum.Enum):
   FACTORIZATION_FAILED = enum.auto()
-  NO_IMPORVEMENT = enum.auto()
+  NO_IMPROVEMENT = enum.auto()
   MAX_ITER = enum.auto()
   DX_TOL = enum.auto()
 
 
 _STATUS_MESSAGE = {
     Status.FACTORIZATION_FAILED: 'factorization failed.',
-    Status.NO_IMPORVEMENT: 'insufficient reduction.',
+    Status.NO_IMPROVEMENT: 'insufficient reduction.',
     Status.MAX_ITER: 'maximum iterations reached.',
     Status.DX_TOL: 'norm(dx) < tol.',
 }
@@ -68,124 +69,109 @@ class IterLog:
   step: Optional[np.ndarray] = None
 
 
-def jacobian_fd(
+class Norm(abc.ABC):
-    residual: Callable[[np.ndarray], np.ndarray],
+  """Abstract interface for norm functions, measuring the magnitude of vectors.
-    x: np.ndarray,
-    r: np.ndarray,
-    eps: np.float64,
-    central: bool,
-    n_res: int,
-    bounds: Optional[List[np.ndarray]] = None,
-):
-  """Finite-difference Jacobian of a residual function.
 
-  Args:
+  Key Concepts:
-    residual: function that returns the residual for a given point.
-    x: point at which to evaluate the Jacobian.
-    r: residual at x.
-    eps: finite-difference step size.
-    central: whether to use central differences.
-    n_res: number or residual evaluations so far.
-    bounds: optional pair of lower and upper bounds.
 
-  Returns:
+  * Norm Value: The value of the norm for a given input vector.
-    jac: Jacobian of the residual at x.
+  * Gradient and Hessian: The gradient (first derivative) and Hessian (second
-    n_res: updated number of residual evaluations.
+    derivative) of the norm function with respect to the input vector.
 
+  Subclasses Must Implement:
+
+  * `value(self, r: np.ndarray)`: Computes and returns the norm value for the
+     input vector `r`.
+  * `grad_hess(self, r: np.ndarray, proj: np.ndarray)`: Computes and returns
+     both  the gradient and Hessian of the norm at `r`, projected onto `proj`.
+     The reason we ask the user to perform the projection themselves is that
+     norm Hessians are often large and sparse, and the "sandwich" projection
+     operator `proj.T @ hess @ proj` can be computed efficiently by taking the
+     specific norm structure into account.
   """
-  nx = x.size
+
-  nr = r.size
+  @abc.abstractmethod
-  jac = np.zeros((nr, nx))
+  def value(self, r: np.ndarray) -> np.float64:
-  xh = x.copy()
+    """Returns the value of the norm at the input vector `y = norm(r)`."""
-  if bounds is None:
+    pass
-    # No bounds, simple forward or central differencing.
+
-    for i in range(nx):
+  @abc.abstractmethod
-      xh[i] = x[i] + eps
+  def grad_hess(self, r: np.ndarray, proj: np.ndarray):
-      rp = residual(xh)
+    """Computes the projected gradient and Hessian of the norm at `r`.
-      if central:
+
-        xh[i] = x[i] - eps
+    Args:
-        rm = residual(xh)
+        r: A NumPy column vector (nr x 1).
-        jac[:, i] = (rp - rm) / (2*eps)
+        proj: A pre-computed projection matrix (nr x nx).
-      else:
+
-        jac[:, i] = (rp - r) / eps
+    Returns:
-      xh[i] = x[i]
+        A tuple containing:
-    n_res += 2*nx if central else nx
+            * Projected gradient: proj.T @ (d_norm/d_r).
-  else:
+            * Projected Hessian: proj.T @ (d^2_norm/d_r^2) @ proj.
-    lower, upper = bounds
+    """
-    midpoint = 0.5 * (upper - lower)
+    pass
-    for i in range(nx):
+
-      # Scale eps, don't cross bounds.
+
-      eps_i = eps * (upper[i] - lower[i])
+class Quadratic(Norm):
-      if central:
+  """Implementation of the quadratic norm."""
-        # Use central differencing if away from bounds.
+
-        if x[i] - eps_i < lower[i]:
+  def value(self, r: np.ndarray):
-          # Near lower bound, use forward.
+    """Returns the quadratic norm of `r`."""
-          xh[i] = x[i] + eps_i
+    return 0.5 * (r.T @ r).item()
-          rp = residual(xh)
+
-          jac[:, i] = (rp - r) / eps_i
+  def grad_hess(self, r: np.ndarray, proj: np.ndarray):
-          n_res += 1
+    """Computes the projected gradient and Hessian of the quadratic norm at `r`.
-        elif x[i] + eps_i > upper[i]:
+
-          # Near upper bound, use backward.
+    Args:
-          xh[i] = x[i] - eps_i
+        r: A NumPy column vector (nr x 1).
-          rm = residual(xh)
+        proj: A pre-computed projection matrix (nr x nx).
-          jac[:, i] = (r - rm) / eps_i
+
-          n_res += 1
+    Returns:
-        else:
+        A tuple containing:
-          # Use central.
+            * Projected gradient: `proj.T @ r`.
-          xh[i] = x[i] + eps_i
+            * Projected Hessian: `proj.T @ proj`.
-          rp = residual(xh)
+    """
-          xh[i] = x[i] - eps_i
+    grad = proj.T @ r
-          rm = residual(xh)
+    hess = proj.T @ proj  # Notionally proj.T @ np.eye(r.size) @ proj
-          jac[:, i] = (rp - rm) / (2*eps_i)
+    return grad, hess
-          n_res += 2
-      else:
-        # Below midpoint use forward differencing, otherwise backward.
-        if x[i] < midpoint[i]:
-          xh[i] = x[i] + eps_i
-          rp = residual(xh)
-          jac[:, i] = (rp - r) / eps_i
-        else:
-          xh[i] = x[i] - eps_i
-          rm = residual(xh)
-          jac[:, i] = (r - rm) / eps_i
-        n_res += 1
-      # Reset.
-      xh[i] = x[i]
-  return jac, n_res
 
 
 def least_squares(
     x0: np.ndarray,
     residual: Callable[[np.ndarray], np.ndarray],
-    bounds: Optional[List[np.ndarray]] = None,
+    bounds: Optional[Sequence[np.ndarray]] = None,
     jacobian: Optional[Callable[[np.ndarray, np.ndarray], np.ndarray]] = None,
+    norm: Norm = Quadratic(),
     eps: float = 1e-6,
-    central: bool = False,
     mu_min: float = 1e-6,
     mu_max: float = 1e8,
     mu_factor: float = 10.0**0.1,
-    tol: float = 1e-7,
+    tol: float = 1e-6,
     max_iter: int = 100,
     verbose: Union[Verbosity, int] = Verbosity.ITER,
     output: Optional[TextIO] = None,
+    iter_callback: Optional[Callable[[List[IterLog]], None]] = None,
+    check_derivatives: bool = False,
 ) -> Tuple[np.ndarray, List[IterLog]]:
   """Nonlinear Least Squares minimization with box bounds.
 
   Args:
-    x0: initial guess
+    x0: Initial guess
-    residual: function that returns the residual for a given point x.
+    residual: Vectorized function returning the residual for 1 or more points.
-    bounds: optional pair of lower and upper bounds on the solution.
+    bounds: Optional pair of lower and upper bounds on the solution.
-    jacobian: optional function that returns Jacobian of the residual at a given
+    jacobian: Optional function that returns Jacobian of the residual at a given
       point and residual. If not given, `residual` will be finite-differenced.
-    eps: perurbation used for automatic finite-differencing.
+    norm: Norm object returning norm scalar or its projected gradient and
-    central: whether to use central differences.
+      Hessian. See Norm class for detailed documentation.
-    mu_min: minimum value of the regularizer.
+    eps: Perurbation used for automatic finite-differencing.
-    mu_max: maximum value of the regularizer.
+    mu_min: Minimum value of the regularizer.
-    mu_factor: factor increasing or decreasing the regularizer.
+    mu_max: Maximum value of the regularizer.
-    tol: termination tolerance on the step size.
+    mu_factor: Factor for increasing or decreasing the regularizer.
-    max_iter: maximum number of iterations.
+    tol: Termination tolerance on the step size.
-    verbose: verbosity level.
+    max_iter: Maximum number of iterations.
-    output: optional file or StringIO to which to print messages.
+    verbose: Verbosity level.
+    output: Optional file or StringIO to which to print messages.
+    iter_callback: Optional iteration callback, takes trace argument.
+    check_derivatives: Compare user-defined Jacobian and norm against fin-diff.
 
   Returns:
     x: best solution found
@@ -202,10 +188,10 @@ def least_squares(
   # Initialize locals.
   status = Status.MAX_ITER
   i = 0
-  x = x0.astype(np.float64)
+  n = x0.size
-  n = x.size
+  x = x0.astype(np.float64).reshape((n, 1))
-  xnew = np.zeros((n,))
+  xnew = np.zeros((n, 1))
-  dx = np.zeros((n,))
+  dx = np.zeros((n, 1))
   scratch = np.zeros((n, n + 7))
   eps = np.float64(eps)
   mu = np.float64(0.0)  # Optimistically start with no regularization.
@@ -234,6 +220,8 @@ def least_squares(
     n_reduc = 0  # Reset n_reduc.
     return mu, n_reduc
 
+  # Make local copy of bounds to avoid reshaping user input.
+  bounds = None if bounds is None else list(bounds)
   if bounds is not None:
     # Checks bounds.
     if len(bounds) != 2:
@@ -244,7 +232,10 @@ def least_squares(
       raise ValueError('bounds must be finite.')
     if not np.all(bounds[0] < bounds[1]):
       raise ValueError('bounds[0] must be smaller than bounds[1].')
-    # Clip.
+
+    # Reshape and clip.
+    bounds[0] = bounds[0].reshape(n, 1)
+    bounds[1] = bounds[1].reshape(n, 1)
     np.clip(x, bounds[0], bounds[1], out=x)
 
   # Check for NaNs.
@@ -267,21 +258,28 @@ def least_squares(
       break
 
     # Get objective y.
-    y = 0.5 * r.dot(r)
+    y = norm.value(r)
 
     # Get Jacobian jac.
     t_start = time.time()
     if jacobian is None:
-      jac, n_res = jacobian_fd(residual, x, r, eps, central, n_res, bounds)
+      jac, n_res = jacobian_fd(residual, x, r, eps, n_res, bounds)
       t_res += time.time() - t_start
     else:
       jac = jacobian(x, r)
       t_jac += time.time() - t_start
       n_jac += 1
 
+      # Check user-provided Jacobian
+      if i == 0 and check_derivatives:
+        n_res = check_jacobian(residual, x, r, jac, eps, n_res, bounds, output)
+
+    # Check user-provided norm
+    if i == 0 and check_derivatives and not isinstance(norm, Quadratic):
+      check_norm(r, norm, eps, output)
+
     # Get gradient, Gauss-Newton Hessian.
-    grad = jac.T @ r
+    grad, hess = norm.grad_hess(r, jac)
-    hess = jac.T @ jac
 
     # Bounds relative to x
     dlower = None if bounds is None else bounds[0] - x
@@ -316,13 +314,13 @@ def least_squares(
       n_res += 1
 
       # New objective, evaluate reduction.
-      ynew = 0.5 * rnew.dot(rnew)
+      ynew = norm.value(rnew)
       reduction = y - ynew
-      armijo = reduction + armijo_c1*grad.dot(dx)
+      armijo = reduction + armijo_c1 * (grad.T @ dx).item()
 
       if armijo < 0:
         if mu >= mu_max:
-          status = Status.NO_IMPORVEMENT
+          status = Status.NO_IMPROVEMENT
           break
         mu, n_reduc = increase_mu(mu)
 
@@ -330,7 +328,7 @@ def least_squares(
       break
 
     # Compute reduction ratio.
-    expected_reduction = -(grad.dot(dx) + 0.5 * dx.T @ hess @ dx)
+    expected_reduction = -(grad.T @ dx + 0.5 * dx.T @ hess @ dx).item()
     reduction_ratio = 0.0
     if expected_reduction <= 0:
       if verbose > Verbosity.SILENT.value:
@@ -352,11 +350,13 @@ def least_squares(
       )
       print(message, file=output)
 
-    # Append log to trace.
+    # Append log to trace, call iter_callback.
     log = IterLog(candidate=x, objective=y, reduction=reduction, regularizer=mu)
     if verbose >= Verbosity.FULLITER.value:
       log = dataclasses.replace(log, residual=r, jacobian=jac, step=dx)
     trace.append(log)
+    if iter_callback is not None:
+      iter_callback(trace)
 
     # Check for success.
     if dx_norm < tol:
@@ -373,12 +373,14 @@ def least_squares(
     x = xnew
     r = rnew
 
-  # Append final log to trace.
+  # Append final log to trace, call iter_callback.
-  # Note: unlike other iter logs, this is at the end point.
+  # Note: unlike other iter logs, values are computed at the end point.
-  yfinal = 0.5 * r.dot(r)
+  yfinal = norm.value(r)
-  red = np.float64(0.0)
+  red = np.float64(0.0)  # No reduction sice we didn't take a step.
   log = IterLog(candidate=x, objective=yfinal, reduction=red, regularizer=mu)
   trace.append(log)
+  if iter_callback is not None:
+    iter_callback(trace)
 
   # Print final diagnostics.
   if verbose > Verbosity.SILENT.value:
@@ -401,4 +403,124 @@ def least_squares(
         message += f' Jacobian {jac_percent:<.1f}%'
       print(message, file=output)
 
-  return x, trace
+  return x.reshape(x0.shape), trace
+
+
+def jacobian_fd(
+    residual: Callable[[np.ndarray], np.ndarray],
+    x: np.ndarray,
+    r: np.ndarray,
+    eps: np.float64,
+    n_res: int,
+    bounds: Optional[List[np.ndarray]] = None,
+) -> Tuple[np.ndarray, int]:
+  """Finite-difference Jacobian of a residual function.
+
+  Args:
+    residual: vectorized function that returns the residual of a vector array.
+    x: point at which to evaluate the Jacobian.
+    r: residual at x.
+    eps: finite-difference step size.
+    n_res: number or residual evaluations so far.
+    bounds: optional pair of lower and upper bounds.
+
+  Returns:
+    jac: Jacobian of the residual at x.
+    n_res: updated number of residual evaluations (add x.size).
+
+  """
+  n = x.size
+  if bounds is None:
+    eps_vec = eps * np.ones(n)
+  else:
+    mid = 0.5 * (bounds[1] - bounds[0])
+    eps_vec = np.where(x > mid, -eps, eps).flatten()
+  xh = x + np.diag(eps_vec)
+  rh = residual(xh)
+  jac = (rh - r) / eps_vec
+  return jac, n_res+n
+
+
+def check_jacobian(
+    residual: Callable[[np.ndarray], np.ndarray],
+    x: np.ndarray,
+    r: np.ndarray,
+    jac: np.ndarray,
+    eps: np.float64,
+    n_res: int,
+    bounds: Optional[List[np.ndarray]] = None,
+    output: Optional[TextIO] = None,
+    name: Optional[str] = 'Jacobian',
+) -> int:
+  """Check user-provided Jacobian against internal finite-differencing.
+
+  Args:
+    residual: vectorized function that returns the residual of a vector array.
+    x: point at which the r and jac were evaluated.
+    r: residual at x.
+    jac: Jacobian at x.
+    eps: finite-difference step size.
+    n_res: number or residual evaluations so far.
+    bounds: optional pair of lower and upper bounds.
+    output: Optional file or StringIO to which to print messages.
+    name: Optional name of the function being tested.
+
+  Returns:
+    n_res: updated number of residual evaluations.
+
+  """
+  jac_fd, n_res = jacobian_fd(residual, x, r, eps, n_res, bounds)
+  denom = np.abs(jac).sum() + np.abs(jac_fd).sum() + 1e-8
+  rel_diff = np.abs(jac - jac_fd) / denom
+  if np.any(rel_diff > 1e-5):
+    raise ValueError(f'User-provided {name} does not match finite-differences '
+                     'to a relative tolerance of 1e-5.')
+  print(f'User-provided {name} matches finite-differences.', file=output)
+  return n_res
+
+
+def check_norm(
+    r: np.ndarray,
+    norm: Norm,
+    eps: np.float64,
+    output: Optional[TextIO] = None,
+):
+  """Check user-provided norm against internal finite-differencing.
+
+  Args:
+    r: residual vector.
+    norm: Norm function returning either the norm scalar or its gradient
+      and Gauss-Newton Hessian.
+    eps: finite-difference step size.
+    output: Optional file or StringIO to which to print messages.
+  """
+  # Get norm(r) value and 1st, 2nd derivatives.
+  n = np.atleast_2d(norm.value(r))  # norm value as 1x1 array.
+  eye = np.eye(r.size)  # Identity projection.
+  n_g, n_h = norm.grad_hess(r, eye)  # Gradient and Hessian.
+
+  # Check that Hessian is symmetric.
+  if not np.allclose(n_h, n_h.T):
+    raise ValueError('User-provided norm Hessian is not symmetric.')
+
+  # Check that Hessian is positive-definite.
+  if np.any(np.linalg.eigvals(n_h) < 0):
+    h_min = np.min(np.linalg.eigvals(n_h))
+    raise ValueError('User-provided norm Hessian is not positive definite. '
+                     f'Minimum eigenvalue is {h_min:<.4g}')
+
+  # Local function returning norm values (vectorized).
+  def norm_vec(v):
+    norms = [np.atleast_2d(norm.value(v[:, i:i+1])) for i in range(v.shape[1])]
+    return np.hstack(norms)
+
+  # Check the norm gradient.
+  check_jacobian(norm_vec, r, n, n_g.T, eps, 0, None, output, 'norm gradient')
+
+  # Local function returning norm gradients (vectorized).
+  def grad_vec(v):
+    gradients = [norm.grad_hess(v[:, i:i+1], eye)[0] for i in range(v.shape[1])]
+    return np.hstack(gradients)
+
+  # Check the norm Hessian.
+  check_jacobian(grad_vec, r, n_g, n_h, eps, 0, None, output, 'norm Hessian')

python/mujoco/minimize_test.py

@@ -15,7 +15,6 @@
 """Tests for minimize.py."""
 
 import io
-from typing import Tuple
 
 from absl.testing import absltest
 from mujoco import minimize
@@ -25,20 +24,19 @@ import numpy as np
 class MinimizeTest(absltest.TestCase):
 
   def test_basic(self) -> None:
-    def residual(x: np.ndarray) -> np.ndarray:
+    def residual(x):
-      return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)], dtype=np.float64)
+      return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
 
-    for central in [False, True]:
+    out = io.StringIO()
-      out = io.StringIO()
+    x0 = np.array((0.0, 0.0))
-      x0 = np.array((0.0, 0.0))
+    x, _ = minimize.least_squares(x0, residual, output=out)
-      x, _ = minimize.least_squares(x0, residual, output=out, central=central)
+    expected_x = np.array((1.0, 1.0))
-      expected_x = np.array((1.0, 1.0))
+    np.testing.assert_array_almost_equal(x, expected_x)
-      np.testing.assert_array_almost_equal(x, expected_x)
+    self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol')
-      self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol')
 
   def test_start_at_minimum(self) -> None:
-    def residual(x: np.ndarray) -> np.ndarray:
+    def residual(x):
-      return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
+      return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
 
     out = io.StringIO()
     x0 = np.array((1.0, 1.0))
@@ -49,33 +47,36 @@ class MinimizeTest(absltest.TestCase):
     self.assertContainsSubsequence(out.getvalue(), 'exact minimum found')
 
   def test_jac_callback(self) -> None:
-    def residual(x: np.ndarray) -> np.ndarray:
+    def residual(x):
-      return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
+      return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
 
-    def jacobian(x: np.ndarray, r: np.ndarray) -> Tuple[float, np.ndarray]:
+    def jacobian(x, r):
       del r  # Unused.
-      return np.array([[-1, 0], [-20 * x[0], 10]])
+      return np.array([[-1, 0], [-20 * x[0, 0], 10]])
 
     x0 = np.array((0.0, 0.0))
     out = io.StringIO()
-    x, _ = minimize.least_squares(x0, residual, jacobian=jacobian, output=out)
+    x, _ = minimize.least_squares(x0, residual, jacobian=jacobian, output=out,
+                                  check_derivatives=True)
     expected_x = np.array((1.0, 1.0))
     np.testing.assert_array_almost_equal(x, expected_x)
     self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol')
+    self.assertContainsSubsequence(out.getvalue(), 'Jacobian matches')
 
-    # Try with bad Jacobian, expect no improvement.
+    # Try with bad Jacobian, ask least_squares to check it.
-    def jac_bad1(x: np.ndarray, r: np.ndarray) -> Tuple[float, np.ndarray]:
+    def bad_jacobian(x, r):
-      return -jacobian(x, r)
+      del r  # Unused.
-    out1 = io.StringIO()
+      return np.array([[-1, 0], [-20 * x[0, 0], 15]])
-    minimize.least_squares(x0, residual, jacobian=jac_bad1, output=out1)
+    with self.assertRaisesRegex(ValueError, r'\bJacobian does not match\b'):
-    self.assertContainsSubsequence(out1.getvalue(), 'insufficient reduction')
+      minimize.least_squares(x0, residual, jacobian=bad_jacobian, output=out,
+                             check_derivatives=True)
 
   def test_max_iter(self) -> None:
     dim = 20  # High-D Rosenbrock
 
-    def residual(x: np.ndarray) -> np.ndarray:
+    def residual(x):
-      res0 = [1 - x[i] for i in range(dim - 1)]
+      res0 = [1 - x[i, :] for i in range(dim - 1)]
-      res1 = [10 * (x[i] - x[i + 1] ** 2) for i in range(dim - 1)]
+      res1 = [10 * (x[i, :] - x[i + 1, :] ** 2) for i in range(dim - 1)]
       return np.asarray(res0 + res1)
 
     # Fail to reach minimum after 20 iterations.
@@ -90,8 +91,8 @@ class MinimizeTest(absltest.TestCase):
     np.testing.assert_array_almost_equal(x, expected_x)
 
   def test_bounds(self) -> None:
-    def residual(x: np.ndarray) -> np.ndarray:
+    def residual(x):
-      return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
+      return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
 
     out = io.StringIO()
     x0 = np.array((0.0, 0.0))
@@ -108,32 +109,28 @@ class MinimizeTest(absltest.TestCase):
     self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol')
 
     # Test different bounds conditions.
-    verbose = minimize.Verbosity.FULLITER
+    for bounds in bounds_types.values():
-
+      out = io.StringIO()
-    for central in [False, True]:
+      x, trace = minimize.least_squares(
-      for bounds in bounds_types.values():
+          x0,
-        out = io.StringIO()
+          residual,
-        x, trace = minimize.least_squares(
+          bounds=bounds,
-            x0,
+          output=out,
-            residual,
+          verbose=minimize.Verbosity.FULLITER,
-            bounds=bounds,
+      )
-            output=out,
+      self.assertContainsSubsequence(out.getvalue(), ' < tol')
-            central=central,
+      grad = trace[-2].jacobian.T @ trace[-2].residual
-            verbose=verbose,
+      # If x_i is on the boundary, gradient points out, otherwise it is 0.
-        )
+      for i, xi in enumerate(x):
-        self.assertContainsSubsequence(out.getvalue(), ' < tol')
+        if xi == bounds[0][i]:
-        grad = trace[-2].jacobian.T @ trace[-2].residual
+          self.assertGreater(grad[i], 0)
-        # If x_i is on the boundary, gradient points out, otherwise it is 0.
+        elif xi == bounds[1][i]:
-        for i, xi in enumerate(x):
+          self.assertLess(grad[i], 0)
-          if xi == bounds[0][i]:
+        else:
-            self.assertGreater(grad[i], 0)
+          self.assertAlmostEqual(grad[i].item(), 0, places=4)
-          elif xi == bounds[1][i]:
-            self.assertLess(grad[i], 0)
-          else:
-            self.assertAlmostEqual(grad[i], 0, places=4)
 
   def test_bad_bounds(self) -> None:
-    def residual(x: np.ndarray) -> np.ndarray:
+    def residual(x):
       return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
 
     out = io.StringIO()
@@ -150,5 +147,114 @@ class MinimizeTest(absltest.TestCase):
       with self.assertRaises(ValueError):
         minimize.least_squares(x0, residual, bounds=bounds, output=out)
 
+  def test_iter_callback(self) -> None:
+    def residual(x):
+      return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
+
+    out = io.StringIO()
+
+    def iter_callback(trace):
+      print(f'Hello iteration {len(trace)}!', file=out)
+
+    x0 = np.array((0.0, 0.0))
+    x, _ = minimize.least_squares(x0, residual, output=out,
+                                  iter_callback=iter_callback)
+    expected_x = np.array((1.0, 1.0))
+    np.testing.assert_array_almost_equal(x, expected_x)
+    self.assertContainsSubsequence(out.getvalue(), 'Hello iteration 3!')
+
+  def test_norm(self) -> None:
+    def residual(x):
+      return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
+
+    p = 0.01  # Smoothing radius for smooth-L2 norm.
+
+    class SmoothL2(minimize.Norm):
+      def value(self, r):
+        return np.sqrt((r.T @ r).item() + p*p) - p
+
+      def grad_hess(self, r, proj):
+        s = np.sqrt((r.T @ r).item() + p*p)
+        y_r = r / s
+        grad = proj.T @ y_r
+        y_rr = (np.eye(r.size) - y_r @ y_r.T) / s
+        hess = proj.T @ y_rr @ proj
+        return grad, hess
+
+    out = io.StringIO()
+    x0 = np.array((0.0, 0.0))
+    x, _ = minimize.least_squares(x0, residual, norm=SmoothL2(), output=out,
+                                  check_derivatives=True)
+    expected_x = np.array((1.0, 1.0))
+    np.testing.assert_array_almost_equal(x, expected_x)
+    self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol')
+    self.assertContainsSubsequence(out.getvalue(),
+                                   'User-provided norm gradient matches')
+    self.assertContainsSubsequence(out.getvalue(),
+                                   'User-provided norm Hessian matches')
+
+    class SmoothL2BadGrad(minimize.Norm):
+      def value(self, r):
+        return np.sqrt((r.T @ r).item() + p*p) - p
+
+      def grad_hess(self, r, proj):
+        s = np.sqrt((r.T @ r).item() + p*p)
+        y_r = r / s
+        grad = proj.T @ (y_r + 0.001)  # 0.001 is erronous.
+        y_rr = (np.eye(r.size) - y_r @ y_r.T) / s
+        hess = proj.T @ y_rr @ proj
+        return grad, hess
+
+    with self.assertRaisesRegex(ValueError, r'\bgradient does not match\b'):
+      minimize.least_squares(x0, residual, norm=SmoothL2BadGrad(), output=out,
+                             check_derivatives=True)
+
+    class SmoothL2BadHess(minimize.Norm):
+      def value(self, r):
+        return np.sqrt((r.T @ r).item() + p*p) - p
+
+      def grad_hess(self, r, proj):
+        s = np.sqrt((r.T @ r).item() + p*p)
+        y_r = r / s
+        grad = proj.T @ y_r
+        y_rr = (1.001 * np.eye(r.size) - y_r @ y_r.T) / s  # 1.001 is erronous.
+        hess = proj.T @ y_rr @ proj
+        return grad, hess
+
+    with self.assertRaisesRegex(ValueError, r'\bHessian does not match\b'):
+      minimize.least_squares(x0, residual, norm=SmoothL2BadHess(), output=out,
+                             check_derivatives=True)
+
+    class SmoothL2AsymHess(minimize.Norm):
+      def value(self, r):
+        return np.sqrt((r.T @ r).item() + p*p) - p
+
+      def grad_hess(self, r, proj):
+        s = np.sqrt((r.T @ r).item() + p*p)
+        y_r = r / s
+        grad = proj.T @ y_r
+        y_rr = (np.eye(r.size) - (y_r + 0.0001) @ y_r.T) / s
+        hess = proj.T @ y_rr @ proj
+        return grad, hess
+
+    with self.assertRaisesRegex(ValueError, r'\bnot symmetric\b'):
+      minimize.least_squares(x0, residual, norm=SmoothL2AsymHess(), output=out,
+                             check_derivatives=True)
+
+    class SmoothL2NegHess(minimize.Norm):
+      def value(self, r):
+        return np.sqrt((r.T @ r).item() + p*p) - p
+
+      def grad_hess(self, r, proj):
+        s = np.sqrt((r.T @ r).item() + p*p)
+        y_r = r / s
+        grad = proj.T @ y_r
+        y_rr = -(np.eye(r.size) - y_r @ y_r.T) / s  # Negative-definite.
+        hess = proj.T @ y_rr @ proj
+        return grad, hess
+
+    with self.assertRaisesRegex(ValueError, r'\bnot positive definite\b'):
+      minimize.least_squares(x0, residual, norm=SmoothL2NegHess(), output=out,
+                             check_derivatives=True)
 if __name__ == '__main__':
   absltest.main()

python/mujoco/mjpython/Info.plist

@@ -7,13 +7,13 @@
     <key>CFBundleIdentifier</key>
     <string>org.mujoco.mjpython</string>
     <key>CFBundleVersion</key>
-    <string>3.1.5</string>
+    <string>3.1.6</string>
     <key>CFBundleGetInfoString</key>
-    <string>3.1.5</string>
+    <string>3.1.6</string>
     <key>CFBundleLongVersionString</key>
-    <string>3.1.5</string>
+    <string>3.1.6</string>
     <key>CFBundleShortVersionString</key>
-    <string>3.1.5</string>
+    <string>3.1.6</string>
     <key>CFBundleExecutable</key>
     <string>mjpython</string>
     <key>CFBundleIconFile</key>

python/pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "mujoco"
-version = "3.1.5"
+version = "3.1.6"
 authors = [
     {name = "Google DeepMind", email = "mujoco@deepmind.com"},
 ]
@@ -36,9 +36,9 @@ dynamic = ["readme", "scripts"]
 
 [project.urls]
 Homepage = "https://github.com/google-deepmind/mujoco"
-Documentation = "https://mujoco.readthedocs.io/en/3.1.5"
+Documentation = "https://mujoco.readthedocs.io/en/3.1.6"
 Repository = "https://github.com/google-deepmind/mujoco"
-Changelog = "https://mujoco.readthedocs.io/en/3.1.5/changelog.html"
+Changelog = "https://mujoco.readthedocs.io/en/3.1.6/changelog.html"
 
 [tool.setuptools]
 include-package-data = false

python/tutorial.ipynb

@@ -10,27 +10,22 @@
     "\n",
     "# <h1><center>Tutorial  <a href=\"https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/tutorial.ipynb\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" width=\"140\" align=\"center\"/></a></center></h1>\n",
     "\n",
-    "This notebook provides an introductory tutorial for [**MuJoCo** physics](https://github.com/google-deepmind/mujoco#readme), using the native Python bindings."
+    "This notebook provides an introductory tutorial for [**MuJoCo** physics](https://github.com/google-deepmind/mujoco#readme), using the native Python bindings.\n",
-   ]
+    "\n",
-  },
+    "<!-- Copyright 2021 DeepMind Technologies Limited\n",
-  {
+    "\n",
-   "cell_type": "markdown",
+    "     Licensed under the Apache License, Version 2.0 (the \"License\");\n",
-   "metadata": {
+    "     you may not use this file except in compliance with the License.\n",
-    "id": "xBSdkbmGN2K-"
+    "     You may obtain a copy of the License at\n",
-   },
+    "\n",
-   "source": [
+    "         http://www.apache.org/licenses/LICENSE-2.0\n",
-    "### Copyright notice"
+    "\n",
-   ]
+    "     Unless required by applicable law or agreed to in writing, software\n",
-  },
+    "     distributed under the License is distributed on an \"AS IS\" BASIS,\n",
-  {
+    "     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n",
-   "cell_type": "markdown",
+    "     See the License for the specific language governing permissions and\n",
-   "metadata": {
+    "     limitations under the License.\n",
-    "id": "_UbO9uhtBSX5"
+    "-->"
-   },
-   "source": [
-    "> <p><small><small>Copyright 2022 DeepMind Technologies Limited.</small></p>\n",
-    "> <p><small><small>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 <a href=\"http://www.apache.org/licenses/LICENSE-2.0\">http://www.apache.org/licenses/LICENSE-2.0</a>.</small></small></p>\n",
-    "> <p><small><small>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.</small></small></p>"
    ]
   },
   {
@@ -2122,7 +2117,6 @@
   "accelerator": "GPU",
   "colab": {
    "collapsed_sections": [
-    "xBSdkbmGN2K-",
     "YvyGCsgSCxHQ"
    ],
    "gpuClass": "premium",

sample/CMakeLists.txt

@@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON)
 
 project(
   mujoco_samples
-  VERSION 3.1.5
+  VERSION 3.1.6
   DESCRIPTION "MuJoCo samples binaries"
   HOMEPAGE_URL "https://mujoco.org"
 )

simulate/CMakeLists.txt

@@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng
 
 project(
   mujoco_simulate
-  VERSION 3.1.5
+  VERSION 3.1.6
   DESCRIPTION "MuJoCo simulate binaries"
   HOMEPAGE_URL "https://mujoco.org"
 )

src/engine/CMakeLists.txt

@@ -76,8 +76,6 @@ set(MUJOCO_ENGINE_SRCS
     engine_util_sparse_avx.h
     engine_util_spatial.c
     engine_util_spatial.h
-    engine_vfs.c
-    engine_vfs.h
     engine_vis_init.c
     engine_vis_init.h
     engine_vis_interact.c

src/engine/engine_collision_convex.c

@@ -29,6 +29,8 @@
 #include "engine/engine_util_misc.h"
 #include "engine/engine_util_spatial.h"
 
+// the LibCCD penetration function we use (ccdMPRPenetration or ccdGJKPenetration)
+#define _mjCCDPENETRATION ccdMPRPenetration
 
 // ccd center function
 void mjccd_center(const void *obj, ccd_vec3_t *center) {
@@ -267,13 +269,23 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
 
 
 
+// initialize CCD structure
+static void mjc_initCCD(ccd_t* ccd, const mjModel* m) {
+  CCD_INIT(ccd);
+  ccd->mpr_tolerance = m->opt.mpr_tolerance;
+  ccd->epa_tolerance = m->opt.mpr_tolerance;  // use MPR tolerance for EPA
+  ccd->max_iterations = m->opt.mpr_iterations;
+}
+
+
+
 // find single convex-convex collision, using libccd
 static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd,
                             const mjModel* m, const mjData* d,
                             mjContact* con, mjtNum margin) {
   ccd_vec3_t dir, pos;
   ccd_real_t depth;
-  if (ccdMPRPenetration(obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
+  if (_mjCCDPENETRATION(obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
     // contact is found but normal is undefined
     if (ccdVec3Eq(&dir, ccd_vec3_origin)) {
       return 0;
@@ -343,16 +355,13 @@ int mjc_Convex(const mjModel* m, const mjData* d,
   mjtCCD obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
 
   // init ccd structure
+  mjc_initCCD(&ccd, m);
   ccd.first_dir = ccdFirstDirDefault;
   ccd.center1 = mjccd_center;
   ccd.center2 = mjccd_center;
   ccd.support1 = mjccd_support;
   ccd.support2 = mjccd_support;
 
-  // set ccd parameters
-  ccd.max_iterations = m->opt.mpr_iterations;
-  ccd.mpr_tolerance = m->opt.mpr_tolerance;
-
   // find initial contact
   int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin);
 
@@ -747,16 +756,13 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
   //------------------------------------- collision testing
 
   // init ccd structure
+  mjc_initCCD(&ccd, m);
   ccd.first_dir = prism_firstdir;
   ccd.center1 = prism_center;
   ccd.center2 = mjccd_center;
   ccd.support1 = prism_support;
   ccd.support2 = mjccd_support;
 
-  // set ccd parameters
-  ccd.max_iterations = m->opt.mpr_iterations;
-  ccd.mpr_tolerance = m->opt.mpr_tolerance;
-
   // geom margin needed for actual collision test
   obj.margin = margin;
 
@@ -787,7 +793,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
           }
 
           // run MPR, save contact
-          if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0 &&
+          if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0 &&
               !ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
             // fill in contact data, transform to global coordinates
             con[cnt].dist = -depth;
@@ -1092,16 +1098,13 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
   mjtCCD obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}};
 
   // init ccd structure
+  mjc_initCCD(&ccd, m);
   ccd.first_dir = ccdFirstDirDefault;
   ccd.center1 = mjccd_center;
   ccd.center2 = mjccd_center;
   ccd.support1 = mjccd_support;
   ccd.support2 = mjccd_support;
 
-  // set ccd parameters
-  ccd.max_iterations = m->opt.mpr_iterations;
-  ccd.mpr_tolerance = m->opt.mpr_tolerance;
-
   // find contacts
   int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin);
 
@@ -1197,6 +1200,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
   //------------------------------------- collision testing
 
   // init ccd structure
+  CCD_INIT(&ccd);
   ccd.first_dir = prism_firstdir;
   ccd.center1 = prism_center;
   ccd.center2 = mjccd_center;
@@ -1234,7 +1238,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
           }
 
           // run MPR, save contact
-          if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0) {
+          if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0) {
             if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
               // fill in contact data, transform to global coordinates
               con[cnt].dist = -depth;
@@ -1266,3 +1270,5 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
 
   return cnt;
 }
+
+#undef _mjCCDPENETRATION

src/engine/engine_io.c

@@ -27,13 +27,11 @@
 #include <mujoco/mjplugin.h>
 #include <mujoco/mjxmacro.h>
 #include "engine/engine_crossplatform.h"
-#include "engine/engine_resource.h"
 #include "engine/engine_macro.h"
 #include "engine/engine_plugin.h"
 #include "engine/engine_util_blas.h"
 #include "engine/engine_util_errmem.h"
 #include "engine/engine_util_misc.h"
-#include "engine/engine_vfs.h"
 #include "thread/thread_pool.h"
 
 #ifdef ADDRESS_SANITIZER
@@ -449,22 +447,40 @@ static int safeAddToBufferSize(intptr_t* offset, size_t* nbuffer,
 
 
 
+// free model memory without destroying the struct
+static void freeModelBuffers(mjModel* m) {
+  mju_free(m->buffer);
+}
+
+
+
 // allocate and initialize mjModel structure
-mjModel* mj_makeModel(
+void mj_makeModel(mjModel** dest,
-  int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic, int njnt,
+    int nq, int nv, int nu, int na, int nbody, int nbvh,
-  int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert, int nflexedge,
+    int nbvhstatic, int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam,
-  int nflexelem, int nflexelemdata, int nflexshelldata, int nflexevpair, int nflextexcoord,
+    int nlight, int nflex, int nflexvert, int nflexedge, int nflexelem,
-  int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,  int nmeshgraph,
+    int nflexelemdata, int nflexshelldata, int nflexevpair, int nflextexcoord,
-  int nskin, int nskinvert, int nskintexvert, int nskinface, int nskinbone, int nskinbonevert,
+    int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
-  int nhfield, int nhfielddata, int ntex, int ntexdata, int nmat, int npair, int nexclude, int neq,
+    int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
-  int ntendon, int nwrap, int nsensor, int nnumeric, int nnumericdata, int ntext, int ntextdata,
+    int nskinbone, int nskinbonevert, int nhfield, int nhfielddata, int ntex,
-  int ntuple, int ntupledata, int nkey, int nmocap, int nplugin, int npluginattr, int nuser_body,
+    int ntexdata, int nmat, int npair, int nexclude, int neq, int ntendon,
-  int nuser_jnt, int nuser_geom, int nuser_site, int nuser_cam, int nuser_tendon,
+    int nwrap, int nsensor, int nnumeric, int nnumericdata, int ntext,
-  int nuser_actuator, int nuser_sensor, int nnames, int npaths) {
+    int ntextdata, int ntuple, int ntupledata, int nkey, int nmocap,
+    int nplugin, int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
+    int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
+    int nuser_sensor, int nnames, int npaths) {
   intptr_t offset = 0;
+  int allocate = *dest ? 0 : 1;
+  mjModel* m = NULL;
 
   // allocate mjModel
-  mjModel* m = (mjModel*)mju_malloc(sizeof(mjModel));
+  if (!allocate) {
+    m = *dest;
+    freeModelBuffers(m);
+  } else {
+    m = (mjModel*)mju_malloc(sizeof(mjModel));
+  }
+
   if (!m) {
     mjERROR("could not allocate mjModel");
   }
@@ -538,43 +554,43 @@ mjModel* mj_makeModel(
                     nhfield + ntex + nmat + npair + nexclude + neq + ntendon + nu + nsensor +
                     nnumeric + ntext + ntuple + nkey + nplugin;
   if (nnames_map >= INT_MAX / mjLOAD_MULTIPLE) {
-    mju_free(m);
+    if (allocate) mju_free(m);
     mju_warning("Invalid model: size of nnames_map is larger than INT_MAX");
-    return 0;
+    return;
   }
   m->nnames_map = mjLOAD_MULTIPLE * nnames_map;
   m->npaths = npaths;
 
 #define X(name)                                    \
   if ((m->name) < 0) {                             \
-    mju_free(m);                                   \
+    if (allocate) mju_free(m);                     \
     mju_warning("Invalid model: negative " #name); \
-    return 0;                                      \
+    return;                                        \
   }
   MJMODEL_INTS;
 #undef X
 
   // nbody should always be positive
   if (m->nbody == 0) {
-    mju_free(m);
+    if (allocate) mju_free(m);
     mju_warning("Invalid model: nbody == 0");
-    return 0;
+    return;
   }
 
   // nmocap is going to get multiplied by 4, and shouldn't overflow
   if (m->nmocap >= MAX_ARRAY_SIZE) {
-    mju_free(m);
+    if (allocate) mju_free(m);
     mju_warning("Invalid model: nmocap too large");
-    return 0;
+    return;
   }
 
   // compute buffer size
   m->nbuffer = 0;
 #define X(type, name, nr, nc)                                                \
   if (!safeAddToBufferSize(&offset, &m->nbuffer, sizeof(type), m->nr, nc)) { \
-    mju_free(m);                                                             \
+    if (allocate) mju_free(m);                                               \
     mju_warning("Invalid model: " #name " too large.");                      \
-    return 0;                                                                \
+    return;                                                                  \
   }
 
   MJMODEL_POINTERS
@@ -583,7 +599,7 @@ mjModel* mj_makeModel(
   // allocate buffer
   m->buffer = mju_malloc(m->nbuffer);
   if (!m->buffer) {
-    mju_free(m);
+    if (allocate) mju_free(m);
     mjERROR("could not allocate mjModel buffer");
   }
 
@@ -600,18 +616,19 @@ mjModel* mj_makeModel(
   mj_defaultVisual(&m->vis);
   mj_defaultStatistic(&m->stat);
 
-  return m;
+  // copy pointer if allocated here
+  if (allocate) {
+    *dest = m;
+  }
 }
 
-
-
 // copy mjModel, if dest==NULL create new model
 mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
   void* save_bufptr;
 
   // allocate new model if needed
   if (!dest) {
-    dest = mj_makeModel(
+    mj_makeModel(&dest,
       src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh,
       src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite,
       src->ncam, src->nlight, src->nflex, src->nflexvert, src->nflexedge,
@@ -714,36 +731,17 @@ void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buff
 }
 
 
-
+// load binary MJB model
-// load model from binary MJB resource
+mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
-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];
   size_t sizes[8];
   int ptrbuf = 0;
   mjModel *m = 0;
-  mjResource* r = NULL;
-
-  // first try vfs, otherwise try a provider or OS filesystem
-  if (!(r = mju_openVfsResource(filename, vfs))) {
-    char error[1024];
-    if (!(r = mju_openResource(filename, error, 1024))) {
-       mju_warning("%s", error);
-      return NULL;
-    }
-  }
-
-  const void* buffer = NULL;
-  int buffer_sz = mju_readResource(r, &buffer);
-  if (buffer_sz <= 0) {
-    mju_closeResource(r);
-    return NULL;
-  }
 
   if (buffer_sz < NHEADER*sizeof(int)) {
     mju_warning("Model file has an incomplete header");
-    mju_closeResource(r);
     return NULL;
   }
 
@@ -755,27 +753,22 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
       switch (i) {
       case 0:
         mju_warning("Model missing header ID");
-        mju_closeResource(r);
         return NULL;
 
       case 1:
         mju_warning("Model and executable have different floating point precision");
-        mju_closeResource(r);
         return NULL;
 
       case 2:
         mju_warning("Model and executable have different number of ints in mjModel");
-        mju_closeResource(r);
         return NULL;
 
       case 3:
         mju_warning("Model and executable have different number of size_t members in mjModel");
-        mju_closeResource(r);
         return NULL;
 
       default:
         mju_warning("Model and executable have different number of pointers in mjModel");
-        mju_closeResource(r);
         return NULL;
       }
     }
@@ -783,7 +776,6 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
 
   // read mjModel structure: info only
   if (ptrbuf + sizeof(int)*getnint() + sizeof(size_t)*getnsize() > buffer_sz) {
-    mju_closeResource(r);
     mju_warning("Truncated model file - ran out of data while reading sizes");
     return NULL;
   }
@@ -791,18 +783,18 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
   bufread(sizes, sizeof(size_t)*getnsize(), buffer_sz, buffer, &ptrbuf);
 
   // allocate new mjModel, check sizes
-  m = mj_makeModel(ints[0],  ints[1],  ints[2],  ints[3],  ints[4],  ints[5],  ints[6],
+  mj_makeModel(&m,
-                   ints[7],  ints[8],  ints[9],  ints[10], ints[11], ints[12], ints[13],
+               ints[0],  ints[1],  ints[2],  ints[3],  ints[4],  ints[5],  ints[6],
-                   ints[14], ints[15], ints[16], ints[17], ints[18], ints[19], ints[20],
+               ints[7],  ints[8],  ints[9],  ints[10], ints[11], ints[12], ints[13],
-                   ints[21], ints[22], ints[23], ints[24], ints[25], ints[26], ints[27],
+               ints[14], ints[15], ints[16], ints[17], ints[18], ints[19], ints[20],
-                   ints[28], ints[29], ints[30], ints[31], ints[32], ints[33], ints[34],
+               ints[21], ints[22], ints[23], ints[24], ints[25], ints[26], ints[27],
-                   ints[35], ints[36], ints[37], ints[38], ints[39], ints[40], ints[41],
+               ints[28], ints[29], ints[30], ints[31], ints[32], ints[33], ints[34],
-                   ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48],
+               ints[35], ints[36], ints[37], ints[38], ints[39], ints[40], ints[41],
-                   ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55],
+               ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48],
-                   ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62],
+               ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55],
-                   ints[63]);
+               ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62],
+               ints[63]);
   if (!m || m->nbuffer != sizes[getnsize()-1]) {
-    mju_closeResource(r);
     mju_warning("Corrupted model, wrong size parameters");
     mj_deleteModel(m);
     return NULL;
@@ -820,7 +812,6 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
 
   // read options and buffer
   if (ptrbuf + sizeof(mjOption) + sizeof(mjVisual) + sizeof(mjStatistic) > buffer_sz) {
-    mju_closeResource(r);
     mju_warning("Truncated model file - ran out of data while reading structs");
     return NULL;
   }
@@ -831,7 +822,6 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
     MJMODEL_POINTERS_PREAMBLE(m)
     #define X(type, name, nr, nc)                                           \
       if (ptrbuf + sizeof(type) * (m->nr) * (nc) > buffer_sz) {             \
-        mju_closeResource(r);                                               \
         mju_warning(                                                        \
             "Truncated model file - ran out of data while reading " #name); \
         mj_deleteModel(m);                                                  \
@@ -845,7 +835,6 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
 
   // make sure buffer is the correct size
   if (ptrbuf != buffer_sz) {
-    mju_closeResource(r);
     mju_warning("Model file is too large");
     mj_deleteModel(m);
     return NULL;
@@ -853,22 +842,19 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
 
   const char* validationError = mj_validateReferences(m);
   if (validationError) {
-    mju_closeResource(r);
     mju_warning("%s", validationError);
     mj_deleteModel(m);
     return NULL;
   }
 
-  mju_closeResource(r);
   return m;
 }
 
 
-
 // de-allocate mjModel
 void mj_deleteModel(mjModel* m) {
   if (m) {
-    mju_free(m->buffer);
+    freeModelBuffers(m);
     mju_free(m);
   }
 }
@@ -1123,12 +1109,40 @@ static void _initPlugin(const mjModel* m, mjData* d) {
 
 
 
+// free mjData memory without destroying the struct
+static void freeDataBuffers(mjData* 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]);
+      if (plugin->destroy) {
+        plugin->destroy(d, i);
+      }
+    }
+    mju_free(d->buffer);
+    mju_free(d->arena);
+}
+
+
+
 // allocate and initialize raw mjData structure
-mjData* mj_makeRawData(const mjModel* m) {
+void mj_makeRawData(mjData** dest, const mjModel* m) {
   intptr_t offset = 0;
+  int allocate = *dest ? 0 : 1;
+  mjData* d = NULL;
 
   // allocate mjData
-  mjData* d = (mjData*) mju_malloc(sizeof(mjData));
+  if (!allocate) {
+    d = *dest;
+    freeDataBuffers(d);
+  } else {
+    d = (mjData*) mju_malloc(sizeof(mjData));
+  }
+
   if (!d) {
     mjERROR("could not allocate mjData");
   }
@@ -1141,9 +1155,9 @@ mjData* mj_makeRawData(const mjModel* m) {
   d->buffer = d->arena = NULL;
 #define X(type, name, nr, nc)                                                \
   if (!safeAddToBufferSize(&offset, &d->nbuffer, sizeof(type), m->nr, nc)) { \
-    mju_free(d);                                                             \
+    if (allocate) mju_free(d);                                               \
     mju_warning("Invalid data: " #name " too large.");                       \
-    return 0;                                                                \
+    return;                                                                  \
   }
 
   MJDATA_POINTERS
@@ -1155,7 +1169,7 @@ mjData* mj_makeRawData(const mjModel* m) {
   // allocate buffer
   d->buffer = mju_malloc(d->nbuffer);
   if (!d->buffer) {
-    mju_free(d);
+    if (allocate) mju_free(d);
     mjERROR("could not allocate mjData buffer");
   }
 
@@ -1163,7 +1177,7 @@ mjData* mj_makeRawData(const mjModel* m) {
   d->arena = mju_malloc(d->narena);
   if (!d->arena) {
     mju_free(d->buffer);
-    mju_free(d);
+    if (allocate) mju_free(d);
     mjERROR("could not allocate mjData arena");
   }
 
@@ -1176,14 +1190,18 @@ mjData* mj_makeRawData(const mjModel* m) {
   // clear nplugin (overwritten by _initPlugin)
   d->nplugin = 0;
 
-  return d;
+  // copy pointer if allocated here
+  if (allocate) {
+    *dest = d;
+  }
 }
 
 
 
 // allocate and initialize mjData structure
 mjData* mj_makeData(const mjModel* m) {
-  mjData* d = mj_makeRawData(m);
+  mjData* d = NULL;
+  mj_makeRawData(&d, m);
   if (d) {
     _initPlugin(m, d);
     mj_resetData(m, d);
@@ -1200,7 +1218,7 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) {
 
   // allocate new data if needed
   if (!dest) {
-    dest = mj_makeRawData(m);
+    mj_makeRawData(&dest, m);
     _initPlugin(m, dest);
   }
 
@@ -1731,20 +1749,7 @@ void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) {
 // de-allocate mjData
 void mj_deleteData(mjData* d) {
   if (d) {
-#ifdef ADDRESS_SANITIZER
+    freeDataBuffers(d);
-    // 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]);
-      if (plugin->destroy) {
-        plugin->destroy(d, i);
-      }
-    }
-    mju_free(d->buffer);
-    mju_free(d->arena);
     mju_free(d);
   }
 }
@@ -1773,6 +1778,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
   case mjSENS_TENDONLIMITPOS:
   case mjSENS_TENDONLIMITVEL:
   case mjSENS_TENDONLIMITFRC:
+  case mjSENS_GEOMDIST:
   case mjSENS_CLOCK:
     return 1;
 
@@ -1797,8 +1803,12 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
   case mjSENS_SUBTREECOM:
   case mjSENS_SUBTREELINVEL:
   case mjSENS_SUBTREEANGMOM:
+  case mjSENS_GEOMNORMAL:
     return 3;
 
+  case mjSENS_GEOMFROMTO:
+    return 6;
+
   case mjSENS_BALLQUAT:
   case mjSENS_FRAMEQUAT:
     return 4;

src/engine/engine_io.h

@@ -51,19 +51,20 @@ void mj_defaultStatistic(mjStatistic* stat);
 //------------------------------- mjModel ----------------------------------------------------------
 
 // allocate mjModel
-mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic,
+void mj_makeModel(mjModel** dest,
-                      int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert,
+    int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic,
-                      int nflexedge, int nflexelem, int nflexelemdata, int nflexshelldata, int nflexevpair,
+    int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert,
-                      int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
+    int nflexedge, int nflexelem, int nflexelemdata, int nflexshelldata, int nflexevpair,
-                      int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
+    int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
-                      int nskinbone, int nskinbonevert, int nhfield, int nhfielddata,
+    int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
-                      int ntex, int ntexdata, int nmat, int npair, int nexclude,
+    int nskinbone, int nskinbonevert, int nhfield, int nhfielddata,
-                      int neq, int ntendon, int nwrap, int nsensor,
+    int ntex, int ntexdata, int nmat, int npair, int nexclude,
-                      int nnumeric, int nnumericdata, int ntext, int ntextdata,
+    int neq, int ntendon, int nwrap, int nsensor,
-                      int ntuple, int ntupledata, int nkey, int nmocap, int nplugin,
+    int nnumeric, int nnumericdata, int ntext, int ntextdata,
-                      int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
+    int ntuple, int ntupledata, int nkey, int nmocap, int nplugin,
-                      int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
+    int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
-                      int nuser_sensor, int nnames, int npaths);
+    int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
+    int nuser_sensor, int nnames, int npaths);
 
 // copy mjModel; allocate new if dest is NULL
 MJAPI mjModel* mj_copyModel(mjModel* dest, const mjModel* src);
@@ -71,9 +72,8 @@ MJAPI mjModel* mj_copyModel(mjModel* dest, const mjModel* src);
 // save model to binary file
 MJAPI void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buffer_sz);
 
-// load model from binary MJB file
+// load binary MJB
-//  if vfs is not NULL, look up file in vfs before reading from disk
+mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz);
-MJAPI mjModel* mj_loadModel(const char* filename, const mjVFS* vfs);
 
 // de-allocate model
 MJAPI void mj_deleteModel(mjModel* m);
@@ -92,7 +92,7 @@ MJAPI const char* mj_validateReferences(const mjModel* m);
 MJAPI mjData* mj_makeData(const mjModel* m);
 
 // allocate mjData corresponding to given model, used internally
-MJAPI mjData* mj_makeRawData(const mjModel* m);
+MJAPI void mj_makeRawData(mjData** dest, const mjModel* m);
 
 // Copy mjData.
 // m is only required to contain the size fields from MJMODEL_INTS.

src/engine/engine_ray.c

@@ -1137,14 +1137,13 @@ static int point_in_box(const mjtNum aabb[6], const mjtNum xpos[3],
 
 
 
-//---------------------------- main entry point ---------------------------------------------------
+//---------------------------- main entry point ----------------------------------------------------
 
 // intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms on bodyexclude
 //  return geomid and distance (x) to nearest surface, or -1 if no intersection
 //  geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion
 mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum* vec,
-              const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
+              const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int geomid[1]) {
-              int geomid[1]) {
   mjtNum dist, newdist;
 
   // check vector length
@@ -1154,7 +1153,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
 
   // clear result
   dist = -1;
-  *geomid = -1;
+  if (geomid) *geomid = -1;
 
   // loop over geoms not eliminated by mask and bodyexclude
   for (int i=0; i < m->ngeom; i++) {
@@ -1177,7 +1176,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
       // update if closer intersection found
       if (newdist >= 0 && (newdist < dist || dist < 0)) {
         dist = newdist;
-        *geomid = i;
+        if (geomid) *geomid = i;
       }
     }
   }

src/engine/engine_sensor.c

@@ -40,6 +40,11 @@ static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
   // process sensors matching stage and having positive cutoff
   for (int i=0; i < m->nsensor; i++) {
     if (m->sensor_needstage[i] == stage && m->sensor_cutoff[i] > 0) {
+      // skip fromto sensors
+      if (m->sensor_type[i] == mjSENS_GEOMFROMTO) {
+        continue;
+      }
+
       // get sensor info
       int adr = m->sensor_adr[i];
       int dim = m->sensor_dim[i];
@@ -214,9 +219,8 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
 
 // position-dependent sensors
 void mj_sensorPos(const mjModel* m, mjData* d) {
-  int rgeomid, objtype, objid, reftype, refid, adr, offset, nusersensor = 0;
+  int ne = d->ne, nf = d->nf, nefc = d->nefc, nsensor = m->nsensor;
-  int ne = d->ne, nf = d->nf, nefc = d->nefc;
+  int nusersensor = 0;
-  mjtNum rvec[3], *xpos, *xmat, *xpos_ref, *xmat_ref;
 
   // disabled sensors: return
   if (mjDISABLED(mjDSBL_SENSOR)) {
@@ -224,22 +228,26 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
   }
 
   // process sensors matching stage
-  for (int i=0; i < m->nsensor; i++) {
+  for (int i=0; i < nsensor; i++) {
+    mjtSensor type = (mjtSensor) m->sensor_type[i];
+
     // skip sensor plugins -- these are handled after builtin sensor types
-    if (m->sensor_type[i] == mjSENS_PLUGIN) {
+    if (type == mjSENS_PLUGIN) {
       continue;
     }
 
     if (m->sensor_needstage[i] == mjSTAGE_POS) {
       // get sensor info
-      objtype = m->sensor_objtype[i];
+      int objtype = m->sensor_objtype[i];
-      objid = m->sensor_objid[i];
+      int objid = m->sensor_objid[i];
-      refid = m->sensor_refid[i];
+      int refid = m->sensor_refid[i];
-      reftype = m->sensor_reftype[i];
+      int reftype = m->sensor_reftype[i];
-      adr = m->sensor_adr[i];
+      int adr = m->sensor_adr[i];
+
+      mjtNum rvec[3], *xpos, *xmat, *xpos_ref, *xmat_ref;
 
       // process according to type
-      switch ((mjtSensor) m->sensor_type[i]) {
+      switch (type) {
       case mjSENS_MAGNETOMETER:                           // magnetometer
         mju_mulMatTVec(d->sensordata+adr, d->site_xmat+9*objid, m->opt.magnetic, 3, 3);
         break;
@@ -255,7 +263,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
         rvec[1] = d->site_xmat[9*objid+5];
         rvec[2] = d->site_xmat[9*objid+8];
         d->sensordata[adr] = mj_ray(m, d, d->site_xpos+3*objid, rvec, NULL, 1,
-                                    m->site_bodyid[objid], &rgeomid);
+                                    m->site_bodyid[objid], NULL);
+
         break;
 
       case mjSENS_JOINTPOS:                               // jointpos
@@ -303,11 +312,11 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
 
         // reference frame unspecified: global frame
         if (refid == -1) {
-          if (m->sensor_type[i] == mjSENS_FRAMEPOS) {
+          if (type == mjSENS_FRAMEPOS) {
             mju_copy3(d->sensordata+adr, xpos);
           } else {
             // offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively
-            offset = m->sensor_type[i] - mjSENS_FRAMEXAXIS;
+            int offset = type - mjSENS_FRAMEXAXIS;
             d->sensordata[adr] = xmat[offset];
             d->sensordata[adr+1] = xmat[offset+3];
             d->sensordata[adr+2] = xmat[offset+6];
@@ -317,12 +326,12 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
         // reference frame specified
         else {
           get_xpos_xmat(d, reftype, refid, i, &xpos_ref, &xmat_ref);
-          if (m->sensor_type[i] == mjSENS_FRAMEPOS) {
+          if (type == mjSENS_FRAMEPOS) {
             mju_sub3(rvec, xpos, xpos_ref);
             mju_rotVecMatT(d->sensordata+adr, rvec, xmat_ref);
           } else {
             // offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively
-            offset = m->sensor_type[i] - mjSENS_FRAMEXAXIS;
+            int offset = type - mjSENS_FRAMEXAXIS;
             mjtNum axis[3] = {xmat[offset], xmat[offset+3], xmat[offset+6]};
             mju_rotVecMatT(d->sensordata+adr, axis, xmat_ref);
           }
@@ -354,6 +363,99 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
         mju_copy3(d->sensordata+adr, d->subtree_com+3*objid);
         break;
 
+      case mjSENS_GEOMDIST:                               // signed distance between two geoms
+      case mjSENS_GEOMNORMAL:                             // normal direction between two geoms
+      case mjSENS_GEOMFROMTO:                             // segment between two geoms
+        {
+          // use cutoff for collision margin
+          mjtNum margin = m->sensor_cutoff[i];
+
+          // initialize outputs
+          mjtNum dist = margin;    // collision distance
+          mjtNum fromto[6] = {0};  // segment between geoms
+
+          // get lists of geoms to collide
+          int n1, id1;
+          if (objtype == mjOBJ_BODY) {
+            n1 = m->body_geomnum[objid];
+            id1 = m->body_geomadr[objid];
+          } else {
+            n1 = 1;
+            id1 = objid;
+          }
+          int n2, id2;
+          if (reftype == mjOBJ_BODY) {
+            n2 = m->body_geomnum[refid];
+            id2 = m->body_geomadr[refid];
+          } else {
+            n2 = 1;
+            id2 = refid;
+          }
+
+          // collide all pairs
+          for (int geom1=id1; geom1 < id1+n1; geom1++) {
+            for (int geom2=id2; geom2 < id2+n2; geom2++) {
+              mjtNum fromto_new[6] = {0};
+              mjtNum dist_new = mj_geomDistance(m, d, geom1, geom2, margin, fromto_new);
+              if (dist_new < dist) {
+                dist = dist_new;
+                mju_copy(fromto, fromto_new, 6);
+              }
+            }
+          }
+
+          // write sensordata for this sensor and all subsequent sensors with identical signature
+          int write_sensor = 1;
+          while (write_sensor) {
+            // write geom distance
+            if (type == mjSENS_GEOMDIST) {
+              d->sensordata[adr] = dist;
+            }
+
+            // write distance normal
+            else if (type == mjSENS_GEOMNORMAL) {
+              mjtNum normal[3] = {fromto[3]-fromto[0], fromto[4]-fromto[1], fromto[5]-fromto[2]};
+              if (normal[0] || normal[1] || normal[2]) {
+                mju_normalize3(normal);
+              }
+              mju_copy3(d->sensordata + adr, normal);
+            }
+
+            // write distance fromto
+            else {
+              mju_copy(d->sensordata + adr, fromto, 6);
+            }
+
+            // if this is the last sensor, break
+            if (i+1 == nsensor) {
+              break;
+            }
+
+            // type of the next sensor
+            mjtSensor type_next = m->sensor_type[i+1];
+
+            // check if signature of next sensor matches this sensor
+            write_sensor = (type_next == mjSENS_GEOMDIST   ||
+                            type_next == mjSENS_GEOMNORMAL ||
+                            type_next == mjSENS_GEOMFROMTO)   &&
+                           m->sensor_objtype[i+1] == objtype  &&
+                           m->sensor_objid[i+1]   == objid    &&
+                           m->sensor_reftype[i+1] == reftype  &&
+                           m->sensor_refid[i+1]   == refid    &&
+                           m->sensor_cutoff[i+1]  == margin;
+
+            // if signature matches, increment external loop variable i
+            if (write_sensor) {
+              i++;
+
+              // update adr and type, everything else is the same
+              adr = m->sensor_adr[i];
+              type = type_next;
+            }
+          }
+        }
+        break;
+
       case mjSENS_CLOCK:                                  // clock
         d->sensordata[adr] = d->time;
         break;

src/engine/engine_setconst.c

@@ -341,12 +341,48 @@ static void set0(mjModel* m, mjData* d) {
 
     // compute positional offsets
     mju_sub3(m->light_pos0+3*i, d->light_xpos+3*i, d->xpos+3*id);
-    mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id));
+    mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com + (id1 >= 0 ? 3*id1 : 3*id));
 
     // copy dir
     mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i);
   }
 
+  // compute actuator damping from dampratio
+  for (int i=0; i < m->nu; i++) {
+    // get bias, gain parameters
+    mjtNum* biasprm = m->actuator_biasprm + i*mjNBIAS;
+    mjtNum* gainprm = m->actuator_gainprm + i*mjNGAIN;
+
+    // not a position-like actuator: skip
+    if (gainprm[0] != -biasprm[1]) {
+      continue;
+    }
+
+    // damping is 0 or negative (interpreted as regular "kv"): skip
+    if (biasprm[2] <= 0) {
+      continue;
+    }
+
+    // === interpret biasprm[2] > 0 as dampratio for position-like actuators
+
+    // "reflected" inertia (inversely scaled by transmission squared)
+    mjtNum* transmission = d->actuator_moment + i*nv;
+    mjtNum mass = 0;
+    for (int j=0; j < nv; j++) {
+      mjtNum trn = mju_abs(transmission[j]);
+      mjtNum trn2 = trn*trn;  // transmission squared
+      if (trn2 > mjMINVAL) {
+        mass += m->dof_M0[j] / trn2;
+      }
+    }
+
+    // damping = dampratio * 2 * sqrt(kp * mass)
+    mjtNum damping = biasprm[2] * 2 * mju_sqrt(gainprm[0] * mass);
+
+    // set biasprm[2] to negative damping
+    biasprm[2] = -damping;
+  }
+
   mj_freeStack(d);
 }
 

src/engine/engine_support.c

@@ -20,6 +20,7 @@
 
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmodel.h>
+#include "engine/engine_collision_driver.h"
 #include "engine/engine_core_constraint.h"
 #include "engine/engine_crossplatform.h"
 #include "engine/engine_io.h"
@@ -38,8 +39,8 @@
 
 //-------------------------- Constants -------------------------------------------------------------
 
- #define mjVERSION 315
+ #define mjVERSION 316
-#define mjVERSIONSTRING "3.1.5"
+#define mjVERSIONSTRING "3.1.6"
 
 // names of disable flags
 const char* mjDISABLESTRING[mjNDISABLE] = {
@@ -1703,6 +1704,48 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d,
 
 //-------------------------- miscellaneous ---------------------------------------------------------
 
+// returns the smallest distance between two geoms
+mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, mjtNum distmax,
+                       mjtNum fromto[6]) {
+  mjContact con[mjMAXCONPAIR];
+  mjtNum dist = distmax;
+  if (fromto) mju_zero(fromto, 6);
+
+  // flip geom order if required
+  int flip = m->geom_type[geom1] > m->geom_type[geom2];
+  int g1 = flip ? geom2 : geom1;
+  int g2 = flip ? geom1 : geom2;
+  int type1 = m->geom_type[g1];
+  int type2 = m->geom_type[g2];
+
+  // call collision function if it exists
+  if (!mjCOLLISIONFUNC[type1][type2]) {
+    return dist;
+  }
+  int num = mjCOLLISIONFUNC[type1][type2](m, d, con, g1, g2, distmax);
+
+  // find smallest distance
+  int smallest = -1;
+  for (int i=0; i < num; i++) {
+    mjtNum dist_i = con[i].dist;
+    if (dist_i < dist) {
+      dist = dist_i;
+      smallest = i;
+    }
+  }
+
+  // write fromto if given and a collision has been found
+  if (fromto && smallest >= 0) {
+    mjtNum sign = flip ? -1 : 1;
+    mju_addScl3(fromto+0, con[smallest].pos, con[smallest].frame, -0.5*sign*dist);
+    mju_addScl3(fromto+3, con[smallest].pos, con[smallest].frame, 0.5*sign*dist);
+  }
+
+  return dist;
+}
+
+
+
 // extract 6D force:torque for one contact, in contact frame
 void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]) {
   mjContact* con;

src/engine/engine_support.h

@@ -189,6 +189,10 @@ MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d,
 
 //-------------------------- miscellaneous ---------------------------------------------------------
 
+// returns the smallest distance between two geoms
+MJAPI mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2,
+                             mjtNum distmax, mjtNum fromto[6]);
+
 // extract 6D force:torque for one contact, in contact frame
 MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]);
 

src/engine/engine_vis_visualize.c

@@ -1974,6 +1974,20 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
         mjv_connector(thisgeom, mjGEOM_LINE, 3, from, to);
         f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
         FINISH
+      } else if (m->sensor_type[i] == mjSENS_GEOMFROMTO) {
+        // sensor data
+        mjtNum* fromto = d->sensordata + m->sensor_adr[i];
+
+        // null output: nothing to render
+        if (mju_isZero(fromto, 6)) {
+          continue;
+        }
+
+        // make ray
+        START
+        mjv_connector(thisgeom, mjGEOM_LINE, 3, fromto, fromto+3);
+        f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
+        FINISH
       }
     }
   }
@@ -2049,7 +2063,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
 
 
 // make list of lights only
-void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn) {
+void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn) {
   mjvLight* thislight;
 
   // clear counter
@@ -2116,7 +2130,7 @@ void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn) {
 
 
 // update camera only
-void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn) {
+void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScene* scn) {
   mjtNum ca, sa, ce, se, move[3], *mat;
   mjtNum headpos[3], forward[3], up[3], right[3], ipd;
 
@@ -2546,7 +2560,7 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
 
 
 // update all skins, here for backward API compatibility
-void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) {
+void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn) {
   mjvOption opt;
   mjv_defaultOption(&opt);
   mjv_updateActiveSkin(m, d, scn, &opt);
@@ -2556,7 +2570,7 @@ void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) {
 
 
 // update visible skins only
-void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) {
+void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn, const mjvOption* opt) {
   // process skins
   for (int i=0; i < m->nskin; i++) {
     // get info

src/engine/engine_vis_visualize.h

@@ -48,19 +48,19 @@ MJAPI void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* opt,
                         const mjvPerturb* pert, int catmask, mjvScene* scn);
 
 // make list of lights only
-MJAPI void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn);
+MJAPI void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn);
 
 // update camera only
-MJAPI void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn);
+MJAPI void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScene* scn);
 
 // update visible flexes only
 MJAPI void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt);
 
 // update skins only
-MJAPI void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn);
+MJAPI void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn);
 
 // update visible skins only
-MJAPI void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt);
+MJAPI void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn, const mjvOption* opt);
 
 #define mjNCATENARY 31
 

src/user/CMakeLists.txt

@@ -29,6 +29,8 @@ set(MUJOCO_USER_SRCS
     user_objects.h
     user_util.cc
     user_util.h
+    user_vfs.c
+    user_vfs.h
 )
 
 target_sources(mujoco PRIVATE ${MUJOCO_USER_SRCS})

src/user/user_api.cc

@@ -29,6 +29,9 @@
 #include "user/user_cache.h"
 #include "xml/xml_util.h"
 
+// global cache size in bytes (default 500MB)
+static constexpr std::size_t kGlobalCacheSize = 500 * (1 << 20);
+
 
 // prepend prefix
 template <typename T>
@@ -115,7 +118,7 @@ int mjs_detachBody(mjSpec* s, mjsBody* b) {
   mjCModel* model = static_cast<mjCModel*>(s->element);
   mjCBody* body = static_cast<mjCBody*>(b->element);
   *model -= *body;
-  mjs_deleteBody(b);
+  mjs_delete(b->element);
   return 0;
 }
 
@@ -137,10 +140,10 @@ void mjs_deleteSpec(mjSpec* s) {
 
 
 
-// delete body
+// delete object, it will call the appropriate destructor since ~mjCBase is virtual
-void mjs_deleteBody(mjsBody* b) {
+void mjs_delete(mjElement* element) {
-  mjCBody* body = static_cast<mjCBody*>(b->element);
+  mjCBase* object = static_cast<mjCBase*>(element);
-  delete body;
+  delete object;
 }
 
 
@@ -426,8 +429,8 @@ mjsPlugin* mjs_addPlugin(mjSpec* s) {
 // add default to model
 mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, int parentid, int* id) {
   mjCModel* modelC = static_cast<mjCModel*>(s->element);
-  *id = (int)modelC->defaults.size();
+  *id = (int)modelC->NumDefaults();
-  mjCDef* def = modelC->AddDef(classname, parentid);
+  mjCDef* def = modelC->AddDefault(classname, parentid);
   if (def) {
     return &def->spec;
   } else {
@@ -454,7 +457,7 @@ mjsDefault* mjs_getDefault(mjElement* element) {
 // Find default with given name in model.
 mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) {
   mjCModel* modelC = static_cast<mjCModel*>(s->element);
-  mjCDef* cdef = modelC->FindDef(classname);
+  mjCDef* cdef = modelC->FindDefault(classname);
   if (!cdef) {
     return nullptr;
   }
@@ -466,7 +469,7 @@ mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) {
 // get default[0] from model
 mjsDefault* mjs_getSpecDefault(mjSpec* s) {
   mjCModel* modelC = static_cast<mjCModel*>(s->element);
-  mjCDef* def = modelC->defaults[0];
+  mjCDef* def = modelC->Defaults(0);
   if (!def) {
     return nullptr;
   }
@@ -551,6 +554,22 @@ void mjs_setDefault(mjElement* element, mjsDefault* defspec) {
 
 
 
+// return first child of selected type
+mjElement* mjs_firstChild(mjsBody* body, mjtObj type) {
+  mjCBody* bodyC = static_cast<mjCBody*>(body->element);
+  return bodyC->NextChild(NULL, type);
+}
+
+
+
+// return body's next child; return NULL if child is last
+mjElement* mjs_nextChild(mjsBody* body, mjElement* child) {
+  mjCBody* bodyC = static_cast<mjCBody*>(body->element);
+  return bodyC->NextChild(child);
+}
+
+
+
 // set string
 void mjs_setString(mjString dest, const char* text) {
   std::string* str = reinterpret_cast<std::string*>(dest);
@@ -674,7 +693,7 @@ void mjs_setActivePlugins(mjSpec* s, void* activeplugins) {
   mjCModel* modelC = static_cast<mjCModel*>(s->element);
   std::vector<std::pair<const mjpPlugin*, int>>* active_plugins =
       reinterpret_cast<std::vector<std::pair<const mjpPlugin*, int>>*>(activeplugins);
-  modelC->active_plugins = std::move(*active_plugins);
+  modelC->SetActivePlugins(std::move(*active_plugins));
 }
 
 
@@ -698,5 +717,12 @@ void mj_setCacheSize(mjCache cache, std::size_t size) {
 
 
 mjCache mj_globalCache() {
-  return NULL;  // currently disabled
+  // mjCCache is not trivially destructible and so the global cache needs to
+  // allocated on the heap
+  if constexpr (kGlobalCacheSize != 0) {
+    static mjCCache* cache = new(std::nothrow) mjCCache(kGlobalCacheSize);
+    return (mjCache) cache;
+  } else {
+    return NULL;
+  }
 }

src/user/user_api.h

@@ -244,7 +244,6 @@ typedef struct _mjsJoint {         // joint specification
   // other
   int group;                       // group
   mjtByte actgravcomp;             // is gravcomp force applied via actuators
-  double urdfeffort;               // effort (urdf)
   mjDoubleVec userdata;            // user data
   mjString info;                   // message appended to compiler errors
 } mjsJoint;
@@ -811,8 +810,8 @@ MJAPI mjsLight* mjs_addLight(mjsBody* body, mjsDefault* def);
 // Add frame to body.
 MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe);
 
-// Delete body. TODO: make this a general mjs_deleteElement function
+// Delete object corresponding to the given element.
-MJAPI void mjs_deleteBody(mjsBody* b);
+MJAPI void mjs_delete(mjElement* element);
 
 
 //---------------------------------- Add non-tree elements -----------------------------------------
@@ -917,6 +916,15 @@ MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s);
 MJAPI int mjs_getId(mjElement* element);
 
 
+//---------------------------------- Tree traversal ------------------------------------------------
+
+// Return body's first child of given type.
+MJAPI mjElement* mjs_firstChild(mjsBody* body, mjtObj type);
+
+// Return body's next child of the same type; return NULL if child is last.
+MJAPI mjElement* mjs_nextChild(mjsBody* body, mjElement* child);
+
+
 //---------------------------------- Attribute setters ---------------------------------------------
 
 // Copy text to string.

src/user/user_cache.cc

@@ -187,16 +187,23 @@ const std::string* mjCCache::HasAsset(const std::string& id) {
 // is updated only if the timestamps disagree
 bool mjCCache::Insert(const mjCAsset& asset) {
   std::lock_guard<std::mutex> lock(mutex_);
+
+  // check if asset is too large to fit in the cache
+  std::size_t nbytes = asset.BytesCount();
   const std::string& id = asset.Id();
+  if ((size_ + nbytes > max_size_) && lookup_.find(id) == lookup_.end()) {
+    return false;
+  }
+
   if (asset.References().size() != 1) {
     return false;
   }
   const std::string& filename = *(asset.References().begin());
   auto [it, inserted] = lookup_.insert({id, asset});
+  mjCAsset* asset_ptr = &(it->second);
 
   if (!inserted) {
-    mjCAsset* asset_ptr = &(it->second);
+    if (size_ - asset_ptr->BytesCount() + nbytes > max_size_) {
-    if (size_ - asset_ptr->BytesCount() + asset.BytesCount() > max_size_) {
       return false;
     }
     models_[filename].insert(asset_ptr);  // add it for the model
@@ -205,19 +212,16 @@ bool mjCCache::Insert(const mjCAsset& asset) {
       return true;
     }
     asset_ptr->SetTimestamp(asset.Timestamp());
-    size_ = size_ - asset_ptr->BytesCount() + asset.BytesCount();
+    size_ = size_ - asset_ptr->BytesCount() + nbytes;
-    asset_ptr->ReplaceBlocks(asset.Blocks(), asset.BytesCount());
+    asset_ptr->ReplaceBlocks(asset.Blocks(), nbytes);
     return true;
-  } else if (size_ + asset.BytesCount() > max_size_) {
-    return false;
   }
 
   // new asset
-  mjCAsset* asset_ptr = &(it->second);
   asset_ptr->SetInsertNum(insert_num_++);
   entries_.insert(asset_ptr);
   models_[filename].insert(asset_ptr);
-  size_ += asset.BytesCount();
+  size_ += nbytes;
   return true;
 }
 
@@ -225,16 +229,22 @@ bool mjCCache::Insert(const mjCAsset& asset) {
 
 bool mjCCache::Insert(mjCAsset&& asset) {
   std::lock_guard<std::mutex> lock(mutex_);
+
+  // check if asset is too large to fit in the cache
+  std::size_t nbytes = asset.BytesCount();
   const std::string& id = asset.Id();
+  if ((size_ + nbytes > max_size_) && lookup_.find(id) == lookup_.end()) {
+    return false;
+  }
+
   if (asset.References().size() != 1) {
     return false;
   }
   const std::string& filename = *(asset.References().begin());
-  std::size_t nbytes = asset.BytesCount();
   auto [it, inserted] = lookup_.try_emplace(id, std::move(asset));
+  mjCAsset* asset_ptr = &(it->second);
 
   if (!inserted) {
-    mjCAsset* asset_ptr = &(it->second);
     if (size_ - asset_ptr->BytesCount() + nbytes > max_size_) {
       return false;
     }
@@ -248,12 +258,9 @@ bool mjCCache::Insert(mjCAsset&& asset) {
     size_ = size_ - asset_ptr->BytesCount() + nbytes;
     asset_ptr->ReplaceBlocks(std::move(asset.blocks_), asset.nbytes_);
     return true;
-  } else if (size_ + nbytes > max_size_) {
-    return false;
   }
 
   // new asset
-  mjCAsset* asset_ptr = &(it->second);
   asset_ptr->SetInsertNum(insert_num_++);
   entries_.insert(asset_ptr);
   models_[filename].insert(asset_ptr);

src/user/user_composite.cc

@@ -561,7 +561,7 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int
 
       // create tendon
       mjsTendon* ten = mjs_addTendon(&model->spec, &def[mjCOMPKIND_TENDON].spec);
-      mjs_setDefault(ten->element, &model->defaults[0]->spec);
+      mjs_setDefault(ten->element, &model->Defaults(0)->spec);
       mjs_setString(ten->name, txt0);
       ten->group = 4;
       mjs_wrapSite(ten, txt1);
@@ -569,7 +569,7 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int
 
       // add equality constraint
       mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
-      mjs_setDefault(eq->element, &model->defaults[0]->spec);
+      mjs_setDefault(eq->element, &model->Defaults(0)->spec);
       eq->type = mjEQ_TENDON;
       mjs_setString(eq->name1, mjs_getString(ten->name));
     }
@@ -675,7 +675,7 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjsBody* body, char* error, int err
 
         // create tendon
         mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_TENDON);
-        ten->def = model->defaults[0];
+        ten->def = model->Defaults(0);
         mju::sprintf_arr(txt, "%sT%d_%d_%d", prefix.c_str(), i, ix, iy);
         ten->name = txt;
         ten->WrapSite(txt1);
@@ -683,7 +683,7 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjsBody* body, char* error, int err
 
         // add equality constraint
         mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
-        mjs_setDefault(eq->element, &model->defaults[0]->spec);
+        mjs_setDefault(eq->element, &model->Defaults(0)->spec);
         eq->type = mjEQ_TENDON;
         mjs_setString(eq->name1, ten->name.c_str());
       }
@@ -1045,7 +1045,7 @@ mjsBody* mjCComposite::AddRopeBody(mjCModel* model, mjsBody* body, int ix, int i
 
     // add constraint
     mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TWIST].spec);
-    mjs_setDefault(eq->element, &model->defaults[0]->spec);
+    mjs_setDefault(eq->element, &model->Defaults(0)->spec);
     eq->type = mjEQ_JOINT;
     mjs_setString(eq->name1, mjs_getString(jnt->name));
   }
@@ -1063,7 +1063,7 @@ mjsBody* mjCComposite::AddRopeBody(mjCModel* model, mjsBody* body, int ix, int i
 
     // add constraint
     mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_STRETCH].spec);
-    mjs_setDefault(eq->element,  &model->defaults[0]->spec);
+    mjs_setDefault(eq->element,  &model->Defaults(0)->spec);
     eq->type = mjEQ_JOINT;
     mjs_setString(eq->name1, mjs_getString(jnt->name));
   }
@@ -1131,7 +1131,7 @@ bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int erro
 
   // fixed tendon for all joints
   mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_TENDON);
-  ten->def = model->defaults[0];
+  ten->def = model->Defaults(0);
   mju::sprintf_arr(txt, "%sT", prefix.c_str());
   ten->name = txt;
 
@@ -1184,7 +1184,7 @@ bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int erro
 
           // add fix constraint
           mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_JOINT].spec);
-          mjs_setDefault(eq->element,  &model->defaults[0]->spec);
+          mjs_setDefault(eq->element,  &model->Defaults(0)->spec);
           eq->type = mjEQ_JOINT;
           mjs_setString(eq->name1, mjs_getString(jnt->name));
 
@@ -1218,7 +1218,7 @@ bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int erro
 
   // finalize fixed tendon
   mjsEquality* eqt = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
-  mjs_setDefault(eqt->element, &model->defaults[0]->spec);
+  mjs_setDefault(eqt->element, &model->Defaults(0)->spec);
   eqt->type = mjEQ_TENDON;
   mjs_setString(eqt->name1, ten->name.c_str());
 
@@ -1244,7 +1244,7 @@ void mjCComposite::MakeShear(mjCModel* model) {
 
       // create tendon
       mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_SHEAR);
-      ten->def = model->defaults[0];
+      ten->def = model->Defaults(0);
       ten->WrapSite(txt1);
       ten->WrapSite(txt2);
 
@@ -1254,7 +1254,7 @@ void mjCComposite::MakeShear(mjCModel* model) {
 
       // equality constraint
       mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_SHEAR].spec);
-      mjs_setDefault(eq->element, &model->defaults[0]->spec);
+      mjs_setDefault(eq->element, &model->Defaults(0)->spec);
       eq->type = mjEQ_TENDON;
       mjs_setString(eq->name1, txt);
     }
@@ -1552,11 +1552,11 @@ void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin) {
       // bind pose
       if (iy==0) {
         bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
-        bindpos.push_back(-def[0].geom.spec.size[1]);
+        bindpos.push_back(-def[0].Geom().spec.size[1]);
         bindpos.push_back(0);
       } else if (iy==2) {
         bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
-        bindpos.push_back(def[0].geom.spec.size[1]);
+        bindpos.push_back(def[0].Geom().spec.size[1]);
         bindpos.push_back(0);
       } else {
         bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);

src/user/user_flexcomp.cc

@@ -400,7 +400,7 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) {
   mjsFlex* pf = &flex->spec;
   int id = flex->id;
 
-  *flex = def.flex;
+  *flex = def.Flex();
   flex->PointToLocal();
 
   flex->model = model;
@@ -521,7 +521,7 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) {
   // create edge equality constraint
   if (equality) {
     mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
-    mjs_setDefault(pe->element, &model->defaults[0]->spec);
+    mjs_setDefault(pe->element, &model->Defaults(0)->spec);
     pe->type = mjEQ_FLEX;
     pe->active = true;
     mjs_setString(pe->name1, name.c_str());
@@ -544,7 +544,7 @@ int mjCFlexcomp::GridID(int ix, int iy, int iz) {
 
 // make grid
 bool mjCFlexcomp::MakeGrid(char* error, int error_sz) {
-  int dim = def.flex.spec.dim;
+  int dim = def.Flex().spec.dim;
   bool hastex = texcoord.empty();
 
   // 1D

src/user/user_init.c

@@ -105,7 +105,6 @@ void mjs_defaultJoint(mjsJoint* joint) {
   joint->actfrclimited = mjLIMITED_AUTO;
   mj_defaultSolRefImp(joint->solref_limit, joint->solimp_limit);
   mj_defaultSolRefImp(joint->solref_friction, joint->solimp_friction);
-  joint->urdfeffort = -1;
 }
 
 

src/user/user_mesh.cc

@@ -146,12 +146,12 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
-    *this = _def->mesh;
+    *this = _def->Mesh();
   }
 
   // set model, def
   model = _model;
-  def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+  def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
 
   // in case this body is not compiled
   CopyFromSpec();
@@ -1426,7 +1426,7 @@ void mjCMesh::Process() {
       // apply formula, accumulate
       GetVolumeRef(type) += vol;
       for (int j=0; j<6; j++) {
-        P[j] += def->geom.density*vol /
+        P[j] += def->Geom().density*vol /
                   (type==mjINERTIA_SHELL ? 12 : 20) * (
                   2*(D[k[j][0]] * D[k[j][1]] +
                     E[k[j][0]] * E[k[j][1]] +
@@ -1467,7 +1467,7 @@ void mjCMesh::Process() {
     }
 
     // compute sizes of equivalent inertia box
-    double mass = GetVolumeRef(type) * def->geom.density;
+    double mass = GetVolumeRef(type) * def->Geom().density;
     double* boxsz = GetInertiaBoxPtr(type);
     boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/mass)/2;
     boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2;
@@ -2560,12 +2560,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
   for (int i=0; i < nvert; i++) {
     // get body id, set vertxpos = body.xpos0
     int b = rigid ? vertbodyid[0] : vertbodyid[i];
-    mju_copy3(vertxpos.data()+3*i, model->bodies[b]->xpos0);
+    mju_copy3(vertxpos.data()+3*i, model->Bodies()[b]->xpos0);
 
     // add vertex offset within body if not centered
     if (!centered) {
       mjtNum offset[3];
-      mju_rotVecQuat(offset, vert_.data()+3*i, model->bodies[b]->xquat0);
+      mju_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0);
       mju_addTo3(vertxpos.data()+3*i, offset);
     }
   }
@@ -2631,9 +2631,9 @@ void mjCFlex::Compile(const mjVFS* vfs) {
   mjXUtil::Vector2String(useredge, edgeidx);
 
   for (const auto& vbodyid : vertbodyid) {
-    if (model->bodies[vbodyid]->plugin.instance) {
+    if (model->Bodies()[vbodyid]->plugin.instance) {
       mjCPlugin* plugin_instance =
-          static_cast<mjCPlugin*>(model->bodies[vbodyid]->plugin.instance);
+          static_cast<mjCPlugin*>(model->Bodies()[vbodyid]->plugin.instance);
       plugin_instance->config_attribs["face"] = userface;
       plugin_instance->config_attribs["edge"] = useredge;
     }

src/user/user_model.h

@@ -192,14 +192,6 @@ class mjCModel : public mjCModel_, private mjSpec {
   mjCKey* AddKey();
   mjCPlugin* AddPlugin();
 
-  // copy vector of elements to this model
-  template <class T> void CopyList(std::vector<T*>& dest,
-                                   const std::vector<T*>& source,
-                                   std::map<mjCDef*, int>& def_map,
-                                   const std::vector<mjCDef*>& defaults);
-
-  // delete from list the elements that are compatible with other but not this model
-  template <class T> void RemoveFromList(std::vector<T*>& list, const mjCModel& other);
 
   // delete elements marked as discard=true
   template <class T> void Delete(std::vector<T*>& elements,
@@ -214,43 +206,59 @@ class mjCModel : public mjCModel_, private mjSpec {
 
   // API for access to other variables
   bool IsCompiled() const;                                       // is model already compiled
-  const mjCError& GetError(void) const;                          // get reference of error object
+  const mjCError& GetError() const;                              // get reference of error object
-  mjCBody* GetWorld() const;                                     // pointer to world body
+  mjCBody* GetWorld();                                           // pointer to world body
-  mjCDef* FindDef(std::string name) const;                       // find default class name
+  mjCDef* FindDefault(std::string name);                         // find defaults class name
-  mjCDef* AddDef(std::string name, int parentid);                // add default class to array
+  mjCDef* AddDefault(std::string name, int parentid);            // add defaults class to array
   mjCBase* FindObject(mjtObj type, std::string name) const;      // find object given type and name
-  mjCBody* FindBody(mjCBody* body, std::string name) const;      // find body given name
+  mjCBody* FindBody(mjCBody* body, std::string name);            // find body given name
   mjCFrame* FindFrame(mjCBody* body, std::string name) const;    // find frame given name
   bool IsNullPose(const mjtNum* pos, const mjtNum* quat) const;  // detect null pose
+  void SetActivePlugins(const std::vector<std::pair<const mjpPlugin*, int>>&& active_plugins) {
+    active_plugins_ = std::move(active_plugins);
+  }
 
   // accessors
-  std::string get_meshdir(void) const { return meshdir_; }
+  std::string get_meshdir() const { return meshdir_; }
-  std::string get_texturedir(void) const { return texturedir_; }
+  std::string get_texturedir() const { return texturedir_; }
+
+  mjCDef* Defaults(int i) const { return defaults_[i]; }
+  int NumDefaults() const { return defaults_.size(); }
+
+  const std::vector<std::pair<const mjpPlugin*, int>>& ActivePlugins() const {
+    return active_plugins_;
+  };
+
+  const std::vector<mjCFlex*>& Flexes() const { return flexes_; }
+  const std::vector<mjCMesh*>& Meshes() const {return meshes_; }
+  const std::vector<mjCSkin*>& Skins() const { return skins_; }
+  const std::vector<mjCHField*>& HFields() const { return hfields_; }
+  const std::vector<mjCTexture*>& Textures() const { return textures_; }
+  const std::vector<mjCMaterial*>& Materials() const { return materials_; }
+  const std::vector<mjCPair*>& Pairs() const { return pairs_; }
+  const std::vector<mjCBodyPair*>& Excludes() const { return excludes_; }
+  const std::vector<mjCEquality*>& Equalities() const { return equalities_; }
+  const std::vector<mjCTendon*>& Tendons() const { return tendons_; }
+  const std::vector<mjCActuator*>& Actuators() const { return actuators_; }
+  const std::vector<mjCSensor*>& Sensors() const { return sensors_; }
+  const std::vector<mjCNumeric*>& Numerics() const { return numerics_; }
+  const std::vector<mjCText*>& Texts() const { return texts_; }
+  const std::vector<mjCTuple*>& Tuples() const { return tuples_; }
+  const std::vector<mjCKey*>& Keys() const { return keys_; }
+  const std::vector<mjCPlugin*>& Plugins() const { return plugins_; }
+  const std::vector<mjCBody*>& Bodies() const { return bodies_; }
+  const std::vector<mjCGeom*>& Geoms() const { return geoms_; }
 
   // resolve plugin instance, create a new one if needed
   void ResolvePlugin(mjCBase* obj, const std::string& plugin_name,
                      const std::string& plugin_instance_name,
                      mjCPlugin** plugin_instance);
 
-  // settings for each defaults class
-  std::vector<mjCDef*> defaults;
-
-  // list of active plugins
-  std::vector<std::pair<const mjpPlugin*, int>> active_plugins;
-
- private:
   void TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs);
   mjModel* _Compile(const mjVFS* vfs);
 
   // clear objects allocated by Compile
-  void Clear(void);
+  void Clear();
-
-  // add object of any type
-  template <class T> T* AddObject(std::vector<T*>& list, std::string type);
-
-  // add object of any type, with def parameter
-  template <class T> T* AddObjectDef(std::vector<T*>& list, std::string type,
-                                     mjCDef* def);
 
   // if asset name is missing, set to filename
   template<class T> void SetDefaultNames(std::vector<T*>& assets);
@@ -259,6 +267,13 @@ class mjCModel : public mjCModel_, private mjSpec {
   template <class T> void DeleteMaterial(std::vector<T*>& list,
                                          std::string_view name = "");
 
+ private:
+  // settings for each defaults class
+  std::vector<mjCDef*> defaults_;
+
+  // list of active plugins
+  std::vector<std::pair<const mjpPlugin*, int>> active_plugins_;
+
   // compile phases
   void MakeLists(mjCBody* body);        // make lists of bodies, geoms, joints, sites
   void SetNuser();                      // set nuser fields
@@ -273,35 +288,51 @@ class mjCModel : public mjCModel_, private mjSpec {
   void CopyTree(mjModel*);              // copy objects inside kinematic tree
 
   // objects created here
-  std::vector<mjCFlex*>     flexes;      // list of flexes
+  std::vector<mjCFlex*>     flexes_;      // list of flexes
-  std::vector<mjCMesh*>     meshes;      // list of meshes
+  std::vector<mjCMesh*>     meshes_;      // list of meshes
-  std::vector<mjCSkin*>     skins;       // list of skins
+  std::vector<mjCSkin*>     skins_;       // list of skins
-  std::vector<mjCHField*>   hfields;     // list of height fields
+  std::vector<mjCHField*>   hfields_;     // list of height fields
-  std::vector<mjCTexture*>  textures;    // list of textures
+  std::vector<mjCTexture*>  textures_;    // list of textures
-  std::vector<mjCMaterial*> materials;   // list of materials
+  std::vector<mjCMaterial*> materials_;   // list of materials
-  std::vector<mjCPair*>     pairs;       // list of geom pairs to include
+  std::vector<mjCPair*>     pairs_;       // list of geom pairs to include
-  std::vector<mjCBodyPair*> excludes;    // list of body pairs to exclude
+  std::vector<mjCBodyPair*> excludes_;    // list of body pairs to exclude
-  std::vector<mjCEquality*> equalities;  // list of equality constraints
+  std::vector<mjCEquality*> equalities_;  // list of equality constraints
-  std::vector<mjCTendon*>   tendons;     // list of tendons
+  std::vector<mjCTendon*>   tendons_;     // list of tendons
-  std::vector<mjCActuator*> actuators;   // list of actuators
+  std::vector<mjCActuator*> actuators_;   // list of actuators
-  std::vector<mjCSensor*>   sensors;     // list of sensors
+  std::vector<mjCSensor*>   sensors_;     // list of sensors
-  std::vector<mjCNumeric*>  numerics;    // list of numeric fields
+  std::vector<mjCNumeric*>  numerics_;    // list of numeric fields
-  std::vector<mjCText*>     texts;       // list of text fields
+  std::vector<mjCText*>     texts_;       // list of text fields
-  std::vector<mjCTuple*>    tuples;      // list of tuple fields
+  std::vector<mjCTuple*>    tuples_;      // list of tuple fields
-  std::vector<mjCKey*>      keys;        // list of keyframe fields
+  std::vector<mjCKey*>      keys_;        // list of keyframe fields
-  std::vector<mjCPlugin*>   plugins;     // list of plugin instances
+  std::vector<mjCPlugin*>   plugins_;     // list of plugin instances
 
   // pointers to objects created inside kinematic tree
-  std::vector<mjCBody*>   bodies;   // list of bodies
+  std::vector<mjCBody*>   bodies_;   // list of bodies
-  std::vector<mjCJoint*>  joints;   // list of joints allowing motion relative to parent
+  std::vector<mjCJoint*>  joints_;   // list of joints allowing motion relative to parent
-  std::vector<mjCGeom*>   geoms;    // list of geoms attached to this body
+  std::vector<mjCGeom*>   geoms_;    // list of geoms attached to this body
-  std::vector<mjCSite*>   sites;    // list of sites attached to this body
+  std::vector<mjCSite*>   sites_;    // list of sites attached to this body
-  std::vector<mjCCamera*> cameras;  // list of cameras
+  std::vector<mjCCamera*> cameras_;  // list of cameras
-  std::vector<mjCLight*>  lights;   // list of lights
+  std::vector<mjCLight*>  lights_;   // list of lights
-  std::vector<mjCFrame*>  frames;   // list of frames
+  std::vector<mjCFrame*>  frames_;   // list of frames
 
   // array of pointers to each object list (enumerated by type)
-  std::array<std::vector<mjCBase*>*, mjNOBJECT> object_lists;
+  std::array<std::vector<mjCBase*>*, mjNOBJECT> object_lists_;
+
+  // add object of any type
+  template <class T> T* AddObject(std::vector<T*>& list, std::string type);
+
+  // add object of any type, with defaults parameter
+  template <class T> T* AddObjectDefault(std::vector<T*>& list, std::string type,
+                                         mjCDef* def);
+
+  // copy vector of elements to this model
+  template <class T> void CopyList(std::vector<T*>& dest,
+                                   const std::vector<T*>& sources,
+                                   std::map<mjCDef*, int>& def_map,
+                                   const std::vector<mjCDef*>& defaults);
+
+  // delete from list the elements that are compatible with other but not this model
+  template <class T> void RemoveFromList(std::vector<T*>& list, const mjCModel& other);
 
   // create mjCBase lists from children lists
   void CreateObjectLists();

src/user/user_objects.cc

@@ -45,11 +45,11 @@
 #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_api.h"
 #include "user/user_cache.h"
 #include "user/user_model.h"
 #include "user/user_util.h"
+#include "user/user_vfs.h"
 
 namespace {
 namespace mju = ::mujoco::util;
@@ -70,7 +70,7 @@ static void checksize(double* size, mjtGeom type, mjCBase* object, const char* n
   else {
     for (int i=0; i<mjGEOMINFO[type]; i++) {
       if (size[i]<=0) {
-        throw mjCError(object, "size %d must be positive in geom", "", i);
+        throw mjCError(object, "size %d must be positive in geom", nullptr, i);
       }
     }
   }
@@ -399,6 +399,7 @@ void mjCBoundingVolumeHierarchy::Set(mjtNum ipos_element[3], mjtNum iquat_elemen
 
 void mjCBoundingVolumeHierarchy::AllocateBoundingVolumes(int nleaf) {
   nbvh = 0;
+  bvh.clear();
   child.clear();
   nodeid.clear();
   level.clear();
@@ -556,22 +557,22 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
 //------------------------- class mjCDef implementation --------------------------------------------
 
 // constructor
-mjCDef::mjCDef(void) {
+mjCDef::mjCDef() {
   name.clear();
   parentid = -1;
   childid.clear();
-  mjs_defaultJoint(&joint.spec);
+  mjs_defaultJoint(&joint_.spec);
-  mjs_defaultGeom(&geom.spec);
+  mjs_defaultGeom(&geom_.spec);
-  mjs_defaultSite(&site.spec);
+  mjs_defaultSite(&site_.spec);
-  mjs_defaultCamera(&camera.spec);
+  mjs_defaultCamera(&camera_.spec);
-  mjs_defaultLight(&light.spec);
+  mjs_defaultLight(&light_.spec);
-  mjs_defaultFlex(&flex.spec);
+  mjs_defaultFlex(&flex_.spec);
-  mjs_defaultMesh(&mesh.spec);
+  mjs_defaultMesh(&mesh_.spec);
-  mjs_defaultMaterial(&material.spec);
+  mjs_defaultMaterial(&material_.spec);
-  mjs_defaultPair(&pair.spec);
+  mjs_defaultPair(&pair_.spec);
-  mjs_defaultEquality(&equality.spec);
+  mjs_defaultEquality(&equality_.spec);
-  mjs_defaultTendon(&tendon.spec);
+  mjs_defaultTendon(&tendon_.spec);
-  mjs_defaultActuator(&actuator.spec);
+  mjs_defaultActuator(&actuator_.spec);
 
   // make sure all the pointers are local
   PointToLocal();
@@ -591,12 +592,12 @@ void mjCDef::Compile(const mjCModel* model) {
   CopyFromSpec();
 
   // enforce length of all default userdata arrays
-  joint.userdata_.resize(model->nuser_jnt);
+  joint_.userdata_.resize(model->nuser_jnt);
-  geom.userdata_.resize(model->nuser_geom);
+  geom_.userdata_.resize(model->nuser_geom);
-  site.userdata_.resize(model->nuser_site);
+  site_.userdata_.resize(model->nuser_site);
-  camera.userdata_.resize(model->nuser_cam);
+  camera_.userdata_.resize(model->nuser_cam);
-  tendon.userdata_.resize(model->nuser_tendon);
+  tendon_.userdata_.resize(model->nuser_tendon);
-  actuator.userdata_.resize(model->nuser_actuator);
+  actuator_.userdata_.resize(model->nuser_actuator);
 }
 
 
@@ -607,18 +608,18 @@ mjCDef& mjCDef::operator=(const mjCDef& other) {
     name = other.name;
     parentid = other.parentid;
     childid = other.childid;
-    joint = other.joint;
+    joint_ = other.joint_;
-    geom = other.geom;
+    geom_ = other.geom_;
-    site = other.site;
+    site_ = other.site_;
-    camera = other.camera;
+    camera_ = other.camera_;
-    light = other.light;
+    light_ = other.light_;
-    flex = other.flex;
+    flex_ = other.flex_;
-    mesh = other.mesh;
+    mesh_ = other.mesh_;
-    material = other.material;
+    material_ = other.material_;
-    pair = other.pair;
+    pair_ = other.pair_;
-    equality = other.equality;
+    equality_ = other.equality_;
-    tendon = other.tendon;
+    tendon_ = other.tendon_;
-    actuator = other.actuator;
+    actuator_ = other.actuator_;
   }
   PointToLocal();
   return *this;
@@ -627,49 +628,49 @@ mjCDef& mjCDef::operator=(const mjCDef& other) {
 
 
 void mjCDef::PointToLocal() {
-  joint.PointToLocal();
+  joint_.PointToLocal();
-  geom.PointToLocal();
+  geom_.PointToLocal();
-  site.PointToLocal();
+  site_.PointToLocal();
-  camera.PointToLocal();
+  camera_.PointToLocal();
-  light.PointToLocal();
+  light_.PointToLocal();
-  flex.PointToLocal();
+  flex_.PointToLocal();
-  mesh.PointToLocal();
+  mesh_.PointToLocal();
-  material.PointToLocal();
+  material_.PointToLocal();
-  pair.PointToLocal();
+  pair_.PointToLocal();
-  equality.PointToLocal();
+  equality_.PointToLocal();
-  tendon.PointToLocal();
+  tendon_.PointToLocal();
-  actuator.PointToLocal();
+  actuator_.PointToLocal();
   spec.element = static_cast<mjElement*>(this);
   spec.name = (mjString)&name;
-  spec.joint = &joint.spec;
+  spec.joint = &joint_.spec;
-  spec.geom = &geom.spec;
+  spec.geom = &geom_.spec;
-  spec.site = &site.spec;
+  spec.site = &site_.spec;
-  spec.camera = &camera.spec;
+  spec.camera = &camera_.spec;
-  spec.light = &light.spec;
+  spec.light = &light_.spec;
-  spec.flex = &flex.spec;
+  spec.flex = &flex_.spec;
-  spec.mesh = &mesh.spec;
+  spec.mesh = &mesh_.spec;
-  spec.material = &material.spec;
+  spec.material = &material_.spec;
-  spec.pair = &pair.spec;
+  spec.pair = &pair_.spec;
-  spec.equality = &equality.spec;
+  spec.equality = &equality_.spec;
-  spec.tendon = &tendon.spec;
+  spec.tendon = &tendon_.spec;
-  spec.actuator = &actuator.spec;
+  spec.actuator = &actuator_.spec;
 }
 
 
 
 void mjCDef::CopyFromSpec() {
-  joint.CopyFromSpec();
+  joint_.CopyFromSpec();
-  geom.CopyFromSpec();
+  geom_.CopyFromSpec();
-  site.CopyFromSpec();
+  site_.CopyFromSpec();
-  camera.CopyFromSpec();
+  camera_.CopyFromSpec();
-  light.CopyFromSpec();
+  light_.CopyFromSpec();
-  flex.CopyFromSpec();
+  flex_.CopyFromSpec();
-  mesh.CopyFromSpec();
+  mesh_.CopyFromSpec();
-  material.CopyFromSpec();
+  material_.CopyFromSpec();
-  pair.CopyFromSpec();
+  pair_.CopyFromSpec();
-  equality.CopyFromSpec();
+  equality_.CopyFromSpec();
-  tendon.CopyFromSpec();
+  tendon_.CopyFromSpec();
-  actuator.CopyFromSpec();
+  actuator_.CopyFromSpec();
 }
 
 
@@ -1254,6 +1255,53 @@ mjCBase* mjCBody::FindObject(mjtObj type, string _name, bool recursive) {
 
 
 
+template <class T>
+static mjElement* GetNext(std::vector<T*>& list, mjElement* child) {
+  for (unsigned int i = 0; i < list.size()-1; i++) {
+    if (list[i]->spec.element == child) {
+      return list[i+1]->spec.element;
+    }
+  }
+  return nullptr;
+}
+
+
+
+// get next child of given type
+mjElement* mjCBody::NextChild(mjElement* child, mjtObj type) {
+  if (type == mjOBJ_UNKNOWN) {
+    if (!child) {
+      throw mjCError(this, "child type must be specified if no child element is given");
+    } else {
+      type = child->elemtype;
+    }
+  } else if (child && child->elemtype != type) {
+    throw mjCError(this, "child element is not of requested type");
+  }
+
+  switch (type) {
+    case mjOBJ_BODY:
+    case mjOBJ_XBODY:
+      return child ? GetNext(bodies, child) : bodies[0];
+    case mjOBJ_JOINT:
+      return child ? GetNext(joints, child) : joints[0];
+    case mjOBJ_GEOM:
+      return child ? GetNext(geoms, child) : geoms[0];
+    case mjOBJ_SITE:
+      return child ? GetNext(sites, child) : sites[0];
+    case mjOBJ_CAMERA:
+      return child ? GetNext(cameras, child) : cameras[0];
+    case mjOBJ_LIGHT:
+      return child ? GetNext(lights, child) : lights[0];
+    case mjOBJ_FRAME:
+      return child ? GetNext(frames, child) : frames[0];
+    default:
+      return nullptr;
+  }
+}
+
+
+
 // compute geom inertial frame: ipos, iquat, mass, inertia
 void mjCBody::GeomFrame(void) {
   int sz;
@@ -1333,6 +1381,23 @@ void mjCBody::MakeInertialExplicit() {
 }
 
 
+
+// compute bounding volume hierarchy
+void mjCBody::ComputeBVH() {
+  if (geoms.empty()) {
+    return;
+  }
+
+  tree.Set(ipos, iquat);
+  tree.AllocateBoundingVolumes(geoms.size());
+  for (int i=0; i<geoms.size(); i++) {
+    geoms[i]->SetBoundingVolume(tree.GetBoundingVolume(i));
+  }
+  tree.CreateBVH();
+}
+
+
+
 // compiler
 void mjCBody::Compile(void) {
   CopyFromSpec();
@@ -1447,14 +1512,7 @@ void mjCBody::Compile(void) {
   }
 
   // compute bounding volume hierarchy
-  if (!geoms.empty()) {
+  ComputeBVH();
-    tree.Set(ipos, iquat);
-    tree.AllocateBoundingVolumes(geoms.size());
-    for (int i=0; i<geoms.size(); i++) {
-      geoms[i]->SetBoundingVolume(tree.GetBoundingVolume(i));
-    }
-    tree.CreateBVH();
-  }
 
   // compile all joints, count dofs
   dofnum = 0;
@@ -1486,7 +1544,7 @@ void mjCBody::Compile(void) {
 
   // compute body global pose (no joint transformations in qpos0)
   if (id>0) {
-    mjCBody* par = model->bodies[parentid];
+    mjCBody* par = model->Bodies()[parentid];
     mju_rotVecQuat(xpos0, pos, par->xquat0);
     mju_addTo3(xpos0, par->xpos0);
     mju_mulQuat(xquat0, par->xquat0, quat);
@@ -1663,12 +1721,12 @@ mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
-    *this = _def->joint;
+    *this = _def->Joint();
   }
 
   // set model, def
   model = _model;
-  def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+  def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
 
   // point to local
   PointToLocal();
@@ -1867,12 +1925,12 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
-    *this = _def->geom;
+    *this = _def->Geom();
   }
 
   // set model, def
   model = _model;
-  def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+  def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
 
   // point to local
   PointToLocal();
@@ -1953,12 +2011,12 @@ void mjCGeom::NameSpace(const mjCModel* m) {
 
 
 // compute geom volume
-double mjCGeom::GetVolume(void) {
+double mjCGeom::GetVolume() const {
   double height;
 
   // get from mesh
   if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
-    if (mesh->id<0 || !((std::size_t) mesh->id <= model->meshes.size())) {
+    if (mesh->id<0 || !((std::size_t) mesh->id <= model->Meshes().size())) {
       throw mjCError(this, "invalid mesh id in mesh geom");
     }
 
@@ -2011,7 +2069,7 @@ void mjCGeom::SetInertia(void) {
 
   // get from mesh
   if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
-    if (mesh->id<0 || !((std::size_t) mesh->id <= model->meshes.size())) {
+    if (mesh->id<0 || !((std::size_t) mesh->id <= model->Meshes().size())) {
       throw mjCError(this, "invalid mesh id in mesh geom");
     }
 
@@ -2411,8 +2469,7 @@ void mjCGeom::Compile(void) {
   if (type==mjGEOM_HFIELD) {
     size[0] = hfield->size[0];
     size[1] = hfield->size[1];
-    size[2] = 0.5*(0.5*hfield->size[2] +
+    size[2] = 0.25 * hfield->size[2] + 0.5 * hfield->size[3];
-                   hfield->size[3]);
   } else if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
     const double* aamm = mesh->aamm();
     size[0] = mju_max(fabs(aamm[0]), fabs(aamm[3]));
@@ -2502,7 +2559,7 @@ mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
-    *this = _def->site;
+    *this = _def->Site();
   }
 
   // point to local
@@ -2513,7 +2570,7 @@ mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) {
 
   // set model, def
   model = _model;
-  def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+  def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
 }
 
 
@@ -2654,12 +2711,12 @@ mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
-    *this = _def->camera;
+    *this = _def->Camera();
   }
 
   // set model, def
   model = _model;
-  def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+  def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
 
   // point to local
   PointToLocal();
@@ -2808,12 +2865,12 @@ mjCLight::mjCLight(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
-    *this = _def->light;
+    *this = _def->Light();
   }
 
   // set model, def
   model = _model;
-  def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+  def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
 
   PointToLocal();
   CopyFromSpec();
@@ -3845,12 +3902,12 @@ mjCMaterial::mjCMaterial(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
-    *this = _def->material;
+    *this = _def->Material();
   }
 
   // set model, def
   model = _model;
-  def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+  def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
 
   // point to local
   PointToLocal();
@@ -3933,12 +3990,12 @@ mjCPair::mjCPair(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
-    *this = _def->pair;
+    *this = _def->Pair();
   }
 
   // set model, def
   model = _model;
-  def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+  def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
 
   // point to local
   PointToLocal();
@@ -4294,12 +4351,12 @@ mjCEquality::mjCEquality(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
-    *this = _def->equality;
+    *this = _def->Equality();
   }
 
   // set model, def
   model = _model;
-  def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+  def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
 
   // point to local
   PointToLocal();
@@ -4434,7 +4491,7 @@ void mjCEquality::Compile(void) {
   ResolveReferences(model);
 
   // make sure flex is not rigid
-  if (type==mjEQ_FLEX && model->flexes[obj1id]->rigid) {
+  if (type==mjEQ_FLEX && model->Flexes()[obj1id]->rigid) {
     throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1_.c_str(), id);
   }
 }
@@ -4456,12 +4513,12 @@ mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
-    *this = _def->tendon;
+    *this = _def->Tendon();
   }
 
   // set model, def
   model = _model;
-  def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+  def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
 
   // point to local
   PointToLocal();
@@ -4619,19 +4676,18 @@ void mjCTendon::WrapPulley(double divisor, std::string_view info) {
 
 
 // get number of wraps
-int mjCTendon::NumWraps(void) {
+int mjCTendon::NumWraps() const {
   return (int)path.size();
 }
 
 
 
 // get pointer to specified wrap
-mjCWrap* mjCTendon::GetWrap(int id) {
+const mjCWrap* mjCTendon::GetWrap(int i) const {
-  if (id>=0 && id<(int)path.size()) {
+  if (i >= 0 && i < (int)path.size()) {
-    return path[id];
+    return path[i];
-  } else {
-    return 0;
   }
+  return nullptr;
 }
 
 
@@ -4753,7 +4809,7 @@ void mjCTendon::Compile(void) {
         }
 
         // mark geoms as non visual
-        model->geoms[path[i]->obj->id]->SetNotVisual();
+        model->Geoms()[path[i]->obj->id]->SetNotVisual();
         break;
 
       case mjWRAP_JOINT:
@@ -4935,12 +4991,12 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
-    *this = _def->actuator;
+    *this = _def->Actuator();
   }
 
   // set model, def
   model = _model;
-  def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+  def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
 
   // in case this actuator is not compiled
   CopyFromSpec();
@@ -5021,7 +5077,6 @@ void mjCActuator::CopyFromSpec() {
 
 
 void mjCActuator::ResolveReferences(const mjCModel* m) {
-  mjCJoint* pjnt;
   switch (trntype) {
   case mjTRN_JOINT:
   case mjTRN_JOINTINPARENT:
@@ -5031,14 +5086,6 @@ void mjCActuator::ResolveReferences(const mjCModel* m) {
       throw mjCError(this,
                      "unknown transmission target '%s' for actuator id = %d", target_.c_str(), id);
     }
-    pjnt = (mjCJoint*) ptarget;
-
-    // apply urdfeffort
-    if (pjnt->spec.urdfeffort>0) {
-      forcerange[0] = -pjnt->spec.urdfeffort;
-      forcerange[1] = pjnt->spec.urdfeffort;
-      forcelimited = mjLIMITED_TRUE;
-    }
     break;
 
   case mjTRN_SLIDERCRANK:
@@ -5125,7 +5172,7 @@ void mjCActuator::Compile(void) {
       gainprm[0] == -biasprm[1] && inheritrange > 0) {
     // semantic of actuator is the same as transmission, inheritrange is applicable
     double* range;
-    if (dyntype == mjDYN_NONE) {
+    if (dyntype == mjDYN_NONE || dyntype == mjDYN_FILTEREXACT) {
       // position actuator
       range = ctrlrange;
     } else if (dyntype == mjDYN_INTEGRATOR) {
@@ -5387,7 +5434,6 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
       ((mjCGeom*)obj)->SetNotVisual();
     }
 
-    // get sensorized object id
   } else if (type != mjSENS_CLOCK && type != mjSENS_PLUGIN && type != mjSENS_USER) {
     throw mjCError(this, "invalid type in sensor");
   }
@@ -5401,7 +5447,7 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
 
     // find name
     if (!ref) {
-      throw mjCError(this, "unrecognized name '%s' of reference frame object", refname_.c_str());
+      throw mjCError(this, "unrecognized name '%s' of object", refname_.c_str());
     }
 
     // must be attached to object with spatial frame
@@ -5459,7 +5505,7 @@ void mjCSensor::Compile(void) {
   case mjSENS_CAMPROJECTION:
     // must be attached to site
     if (objtype!=mjOBJ_SITE) {
-      throw mjCError(this, "sensor must be attached to site: sensor");
+      throw mjCError(this, "sensor must be attached to site");
     }
 
     // set dim and datatype
@@ -5497,7 +5543,7 @@ void mjCSensor::Compile(void) {
   case mjSENS_JOINTACTFRC:
     // must be attached to joint
     if (objtype!=mjOBJ_JOINT) {
-      throw mjCError(this, "sensor must be attached to joint: sensor");
+      throw mjCError(this, "sensor must be attached to joint");
     }
 
     // make sure joint is slide or hinge
@@ -5521,7 +5567,7 @@ void mjCSensor::Compile(void) {
   case mjSENS_TENDONVEL:
     // must be attached to tendon
     if (objtype!=mjOBJ_TENDON) {
-      throw mjCError(this, "sensor must be attached to tendon: sensor");
+      throw mjCError(this, "sensor must be attached to tendon");
     }
 
     // set
@@ -5539,7 +5585,7 @@ void mjCSensor::Compile(void) {
   case mjSENS_ACTUATORFRC:
     // must be attached to actuator
     if (objtype!=mjOBJ_ACTUATOR) {
-      throw mjCError(this, "sensor must be attached to actuator: sensor");
+      throw mjCError(this, "sensor must be attached to actuator");
     }
 
     // set
@@ -5558,7 +5604,7 @@ void mjCSensor::Compile(void) {
   case mjSENS_BALLANGVEL:
     // must be attached to joint
     if (objtype!=mjOBJ_JOINT) {
-      throw mjCError(this, "sensor must be attached to joint: sensor");
+      throw mjCError(this, "sensor must be attached to joint");
     }
 
     // make sure joint is ball
@@ -5583,7 +5629,7 @@ void mjCSensor::Compile(void) {
   case mjSENS_JOINTLIMITFRC:
     // must be attached to joint
     if (objtype!=mjOBJ_JOINT) {
-      throw mjCError(this, "sensor must be attached to joint: sensor");
+      throw mjCError(this, "sensor must be attached to joint");
     }
 
     // make sure joint has limit
@@ -5608,7 +5654,7 @@ void mjCSensor::Compile(void) {
   case mjSENS_TENDONLIMITFRC:
     // must be attached to tendon
     if (objtype!=mjOBJ_TENDON) {
-      throw mjCError(this, "sensor must be attached to tendon: sensor");
+      throw mjCError(this, "sensor must be attached to tendon");
     }
 
     // make sure tendon has limit
@@ -5674,7 +5720,7 @@ void mjCSensor::Compile(void) {
   case mjSENS_SUBTREEANGMOM:
     // must be attached to body
     if (objtype!=mjOBJ_BODY) {
-      throw mjCError(this, "sensor must be attached to body: sensor");
+      throw mjCError(this, "sensor must be attached to body");
     }
 
     // set
@@ -5687,6 +5733,34 @@ void mjCSensor::Compile(void) {
     }
     break;
 
+  case mjSENS_GEOMDIST:
+  case mjSENS_GEOMNORMAL:
+  case mjSENS_GEOMFROMTO:
+    // must be attached to body or geom
+    if ((objtype!=mjOBJ_BODY && objtype!=mjOBJ_GEOM) ||
+        (reftype!=mjOBJ_BODY && reftype!=mjOBJ_GEOM)) {
+      throw mjCError(this, "sensor must be attached to body or geom");
+    }
+
+    // objects must be different
+    if (objtype == reftype && obj == ref) {
+      throw mjCError(this, "1st body/geom must be different from 2nd body/geom");
+    }
+
+    // set
+    needstage = mjSTAGE_POS;
+    if (type==mjSENS_GEOMDIST) {
+      dim = 1;
+      datatype = mjDATATYPE_POSITIVE;
+    } else if (type==mjSENS_GEOMNORMAL) {
+      dim = 3;
+      datatype = mjDATATYPE_AXIS;
+    } else {
+      dim = 6;
+      datatype = mjDATATYPE_REAL;
+    }
+    break;
+
   case mjSENS_CLOCK:
     dim = 1;
     needstage = mjSTAGE_POS;
@@ -5704,7 +5778,7 @@ void mjCSensor::Compile(void) {
       throw mjCError(this,
                      "datatype AXIS requires dim=3 in sensor");
     }
-    if (datatype==mjDATATYPE_QUATERNION && dim!=4) {
+    if (datatype==mjDATATYPE_QUATERNION && dim != 4) {
       throw mjCError(this, "datatype QUATERNION requires dim=4 in sensor");
     }
     break;
@@ -5736,7 +5810,8 @@ void mjCSensor::Compile(void) {
   }
 
   // check cutoff for incompatible data types
-  if (cutoff>0 && (datatype==mjDATATYPE_AXIS || datatype==mjDATATYPE_QUATERNION)) {
+  if (cutoff > 0 && (datatype == mjDATATYPE_QUATERNION ||
+                     (datatype == mjDATATYPE_AXIS && type != mjSENS_GEOMNORMAL))) {
     throw mjCError(this, "cutoff applied to axis or quaternion datatype in sensor");
   }
 }

src/user/user_objects.h

@@ -107,8 +107,8 @@ class mjCBoundingVolume {
   void SetId(int val) { idval_ = val; }
 
  private:
-  int idval_;                   // local id copy for nodes not storing their id's (e.g. faces)
+  int idval_;      // local id copy for nodes not storing their id's (e.g. faces)
-  const int* id_;               // pointer to object id
+  const int* id_;  // pointer to object id
 };
 
 
@@ -208,10 +208,11 @@ class mjCBase : public mjCBase_ {
   mjCFrame* frame;                // pointer to frame transformation
   mjCModel* model;                // pointer to model that created object
 
+  virtual ~mjCBase() = default;   // destructor
+
  protected:
   mjCBase();                                 // constructor
   mjCBase(const mjCBase& other);             // copy constructor
-  virtual ~mjCBase() = default;              // destructor
 };
 
 
@@ -293,6 +294,9 @@ class mjCBody : public mjCBody_, private mjsBody {
   // set explicitinertial to true
   void MakeInertialExplicit();
 
+  // compute the bounding volume hierarchy of the body.
+  void ComputeBVH();
+
   // variables set by user
   mjsBody spec;
 
@@ -304,6 +308,9 @@ class mjCBody : public mjCBody_, private mjsBody {
   // used by mjXWriter and mjCModel
   const std::vector<double>& get_userdata() { return userdata_; }
 
+  // get next child of given type
+  mjElement* NextChild(mjElement* child, mjtObj type = mjOBJ_UNKNOWN);
+
  private:
   mjCBody(const mjCBody& other, mjCModel* _model);  // copy constructor
   mjCBody& operator=(const mjCBody& other);         // copy assignment
@@ -351,6 +358,10 @@ class mjCFrame : public mjCFrame_, private mjsFrame {
   friend class mjCModel;
 
  public:
+  mjCFrame(mjCModel* = 0, mjCFrame* = 0);
+  mjCFrame(const mjCFrame& other);
+  mjCFrame& operator=(const mjCFrame& other);
+
   mjsFrame spec;
   using mjCBase::name;
   using mjCBase::classname;
@@ -365,9 +376,6 @@ class mjCFrame : public mjCFrame_, private mjsFrame {
   bool IsAncestor(const mjCFrame* child) const;  // true if child is contained in this frame
 
  private:
-  mjCFrame(mjCModel* = 0, mjCFrame* = 0);      // constructor
-  mjCFrame(const mjCFrame& other);             // copy constructor
-  mjCFrame& operator=(const mjCFrame& other);  // copy assignment
   void Compile(void);                          // compiler
 
   mjCBody* body;  // body that owns the frame
@@ -397,6 +405,10 @@ class mjCJoint : public mjCJoint_, private mjsJoint {
   friend class mjXURDF;
 
  public:
+  mjCJoint(mjCModel* = nullptr, mjCDef* = nullptr);
+  mjCJoint(const mjCJoint& other);
+  mjCJoint& operator=(const mjCJoint& other);
+
   mjsJoint spec;
   using mjCBase::name;
   using mjCBase::classname;
@@ -405,18 +417,14 @@ class mjCJoint : public mjCJoint_, private mjsJoint {
   void CopyFromSpec(void);
 
   // used by mjXWriter and mjCModel
-  const std::vector<double>& get_userdata() { return userdata_; }
+  const std::vector<double>& get_userdata() const { return userdata_; }
-  const double* get_range() { return range; }
+  const double* get_range() const { return range; }
 
   bool is_limited() const;
   bool is_actfrclimited() const;
 
 
  private:
-  mjCJoint(mjCModel* = 0, mjCDef* = 0);
-  mjCJoint(const mjCJoint& other);             // copy constructor
-  mjCJoint& operator=(const mjCJoint& other);  // copy assignment
-
   int Compile(void);               // compiler; return dofnum
   void PointToLocal(void);
 };
@@ -465,9 +473,13 @@ class mjCGeom : public mjCGeom_, private mjsGeom {
   friend class mjXURDF;
 
  public:
+  mjCGeom(mjCModel* = nullptr, mjCDef* = nullptr);
+  mjCGeom(const mjCGeom& other);
+  mjCGeom& operator=(const mjCGeom& other);
+
   using mjCBase::name;
   mjsGeom spec;                       // variables set by user
-  double GetVolume(void);             // compute geom volume
+  double GetVolume() const;           // compute geom volume
   void SetInertia(void);              // compute and set geom inertia
   bool IsVisual(void) const { return visual_; }
   void SetNotVisual(void) { visual_ = false; }
@@ -481,17 +493,13 @@ class mjCGeom : public mjCGeom_, private mjsGeom {
   void SetBoundingVolume(mjCBoundingVolume* bv) const;
 
   // used by mjXWriter and mjCModel
-  const std::vector<double>& get_userdata() { return userdata_; }
+  const std::vector<double>& get_userdata() const { return userdata_; }
-  const std::string& get_hfieldname() { return spec_hfieldname_; }
+  const std::string& get_hfieldname() const { return spec_hfieldname_; }
-  const std::string& get_meshname() { return spec_meshname_; }
+  const std::string& get_meshname() const { return spec_meshname_; }
-  const std::string& get_material() { return spec_material_; }
+  const std::string& get_material() const { return spec_material_; }
   void del_material() { spec_material_.clear(); }
 
  private:
-  mjCGeom(mjCModel* = 0, mjCDef* = 0);
-  mjCGeom(const mjCGeom& other);             // copy constructor
-  mjCGeom& operator=(const mjCGeom& other);  // copy assignment
-
   void Compile(void);                 // compiler
   double GetRBound(void);             // compute bounding sphere radius
   void ComputeAABB(void);             // compute axis-aligned bounding box
@@ -530,6 +538,10 @@ class mjCSite : public mjCSite_, private mjsSite {
   friend class mjXURDF;
 
  public:
+  mjCSite(mjCModel* = nullptr, mjCDef* = nullptr);
+  mjCSite(const mjCSite& other);
+  mjCSite& operator=(const mjCSite& other);
+
   mjsSite spec;                   // variables set by user
 
   // use strings from mjCBase rather than mjStrings from mjsSite
@@ -538,15 +550,11 @@ class mjCSite : public mjCSite_, private mjsSite {
   using mjCBase::info;
 
   // used by mjXWriter and mjCModel
-  const std::vector<double>& get_userdata() { return userdata_; }
+  const std::vector<double>& get_userdata() const { return userdata_; }
-  const std::string& get_material() { return material_; }
+  const std::string& get_material() const { return material_; }
   void del_material() { material_.clear(); }
 
  private:
-  mjCSite(mjCModel* = 0, mjCDef* = 0);       // constructor
-  mjCSite(const mjCSite& other);             // copy constructor
-  mjCSite& operator=(const mjCSite& other);  // copy assignment
-
   void Compile(void);                     // compiler
   void CopyFromSpec();                    // copy spec into attributes
   void PointToLocal(void);
@@ -575,20 +583,20 @@ class mjCCamera : public mjCCamera_, private mjsCamera {
   friend class mjXWriter;
 
  public:
+  mjCCamera(mjCModel* = nullptr, mjCDef* = nullptr);
+  mjCCamera(const mjCCamera& other);
+  mjCCamera& operator=(const mjCCamera& other);
+
   mjsCamera spec;
   using mjCBase::name;
   using mjCBase::classname;
   using mjCBase::info;
 
   // used by mjXWriter and mjCModel
-  const std::string& get_targetbody() { return targetbody_; }
+  const std::string& get_targetbody() const { return targetbody_; }
-  const std::vector<double>& get_userdata() { return userdata_; }
+  const std::vector<double>& get_userdata() const { return userdata_; }
 
  private:
-  mjCCamera(mjCModel* = 0, mjCDef* = 0);         // constructor
-  mjCCamera(const mjCCamera& other);             // copy constructor
-  mjCCamera& operator=(const mjCCamera& other);  // copy assignment
-
   void Compile(void);                     // compiler
   void CopyFromSpec(void);
   void PointToLocal(void);
@@ -615,19 +623,19 @@ class mjCLight : public mjCLight_, private mjsLight {
   friend class mjXWriter;
 
  public:
+  mjCLight(mjCModel* = nullptr, mjCDef* = nullptr);
+  mjCLight(const mjCLight& other);
+  mjCLight& operator=(const mjCLight& other);
+
   mjsLight spec;
   using mjCBase::name;
   using mjCBase::classname;
   using mjCBase::info;
 
   // used by mjXWriter and mjCModel
-  const std::string& get_targetbody() { return targetbody_; }
+  const std::string& get_targetbody() const { return targetbody_; }
 
  private:
-  mjCLight(mjCModel* = 0, mjCDef* = 0);        // constructor
-  mjCLight(const mjCLight& other);             // copy constructor
-  mjCLight& operator=(const mjCLight& other);  // copy assignment
-
   void Compile(void);                     // compiler
   void CopyFromSpec(void);
   void PointToLocal(void);
@@ -678,6 +686,10 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
   friend class mjXWriter;
 
  public:
+  mjCFlex(mjCModel* = nullptr);
+  mjCFlex(const mjCFlex& other);
+  mjCFlex& operator=(const mjCFlex& other);
+
   mjsFlex spec;
   using mjCBase::name;
   using mjCBase::classname;
@@ -689,21 +701,17 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
   void NameSpace(const mjCModel* m);
 
   // used by mjXWriter and mjCModel
-  const std::string& get_material() { return material_; }
+  const std::string& get_material() const { return material_; }
-  const std::vector<std::string>& get_vertbody() { return vertbody_; }
+  const std::vector<std::string>& get_vertbody() const { return vertbody_; }
-  const std::vector<double>& get_vert() { return vert_; }
+  const std::vector<double>& get_vert() const { return vert_; }
-  const std::vector<double>& get_elemaabb() { return elemaabb_; }
+  const std::vector<double>& get_elemaabb() const { return elemaabb_; }
-  const std::vector<int>& get_elem() { return elem_; }
+  const std::vector<int>& get_elem() const { return elem_; }
-  const std::vector<float>& get_texcoord() { return texcoord_; }
+  const std::vector<float>& get_texcoord() const { return texcoord_; }
 
   bool HasTexcoord() const;               // texcoord not null
   void DelTexcoord();                     // delete texcoord
 
  private:
-  mjCFlex(mjCModel* = 0);
-  mjCFlex(const mjCFlex& other);             // copy constructor
-  mjCFlex& operator=(const mjCFlex& other);  // copy assignment
-
   void Compile(const mjVFS* vfs);         // compiler
   void CreateBVH(void);                   // create flex BVH
   void CreateShellPair(void);             // create shells and evpairs
@@ -776,9 +784,9 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
   friend class mjCFlexcomp;
   friend class mjXWriter;
  public:
-  mjCMesh(mjCModel* = 0, mjCDef* = 0);
+  mjCMesh(mjCModel* = nullptr, mjCDef* = nullptr);
-  mjCMesh(const mjCMesh& other);             // copy constructor
+  mjCMesh(const mjCMesh& other);
-  mjCMesh& operator=(const mjCMesh& other);  // copy assignment
+  mjCMesh& operator=(const mjCMesh& other);
   ~mjCMesh();
 
   mjsMesh spec;
@@ -907,32 +915,32 @@ class mjCSkin: public mjCSkin_, private mjsSkin {
   friend class mjXWriter;
 
  public:
+  mjCSkin(mjCModel* = nullptr);
+  mjCSkin(const mjCSkin& other);
+  mjCSkin& operator=(const mjCSkin& other);
+  ~mjCSkin();
+
   mjsSkin spec;
   using mjCBase::name;
   using mjCBase::classname;
   using mjCBase::info;
 
-  std::string get_file() const { return file_; }
+  const std::string& get_file() const { return file_; }
-  std::string& get_material() { return material_; }
+  const std::string& get_material() const { return material_; }
-  std::vector<float>& get_vert() { return vert_; }
+  const std::vector<float>& get_vert() const { return vert_; }
-  std::vector<float>& get_texcoord() { return texcoord_; }
+  const std::vector<float>& get_texcoord() const { return texcoord_; }
-  std::vector<int>& get_face() { return face_; }
+  const std::vector<int>& get_face() const { return face_; }
-  std::vector<std::string>& get_bodyname() { return bodyname_; }
+  const std::vector<std::string>& get_bodyname() const { return bodyname_; }
-  std::vector<float>& get_bindpos() { return bindpos_; }
+  const std::vector<float>& get_bindpos() const { return bindpos_; }
-  std::vector<float>& get_bindquat() { return bindquat_; }
+  const std::vector<float>& get_bindquat() const { return bindquat_; }
-  std::vector<std::vector<int>>& get_vertid() { return vertid_; }
+  const std::vector<std::vector<int>>& get_vertid() const { return vertid_; }
-  std::vector<std::vector<float>>& get_vertweight() { return vertweight_; }
+  const std::vector<std::vector<float>>& get_vertweight() const { return vertweight_; }
   void del_material() { material_.clear(); }
 
   void CopyFromSpec();
   void PointToLocal();
 
  private:
-  mjCSkin(mjCModel* = 0);                     // constructor
-  mjCSkin(const mjCSkin& other);             // copy constructor
-  mjCSkin& operator=(const mjCSkin& other);  // copy assignment
-  ~mjCSkin();                                 // destructor
-
   void ResolveReferences(const mjCModel* m);
   void NameSpace(const mjCModel* m);
   void Compile(const mjVFS* vfs);             // compiler
@@ -962,6 +970,11 @@ class mjCHField : public mjCHField_, private mjsHField {
   friend class mjXWriter;
 
  public:
+  mjCHField(mjCModel* model);
+  mjCHField(const mjCHField& other);
+  mjCHField& operator=(const mjCHField& other);
+  ~mjCHField();
+
   mjsHField spec;
   using mjCBase::name;
   using mjCBase::info;
@@ -975,11 +988,6 @@ class mjCHField : public mjCHField_, private mjsHField {
   std::vector<float>& get_userdata() { return userdata_; }
 
  private:
-  mjCHField(mjCModel* model);                    // constructor
-  mjCHField(const mjCHField& other);             // copy constructor
-  mjCHField& operator=(const mjCHField& other);  // copy assignment
-  ~mjCHField();                                  // destructor
-
   void Compile(const mjVFS* vfs);         // compiler
 
   void LoadCustom(mjResource* resource);  // load from custom format
@@ -1009,7 +1017,10 @@ class mjCTexture : public mjCTexture_, private mjsTexture {
   friend class mjXWriter;
 
  public:
-  ~mjCTexture();                  // destructor
+  mjCTexture(mjCModel*);
+  mjCTexture(const mjCTexture& other);
+  mjCTexture& operator=(const mjCTexture& other);
+  ~mjCTexture();
 
   mjsTexture spec;
   using mjCBase::name;
@@ -1024,10 +1035,6 @@ class mjCTexture : public mjCTexture_, private mjsTexture {
   std::vector<std::string> get_cubefiles() const { return cubefiles_; }
 
  private:
-  mjCTexture(mjCModel*);                           // constructor
-  mjCTexture(const mjCTexture& other);             // copy constructor
-  mjCTexture& operator=(const mjCTexture& other);  // copy assignment
-
   void Compile(const mjVFS* vfs);         // compiler
 
   void Builtin2D(void);                   // make builtin 2D
@@ -1066,6 +1073,10 @@ class mjCMaterial : public mjCMaterial_, private mjsMaterial {
   friend class mjXWriter;
 
  public:
+  mjCMaterial(mjCModel* = nullptr, mjCDef* = nullptr);
+  mjCMaterial(const mjCMaterial& other);
+  mjCMaterial& operator=(const mjCMaterial& other);
+
   mjsMaterial spec;
   using mjCBase::name;
   using mjCBase::classname;
@@ -1075,14 +1086,10 @@ class mjCMaterial : public mjCMaterial_, private mjsMaterial {
   void PointToLocal();
   void NameSpace(const mjCModel* m);
 
-  std::string get_texture() { return texture_; }
+  const std::string& get_texture() const { return texture_; }
   void del_texture() { texture_.clear(); }
 
  private:
-  mjCMaterial(mjCModel* = 0, mjCDef* = 0);           // constructor
-  mjCMaterial(const mjCMaterial& other);             // copy constructor
-  mjCMaterial& operator=(const mjCMaterial& other);  // copy assignment
-
   void Compile(void);                       // compiler
 };
 
@@ -1107,6 +1114,10 @@ class mjCPair : public mjCPair_, private mjsPair {
   friend class mjXWriter;
 
  public:
+  mjCPair(mjCModel* = nullptr, mjCDef* = nullptr);
+  mjCPair(const mjCPair& other);
+  mjCPair& operator=(const mjCPair& other);
+
   mjsPair spec;
   using mjCBase::name;
   using mjCBase::classname;
@@ -1117,18 +1128,14 @@ class mjCPair : public mjCPair_, private mjsPair {
   void ResolveReferences(const mjCModel* m);
   void NameSpace(const mjCModel* m);
 
-  std::string get_geomname1() { return geomname1_; }
+  const std::string& get_geomname1() const { return geomname1_; }
-  std::string get_geomname2() { return geomname2_; }
+  const std::string& get_geomname2() const { return geomname2_; }
 
   int GetSignature(void) {
     return signature;
   }
 
  private:
-  mjCPair(mjCModel* = 0, mjCDef* = 0);       // constructor
-  mjCPair(const mjCPair& other);             // copy constructor
-  mjCPair& operator=(const mjCPair& other);  // copy assignment
-
   void Compile(void);                   // compiler
 
   mjCGeom* geom1;                 // geom1
@@ -1157,6 +1164,10 @@ class mjCBodyPair : public mjCBodyPair_, private mjsExclude {
   friend class mjCModel;
 
  public:
+  mjCBodyPair(mjCModel*);
+  mjCBodyPair(const mjCBodyPair& other);
+  mjCBodyPair& operator=(const mjCBodyPair& other);
+
   mjsExclude spec;
   using mjCBase::name;
   using mjCBase::info;
@@ -1174,9 +1185,6 @@ class mjCBodyPair : public mjCBodyPair_, private mjsExclude {
   }
 
  private:
-  mjCBodyPair(mjCModel*);                            // constructor
-  mjCBodyPair(const mjCBodyPair& other);             // copy constructor
-  mjCBodyPair& operator=(const mjCBodyPair& other);  // copy assignment
 
   void Compile(void);              // compiler
 };
@@ -1203,6 +1211,10 @@ class mjCEquality : public mjCEquality_, private mjsEquality {
   friend class mjXWriter;
 
  public:
+  mjCEquality(mjCModel* = 0, mjCDef* = 0);
+  mjCEquality(const mjCEquality& other);
+  mjCEquality& operator=(const mjCEquality& other);
+
   mjsEquality spec;
   using mjCBase::name;
   using mjCBase::classname;
@@ -1214,10 +1226,6 @@ class mjCEquality : public mjCEquality_, private mjsEquality {
   void NameSpace(const mjCModel* m);
 
  private:
-  mjCEquality(mjCModel* = 0, mjCDef* = 0);           // constructor
-  mjCEquality(const mjCEquality& other);             // copy constructor
-  mjCEquality& operator=(const mjCEquality& other);  // copy assignment
-
   void Compile(void);                       // compiler
 };
 
@@ -1243,13 +1251,18 @@ class mjCTendon : public mjCTendon_, private mjsTendon {
   friend class mjXWriter;
 
  public:
+  mjCTendon(mjCModel* = nullptr, mjCDef* = nullptr);
+  mjCTendon(const mjCTendon& other);
+  mjCTendon& operator=(const mjCTendon& other);
+  ~mjCTendon();
+
   mjsTendon spec;
   using mjCBase::name;
   using mjCBase::classname;
   using mjCBase::info;
 
   void set_material(std::string _material) { material_ = _material; }
-  std::string& get_material() { return material_; }
+  const std::string& get_material() const { return material_; }
   void del_material() { material_.clear(); }
 
   // API for adding wrapping objects
@@ -1259,12 +1272,12 @@ class mjCTendon : public mjCTendon_, private mjsTendon {
   void WrapPulley(double divisor, std::string_view info = "");                    // pulley
 
   // API for access to wrapping objects
-  int NumWraps(void);                         // number of wraps
+  int NumWraps() const;                       // number of wraps
-  mjCWrap* GetWrap(int);                      // pointer to wrap
+  const mjCWrap* GetWrap(int i) const;        // pointer to wrap
   std::vector<mjCWrap*> path;                 // wrapping objects
 
   // used by mjXWriter and mjCModel
-  const std::vector<double>& get_userdata() { return userdata_; }
+  const std::vector<double>& get_userdata() const { return userdata_; }
   const double* get_range() { return range; }
 
   void CopyFromSpec();
@@ -1276,11 +1289,6 @@ class mjCTendon : public mjCTendon_, private mjsTendon {
   bool is_limited() const;
 
  private:
-  mjCTendon(mjCModel* = 0, mjCDef* = 0);         // constructor
-  mjCTendon(const mjCTendon& other);             // copy constructor
-  mjCTendon& operator=(const mjCTendon& other);  // copy assignment
-  ~mjCTendon();                                  // destructor
-
   void Compile(void);                         // compiler
 };
 
@@ -1339,14 +1347,13 @@ class mjCPlugin : public mjCPlugin_ {
   friend class mjXWriter;
 
  public:
+  mjCPlugin(mjCModel*);
+  mjCPlugin(const mjCPlugin& other);
+  mjCPlugin& operator=(const mjCPlugin& other);
   mjsPlugin spec;
   mjCBase* parent;   // parent object (only used when generating error message)
-  mjCPlugin(const mjCPlugin& other);             // copy constructor
 
  private:
-  mjCPlugin(mjCModel*);                          // constructor
-  mjCPlugin& operator=(const mjCPlugin& other);  // copy assignment
-
   void Compile(void);              // compiler
 };
 
@@ -1378,26 +1385,26 @@ class mjCActuator : public mjCActuator_, private mjsActuator {
   friend class mjXWriter;
 
  public:
+  mjCActuator(mjCModel* = nullptr, mjCDef* = nullptr);
+  mjCActuator(const mjCActuator& other);
+  mjCActuator& operator=(const mjCActuator& other);
+
   mjsActuator spec;
   using mjCBase::name;
   using mjCBase::classname;
   using mjCBase::info;
 
   // used by mjXWriter and mjCModel
-  const std::vector<double>& get_userdata() { return userdata_; }
+  const std::vector<double>& get_userdata() const { return userdata_; }
-  const std::string& get_target() { return spec_target_; }
+  const std::string& get_target() const { return spec_target_; }
-  const std::string& get_slidersite() { return spec_slidersite_; }
+  const std::string& get_slidersite() const { return spec_slidersite_; }
-  const std::string& get_refsite() { return spec_refsite_; }
+  const std::string& get_refsite() const { return spec_refsite_; }
 
   bool is_ctrllimited() const;
   bool is_forcelimited() const;
   bool is_actlimited() const;
 
  private:
-  mjCActuator(mjCModel* = 0, mjCDef* = 0);           // constructor
-  mjCActuator(const mjCActuator& other);             // copy constructor
-  mjCActuator& operator=(const mjCActuator& other);  // copy assignment
-
   void Compile(void);                       // compiler
   void CopyFromSpec();
   void PointToLocal();
@@ -1433,6 +1440,10 @@ class mjCSensor : public mjCSensor_, private mjsSensor {
   friend class mjXWriter;
 
  public:
+  mjCSensor(mjCModel*);
+  mjCSensor(const mjCSensor& other);
+  mjCSensor& operator=(const mjCSensor& other);
+
   mjsSensor spec;
   using mjCBase::name;
   using mjCBase::classname;
@@ -1444,10 +1455,6 @@ class mjCSensor : public mjCSensor_, private mjsSensor {
   const std::string& get_refname() { return spec_refname_; }
 
  private:
-  mjCSensor(mjCModel*);                          // constructor
-  mjCSensor(const mjCSensor& other);             // copy constructor
-  mjCSensor& operator=(const mjCSensor& other);  // copy assignment
-
   void Compile(void);             // compiler
   void CopyFromSpec();
   void PointToLocal();
@@ -1474,6 +1481,11 @@ class mjCNumeric : public mjCNumeric_, private mjsNumeric {
   friend class mjXWriter;
 
  public:
+  mjCNumeric(mjCModel*);
+  mjCNumeric(const mjCNumeric& other);
+  mjCNumeric& operator=(const mjCNumeric& other);
+  ~mjCNumeric();
+
   mjsNumeric spec;
   using mjCBase::name;
   using mjCBase::info;
@@ -1482,11 +1494,6 @@ class mjCNumeric : public mjCNumeric_, private mjsNumeric {
   void CopyFromSpec();
 
  private:
-  mjCNumeric(mjCModel*);                           // constructor
-  mjCNumeric(const mjCNumeric& other);             // copy constructor
-  mjCNumeric& operator=(const mjCNumeric& other);  // copy assignment
-  ~mjCNumeric();                                   // destructor
-
   void Compile(void);                 // compiler
 };
 
@@ -1506,6 +1513,11 @@ class mjCText : public mjCText_, private mjsText {
   friend class mjXWriter;
 
  public:
+  mjCText(mjCModel*);
+  mjCText(const mjCText& other);
+  mjCText& operator=(const mjCText& other);
+  ~mjCText();
+
   mjsText spec;
   using mjCBase::name;
   using mjCBase::info;
@@ -1514,11 +1526,6 @@ class mjCText : public mjCText_, private mjsText {
   void CopyFromSpec();
 
  private:
-  mjCText(mjCModel*);                        // constructor
-  mjCText(const mjCText& other);             // copy constructor
-  mjCText& operator=(const mjCText& other);  // copy assignment
-  ~mjCText();                                // destructor
-
   void Compile(void);                 // compiler
 };
 
@@ -1543,6 +1550,11 @@ class mjCTuple : public mjCTuple_, private mjsTuple {
   friend class mjXWriter;
 
  public:
+  mjCTuple(mjCModel*);
+  mjCTuple(const mjCTuple& other);
+  mjCTuple& operator=(const mjCTuple& other);
+  ~mjCTuple();
+
   mjsTuple spec;
   using mjCBase::name;
   using mjCBase::info;
@@ -1553,11 +1565,6 @@ class mjCTuple : public mjCTuple_, private mjsTuple {
   void NameSpace(const mjCModel* m);
 
  private:
-  mjCTuple(mjCModel*);                         // constructor
-  mjCTuple(const mjCTuple& other);             // copy constructor
-  mjCTuple& operator=(const mjCTuple& other);  // copy assignment
-  ~mjCTuple();                                 // destructor
-
   void Compile(void);             // compiler
 };
 
@@ -1587,6 +1594,11 @@ class mjCKey : public mjCKey_, private mjsKey {
   friend class mjXWriter;
 
  public:
+  mjCKey(mjCModel*);
+  mjCKey(const mjCKey& other);
+  mjCKey& operator=(const mjCKey& other);
+  ~mjCKey();
+
   mjsKey spec;
   using mjCBase::name;
   using mjCBase::info;
@@ -1595,11 +1607,6 @@ class mjCKey : public mjCKey_, private mjsKey {
   void CopyFromSpec();
 
  private:
-  mjCKey(mjCModel*);                       // constructor
-  mjCKey(const mjCKey& other);             // copy constructor
-  mjCKey& operator=(const mjCKey& other);  // copy assignment
-  ~mjCKey();                               // destructor
-
   void Compile(const mjModel* m);  // compiler
 };
 
@@ -1612,13 +1619,29 @@ class mjCDef : public mjElement {
   friend class mjXWriter;
 
  public:
-  mjCDef(void);                            // constructor
+  mjCDef();
-  mjCDef(const mjCDef& other);             // copy constructor
+  mjCDef(const mjCDef& other);
-  void Compile(const mjCModel* model);     // compiler
+  mjCDef& operator=(const mjCDef& other);
-  mjCDef& operator=(const mjCDef& other);  // copy assignment
+
   void PointToLocal(void);
   void CopyFromSpec(void);
 
+  void Compile(const mjCModel* model);
+
+  // accessors
+  mjCJoint& Joint() { return joint_; }
+  mjCGeom& Geom() { return geom_; }
+  mjCSite& Site() { return site_; }
+  mjCCamera& Camera() { return camera_; }
+  mjCLight& Light() { return light_; }
+  mjCFlex& Flex() { return flex_; }
+  mjCMesh& Mesh() { return mesh_; }
+  mjCMaterial& Material() { return material_; }
+  mjCPair& Pair() { return pair_; }
+  mjCEquality& Equality() { return equality_; }
+  mjCTendon& Tendon() { return tendon_; }
+  mjCActuator& Actuator() { return actuator_; }
+
   // identifiers
   std::string name;               // class name
   int parentid;                   // id of parent class
@@ -1626,19 +1649,19 @@ class mjCDef : public mjElement {
 
   mjsDefault spec;
 
-  // default objects (TODO: they should become private)
+ private:
-  mjCJoint    joint;
+  mjCJoint joint_;
-  mjCGeom     geom;
+  mjCGeom geom_;
-  mjCSite     site;
+  mjCSite site_;
-  mjCCamera   camera;
+  mjCCamera camera_;
-  mjCLight    light;
+  mjCLight light_;
-  mjCFlex     flex;
+  mjCFlex flex_;
-  mjCMesh     mesh;
+  mjCMesh mesh_;
-  mjCMaterial material;
+  mjCMaterial material_;
-  mjCPair     pair;
+  mjCPair pair_;
-  mjCEquality equality;
+  mjCEquality equality_;
-  mjCTendon   tendon;
+  mjCTendon tendon_;
-  mjCActuator actuator;
+  mjCActuator actuator_;
 };
 
 #endif  // MUJOCO_SRC_USER_USER_OBJECTS_H_

src/user/user_vfs.c

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "engine/engine_vfs.h"
+#include "user/user_vfs.h"
 
 #include <stddef.h>
 #include <stdint.h>

src/user/user_vfs.h

@@ -11,9 +11,11 @@
 // 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/mujoco/src/.*"
 
-#ifndef MUJOCO_SRC_ENGINE_ENGINE_VFS_H_
+#ifndef MUJOCO_SRC_USER_USER_VFS_H_
-#define MUJOCO_SRC_ENGINE_ENGINE_VFS_H_
+#define MUJOCO_SRC_USER_USER_VFS_H_
 
 #include <stddef.h>
 
@@ -53,4 +55,4 @@ MJAPI mjResource* mju_openVfsResource(const char* name, const mjVFS* vfs);
 }
 #endif
 
-#endif  // MUJOCO_SRC_ENGINE_ENGINE_VFS_H_
+#endif  // MUJOCO_SRC_USER_USER_VFS_H_

src/xml/xml.cc

@@ -35,9 +35,9 @@
 #include "cc/array_safety.h"
 #include "engine/engine_crossplatform.h"
 #include "engine/engine_resource.h"
-#include "engine/engine_vfs.h"
 #include "user/user_api.h"
 #include "user/user_util.h"
+#include "user/user_vfs.h"
 #include "xml/xml_native_reader.h"
 #include "xml/xml_native_writer.h"
 #include "xml/xml_urdf.h"
@@ -100,17 +100,17 @@ class LocaleOverride {
 }  // namespace
 
 // Main writer function - calls mjXWrite
-std::string mjWriteXML(mjSpec* model, char* error, int error_sz) {
+std::string mjWriteXML(mjSpec* spec, char* error, int error_sz) {
   LocaleOverride locale_override;
 
   // check for empty model
-  if (!model) {
+  if (!spec) {
     mjCopyError(error, "Cannot write empty model", error_sz);
     return "";
   }
 
   mjXWriter writer;
-  writer.SetModel(model);
+  writer.SetModel(spec);
   return writer.Write(error, error_sz);
 }
 
@@ -286,7 +286,7 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
   }
 
   // clear
-  mjSpec* model = nullptr;
+  mjSpec* spec = nullptr;
   if (error) {
     error[0] = '\0';
   }
@@ -345,14 +345,14 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
   }
 
   // create model, set filedir
-  model = mjs_createSpec();
+  spec = mjs_createSpec();
   const char* dir;
   int ndir = 0;
   mju_getResourceDir(resource, &dir, &ndir);
   if (dir != nullptr) {
-    mjs_setString(model->modelfiledir, std::string(dir, ndir).c_str());
+    mjs_setString(spec->modelfiledir, std::string(dir, ndir).c_str());
   } else {
-    mjs_setString(model->modelfiledir, "");
+    mjs_setString(spec->modelfiledir, "");
   }
 
   // close resource
@@ -364,11 +364,11 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
       // find include elements, replace them with subtree from xml file
       std::unordered_set<std::string> included = {filename};
       mjXReader parser;
-      parser.SetModelFileDir(mjs_getString(model->modelfiledir));
+      parser.SetModelFileDir(mjs_getString(spec->modelfiledir));
-      mjIncludeXML(parser, root, mjs_getString(model->modelfiledir), vfs, included);
+      mjIncludeXML(parser, root, mjs_getString(spec->modelfiledir), vfs, included);
 
       // parse MuJoCo model
-      parser.SetModel(model);
+      parser.SetModel(spec);
       parser.Parse(root);
     }
 
@@ -378,11 +378,11 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
 
       // set reasonable default for parsing a URDF
       // this is separate from the Parser to allow multiple URDFs to be loaded.
-      model->strippath = true;
+      spec->strippath = true;
-      model->fusestatic = true;
+      spec->fusestatic = true;
-      model->discardvisual = true;
+      spec->discardvisual = true;
 
-      parser.SetModel(model);
+      parser.SetModel(spec);
       parser.Parse(root);
     }
 
@@ -394,11 +394,11 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
   // catch known errors
   catch (mjXError err) {
     mjCopyError(error, err.message, error_sz);
-    mjs_deleteSpec(model);
+    mjs_deleteSpec(spec);
     return nullptr;
   }
 
-  return model;
+  return spec;
 }
 
 

src/xml/xml.h

@@ -24,7 +24,7 @@
 // Top level API
 
 // Main writer function
-std::string mjWriteXML(mjSpec* model, char* error, int error_sz);
+std::string mjWriteXML(mjSpec* spec, char* error, int error_sz);
 
 // Main parser function
 MJAPI mjSpec* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz);

src/xml/xml_api.cc

@@ -26,7 +26,10 @@
 #include <type_traits>
 
 #include <mujoco/mjmodel.h>
+#include "engine/engine_io.h"
+#include "engine/engine_resource.h"
 #include "user/user_api.h"
+#include "user/user_vfs.h"
 #include "xml/xml.h"
 #include "xml/xml_native_reader.h"
 #include "xml/xml_util.h"
@@ -37,7 +40,7 @@
 class GlobalModel {
  public:
   // deletes current model and takes ownership of model
-  void Set(mjSpec* model = nullptr);
+  void Set(mjSpec* spec = nullptr);
 
   // writes XML to string
   std::optional<std::string> ToXML(const mjModel* m, char* error,
@@ -46,30 +49,30 @@ class GlobalModel {
  private:
   // using raw pointers as GlobalModel needs to be trivially destructible
   std::mutex* mutex_ = new std::mutex();
-  mjSpec* model_ = nullptr;
+  mjSpec* spec_ = nullptr;
 };
 
 std::optional<std::string> GlobalModel::ToXML(const mjModel* m, char* error,
                                               int error_sz) {
   std::lock_guard<std::mutex> lock(*mutex_);
-  if (!model_) {
+  if (!spec_) {
     mjCopyError(error, "No XML model loaded", error_sz);
     return std::nullopt;
   }
-  mjs_copyBack(model_, m);
+  mjs_copyBack(spec_, m);
-  std::string result = mjWriteXML(model_, error, error_sz);
+  std::string result = mjWriteXML(spec_, error, error_sz);
   if (result.empty()) {
     return std::nullopt;
   }
   return result;
 }
 
-void GlobalModel::Set(mjSpec* model) {
+void GlobalModel::Set(mjSpec* spec) {
   std::lock_guard<std::mutex> lock(*mutex_);
-  if (model_ != nullptr) {
+  if (spec_ != nullptr) {
-    mjs_deleteSpec(model_);
+    mjs_deleteSpec(spec_);
   }
-  model_ = model;
+  spec_ = spec;
 }
 
 
@@ -91,29 +94,29 @@ mjModel* mj_loadXML(const char* filename, const mjVFS* vfs,
                     char* error, int error_sz) {
 
   // parse new model
-  std::unique_ptr<mjSpec, std::function<void(mjSpec*)>> model(
+  std::unique_ptr<mjSpec, std::function<void(mjSpec*)>> spec(
       mjParseXML(filename, vfs, error, error_sz),
-      [](mjSpec* m) { mjs_deleteSpec(m); });
+      [](mjSpec* s) { mjs_deleteSpec(s); });
-  if (!model) {
+  if (!spec) {
     return nullptr;
   }
 
   // compile new model
-  mjModel* m = mjs_compile(model.get(), vfs);
+  mjModel* m = mjs_compile(spec.get(), vfs);
   if (!m) {
-    mjCopyError(error, mjs_getError(model.get()), error_sz);
+    mjCopyError(error, mjs_getError(spec.get()), error_sz);
     return nullptr;
   }
 
   // handle compile warning
-  if (mjs_isWarning(model.get())) {
+  if (mjs_isWarning(spec.get())) {
-    mjCopyError(error, mjs_getError(model.get()), error_sz);
+    mjCopyError(error, mjs_getError(spec.get()), error_sz);
   } else if (error) {
     error[0] = '\0';
   }
 
   // clear old and assign new
-  GetGlobalModel().Set(model.release());
+  GetGlobalModel().Set(spec.release());
   return m;
 }
 
@@ -178,3 +181,31 @@ int mj_printSchema(const char* filename, char* buffer, int buffer_sz, int flg_ht
   // return string length
   return str.str().size();
 }
+
+
+
+// load model from binary MJB resource
+mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
+  mjResource* resource = nullptr;
+
+  // first try vfs, otherwise try a provider or OS filesystem
+  if (!(resource = mju_openVfsResource(filename, vfs))) {
+    char error[1024];
+    if (!(resource = mju_openResource(filename, error, 1024))) {
+       mju_warning("%s", error);
+      return nullptr;
+    }
+  }
+
+  const void* buffer = NULL;
+  int buffer_sz = mju_readResource(resource, &buffer);
+  if (buffer_sz < 1) {
+    mju_closeResource(resource);
+    return nullptr;
+  }
+
+  mjModel* m = mj_loadModelBuffer(buffer, buffer_sz);
+  mju_closeResource(resource);
+  return m;
+}
+

src/xml/xml_api.h

@@ -39,6 +39,9 @@ MJAPI void mj_freeLastXML(void);
 MJAPI int mj_printSchema(const char* filename, char* buffer, int buffer_sz,
                          int flg_html, int flg_pad);
 
+// load model from binary MJB file
+// if vfs is not NULL, look up file in vfs before reading from disk
+MJAPI mjModel* mj_loadModel(const char* filename, const mjVFS* vfs);
 
 #ifdef __cplusplus
 }

src/xml/xml_native_reader.cc

@@ -177,19 +177,16 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
             "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"},
         {"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
             "gear", "cranklength", "user", "group"},
-        {"position", "?", "11", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange",
+        {"position", "?", "13", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange",
-            "forcerange", "gear", "cranklength", "user", "group",
+            "forcerange", "gear", "cranklength", "user", "group", "kp", "kv", "dampratio", "timeconst"},
-            "kp", "kv"},
         {"velocity", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
-            "gear", "cranklength", "user", "group",
+            "gear", "cranklength", "user", "group", "kv"},
-            "kv"},
+        {"intvelocity", "?", "13", "ctrllimited", "forcelimited",
-        {"intvelocity", "?", "12", "ctrllimited", "forcelimited",
             "ctrlrange", "forcerange", "actrange", "inheritrange",
             "gear", "cranklength", "user", "group",
-            "kp", "kv"},
+            "kp", "kv", "dampratio"},
         {"damper", "?", "8", "forcelimited", "ctrlrange", "forcerange",
-            "gear", "cranklength", "user", "group",
+            "gear", "cranklength", "user", "group", "kv"},
-            "kv"},
         {"cylinder", "?", "12", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
             "gear", "cranklength", "user", "group",
             "timeconst", "area", "diameter", "bias"},
@@ -390,22 +387,22 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
             "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
             "lengthrange", "gear", "cranklength", "user",
             "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"},
-        {"position", "*", "21", "name", "class", "group",
+        {"position", "*", "23", "name", "class", "group",
             "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange",
             "lengthrange", "gear", "cranklength", "user",
             "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
-            "kp", "kv"},
+            "kp", "kv", "dampratio", "timeconst"},
         {"velocity", "*", "19", "name", "class", "group",
             "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
             "lengthrange", "gear", "cranklength", "user",
             "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
             "kv"},
-        {"intvelocity", "*", "22", "name", "class", "group",
+        {"intvelocity", "*", "23", "name", "class", "group",
             "ctrllimited", "forcelimited",
             "ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange",
             "gear", "cranklength", "user",
             "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
-            "kp", "kv"},
+            "kp", "kv", "dampratio"},
         {"damper", "*", "18", "name", "class", "group",
             "forcelimited", "ctrlrange", "forcerange",
             "lengthrange", "gear", "cranklength", "user",
@@ -472,6 +469,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
         {"subtreecom", "*", "5", "name", "body", "cutoff", "noise", "user"},
         {"subtreelinvel", "*", "5", "name", "body", "cutoff", "noise", "user"},
         {"subtreeangmom", "*", "5", "name", "body", "cutoff", "noise", "user"},
+        {"distance", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
+        {"normal", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
+        {"fromto", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
         {"clock", "*", "4", "name", "cutoff", "noise", "user"},
         {"user", "*", "9", "name", "objtype", "objname", "datatype", "needstage",
             "dim", "cutoff", "noise", "user"},
@@ -924,25 +924,25 @@ void mjXReader::Parse(XMLElement* root) {
 
 
 // compiler section parser
-void mjXReader::Compiler(XMLElement* section, mjSpec* mod) {
+void mjXReader::Compiler(XMLElement* section, mjSpec* spec) {
   string text;
   int n;
 
   // top-level attributes
   if (MapValue(section, "autolimits", &n, bool_map, 2)) {
-    mod->autolimits = (n==1);
+    spec->autolimits = (n==1);
   }
-  ReadAttr(section, "boundmass", 1, &mod->boundmass, text);
+  ReadAttr(section, "boundmass", 1, &spec->boundmass, text);
-  ReadAttr(section, "boundinertia", 1, &mod->boundinertia, text);
+  ReadAttr(section, "boundinertia", 1, &spec->boundinertia, text);
-  ReadAttr(section, "settotalmass", 1, &mod->settotalmass, text);
+  ReadAttr(section, "settotalmass", 1, &spec->settotalmass, text);
   if (MapValue(section, "balanceinertia", &n, bool_map, 2)) {
-    mod->balanceinertia = (n==1);
+    spec->balanceinertia = (n==1);
   }
   if (MapValue(section, "strippath", &n, bool_map, 2)) {
-    mod->strippath = (n==1);
+    spec->strippath = (n==1);
   }
   if (MapValue(section, "fitaabb", &n, bool_map, 2)) {
-    mod->fitaabb = (n==1);
+    spec->fitaabb = (n==1);
   }
   if (MapValue(section, "coordinate", &n, coordinate_map, 2)) {
     if (n==1) {
@@ -951,48 +951,48 @@ void mjXReader::Compiler(XMLElement* section, mjSpec* mod) {
     }
   }
   if (MapValue(section, "angle", &n, angle_map, 2)) {
-    mod->degree = (n==1);
+    spec->degree = (n==1);
   }
   if (ReadAttrTxt(section, "eulerseq", text)) {
     if (text.size()!=3) {
       throw mjXError(section, "euler format must have length 3");
     }
-    memcpy(mod->euler, text.c_str(), 3);
+    memcpy(spec->euler, text.c_str(), 3);
   }
   if (ReadAttrTxt(section, "assetdir", text)) {
-    mjs_setString(mod->meshdir, text.c_str());
+    mjs_setString(spec->meshdir, text.c_str());
-    mjs_setString(mod->texturedir, text.c_str());
+    mjs_setString(spec->texturedir, text.c_str());
   }
   // meshdir and texturedir take precedence over assetdir
   std::string meshdir, texturedir;
   if (ReadAttrTxt(section, "meshdir", meshdir)) {
-    mjs_setString(mod->meshdir, meshdir.c_str());
+    mjs_setString(spec->meshdir, meshdir.c_str());
   };
   if (ReadAttrTxt(section, "texturedir", texturedir)) {
-    mjs_setString(mod->texturedir, texturedir.c_str());
+    mjs_setString(spec->texturedir, texturedir.c_str());
   }
   if (MapValue(section, "discardvisual", &n, bool_map, 2)) {
-    mod->discardvisual = (n==1);
+    spec->discardvisual = (n==1);
   }
   if (MapValue(section, "convexhull", &n, bool_map, 2)) {
-    mod->convexhull = (n==1);
+    spec->convexhull = (n==1);
   }
   if (MapValue(section, "usethread", &n, bool_map, 2)) {
-    mod->usethread = (n==1);
+    spec->usethread = (n==1);
   }
   if (MapValue(section, "fusestatic", &n, bool_map, 2)) {
-    mod->fusestatic = (n==1);
+    spec->fusestatic = (n==1);
   }
-  MapValue(section, "inertiafromgeom", &mod->inertiafromgeom, TFAuto_map, 3);
+  MapValue(section, "inertiafromgeom", &spec->inertiafromgeom, TFAuto_map, 3);
-  ReadAttr(section, "inertiagrouprange", 2, mod->inertiagrouprange, text);
+  ReadAttr(section, "inertiagrouprange", 2, spec->inertiagrouprange, text);
   if (MapValue(section, "exactmeshinertia", &n, bool_map, 2)){
-    mod->exactmeshinertia = (n==1);
+    spec->exactmeshinertia = (n==1);
   }
 
   // lengthrange subelement
   XMLElement* elem = FindSubElem(section, "lengthrange");
   if (elem) {
-    mjLROpt* opt = &(mod->LRopt);
+    mjLROpt* opt = &(spec->LRopt);
 
     // flags
     MapValue(elem, "mode", &opt->mode, lrmode_map, lrmode_sz);
@@ -1110,7 +1110,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
 
 
 // size section parser
-void mjXReader::Size(XMLElement* section, mjSpec* mod) {
+void mjXReader::Size(XMLElement* section, mjSpec* spec) {
   // read memory bytes
   {
     constexpr char err_msg[] =
@@ -1199,69 +1199,69 @@ void mjXReader::Size(XMLElement* section, mjSpec* mod) {
       if (*memory / sizeof(mjtNum) > std::numeric_limits<std::size_t>::max()) {
         throw mjXError(section, "%s", err_msg);
       }
-      mod->memory = *memory;
+      spec->memory = *memory;
     }
   }
 
   // read sizes
-  ReadAttrInt(section, "nuserdata", &mod->nuserdata);
+  ReadAttrInt(section, "nuserdata", &spec->nuserdata);
-  ReadAttrInt(section, "nkey", &mod->nkey);
+  ReadAttrInt(section, "nkey", &spec->nkey);
 
-  ReadAttrInt(section, "nconmax", &mod->nconmax);
+  ReadAttrInt(section, "nconmax", &spec->nconmax);
-  if (mod->nconmax < -1) throw mjXError(section, "nconmax must be >= -1");
+  if (spec->nconmax < -1) throw mjXError(section, "nconmax must be >= -1");
 
   {
     int nstack = -1;
     const bool has_nstack = ReadAttrInt(section, "nstack", &nstack);
     if (has_nstack) {
-      if (mod->nstack < -1) {
+      if (spec->nstack < -1) {
         throw mjXError(section, "nstack must be >= -1");
       }
-      if (mod->memory != -1 && nstack != -1) {
+      if (spec->memory != -1 && nstack != -1) {
         throw mjXError(section,
                        "either 'memory' and 'nstack' attribute can be specified, not both");
       }
-      mod->nstack = nstack;
+      spec->nstack = nstack;
     }
   }
   {
     int njmax = -1;
     const bool has_njmax = ReadAttrInt(section, "njmax", &njmax);
     if (has_njmax) {
-      if (mod->njmax < -1) {
+      if (spec->njmax < -1) {
         throw mjXError(section, "njmax must be >= -1");
       }
-      if (mod->memory != -1 && njmax != -1) {
+      if (spec->memory != -1 && njmax != -1) {
         throw mjXError(section,
                        "either 'memory' and 'njmax' attribute can be specified, not both");
       }
-      mod->njmax = njmax;
+      spec->njmax = njmax;
     }
   }
 
-  ReadAttrInt(section, "nuser_body", &mod->nuser_body);
+  ReadAttrInt(section, "nuser_body", &spec->nuser_body);
-  if (mod->nuser_body < -1) throw mjXError(section, "nuser_body must be >= -1");
+  if (spec->nuser_body < -1) throw mjXError(section, "nuser_body must be >= -1");
 
-  ReadAttrInt(section, "nuser_jnt", &mod->nuser_jnt);
+  ReadAttrInt(section, "nuser_jnt", &spec->nuser_jnt);
-  if (mod->nuser_jnt < -1) throw mjXError(section, "nuser_jnt must be >= -1");
+  if (spec->nuser_jnt < -1) throw mjXError(section, "nuser_jnt must be >= -1");
 
-  ReadAttrInt(section, "nuser_geom", &mod->nuser_geom);
+  ReadAttrInt(section, "nuser_geom", &spec->nuser_geom);
-  if (mod->nuser_geom < -1) throw mjXError(section, "nuser_geom must be >= -1");
+  if (spec->nuser_geom < -1) throw mjXError(section, "nuser_geom must be >= -1");
 
-  ReadAttrInt(section, "nuser_site", &mod->nuser_site);
+  ReadAttrInt(section, "nuser_site", &spec->nuser_site);
-  if (mod->nuser_site < -1) throw mjXError(section, "nuser_site must be >= -1");
+  if (spec->nuser_site < -1) throw mjXError(section, "nuser_site must be >= -1");
 
-  ReadAttrInt(section, "nuser_cam", &mod->nuser_cam);
+  ReadAttrInt(section, "nuser_cam", &spec->nuser_cam);
-  if (mod->nuser_cam < -1) throw mjXError(section, "nuser_cam must be >= -1");
+  if (spec->nuser_cam < -1) throw mjXError(section, "nuser_cam must be >= -1");
 
-  ReadAttrInt(section, "nuser_tendon", &mod->nuser_tendon);
+  ReadAttrInt(section, "nuser_tendon", &spec->nuser_tendon);
-  if (mod->nuser_tendon < -1) throw mjXError(section, "nuser_tendon must be >= -1");
+  if (spec->nuser_tendon < -1) throw mjXError(section, "nuser_tendon must be >= -1");
 
-  ReadAttrInt(section, "nuser_actuator", &mod->nuser_actuator);
+  ReadAttrInt(section, "nuser_actuator", &spec->nuser_actuator);
-  if (mod->nuser_actuator < -1) throw mjXError(section, "nuser_actuator must be >= -1");
+  if (spec->nuser_actuator < -1) throw mjXError(section, "nuser_actuator must be >= -1");
 
-  ReadAttrInt(section, "nuser_sensor", &mod->nuser_sensor);
+  ReadAttrInt(section, "nuser_sensor", &spec->nuser_sensor);
-  if (mod->nuser_sensor < -1) throw mjXError(section, "nuser_sensor must be >= -1");
+  if (spec->nuser_sensor < -1) throw mjXError(section, "nuser_sensor must be >= -1");
 }
 
 
@@ -1882,13 +1882,13 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* pequality) {
     case mjEQ_JOINT:
       ReadAttrTxt(elem, "joint1", name1, true);
       ReadAttrTxt(elem, "joint2", name2);
-      ReadAttr(elem, "polycoef", 5, pequality->data, text);
+      ReadAttr(elem, "polycoef", 5, pequality->data, text, false, false);
       break;
 
     case mjEQ_TENDON:
       ReadAttrTxt(elem, "tendon1", name1, true);
       ReadAttrTxt(elem, "tendon2", name2);
-      ReadAttr(elem, "polycoef", 5, pequality->data, text);
+      ReadAttr(elem, "polycoef", 5, pequality->data, text, false, false);
       break;
 
     case mjEQ_FLEX:
@@ -2085,12 +2085,33 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* pact) {
     ReadAttr(elem, "kp", 1, pact->gainprm, text);
     pact->biasprm[1] = -pact->gainprm[0];
 
-    if (ReadAttr(elem, "kv", 1, pact->biasprm + 2, text)) {
+    // read kv
-      if (pact->biasprm[2] < 0)
+    double kv = -1;  // -1: undefined
-        throw mjXError(elem, "kv cannot be negative");
+    if (ReadAttr(elem, "kv", 1, &kv, text)) {
-      pact->biasprm[2] *= -1;
+      if (kv < 0) throw mjXError(elem, "kv cannot be negative");
     }
 
+    // read dampratio
+    double dampratio = -1;  // -1: undefined
+    if (ReadAttr(elem, "dampratio", 1, &dampratio, text)) {
+      if (dampratio < 0) throw mjXError(elem, "dampratio cannot be negative");
+    }
+
+    // set biasprm[2]; negative: regular damping, positive: dampratio
+    if (dampratio > 0 && kv > 0) {
+      throw mjXError(elem, "kv and dampratio cannot both be defined");
+    }
+    if (kv > 0) pact->biasprm[2] = -kv;
+    if (dampratio > 0) pact->biasprm[2] = dampratio;
+
+    // read timeconst, set dyntype
+    if (ReadAttr(elem, "timeconst", 1, pact->dynprm, text)) {
+      if (pact->dynprm[0] < 0)
+        throw mjXError(elem, "timeconst cannot be negative");
+      pact->dyntype = pact->dynprm[0] ? mjDYN_FILTEREXACT : mjDYN_NONE;
+    }
+
+    // handle inheritrange
     ReadAttr(elem, "inheritrange", 1, &pact->inheritrange, text);
     if (pact->inheritrange > 0) {
       if (type == "position") {
@@ -3949,6 +3970,29 @@ void mjXReader::Sensor(XMLElement* section) {
       ReadAttrTxt(elem, "body", objname, true);
     }
 
+    // sensors for geometric distance; attached to geoms or bodies
+    else if (type=="distance" || type=="normal" || type=="fromto") {
+      bool has_body1 = ReadAttrTxt(elem, "body1", objname);
+      bool has_geom1 = ReadAttrTxt(elem, "geom1", objname);
+      if (has_body1 == has_geom1) {
+        throw mjXError(elem, "exactly one of (geom1, body1) must be specified");
+      }
+      psen->objtype = has_body1 ? mjOBJ_BODY : mjOBJ_GEOM;
+      bool has_body2 = ReadAttrTxt(elem, "body2", refname);
+      bool has_geom2 = ReadAttrTxt(elem, "geom2", refname);
+      if (has_body2 == has_geom2) {
+        throw mjXError(elem, "exactly one of (geom2, body2) must be specified");
+      }
+      psen->reftype = has_body2 ? mjOBJ_BODY : mjOBJ_GEOM;
+      if (type=="distance") {
+        psen->type = mjSENS_GEOMDIST;
+      } else if (type=="normal") {
+        psen->type = mjSENS_GEOMNORMAL;
+      } else {
+        psen->type = mjSENS_GEOMFROMTO;
+      }
+    }
+
     // global sensors
     else if (type=="clock") {
       psen->type = mjSENS_CLOCK;

src/xml/xml_native_reader.h

@@ -42,9 +42,9 @@ class mjXReader : public mjXBase {
   void SetTextureDir(std::string texturedir);
 
   // XML sections embedded in all formats
-  static void Compiler(tinyxml2::XMLElement* section, mjSpec* mod);  // compiler section
+  static void Compiler(tinyxml2::XMLElement* section, mjSpec* spec);   // compiler section
   static void Option(tinyxml2::XMLElement* section, mjOption* opt);    // option section
-  static void Size(tinyxml2::XMLElement* section, mjSpec* mod);      // size section
+  static void Size(tinyxml2::XMLElement* section, mjSpec* spec);       // size section
 
  private:
   // XML section specific to MJCF
@@ -99,7 +99,7 @@ class mjXReader : public mjXBase {
 };
 
 // MJCF schema
-#define nMJCF 227
+#define nMJCF 230
 extern const char* MJCF[nMJCF][mjXATTRNUM];
 
 #endif  // MUJOCO_SRC_XML_XML_NATIVE_READER_H_

src/xml/xml_native_writer.cc

@@ -84,7 +84,7 @@ XMLElement* mjXWriter::InsertEnd(XMLElement* parent, const char* name) {
 //---------------------------------- class mjXWriter: one-element writers --------------------------
 
 // write flex
-void mjXWriter::OneFlex(XMLElement* elem, mjCFlex* pflex) {
+void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* pflex) {
   string text;
   mjCFlex defflex;
 
@@ -152,7 +152,7 @@ void mjXWriter::OneFlex(XMLElement* elem, mjCFlex* pflex) {
 
 
 // write mesh
-void mjXWriter::OneMesh(XMLElement* elem, mjCMesh* pmesh, mjCDef* def) {
+void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* pmesh, mjCDef* def) {
   string text;
 
   // regular
@@ -188,17 +188,17 @@ void mjXWriter::OneMesh(XMLElement* elem, mjCMesh* pmesh, mjCDef* def) {
   }
 
   // defaults and regular
-  WriteAttr(elem, "refpos", 3, pmesh->refpos, def->mesh.refpos);
+  WriteAttr(elem, "refpos", 3, pmesh->refpos, def->Mesh().refpos);
-  WriteAttr(elem, "refquat", 4, pmesh->refquat, def->mesh.refquat);
+  WriteAttr(elem, "refquat", 4, pmesh->refquat, def->Mesh().refquat);
-  WriteAttr(elem, "scale", 3, pmesh->scale, def->mesh.scale);
+  WriteAttr(elem, "scale", 3, pmesh->scale, def->Mesh().scale);
   WriteAttrKey(elem, "smoothnormal", bool_map, 2, pmesh->get_smoothnormal(),
-               def->mesh.get_smoothnormal());
+               def->Mesh().get_smoothnormal());
 }
 
 
 
 // write skin
-void mjXWriter::OneSkin(XMLElement* elem, mjCSkin* pskin) {
+void mjXWriter::OneSkin(XMLElement* elem, const mjCSkin* pskin) {
   string text;
   mjCDef mydef;
   float zero = 0;
@@ -208,7 +208,7 @@ void mjXWriter::OneSkin(XMLElement* elem, mjCSkin* pskin) {
   WriteAttrTxt(elem, "file", pskin->get_file());
   WriteAttrTxt(elem, "material", pskin->get_material());
   WriteAttrInt(elem, "group", pskin->group, 0);
-  WriteAttr(elem, "rgba", 4, pskin->rgba, mydef.geom.rgba);
+  WriteAttr(elem, "rgba", 4, pskin->rgba, mydef.Geom().rgba);
   WriteAttr(elem, "inflate", 1, &pskin->inflate, &zero);
 
   // write data if no file
@@ -251,7 +251,7 @@ void mjXWriter::OneSkin(XMLElement* elem, mjCSkin* pskin) {
 
 
 // write material
-void mjXWriter::OneMaterial(XMLElement* elem, mjCMaterial* pmat, mjCDef* def) {
+void mjXWriter::OneMaterial(XMLElement* elem, const mjCMaterial* pmat, mjCDef* def) {
   // regular
   if (!writingdefaults) {
     WriteAttrTxt(elem, "name", pmat->name);
@@ -259,24 +259,24 @@ void mjXWriter::OneMaterial(XMLElement* elem, mjCMaterial* pmat, mjCDef* def) {
   }
 
   // defaults and regular
-  if (pmat->texture != def->material.texture) {
+  if (pmat->texture != def->Material().texture) {
     WriteAttrTxt(elem, "texture", pmat->get_texture());
   }
-  WriteAttrKey(elem, "texuniform", bool_map, 2, pmat->texuniform, def->material.texuniform);
+  WriteAttrKey(elem, "texuniform", bool_map, 2, pmat->texuniform, def->Material().texuniform);
-  WriteAttr(elem, "texrepeat", 2, pmat->texrepeat, def->material.texrepeat);
+  WriteAttr(elem, "texrepeat", 2, pmat->texrepeat, def->Material().texrepeat);
-  WriteAttr(elem, "emission", 1, &pmat->emission, &def->material.emission);
+  WriteAttr(elem, "emission", 1, &pmat->emission, &def->Material().emission);
-  WriteAttr(elem, "specular", 1, &pmat->specular, &def->material.specular);
+  WriteAttr(elem, "specular", 1, &pmat->specular, &def->Material().specular);
-  WriteAttr(elem, "shininess", 1, &pmat->shininess, &def->material.shininess);
+  WriteAttr(elem, "shininess", 1, &pmat->shininess, &def->Material().shininess);
-  WriteAttr(elem, "reflectance", 1, &pmat->reflectance, &def->material.reflectance);
+  WriteAttr(elem, "reflectance", 1, &pmat->reflectance, &def->Material().reflectance);
-  WriteAttr(elem, "metallic", 1, &pmat->metallic, &def->material.metallic);
+  WriteAttr(elem, "metallic", 1, &pmat->metallic, &def->Material().metallic);
-  WriteAttr(elem, "roughness", 1, &pmat->roughness, &def->material.roughness);
+  WriteAttr(elem, "roughness", 1, &pmat->roughness, &def->Material().roughness);
-  WriteAttr(elem, "rgba", 4, pmat->rgba, def->material.rgba);
+  WriteAttr(elem, "rgba", 4, pmat->rgba, def->Material().rgba);
 }
 
 
 
 // write joint
-void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
+void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* pjoint, mjCDef* def) {
   double zero = 0;
 
   // regular
@@ -292,42 +292,42 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
   }
 
   // defaults and regular
-  if (pjoint->type != def->joint.type) {
+  if (pjoint->type != def->Joint().type) {
     WriteAttrTxt(elem, "type", FindValue(joint_map, joint_sz, pjoint->type));
   }
-  WriteAttrInt(elem, "group", pjoint->group, def->joint.group);
+  WriteAttrInt(elem, "group", pjoint->group, def->Joint().group);
   WriteAttr(elem, "ref", 1, &pjoint->ref, &zero);
   WriteAttr(elem, "springref", 1, &pjoint->springref, &zero);
-  WriteAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, def->joint.solref_limit, true);
+  WriteAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, def->Joint().solref_limit, true);
-  WriteAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, def->joint.solimp_limit, true);
+  WriteAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, def->Joint().solimp_limit, true);
-  WriteAttr(elem, "solreffriction", mjNREF, pjoint->solref_friction, def->joint.solref_friction,
+  WriteAttr(elem, "solreffriction", mjNREF, pjoint->solref_friction, def->Joint().solref_friction,
             true);
-  WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->joint.solimp_friction,
+  WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->Joint().solimp_friction,
             true);
-  WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->joint.stiffness);
+  WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->Joint().stiffness);
-  WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited);
+  WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->Joint().limited);
-  WriteAttr(elem, "range", 2, pjoint->range, def->joint.range);
+  WriteAttr(elem, "range", 2, pjoint->range, def->Joint().range);
   WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, pjoint->actfrclimited,
-               def->joint.actfrclimited);
+               def->Joint().actfrclimited);
-  WriteAttrKey(elem, "actuatorgravcomp", bool_map, 2, pjoint->actgravcomp, def->joint.actgravcomp);
+  WriteAttrKey(elem, "actuatorgravcomp", bool_map, 2, pjoint->actgravcomp, def->Joint().actgravcomp);
-  WriteAttr(elem, "actuatorfrcrange", 2, pjoint->actfrcrange, def->joint.actfrcrange);
+  WriteAttr(elem, "actuatorfrcrange", 2, pjoint->actfrcrange, def->Joint().actfrcrange);
-  WriteAttr(elem, "margin", 1, &pjoint->margin, &def->joint.margin);
+  WriteAttr(elem, "margin", 1, &pjoint->margin, &def->Joint().margin);
-  WriteAttr(elem, "armature", 1, &pjoint->armature, &def->joint.armature);
+  WriteAttr(elem, "armature", 1, &pjoint->armature, &def->Joint().armature);
-  WriteAttr(elem, "damping", 1, &pjoint->damping, &def->joint.damping);
+  WriteAttr(elem, "damping", 1, &pjoint->damping, &def->Joint().damping);
-  WriteAttr(elem, "frictionloss", 1, &pjoint->frictionloss, &def->joint.frictionloss);
+  WriteAttr(elem, "frictionloss", 1, &pjoint->frictionloss, &def->Joint().frictionloss);
 
   // userdata
   if (writingdefaults) {
     WriteVector(elem, "user", pjoint->get_userdata());
   } else {
-    WriteVector(elem, "user", pjoint->get_userdata(), def->joint.get_userdata());
+    WriteVector(elem, "user", pjoint->get_userdata(), def->Joint().get_userdata());
   }
 }
 
 
 
 // write geom
-void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
+void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* pgeom, mjCDef* def) {
   double unitq[4] = {1, 0, 0, 0};
   double mass = 0;
 
@@ -336,10 +336,10 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
     WriteAttrTxt(elem, "name", pgeom->name);
     WriteAttrTxt(elem, "class", pgeom->classname);
     if (mjGEOMINFO[pgeom->type]) {
-      WriteAttr(elem, "size", mjGEOMINFO[pgeom->type], pgeom->size, def->geom.size);
+      WriteAttr(elem, "size", mjGEOMINFO[pgeom->type], pgeom->size, def->Geom().size);
     }
     if (mjuu_defined(pgeom->mass)) {
-      mass = pgeom->GetVolume() * def->geom.density;
+      mass = pgeom->GetVolume() * def->Geom().density;
     }
 
     // mesh geom
@@ -368,35 +368,35 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
       WriteAttr(elem, "quat", 4, pgeom->quat, unitq);
     }
   } else {
-    WriteAttr(elem, "size", 3, pgeom->size, def->geom.size);
+    WriteAttr(elem, "size", 3, pgeom->size, def->Geom().size);
   }
 
   // defaults and regular
-  WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, pgeom->type, def->geom.type);
+  WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, pgeom->type, def->Geom().type);
-  WriteAttrInt(elem, "contype", pgeom->contype, def->geom.contype);
+  WriteAttrInt(elem, "contype", pgeom->contype, def->Geom().contype);
-  WriteAttrInt(elem, "conaffinity", pgeom->conaffinity, def->geom.conaffinity);
+  WriteAttrInt(elem, "conaffinity", pgeom->conaffinity, def->Geom().conaffinity);
-  WriteAttrInt(elem, "condim", pgeom->condim, def->geom.condim);
+  WriteAttrInt(elem, "condim", pgeom->condim, def->Geom().condim);
-  WriteAttrInt(elem, "group", pgeom->group, def->geom.group);
+  WriteAttrInt(elem, "group", pgeom->group, def->Geom().group);
-  WriteAttrInt(elem, "priority", pgeom->priority, def->geom.priority);
+  WriteAttrInt(elem, "priority", pgeom->priority, def->Geom().priority);
-  WriteAttr(elem, "friction", 3, pgeom->friction, def->geom.friction, true);
+  WriteAttr(elem, "friction", 3, pgeom->friction, def->Geom().friction, true);
-  WriteAttr(elem, "solmix", 1, &pgeom->solmix, &def->geom.solmix);
+  WriteAttr(elem, "solmix", 1, &pgeom->solmix, &def->Geom().solmix);
-  WriteAttr(elem, "solref", mjNREF, pgeom->solref, def->geom.solref, true);
+  WriteAttr(elem, "solref", mjNREF, pgeom->solref, def->Geom().solref, true);
-  WriteAttr(elem, "solimp", mjNIMP, pgeom->solimp, def->geom.solimp, true);
+  WriteAttr(elem, "solimp", mjNIMP, pgeom->solimp, def->Geom().solimp, true);
-  WriteAttr(elem, "margin", 1, &pgeom->margin, &def->geom.margin);
+  WriteAttr(elem, "margin", 1, &pgeom->margin, &def->Geom().margin);
-  WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap);
+  WriteAttr(elem, "gap", 1, &pgeom->gap, &def->Geom().gap);
-  WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap);
+  WriteAttr(elem, "gap", 1, &pgeom->gap, &def->Geom().gap);
-  WriteAttrKey(elem, "fluidshape", fluid_map, 2, pgeom->fluid_ellipsoid, def->geom.fluid_ellipsoid);
+  WriteAttrKey(elem, "fluidshape", fluid_map, 2, pgeom->fluid_ellipsoid, def->Geom().fluid_ellipsoid);
-  WriteAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, def->geom.fluid_coefs);
+  WriteAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, def->Geom().fluid_coefs);
-  WriteAttrKey(elem, "shellinertia", meshtype_map, 2, pgeom->typeinertia, def->geom.typeinertia);
+  WriteAttrKey(elem, "shellinertia", meshtype_map, 2, pgeom->typeinertia, def->Geom().typeinertia);
   if (mjuu_defined(pgeom->mass)) {
     WriteAttr(elem, "mass", 1, &pgeom->mass_, &mass);
   } else {
-    WriteAttr(elem, "density", 1, &pgeom->density, &def->geom.density);
+    WriteAttr(elem, "density", 1, &pgeom->density, &def->Geom().density);
   }
-  if (pgeom->get_material() != def->geom.get_material()) {
+  if (pgeom->get_material() != def->Geom().get_material()) {
     WriteAttrTxt(elem, "material", pgeom->get_material());
   }
-  WriteAttr(elem, "rgba", 4, pgeom->rgba, def->geom.rgba);
+  WriteAttr(elem, "rgba", 4, pgeom->rgba, def->Geom().rgba);
 
   // hfield and mesh attributes
   if (pgeom->type==mjGEOM_HFIELD) {
@@ -410,7 +410,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
   if (writingdefaults) {
     WriteVector(elem, "user", pgeom->get_userdata());
   } else {
-    WriteVector(elem, "user", pgeom->get_userdata(), def->geom.get_userdata());
+    WriteVector(elem, "user", pgeom->get_userdata(), def->Geom().get_userdata());
   }
 
   // write plugin
@@ -422,7 +422,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
 
 
 // write site
-void mjXWriter::OneSite(XMLElement* elem, mjCSite* psite, mjCDef* def) {
+void mjXWriter::OneSite(XMLElement* elem, const mjCSite* psite, mjCDef* def) {
   double unitq[4] = {1, 0, 0, 0};
 
   // regular
@@ -432,32 +432,32 @@ void mjXWriter::OneSite(XMLElement* elem, mjCSite* psite, mjCDef* def) {
     WriteAttr(elem, "pos", 3, psite->pos);
     WriteAttr(elem, "quat", 4, psite->quat, unitq);
     if (mjGEOMINFO[psite->type]) {
-      WriteAttr(elem, "size", mjGEOMINFO[psite->type], psite->size, def->site.size);
+      WriteAttr(elem, "size", mjGEOMINFO[psite->type], psite->size, def->Site().size);
     }
   } else {
-    WriteAttr(elem, "size", 3, psite->size, def->site.size);
+    WriteAttr(elem, "size", 3, psite->size, def->Site().size);
   }
 
   // defaults and regular
-  WriteAttrInt(elem, "group", psite->group, def->site.group);
+  WriteAttrInt(elem, "group", psite->group, def->Site().group);
-  WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, psite->type, def->site.type);
+  WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, psite->type, def->Site().type);
-  if (psite->get_material() != def->site.get_material()) {
+  if (psite->get_material() != def->Site().get_material()) {
     WriteAttrTxt(elem, "material", psite->get_material());
   }
-  WriteAttr(elem, "rgba", 4, psite->rgba, def->site.rgba);
+  WriteAttr(elem, "rgba", 4, psite->rgba, def->Site().rgba);
 
   // userdata
   if (writingdefaults) {
     WriteVector(elem, "user", psite->get_userdata());
   } else {
-    WriteVector(elem, "user", psite->get_userdata(), def->site.get_userdata());
+    WriteVector(elem, "user", psite->get_userdata(), def->Site().get_userdata());
   }
 }
 
 
 
 // write camera
-void mjXWriter::OneCamera(XMLElement* elem, mjCCamera* pcam, mjCDef* def) {
+void mjXWriter::OneCamera(XMLElement* elem, const mjCCamera* pcam, mjCDef* def) {
   double unitq[4] = {1, 0, 0, 0};
 
   // regular
@@ -470,36 +470,36 @@ void mjXWriter::OneCamera(XMLElement* elem, mjCCamera* pcam, mjCDef* def) {
   }
 
   // defaults and regular
-  WriteAttr(elem, "ipd", 1, &pcam->ipd, &def->camera.ipd);
+  WriteAttr(elem, "ipd", 1, &pcam->ipd, &def->Camera().ipd);
-  WriteAttrKey(elem, "mode", camlight_map, camlight_sz, pcam->mode, def->camera.mode);
+  WriteAttrKey(elem, "mode", camlight_map, camlight_sz, pcam->mode, def->Camera().mode);
-  WriteAttr(elem, "resolution", 2, pcam->resolution, def->camera.resolution);
+  WriteAttr(elem, "resolution", 2, pcam->resolution, def->Camera().resolution);
 
   // resolution if positive
-  WriteAttr(elem, "resolution", 2, pcam->resolution, def->camera.resolution);
+  WriteAttr(elem, "resolution", 2, pcam->resolution, def->Camera().resolution);
 
   // camera intrinsics if specified
   if (pcam->sensor_size[0]>0 && pcam->sensor_size[1]>0) {
     WriteAttr(elem, "sensorsize", 2, pcam->sensor_size);
-    WriteAttr(elem, "focal", 2, pcam->focal_length, def->camera.focal_length);
+    WriteAttr(elem, "focal", 2, pcam->focal_length, def->Camera().focal_length);
-    WriteAttr(elem, "focalpixel", 2, pcam->focal_pixel, def->camera.focal_pixel);
+    WriteAttr(elem, "focalpixel", 2, pcam->focal_pixel, def->Camera().focal_pixel);
-    WriteAttr(elem, "principal", 2, pcam->principal_length, def->camera.principal_length);
+    WriteAttr(elem, "principal", 2, pcam->principal_length, def->Camera().principal_length);
-    WriteAttr(elem, "principalpixel", 2, pcam->principal_pixel, def->camera.principal_pixel);
+    WriteAttr(elem, "principalpixel", 2, pcam->principal_pixel, def->Camera().principal_pixel);
   } else {
-    WriteAttr(elem, "fovy", 1, &pcam->fovy, &def->camera.fovy);
+    WriteAttr(elem, "fovy", 1, &pcam->fovy, &def->Camera().fovy);
   }
 
   // userdata
   if (writingdefaults) {
     WriteVector(elem, "user", pcam->get_userdata());
   } else {
-    WriteVector(elem, "user", pcam->get_userdata(), def->camera.get_userdata());
+    WriteVector(elem, "user", pcam->get_userdata(), def->Camera().get_userdata());
   }
 }
 
 
 
 // write light
-void mjXWriter::OneLight(XMLElement* elem, mjCLight* plight, mjCDef* def) {
+void mjXWriter::OneLight(XMLElement* elem, const mjCLight* plight, mjCDef* def) {
   // regular
   if (!writingdefaults) {
     WriteAttrTxt(elem, "name", plight->name);
@@ -510,23 +510,23 @@ void mjXWriter::OneLight(XMLElement* elem, mjCLight* plight, mjCDef* def) {
   }
 
   // defaults and regular
-  WriteAttr(elem, "bulbradius", 1, &plight->bulbradius, &def->light.bulbradius);
+  WriteAttr(elem, "bulbradius", 1, &plight->bulbradius, &def->Light().bulbradius);
-  WriteAttrKey(elem, "directional", bool_map, 2, plight->directional, def->light.directional);
+  WriteAttrKey(elem, "directional", bool_map, 2, plight->directional, def->Light().directional);
-  WriteAttrKey(elem, "castshadow", bool_map, 2, plight->castshadow, def->light.castshadow);
+  WriteAttrKey(elem, "castshadow", bool_map, 2, plight->castshadow, def->Light().castshadow);
-  WriteAttrKey(elem, "active", bool_map, 2, plight->active, def->light.active);
+  WriteAttrKey(elem, "active", bool_map, 2, plight->active, def->Light().active);
-  WriteAttr(elem, "attenuation", 3, plight->attenuation, def->light.attenuation);
+  WriteAttr(elem, "attenuation", 3, plight->attenuation, def->Light().attenuation);
-  WriteAttr(elem, "cutoff", 1, &plight->cutoff, &def->light.cutoff);
+  WriteAttr(elem, "cutoff", 1, &plight->cutoff, &def->Light().cutoff);
-  WriteAttr(elem, "exponent", 1, &plight->exponent, &def->light.exponent);
+  WriteAttr(elem, "exponent", 1, &plight->exponent, &def->Light().exponent);
-  WriteAttr(elem, "ambient", 3, plight->ambient, def->light.ambient);
+  WriteAttr(elem, "ambient", 3, plight->ambient, def->Light().ambient);
-  WriteAttr(elem, "diffuse", 3, plight->diffuse, def->light.diffuse);
+  WriteAttr(elem, "diffuse", 3, plight->diffuse, def->Light().diffuse);
-  WriteAttr(elem, "specular", 3, plight->specular, def->light.specular);
+  WriteAttr(elem, "specular", 3, plight->specular, def->Light().specular);
-  WriteAttrKey(elem, "mode", camlight_map, camlight_sz, plight->mode, def->light.mode);
+  WriteAttrKey(elem, "mode", camlight_map, camlight_sz, plight->mode, def->Light().mode);
 }
 
 
 
 // write pair
-void mjXWriter::OnePair(XMLElement* elem, mjCPair* ppair, mjCDef* def) {
+void mjXWriter::OnePair(XMLElement* elem, const mjCPair* ppair, mjCDef* def) {
   // regular
   if (!writingdefaults) {
     WriteAttrTxt(elem, "class", ppair->classname);
@@ -536,20 +536,20 @@ void mjXWriter::OnePair(XMLElement* elem, mjCPair* ppair, mjCDef* def) {
 
   // defaults and regular
   WriteAttrTxt(elem, "name", ppair->name);
-  WriteAttrInt(elem, "condim", ppair->condim, def->pair.spec.condim);
+  WriteAttrInt(elem, "condim", ppair->condim, def->Pair().spec.condim);
-  WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.spec.margin);
+  WriteAttr(elem, "margin", 1, &ppair->margin, &def->Pair().spec.margin);
-  WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.spec.gap);
+  WriteAttr(elem, "gap", 1, &ppair->gap, &def->Pair().spec.gap);
-  WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.spec.solref, true);
+  WriteAttr(elem, "solref", mjNREF, ppair->solref, def->Pair().spec.solref, true);
-  WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.spec.solreffriction,
+  WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->Pair().spec.solreffriction,
             true);
-  WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.spec.solimp, true);
+  WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->Pair().spec.solimp, true);
-  WriteAttr(elem, "friction", 5, ppair->friction, def->pair.spec.friction);  // all 5 values
+  WriteAttr(elem, "friction", 5, ppair->friction, def->Pair().spec.friction);  // all 5 values
 }
 
 
 
 // write equality
-void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) {
+void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* peq, mjCDef* def) {
   // regular
   if (!writingdefaults) {
     WriteAttrTxt(elem, "name", peq->name);
@@ -592,15 +592,15 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) {
   }
 
   // defaults and regular
-  WriteAttrKey(elem, "active", bool_map, 2, peq->active, def->equality.active);
+  WriteAttrKey(elem, "active", bool_map, 2, peq->active, def->Equality().active);
-  WriteAttr(elem, "solref", mjNREF, peq->solref, def->equality.solref, true);
+  WriteAttr(elem, "solref", mjNREF, peq->solref, def->Equality().solref, true);
-  WriteAttr(elem, "solimp", mjNIMP, peq->solimp, def->equality.solimp, true);
+  WriteAttr(elem, "solimp", mjNIMP, peq->solimp, def->Equality().solimp, true);
 }
 
 
 
 // write tendon
-void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) {
+void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* pten, mjCDef* def) {
   bool fixed = (pten->GetWrap(0) && pten->GetWrap(0)->type==mjWRAP_JOINT);
 
   // regular
@@ -610,46 +610,46 @@ void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) {
   }
 
   // defaults and regular
-  WriteAttrInt(elem, "group", pten->group, def->tendon.group);
+  WriteAttrInt(elem, "group", pten->group, def->Tendon().group);
-  WriteAttr(elem, "solreflimit", mjNREF, pten->solref_limit, def->tendon.solref_limit, true);
+  WriteAttr(elem, "solreflimit", mjNREF, pten->solref_limit, def->Tendon().solref_limit, true);
-  WriteAttr(elem, "solimplimit", mjNIMP, pten->solimp_limit, def->tendon.solimp_limit, true);
+  WriteAttr(elem, "solimplimit", mjNIMP, pten->solimp_limit, def->Tendon().solimp_limit, true);
-  WriteAttr(elem, "solreffriction", mjNREF, pten->solref_friction, def->tendon.solref_friction,
+  WriteAttr(elem, "solreffriction", mjNREF, pten->solref_friction, def->Tendon().solref_friction,
             true);
-  WriteAttr(elem, "solimpfriction", mjNIMP, pten->solimp_friction, def->tendon.solimp_friction,
+  WriteAttr(elem, "solimpfriction", mjNIMP, pten->solimp_friction, def->Tendon().solimp_friction,
             true);
-  WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->tendon.limited);
+  WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->Tendon().limited);
-  WriteAttr(elem, "range", 2, pten->range, def->tendon.range);
+  WriteAttr(elem, "range", 2, pten->range, def->Tendon().range);
-  WriteAttr(elem, "margin", 1, &pten->margin, &def->tendon.margin);
+  WriteAttr(elem, "margin", 1, &pten->margin, &def->Tendon().margin);
-  WriteAttr(elem, "stiffness", 1, &pten->stiffness, &def->tendon.stiffness);
+  WriteAttr(elem, "stiffness", 1, &pten->stiffness, &def->Tendon().stiffness);
-  WriteAttr(elem, "damping", 1, &pten->damping, &def->tendon.damping);
+  WriteAttr(elem, "damping", 1, &pten->damping, &def->Tendon().damping);
-  WriteAttr(elem, "frictionloss", 1, &pten->frictionloss, &def->tendon.frictionloss);
+  WriteAttr(elem, "frictionloss", 1, &pten->frictionloss, &def->Tendon().frictionloss);
   if (pten->springlength[0] != pten->springlength[1] ||
-      def->tendon.springlength[0] != def->tendon.springlength[1]) {
+      def->Tendon().springlength[0] != def->Tendon().springlength[1]) {
-    WriteAttr(elem, "springlength", 2, pten->springlength, def->tendon.springlength);
+    WriteAttr(elem, "springlength", 2, pten->springlength, def->Tendon().springlength);
   } else {
-    WriteAttr(elem, "springlength", 1, pten->springlength, def->tendon.springlength);
+    WriteAttr(elem, "springlength", 1, pten->springlength, def->Tendon().springlength);
   }
   // spatial only
   if (!fixed) {
-    if (pten->get_material()!=def->tendon.get_material()) {
+    if (pten->get_material()!=def->Tendon().get_material()) {
       WriteAttrTxt(elem, "material", pten->get_material());
     }
-    WriteAttr(elem, "width", 1, &pten->width, &def->tendon.width);
+    WriteAttr(elem, "width", 1, &pten->width, &def->Tendon().width);
-    WriteAttr(elem, "rgba", 4, pten->rgba, def->tendon.rgba);
+    WriteAttr(elem, "rgba", 4, pten->rgba, def->Tendon().rgba);
   }
 
   // userdata
   if (writingdefaults) {
     WriteVector(elem, "user", pten->get_userdata());
   } else {
-    WriteVector(elem, "user", pten->get_userdata(), def->tendon.get_userdata());
+    WriteVector(elem, "user", pten->get_userdata(), def->Tendon().get_userdata());
   }
 }
 
 
 
 // write actuator
-void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
+void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* pact, mjCDef* def) {
   // regular
   if (!writingdefaults) {
     WriteAttrTxt(elem, "name", pact->name);
@@ -689,20 +689,20 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
   }
 
   // defaults and regular
-  WriteAttrInt(elem, "group", pact->group, def->actuator.group);
+  WriteAttrInt(elem, "group", pact->group, def->Actuator().group);
-  WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, pact->ctrllimited, def->actuator.ctrllimited);
+  WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, pact->ctrllimited, def->Actuator().ctrllimited);
-  WriteAttr(elem, "ctrlrange", 2, pact->ctrlrange, def->actuator.ctrlrange);
+  WriteAttr(elem, "ctrlrange", 2, pact->ctrlrange, def->Actuator().ctrlrange);
-  WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, pact->forcelimited, def->actuator.forcelimited);
+  WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, pact->forcelimited, def->Actuator().forcelimited);
-  WriteAttr(elem, "forcerange", 2, pact->forcerange, def->actuator.forcerange);
+  WriteAttr(elem, "forcerange", 2, pact->forcerange, def->Actuator().forcerange);
-  WriteAttrKey(elem, "actlimited", TFAuto_map, 3, pact->actlimited, def->actuator.actlimited);
+  WriteAttrKey(elem, "actlimited", TFAuto_map, 3, pact->actlimited, def->Actuator().actlimited);
-  WriteAttr(elem, "actrange", 2, pact->actrange, def->actuator.actrange);
+  WriteAttr(elem, "actrange", 2, pact->actrange, def->Actuator().actrange);
-  WriteAttr(elem, "lengthrange", 2, pact->lengthrange, def->actuator.lengthrange);
+  WriteAttr(elem, "lengthrange", 2, pact->lengthrange, def->Actuator().lengthrange);
-  WriteAttr(elem, "gear", 6, pact->gear, def->actuator.gear);
+  WriteAttr(elem, "gear", 6, pact->gear, def->Actuator().gear);
-  WriteAttr(elem, "cranklength", 1, &pact->cranklength, &def->actuator.cranklength);
+  WriteAttr(elem, "cranklength", 1, &pact->cranklength, &def->Actuator().cranklength);
   WriteAttrKey(elem, "actearly", bool_map, 2, pact->actearly,
-               def->actuator.actearly);
+               def->Actuator().actearly);
-  WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->actuator.dyntype);
+  WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->Actuator().dyntype);
-  WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->actuator.dynprm);
+  WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->Actuator().dynprm);
 
   // plugins: write config attributes
   if (pact->plugin.active) {
@@ -713,29 +713,29 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
   else {
     // special handling of actdim which has default value of -1
     if (writingdefaults) {
-      WriteAttrInt(elem, "actdim", pact->actdim, def->actuator.actdim);
+      WriteAttrInt(elem, "actdim", pact->actdim, def->Actuator().actdim);
     } else {
       int default_actdim = pact->dyntype == mjDYN_NONE ? 0 : 1;
       WriteAttrInt(elem, "actdim", pact->actdim, default_actdim);
     }
-    WriteAttrKey(elem, "gaintype", gain_map, gain_sz, pact->gaintype, def->actuator.gaintype);
+    WriteAttrKey(elem, "gaintype", gain_map, gain_sz, pact->gaintype, def->Actuator().gaintype);
-    WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->actuator.biastype);
+    WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->Actuator().biastype);
-    WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->actuator.gainprm, true);
+    WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->Actuator().gainprm, true);
-    WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->actuator.biasprm, true);
+    WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->Actuator().biasprm, true);
   }
 
   // userdata
   if (writingdefaults) {
     WriteVector(elem, "user", pact->get_userdata());
   } else {
-    WriteVector(elem, "user", pact->get_userdata(), def->actuator.get_userdata());
+    WriteVector(elem, "user", pact->get_userdata(), def->Actuator().get_userdata());
   }
 }
 
 
 
 // write plugin
-void mjXWriter::OnePlugin(XMLElement* elem, mjsPlugin* plugin) {
+void mjXWriter::OnePlugin(XMLElement* elem, const mjsPlugin* plugin) {
   const std::string instance_name = std::string(mjs_getString(plugin->instance_name));
   const std::string plugin_name = std::string(mjs_getString(plugin->name));
   if (!instance_name.empty()) {
@@ -769,9 +769,9 @@ mjXWriter::mjXWriter(void) {
 
 
 // cast model
-void mjXWriter::SetModel(mjSpec* modelspec) {
+void mjXWriter::SetModel(mjSpec* spec) {
-  if (modelspec) {
+  if (spec) {
-    model = (mjCModel*)modelspec->element;
+    model = (mjCModel*)spec->element;
   }
 }
 
@@ -806,7 +806,7 @@ string mjXWriter::Write(char *error, size_t error_sz) {
   Visual(root);
   Statistic(root);
   writingdefaults = true;
-  Default(root, model->defaults[0]);
+  Default(root, model->Defaults(0));
   writingdefaults = false;
   Extension(root);
   Custom(root);
@@ -1136,7 +1136,7 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) {
   // pointer to parent defaults
   mjCDef* par;
   if (def->parentid>=0) {
-    par = model->defaults[def->parentid];
+    par = model->Defaults(def->parentid);
   } else {
     par = new mjCDef;
   }
@@ -1147,57 +1147,57 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) {
 
   // mesh
   elem = InsertEnd(section, "mesh");
-  OneMesh(elem, &def->mesh, par);
+  OneMesh(elem, &def->Mesh(), par);
   if (!elem->FirstAttribute()) section->DeleteChild(elem);
 
   // material
   elem = InsertEnd(section, "material");
-  OneMaterial(elem, &def->material, par);
+  OneMaterial(elem, &def->Material(), par);
   if (!elem->FirstAttribute()) section->DeleteChild(elem);
 
   // joint
   elem = InsertEnd(section, "joint");
-  OneJoint(elem, &def->joint, par);
+  OneJoint(elem, &def->Joint(), par);
   if (!elem->FirstAttribute()) section->DeleteChild(elem);
 
   // geom
   elem = InsertEnd(section, "geom");
-  OneGeom(elem, &def->geom, par);
+  OneGeom(elem, &def->Geom(), par);
   if (!elem->FirstAttribute()) section->DeleteChild(elem);
 
   // site
   elem = InsertEnd(section, "site");
-  OneSite(elem, &def->site, par);
+  OneSite(elem, &def->Site(), par);
   if (!elem->FirstAttribute()) section->DeleteChild(elem);
 
   // camera
   elem = InsertEnd(section, "camera");
-  OneCamera(elem, &def->camera, par);
+  OneCamera(elem, &def->Camera(), par);
   if (!elem->FirstAttribute()) section->DeleteChild(elem);
 
   // light
   elem = InsertEnd(section, "light");
-  OneLight(elem, &def->light, par);
+  OneLight(elem, &def->Light(), par);
   if (!elem->FirstAttribute()) section->DeleteChild(elem);
 
   // pair
   elem = InsertEnd(section, "pair");
-  OnePair(elem, &def->pair, par);
+  OnePair(elem, &def->Pair(), par);
   if (!elem->FirstAttribute()) section->DeleteChild(elem);
 
   // equality
   elem = InsertEnd(section, "equality");
-  OneEquality(elem, &def->equality, par);
+  OneEquality(elem, &def->Equality(), par);
   if (!elem->FirstAttribute()) section->DeleteChild(elem);
 
   // tendon
   elem = InsertEnd(section, "tendon");
-  OneTendon(elem, &def->tendon, par);
+  OneTendon(elem, &def->Tendon(), par);
   if (!elem->FirstAttribute()) section->DeleteChild(elem);
 
   // actuator
   elem = InsertEnd(section, "general");
-  OneActuator(elem, &def->actuator, par);
+  OneActuator(elem, &def->Actuator(), par);
   if (!elem->FirstAttribute()) section->DeleteChild(elem);
 
   // if top-level class has no members or children, delete it and return
@@ -1209,7 +1209,7 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) {
 
   // add children recursively
   for (int i=0; i<(int)def->childid.size(); i++) {
-    Default(section, model->defaults[def->childid[i]]);
+    Default(section, model->Defaults(def->childid[i]));
   }
 
   // delete parent defaults if allocated here
@@ -1223,7 +1223,7 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) {
 // extension section
 void mjXWriter::Extension(XMLElement* root) {
   // skip section if there is no required plugin
-  if (model->active_plugins.empty()) {
+  if (model->ActivePlugins().empty()) {
     return;
   }
 
@@ -1236,7 +1236,7 @@ void mjXWriter::Extension(XMLElement* root) {
   // write all plugins
   const mjpPlugin* last_plugin = nullptr;
   XMLElement* plugin_elem = nullptr;
-  for (int i = 0; i < model->plugins.size(); ++i) {
+  for (int i = 0; i < model->Plugins().size(); ++i) {
     mjCPlugin* pp = static_cast<mjCPlugin*>(model->GetObject(mjOBJ_PLUGIN, i));
 
     if (pp->name.empty()) {
@@ -1273,7 +1273,7 @@ void mjXWriter::Extension(XMLElement* root) {
   }
 
   // write <plugin> elements for plugins without explicit instances
-  for (const auto& [plugin, slot] : model->active_plugins) {
+  for (const auto& [plugin, slot] : model->ActivePlugins()) {
     if (seen_plugins.find(plugin) == seen_plugins.end()) {
       plugin_elem = InsertEnd(section, "plugin");
       WriteAttrTxt(plugin_elem, "plugin", plugin->name);
@@ -1677,7 +1677,7 @@ void mjXWriter::Tendon(XMLElement* root) {
     // write wraps
     XMLElement* wrap;
     for (int j=0; j<pten->NumWraps(); j++) {
-      mjCWrap* pw = pten->GetWrap(j);
+      const mjCWrap* pw = pten->GetWrap(j);
       switch (pw->type) {
       case mjWRAP_JOINT:
         wrap = InsertEnd(elem, "joint");
@@ -1755,7 +1755,7 @@ void mjXWriter::Sensor(XMLElement* root) {
   // write all sensors
   for (int i=0; i<num; i++) {
     XMLElement* elem = 0;
-    mjCSensor* psen = model->sensors[i];
+    mjCSensor* psen = model->Sensors()[i];
     std::string instance_name = "";
     std::string plugin_name = "";
 
@@ -1875,46 +1875,82 @@ void mjXWriter::Sensor(XMLElement* root) {
       elem = InsertEnd(section, "framepos");
       WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
       WriteAttrTxt(elem, "objname", psen->get_objname());
+      if (psen->reftype != mjOBJ_UNKNOWN) {
+        WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
+        WriteAttrTxt(elem, "refname", psen->get_refname());
+      }
       break;
     case mjSENS_FRAMEQUAT:
       elem = InsertEnd(section, "framequat");
       WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
       WriteAttrTxt(elem, "objname", psen->get_objname());
+      if (psen->reftype != mjOBJ_UNKNOWN) {
+        WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
+        WriteAttrTxt(elem, "refname", psen->get_refname());
+      }
       break;
     case mjSENS_FRAMEXAXIS:
       elem = InsertEnd(section, "framexaxis");
       WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
       WriteAttrTxt(elem, "objname", psen->get_objname());
+      if (psen->reftype != mjOBJ_UNKNOWN) {
+        WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
+        WriteAttrTxt(elem, "refname", psen->get_refname());
+      }
       break;
     case mjSENS_FRAMEYAXIS:
       elem = InsertEnd(section, "frameyaxis");
       WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
       WriteAttrTxt(elem, "objname", psen->get_objname());
+      if (psen->reftype != mjOBJ_UNKNOWN) {
+        WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
+        WriteAttrTxt(elem, "refname", psen->get_refname());
+      }
       break;
     case mjSENS_FRAMEZAXIS:
       elem = InsertEnd(section, "framezaxis");
       WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
       WriteAttrTxt(elem, "objname", psen->get_objname());
+      if (psen->reftype != mjOBJ_UNKNOWN) {
+        WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
+        WriteAttrTxt(elem, "refname", psen->get_refname());
+      }
       break;
     case mjSENS_FRAMELINVEL:
       elem = InsertEnd(section, "framelinvel");
       WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
       WriteAttrTxt(elem, "objname", psen->get_objname());
+      if (psen->reftype != mjOBJ_UNKNOWN) {
+        WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
+        WriteAttrTxt(elem, "refname", psen->get_refname());
+      }
       break;
     case mjSENS_FRAMEANGVEL:
       elem = InsertEnd(section, "frameangvel");
       WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
       WriteAttrTxt(elem, "objname", psen->get_objname());
+      if (psen->reftype != mjOBJ_UNKNOWN) {
+        WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
+        WriteAttrTxt(elem, "refname", psen->get_refname());
+      }
       break;
     case mjSENS_FRAMELINACC:
       elem = InsertEnd(section, "framelinacc");
       WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
       WriteAttrTxt(elem, "objname", psen->get_objname());
+      if (psen->reftype != mjOBJ_UNKNOWN) {
+        WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
+        WriteAttrTxt(elem, "refname", psen->get_refname());
+      }
       break;
     case mjSENS_FRAMEANGACC:
       elem = InsertEnd(section, "frameangacc");
       WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
       WriteAttrTxt(elem, "objname", psen->get_objname());
+      if (psen->reftype != mjOBJ_UNKNOWN) {
+        WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
+        WriteAttrTxt(elem, "refname", psen->get_refname());
+      }
       break;
 
     // sensors related to kinematic subtrees; attached to a body (which is the subtree root)
@@ -1930,6 +1966,21 @@ void mjXWriter::Sensor(XMLElement* root) {
       elem = InsertEnd(section, "subtreeangmom");
       WriteAttrTxt(elem, "body", psen->get_objname());
       break;
+    case mjSENS_GEOMDIST:
+      elem = InsertEnd(section, "distance");
+      WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname());
+      WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname());
+      break;
+    case mjSENS_GEOMNORMAL:
+      elem = InsertEnd(section, "normal");
+      WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname());
+      WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname());
+      break;
+    case mjSENS_GEOMFROMTO:
+      elem = InsertEnd(section, "fromto");
+      WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname());
+      WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname());
+      break;
 
     // global sensors
     case mjSENS_CLOCK:
@@ -1968,12 +2019,6 @@ void mjXWriter::Sensor(XMLElement* root) {
       WriteAttr(elem, "noise", 1, &psen->noise, &zero);
     }
     WriteVector(elem, "user", psen->get_userdata());
-
-    // add reference if present
-    if (psen->reftype != mjOBJ_UNKNOWN && psen->type != mjSENS_CAMPROJECTION) {
-      WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
-      WriteAttrTxt(elem, "refname", psen->get_refname());
-    }
   }
 
   // remove section if empty
@@ -1994,7 +2039,7 @@ void mjXWriter::Keyframe(XMLElement* root) {
     XMLElement* elem = InsertEnd(section, "key");
     bool change = false;
 
-    mjCKey* pk = model->keys[i];
+    mjCKey* pk = model->Keys()[i];
 
     // check name and write
     if (!pk->name.empty()) {
@@ -2038,8 +2083,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
     // check mpos and write
     if (model->nmocap) {
       for (int j=0; j<model->nbody; j++) {
-        if (model->bodies[j]->mocap) {
+        if (model->Bodies()[j]->mocap) {
-          mjCBody* pb = model->bodies[j];
+          mjCBody* pb = model->Bodies()[j];
           int id = pb->mocapid;
           if (pb->pos[0] != pk->mpos_[3*id] ||
               pb->pos[1] != pk->mpos_[3*id+1] ||
@@ -2055,8 +2100,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
     // check mquat and write
     if (model->nmocap) {
       for (int j=0; j<model->nbody; j++) {
-        if (model->bodies[j]->mocap) {
+        if (model->Bodies()[j]->mocap) {
-          mjCBody* pb = model->bodies[j];
+          mjCBody* pb = model->Bodies()[j];
           int id = pb->mocapid;
           if (pb->quat[0] != pk->mquat_[4*id] ||
               pb->quat[1] != pk->mquat_[4*id+1] ||

src/xml/xml_native_writer.h

@@ -27,7 +27,7 @@ class mjXWriter : public mjXBase {
  public:
   mjXWriter();                                        // constructor
   virtual ~mjXWriter() = default;                     // destructor
-  void SetModel(mjSpec* modelspec);
+  void SetModel(mjSpec* spec);
 
   // write XML document to string
   std::string Write(char *error, std::size_t error_sz);
@@ -59,20 +59,20 @@ class mjXWriter : public mjXBase {
   void Keyframe(tinyxml2::XMLElement* root);                              // keyframe section
 
   // single element writers, used in defaults and main body
-  void OneFlex(tinyxml2::XMLElement* elem,      mjCFlex* pflex);
+  void OneFlex(tinyxml2::XMLElement* elem, const mjCFlex* pflex);
-  void OneMesh(tinyxml2::XMLElement* elem,      mjCMesh* pmesh,         mjCDef* def);
+  void OneMesh(tinyxml2::XMLElement* elem, const mjCMesh* pmesh,         mjCDef* def);
-  void OneSkin(tinyxml2::XMLElement* elem,      mjCSkin* pskin);
+  void OneSkin(tinyxml2::XMLElement* elem, const mjCSkin* pskin);
-  void OneMaterial(tinyxml2::XMLElement* elem,  mjCMaterial* pmaterial, mjCDef* def);
+  void OneMaterial(tinyxml2::XMLElement* elem, const mjCMaterial* pmaterial, mjCDef* def);
-  void OneJoint(tinyxml2::XMLElement* elem,     mjCJoint* pjoint,       mjCDef* def);
+  void OneJoint(tinyxml2::XMLElement* elem, const mjCJoint* pjoint,       mjCDef* def);
-  void OneGeom(tinyxml2::XMLElement* elem,      mjCGeom* pgeom,         mjCDef* def);
+  void OneGeom(tinyxml2::XMLElement* elem, const mjCGeom* pgeom,         mjCDef* def);
-  void OneSite(tinyxml2::XMLElement* elem,      mjCSite* psite,         mjCDef* def);
+  void OneSite(tinyxml2::XMLElement* elem, const mjCSite* psite,         mjCDef* def);
-  void OneCamera(tinyxml2::XMLElement* elem,    mjCCamera* pcamera,     mjCDef* def);
+  void OneCamera(tinyxml2::XMLElement* elem, const mjCCamera* pcamera,     mjCDef* def);
-  void OneLight(tinyxml2::XMLElement* elem,     mjCLight* plight,       mjCDef* def);
+  void OneLight(tinyxml2::XMLElement* elem, const mjCLight* plight,       mjCDef* def);
-  void OnePair(tinyxml2::XMLElement* elem,      mjCPair* ppair,         mjCDef* def);
+  void OnePair(tinyxml2::XMLElement* elem, const mjCPair* ppair,         mjCDef* def);
-  void OneEquality(tinyxml2::XMLElement* elem,  mjCEquality* pequality, mjCDef* def);
+  void OneEquality(tinyxml2::XMLElement* elem, const mjCEquality* pequality, mjCDef* def);
-  void OneTendon(tinyxml2::XMLElement* elem,    mjCTendon* ptendon,     mjCDef* def);
+  void OneTendon(tinyxml2::XMLElement* elem, const mjCTendon* ptendon,     mjCDef* def);
-  void OneActuator(tinyxml2::XMLElement* elem,  mjCActuator* pactuator, mjCDef* def);
+  void OneActuator(tinyxml2::XMLElement* elem, const mjCActuator* pactuator, mjCDef* def);
-  void OnePlugin(tinyxml2::XMLElement* elem,    mjsPlugin* plugin);
+  void OnePlugin(tinyxml2::XMLElement* elem, const mjsPlugin* plugin);
   tinyxml2::XMLElement* OneFrame(tinyxml2::XMLElement* elem, mjCFrame* frame);
 
   bool writingdefaults;                       // true during defaults write

src/xml/xml_urdf.cc

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <array>
+#include <cstdlib>
 #include <cstring>
 #include <iostream>
 #include <string>
@@ -488,17 +489,23 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
 
   // limit element
   if ((elem = FindSubElem(joint_elem, "limit"))) {
-    ReadAttr(elem, "lower", 1, pjoint->range, text);
+    bool haslower = ReadAttr(elem, "lower", 1, pjoint->range, text);
-    ReadAttr(elem, "upper", 1, pjoint->range+1, text);
+    bool hasupper = ReadAttr(elem, "upper", 1, pjoint->range+1, text);
-    bool is_limited = mjuu_defined(pjoint->range[0]) &&
+
-                      mjuu_defined(pjoint->range[1]) &&
+    // handle range mis-specification, otherwise the default mjLIMITED_AUTO will do the right thing
-                      pjoint->range[0] < pjoint->range[1];
+    bool bad_range = (haslower != hasupper) || pjoint->range[0] > pjoint->range[1];
-    pjoint->limited = is_limited ? mjLIMITED_TRUE : mjLIMITED_FALSE;
+    if (bad_range) {
+      pjoint->limited = mjLIMITED_FALSE;
+    }
 
     // ReadAttr(elem, "velocity", 1, &pjoint->maxvel, text); // no maxvel in MuJoCo
-    ReadAttr(elem, "effort", 1, &pjoint->urdfeffort, text);
+    double effort = 0;
-  } else {
+    ReadAttr(elem, "effort", 1, &effort, text);
-    pjoint->limited = mjLIMITED_FALSE;
+    effort = std::abs(effort);
+    if (effort > 0) {
+      pjoint->actfrcrange[0] = -effort;
+      pjoint->actfrcrange[1] = effort;
+    }
   }
 }
 

src/xml/xml_util.cc

@@ -481,6 +481,16 @@ bool mjXUtil::ReadAttrValues(XMLElement* elem, const char* attr,
   // read numbers
   for (int i = 0; (max < 0 || i < max) && !strm.eof(); ++i) {
     strm >> token;
+
+    // some C++ libraries do not allow .x instead of 0.x
+    if (!token.empty()) {
+      if (token[0] == std::string(".")[0]) {
+        token = "0" + token;
+      } else if (token[0] == std::string("-")[0] && token[1] == std::string(".")[0]) {
+        token.insert(1, "0");
+      }
+    }
+
     std::istringstream token_strm(token);
     token_strm >> item;
     if (token_strm.fail() || !token_strm.eof()) {

test/engine/CMakeLists.txt

@@ -96,9 +96,6 @@ target_link_libraries(engine_util_solve_test fixture gmock)
 mujoco_test(engine_util_spatial_test)
 target_link_libraries(engine_util_spatial_test fixture gmock)
 
-mujoco_test(engine_vfs_test)
-target_link_libraries(engine_vfs_test fixture gmock)
-
 mujoco_test(
   engine_vis_state_test
   PROPERTIES