Remove batching optimization from GEOM* sensors

PiperOrigin-RevId: 861938606
Change-Id: Ibb135a4d99cab9ce23a04f844f7c730e8f3b31c8

doc/XMLreference.rst

@@ -7452,11 +7452,6 @@ pipeline. These 3 sensors share some common properties:
    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** |*|

doc/changelog.rst

@@ -56,12 +56,18 @@ General
   Unused by the renderer, it serves as a convenient location to store a camera's supported output types.
 - Added :ref:`mj_mountVFS` and :ref:`mj_unmountVFS` functions for mounting a custom VFS provider. Mounting Allows
   providers to be used to open/read/close resources dynamically at arbitrary paths.
+- The optimization whereby sequential :ref:`collision sensors<collision-sensors>` with identical attributes shared
+  computation has been removed. This results in a (likely minor) performance regression for models which exploited
+  this optimization. To recover the performance, use the :ref:`fromto<sensor-fromto>` and compute the other values
+  manually. If ``from = fromto[0:3]`` and ``to = fromto[3:6]`` then ``distance = norm(to-from)`` and
+  ``normal = normalize(to-from)``.
 - Non-breaking ABI changes:
 
   - The type of the ``sig`` (signature) argument of :ref:`mj_stateSize` and related functions has been changed from
     ``unsigned int`` to ``int``. Before this change, invalid negative arguments passed to this function would result in
     a silent implicit cast, now negativity will trigger an error.
   - Added a :ref:`depth<mjtRndFlag>` rendering flag.
+  - Allocating sizes in :ref:`mjModel` now use 64-bit rather than 32-bit integers to accommodate larger scenes.
 
 - :doc:`OpenUSD <OpenUSD/index>`:
 
@@ -71,10 +77,6 @@ General
     CMake utility project.
   - ``USD_DIR`` is no longer used by the MuJoCo CMake project, instead use ``pxr_DIR`` if you have a pre-built USD library.
   - Users no longer have to set ``PXR_PLUGINPATH_NAME`` environment variable, MuJoCo should load USD plugins automatically.
-- Non-breaking ABI changes:
-
-  - Allcating sizes in :ref:`mjModel` now use 64-bit rather than 32-bit integers to accommodate larger
-    scenes.
 
 
 MJX

src/engine/engine_sensor.c

@@ -735,54 +735,17 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
             }
           }
 
-          // write sensordata for this sensor and all subsequent sensors with identical signature
+          // write sensordata
-          int write_sensor = 1;
+          if (type == mjSENS_GEOMDIST) {
-          while (write_sensor) {
+            d->sensordata[adr] = dist;
-            // write geom distance
+          } else if (type == mjSENS_GEOMNORMAL) {
-            if (type == mjSENS_GEOMDIST) {
+            mjtNum normal[3] = {fromto[3]-fromto[0], fromto[4]-fromto[1], fromto[5]-fromto[2]};
-              d->sensordata[adr] = dist;
+            if (normal[0] || normal[1] || normal[2]) {
-            }
+              mju_normalize3(normal);
-
-            // 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]  == cutoff;
-
-            // 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;
             }
+            mju_copy3(d->sensordata + adr, normal);
+          } else {  // mjSENS_GEOMFROMTO
+            mju_copy(d->sensordata + adr, fromto, 6);
           }
         }
         break;