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

Fix multiccd edge bug that occurs in certain situations.

Browse Source 
PiperOrigin-RevId: 922251109
Change-Id: I74c08ae49d5dfd2a41b2dcd28eb9f2b5ba113216
This commit is contained in:
Kyle Bayes
2026-05-27 11:31:53 -07:00
committed by Copybara-Service
parent f6c85287bf
commit 4035c66428
2 changed files with 47 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+9 -1
View File
@@ -2169,8 +2169,16 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
// face1 is an edge; clip face1 against face2
if (edgecon1) {
scl3(approx_dir, n2 + 3*j, norm3(dir));
scl3(approx_dir, n2 + 3*j, -norm3(dir));
polygonClip(status, face2, nface2, face1, nface1, n2 + 3*j, approx_dir);
// x1 and x2 must be flipped as we flipped the faces in polygonClip
int nx = status->nx;
for (int k = 0; k < nx; k++) {
mjtNum tmp[3];
copy3(tmp, status->x1 + 3*k);
copy3(status->x1 + 3*k, status->x2 + 3*k);
copy3(status->x2 + 3*k, tmp);
}
return;
}
test/engine/engine_collision_gjk_test.cc
+38
View File
@@ -2028,5 +2028,43 @@ TEST_F(MjGjkTest, CylinderBoxMargin) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxEdgeFlipped) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" pos="1.10164554 -0.11389316 0.74"
quat="-0.348312918 0 0 0.937378318" type="box" size="0.65 0.48 0.04"/>
<geom name="geom2" type="box" size="0.1 1.2 1.4" pos="1.4 0 1.425"/>
</worldbody>
</mujoco>)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error;
mjData* data = mj_makeData(model);
mj_forward(model, data);
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_EQ(ncons, 2);
EXPECT_NEAR(status.x1[0], 1.907368, kTolerance);
EXPECT_NEAR(status.x1[1], -0.052973, kTolerance);
EXPECT_NEAR(status.x1[2], 0.700000, kTolerance);
EXPECT_NEAR(status.x2[0], 1.30000, kTolerance);
EXPECT_NEAR(status.x2[1], -0.052973, kTolerance);
EXPECT_NEAR(status.x2[2], 0.700000, kTolerance);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco