Add mj_geomDistance API function and sensors for geometric distance, normal direction and segment between between two geoms. Fixes #51

PiperOrigin-RevId: 632458301
Change-Id: I65b7e5bb0df59008028114cf4a07e0d6365ff3d3

doc/APIreference/functions.rst

@@ -57,8 +57,8 @@ 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 will
-called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and
+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.
@@ -72,8 +72,8 @@ be set before calling this function. Given the state (qpos, qvel, act), mj_forwa
 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
@@ -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,8 +33,8 @@ 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 will
-called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and
+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.
@@ -48,8 +48,8 @@ be set before calling this function. Given the state (qpos, qvel, act), mj_forwa
 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
@@ -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

@@ -6723,6 +6723,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 catagories.
+   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 catagories.
+   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 catagories.
+   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:
 

doc/XMLschema.rst

@@ -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>`                    |    |                                                                                                                                                                                                                                                                              |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |

doc/changelog.rst

@@ -5,12 +5,19 @@ Changelog
 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`.
+
 Bug fixes
 ^^^^^^^^^
 
-1. Fixed a bug the could cause collisions to be missed when :ref:`fusestatic<compiler-fusestatic>` is enabled, as is
+2. 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`.
-2. Fixed a bug that was causing the visualization of SDF iterations to write outside the size of the vector storing
+3. 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)

doc/includes/references.h

@@ -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);

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

@@ -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]);
 

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',

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,

src/engine/engine_io.c

@@ -1773,6 +1773,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 +1798,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_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_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"
@@ -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
       }
     }
   }

src/user/user_objects.cc

@@ -5388,7 +5388,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");
   }
@@ -5402,7 +5401,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
@@ -5460,7 +5459,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
@@ -5498,7 +5497,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
@@ -5522,7 +5521,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
@@ -5540,7 +5539,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
@@ -5559,7 +5558,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
@@ -5584,7 +5583,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
@@ -5609,7 +5608,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
@@ -5675,7 +5674,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
@@ -5688,6 +5687,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;
@@ -5737,7 +5764,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/xml/xml_native_reader.cc

@@ -472,6 +472,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"},
@@ -3949,6 +3952,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

@@ -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

@@ -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

test/engine/engine_sensor_test.cc

@@ -14,6 +14,8 @@
 
 // Tests for engine/engine_sensor.c.
 
+#include <vector>
+
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 #include <mujoco/mjmodel.h>
@@ -44,7 +46,7 @@ using ::testing::StrEq;
 
 using SensorTest = MujocoTest;
 
-// --------------------- test sensor disableflag  -----------------------------
+// --------------------- test sensor disableflag  ------------------------------
 
 // hand-picked positions and orientations for simple expected values
 TEST_F(SensorTest, DisableSensors) {
@@ -255,7 +257,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelLinearFixed) {
   mj_deleteModel(model);
 }
 
-// object and reference in the same body, expect angular velocites to be zero
+// object and reference in the same body, expect angular velocities to be zero
 TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) {
   constexpr char xml[] = R"(
   <mujoco>
@@ -279,7 +281,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) {
   data->qvel[0] = 1;
   mj_forward(model, data);
 
-  // obj and ref rotate together, relative angular velocites should be zero
+  // obj and ref rotate together, relative angular velocities should be zero
   std::vector angvel = GetSensor(model, data, 0);
   EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), {0, 0, 0}));
 
@@ -422,7 +424,7 @@ TEST_F(SensorTest, Clock) {
     EXPECT_EQ(data->sensordata[1], mju_min(data->time, 3e-3));
   }
 
-  // chack names
+  // check names
   const char* name0 = mj_id2name(model, mjOBJ_SENSOR, 0);
   EXPECT_EQ(name0, nullptr);
   const char* name1 = mj_id2name(model, mjOBJ_SENSOR, 1);
@@ -432,7 +434,80 @@ TEST_F(SensorTest, Clock) {
   mj_deleteModel(model);
 }
 
-// ------------------------- camera sensor tests  -----------------------------
+// test clock sensor
+TEST_F(SensorTest, CollisionSequential) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom name="plane" type="plane" size="1 1 1"/>
+      <geom name="sphere1" pos="0 0 1" size="0.2"/>
+      <geom name="sphere2" pos="1 0 1" size="0.3"/>
+    </worldbody>
+    <sensor>
+      <distance name="0"  geom1="plane"   geom2="sphere1" cutoff="1"/>
+      <distance name="1"  geom1="sphere2" geom2="plane" cutoff="1"/>
+      <distance name="2"  geom1="sphere1" geom2="sphere2" cutoff="1"/>
+      <normal   name="3"  geom1="plane"   geom2="sphere1" cutoff="1"/>
+      <normal   name="4"  geom1="sphere2" geom2="plane" cutoff="1"/>
+      <normal   name="5"  geom1="sphere1" geom2="sphere2" cutoff="1"/>
+      <fromto   name="6"  geom1="plane"   geom2="sphere1" cutoff="1"/>
+      <fromto   name="7"  geom1="sphere2" geom2="plane" cutoff="1"/>
+      <fromto   name="8"  geom1="sphere1" geom2="sphere2" cutoff="1"/>
+      <!-- sequential sensors with identical signature -->
+      <distance name="9"  geom1="plane"   geom2="sphere1" cutoff="1"/>
+      <fromto   name="10" geom1="plane"   geom2="sphere1" cutoff="1"/>
+      <normal   name="11" geom1="plane"   geom2="sphere1" cutoff="1"/>
+      <normal   name="12"  geom1="sphere1" geom2="sphere2" cutoff="1"/>
+      <fromto   name="13"  geom1="sphere1" geom2="sphere2" cutoff="1"/>
+      <distance name="14"  geom1="sphere1" geom2="sphere2" cutoff="1"/>
+
+    </sensor>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml);
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  EXPECT_DOUBLE_EQ(data->sensordata[0], 0.8);
+  EXPECT_DOUBLE_EQ(data->sensordata[1], 0.7);
+  EXPECT_DOUBLE_EQ(data->sensordata[2], 0.5);
+
+  mjtNum eps = 1e-14;
+
+  EXPECT_THAT(GetSensor(model, data, 3),
+              Pointwise(DoubleNear(eps), std::vector<mjtNum>{0, 0, 1}));
+  EXPECT_THAT(GetSensor(model, data, 4),
+              Pointwise(DoubleNear(eps), std::vector<mjtNum>{0, 0, -1}));
+  EXPECT_THAT(GetSensor(model, data, 5),
+              Pointwise(DoubleNear(eps), std::vector<mjtNum>{1, 0, 0}));
+  EXPECT_THAT(GetSensor(model, data, 6),
+              Pointwise(DoubleNear(eps),
+                        std::vector<mjtNum>{0, 0, 0, 0, 0, .8}));
+  EXPECT_THAT(GetSensor(model, data, 7),
+              Pointwise(DoubleNear(eps),
+                        std::vector<mjtNum>{1, 0, .7, 1, 0, 0}));
+  EXPECT_THAT(GetSensor(model, data, 8),
+              Pointwise(DoubleNear(eps),
+                        std::vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
+
+  EXPECT_THAT(GetSensor(model, data, 9),
+              Pointwise(DoubleNear(eps), GetSensor(model, data, 0)));
+  EXPECT_THAT(GetSensor(model, data, 10),
+              Pointwise(DoubleNear(eps), GetSensor(model, data, 6)));
+  EXPECT_THAT(GetSensor(model, data, 11),
+              Pointwise(DoubleNear(eps), GetSensor(model, data, 3)));
+  EXPECT_THAT(GetSensor(model, data, 12),
+              Pointwise(DoubleNear(eps), GetSensor(model, data, 5)));
+  EXPECT_THAT(GetSensor(model, data, 13),
+              Pointwise(DoubleNear(eps), GetSensor(model, data, 8)));
+  EXPECT_THAT(GetSensor(model, data, 14),
+              Pointwise(DoubleNear(eps), GetSensor(model, data, 2)));
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+// ------------------------- camera sensor tests  ------------------------------
 
 // test clock sensor
 TEST_F(SensorTest, CameraProjection) {

test/engine/engine_support_test.cc

@@ -34,7 +34,8 @@ std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
 }
 
 using ::testing::DoubleNear;
-using ::testing::ContainsRegex;
+using ::testing::Eq;
+using ::testing::ContainsRegex;  // NOLINT
 using ::testing::MatchesRegex;
 using ::testing::Pointwise;
 using ::testing::ElementsAreArray;
@@ -647,5 +648,79 @@ TEST_F(SupportTest, MulMIsland) {
   mj_deleteModel(model);
 }
 
+static constexpr char GeomDistanceTestingModel[] = R"(
+<mujoco>
+  <asset>
+    <mesh name="box" scale=".1 .1 .1" vertex="0 0 0  1 0 0  0 1 0  1 1 0
+                                              0 0 1  1 0 1  0 1 1  1 1 1"/>
+  </asset>
+
+  <worldbody>
+    <geom type="plane" size="1 1 1"/>
+    <geom pos="0 0 1" size="0.2"/>
+    <geom pos="1 0 1" size="0.3"/>
+    <geom type="mesh" mesh="box"/>
+  </worldbody>
+</mujoco>
+)";
+
+TEST_F(SupportTest, GeomDistance) {
+  mjModel* model = LoadModelFromString(GeomDistanceTestingModel);
+  mjData* data = mj_makeData(model);
+  mj_kinematics(model, data);
+
+  // plane-sphere, distmax too small
+  mjtNum distmax = 0.5;
+  EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, nullptr), 0.5);
+  mjtNum fromto[6];
+  EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, fromto), 0.5);
+  EXPECT_THAT(fromto, Pointwise(Eq(), std::vector<mjtNum>{0, 0, 0, 0, 0, 0}));
+
+  // plane-sphere
+  distmax = 1.0;
+  EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 0, 1, 1.0, fromto), 0.8);
+  mjtNum eps = 1e-12;
+  EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
+                                std::vector<mjtNum>{0, 0, 0, 0, 0, 0.8}));
+
+  // sphere-plane
+  EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 1, 0, 1.0, fromto), 0.8);
+  EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
+                                std::vector<mjtNum>{0, 0, 0.8, 0, 0, 0}));
+
+  // sphere-sphere
+  EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 1, 2, 1.0, fromto), 0.5);
+  EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
+                                std::vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
+
+  // sphere-sphere, flipped order
+  EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 2, 1, 1.0, fromto), 0.5);
+  EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
+                                std::vector<mjtNum>{.7, 0, 1, .2, 0, 1}));
+
+  // TODO: b/339596989 - Improve the bounds below (mjc_Convex).
+
+  // mesh-sphere (close distmax)
+  distmax = 0.701;
+  eps = 1e-5;
+  EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto),
+              DoubleNear(0.7, eps));
+  eps = 1e-3;
+  EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
+                                std::vector<mjtNum>{0, 0, .1, 0, 0, .8}));
+
+  // mesh-sphere (far distmax)
+  distmax = 1.0;
+  eps = 1e-3;
+  EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto),
+              DoubleNear(0.7, eps));
+  eps = 2e-2;
+  EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
+                                std::vector<mjtNum>{0, 0, .1, 0, 0, .8}));
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco

test/engine/testdata/sensor/fromto_body_body.xml

@@ -0,0 +1,29 @@
+<mujoco model="Body-Body collisions fromto">
+  <option>
+    <flag gravity="disable"/>
+  </option>
+
+  <worldbody>
+    <light pos=".5 0 3"/>
+    <light pos="-.5 0 3"/>
+    <geom type="plane" size="2 2 .01"/>
+
+    <body name="1" pos="0 0 .3">
+      <freejoint/>
+      <replicate count="50" offset="0 0 0.01" euler="0 0 25">
+        <geom type="capsule" size=".01 .05" pos="0 -.23 -.2" euler="0 84.5 0" rgba="0 0 1 1"/>
+      </replicate>
+    </body>
+
+    <body name="2" pos=".6 0 .3" euler="0 0 90">
+      <freejoint/>
+      <replicate count="50" offset="0 0 0.01" euler="0 0 15">
+        <geom type="capsule" pos=".025 -.2 -.2" size=".032 .04" euler="-24 -12 2" rgba=".8 0 0 1"/>
+      </replicate>
+    </body>
+  </worldbody>
+
+  <sensor>
+    <fromto body1="1" body2="2" cutoff="1"/>
+  </sensor>
+</mujoco>

test/engine/testdata/sensor/fromto_convex.xml

@@ -0,0 +1,83 @@
+<mujoco model="Convex-Convex collisions">
+  <option>
+    <flag gravity="disable"/>
+  </option>
+
+  <asset>
+    <mesh name="halfbox1" scale=".2 .3 .1"
+          vertex="0 0 0  1 0 0  0 1 0  1 1 0
+                  0 0 1  1 0 1  0 1 1 "/>
+    <mesh name="halfbox2" scale=".2 .3 .1"
+          vertex="1 0 0  0 1 0  1 1 0
+                  0 0 1  1 0 1  0 1 1 "/>
+    <hfield name="hfield" nrow="3" ncol="4" size="1 1 .1 .08"
+            elevation="1 0 1 2
+                       0 2 1 1
+                       1 0 1 0"/>
+  </asset>
+
+  <default>
+    <geom rgba=".2 .2 .6 1"/>
+  </default>
+
+  <worldbody>
+    <light pos=".5 0 3"/>
+    <light pos="-.5 0 3"/>
+
+    <geom name="hfield" type="hfield" hfield="hfield" rgba="0.5 0.5 0.5 1"/>
+
+    <body pos=".2 0 .6">
+      <freejoint/>
+      <geom name="sphere" type="sphere" size=".15"/>
+    </body>
+
+    <body pos="0 0 .3">
+      <freejoint/>
+      <geom name="ellipsoid" type="ellipsoid" size=".1 .1 .2"/>
+    </body>
+
+    <body pos=".4 0 .3">
+      <freejoint/>
+      <geom name="cylinder" type="cylinder" size=".1 .15"/>
+    </body>
+
+    <body pos="-.5 0 .3">
+      <freejoint/>
+      <geom name="box" type="box" size=".2 .15 .1"/>
+    </body>
+
+    <body pos="-.5 0 .6">
+      <freejoint/>
+      <geom name="mesh1" type="mesh" mesh="halfbox1"/>
+    </body>
+
+    <body pos="-.5 -.6 .3">
+      <freejoint/>
+      <geom name="mesh2" type="mesh" mesh="halfbox2"/>
+    </body>
+  </worldbody>
+
+  <sensor>
+    <fromto geom1="ellipsoid" geom2="box" cutoff="1"/>
+    <fromto geom1="ellipsoid" geom2="cylinder" cutoff="1"/>
+    <fromto geom1="ellipsoid" geom2="mesh2" cutoff="1"/>
+    <fromto geom1="ellipsoid" geom2="hfield" cutoff="1"/>
+    <fromto geom1="mesh1" geom2="sphere" cutoff="1"/>
+    <fromto geom1="mesh1" geom2="box" cutoff="1"/>
+    <fromto geom1="mesh1" geom2="mesh2" cutoff="1"/>
+    <fromto geom1="hfield" geom2="sphere" cutoff="1"/>
+    <fromto geom1="hfield" geom2="box" cutoff="1"/>
+    <fromto geom1="hfield" geom2="mesh1" cutoff="1"/>
+
+    <fromto geom1="ellipsoid" geom2="box" cutoff="0.2"/>
+    <fromto geom1="ellipsoid" geom2="cylinder" cutoff="0.2"/>
+    <fromto geom1="ellipsoid" geom2="mesh2" cutoff="0.2"/>
+    <fromto geom1="ellipsoid" geom2="hfield" cutoff="0.2"/>
+    <fromto geom1="mesh1" geom2="sphere" cutoff="0.2"/>
+    <fromto geom1="mesh1" geom2="box" cutoff="0.2"/>
+    <fromto geom1="mesh1" geom2="mesh2" cutoff="0.2"/>
+    <fromto geom1="hfield" geom2="sphere" cutoff="0.2"/>
+    <fromto geom1="hfield" geom2="box" cutoff="0.2"/>
+    <fromto geom1="hfield" geom2="mesh1" cutoff="0.2"/>
+  </sensor>
+</mujoco>

test/engine/testdata/sensor/fromto_primitive.xml

@@ -0,0 +1,71 @@
+<mujoco model="Primitive collisions">
+  <asset>
+    <mesh name="halfbox" scale=".2 .3 .1"
+          vertex="0 0 0  1 0 0  0 1 0  1 1 0
+                  0 0 1  1 0 1  0 1 1 "/>
+  </asset>
+
+
+  <default>
+    <geom rgba=".2 .2 .6 1"/>
+  </default>
+
+  <worldbody>
+    <light pos=".5 0 3"/>
+    <light pos="-.5 0 3"/>
+    <geom name="plane" type="plane" size="2 2 .01" rgba="0.5 0.5 0.5 1"/>
+
+    <body pos=".2 0 .6">
+      <freejoint/>
+      <geom name="sphere" type="sphere" size=".15"/>
+    </body>
+
+    <body pos="0 0 .3">
+      <freejoint/>
+      <geom name="capsule1" type="capsule" size=".1 .1"/>
+    </body>
+
+    <body pos="-.2 0 .4" euler="90 0 0">
+      <freejoint/>
+      <geom name="capsule2" type="capsule" size=".05 .1"/>
+    </body>
+
+    <body pos=".4 0 .3">
+      <freejoint/>
+      <geom name="cylinder" type="cylinder" size=".1 .15"/>
+    </body>
+
+    <body pos="-.5 0 .3">
+      <freejoint/>
+      <geom name="box1" type="box" size=".2 .15 .1"/>
+    </body>
+
+    <body pos="-.5 0 .6">
+      <freejoint/>
+      <geom name="box2" type="box" size=".1 .05 .1"/>
+    </body>
+
+    <body pos="-.5 -.6 .3">
+      <freejoint/>
+      <geom name="mesh" type="mesh" mesh="halfbox"/>
+    </body>
+
+  </worldbody>
+
+  <sensor>
+    <fromto geom1="plane" geom2="sphere" cutoff="1"/>
+    <fromto geom1="plane" geom2="capsule1" cutoff="1"/>
+    <fromto geom1="plane" geom2="box1" cutoff="1"/>
+    <fromto geom1="plane" geom2="cylinder" cutoff="1"/>
+    <fromto geom1="plane" geom2="mesh" cutoff="1"/>
+
+    <fromto geom1="capsule2" geom2="capsule1" cutoff="1"/>
+    <fromto geom1="capsule1" geom2="sphere" cutoff="1"/>
+
+    <fromto geom1="sphere" geom2="cylinder" cutoff="1"/>
+
+    <fromto geom1="box1" geom2="sphere" cutoff="1"/>
+    <fromto geom1="box2" geom2="capsule2" cutoff="1"/>
+    <fromto geom1="box1" geom2="box2" cutoff="1"/>
+  </sensor>
+</mujoco>

test/user/user_objects_test.cc

@@ -542,7 +542,7 @@ TEST_F(RelativeFrameSensorParsingTest, BadRefName) {
   std::array<char, 1024> error;
   LoadModelFromString(xml, error.data(), error.size());
   EXPECT_THAT(error.data(),
-              HasSubstr("unrecognized name 'wrong_name' of reference frame"));
+              HasSubstr("unrecognized name 'wrong_name' of object"));
   EXPECT_THAT(error.data(), HasSubstr("line 8"));
 }
 
@@ -601,7 +601,7 @@ TEST_F(RelativeFrameSensorParsingTest, BadObjRefName) {
   ASSERT_THAT(model, IsNull());
   EXPECT_THAT(
       error.data(),
-      HasSubstr("unrecognized name 'alessio' of reference frame object"));
+      HasSubstr("unrecognized name 'alessio' of object"));
   EXPECT_THAT(error.data(), HasSubstr("name 'tom'"));
   EXPECT_THAT(error.data(), HasSubstr("line 7"));
 }

unity/Runtime/Bindings/MjBindings.cs

@@ -358,9 +358,12 @@ public enum mjtSensor : int{
   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,
 }
 public enum mjtStage : int{
   mjSTAGE_NONE = 0,
@@ -6668,6 +6671,9 @@ public static unsafe extern void mj_objectVelocity(mjModel_* m, mjData_* d, int
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mj_objectAcceleration(mjModel_* m, mjData_* d, int objtype, int objid, double* res, int flg_local);
 
+[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
+public static unsafe extern double mj_geomDistance(mjModel_* m, mjData_* d, int geom1, int geom2, double distmax, double* fromto);
+
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mj_contactForce(mjModel_* m, mjData_* d, int id, double* result);