Extend rangefinder sensor to support cameras.

PiperOrigin-RevId: 848255889
Change-Id: I6e1a9ed13d29d2242558625a47e24e47ad6e87db

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
+This element creates a rangefinder.
-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
+- If associated with a :ref:`site<sensor-rangefinder-site>`, it measures the distance to the nearest geom surface, along
-body as the sensor site are excluded. Invisible geoms, defined as geoms whose rgba (or whose material rgba) has alpha=0,
+  the ray defined by the positive Z-axis of the site.
-are also excluded. Note however that geoms made invisible in the visualizer by disabling their geom group are not
+- If associated with a :ref:`camera<sensor-rangefinder-camera>`, it outputs one distance measurement for each pixel in
-excluded; this is because sensor calculations are independent of the visualizer.
+  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** (*)

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

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

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

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

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];
+        if (objtype == mjOBJ_SITE) {
-        rvec[1] = d->site_xmat[9*objid+5];
+          rvec[0] = d->site_xmat[9*objid+2];
-        rvec[2] = d->site_xmat[9*objid+8];
+          rvec[1] = d->site_xmat[9*objid+5];
-        d->sensordata[adr] = mj_ray(m, d, d->site_xpos+3*objid, rvec, NULL, 1,
+          rvec[2] = d->site_xmat[9*objid+8];
-                                    m->site_bodyid[objid], NULL);
+          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;
 

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];
+}

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

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) {

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

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
+      int objid = m->sensor_objid[i];
-      mjtNum dst = d->sensordata[m->sensor_adr[i]];
+      int adr = m->sensor_adr[i];
-      int sid = m->sensor_objid[i];
 
-      // null output: nothing to render
+      // site-attached rangefinder
-      if (dst < 0) {
+      if (m->sensor_objtype[i] == mjOBJ_SITE) {
-        continue;
+        mjtNum dst = d->sensordata[adr];
+
+        // 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);
       }
 
-      // make ray
+      // camera-attached rangefinder
-      mjtNum* from = d->site_xpos+3*sid;
+      else if (m->sensor_objtype[i] == mjOBJ_CAMERA) {
-      mjtNum to[3] = {from[0] + d->site_xmat[9*sid+2]*dst,
+        const int width = m->cam_resolution[2*objid];
-                      from[1] + d->site_xmat[9*sid+5]*dst,
+        const int height = m->cam_resolution[2*objid+1];
-                      from[2] + d->site_xmat[9*sid+8]*dst};
+        const mjtNum* cam_xpos = d->cam_xpos + 3*objid;
-      addConnector(scn, mjGEOM_LINE, 3, from, to, m->vis.rgba.rangefinder,
+        const mjtNum* cam_xmat = d->cam_xmat + 9*objid;
-                   i, mjCAT_DECOR, mjOBJ_SENSOR);
+        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];

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;
 

src/user/user_objects.cc

@@ -7313,7 +7313,6 @@ void mjCSensor::Compile(void) {
     case mjSENS_FORCE:
     case mjSENS_TORQUE:
     case mjSENS_MAGNETOMETER:
-    case mjSENS_RANGEFINDER:
     case mjSENS_CAMPROJECTION:
       // must be attached to site
       if (objtype != mjOBJ_SITE) {
@@ -7329,6 +7328,13 @@ void mjCSensor::Compile(void) {
       }
       break;
 
+    case mjSENS_RANGEFINDER:
+      // must be attached to site or camera
+      if (objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) {
+        throw mjCError(this, "sensor must be attached to site or camera");
+      }
+      break;
+
     case mjSENS_JOINTPOS:
     case mjSENS_JOINTVEL:
     case mjSENS_JOINTACTFRC:

src/xml/xml_native_reader.cc

@@ -454,7 +454,7 @@ std::vector<const char*> MJCF[nMJCF] = {
         {"torque", "*", "name", "site", "cutoff", "noise", "user"},
         {"magnetometer", "*", "name", "site", "cutoff", "noise", "user"},
         {"camprojection", "*", "name", "site", "camera", "cutoff", "noise", "user"},
-        {"rangefinder", "*", "name", "site", "cutoff", "noise", "user"},
+        {"rangefinder", "*", "name", "site", "camera", "cutoff", "noise", "user"},
         {"jointpos", "*", "name", "joint", "cutoff", "noise", "user"},
         {"jointvel", "*", "name", "joint", "cutoff", "noise", "user"},
         {"tendonpos", "*", "name", "tendon", "cutoff", "noise", "user"},
@@ -4052,8 +4052,12 @@ void mjXReader::Sensor(XMLElement* section) {
       sensor->reftype = mjOBJ_CAMERA;
     } else if (type == "rangefinder") {
       sensor->type = mjSENS_RANGEFINDER;
-      sensor->objtype = mjOBJ_SITE;
+      bool use_site = ReadAttrTxt(elem, "site", objname, false);
-      ReadAttrTxt(elem, "site", objname, true);
+      bool use_camera = ReadAttrTxt(elem, "camera", objname, false);
+      if (use_site == use_camera) {
+        throw mjXError(elem, "rangefinder requires exactly one of 'site' or 'camera'");
+      }
+      sensor->objtype = use_site ? mjOBJ_SITE : mjOBJ_CAMERA;
     }
 
     // sensors related to scalar joints, tendons, actuators

src/xml/xml_native_writer.cc

@@ -2029,7 +2029,11 @@ void mjXWriter::Sensor(XMLElement* root) {
         break;
       case mjSENS_RANGEFINDER:
         elem = InsertEnd(section, "rangefinder");
-        WriteAttrTxt(elem, "site", sensor->get_objname());
+        if (sensor->objtype == mjOBJ_SITE) {
+          WriteAttrTxt(elem, "site", sensor->get_objname());
+        } else {
+          WriteAttrTxt(elem, "camera", sensor->get_objname());
+        }
         break;
       case mjSENS_CAMPROJECTION:
         elem = InsertEnd(section, "camprojection");

test/engine/engine_sensor_test.cc

@@ -67,7 +67,7 @@ static vector<mjtNum> GetSensor(const mjModel* model,
 
 using SensorTest = MujocoTest;
 
-// --------------------- test sensor disableflag  ------------------------------
+// --------------------- test sensor disable flag ------------------------------
 
 TEST_F(SensorTest, DisableSensors) {
   constexpr char xml[] = R"(
@@ -1052,5 +1052,95 @@ TEST_F(SensorTest, InsideSite) {
   mj_deleteModel(model);
 }
 
+TEST_F(SensorTest, RangefinderCamera) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom type="plane" size="10 10 .1"/>
+      <body pos="0 0 2">
+        <camera name="persp" xyaxes="1 0 0 0 1 0" resolution="3 3" fovy="90"/>
+        <camera name="ortho" euler="0 45 0" resolution="3 3"
+          projection="orthographic" fovy="2"/>
+      </body>
+    </worldbody>
+
+    <sensor>
+      <rangefinder camera="persp"/>
+      <rangefinder camera="ortho"/>
+    </sensor>
+  </mujoco>
+  )";
+  char error[1024];
+  mjModel* model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model, NotNull()) << error;
+
+  // sensordata dimension should be 3x3 + 3x3 = 18
+  EXPECT_EQ(model->nsensordata, 18);
+
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  mjtNum tol = 1e-6;
+  mjtNum height = 2.0;
+  mjtNum fy = 1.5;
+  mjtNum offsets[3] = {-1.0, 0.0, 1.0};  // pixel center - principal point
+
+  // perspective camera: rays diverge, distance varies with angle
+  for (int row = 0; row < 3; row++) {
+    for (int col = 0; col < 3; col++) {
+      int idx = row * 3 + col;
+      mjtNum dx = offsets[col] / fy;
+      mjtNum dy = offsets[row] / fy;
+      mjtNum expected = height * mju_sqrt(1 + dx*dx + dy*dy);
+      EXPECT_NEAR(data->sensordata[idx], expected, tol)
+          << "perspective pixel (" << row << ", " << col << ")";
+    }
+  }
+
+  // orthographic camera: tilted 45 degrees around Y axis
+  // rays are parallel at 45 degrees, distance depends on pixel x-offset
+  // for center pixel at (0,0,2): distance = 2 / cos(45) = 2*sqrt(2)
+  // for off-center pixels: x-offset shifts origin, affecting where ray hits z=0
+  mjtNum extent = 2.0;  // fovy for orthographic
+  mjtNum half_extent = extent / 2;
+  mjtNum fx = 1.5;  // width / 2 for 3x3 image
+  mjtNum cx = 1.5;  // principal point
+  mjtNum cos45 = mju_sqrt(0.5);
+  mjtNum sin45 = mju_sqrt(0.5);
+  for (int row = 0; row < 3; row++) {
+    for (int col = 0; col < 3; col++) {
+      int idx = 9 + row * 3 + col;  // offset by first sensor's 9 values
+
+      // pixel offset in camera frame: matches mju_camPixelRay formula
+      mjtNum px_cam = (col + 0.5 - cx) / fx * half_extent;
+
+      // camera tilted 45° around Y: local +X maps to world (+cos45, 0, -sin45)
+      mjtNum origin_z = height - px_cam * sin45;
+
+      // ray hits z=0 plane: distance = origin_z / cos45
+      mjtNum expected = origin_z / cos45;
+      EXPECT_NEAR(data->sensordata[idx], expected, tol)
+          << "orthographic pixel (" << row << ", " << col << ")";
+    }
+  }
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(SensorTest, RFCamera) {
+  const string xml_path =
+      GetTestDataFilePath("engine/testdata/sensor/rfcamera.xml");
+  char error[1024];
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
+  ASSERT_THAT(model, NotNull()) << error;
+
+  mjData* data = mj_makeData(model);
+  mj_step(model, data);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco

test/engine/testdata/sensor/rfcamera.xml

@@ -0,0 +1,36 @@
+<mujoco model="rangefinder camera">
+  <visual>
+    <rgba frustum="1 1 0 0.1"/>
+  </visual>
+
+  <worldbody>
+    <light pos="0 0 3"/>
+
+    <!-- ground plane -->
+    <geom type="plane" size="5 5 .1" rgba=".3 .4 .5 1"/>
+
+    <!-- scattered objects for rays to hit -->
+    <geom type="sphere" pos="0 0 .4" size=".4" rgba="1 .3 .3 1"/>
+    <geom type="box" pos="1 0.5 .35" size=".35 .35 .35" rgba=".3 1 .3 1"/>
+    <geom type="cylinder" pos="-0.8 0.6 .4" size=".4 .4" rgba=".3 .3 1 1"/>
+    <geom type="capsule" pos="0.5 -0.7 0" size=".25 .35" zaxis="1 0.5 0" rgba="1 1 .3 1"/>
+    <geom type="ellipsoid" pos="-0.5 -0.5 .2" size=".4 .3 .25" rgba="1 .3 1 1"/>
+
+    <!-- mocap body with perspective camera -->
+    <body pos="1 0 2" euler="0 20 0" mocap="true">
+      <geom type="box" pos="0 0 .3" size=".2 .2 .2"/>
+      <camera name="perspective" resolution="4 4" fovy="60"/>
+    </body>
+
+    <!-- mocap body with orthographic camera -->
+    <body pos="-1 0 2" euler="0 -20 0" mocap="true">
+      <geom type="box" pos="0 0 .3" size=".2 .2 .2"/>
+      <camera name="orthographic" resolution="4 4" fovy="1.5" projection="orthographic"/>
+    </body>
+  </worldbody>
+
+  <sensor>
+    <rangefinder camera="perspective"/>
+    <rangefinder camera="orthographic"/>
+  </sensor>
+</mujoco>