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

Tune constants for nativeccd to perform better with single precision.

Browse Source 
PiperOrigin-RevId: 928549147
Change-Id: Ic28c60f73e92bb1b9578c56f790b6cda94875907
This commit is contained in:
Kyle Bayes
2026-06-08 07:20:36 -07:00
committed by Copybara-Service
parent a0f014593b
commit ac7f198536
3 changed files with 68 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+37 -21
View File
@@ -28,6 +28,23 @@
#define mjMINVAL2 (mjMINVAL * mjMINVAL)
#define mjMAXVAL2 (mjMAXVAL * mjMAXVAL)
// numerical limits for single and double precision
#ifdef mjUSESINGLE
// minimal distance squared for origin inside tetrahedron in polytope2
#define mjMINDIST2 1e-10f
// minimal distance squared for origin inside tetrahedron in polytope3
#define mjMINDIST3 1e-10f
// minimal distance squared for origin inside tetrahedron in polytope4
#define mjMINDIST4 1e-17f
// minimal tolerance for EPA
#define mjMINEPATOL 1e-7f
#else
#define mjMINDIST2 mjMINVAL2
#define mjMINDIST3 mjMINVAL2
#define mjMINDIST4 mjMINVAL2
#define mjMINEPATOL mjMINVAL
#endif
// align memory size on 8-byte boundary; needed for single precision
static inline size_t align8(size_t size) {
return ((size + 7) / 8) * 8;
@@ -950,27 +967,27 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
mjtNum* v5 = pt->verts[v5i].vert;
// build hexahedron
if (attachFace(pt, v1i, v3i, v4i, 1, 3, 2) < mjMINVAL2) {
if (attachFace(pt, v1i, v3i, v4i, 1, 3, 2) < mjMINDIST2) {
replaceSimplex3(pt, status, v1i, v3i, v4i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1i, v5i, v3i, 2, 4, 0) < mjMINVAL2) {
if (attachFace(pt, v1i, v5i, v3i, 2, 4, 0) < mjMINDIST2) {
replaceSimplex3(pt, status, v1i, v5i, v3i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1i, v4i, v5i, 0, 5, 1) < mjMINVAL2) {
if (attachFace(pt, v1i, v4i, v5i, 0, 5, 1) < mjMINDIST2) {
replaceSimplex3(pt, status, v1i, v4i, v5i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v2i, v4i, v3i, 5, 0, 4) < mjMINVAL2) {
if (attachFace(pt, v2i, v4i, v3i, 5, 0, 4) < mjMINDIST2) {
replaceSimplex3(pt, status, v2i, v4i, v3i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v2i, v3i, v5i, 3, 1, 5) < mjMINVAL2) {
if (attachFace(pt, v2i, v3i, v5i, 3, 1, 5) < mjMINDIST2) {
replaceSimplex3(pt, status, v2i, v3i, v5i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v2i, v5i, v4i, 4, 2, 3) < mjMINVAL2) {
if (attachFace(pt, v2i, v5i, v4i, 4, 2, 3) < mjMINDIST2) {
replaceSimplex3(pt, status, v2i, v5i, v4i);
return polytope3(pt, status, obj1, obj2);
}
@@ -1103,22 +1120,22 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
}
// create hexahedron for EPA
if (attachFace(pt, v4i, v1i, v2i, 1, 3, 2) < mjMINVAL2) {
if (attachFace(pt, v4i, v1i, v2i, 1, 3, 2) < mjMINDIST3) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
if (attachFace(pt, v4i, v3i, v1i, 2, 4, 0) < mjMINVAL2) {
if (attachFace(pt, v4i, v3i, v1i, 2, 4, 0) < mjMINDIST3) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
if (attachFace(pt, v4i, v2i, v3i, 0, 5, 1) < mjMINVAL2) {
if (attachFace(pt, v4i, v2i, v3i, 0, 5, 1) < mjMINDIST3) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
if (attachFace(pt, v5i, v2i, v1i, 5, 0, 4) < mjMINVAL2) {
if (attachFace(pt, v5i, v2i, v1i, 5, 0, 4) < mjMINDIST3) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
if (attachFace(pt, v5i, v1i, v3i, 3, 1, 5) < mjMINVAL2) {
if (attachFace(pt, v5i, v1i, v3i, 3, 1, 5) < mjMINDIST3) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
if (attachFace(pt, v5i, v3i, v2i, 4, 2, 3) < mjMINVAL2) {
if (attachFace(pt, v5i, v3i, v2i, 4, 2, 3) < mjMINDIST3) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
@@ -1140,23 +1157,24 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
int v4 = insertVertex(pt, status->simplex + 3);
// if the origin is on a face, replace the 3-simplex with a 2-simplex
if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINVAL2) {
if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINDIST4) {
replaceSimplex3(pt, status, v1, v2, v3);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1, v4, v2, 2, 3, 0) < mjMINVAL2) {
if (attachFace(pt, v1, v4, v2, 2, 3, 0) < mjMINDIST4) {
replaceSimplex3(pt, status, v1, v4, v2);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1, v3, v4, 0, 3, 1) < mjMINVAL2) {
if (attachFace(pt, v1, v3, v4, 0, 3, 1) < mjMINDIST4) {
replaceSimplex3(pt, status, v1, v3, v4);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v4, v3, v2, 2, 0, 1) < mjMINVAL2) {
if (attachFace(pt, v4, v3, v2, 2, 0, 1) < mjMINDIST4) {
replaceSimplex3(pt, status, v4, v3, v2);
return polytope3(pt, status, obj1, obj2);
}
// numerically verify that the origin lies in the tetrahedron interior
if (!testTetra(pt->verts[v1].vert, pt->verts[v2].vert, pt->verts[v3].vert, pt->verts[v4].vert)) {
return mjEPA_P4_MISSING_ORIGIN;
}
@@ -1311,13 +1329,11 @@ static mjtNum epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjt
static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum upper = mjMAX_LIMIT, upper2 = mjMAX_LIMIT, lower2;
Face* face = NULL, *pface = NULL; // face closest to origin
mjtNum tolerance = status->tolerance;
int discrete = discreteGeoms(obj1, obj2);
// tolerance is not used for discrete geoms
if (discrete && sizeof(mjtNum) == sizeof(double)) {
tolerance = mjMINVAL;
}
// discrete geoms return in a finite number of iterations, a non-zero tolerance avoids
// absurdly small lower and upper bounds
mjtNum tolerance = discrete ? mjMINEPATOL : status->tolerance;
int k, kmax = status->max_iterations < 1000 ? status->max_iterations : 1000;
for (k = 0; k < kmax; k++) {
src/engine/engine_collision_gjk.h
+12 -10
View File
@@ -28,19 +28,21 @@
extern "C" {
#endif
// numerical max limit
#ifndef mjUSESINGLE
#define mjMAX_LIMIT DBL_MAX
#else
// numerical limits
#ifdef mjUSESINGLE
#define mjMAX_LIMIT FLT_MAX
// tolerance for normal alignment of two faces (cosine of 1.6e-3)
#define mjFACE_TOL 0.99999872f
// tolerance for edge-face alignment (sine of 1.6e-3)
#define mjEDGE_TOL 0.00159999931f
#else
#define mjMAX_LIMIT DBL_MAX
// tolerance for normal alignment of two faces (cosine of 1.6e-3)
#define mjFACE_TOL 0.99999872
// tolerance for edge-face alignment (sine of 1.6e-3)
#define mjEDGE_TOL 0.00159999931
#endif
// tolerance for normal alignment of two faces (cosine of 1.6e-3)
#define mjFACE_TOL 0.99999872
// tolerance for edge-face alignment (sine of 1.6e-3)
#define mjEDGE_TOL 0.00159999931
// max number of supported vertices in a polygon face of a mesh
#define mjMAX_POLYVERT 150
test/engine/engine_collision_gjk_test.cc
+19 -22
View File
@@ -1049,8 +1049,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD11) {
mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
ASSERT_EQ(ncons, 4);
// contact unrecoverable under single precision
ASSERT_EQ(ncons, sizeof(mjtNum) == 8 ? 4 : 0);
}
TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
@@ -1228,9 +1228,7 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD14) {
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
ASSERT_EQ(ncons, 4);
}
TEST_F(MjGjkTest, SmallBoxMesh) {
@@ -1275,19 +1273,23 @@ TEST_F(MjGjkTest, SmallBoxMesh) {
mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2);
ASSERT_EQ(ncons, 1);
EXPECT_NEAR(dist, 0, kTolerance);
// contact unrecoverable under single precision
ASSERT_EQ(ncons, sizeof(mjtNum) == 8 ? 1 : 0);
if (ncons) {
EXPECT_NEAR(dist, 0, kTolerance);
// direction
EXPECT_NEAR(dir[0], 0, kTolerance);
EXPECT_NEAR(dir[1], 0, kTolerance);
EXPECT_NEAR(dir[2], 1, kTolerance);
// direction
EXPECT_NEAR(dir[0], 0, kTolerance);
EXPECT_NEAR(dir[1], 0, kTolerance);
EXPECT_NEAR(dir[2], 1, kTolerance);
// position
EXPECT_NEAR(pos[0], 0, kTolerance);
EXPECT_NEAR(pos[1], 0, kTolerance);
EXPECT_NEAR(pos[2], 0, kTolerance);
// position
EXPECT_NEAR(pos[0], 0, kTolerance);
EXPECT_NEAR(pos[1], 0, kTolerance);
EXPECT_NEAR(pos[2], 0, kTolerance);
}
}
TEST_F(MjGjkTest, BoxMesh) {
static constexpr char xml[] = R"(
<mujoco>
@@ -1801,18 +1803,13 @@ static constexpr char xml[] = R"(
ASSERT_EQ(ncons, 1);
EXPECT_NEAR(dist, -0.01, kTolerance);
EXPECT_NEAR(dir[0], 0, kTolerance);
EXPECT_NEAR(dir[1], 0, kTolerance);
EXPECT_NEAR(dir[2], 1, kTolerance);
EXPECT_NEAR(pos[0], 0, kTolerance);
EXPECT_NEAR(pos[1], 0, kTolerance);
EXPECT_NEAR(pos[2], -0.005, kTolerance);
EXPECT_THAT(dir[0], MjNear(0, kTolerance, 1e-5));
EXPECT_THAT(dir[1], MjNear(0, kTolerance, 1e-5));
EXPECT_THAT(dir[2], MjNear(1, kTolerance, kTolerance));
// multicontact
ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
ASSERT_EQ(ncons, 4);
}
TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {