chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 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
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>