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

Breaking change: Add surface normal output to MuJoCo raycast functions.

Browse Source 
PiperOrigin-RevId: 855781592
Change-Id: Id96b1ca7eaf722e260cc69d7706c28dc51f52d92
This commit is contained in:
Yuval Tassa
2026-01-13 10:21:31 -08:00
committed by Copybara-Service
parent 37762e3f70
commit 218226fc95
17 changed files with 457 additions and 326 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_ray_test.cc
+32 -27
View File
@@ -100,7 +100,7 @@ TEST_F(RayTest, NoExclusions) {
mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid);
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
EXPECT_FLOAT_EQ(distance, 0.9);
mj_deleteData(data);
@@ -123,26 +123,26 @@ TEST_F(RayTest, Exclusions) {
mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid);
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
EXPECT_FLOAT_EQ(distance, 0.9);
// Exclude nearest geom
geomgroup[1] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid);
&geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
EXPECT_FLOAT_EQ(distance, 2.9);
geomgroup[0] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid);
&geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group2");
EXPECT_FLOAT_EQ(distance, 4.9);
geomgroup[2] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid);
&geomid, nullptr);
EXPECT_EQ(geomid, -1);
EXPECT_FLOAT_EQ(distance, -1);
@@ -166,7 +166,7 @@ TEST_F(RayTest, ExcludeStatic) {
mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid);
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
EXPECT_FLOAT_EQ(distance, 2.9);
mj_deleteData(data);
@@ -206,7 +206,7 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
mjtNum dist_multiray[N*M];
int rgeomid_multiray[N*M];
mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, dist_multiray,
N * M, mjMAXVAL);
nullptr, N * M, mjMAXVAL);
// compare results with single ray function
mjtNum dist;
@@ -215,7 +215,7 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
int idx = i * M + j;
dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid);
dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, nullptr);
EXPECT_FLOAT_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
nhits += dist >= 0;
@@ -258,8 +258,8 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
mjtNum dist_multiray[N*M];
int rgeomid_multiray[N*M];
mjtNum normal_multiray[3*N*M];
mj_multiRayNormal(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray,
dist_multiray, normal_multiray, N * M, mjMAXVAL);
mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray,
dist_multiray, normal_multiray, N * M, mjMAXVAL);
// compare results with single ray normal function
mjtNum dist;
@@ -269,8 +269,8 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
int idx = i * M + j;
dist = mj_rayNormal(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
normal);
dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
normal);
EXPECT_FLOAT_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
EXPECT_FLOAT_EQ(normal[0], normal_multiray[3*idx]);
@@ -308,7 +308,8 @@ TEST_F(RayTest, EdgeCases) {
EXPECT_FLOAT_EQ(geom_ba[2], mjPI);
EXPECT_FLOAT_EQ(geom_ba[3], mjPI);
mjtNum vec1[] = {1, 0, 0};
mj_multiRay(m, d, pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, 1, mjMAXVAL);
mj_multiRay(m, d, pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.1);
// pnt at phi = Pi, -Pi
@@ -317,7 +318,8 @@ TEST_F(RayTest, EdgeCases) {
EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); // atan(y<0, x<0)
EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0)
mjtNum vec2[] = {-1, 0, 0};
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, 1, mjMAXVAL);
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.4);
// with cutoff
@@ -326,7 +328,8 @@ TEST_F(RayTest, EdgeCases) {
EXPECT_EQ(flags[0], 0);
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff2, geom_ba, flags);
EXPECT_EQ(flags[0], 1);
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, 1, cutoff2);
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
cutoff2);
EXPECT_FLOAT_EQ(dist, -1);
// pnt on the boundary of the box
@@ -335,7 +338,8 @@ TEST_F(RayTest, EdgeCases) {
EXPECT_FLOAT_EQ(geom_ba[1], 0);
EXPECT_FLOAT_EQ(geom_ba[3], mjPI);
mjtNum vec3[] = {1, 1, 0};
mj_multiRay(m, d, pnt3, vec3, NULL, 1, -1, &rgeomid, &dist, 1, mjMAXVAL);
mj_multiRay(m, d, pnt3, vec3, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
EXPECT_FLOAT_EQ(dist, -1);
// size 0 geom
@@ -346,14 +350,16 @@ TEST_F(RayTest, EdgeCases) {
// 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 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);
mj_multiRay(m, d, pnt4, vec4, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.9);
mj_deleteData(d);
@@ -457,7 +463,7 @@ void _rayMeshTest(const mjModel* m) {
for (int j = 0; j < M; ++j) {
int idx = i * M + j;
dist_old = _rayMesh(m, d, /*geomid=*/0, pnt, vec + 3 * idx);
dist_new = mj_rayMesh(m, d, /*geomid=*/0, pnt, vec + 3 * idx);
dist_new = mj_rayMesh(m, d, /*geomid=*/0, pnt, vec + 3 * idx, nullptr);
EXPECT_FLOAT_EQ(dist_new, dist_old);
}
}
@@ -585,16 +591,16 @@ TEST_F(RayTest, RayNormal) {
mjtNum r, normal[3];
if (!is_flex) {
int geomid;
r = mj_rayNormal(m, d, pnt, vec, nullptr, 1, -1, &geomid, normal);
r = mj_ray(m, d, pnt, vec, nullptr, 1, -1, &geomid, normal);
// compare with sensor, expect geomid to be 0
EXPECT_EQ(r, d->sensordata[0]) << path << ", time " << d->time;
EXPECT_EQ(geomid, r >= 0 ? 0 : -1);
} else {
r = mju_rayFlexNormal(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1,
/*flg_edge*/ 1, /*flg_face*/ 1,
/*flg_skin*/ 1, /*flex_id*/ 0,
pnt, vec, nullptr, normal);
r = mj_rayFlex(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1,
/*flg_edge*/ 1, /*flg_face*/ 1,
/*flg_skin*/ 1, /*flex_id*/ 0,
pnt, vec, nullptr, normal);
// no sensor comparison: rangefinders only intersect with geoms
}
@@ -619,10 +625,9 @@ TEST_F(RayTest, RayNormal) {
mjtNum dr, dpnt[3];
mju_addScl3(dpnt, pnt, nudge, eps);
if (!is_flex) {
dr = mj_rayNormal(m, d, dpnt, vec, NULL, 1, -1, nullptr, nullptr);
dr = mj_ray(m, d, dpnt, vec, NULL, 1, -1, nullptr, nullptr);
} else {
dr = mju_rayFlexNormal(m, d, 0, 1, 1, 1, 1, 0, dpnt, vec, nullptr,
nullptr);
dr = mj_rayFlex(m, d, 0, 1, 1, 1, 1, 0, dpnt, vec, nullptr, nullptr);
}
mju_addScl3(ds[i], dpnt, vec, dr);
}