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

Add tolerance to upper > lower bound check on first iteration of EPA.

Browse Source 
PiperOrigin-RevId: 933615381
Change-Id: I175d3b97fa65f71a8f1cc34da5f6b9a23ab856d2
This commit is contained in:
Kyle Bayes
2026-06-17 03:18:29 -07:00
committed by Copybara-Service
parent f6fb4af091
commit 386b45fb75
2 changed files with 46 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+1 -1
View File
@@ -1394,7 +1394,7 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
}
if (upper - lower < tolerance) {
// terminate without contact when upper < lower on first iteration
if (k == 0 && upper < lower) {
if (k == 0 && upper < lower - 1e-10) {
face = NULL;
}
break;
test/engine/engine_collision_gjk_test.cc
+45
View File
@@ -1954,6 +1954,51 @@ TEST_F(MjGjkTest, BoxBox) {
EXPECT_EQ(dist, 1);
}
TEST_F(MjGjkTest, BoxBoxLarge) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 0.25" size="50 50 0.25"/>
<geom name="geom2" type="box" pos="0 0 0.60" size="0.1 0.1 0.1"/>
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
mjtNum* xmat = data->geom_xmat;
mjtNum* xpos = data->geom_xpos;
xpos = data->geom_xpos + 3;
xmat = data->geom_xmat + 9;
xpos[0] = -0.000000000043537;
xpos[1] = -0.000000000012973;
xpos[2] = 0.599245786666870;
xmat[0] = 1.000000000000000;
xmat[1] = -0.000000000004566;
xmat[2] = -0.000000000167641;
xmat[3] = 0.000000000004566;
xmat[4] = 1.000000000000000;
xmat[5] = -0.000000000017877;
xmat[6] = 0.000000000167641;
xmat[7] = 0.000000000017877;
xmat[8] = 1.000000000000000;
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncon = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
EXPECT_NEAR(dist, -0.000754, kTolerance);
EXPECT_EQ(ncon, 4);
}
TEST_F(MjGjkTest, LongBox) {
static constexpr char xml[] = R"(
<mujoco>