Add additional data fields that can be reported by rangefinder sensors.

PiperOrigin-RevId: 848316991
Change-Id: Idbf7ba81b4da711a22c23302c8782ab2b0b98d82

doc/XMLreference.rst

@@ -6382,12 +6382,46 @@ This element creates a rangefinder.
   measurements in this case is equal to product of the camera's width and height
   :ref:`resolutions<body-camera-resolution>`.
 
+.. image:: images/XMLreference/rfcamera.png
+   :width: 45%
+   :align: right
+   :target: https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/sensor/rfcamera.xml
+
 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.
 
+The image on the right (click to see the model being visualized) shows two rangefinder sensors attached to a perspective and
+an orthographic camera, with frustums visualized. Both cameras have 4x4 resolution, for 16 rays each. The rangefinder
+sensors report :at:`data` = :at-val:`"dist point normal"` (see below), so we can see the rays (lines), the intersection
+points (spheres) and the surface normals (arrows).
+
+.. _sensor-rangefinder-data:
+
+:at:`data`: :at-val:`[dist, dir, origin, point, normal, depth], "dist"`
+   By default, the rangefinder outputs a distance measurement, as described above. However, it is also possible to
+   specify a set of output data fields. The :at:`data` attribute can contain **multiple sequential data types**, as long
+   as the relative order---as listed above---is maintained. For example, :at:`data` = :at-val:`"dist point normal"` will
+   return 7 numbers per ray, while :at:`data` = :at-val:`"point origin"` is an error because :at-val:`origin` must come
+   before :at-val:`point`.
+
+   - :at-val:`dist` **real(1)**: The distance from the ray origin to the nearest geom surface, -1 if no surface was hit.
+     If this data type is included, rays will be visualized as lines.
+   - :at-val:`dir` **real(3)**: Normalized direction of the ray, or (0, 0, 0) if no surface was hit.
+   - :at-val:`origin` **real(3)**: The point from which the ray emanates (global frame). For sites and perspective
+     cameras, this is the site/camera xpos. However for orthographic cameras, ray origins are spatially distributed
+     along the image plane.
+   - :at-val:`point` **real(3)**: The point where the ray intersects the nearest geom surface in the global frame, or
+     (0, 0, 0) if no surface was hit. If this data type is included, intersection points will be visualized as spheres.
+   - :at-val:`normal`: **real(3)**: The geom surface normal at the point where the ray intersects it, in the global
+     frame, or (0, 0, 0) if no surface was hit. Note that normals always point towards the outside of the geom surface,
+     regardless of the ray origin. If this data type is included along with either :at-val:`dist` or :at-val:`point`,
+     normals will be visualized as arrows at the intersection points.
+   - :at-val:`depth`: **real(1)**: The distance of the hit point from the camera plane, -1 if no surface was hit. Note
+     that this depth sematic corresponds to depth images in the computer graphics sense.
+
 .. _sensor-rangefinder-name:
 
 .. _sensor-rangefinder-noise:

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:`camera<sensor-rangefinder-camera>`                        | :ref:`cutoff<sensor-rangefinder-cutoff>`                        | |
+|                                    |    |    | :ref:`name<sensor-rangefinder-name>`                            | :ref:`site<sensor-rangefinder-site>`                            | :ref:`camera<sensor-rangefinder-camera>`                        | :ref:`data<sensor-rangefinder-data>`                            | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`noise<sensor-rangefinder-noise>`                          | :ref:`user<sensor-rangefinder-user>`                            |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`cutoff<sensor-rangefinder-cutoff>`                        | :ref:`noise<sensor-rangefinder-noise>`                          | :ref:`user<sensor-rangefinder-user>`                            |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| sensor |br| |_| |L|            |    | .. table::                                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -18,16 +18,24 @@ Upcoming version (not yet released)
 
 General
 ^^^^^^^
+
+.. image:: images/XMLreference/rfcamera.png
+   :width: 45%
+   :align: right
+   :target: https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/sensor/rfcamera.xml
+
 - 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.
+  attribute. In this case, the sensor will cast multiple ray, one for each pixel.
+- Rangefinder sensors can now now report various kinds of information besides ray distances, including surface normals.
+  See :ref:`rangefinder<sensor-rangefinder>` for details.
 - 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
+  - Added a :ref:`depth<mjtRndFlag>` rendering flag.
 
 MJX
 ^^^

doc/includes/references.h

@@ -774,8 +774,18 @@ typedef enum mjtConDataField_ {   // data fields returned by contact sensors
   mjCONDATA_NORMAL,               // contact frame normal
   mjCONDATA_TANGENT,              // contact frame first tangent
 
-  mjNCONDATA          = 7         // number of contact sensor data fields
+  mjNCONDATA                      // number of contact sensor data fields
 } mjtConDataField;
+typedef enum mjtRayDataField_ {   // data fields returned by rangefinder sensors
+  mjRAYDATA_DIST     = 0,         // distance from ray origin to nearest surface
+  mjRAYDATA_DIR,                  // normalized ray direction
+  mjRAYDATA_ORIGIN,               // ray origin
+  mjRAYDATA_POINT,                // point at which ray intersects nearest surface
+  mjRAYDATA_NORMAL,               // surface normal at intersection point
+  mjRAYDATA_DEPTH,                // depth along z-axis
+
+  mjNRAYDATA                      // number of rangefinder sensor data fields
+} mjtRayDataField;
 typedef enum mjtSameFrame_ {      // frame alignment of bodies with their children
   mjSAMEFRAME_NONE    = 0,        // no alignment
   mjSAMEFRAME_BODY,               // frame is same as body frame

include/mujoco/mjmodel.h

@@ -400,10 +400,22 @@ typedef enum mjtConDataField_ {   // data fields returned by contact sensors
   mjCONDATA_NORMAL,               // contact frame normal
   mjCONDATA_TANGENT,              // contact frame first tangent
 
-  mjNCONDATA          = 7         // number of contact sensor data fields
+  mjNCONDATA                      // number of contact sensor data fields
 } mjtConDataField;
 
 
+typedef enum mjtRayDataField_ {   // data fields returned by rangefinder sensors
+  mjRAYDATA_DIST     = 0,         // distance from ray origin to nearest surface
+  mjRAYDATA_DIR,                  // normalized ray direction
+  mjRAYDATA_ORIGIN,               // ray origin
+  mjRAYDATA_POINT,                // point at which ray intersects nearest surface
+  mjRAYDATA_NORMAL,               // surface normal at intersection point
+  mjRAYDATA_DEPTH,                // depth along z-axis
+
+  mjNRAYDATA                      // number of rangefinder sensor data fields
+} mjtRayDataField;
+
+
 typedef enum mjtSameFrame_ {      // frame alignment of bodies with their children
   mjSAMEFRAME_NONE    = 0,        // no alignment
   mjSAMEFRAME_BODY,               // frame is same as body frame

python/mujoco/introspect/enums.py

@@ -416,6 +416,20 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjNCONDATA', 7),
          ]),
      )),
+    ('mjtRayDataField',
+     EnumDecl(
+         name='mjtRayDataField',
+         declname='enum mjtRayDataField_',
+         values=dict([
+             ('mjRAYDATA_DIST', 0),
+             ('mjRAYDATA_DIR', 1),
+             ('mjRAYDATA_ORIGIN', 2),
+             ('mjRAYDATA_POINT', 3),
+             ('mjRAYDATA_NORMAL', 4),
+             ('mjRAYDATA_DEPTH', 5),
+             ('mjNRAYDATA', 6),
+         ]),
+     )),
     ('mjtSameFrame',
      EnumDecl(
          name='mjtSameFrame',

python/mujoco/specs_test.py

@@ -1662,5 +1662,126 @@ class SpecsTest(absltest.TestCase):
         string_spec.compile()
         self.assertEqual(spec.to_xml(), string_spec.to_xml())
 
+  def test_rangefinder_sensor(self):
+    """Test rangefinder sensor with mjSpec, iterative model building."""
+    # Raydata field enum values for dataspec bitfield
+    rd = mujoco.mjtRayDataField
+    dist_val = int(rd.mjRAYDATA_DIST)
+    dir_val = int(rd.mjRAYDATA_DIR)
+    origin_val = int(rd.mjRAYDATA_ORIGIN)
+    point_val = int(rd.mjRAYDATA_POINT)
+    normal_val = int(rd.mjRAYDATA_NORMAL)
+    depth_val = int(rd.mjRAYDATA_DEPTH)
+
+    # Step 1: Create a rangefinder sensor attached to a site, no dataspec set.
+    # Note: site goes on a child body because rangefinder excludes the site's
+    # parent body from ray casting.
+    spec = mujoco.MjSpec()
+    sensor_body = spec.worldbody.add_body(name='sensor_body', pos=[0, 0, 1])
+    sensor_body.add_site(name='rf_site', zaxis=[0, 0, -1])
+    rf_sensor = spec.add_sensor(
+        name='rf',
+        type=mujoco.mjtSensor.mjSENS_RANGEFINDER,
+        objtype=mujoco.mjtObj.mjOBJ_SITE,
+        objname='rf_site',
+    )
+
+    # This should fail: data spec (intprm[0]) must be positive
+    with self.assertRaisesWithPredicateMatch(
+        ValueError,
+        lambda e: 'data spec (intprm[0]) must be positive' in str(e)
+    ):
+      spec.compile()
+
+    # Step 2: Set dataspec to just mjRAYDATA_DIST
+    rf_sensor.intprm[0] = 1 << dist_val
+    model = spec.compile()
+    data = mujoco.MjData(model)
+    mujoco.mj_forward(model, data)
+
+    # With no geometry, the ray should miss: dist = -1
+    self.assertEqual(model.nsensordata, 1)
+    self.assertEqual(data.bind(rf_sensor).data[0], -1)
+
+    # Step 3: Add all raydata fields and check no-hit values
+    all_fields = (
+        (1 << dist_val) | (1 << dir_val) | (1 << origin_val) |
+        (1 << point_val) | (1 << normal_val) | (1 << depth_val)
+    )
+    rf_sensor.intprm[0] = all_fields
+    model = spec.compile()
+    data = mujoco.MjData(model)
+    mujoco.mj_forward(model, data)
+
+    # Expected size: dist(1) + dir(3) + origin(3) + point(3) + normal(3) +
+    # depth(1) = 14
+    self.assertEqual(model.nsensordata, 14)
+
+    # No-hit values
+    sd = data.bind(rf_sensor).data
+    self.assertEqual(sd[0], -1)  # dist
+    np.testing.assert_allclose(sd[1:4], [0, 0, 0])  # dir
+    np.testing.assert_allclose(sd[4:7], [0, 0, 1])  # origin
+    np.testing.assert_allclose(sd[7:10], [0, 0, 0])  # point
+    np.testing.assert_allclose(sd[10:13], [0, 0, 0])  # normal
+    self.assertEqual(sd[13], -1)  # depth
+
+    # Step 4: Add a floor plane, now the ray should hit
+    spec.worldbody.add_geom(
+        name='floor',
+        type=mujoco.mjtGeom.mjGEOM_PLANE,
+        size=[10, 10, 0.1],
+    )
+    model = spec.compile()
+    data = mujoco.MjData(model)
+    mujoco.mj_forward(model, data)
+
+    # Ray starts at z=1 pointing down, hits floor at z=0
+    # For site sensor, depth = dist
+    sd = data.bind(rf_sensor).data
+    self.assertAlmostEqual(sd[0], 1.0, places=6)  # dist
+    np.testing.assert_allclose(sd[1:4], [0, 0, -1], atol=1e-10)  # dir
+    np.testing.assert_allclose(sd[4:7], [0, 0, 1], atol=1e-10)  # origin
+    np.testing.assert_allclose(sd[7:10], [0, 0, 0], atol=1e-10)  # point
+    np.testing.assert_allclose(sd[10:13], [0, 0, 1], atol=1e-10)  # normal
+    self.assertAlmostEqual(sd[13], 1.0, places=6)  # depth
+
+    # Step 5: Add a camera-based rangefinder sensor
+    # Camera also on child body so it doesn't exclude the floor
+    cam_body = spec.worldbody.add_body(name='cam_body', pos=[0, 0, 2])
+    cam_body.add_camera(
+        name='rf_cam',
+        xyaxes=[1, 0, 0, 0, 1, 0],  # z=[0,0,1], looks along -z (down)
+        resolution=[3, 3],
+        fovy=90,
+    )
+    cam_sensor = spec.add_sensor(
+        name='rf_cam_sensor',
+        type=mujoco.mjtSensor.mjSENS_RANGEFINDER,
+        objtype=mujoco.mjtObj.mjOBJ_CAMERA,
+        objname='rf_cam',
+        intprm=[(1 << dist_val) | (1 << depth_val), 0, 0],
+    )
+
+    model = spec.compile()
+    data = mujoco.MjData(model)
+    mujoco.mj_forward(model, data)
+
+    # Site sensor: 14 values, Camera sensor: (1+1)*9 = 18 values
+    self.assertEqual(model.nsensordata, 14 + 18)
+
+    # Check camera sensor data using bind
+    cam_sd = data.bind(cam_sensor).data
+    stride = 2  # dist + depth per pixel
+    center_pixel = 4  # center of 3x3 = row 1, col 1
+
+    # Center pixel: ray straight down from z=2 to z=0
+    self.assertAlmostEqual(cam_sd[center_pixel * stride], 2.0, places=6)
+    self.assertAlmostEqual(cam_sd[center_pixel * stride + 1], 2.0, places=6)
+
+    # Corner pixel: off-axis ray, dist > depth
+    self.assertGreater(cam_sd[0], cam_sd[1])  # dist > depth
+    self.assertAlmostEqual(cam_sd[1], 2.0, places=6)  # depth is still 2.0
+
 if __name__ == '__main__':
   absltest.main()

src/engine/engine_ray.c

@@ -662,11 +662,11 @@ static mjtNum mj_rayHfieldNormal(const mjModel* m, const mjData* d, int geomid,
       // triangle normal
       mjtNum normal_tri[3];
 
-      // first triangle
+      // first triangle: swap v1 and v2 for consistent CCW winding (normals point up)
       mjtNum va[3][3] = {
         {dx*c-size[0], dy*r-size[1], data[r*ncol+c]*size[2]},
-        {dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]},
+        {dx*(c+1)-size[0], dy*(r+0)-size[1], data[(r+0)*ncol+(c+1)]*size[2]},
-        {dx*(c+1)-size[0], dy*(r+0)-size[1], data[(r+0)*ncol+(c+1)]*size[2]}
+        {dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]}
       };
       mjtNum sol = ray_triangle(va, lpnt, lvec, b0, b1, normal ? normal_tri : NULL);
       if (sol >= 0 && (x < 0 || sol < x)) {
@@ -1458,6 +1458,18 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3],
         AABB[3] = mju_max(AABB[3], elevation);
       }
 
+      // add distance-dependent angular margin to account for edge/face curvature
+      // margin = atan(max_half_size / dist) bounds the angular deviation of face centers
+      mjtNum max_half = mju_max(aabb[3], mju_max(aabb[4], aabb[5]));
+      mjtNum dist = mju_dist3(pnt, xpos);
+      if (dist > mjMINVAL) {
+        mjtNum margin = mju_atan2(max_half, dist);
+        AABB[0] -= margin;
+        AABB[1] -= margin;
+        AABB[2] += margin;
+        AABB[3] += margin;
+      }
+
       // azimuth crosses discontinuity, fall back to no angular culling
       if (AABB[2]-AABB[0] > mjPI) {
         AABB[0] = -mjPI;
@@ -1520,8 +1532,23 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
 
       // exclude geom using bounding angles
       if (m->body_bvhadr[b] != -1) {
-        if (azimuth < (geom_ba+4*i)[0] || elevation < (geom_ba+4*i)[1] ||
+        mjtNum az_min = (geom_ba+4*i)[0];
-            azimuth > (geom_ba+4*i)[2] || elevation > (geom_ba+4*i)[3]) {
+        mjtNum az_max = (geom_ba+4*i)[2];
+        mjtNum el_min = (geom_ba+4*i)[1];
+        mjtNum el_max = (geom_ba+4*i)[3];
+
+        // check elevation
+        if (elevation < el_min || elevation > el_max) {
+          continue;
+        }
+
+        // check azimuth with wraparound
+        mjtNum az_center = (az_min + az_max) * 0.5;
+        mjtNum az_half_width = (az_max - az_min) * 0.5;
+        mjtNum az_diff = azimuth - az_center;
+        if (az_diff > mjPI) az_diff -= 2*mjPI;
+        else if (az_diff < -mjPI) az_diff += 2*mjPI;
+        if (mju_abs(az_diff) > az_half_width) {
           continue;
         }
       }

src/engine/engine_sensor.c

@@ -385,6 +385,61 @@ static void total_wrench(mjtNum force[3], mjtNum torque[3], const mjtNum point[3
 
 //-------------------------------- sensor ----------------------------------------------------------
 
+// fill one pixel's worth of rangefinder data, advance ptr
+static mjtNum* fill_raydata(mjtNum* ptr, int dataspec, mjtNum dist,
+                            const mjtNum origin[3], const mjtNum direction[3],
+                            const mjtNum normal[3], const mjtNum cam_xpos[3],
+                            const mjtNum cam_z[3]) {
+  int hit = (dist >= 0);
+
+  if (dataspec & (1 << mjRAYDATA_DIST)) {
+    *ptr++ = dist;
+  }
+  if (dataspec & (1 << mjRAYDATA_DIR)) {
+    if (hit) mju_copy3(ptr, direction);
+    else     mju_zero3(ptr);
+    ptr += 3;
+  }
+  if (dataspec & (1 << mjRAYDATA_ORIGIN)) {
+    mju_copy3(ptr, origin);
+    ptr += 3;
+  }
+
+  // compute point if needed for POINT or DEPTH fields
+  mjtNum point[3] = {0, 0, 0};
+  if ((dataspec & (1 << mjRAYDATA_POINT)) || (dataspec & (1 << mjRAYDATA_DEPTH))) {
+    if (hit) mju_addScl3(point, origin, direction, dist);
+  }
+
+  if (dataspec & (1 << mjRAYDATA_POINT)) {
+    mju_copy3(ptr, point);
+    ptr += 3;
+  }
+  if (dataspec & (1 << mjRAYDATA_NORMAL)) {
+    if (hit) mju_copy3(ptr, normal);
+    else     mju_zero3(ptr);
+    ptr += 3;
+  }
+  if (dataspec & (1 << mjRAYDATA_DEPTH)) {
+    if (hit) {
+      if (cam_z) {
+        // camera depth: project onto camera z-axis
+        mjtNum delta[3];
+        mju_sub3(delta, point, cam_xpos);
+        *ptr++ = -mju_dot3(delta, cam_z);
+      } else {
+        // site sensor: depth = dist
+        *ptr++ = dist;
+      }
+    } else {
+      *ptr++ = -1;
+    }
+  }
+
+  return ptr;
+}
+
+
 // position-dependent sensors
 void mj_sensorPos(const mjModel* m, mjData* d) {
   int ne = d->ne, nf = d->nf, nefc = d->nefc, nsensor = m->nsensor;
@@ -435,58 +490,98 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
         break;
 
       case mjSENS_RANGEFINDER:                            // rangefinder
-        if (objtype == mjOBJ_SITE) {
+        {
-          rvec[0] = d->site_xmat[9*objid+2];
+          // get dataspec
-          rvec[1] = d->site_xmat[9*objid+5];
+          int dataspec = m->sensor_intprm[i*mjNSENS];
-          rvec[2] = d->site_xmat[9*objid+8];
+          mjtNum* ptr = d->sensordata + adr;
-          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
+          if (objtype == mjOBJ_SITE) {
-          mjtNum fx, fy, cx, cy, ortho_extent;
+            // site-attached rangefinder: single ray
-          mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent);
+            rvec[0] = d->site_xmat[9*objid+2];
+            rvec[1] = d->site_xmat[9*objid+5];
+            rvec[2] = d->site_xmat[9*objid+8];
+            const mjtNum* origin = d->site_xpos + 3*objid;
 
-          if (projection == mjPROJ_PERSPECTIVE) {
+            int geomid;
-            // perspective: all rays share origin, different directions
+            mjtNum normal[3];
-            const int npixel = width * height;
+            mjtNum* p_normal = (dataspec & (1 << mjRAYDATA_NORMAL)) ? normal : NULL;
-            mj_markStack(d);
+            mjtNum dist = mj_rayNormal(m, d, origin, rvec, NULL, 1,
-            mjtNum* vec = mjSTACKALLOC(d, 3*npixel, mjtNum);
+                                       m->site_bodyid[objid], &geomid, p_normal);
-            int* geomid = mjSTACKALLOC(d, npixel, int);
 
-            // compute ray directions using helper (normalized)
+            // for site sensor: pass NULL for cam_z so depth = dist
-            for (int row = 0; row < height; row++) {
+            fill_raydata(ptr, dataspec, dist, origin, rvec, normal, NULL, NULL);
-              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
+            // camera-attached rangefinder: depth image
-            for (int row = 0; row < height; row++) {
+            const int width = m->cam_resolution[2*objid];
-              for (int col = 0; col < width; col++) {
+            const int height = m->cam_resolution[2*objid+1];
-                int idx = row*width + col;
+            const int bodyexclude = m->cam_bodyid[objid];
-                mjtNum origin[3], direction[3];
+            const mjtNum* cam_xpos = d->cam_xpos + 3*objid;
-                mju_camPixelRay(origin, direction, cam_xpos, cam_xmat,
+            const mjtNum* cam_xmat = d->cam_xmat + 9*objid;
-                                col, row, fx, fy, cx, cy, projection, ortho_extent);
+            const int projection = m->cam_projection[objid];
 
-                d->sensordata[adr + idx] = mj_ray(m, d, origin, direction, NULL, 1,
+            // camera z-axis (pointing into scene, negative of optical axis)
-                                                  bodyexclude, NULL);
+            mjtNum cam_z[3] = {cam_xmat[2], cam_xmat[5], cam_xmat[8]};
+
+            // 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);
+              mjtNum* dist = mjSTACKALLOC(d, npixel, mjtNum);
+              mjtNum* normals = NULL;
+              if (dataspec & (1 << mjRAYDATA_NORMAL)) {
+                normals = mjSTACKALLOC(d, 3*npixel, mjtNum);
+              }
+
+              // 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 with normals if needed
+              mj_multiRayNormal(m, d, cam_xpos, vec, NULL, 1, bodyexclude,
+                                geomid, dist, normals, npixel, mjMAXVAL);
+
+              // fill in output for each pixel
+              ptr = d->sensordata + adr;
+              for (int row = 0; row < height; row++) {
+                for (int col = 0; col < width; col++) {
+                  int idx = row*width + col;
+                  mjtNum* normal_ptr = normals ? normals + 3*idx : NULL;
+                  ptr = fill_raydata(ptr, dataspec, dist[idx], cam_xpos, vec + 3*idx,
+                                     normal_ptr, cam_xpos, cam_z);
+                }
+              }
+
+              mj_freeStack(d);
+            } else {
+              // orthographic: parallel rays, different origins
+              ptr = d->sensordata + adr;
+              for (int row = 0; row < height; row++) {
+                for (int col = 0; col < width; col++) {
+                  mjtNum origin[3], direction[3];
+                  mju_camPixelRay(origin, direction, cam_xpos, cam_xmat,
+                                  col, row, fx, fy, cx, cy, projection, ortho_extent);
+
+                  int geomid;
+                  mjtNum normal[3];
+                  mjtNum dist = mj_rayNormal(m, d, origin, direction, NULL, 1,
+                                             bodyexclude, &geomid, normal);
+
+                  ptr = fill_raydata(ptr, dataspec, dist, origin, direction,
+                                     normal, cam_xpos, cam_z);
+                }
               }
             }
           }

src/engine/engine_support.c

@@ -112,6 +112,16 @@ const int mjCONDATA_SIZE[mjNCONDATA] = {
 };
 
 
+// size of ray data fields
+const int mjRAYDATA_SIZE[mjNRAYDATA] = {
+  1,  // mjRAYDATA_DIST
+  3,  // mjRAYDATA_DIR
+  3,  // mjRAYDATA_ORIGIN
+  3,  // mjRAYDATA_POINT
+  3,  // mjRAYDATA_NORMAL
+  1   // mjRAYDATA_DEPTH
+};
+
 //-------------------------- get/set state ---------------------------------------------------------
 
 // return size of a single state element
@@ -745,6 +755,18 @@ int mju_condataSize(int dataspec) {
 }
 
 
+// return total size of data in a rangefinder sensor bitfield specification
+int mju_raydataSize(int dataspec) {
+  int size = 0;
+  for (int i=0; i < mjNRAYDATA; i++) {
+    if (dataspec & (1 << i)) {
+      size += mjRAYDATA_SIZE[i];
+    }
+  }
+  return size;
+}
+
+
 // compute camera pixel parameters from model, output are:
 //   pixel units: fx, fy (focal lengths), cx, cy (principal point)
 //   length units: extent

src/engine/engine_support.h

@@ -31,6 +31,7 @@ MJAPI extern const char* mjTIMERSTRING[mjNTIMER];
 
 // arrays
 MJAPI extern const int mjCONDATA_SIZE[mjNCONDATA];  // TODO(tassa): expose in public header?
+extern const int mjRAYDATA_SIZE[mjNRAYDATA];
 
 
 //-------------------------- get/set state ---------------------------------------------------------
@@ -120,6 +121,9 @@ MJAPI const char* mj_versionString(void);
 // return total size of data fields in a contact sensor bitfield specification
 MJAPI int mju_condataSize(int dataSpec);
 
+// return total size of data fields in a rangefinder sensor bitfield specification
+int mju_raydataSize(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,

src/engine/engine_vis_visualize.c

@@ -2476,27 +2476,74 @@ static void addRangefinderGeoms(const mjModel* m, mjData* d, const mjvOption* vo
     return;
   }
 
+  const float scl = m->stat.meansize;
+  mjtNum framewidth = m->vis.scale.framewidth * scl;
+  mjtNum framelength = m->vis.scale.framelength * scl;
+
   for (int i=0; i < m->nsensor; i++) {
     if (m->sensor_type[i] == mjSENS_RANGEFINDER) {
       int objid = m->sensor_objid[i];
       int adr = m->sensor_adr[i];
 
+      // get dataspec and compute field offsets
+      int dataspec = m->sensor_intprm[i*mjNSENS];
+      int size = mju_raydataSize(dataspec);
+      int offset[mjNRAYDATA] = {0};
+      int increment = 0;
+      for (int j=0; j < mjNRAYDATA; j++) {
+        offset[j] = increment;
+        if (dataspec & (1 << j)) {
+          increment += mjRAYDATA_SIZE[j];
+        }
+      }
+
       // site-attached rangefinder
       if (m->sensor_objtype[i] == mjOBJ_SITE) {
-        mjtNum dst = d->sensordata[adr];
+        const mjtNum* ptr = d->sensordata + adr;
 
-        // null output: nothing to render
+        // get distance (if present)
-        if (dst < 0) {
+        mjtNum dist = -1;
-          continue;
+        if (dataspec & (1 << mjRAYDATA_DIST)) {
+          dist = ptr[offset[mjRAYDATA_DIST]];
         }
 
-        // make ray
+        // get point and draw line if dist is valid
-        mjtNum* from = d->site_xpos+3*objid;
+        mjtNum point[3] = {0};
-        mjtNum to[3] = {from[0] + d->site_xmat[9*objid+2]*dst,
+        if (dist >= 0) {
-                        from[1] + d->site_xmat[9*objid+5]*dst,
+          mjtNum* origin = d->site_xpos + 3*objid;
-                        from[2] + d->site_xmat[9*objid+8]*dst};
+          point[0] = origin[0] + d->site_xmat[9*objid+2]*dist;
-        addConnector(scn, mjGEOM_LINE, 3, from, to, m->vis.rgba.rangefinder,
+          point[1] = origin[1] + d->site_xmat[9*objid+5]*dist;
-                     i, mjCAT_DECOR, mjOBJ_SENSOR);
+          point[2] = origin[2] + d->site_xmat[9*objid+8]*dist;
+          addConnector(scn, mjGEOM_LINE, 3, origin, point, m->vis.rgba.rangefinder,
+                       i, mjCAT_DECOR, mjOBJ_SENSOR);
+        }
+
+        // draw point if present and non-zero
+        if (dataspec & (1 << mjRAYDATA_POINT)) {
+          const mjtNum* point_data = ptr + offset[mjRAYDATA_POINT];
+          if (point_data[0] || point_data[1] || point_data[2]) {
+            mju_copy3(point, point_data);
+            mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_SENSOR);
+            if (thisgeom) {
+              thisgeom->type = mjGEOM_SPHERE;
+              thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = 1.5 * framewidth;
+              mju_n2f(thisgeom->pos, point, 3);
+              mju_n2f(thisgeom->mat, IDENTITY, 9);
+              f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
+              releaseGeom(&thisgeom, scn);
+            }
+          }
+        }
+
+        // draw normal if present and point is valid
+        int valid_point = dist >= 0 || point[0] || point[1] || point[2];
+        if (valid_point && (dataspec & (1 << mjRAYDATA_NORMAL))) {
+          const mjtNum* normal = ptr + offset[mjRAYDATA_NORMAL];
+          mjtNum to[3];
+          mju_addScl3(to, point, normal, 2*framelength);
+          addConnector(scn, mjGEOM_ARROW1, framewidth, point, to,
+                       m->vis.rgba.rangefinder, i, mjCAT_DECOR, mjOBJ_SENSOR);
+        }
       }
 
       // camera-attached rangefinder
@@ -2511,15 +2558,16 @@ static void addRangefinderGeoms(const mjModel* m, mjData* d, const mjvOption* vo
         mjtNum fx, fy, cx, cy, ortho_extent;
         mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent);
 
-        // draw ray for each pixel
+        // draw 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];
+            const mjtNum* ptr = d->sensordata + adr + idx*size;
 
-            // null output: nothing to render
+            // get distance (if present)
-            if (dst < 0) {
+            mjtNum dist = -1;
-              continue;
+            if (dataspec & (1 << mjRAYDATA_DIST)) {
+              dist = ptr[offset[mjRAYDATA_DIST]];
             }
 
             // compute ray origin and direction
@@ -2527,12 +2575,40 @@ static void addRangefinderGeoms(const mjModel* m, mjData* d, const mjvOption* vo
             mju_camPixelRay(origin, direction, cam_xpos, cam_xmat,
                             col, row, fx, fy, cx, cy, projection, ortho_extent);
 
-            // compute endpoint
+            // get point and draw line if dist is valid
-            mjtNum to[3];
+            mjtNum point[3] = {0};
-            mju_addScl3(to, origin, direction, dst);
+            if (dist >= 0) {
+              mju_addScl3(point, origin, direction, dist);
+              addConnector(scn, mjGEOM_LINE, 3, origin, point, m->vis.rgba.rangefinder,
+                           i, mjCAT_DECOR, mjOBJ_SENSOR);
+            }
 
-            addConnector(scn, mjGEOM_LINE, 3, origin, to, m->vis.rgba.rangefinder,
+            // draw point if present and non-zero
-                         i, mjCAT_DECOR, mjOBJ_SENSOR);
+            if (dataspec & (1 << mjRAYDATA_POINT)) {
+              const mjtNum* point_data = ptr + offset[mjRAYDATA_POINT];
+              if (point_data[0] || point_data[1] || point_data[2]) {
+                mju_copy3(point, point_data);
+                mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_SENSOR);
+                if (thisgeom) {
+                  thisgeom->type = mjGEOM_SPHERE;
+                  thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = 1.3 * framewidth;
+                  mju_n2f(thisgeom->pos, point, 3);
+                  mju_n2f(thisgeom->mat, IDENTITY, 9);
+                  f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
+                  releaseGeom(&thisgeom, scn);
+                }
+              }
+            }
+
+            // draw normal if present and point is valid
+            int valid_point = dist >= 0 || point[0] || point[1] || point[2];
+            if (valid_point && (dataspec & (1 << mjRAYDATA_NORMAL))) {
+              const mjtNum* normal = ptr + offset[mjRAYDATA_NORMAL];
+              mjtNum to[3];
+              mju_addScl3(to, point, normal, 2*framelength);
+              addConnector(scn, mjGEOM_ARROW1, framewidth, point, to,
+                           m->vis.rgba.rangefinder, i, mjCAT_DECOR, mjOBJ_SENSOR);
+            }
           }
         }
       }

src/user/user_api.cc

@@ -1274,14 +1274,6 @@ 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 camera->spec.resolution[0] * camera->spec.resolution[1];
-    }
-    return 1;  // site-attached: single ray
-
   case mjSENS_CAMPROJECTION:
     return 2;
 
@@ -1321,6 +1313,18 @@ int mjs_sensorDim(const mjsSensor* sensor) {
                    static_cast<mjCSensor*>(sensor->element)->get_obj())
                    ->nvert();
 
+  case mjSENS_RANGEFINDER:
+    {
+      int size = mju_raydataSize(sensor->intprm[0]);
+      int num_rays = 1;
+      if (sensor->objtype == mjOBJ_CAMERA) {
+        const mjCCamera* camera = static_cast<const mjCCamera*>(
+            static_cast<mjCSensor*>(sensor->element)->get_obj());
+        num_rays = camera->spec.resolution[0] * camera->spec.resolution[1];
+      }
+      return size * num_rays;
+    }
+
   case mjSENS_USER:
     return sensor->dim;
 

src/user/user_objects.cc

@@ -7149,7 +7149,6 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
 mjtDataType sensorDatatype(mjtSensor type) {
   switch (type) {
   case mjSENS_TOUCH:
-  case mjSENS_RANGEFINDER:
   case mjSENS_INSIDESITE:
     return mjDATATYPE_POSITIVE;
 
@@ -7196,6 +7195,7 @@ mjtDataType sensorDatatype(mjtSensor type) {
   case mjSENS_SUBTREEANGMOM:
   case mjSENS_GEOMDIST:
   case mjSENS_GEOMFROMTO:
+  case mjSENS_RANGEFINDER:
   case mjSENS_CONTACT:
   case mjSENS_TACTILE:
   case mjSENS_E_POTENTIAL:
@@ -7329,9 +7329,26 @@ void mjCSensor::Compile(void) {
       break;
 
     case mjSENS_RANGEFINDER:
-      // must be attached to site or camera
+      {
-      if (objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) {
+        // must be attached to site or camera
-        throw mjCError(this, "sensor 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");
+        }
+
+        // check for dataspec correctness
+        int dataspec = intprm[0];
+        if (dataspec <= 0) {
+          throw mjCError(this, "data spec (intprm[0]) must be positive, got %d", nullptr, dataspec);
+        }
+        int mask = (1 << mjNRAYDATA) - 1;
+        if (!(dataspec & mask)) {
+          throw mjCError(this, "data spec intprm[0]=%d must have at least one bit set of the first "
+                         "mjNRAYDATA bits", nullptr, dataspec);
+        }
+        if (dataspec & ~mask) {
+          throw mjCError(this, "data spec intprm[0]=%d has bits set beyond the first "
+                         "mjNRAYDATA bits", nullptr, dataspec);
+        }
       }
       break;
 

src/xml/xml_base.h

@@ -76,6 +76,7 @@ extern const mjMap bias_map[];
 extern const mjMap stage_map[];
 extern const mjMap datatype_map[];
 extern const mjMap condata_map[];
+extern const mjMap raydata_map[];
 extern const mjMap reduce_map[];
 extern const mjMap meshtype_map[];
 extern const mjMap meshinertia_map[];

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", "camera", "cutoff", "noise", "user"},
+        {"rangefinder", "*", "name", "site", "camera", "data", "cutoff", "noise", "user"},
         {"jointpos", "*", "name", "joint", "cutoff", "noise", "user"},
         {"jointvel", "*", "name", "joint", "cutoff", "noise", "user"},
         {"tendonpos", "*", "name", "tendon", "cutoff", "noise", "user"},
@@ -783,6 +783,17 @@ const mjMap condata_map[mjNCONDATA] = {
 };
 
 
+// rangefinder data type
+const mjMap raydata_map[mjNRAYDATA] = {
+  {"dist",          mjRAYDATA_DIST},
+  {"dir",           mjRAYDATA_DIR},
+  {"origin",        mjRAYDATA_ORIGIN},
+  {"point",         mjRAYDATA_POINT},
+  {"normal",        mjRAYDATA_NORMAL},
+  {"depth",         mjRAYDATA_DEPTH}
+};
+
+
 // contact reduction type
 const int reduce_sz = 4;
 const mjMap reduce_map[reduce_sz] = {
@@ -4058,6 +4069,28 @@ void mjXReader::Sensor(XMLElement* section) {
         throw mjXError(elem, "rangefinder requires exactly one of 'site' or 'camera'");
       }
       sensor->objtype = use_site ? mjOBJ_SITE : mjOBJ_CAMERA;
+
+      // process data specification (intprm[0])
+      int dataspec = 1 << mjRAYDATA_DIST;
+      std::vector<int> raydata(mjNRAYDATA);
+      int nkeys = MapValues(elem, "data", raydata.data(), raydata_map, mjNRAYDATA);
+      if (nkeys) {
+        dataspec = 1 << raydata[0];
+
+        // check ordering while adding bits to dataspec
+        for (int i = 1; i < nkeys; ++i) {
+          if (raydata[i] <= raydata[i-1]) {
+            std::string correct_order;
+            for (int j = 0; j < mjNRAYDATA; ++j) {
+              correct_order += raydata_map[j].key;
+              if (j < mjNRAYDATA - 1) correct_order += ", ";
+            }
+            throw mjXError(elem, "data attributes must be in order: %s", correct_order.c_str());
+          }
+          dataspec |= 1 << raydata[i];
+        }
+      }
+      sensor->intprm[0] = dataspec;
     }
 
     // sensors related to scalar joints, tendons, actuators

src/xml/xml_native_writer.cc

@@ -2028,11 +2028,22 @@ void mjXWriter::Sensor(XMLElement* root) {
         WriteAttrTxt(elem, "site", sensor->get_objname());
         break;
       case mjSENS_RANGEFINDER:
-        elem = InsertEnd(section, "rangefinder");
+        {
-        if (sensor->objtype == mjOBJ_SITE) {
+          elem = InsertEnd(section, "rangefinder");
-          WriteAttrTxt(elem, "site", sensor->get_objname());
+          if (sensor->objtype == mjOBJ_SITE) {
-        } else {
+            WriteAttrTxt(elem, "site", sensor->get_objname());
-          WriteAttrTxt(elem, "camera", sensor->get_objname());
+          } else {
+            WriteAttrTxt(elem, "camera", sensor->get_objname());
+          }
+          int dataspec = sensor->intprm[0];
+          int data[mjNRAYDATA];
+          int ndata = 0;
+          for (int i=0; i < mjNRAYDATA; i++) {
+            if (dataspec & (1 << i)) {
+              data[ndata++] = i;
+            }
+          }
+          WriteAttrKeys(elem, "data", raydata_map, mjNRAYDATA, data, ndata, 0);
         }
         break;
       case mjSENS_CAMPROJECTION:

test/engine/engine_ray_test.cc

@@ -343,10 +343,15 @@ TEST_F(RayTest, EdgeCases) {
   m->geom_aabb[0] = m->geom_aabb[1] = m->geom_aabb[2] = 0;
   m->geom_aabb[3] = m->geom_aabb[4] = m->geom_aabb[5] = 0;
   mju_multiRayPrepare(m, d, pnt4, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
-  EXPECT_FLOAT_EQ(geom_ba[0], 0);
+  // margin = atan(max_half / dist) where max_half = max(aabb[3..5])
-  EXPECT_FLOAT_EQ(geom_ba[1], mjPI/2);
+  // For a zero-size AABB: max_half = 0, so margin = 0
-  EXPECT_FLOAT_EQ(geom_ba[2], 0);
+  mjtNum dist4 = mju_dist3(pnt4, d->geom_xpos);
-  EXPECT_FLOAT_EQ(geom_ba[3], mjPI/2);
+  mjtNum max_half4 = mju_max(m->geom_aabb[3], mju_max(m->geom_aabb[4], m->geom_aabb[5]));
+  mjtNum margin4 = mju_atan2(max_half4, dist4);
+  EXPECT_NEAR(geom_ba[0], 0 - margin4, 1e-6);
+  EXPECT_NEAR(geom_ba[1], mjPI/2 - margin4, 1e-6);
+  EXPECT_NEAR(geom_ba[2], 0 + margin4, 1e-6);
+  EXPECT_NEAR(geom_ba[3], mjPI/2 + margin4, 1e-6);
   mjtNum vec4[] = {1, 0, 0};
   mj_multiRay(m, d, pnt4, vec4, NULL, 1, -1, &rgeomid, &dist, 1, mjMAXVAL);
   EXPECT_FLOAT_EQ(dist, 0.9);

test/engine/engine_sensor_test.cc

@@ -1065,8 +1065,8 @@ TEST_F(SensorTest, RangefinderCamera) {
     </worldbody>
 
     <sensor>
-      <rangefinder camera="persp"/>
+      <rangefinder camera="persp" data="dist depth"/>
-      <rangefinder camera="ortho"/>
+      <rangefinder camera="ortho" data="dist dir origin point"/>
     </sensor>
   </mujoco>
   )";
@@ -1074,8 +1074,9 @@ TEST_F(SensorTest, RangefinderCamera) {
   mjModel* model = LoadModelFromString(xml, error, sizeof(error));
   ASSERT_THAT(model, NotNull()) << error;
 
-  // sensordata dimension should be 3x3 + 3x3 = 18
+  // first sensor: data="dist depth" => (1+1)*9 = 18
-  EXPECT_EQ(model->nsensordata, 18);
+  // second sensor: data="dist dir origin point" => (1+3+3+3)*9 = 90
+  EXPECT_EQ(model->nsensordata, 108);
 
   mjData* data = mj_makeData(model);
   mj_forward(model, data);
@@ -1085,42 +1086,63 @@ TEST_F(SensorTest, RangefinderCamera) {
   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
+  // test 1: perspective camera - rays diverge, distance varies with angle
+  int adr0 = model->sensor_adr[0];
+  constexpr int stride0 = 2;  // dist(1) + depth(1)
   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);
+      mjtNum expected_dist = height * mju_sqrt(1 + dx*dx + dy*dy);
-      EXPECT_NEAR(data->sensordata[idx], expected, tol)
+      mjtNum dist = data->sensordata[adr0 + idx*stride0];
-          << "perspective pixel (" << row << ", " << col << ")";
+      EXPECT_NEAR(dist, expected_dist, tol)
+          << "perspective dist pixel (" << row << ", " << col << ")";
+
+      // depth should equal camera height (2.0) for all pixels
+      mjtNum depth = data->sensordata[adr0 + idx*stride0 + 1];
+      EXPECT_NEAR(depth, height, tol)
+          << "perspective depth pixel (" << row << ", " << col << ")";
     }
   }
 
-  // orthographic camera: tilted 45 degrees around Y axis
+  // test 2: orthographic camera distance - 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);
+  int adr1 = model->sensor_adr[1];
+  constexpr int stride1 = 10;  // dist(1) + dir(3) + origin(3) + point(3)
   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
+      int idx = row * 3 + col;
 
       // 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)
+      // 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;
+      mjtNum expected_dist = origin_z / cos45;
-      EXPECT_NEAR(data->sensordata[idx], expected, tol)
+      mjtNum dist = data->sensordata[adr1 + idx*stride1];
-          << "orthographic pixel (" << row << ", " << col << ")";
+      EXPECT_NEAR(dist, expected_dist, tol)
+          << "orthographic dist pixel (" << row << ", " << col << ")";
+
+      // verify point = origin + dir * dist
+      mjtNum* dir = data->sensordata + adr1 + idx*stride1 + 1;
+      mjtNum* origin = data->sensordata + adr1 + idx*stride1 + 4;
+      mjtNum* point = data->sensordata + adr1 + idx*stride1 + 7;
+      mjtNum expected_point[3];
+      mju_addScl3(expected_point, origin, dir, dist);
+      EXPECT_NEAR(point[0], expected_point[0], tol)
+          << "ortho point[0] pixel (" << row << ", " << col << ")";
+      EXPECT_NEAR(point[1], expected_point[1], tol)
+          << "ortho point[1] pixel (" << row << ", " << col << ")";
+      EXPECT_NEAR(point[2], expected_point[2], tol)
+          << "ortho point[2] pixel (" << row << ", " << col << ")";
     }
   }
 
@@ -1135,9 +1157,38 @@ TEST_F(SensorTest, RFCamera) {
   mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
   ASSERT_THAT(model, NotNull()) << error;
 
+  // both sensors have data="dist point normal" => (1+3+3)*16 = 112
+  ASSERT_EQ(model->nsensor, 2);
+  EXPECT_EQ(model->sensor_dim[0], 112);
+  EXPECT_EQ(model->sensor_dim[1], 112);
+
   mjData* data = mj_makeData(model);
   mj_step(model, data);
 
+  // check both sensors: dist, point, normal
+  constexpr int stride = 7;  // dist(1) + point(3) + normal(3)
+  for (int s = 0; s < 2; s++) {
+    int adr = model->sensor_adr[s];
+    for (int i = 0; i < 16; i++) {
+      mjtNum dist = data->sensordata[adr + i*stride];
+      mjtNum* point = data->sensordata + adr + i*stride + 1;
+      mjtNum* normal = data->sensordata + adr + i*stride + 4;
+
+      EXPECT_TRUE(dist > 0 || dist == -1) << "sensor " << s << " pixel " << i;
+
+      if (dist > 0) {
+        EXPECT_GT(mju_norm3(point), 0.0) << "sensor " << s << " point " << i;
+        EXPECT_NEAR(mju_norm3(normal), 1.0, 1e-6)
+            << "sensor " << s << " normal " << i;
+      } else {
+        EXPECT_NEAR(mju_norm3(point), 0.0, 1e-6)
+            << "sensor " << s << " point " << i;
+        EXPECT_NEAR(mju_norm3(normal), 0.0, 1e-6)
+            << "sensor " << s << " normal " << i;
+      }
+    }
+  }
+
   mj_deleteData(data);
   mj_deleteModel(model);
 }

test/engine/testdata/ray/hfield.xml

@@ -1,5 +1,4 @@
 <mujoco>
-
   <asset>
     <hfield name="hfield" nrow="5" ncol="5" size=".8 .8 .8 .3"
             elevation="1 0 1 1 0

test/engine/testdata/ray/ray_swing.xml

@@ -1,6 +1,8 @@
 <mujoco>
+  <statistic meansize="0.15"/>
+
   <visual>
-    <global realtime=".25"/>
+    <global realtime=".25" elevation="-10"/>
   </visual>
 
   <default>
@@ -17,6 +19,6 @@
   </worldbody>
 
   <sensor>
-    <rangefinder site="rf"/>
+    <rangefinder site="rf" data="dist normal"/>
   </sensor>
 </mujoco>

test/engine/testdata/sensor/rfcamera.xml

@@ -1,36 +1,51 @@
 <mujoco model="rangefinder camera">
+  <asset>
+    <hfield name="hfield" nrow="5" ncol="5" size="2 2 .3 .1"
+            elevation=".7 .1 .4 .9 .0
+                       .3 .6 .2 .8 .5
+                       .1 .4 .7 .0 .9
+                       .2 .5 .8 .3 .6
+                       .4 .7 .1 .9 .2"/>
+    <mesh name="ss" builtin="supersphere" params="30 .2 .3" scale=".4 .42 .5"/>
+  </asset>
+
   <visual>
-    <rgba frustum="1 1 0 0.1"/>
+    <rgba frustum="1 1 0 0.15"/>
   </visual>
 
-  <worldbody>
+  <statistic meansize="0.17"/>
-    <light pos="0 0 3"/>
 
-    <!-- ground plane -->
+  <worldbody>
-    <geom type="plane" size="5 5 .1" rgba=".3 .4 .5 1"/>
+    <light pos="0 0 5"/>
+    <camera pos="0 -5.8 2.4" xyaxes="1 0 0 0 0.28 0.96"/>
+    <camera pos="-0.32 -3.23 2.92" xyaxes="1 0 0 0.05 0.5 0.85" fovy="60"/>
+
+
+    <!-- ground hfield -->
+    <geom type="hfield" hfield="hfield" rgba=".3 .4 .5 1" pos="0 0 -.3"/>
 
     <!-- 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="mesh" mesh="ss" pos="1 0.5 .35" rgba=".3 1 .3 1" euler="20 0 30"/>
-    <geom type="cylinder" pos="-0.8 0.6 .4" size=".4 .4" rgba=".3 .3 1 1"/>
+    <geom type="cylinder" pos="-0.8 0.8 .4" size=".4 .5" euler="0 40 0" 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="capsule" pos="0.5 -0.7 -.2" size=".3 .5" zaxis="1 0.5 0" rgba="0 1 1 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">
+    <body pos="1 0 2" euler="0 20 90" 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">
+    <body pos="-1 0 2" euler="0 -20 -90" 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="perspective" data="dist point normal"/>
-    <rangefinder camera="orthographic"/>
+    <rangefinder camera="orthographic" data="dist point normal"/>
   </sensor>
 </mujoco>

test/testdata/model.xml

@@ -157,7 +157,7 @@
 
   <sensor>
     <framepos objtype="site" objname="wheel_0" reftype="site" refname="wheel_2"/>
-    <rangefinder site="rf"/>
+    <rangefinder site="rf" data="dist dir origin point normal depth"/>
     <gyro site="wheel_2"/>
     <touch site="wheel_1"/>
     <force site="knee"/>

test/xml/xml_native_writer_test.cc

@@ -1393,6 +1393,7 @@ TEST_F(XMLWriterTest, WriteReadCompare) {
               absl::StrContains(p.path().string(), "shark_") ||
               absl::StrContains(p.path().string(), "perf") ||
               // exclude files that fail the comparison test
+              absl::StrContains(p.path().string(), "rfcamera") ||
               absl::StrContains(p.path().string(), "tactile") ||
               absl::StrContains(p.path().string(), "makemesh") ||
               absl::StrContains(p.path().string(), "many_dependencies") ||

unity/Runtime/Bindings/MjBindings.cs

@@ -431,6 +431,15 @@ public enum mjtConDataField : int{
   mjCONDATA_TANGENT = 6,
   mjNCONDATA = 7,
 }
+public enum mjtRayDataField : int{
+  mjRAYDATA_DIST = 0,
+  mjRAYDATA_DIR = 1,
+  mjRAYDATA_ORIGIN = 2,
+  mjRAYDATA_POINT = 3,
+  mjRAYDATA_NORMAL = 4,
+  mjRAYDATA_DEPTH = 5,
+  mjNRAYDATA = 6,
+}
 public enum mjtSameFrame : int{
   mjSAMEFRAME_NONE = 0,
   mjSAMEFRAME_BODY = 1,

wasm/codegen/generated/bindings.cc

@@ -10506,6 +10506,14 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
   enum_<mjtProjection>("mjtProjection")
     .value("mjPROJ_PERSPECTIVE", mjPROJ_PERSPECTIVE)
     .value("mjPROJ_ORTHOGRAPHIC", mjPROJ_ORTHOGRAPHIC);
+  enum_<mjtRayDataField>("mjtRayDataField")
+    .value("mjRAYDATA_DIST", mjRAYDATA_DIST)
+    .value("mjRAYDATA_DIR", mjRAYDATA_DIR)
+    .value("mjRAYDATA_ORIGIN", mjRAYDATA_ORIGIN)
+    .value("mjRAYDATA_POINT", mjRAYDATA_POINT)
+    .value("mjRAYDATA_NORMAL", mjRAYDATA_NORMAL)
+    .value("mjRAYDATA_DEPTH", mjRAYDATA_DEPTH)
+    .value("mjNRAYDATA", mjNRAYDATA);
   enum_<mjtRndFlag>("mjtRndFlag")
     .value("mjRND_SHADOW", mjRND_SHADOW)
     .value("mjRND_WIREFRAME", mjRND_WIREFRAME)