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

Fix a box-box bad collision in NativeCCD.

Browse Source 
PiperOrigin-RevId: 711773153
Change-Id: I8a526ef5933be19d863e67f40edeb95b0b1249cd
This commit is contained in:
Kyle Bayes
2025-01-03 09:21:24 -08:00
committed by Copybara-Service
parent 63e2836f63
commit 340c0d7c97
3 changed files with 62 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+15 -9
View File
@@ -890,7 +890,12 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
if (mju_abs(det3(v1, v3, v4)) < mjMINVAL || mju_abs(det3(v1, v3, v5)) < mjMINVAL ||
mju_abs(det3(v1, v3, v5)) < mjMINVAL || mju_abs(det3(v2, v3, v4)) < mjMINVAL ||
mju_abs(det3(v2, v3, v5)) < mjMINVAL || mju_abs(det3(v2, v4, v5)) < mjMINVAL) {
return 2;
return mjEPA_P2_INVALID_FACES;
}
// check that origin is in the hexahedron
if (!testTetra(v1, v3, v4, v5) && !testTetra(v2, v3, v4, v5)) {
return mjEPA_P2_MISSING_ORIGIN;
}
// save vertices and get indices for each one
@@ -914,7 +919,7 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
if (pt->faces[i].dist < mjMINVAL) {
return 3;
return mjEPA_P2_ORIGIN_ON_FACE;
}
}
pt->nmap = 6;
@@ -1003,7 +1008,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
cross3(n, diff1, diff2);
mjtNum n_norm = mju_norm3(n);
if (n_norm < mjMINVAL) {
return 4;
return mjEPA_P3_BAD_NORMAL;
}
// negative of triangle normal n
@@ -1016,7 +1021,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
// check that v4 is not contained in the 2-simplex
if (triPointIntersect(v1, v2, v3, v4)) {
return 5;
return mjEPA_P3_INVALID_V4;
}
// get 5th vertex in -n direction
@@ -1026,7 +1031,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
// check that v5 is not contained in the 2-simplex
if (triPointIntersect(v1, v2, v3, v5)) {
return 6;
return mjEPA_P3_INVALID_V5;
}
// if origin does not lie on simplex then we need to check that the hexahedron contains the
@@ -1036,7 +1041,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
// it but within tolerance from it. In that case the hexahedron could possibly be constructed
// that doesn't contain the origin, but nonetheless there is penetration depth.
if (status->dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) {
return 7;
return mjEPA_P3_MISSING_ORIGIN;
}
// save vertices and get indices for each one
@@ -1061,7 +1066,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
if (pt->faces[i].dist < mjMINVAL) {
return 8;
return mjEPA_P3_ORIGIN_ON_FACE;
}
}
pt->nmap = 6;
@@ -1444,7 +1449,8 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
obj1->center(status->x1, obj1);
obj2->center(status->x2, obj2);
status->gjk_iterations = 0;
status->epa_iterations = -1;
status->epa_iterations = 0;
status->epa_status = mjEPA_NOCONTACT;
status->tolerance = config->tolerance;
status->max_iterations = config->max_iterations;
status->max_contacts = config->max_contacts;
@@ -1551,12 +1557,12 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
} else {
ret = polytope4(&pt, status, obj1, obj2);
}
status->epa_status = ret;
// simplex not on boundary (objects are penetrating)
if (!ret) {
dist = -epa(status, &pt, obj1, obj2);
} else {
status->epa_iterations = -ret;
dist = 0;
}
mj_freeStack(d);
src/engine/engine_collision_gjk.h
+20 -7
View File
@@ -25,17 +25,30 @@
extern "C" {
#endif
// Status of an EPA run
typedef enum {
mjEPA_NOCONTACT = -1,
mjEPA_SUCCESS = 0,
mjEPA_P2_INVALID_FACES,
mjEPA_P2_MISSING_ORIGIN,
mjEPA_P2_ORIGIN_ON_FACE,
mjEPA_P3_BAD_NORMAL,
mjEPA_P3_INVALID_V4,
mjEPA_P3_INVALID_V5,
mjEPA_P3_MISSING_ORIGIN,
mjEPA_P3_ORIGIN_ON_FACE,
} mjEPAStatus;
// configuration for convex collision detection
struct _mjCCDConfig {
typedef struct {
int max_iterations; // the maximum number of iterations for GJK and EPA
mjtNum tolerance; // tolerance used by GJK and EPA
int max_contacts; // set to max number of contact points to recover
mjtNum dist_cutoff; // set to max geom distance to recover
};
typedef struct _mjCCDConfig mjCCDConfig;
} mjCCDConfig;
// data produced from running GJK and EPA
struct _mjCCDStatus {
typedef struct {
// geom distance information
mjtNum dist; // distance between geoms
mjtNum x1[3 * mjMAXCONPAIR]; // witness points for geom 1
@@ -50,13 +63,13 @@ struct _mjCCDStatus {
// statistics for debugging purposes
int gjk_iterations; // number of iterations that GJK ran
int epa_iterations; // number of iterations that EPA ran (negative if EPA did not run)
int epa_iterations; // number of iterations that EPA ran (zero if EPA did not run)
mjEPAStatus epa_status; // status of the EPA run
mjtNum simplex1[12]; // the simplex that GJK returned for obj1
mjtNum simplex2[12]; // the simplex that GJK returned for obj2
mjtNum simplex[12]; // the simplex that GJK returned for the Minkowski difference
int nsimplex; // size of simplex 1 & 2
};
typedef struct _mjCCDStatus mjCCDStatus;
} mjCCDStatus;
// run general convex collision detection, returns positive for distance, negative for penetration
MJAPI mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2);
test/engine/engine_collision_gjk_test.cc
+27
View File
@@ -335,6 +335,33 @@ TEST_F(MjGjkTest, BoxBoxDepth2) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBoxTouching) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 1.859913200000001376466229885409" size="1 1 1"/>
<geom name="geom2" type="box" pos="0 2 1.859913200000001376466229885409" size="1 1 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dir[3], pos[3];
mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
EXPECT_EQ(dist, mjMAXVAL);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, SmallBoxMesh) {
static constexpr char xml[] = R"(
<mujoco>