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

Make polytope2 and polytope4 more numerically robust in nativeccd.

Browse Source 
PiperOrigin-RevId: 723412555
Change-Id: Id262a3c19bcd30b74b1495591bdc95f951bfca50
This commit is contained in:
Kyle Bayes
2025-02-05 01:39:52 -08:00
committed by Copybara-Service
parent 6bd88711a8
commit 3235f69afc
3 changed files with 125 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+62 -46
View File
@@ -81,6 +81,11 @@ static mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj
// status must have initial tetrahedrons
static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2);
// create initial polytope for EPA for a 2, 3, or 4-simplex
static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2);
static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2);
static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2);
// return a face of the expanded polytope that best approximates the pentration depth
// witness points are in status->{x1, x2}
static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2);
@@ -808,8 +813,30 @@ static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]) {
}
}
// ---------------------------------------- EPA ---------------------------------------------------
// replace a 3-simplex with one of its faces
static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, int v2, int v3) {
status->nsimplex = 3;
copy3(status->simplex1 + 0, pt->verts1 + v1);
copy3(status->simplex1 + 3, pt->verts1 + v2);
copy3(status->simplex1 + 6, pt->verts1 + v3);
copy3(status->simplex2 + 0, pt->verts2 + v1);
copy3(status->simplex2 + 3, pt->verts2 + v2);
copy3(status->simplex2 + 6, pt->verts2 + v3);
copy3(status->simplex + 0, pt->verts + v1);
copy3(status->simplex + 3, pt->verts + v2);
copy3(status->simplex + 6, pt->verts + v3);
pt->nfaces = 0;
pt->nverts = 0;
}
// return 1 if the origin and p3 are on the same side of the plane defined by p0, p1, p2
static int sameSide(const mjtNum p0[3], const mjtNum p1[3],
const mjtNum p2[3], const mjtNum p3[3]) {
@@ -861,7 +888,7 @@ static void rotmat(mjtNum R[9], const mjtNum axis[3]) {
// create a polytope from a 1-simplex (returns 0 on success)
static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum v1[3], v2[3];
sub3(v1, status->simplex1 + 0, status->simplex2 + 0);
sub3(v2, status->simplex1 + 3, status->simplex2 + 3);
@@ -905,18 +932,6 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
epaSupport(v5a, v5b, obj1, obj2, d3, mju_norm3(d3));
sub3(v5, v5a, v5b);
// check that all six faces are valid triangles (not collinear)
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 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
int v1i = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0);
int v2i = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3);
@@ -925,21 +940,39 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
int v5i = newVertex(pt, v5a, v5b);
// build hexahedron
attachFace(pt, v1i, v3i, v4i, 1, 3, 2);
attachFace(pt, v1i, v5i, v3i, 2, 4, 0);
attachFace(pt, v1i, v4i, v5i, 0, 5, 1);
attachFace(pt, v2i, v4i, v3i, 5, 0, 4);
attachFace(pt, v2i, v3i, v5i, 3, 1, 5);
attachFace(pt, v2i, v5i, v4i, 4, 2, 3);
if (attachFace(pt, v1i, v3i, v4i, 1, 3, 2) < mjMINVAL) {
replaceSimplex3(pt, status, v1i, v3i, v4i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1i, v5i, v3i, 2, 4, 0) < mjMINVAL) {
replaceSimplex3(pt, status, v1i, v5i, v3i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1i, v4i, v5i, 0, 5, 1) < mjMINVAL) {
replaceSimplex3(pt, status, v1i, v4i, v5i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v2i, v4i, v3i, 5, 0, 4) < mjMINVAL) {
replaceSimplex3(pt, status, v2i, v4i, v3i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v2i, v3i, v5i, 3, 1, 5) < mjMINVAL) {
replaceSimplex3(pt, status, v2i, v3i, v5i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v2i, v5i, v4i, 4, 2, 3) < mjMINVAL) {
replaceSimplex3(pt, status, v2i, v5i, v4i);
return polytope3(pt, status, obj1, obj2);
}
// check that origin is in the hexahedron
if (status->dist > 10*mjMINVAL && !testTetra(v1, v3, v4, v5) && !testTetra(v2, v3, v4, v5)) {
return mjEPA_P2_MISSING_ORIGIN;
}
// if the origin is on the affine hull of any of the faces then the origin is not in the
// hexahedron or the hexahedron is degenerate
for (int i = 0; i < 6; i++) {
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
if (pt->faces[i].dist < mjMINVAL) {
return mjEPA_P2_ORIGIN_ON_FACE;
}
}
pt->nmap = 6;
@@ -1014,7 +1047,7 @@ static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNu
// create a polytope from a 2-simplex (returns 0 on success)
static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
// get vertices of simplex from GJK
const mjtNum *v1 = status->simplex,
*v2 = status->simplex + 3,
@@ -1094,27 +1127,6 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
// replace a 3-simplex with one of its faces
static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, int v2, int v3) {
status->nsimplex = 3;
copy3(status->simplex1 + 0, pt->verts1 + v1);
copy3(status->simplex1 + 3, pt->verts1 + v2);
copy3(status->simplex1 + 6, pt->verts1 + v3);
copy3(status->simplex2 + 0, pt->verts2 + v1);
copy3(status->simplex2 + 3, pt->verts2 + v2);
copy3(status->simplex2 + 6, pt->verts2 + v3);
copy3(status->simplex + 0, pt->verts + v1);
copy3(status->simplex + 3, pt->verts + v2);
copy3(status->simplex + 6, pt->verts + v3);
pt->nfaces = 0;
pt->nverts = 0;
}
// create a polytope from a 3-simplex (returns 0 on success)
static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
int v1 = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0);
@@ -1140,6 +1152,10 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
return polytope3(pt, status, obj1, obj2);
}
if (!testTetra(pt->verts + v1, pt->verts + v2, pt->verts + v3, pt->verts + v4)) {
return mjEPA_P4_MISSING_ORIGIN;
}
for (int i = 0; i < 4; i++) {
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
src/engine/engine_collision_gjk.h
+1
View File
@@ -40,6 +40,7 @@ typedef enum {
mjEPA_P3_INVALID_V5,
mjEPA_P3_MISSING_ORIGIN,
mjEPA_P3_ORIGIN_ON_FACE,
mjEPA_P4_MISSING_ORIGIN,
} mjEPAStatus;
// configuration for convex collision detection
test/engine/engine_collision_gjk_test.cc
+62
View File
@@ -877,6 +877,68 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD8) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBoxMultiCCD9) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size=".025 .025 .025" pos="0 0 0"/>
<geom name="geom2" type="box" size=".025 .025 .025" pos="0 0 0"/>
</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);
mjtNum* xmat = data->geom_xmat;
mjtNum* xpos = data->geom_xpos;
xmat[0] = 1.0000000000000000000000000000000000000000;
xmat[1] = -0.0000000000000000000000000000000000050579;
xmat[2] = 0.0000000000000000001927715439853908006818;
xmat[3] = 0.0000000000000000000000000000000000056403;
xmat[4] = 1.0000000000000000000000000000000000000000;
xmat[5] = -0.0000000000000000030208514688407265124010;
xmat[6] = -0.0000000000000000001927715439853908006818;
xmat[7] = 0.0000000000000000030208514688407265124010;
xmat[8] = 1.0000000000000000000000000000000000000000;
xpos[0] = -0.1071400000000000268807198722242901567370;
xpos[1] = -0.1928599999999999758948376893386011943221;
xpos[2] = 0.1749951524564917204607183975895168259740;
xmat = data->geom_xmat + 9;
xpos = data->geom_xpos + 3;
xmat[0] = 1.0000000000000000000000000000000000000000;
xmat[1] = 0.0000000000000000000000000000000037070001;
xmat[2] = -0.0000000000000000649578747741268744461630;
xmat[3] = 0.0000000000000000000000000000000064174485;
xmat[4] = 1.0000000000000000000000000000000000000000;
xmat[5] = 0.0000000000000001558617582226398399563910;
xmat[6] = 0.0000000000000000649578747741268744461630;
xmat[7] = -0.0000000000000001558617582226398399563910;
xmat[8] = 1.0000000000000000000000000000000000000000;
xpos[0] = -0.1071400000000000268807198722242901567370;
xpos[1] = -0.1928599999999999758948376893386011943221;
xpos[2] = 0.2156259187793853615566774806211469694972;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000);
EXPECT_EQ(ncons, 4);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, SmallBoxMesh) {
static constexpr char xml[] = R"(
<mujoco>