From 9d646e65483b6f8f2c2f3c503e07150bfb4d2b38 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 23 Dec 2025 12:03:37 -0800 Subject: [PATCH] Extend rangefinder sensor to support cameras. PiperOrigin-RevId: 848255889 Change-Id: I6e1a9ed13d29d2242558625a47e24e47ad6e87db --- doc/XMLreference.rst | 27 +++++-- doc/XMLschema.rst | 4 +- doc/changelog.rst | 2 + doc/includes/references.h | 3 +- include/mujoco/mjmodel.h | 3 +- src/engine/engine_sensor.c | 61 ++++++++++++++-- src/engine/engine_support.c | 40 +++++++++++ src/engine/engine_support.h | 6 ++ src/engine/engine_util_misc.c | 39 +++++++++- src/engine/engine_util_misc.h | 7 ++ src/engine/engine_vis_visualize.c | 68 ++++++++++++++---- src/user/user_api.cc | 10 ++- src/user/user_objects.cc | 8 ++- src/xml/xml_native_reader.cc | 10 ++- src/xml/xml_native_writer.cc | 6 +- test/engine/engine_sensor_test.cc | 92 +++++++++++++++++++++++- test/engine/testdata/sensor/rfcamera.xml | 36 ++++++++++ 17 files changed, 383 insertions(+), 39 deletions(-) create mode 100644 test/engine/testdata/sensor/rfcamera.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index e54c3e82..d6f459ab 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -6373,12 +6373,20 @@ site frame. The output is a 3D vector. :el-prefix:`sensor/` |-| **rangefinder** (*) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This element creates a rangefinder. It measures the distance to the nearest geom surface, along the ray defined by the -positive Z-axis of the sensor site. If the ray does not intersect any geom surface, the sensor output is -1. If the -origin of the ray is inside a geom, the surface is still sensed (but not the inner volume). Geoms attached to the same -body as the sensor site are excluded. Invisible geoms, defined as geoms whose rgba (or whose material rgba) has alpha=0, -are also excluded. Note however that geoms made invisible in the visualizer by disabling their geom group are not -excluded; this is because sensor calculations are independent of the visualizer. +This element creates a rangefinder. + +- If associated with a :ref:`site`, it measures the distance to the nearest geom surface, along + the ray defined by the positive Z-axis of the site. +- If associated with a :ref:`camera`, it outputs one distance measurement for each pixel in + the camera image. Note that cameras face the :ref:`negative Z-axis` of their frame. The number of + measurements in this case is equal to product of the camera's width and height + :ref:`resolutions`. + +If a ray does not intersect any geom surface, the sensor output is -1. If the origin of the ray is inside a geom, the +surface is still detected. Geoms attached to the same body as the sensor site/camera are excluded. Invisible geoms, +defined as geoms whose rgba (or whose material rgba) has alpha=0, are also excluded. Note however that geoms made +invisible in the visualizer by disabling their geom group are not excluded; this is because sensor calculations are +independent of the visualizer. .. _sensor-rangefinder-name: @@ -6393,9 +6401,14 @@ excluded; this is because sensor calculations are independent of the visualizer. .. _sensor-rangefinder-site: -:at:`site`: :at-val:`string, required` +:at:`site`: :at-val:`string, optional` The site where the sensor is attached. +.. _sensor-rangefinder-camera: + +:at:`camera`: :at-val:`string, optional` + The camera where the sensor is attached. + .. _sensor-camprojection: :el-prefix:`sensor/` |-| **camprojection** (*) diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 781293b7..86a72cf3 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -940,9 +940,9 @@ | :ref:`rangefinder | \* | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`name` | :ref:`site` | :ref:`cutoff` | :ref:`noise` | | +| | | | :ref:`name` | :ref:`site` | :ref:`camera` | :ref:`cutoff` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`user` | | | | | +| | | | :ref:`noise` | :ref:`user` | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| sensor |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 990afd33..70a83ad5 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -20,6 +20,8 @@ General ^^^^^^^ - 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. - Non-breaking ABI changes: - The type of the ``sig`` (signature) argument of :ref:`mj_stateSize` and related functions has been changed from diff --git a/doc/includes/references.h b/doc/includes/references.h index 5135c431..3369e9b0 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -678,7 +678,6 @@ typedef enum mjtObj_ { // type of MujoCo object mjOBJ_FRAME = 100, // frame mjOBJ_DEFAULT, // default mjOBJ_MODEL // entire model - } mjtObj; typedef enum mjtSensor_ { // type of sensor // common robotic sensors, attached to a site @@ -689,7 +688,7 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_FORCE, // 3D force between site's body and its parent body mjSENS_TORQUE, // 3D torque between site's body and its parent body mjSENS_MAGNETOMETER, // 3D magnetometer - mjSENS_RANGEFINDER, // scalar distance to nearest geom or site along z-axis + mjSENS_RANGEFINDER, // scalar distance to nearest geom along z-axis mjSENS_CAMPROJECTION, // pixel coordinates of a site in the camera image // sensors related to scalar joints, tendons, actuators diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 6910d861..8d355403 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -296,7 +296,6 @@ typedef enum mjtObj_ { // type of MujoCo object mjOBJ_FRAME = 100, // frame mjOBJ_DEFAULT, // default mjOBJ_MODEL // entire model - } mjtObj; @@ -309,7 +308,7 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_FORCE, // 3D force between site's body and its parent body mjSENS_TORQUE, // 3D torque between site's body and its parent body mjSENS_MAGNETOMETER, // 3D magnetometer - mjSENS_RANGEFINDER, // scalar distance to nearest geom or site along z-axis + mjSENS_RANGEFINDER, // scalar distance to nearest geom along z-axis mjSENS_CAMPROJECTION, // pixel coordinates of a site in the camera image // sensors related to scalar joints, tendons, actuators diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 0330313f..79013e18 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -435,11 +435,62 @@ void mj_sensorPos(const mjModel* m, mjData* d) { break; case mjSENS_RANGEFINDER: // rangefinder - rvec[0] = d->site_xmat[9*objid+2]; - rvec[1] = d->site_xmat[9*objid+5]; - rvec[2] = d->site_xmat[9*objid+8]; - d->sensordata[adr] = mj_ray(m, d, d->site_xpos+3*objid, rvec, NULL, 1, - m->site_bodyid[objid], NULL); + if (objtype == mjOBJ_SITE) { + rvec[0] = d->site_xmat[9*objid+2]; + rvec[1] = d->site_xmat[9*objid+5]; + rvec[2] = d->site_xmat[9*objid+8]; + d->sensordata[adr] = mj_ray(m, d, d->site_xpos+3*objid, rvec, NULL, 1, + m->site_bodyid[objid], NULL); + } else { + // camera-attached rangefinder: depth image + const int width = m->cam_resolution[2*objid]; + const int height = m->cam_resolution[2*objid+1]; + const int bodyexclude = m->cam_bodyid[objid]; + const mjtNum* cam_xpos = d->cam_xpos + 3*objid; + const mjtNum* cam_xmat = d->cam_xmat + 9*objid; + const int projection = m->cam_projection[objid]; + + // compute focal length in pixels using helper + mjtNum fx, fy, cx, cy, ortho_extent; + mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent); + + if (projection == mjPROJ_PERSPECTIVE) { + // perspective: all rays share origin, different directions + const int npixel = width * height; + mj_markStack(d); + mjtNum* vec = mjSTACKALLOC(d, 3*npixel, mjtNum); + int* geomid = mjSTACKALLOC(d, npixel, int); + + // compute ray directions using helper (normalized) + for (int row = 0; row < height; row++) { + for (int col = 0; col < width; col++) { + int idx = row*width + col; + mjtNum origin[3]; + mju_camPixelRay(origin, vec + 3*idx, cam_xpos, cam_xmat, + col, row, fx, fy, cx, cy, projection, ortho_extent); + } + } + + // cast all rays + mj_multiRay(m, d, cam_xpos, vec, NULL, 1, bodyexclude, + geomid, d->sensordata + adr, npixel, mjMAXVAL); + + mj_freeStack(d); + } else { + // orthographic: parallel rays, different origins + for (int row = 0; row < height; row++) { + for (int col = 0; col < width; col++) { + int idx = row*width + col; + mjtNum origin[3], direction[3]; + mju_camPixelRay(origin, direction, cam_xpos, cam_xmat, + col, row, fx, fy, cx, cy, projection, ortho_extent); + + d->sensordata[adr + idx] = mj_ray(m, d, origin, direction, NULL, 1, + bodyexclude, NULL); + } + } + } + } break; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index a896dd4a..944273df 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -743,3 +743,43 @@ int mju_condataSize(int dataspec) { } return size; } + + +// compute camera pixel parameters from model, output are: +// pixel units: fx, fy (focal lengths), cx, cy (principal point) +// length units: extent +void mju_camIntrinsics(const mjModel* m, int camid, + mjtNum* fx, mjtNum* fy, mjtNum* cx, mjtNum* cy, mjtNum* extent) { + const int width = m->cam_resolution[2*camid]; + const int height = m->cam_resolution[2*camid+1]; + const float* sensorsize = m->cam_sensorsize + 2*camid; + const float* intrinsic = m->cam_intrinsic + 4*camid; + const mjtProjection projection = (mjtProjection)m->cam_projection[camid]; + + switch (projection) { + case mjPROJ_PERSPECTIVE: + if (sensorsize[0] && sensorsize[1]) { + // intrinsic-based perspective camera + *fx = intrinsic[0] / sensorsize[0] * width; + *fy = intrinsic[1] / sensorsize[1] * height; + *cx = intrinsic[2] / sensorsize[0] * width; + *cy = intrinsic[3] / sensorsize[1] * height; + } else { + // fovy-based perspective camera + *fx = *fy = 0.5 / mju_tan(m->cam_fovy[camid] * mjPI / 360.0) * height; + *cx = (mjtNum)width / 2.0; + *cy = (mjtNum)height / 2.0; + } + break; + case mjPROJ_ORTHOGRAPHIC: + // orthographic: normalize pixel offset to [-1, 1] + *fx = (mjtNum)width / 2.0; + *fy = (mjtNum)height / 2.0; + *cx = *fx; + *cy = *fy; + break; + } + + // extent only used for orthographic cameras + *extent = m->cam_fovy[camid]; +} diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index f4366b96..137eaeeb 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -120,6 +120,12 @@ MJAPI const char* mj_versionString(void); // return total size of data fields in a contact sensor bitfield specification MJAPI int mju_condataSize(int dataSpec); +// compute camera pixel parameters from model +// outputs: fx, fy (focal length in pixels), cx, cy (principal point), ortho_extent +void mju_camIntrinsics(const mjModel* m, int camid, + mjtNum* fx, mjtNum* fy, mjtNum* cx, mjtNum* cy, + mjtNum* ortho_extent); + #ifdef __cplusplus } #endif diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 3ceb521c..69296b00 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -417,6 +417,8 @@ mjtNum mju_wrap(mjtNum wpnt[6], const mjtNum x0[3], const mjtNum x1[3], } +//------------------------------ misc geometry ----------------------------------------------------- + // all 3 semi-axes of a geom void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom type) { switch (type) { @@ -494,7 +496,42 @@ int mju_insideGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3 } -// ----------------------------- Flex interpolation ------------------------------------------------ +// compute ray origin and direction for pixel (col, row) in camera image +// for perspective: origin is unchanged, direction is computed +// for orthographic: direction is -Z in camera frame, origin is offset from camera center +void mju_camPixelRay(mjtNum origin[3], mjtNum direction[3], + const mjtNum cam_xpos[3], const mjtNum cam_xmat[9], + int col, int row, mjtNum fx, mjtNum fy, mjtNum cx, mjtNum cy, + int projection, mjtNum ortho_extent) { + // pixel center (row 0 = top of image) + mjtNum px = col + 0.5 - cx; + mjtNum py = row + 0.5 - cy; + + if (projection == mjPROJ_PERSPECTIVE) { + // origin is camera position + mju_copy3(origin, cam_xpos); + + // direction in camera frame: (x/fx, -y/fy, -1), then normalized + mjtNum dir_cam[3] = {px / fx, -py / fy, -1.0}; + mju_mulMatVec3(direction, cam_xmat, dir_cam); + mju_normalize3(direction); + } else { + // orthographic: parallel rays, direction is -Z in camera frame + direction[0] = -cam_xmat[2]; + direction[1] = -cam_xmat[5]; + direction[2] = -cam_xmat[8]; + + // origin offset in camera frame (ortho_extent is full height, use half for each side) + mjtNum half_extent = ortho_extent / 2; + mjtNum offset_cam[3] = {px / fx * half_extent, -py / fy * half_extent, 0}; + mjtNum offset_world[3]; + mju_mulMatVec3(offset_world, cam_xmat, offset_cam); + mju_add3(origin, cam_xpos, offset_world); + } +} + + +// ----------------------------- flex interpolation ------------------------------------------------ mjtNum static inline phi(mjtNum s, int i, int order) { if (order == 1) { diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 7457f460..798657cf 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -57,6 +57,13 @@ MJAPI void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom ty int mju_insideGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], mjtGeom type, const mjtNum point[3]); +// compute ray origin and direction for pixel (col, row) in camera image +// directions are normalized so ray functions return actual 3D distance +void mju_camPixelRay(mjtNum origin[3], mjtNum direction[3], + const mjtNum cam_xpos[3], const mjtNum cam_xmat[9], + int col, int row, mjtNum fx, mjtNum fy, mjtNum cx, mjtNum cy, + int projection, mjtNum ortho_extent); + // ----------------------------- Flex interpolation ------------------------------------------------ // evaluate the deformation gradient at p using the nodal dof values diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 9253c260..fcb2c8d8 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -2478,22 +2478,64 @@ static void addRangefinderGeoms(const mjModel* m, mjData* d, const mjvOption* vo for (int i=0; i < m->nsensor; i++) { if (m->sensor_type[i] == mjSENS_RANGEFINDER) { - // sensor data - mjtNum dst = d->sensordata[m->sensor_adr[i]]; - int sid = m->sensor_objid[i]; + int objid = m->sensor_objid[i]; + int adr = m->sensor_adr[i]; - // null output: nothing to render - if (dst < 0) { - continue; + // site-attached rangefinder + if (m->sensor_objtype[i] == mjOBJ_SITE) { + mjtNum dst = d->sensordata[adr]; + + // null output: nothing to render + if (dst < 0) { + continue; + } + + // make ray + mjtNum* from = d->site_xpos+3*objid; + mjtNum to[3] = {from[0] + d->site_xmat[9*objid+2]*dst, + from[1] + d->site_xmat[9*objid+5]*dst, + from[2] + d->site_xmat[9*objid+8]*dst}; + addConnector(scn, mjGEOM_LINE, 3, from, to, m->vis.rgba.rangefinder, + i, mjCAT_DECOR, mjOBJ_SENSOR); } - // make ray - mjtNum* from = d->site_xpos+3*sid; - mjtNum to[3] = {from[0] + d->site_xmat[9*sid+2]*dst, - from[1] + d->site_xmat[9*sid+5]*dst, - from[2] + d->site_xmat[9*sid+8]*dst}; - addConnector(scn, mjGEOM_LINE, 3, from, to, m->vis.rgba.rangefinder, - i, mjCAT_DECOR, mjOBJ_SENSOR); + // camera-attached rangefinder + else if (m->sensor_objtype[i] == mjOBJ_CAMERA) { + const int width = m->cam_resolution[2*objid]; + const int height = m->cam_resolution[2*objid+1]; + const mjtNum* cam_xpos = d->cam_xpos + 3*objid; + const mjtNum* cam_xmat = d->cam_xmat + 9*objid; + const int projection = m->cam_projection[objid]; + + // compute focal length in pixels using helper + mjtNum fx, fy, cx, cy, ortho_extent; + mju_camIntrinsics(m, objid, &fx, &fy, &cx, &cy, &ortho_extent); + + // draw ray for each pixel + for (int row = 0; row < height; row++) { + for (int col = 0; col < width; col++) { + int idx = row*width + col; + mjtNum dst = d->sensordata[adr + idx]; + + // null output: nothing to render + if (dst < 0) { + continue; + } + + // compute ray origin and direction + mjtNum origin[3], direction[3]; + mju_camPixelRay(origin, direction, cam_xpos, cam_xmat, + col, row, fx, fy, cx, cy, projection, ortho_extent); + + // compute endpoint + mjtNum to[3]; + mju_addScl3(to, origin, direction, dst); + + addConnector(scn, mjGEOM_LINE, 3, origin, to, m->vis.rgba.rangefinder, + i, mjCAT_DECOR, mjOBJ_SENSOR); + } + } + } } else if (m->sensor_type[i] == mjSENS_GEOMFROMTO) { // sensor data mjtNum* fromto = d->sensordata + m->sensor_adr[i]; diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 257bb453..5baa520b 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -1252,7 +1252,6 @@ void mjs_deleteUserValue(mjsElement* element, const char* key) { int mjs_sensorDim(const mjsSensor* sensor) { switch (sensor->type) { case mjSENS_TOUCH: - case mjSENS_RANGEFINDER: case mjSENS_JOINTPOS: case mjSENS_JOINTVEL: case mjSENS_TENDONPOS: @@ -1275,6 +1274,15 @@ int mjs_sensorDim(const mjsSensor* sensor) { case mjSENS_CLOCK: return 1; + case mjSENS_RANGEFINDER: + if (sensor->objtype == mjOBJ_CAMERA) { + const mjCCamera* camera = static_cast( + static_cast(sensor->element)->get_obj()); + return static_cast(camera->spec.resolution[0]) * + static_cast(camera->spec.resolution[1]); + } + return 1; // site-attached: single ray + case mjSENS_CAMPROJECTION: return 2; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 3165e34f..29487a9d 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -7313,7 +7313,6 @@ void mjCSensor::Compile(void) { case mjSENS_FORCE: case mjSENS_TORQUE: case mjSENS_MAGNETOMETER: - case mjSENS_RANGEFINDER: case mjSENS_CAMPROJECTION: // must be attached to site if (objtype != mjOBJ_SITE) { @@ -7329,6 +7328,13 @@ void mjCSensor::Compile(void) { } break; + case mjSENS_RANGEFINDER: + // must be attached to site or camera + if (objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) { + throw mjCError(this, "sensor must be attached to site or camera"); + } + break; + case mjSENS_JOINTPOS: case mjSENS_JOINTVEL: case mjSENS_JOINTACTFRC: diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 39162738..294e71d4 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", "cutoff", "noise", "user"}, + {"rangefinder", "*", "name", "site", "camera", "cutoff", "noise", "user"}, {"jointpos", "*", "name", "joint", "cutoff", "noise", "user"}, {"jointvel", "*", "name", "joint", "cutoff", "noise", "user"}, {"tendonpos", "*", "name", "tendon", "cutoff", "noise", "user"}, @@ -4052,8 +4052,12 @@ void mjXReader::Sensor(XMLElement* section) { sensor->reftype = mjOBJ_CAMERA; } else if (type == "rangefinder") { sensor->type = mjSENS_RANGEFINDER; - sensor->objtype = mjOBJ_SITE; - ReadAttrTxt(elem, "site", objname, true); + bool use_site = ReadAttrTxt(elem, "site", objname, false); + bool use_camera = ReadAttrTxt(elem, "camera", objname, false); + if (use_site == use_camera) { + throw mjXError(elem, "rangefinder requires exactly one of 'site' or 'camera'"); + } + sensor->objtype = use_site ? mjOBJ_SITE : mjOBJ_CAMERA; } // sensors related to scalar joints, tendons, actuators diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index f3c29330..b373a867 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -2029,7 +2029,11 @@ void mjXWriter::Sensor(XMLElement* root) { break; case mjSENS_RANGEFINDER: elem = InsertEnd(section, "rangefinder"); - WriteAttrTxt(elem, "site", sensor->get_objname()); + if (sensor->objtype == mjOBJ_SITE) { + WriteAttrTxt(elem, "site", sensor->get_objname()); + } else { + WriteAttrTxt(elem, "camera", sensor->get_objname()); + } break; case mjSENS_CAMPROJECTION: elem = InsertEnd(section, "camprojection"); diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index ab43f1e4..0ef875b4 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -67,7 +67,7 @@ static vector GetSensor(const mjModel* model, using SensorTest = MujocoTest; -// --------------------- test sensor disableflag ------------------------------ +// --------------------- test sensor disable flag ------------------------------ TEST_F(SensorTest, DisableSensors) { constexpr char xml[] = R"( @@ -1052,5 +1052,95 @@ TEST_F(SensorTest, InsideSite) { mj_deleteModel(model); } +TEST_F(SensorTest, RangefinderCamera) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + // sensordata dimension should be 3x3 + 3x3 = 18 + EXPECT_EQ(model->nsensordata, 18); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + mjtNum tol = 1e-6; + mjtNum height = 2.0; + mjtNum fy = 1.5; + mjtNum offsets[3] = {-1.0, 0.0, 1.0}; // pixel center - principal point + + // perspective camera: rays diverge, distance varies with angle + for (int row = 0; row < 3; row++) { + for (int col = 0; col < 3; col++) { + int idx = row * 3 + col; + mjtNum dx = offsets[col] / fy; + mjtNum dy = offsets[row] / fy; + mjtNum expected = height * mju_sqrt(1 + dx*dx + dy*dy); + EXPECT_NEAR(data->sensordata[idx], expected, tol) + << "perspective pixel (" << row << ", " << col << ")"; + } + } + + // orthographic camera: tilted 45 degrees around Y axis + // rays are parallel at 45 degrees, distance depends on pixel x-offset + // for center pixel at (0,0,2): distance = 2 / cos(45) = 2*sqrt(2) + // for off-center pixels: x-offset shifts origin, affecting where ray hits z=0 + mjtNum extent = 2.0; // fovy for orthographic + mjtNum half_extent = extent / 2; + mjtNum fx = 1.5; // width / 2 for 3x3 image + mjtNum cx = 1.5; // principal point + mjtNum cos45 = mju_sqrt(0.5); + mjtNum sin45 = mju_sqrt(0.5); + for (int row = 0; row < 3; row++) { + for (int col = 0; col < 3; col++) { + int idx = 9 + row * 3 + col; // offset by first sensor's 9 values + + // pixel offset in camera frame: matches mju_camPixelRay formula + mjtNum px_cam = (col + 0.5 - cx) / fx * half_extent; + + // camera tilted 45° around Y: local +X maps to world (+cos45, 0, -sin45) + mjtNum origin_z = height - px_cam * sin45; + + // ray hits z=0 plane: distance = origin_z / cos45 + mjtNum expected = origin_z / cos45; + EXPECT_NEAR(data->sensordata[idx], expected, tol) + << "orthographic pixel (" << row << ", " << col << ")"; + } + } + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SensorTest, RFCamera) { + const string xml_path = + GetTestDataFilePath("engine/testdata/sensor/rfcamera.xml"); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + mjData* data = mj_makeData(model); + mj_step(model, data); + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco diff --git a/test/engine/testdata/sensor/rfcamera.xml b/test/engine/testdata/sensor/rfcamera.xml new file mode 100644 index 00000000..af3e3210 --- /dev/null +++ b/test/engine/testdata/sensor/rfcamera.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +