Extend rangefinder sensor to support cameras.

yuvaltassa · copybara-github · commit 9d646e65483b · 2025-12-23T12:04:49.000-08:00
PiperOrigin-RevId: 848255889
Change-Id: I6e1a9ed13d29d2242558625a47e24e47ad6e87db
diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst
@@ -6373,12 +6373,20 @@ site frame. The output is a 3D vector.
 :el-prefix:`sensor/` |-| **rangefinder** (*)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-This element creates a rangefinder. It measures the distance to the nearest geom surface, along the ray defined by the
-positive Z-axis of the sensor site. If the ray does not intersect any geom surface, the sensor output is -1. If the
-origin of the ray is inside a geom, the surface is still sensed (but not the inner volume). Geoms attached to the same
-body as the sensor site are excluded. Invisible geoms, defined as geoms whose rgba (or whose material rgba) has alpha=0,
-are also excluded. Note however that geoms made invisible in the visualizer by disabling their geom group are not
-excluded; this is because sensor calculations are independent of the visualizer.
+This element creates a rangefinder.
+
+- If associated with a :ref:`site<sensor-rangefinder-site>`, it measures the distance to the nearest geom surface, along
+  the ray defined by the positive Z-axis of the site.
+- If associated with a :ref:`camera<sensor-rangefinder-camera>`, it outputs one distance measurement for each pixel in
+  the camera image. Note that cameras face the :ref:`negative Z-axis<body-camera>` of their frame. The number of
+  measurements in this case is equal to product of the camera's width and height
+  :ref:`resolutions<body-camera-resolution>`.
+
+If a ray does not intersect any geom surface, the sensor output is -1. If the origin of the ray is inside a geom, the
+surface is still detected. Geoms attached to the same body as the sensor site/camera are excluded. Invisible geoms,
+defined as geoms whose rgba (or whose material rgba) has alpha=0, are also excluded. Note however that geoms made
+invisible in the visualizer by disabling their geom group are not excluded; this is because sensor calculations are
+independent of the visualizer.
 
 .. _sensor-rangefinder-name:
 
@@ -6393,9 +6401,14 @@ excluded; this is because sensor calculations are independent of the visualizer.
 
 .. _sensor-rangefinder-site:
 
-:at:`site`: :at-val:`string, required`
+:at:`site`: :at-val:`string, optional`
    The site where the sensor is attached.
 
+.. _sensor-rangefinder-camera:
+
+:at:`camera`: :at-val:`string, optional`
+   The camera where the sensor is attached.
+
 .. _sensor-camprojection:
 
 :el-prefix:`sensor/` |-| **camprojection** (*)
diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst
@@ -940,9 +940,9 @@
 | :ref:`rangefinder                  | \* |    :class: mjcf-attributes                                                                                                                                                                                                                                                   |
 | <sensor-rangefinder>`              |    |                                                                                                                                                                                                                                                                              |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`name<sensor-rangefinder-name>`                            | :ref:`site<sensor-rangefinder-site>`                            | :ref:`cutoff<sensor-rangefinder-cutoff>`                        | :ref:`noise<sensor-rangefinder-noise>`                          | |
+|                                    |    |    | :ref:`name<sensor-rangefinder-name>`                            | :ref:`site<sensor-rangefinder-site>`                            | :ref:`camera<sensor-rangefinder-camera>`                        | :ref:`cutoff<sensor-rangefinder-cutoff>`                        | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`user<sensor-rangefinder-user>`                            |                                                                 |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`noise<sensor-rangefinder-noise>`                          | :ref:`user<sensor-rangefinder-user>`                            |                                                                 |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| sensor |br| |_| |L|            |    | .. table::                                                                                                                                                                                                                                                                   |
diff --git a/doc/changelog.rst b/doc/changelog.rst
@@ -20,6 +20,8 @@ General
 ^^^^^^^
 - Camera frustum visualization is now triggered by setting :ref:`resolution<body-camera-resolution>` to values larger
   than 1. Relatedly, frustum visualization also works for :ref:`orthographic<body-camera-projection>` cameras.
+- Rangefinder sensors can now be attached to a camera using the :ref:`ragefinder/camera<sensor-rangefinder-camera>`
+  attribute. In this case, the sensor will cast multiple ray, one for each camera pixel.
 - Non-breaking ABI changes:
 
   - The type of the ``sig`` (signature) argument of :ref:`mj_stateSize` and related functions has been changed from
diff --git a/doc/includes/references.h b/doc/includes/references.h
@@ -678,7 +678,6 @@ typedef enum mjtObj_ {            // type of MujoCo object
   mjOBJ_FRAME         = 100,      // frame
   mjOBJ_DEFAULT,                  // default
   mjOBJ_MODEL                     // entire model
-
 } mjtObj;
 typedef enum mjtSensor_ {         // type of sensor
   // common robotic sensors, attached to a site
@@ -689,7 +688,7 @@ typedef enum mjtSensor_ {         // type of sensor
   mjSENS_FORCE,                   // 3D force between site's body and its parent body
   mjSENS_TORQUE,                  // 3D torque between site's body and its parent body
   mjSENS_MAGNETOMETER,            // 3D magnetometer
-  mjSENS_RANGEFINDER,             // scalar distance to nearest geom or site along z-axis
+  mjSENS_RANGEFINDER,             // scalar distance to nearest geom along z-axis
   mjSENS_CAMPROJECTION,           // pixel coordinates of a site in the camera image
 
   // sensors related to scalar joints, tendons, actuators
diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h
@@ -296,7 +296,6 @@ typedef enum mjtObj_ {            // type of MujoCo object
   mjOBJ_FRAME         = 100,      // frame
   mjOBJ_DEFAULT,                  // default
   mjOBJ_MODEL                     // entire model
-
 } mjtObj;
 
 
@@ -309,7 +308,7 @@ typedef enum mjtSensor_ {         // type of sensor
   mjSENS_FORCE,                   // 3D force between site's body and its parent body
   mjSENS_TORQUE,                  // 3D torque between site's body and its parent body
   mjSENS_MAGNETOMETER,            // 3D magnetometer
-  mjSENS_RANGEFINDER,             // scalar distance to nearest geom or site along z-axis
+  mjSENS_RANGEFINDER,             // scalar distance to nearest geom along z-axis
   mjSENS_CAMPROJECTION,           // pixel coordinates of a site in the camera image
 
   // sensors related to scalar joints, tendons, actuators
diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c
@@ -435,11 +435,62 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
         break;
 
       case mjSENS_RANGEFINDER:                            // rangefinder
-        rvec[0] = d->site_xmat[9*objid+2];
-        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], NULL);
+        if (objtype == mjOBJ_SITE) {
+          rvec[0] = d->site_xmat[9*objid+2];
+          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], NULL);
+        } else {
+          // camera-attached rangefinder: depth image
+          const int width = m->cam_resolution[2*objid];
+          const int height = m->cam_resolution[2*objid+1];
+          const int bodyexclude = m->cam_bodyid[objid];
+          const mjtNum* cam_xpos = d->cam_xpos + 3*objid;
+          const mjtNum* cam_xmat = d->cam_xmat + 9*objid;
+          const int projection = m->cam_projection[objid];
+
+          // compute focal length in pixels using helper
+          mjtNum fx, fy, cx, cy, ortho_extent;
+          mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent);
+
+          if (projection == mjPROJ_PERSPECTIVE) {
+            // perspective: all rays share origin, different directions
+            const int npixel = width * height;
+            mj_markStack(d);
+            mjtNum* vec = mjSTACKALLOC(d, 3*npixel, mjtNum);
+            int* geomid = mjSTACKALLOC(d, npixel, int);
+
+            // compute ray directions using helper (normalized)
+            for (int row = 0; row < height; row++) {
+              for (int col = 0; col < width; col++) {
+                int idx = row*width + col;
+                mjtNum origin[3];
+                mju_camPixelRay(origin, vec + 3*idx, cam_xpos, cam_xmat,
+                                col, row, fx, fy, cx, cy, projection, ortho_extent);
+              }
+            }
+
+            // cast all rays
+            mj_multiRay(m, d, cam_xpos, vec, NULL, 1, bodyexclude,
+                        geomid, d->sensordata + adr, npixel, mjMAXVAL);
+
+            mj_freeStack(d);
+          } else {
+            // orthographic: parallel rays, different origins
+            for (int row = 0; row < height; row++) {
+              for (int col = 0; col < width; col++) {
+                int idx = row*width + col;
+                mjtNum origin[3], direction[3];
+                mju_camPixelRay(origin, direction, cam_xpos, cam_xmat,
+                                col, row, fx, fy, cx, cy, projection, ortho_extent);
+
+                d->sensordata[adr + idx] = mj_ray(m, d, origin, direction, NULL, 1,
+                                                  bodyexclude, NULL);
+              }
+            }
+          }
+        }
 
         break;
 
diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c
@@ -743,3 +743,43 @@ int mju_condataSize(int dataspec) {
   }
   return size;
 }
+
+
+// compute camera pixel parameters from model, output are:
+//   pixel units: fx, fy (focal lengths), cx, cy (principal point)
+//   length units: extent
+void mju_camIntrinsics(const mjModel* m, int camid,
+                       mjtNum* fx, mjtNum* fy, mjtNum* cx, mjtNum* cy, mjtNum* extent) {
+  const int width = m->cam_resolution[2*camid];
+  const int height = m->cam_resolution[2*camid+1];
+  const float* sensorsize = m->cam_sensorsize + 2*camid;
+  const float* intrinsic = m->cam_intrinsic + 4*camid;
+  const mjtProjection projection = (mjtProjection)m->cam_projection[camid];
+
+  switch (projection) {
+  case mjPROJ_PERSPECTIVE:
+    if (sensorsize[0] && sensorsize[1]) {
+      // intrinsic-based perspective camera
+      *fx = intrinsic[0] / sensorsize[0] * width;
+      *fy = intrinsic[1] / sensorsize[1] * height;
+      *cx = intrinsic[2] / sensorsize[0] * width;
+      *cy = intrinsic[3] / sensorsize[1] * height;
+    } else {
+      // fovy-based perspective camera
+      *fx = *fy = 0.5 / mju_tan(m->cam_fovy[camid] * mjPI / 360.0) * height;
+      *cx = (mjtNum)width / 2.0;
+      *cy = (mjtNum)height / 2.0;
+    }
+    break;
+  case mjPROJ_ORTHOGRAPHIC:
+    // orthographic: normalize pixel offset to [-1, 1]
+    *fx = (mjtNum)width / 2.0;
+    *fy = (mjtNum)height / 2.0;
+    *cx = *fx;
+    *cy = *fy;
+    break;
+  }
+
+  // extent only used for orthographic cameras
+  *extent = m->cam_fovy[camid];
+}
diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h
@@ -120,6 +120,12 @@ MJAPI const char* mj_versionString(void);
 // return total size of data fields in a contact sensor bitfield specification
 MJAPI int mju_condataSize(int dataSpec);
 
+// compute camera pixel parameters from model
+// outputs: fx, fy (focal length in pixels), cx, cy (principal point), ortho_extent
+void mju_camIntrinsics(const mjModel* m, int camid,
+                       mjtNum* fx, mjtNum* fy, mjtNum* cx, mjtNum* cy,
+                       mjtNum* ortho_extent);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c
@@ -417,6 +417,8 @@ mjtNum mju_wrap(mjtNum wpnt[6], const mjtNum x0[3], const mjtNum x1[3],
 }
 
 
+//------------------------------ misc geometry -----------------------------------------------------
+
 // all 3 semi-axes of a geom
 void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom type) {
   switch (type) {
@@ -494,7 +496,42 @@ int mju_insideGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3
 }
 
 
-// ----------------------------- Flex interpolation ------------------------------------------------
+// compute ray origin and direction for pixel (col, row) in camera image
+// for perspective: origin is unchanged, direction is computed
+// for orthographic: direction is -Z in camera frame, origin is offset from camera center
+void mju_camPixelRay(mjtNum origin[3], mjtNum direction[3],
+                     const mjtNum cam_xpos[3], const mjtNum cam_xmat[9],
+                     int col, int row, mjtNum fx, mjtNum fy, mjtNum cx, mjtNum cy,
+                     int projection, mjtNum ortho_extent) {
+  // pixel center (row 0 = top of image)
+  mjtNum px = col + 0.5 - cx;
+  mjtNum py = row + 0.5 - cy;
+
+  if (projection == mjPROJ_PERSPECTIVE) {
+    // origin is camera position
+    mju_copy3(origin, cam_xpos);
+
+    // direction in camera frame: (x/fx, -y/fy, -1), then normalized
+    mjtNum dir_cam[3] = {px / fx, -py / fy, -1.0};
+    mju_mulMatVec3(direction, cam_xmat, dir_cam);
+    mju_normalize3(direction);
+  } else {
+    // orthographic: parallel rays, direction is -Z in camera frame
+    direction[0] = -cam_xmat[2];
+    direction[1] = -cam_xmat[5];
+    direction[2] = -cam_xmat[8];
+
+    // origin offset in camera frame (ortho_extent is full height, use half for each side)
+    mjtNum half_extent = ortho_extent / 2;
+    mjtNum offset_cam[3] = {px / fx * half_extent, -py / fy * half_extent, 0};
+    mjtNum offset_world[3];
+    mju_mulMatVec3(offset_world, cam_xmat, offset_cam);
+    mju_add3(origin, cam_xpos, offset_world);
+  }
+}
+
+
+// ----------------------------- flex interpolation ------------------------------------------------
 
 mjtNum static inline phi(mjtNum s, int i, int order) {
   if (order == 1) {
diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h
@@ -57,6 +57,13 @@ MJAPI void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom ty
 int mju_insideGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], mjtGeom type,
                    const mjtNum point[3]);
 
+// compute ray origin and direction for pixel (col, row) in camera image
+// directions are normalized so ray functions return actual 3D distance
+void mju_camPixelRay(mjtNum origin[3], mjtNum direction[3],
+                     const mjtNum cam_xpos[3], const mjtNum cam_xmat[9],
+                     int col, int row, mjtNum fx, mjtNum fy, mjtNum cx, mjtNum cy,
+                     int projection, mjtNum ortho_extent);
+
 // ----------------------------- Flex interpolation ------------------------------------------------
 
 // evaluate the deformation gradient at p using the nodal dof values
diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c
@@ -2478,22 +2478,64 @@ static void addRangefinderGeoms(const mjModel* m, mjData* d, const mjvOption* vo
 
   for (int i=0; i < m->nsensor; i++) {
     if (m->sensor_type[i] == mjSENS_RANGEFINDER) {
-      // sensor data
-      mjtNum dst = d->sensordata[m->sensor_adr[i]];
-      int sid = m->sensor_objid[i];
+      int objid = m->sensor_objid[i];
+      int adr = m->sensor_adr[i];
 
-      // null output: nothing to render
-      if (dst < 0) {
-        continue;
-      }
+      // site-attached rangefinder
+      if (m->sensor_objtype[i] == mjOBJ_SITE) {
+        mjtNum dst = d->sensordata[adr];
 
-      // make ray
-      mjtNum* from = d->site_xpos+3*sid;
-      mjtNum to[3] = {from[0] + d->site_xmat[9*sid+2]*dst,
-                      from[1] + d->site_xmat[9*sid+5]*dst,
-                      from[2] + d->site_xmat[9*sid+8]*dst};
-      addConnector(scn, mjGEOM_LINE, 3, from, to, m->vis.rgba.rangefinder,
-                   i, mjCAT_DECOR, mjOBJ_SENSOR);
+        // null output: nothing to render
+        if (dst < 0) {
+          continue;
+        }
+
+        // make ray
+        mjtNum* from = d->site_xpos+3*objid;
+        mjtNum to[3] = {from[0] + d->site_xmat[9*objid+2]*dst,
+                        from[1] + d->site_xmat[9*objid+5]*dst,
+                        from[2] + d->site_xmat[9*objid+8]*dst};
+        addConnector(scn, mjGEOM_LINE, 3, from, to, m->vis.rgba.rangefinder,
+                     i, mjCAT_DECOR, mjOBJ_SENSOR);
+      }
+
+      // camera-attached rangefinder
+      else if (m->sensor_objtype[i] == mjOBJ_CAMERA) {
+        const int width = m->cam_resolution[2*objid];
+        const int height = m->cam_resolution[2*objid+1];
+        const mjtNum* cam_xpos = d->cam_xpos + 3*objid;
+        const mjtNum* cam_xmat = d->cam_xmat + 9*objid;
+        const int projection = m->cam_projection[objid];
+
+        // compute focal length in pixels using helper
+        mjtNum fx, fy, cx, cy, ortho_extent;
+        mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent);
+
+        // draw ray for each pixel
+        for (int row = 0; row < height; row++) {
+          for (int col = 0; col < width; col++) {
+            int idx = row*width + col;
+            mjtNum dst = d->sensordata[adr + idx];
+
+            // null output: nothing to render
+            if (dst < 0) {
+              continue;
+            }
+
+            // compute ray origin and direction
+            mjtNum origin[3], direction[3];
+            mju_camPixelRay(origin, direction, cam_xpos, cam_xmat,
+                            col, row, fx, fy, cx, cy, projection, ortho_extent);
+
+            // compute endpoint
+            mjtNum to[3];
+            mju_addScl3(to, origin, direction, dst);
+
+            addConnector(scn, mjGEOM_LINE, 3, origin, to, m->vis.rgba.rangefinder,
+                         i, mjCAT_DECOR, mjOBJ_SENSOR);
+          }
+        }
+      }
     } else if (m->sensor_type[i] == mjSENS_GEOMFROMTO) {
       // sensor data
       mjtNum* fromto = d->sensordata + m->sensor_adr[i];
diff --git a/src/user/user_api.cc b/src/user/user_api.cc
@@ -1252,7 +1252,6 @@ void mjs_deleteUserValue(mjsElement* element, const char* key) {
 int mjs_sensorDim(const mjsSensor* sensor) {
   switch (sensor->type) {
   case mjSENS_TOUCH:
-  case mjSENS_RANGEFINDER:
   case mjSENS_JOINTPOS:
   case mjSENS_JOINTVEL:
   case mjSENS_TENDONPOS:
@@ -1275,6 +1274,15 @@ int mjs_sensorDim(const mjsSensor* sensor) {
   case mjSENS_CLOCK:
     return 1;
 
+  case mjSENS_RANGEFINDER:
+    if (sensor->objtype == mjOBJ_CAMERA) {
+      const mjCCamera* camera = static_cast<const mjCCamera*>(
+          static_cast<mjCSensor*>(sensor->element)->get_obj());
+      return static_cast<int>(camera->spec.resolution[0]) *
+             static_cast<int>(camera->spec.resolution[1]);
+    }
+    return 1;  // site-attached: single ray
+
   case mjSENS_CAMPROJECTION:
     return 2;
 
diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc
diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc
diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc
diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc
diff --git a/test/engine/testdata/sensor/rfcamera.xml b/test/engine/testdata/sensor/rfcamera.xml
