diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index d6f459ab..fb4391fa 100644 --- a/doc/XMLreference.rst +++ b/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`. +.. 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: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 86a72cf3..f3e4930d 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -940,9 +940,9 @@ | :ref:`rangefinder | \* | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`name` | :ref:`site` | :ref:`camera` | :ref:`cutoff` | | +| | | | :ref:`name` | :ref:`site` | :ref:`camera` | :ref:`data` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`noise` | :ref:`user` | | | | +| | | | :ref:`cutoff` | :ref:`noise` | :ref:`user` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| sensor |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 70a83ad5..edb8faa9 100644 --- a/doc/changelog.rst +++ b/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` to values larger than 1. Relatedly, frustum visualization also works for :ref:`orthographic` cameras. - Rangefinder sensors can now be attached to a camera using the :ref:`ragefinder/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` 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` rendering flag + - Added a :ref:`depth` rendering flag. MJX ^^^ diff --git a/doc/images/XMLreference/rfcamera.png b/doc/images/XMLreference/rfcamera.png new file mode 100644 index 00000000..d300718d Binary files /dev/null and b/doc/images/XMLreference/rfcamera.png differ diff --git a/doc/includes/references.h b/doc/includes/references.h index 28902f55..519bfe52 100644 --- a/doc/includes/references.h +++ b/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 diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 8d355403..64036073 100644 --- a/include/mujoco/mjmodel.h +++ b/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 diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index db5f1e08..9dc5da46 100644 --- a/python/mujoco/introspect/enums.py +++ b/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', diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 19778d8e..565aca66 100644 --- a/python/mujoco/specs_test.py +++ b/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() diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index e7b26ed6..66e7a1d7 100644 --- a/src/engine/engine_ray.c +++ b/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] || - azimuth > (geom_ba+4*i)[2] || elevation > (geom_ba+4*i)[3]) { + mjtNum az_min = (geom_ba+4*i)[0]; + 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; } } diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 79013e18..2a337d25 100644 --- a/src/engine/engine_sensor.c +++ b/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]; - 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]; + { + // get dataspec + int dataspec = m->sensor_intprm[i*mjNSENS]; + mjtNum* ptr = d->sensordata + adr; - // 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 (objtype == mjOBJ_SITE) { + // site-attached rangefinder: single ray + 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) { - // 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); + int geomid; + mjtNum normal[3]; + mjtNum* p_normal = (dataspec & (1 << mjRAYDATA_NORMAL)) ? normal : NULL; + mjtNum dist = mj_rayNormal(m, d, origin, rvec, NULL, 1, + m->site_bodyid[objid], &geomid, p_normal); - // 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); - } - } + // for site sensor: pass NULL for cam_z so depth = dist + fill_raydata(ptr, dataspec, dist, origin, rvec, normal, NULL, NULL); - // 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); + // 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]; - d->sensordata[adr + idx] = mj_ray(m, d, origin, direction, NULL, 1, - bodyexclude, NULL); + // camera z-axis (pointing into scene, negative of optical axis) + 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); + } } } } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 944273df..e54ae1ce 100644 --- a/src/engine/engine_support.c +++ b/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 diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index 137eaeeb..552f1ba4 100644 --- a/src/engine/engine_support.h +++ b/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, diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index fcb2c8d8..3bd9f1a2 100644 --- a/src/engine/engine_vis_visualize.c +++ b/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 - if (dst < 0) { - continue; + // get distance (if present) + mjtNum dist = -1; + if (dataspec & (1 << mjRAYDATA_DIST)) { + dist = ptr[offset[mjRAYDATA_DIST]]; } - // 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); + // get point and draw line if dist is valid + mjtNum point[3] = {0}; + if (dist >= 0) { + mjtNum* origin = d->site_xpos + 3*objid; + point[0] = origin[0] + d->site_xmat[9*objid+2]*dist; + point[1] = origin[1] + d->site_xmat[9*objid+5]*dist; + 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 - if (dst < 0) { - continue; + // get distance (if present) + mjtNum dist = -1; + 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 - mjtNum to[3]; - mju_addScl3(to, origin, direction, dst); + // get point and draw line if dist is valid + mjtNum point[3] = {0}; + 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, - 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.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); + } } } } diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 4d5a529a..ac4f7888 100644 --- a/src/user/user_api.cc +++ b/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( - static_cast(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(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( + static_cast(sensor->element)->get_obj()); + num_rays = camera->spec.resolution[0] * camera->spec.resolution[1]; + } + return size * num_rays; + } + case mjSENS_USER: return sensor->dim; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 29487a9d..e60f9d93 100644 --- a/src/user/user_objects.cc +++ b/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) { - throw mjCError(this, "sensor must be attached to site or camera"); + { + // 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; diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index 5cedbc95..4eee0698 100644 --- a/src/xml/xml_base.h +++ b/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[]; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 294e71d4..9c05102d 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -454,7 +454,7 @@ std::vector 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 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 diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index b373a867..a2bc6961 100644 --- a/src/xml/xml_native_writer.cc +++ b/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) { - WriteAttrTxt(elem, "site", sensor->get_objname()); - } else { - WriteAttrTxt(elem, "camera", sensor->get_objname()); + { + elem = InsertEnd(section, "rangefinder"); + if (sensor->objtype == mjOBJ_SITE) { + WriteAttrTxt(elem, "site", 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: diff --git a/test/engine/engine_ray_test.cc b/test/engine/engine_ray_test.cc index 7797cef0..cacbdde4 100644 --- a/test/engine/engine_ray_test.cc +++ b/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); - EXPECT_FLOAT_EQ(geom_ba[1], mjPI/2); - EXPECT_FLOAT_EQ(geom_ba[2], 0); - EXPECT_FLOAT_EQ(geom_ba[3], mjPI/2); + // margin = atan(max_half / dist) where max_half = max(aabb[3..5]) + // For a zero-size AABB: max_half = 0, so margin = 0 + mjtNum dist4 = mju_dist3(pnt4, d->geom_xpos); + 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); diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index 0ef875b4..3f061aec 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -1065,8 +1065,8 @@ TEST_F(SensorTest, RangefinderCamera) { - - + + )"; @@ -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 - EXPECT_EQ(model->nsensordata, 18); + // first sensor: data="dist depth" => (1+1)*9 = 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); - EXPECT_NEAR(data->sensordata[idx], expected, tol) - << "perspective pixel (" << row << ", " << col << ")"; + mjtNum expected_dist = height * mju_sqrt(1 + dx*dx + dy*dy); + mjtNum dist = data->sensordata[adr0 + idx*stride0]; + 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 - // 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 + // test 2: orthographic camera distance - tilted 45 degrees around Y axis 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; - EXPECT_NEAR(data->sensordata[idx], expected, tol) - << "orthographic pixel (" << row << ", " << col << ")"; + mjtNum expected_dist = origin_z / cos45; + mjtNum dist = data->sensordata[adr1 + idx*stride1]; + 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); } diff --git a/test/engine/testdata/ray/hfield.xml b/test/engine/testdata/ray/hfield.xml index f702a197..d188bc50 100644 --- a/test/engine/testdata/ray/hfield.xml +++ b/test/engine/testdata/ray/hfield.xml @@ -1,5 +1,4 @@ - + - + @@ -17,6 +19,6 @@ - + diff --git a/test/engine/testdata/sensor/rfcamera.xml b/test/engine/testdata/sensor/rfcamera.xml index af3e3210..ffafd952 100644 --- a/test/engine/testdata/sensor/rfcamera.xml +++ b/test/engine/testdata/sensor/rfcamera.xml @@ -1,36 +1,51 @@ + + + + + - + - - + - - + + + + + + + + - - - + + + - + - + - - + + diff --git a/test/testdata/model.xml b/test/testdata/model.xml index 82bc2906..20328265 100644 --- a/test/testdata/model.xml +++ b/test/testdata/model.xml @@ -157,7 +157,7 @@ - + diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index b521c84c..82bafd81 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/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") || diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index e1c48435..30b42ecd 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/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, diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 43fdbdd3..e56faa41 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -10506,6 +10506,14 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { enum_("mjtProjection") .value("mjPROJ_PERSPECTIVE", mjPROJ_PERSPECTIVE) .value("mjPROJ_ORTHOGRAPHIC", mjPROJ_ORTHOGRAPHIC); + enum_("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") .value("mjRND_SHADOW", mjRND_SHADOW) .value("mjRND_WIREFRAME", mjRND_WIREFRAME)