chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
PiperOrigin-RevId: 848316991
Change-Id: Idbf7ba81b4da711a22c23302c8782ab2b0b98d82
This commit is contained in:
Yuval Tassa
2025-12-23 15:31:53 -08:00
committed by Copybara-Service
parent f2e9097ed6
commit 70bc7be4bc
27 changed files with 712 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+34
View File
@@ -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
+2 -2
View File
@@ -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
+10 -2
View File
@@ -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/images/XMLreference/rfcamera.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 562 KiB

doc/includes/references.h
+11 -1
View File
@@ -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
+13 -1
View File
@@ -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
+14
View File
@@ -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
+121
View File
@@ -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
+32 -5
View File
@@ -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;
}
}
src/engine/engine_sensor.c
+141 -46
View File
@@ -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);
}
}
}
}
src/engine/engine_support.c
+22
View File
@@ -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
+4
View File
@@ -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
+97 -21
View File
@@ -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);
}
}
}
}
src/user/user_api.cc
+12 -8
View File
@@ -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
+21 -4
View File
@@ -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;
src/xml/xml_base.h
+1
View File
@@ -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
+34 -1
View File
@@ -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
+16 -5
View File
@@ -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:
test/engine/engine_ray_test.cc
+9 -4
View File
@@ -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);
test/engine/engine_sensor_test.cc
+68 -17
View File
@@ -1065,8 +1065,8 @@ TEST_F(SensorTest, RangefinderCamera) {
</worldbody>
<sensor>
<rangefinder camera="persp"/>
<rangefinder camera="ortho"/>
<rangefinder camera="persp" data="dist depth"/>
<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
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);
}
test/engine/testdata/ray/hfield.xml
Vendored
-1
View File
@@ -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
Vendored
+4 -2
View File
@@ -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
Vendored
+27 -12
View File
@@ -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>
<light pos="0 0 3"/>
<statistic meansize="0.17"/>
<!-- ground plane -->
<geom type="plane" size="5 5 .1" rgba=".3 .4 .5 1"/>
<worldbody>
<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="cylinder" pos="-0.8 0.6 .4" size=".4 .4" rgba=".3 .3 1 1"/>
<geom type="capsule" pos="0.5 -0.7 0" size=".25 .35" zaxis="1 0.5 0" rgba="1 1 .3 1"/>
<geom type="mesh" mesh="ss" pos="1 0.5 .35" rgba=".3 1 .3 1" euler="20 0 30"/>
<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 -.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="orthographic"/>
<rangefinder camera="perspective" data="dist point normal"/>
<rangefinder camera="orthographic" data="dist point normal"/>
</sensor>
</mujoco>
test/testdata/model.xml
Vendored
+1 -1
View File
@@ -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
+1
View File
@@ -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
+9
View File
@@ -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
+8
View File
@@ -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)