chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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 mjMINVAL2 (mjMINVAL * mjMINVAL)
#define mjMAXVAL2 (mjMAXVAL * mjMAXVAL) #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 // align memory size on 8-byte boundary; needed for single precision
static inline size_t align8(size_t size) { static inline size_t align8(size_t size) {
return ((size + 7) / 8) * 8; 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; mjtNum* v5 = pt->verts[v5i].vert;
// build hexahedron // 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); replaceSimplex3(pt, status, v1i, v3i, v4i);
return polytope3(pt, status, obj1, obj2); 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); replaceSimplex3(pt, status, v1i, v5i, v3i);
return polytope3(pt, status, obj1, obj2); 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); replaceSimplex3(pt, status, v1i, v4i, v5i);
return polytope3(pt, status, obj1, obj2); 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); replaceSimplex3(pt, status, v2i, v4i, v3i);
return polytope3(pt, status, obj1, obj2); 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); replaceSimplex3(pt, status, v2i, v3i, v5i);
return polytope3(pt, status, obj1, obj2); 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); replaceSimplex3(pt, status, v2i, v5i, v4i);
return polytope3(pt, status, obj1, obj2); return polytope3(pt, status, obj1, obj2);
} }
@@ -1103,22 +1120,22 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
} }
// create hexahedron for EPA // 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; 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; 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; 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; 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; 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; 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); int v4 = insertVertex(pt, status->simplex + 3);
// if the origin is on a face, replace the 3-simplex with a 2-simplex // 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); replaceSimplex3(pt, status, v1, v2, v3);
return polytope3(pt, status, obj1, obj2); 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); replaceSimplex3(pt, status, v1, v4, v2);
return polytope3(pt, status, obj1, obj2); 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); replaceSimplex3(pt, status, v1, v3, v4);
return polytope3(pt, status, obj1, obj2); 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); replaceSimplex3(pt, status, v4, v3, v2);
return polytope3(pt, status, obj1, obj2); 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)) { if (!testTetra(pt->verts[v1].vert, pt->verts[v2].vert, pt->verts[v3].vert, pt->verts[v4].vert)) {
return mjEPA_P4_MISSING_ORIGIN; 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) { static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum upper = mjMAX_LIMIT, upper2 = mjMAX_LIMIT, lower2; mjtNum upper = mjMAX_LIMIT, upper2 = mjMAX_LIMIT, lower2;
Face* face = NULL, *pface = NULL; // face closest to origin Face* face = NULL, *pface = NULL; // face closest to origin
mjtNum tolerance = status->tolerance;
int discrete = discreteGeoms(obj1, obj2); int discrete = discreteGeoms(obj1, obj2);
// tolerance is not used for discrete geoms // discrete geoms return in a finite number of iterations, a non-zero tolerance avoids
if (discrete && sizeof(mjtNum) == sizeof(double)) { // absurdly small lower and upper bounds
tolerance = mjMINVAL; mjtNum tolerance = discrete ? mjMINEPATOL : status->tolerance;
}
int k, kmax = status->max_iterations < 1000 ? status->max_iterations : 1000; int k, kmax = status->max_iterations < 1000 ? status->max_iterations : 1000;
for (k = 0; k < kmax; k++) { for (k = 0; k < kmax; k++) {
src/engine/engine_collision_gjk.h
+12 -10
View File
@@ -28,19 +28,21 @@
extern "C" { extern "C" {
#endif #endif
// numerical max limit // numerical limits
#ifndef mjUSESINGLE #ifdef mjUSESINGLE
#define mjMAX_LIMIT DBL_MAX
#else
#define mjMAX_LIMIT FLT_MAX #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 #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 // max number of supported vertices in a polygon face of a mesh
#define mjMAX_POLYVERT 150 #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; mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8); 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) { TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
@@ -1228,9 +1228,7 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD14) {
std::vector<mjtNum> dir, pos; std::vector<mjtNum> dir, pos;
mjtNum dist; mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8); int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
ASSERT_EQ(ncons, 4); ASSERT_EQ(ncons, 4);
} }
TEST_F(MjGjkTest, SmallBoxMesh) { TEST_F(MjGjkTest, SmallBoxMesh) {
@@ -1275,19 +1273,23 @@ TEST_F(MjGjkTest, SmallBoxMesh) {
mjtNum dist; mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2); int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2);
ASSERT_EQ(ncons, 1); // contact unrecoverable under single precision
EXPECT_NEAR(dist, 0, kTolerance); ASSERT_EQ(ncons, sizeof(mjtNum) == 8 ? 1 : 0);
if (ncons) {
EXPECT_NEAR(dist, 0, kTolerance);
// direction // direction
EXPECT_NEAR(dir[0], 0, kTolerance); EXPECT_NEAR(dir[0], 0, kTolerance);
EXPECT_NEAR(dir[1], 0, kTolerance); EXPECT_NEAR(dir[1], 0, kTolerance);
EXPECT_NEAR(dir[2], 1, kTolerance); EXPECT_NEAR(dir[2], 1, kTolerance);
// position // position
EXPECT_NEAR(pos[0], 0, kTolerance); EXPECT_NEAR(pos[0], 0, kTolerance);
EXPECT_NEAR(pos[1], 0, kTolerance); EXPECT_NEAR(pos[1], 0, kTolerance);
EXPECT_NEAR(pos[2], 0, kTolerance); EXPECT_NEAR(pos[2], 0, kTolerance);
}
} }
TEST_F(MjGjkTest, BoxMesh) { TEST_F(MjGjkTest, BoxMesh) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -1801,18 +1803,13 @@ static constexpr char xml[] = R"(
ASSERT_EQ(ncons, 1); ASSERT_EQ(ncons, 1);
EXPECT_NEAR(dist, -0.01, kTolerance); EXPECT_NEAR(dist, -0.01, kTolerance);
EXPECT_NEAR(dir[0], 0, kTolerance); EXPECT_THAT(dir[0], MjNear(0, kTolerance, 1e-5));
EXPECT_NEAR(dir[1], 0, kTolerance); EXPECT_THAT(dir[1], MjNear(0, kTolerance, 1e-5));
EXPECT_NEAR(dir[2], 1, kTolerance); EXPECT_THAT(dir[2], MjNear(1, kTolerance, kTolerance));
EXPECT_NEAR(pos[0], 0, kTolerance);
EXPECT_NEAR(pos[1], 0, kTolerance);
EXPECT_NEAR(pos[2], -0.005, kTolerance);
// multicontact // multicontact
ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
ASSERT_EQ(ncons, 4); ASSERT_EQ(ncons, 4);
} }
TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {