From 606f00f8024a96874ac9221f07e1114ed81e1f3c Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 7 Apr 2025 08:32:58 -0700 Subject: [PATCH] Remove contact pruning with box-box collisions in nativeccd and add benchmarks. PiperOrigin-RevId: 744732382 Change-Id: I2e20b646c541ee99888f1e964302d1b4363b2dc4 --- src/engine/engine_collision_convex.c | 30 ++++++++++++++++------------ src/engine/engine_collision_convex.h | 12 +++++------ src/engine/engine_collision_gjk.c | 22 ++++++++++---------- test/benchmark/CMakeLists.txt | 2 +- test/benchmark/ccd_benchmark_test.cc | 13 +++++++++++- 5 files changed, 48 insertions(+), 31 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index b724d280..16b3b05c 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -908,24 +908,31 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9], -// return true if multiccd can run in a single pass -static int singlePass(const mjCCDObj* obj1, const mjCCDObj* obj2) { +// return number of contacts supported by a single pass of narrowphase +static int maxContacts(const mjCCDObj* obj1, const mjCCDObj* obj2) { const mjModel* m = obj1->model; // single pass not supported for margins if (obj1->margin > 0 || obj2->margin > 0) { - return 0; + return 1; } - // supported geoms for single pass + // can return 8 contacts for box-box collision in one pass int type1 = m->geom_type[obj1->geom]; int type2 = m->geom_type[obj2->geom]; + if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) { + return 8; + } + + // reduce mesh collisions to 4 contacts max if (type1 == mjGEOM_BOX || type1 == mjGEOM_MESH) { if (type2 == mjGEOM_BOX || type2 == mjGEOM_MESH) { - return 1; + return mjENABLED(mjENBL_MULTICCD) ? 4 : 1; } } - return 0; + + // not supported for other geom types + return 1; } @@ -937,17 +944,14 @@ int mjc_Convex(const mjModel* m, const mjData* d, mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, margin); mjc_initCCDObj(&obj2, m, d, g2, margin); - int max_contacts = 1; - - if (mjENABLED(mjENBL_MULTICCD) && singlePass(&obj1, &obj2)) { - max_contacts = 4; - } + int max_contacts = maxContacts(&obj1, &obj2); // find initial contact int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, max_contacts, margin); - // nativeccd supports multi Box-Box collision directly - if (!mjDISABLED(mjDSBL_NATIVECCD) && singlePass(&obj1, &obj2)) { + + // no additional contacts needed + if (!mjDISABLED(mjDSBL_NATIVECCD) && max_contacts > 1) { return ncon; } diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index c3a4b220..76df2a28 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -81,12 +81,12 @@ void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]); void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]); // pairwise geom collision functions using ccd -int mjc_PlaneConvex (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_ConvexHField (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_Convex (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); +int mjc_PlaneConvex(const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +int mjc_ConvexHField(const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_Convex(const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); // geom-elem or elem-elem or vert-elem collision function using ccd int mjc_ConvexElem (const mjModel* m, const mjData* d, mjContact* con, diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 807942d2..e1ac8739 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -1863,36 +1863,38 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj, const mjtNum* mat = obj->data->geom_xmat + 3*g; if (dim == 3) { + int c = 0; int x = ((v1 & 1) && (v2 & 1) && (v3 & 1)) - (!(v1 & 1) && !(v2 & 1) && !(v3 & 1)); int y = ((v1 & 2) && (v2 & 2) && (v3 & 2)) - (!(v1 & 2) && !(v2 & 2) && !(v3 & 2)); int z = ((v1 & 4) && (v2 & 4) && (v3 & 4)) - (!(v1 & 4) && !(v2 & 4) && !(v3 & 4)); globalcoord(res, mat, NULL, x, y, z); int sgn = x + y + z; - if (x) resind[0] = 0; - if (y) resind[0] = 2; - if (z) resind[0] = 4; + if (x) resind[c++] = 0; + if (y) resind[c++] = 2; + if (z) resind[c++] = 4; if (sgn == -1) resind[0]++; - return 1; + return c == 1 ? 1 : 0; // return 1 only if vertices make a valid face } if (dim == 2) { + int c = 0; int x = ((v1 & 1) && (v2 & 1)) - (!(v1 & 1) && !(v2 & 1)); int y = ((v1 & 2) && (v2 & 2)) - (!(v1 & 2) && !(v2 & 2)); int z = ((v1 & 4) && (v2 & 4)) - (!(v1 & 4) && !(v2 & 4)); if (x) { globalcoord(res, mat, NULL, x, 0, 0); - resind[0] = (x > 0) ? 0 : 1; + resind[c++] = (x > 0) ? 0 : 1; } if (y) { - int i = (x ? 1 : 0); - globalcoord(res + 3*i, mat, NULL, 0, y, 0); - resind[i] = (y > 0) ? 2 : 3; + globalcoord(res + 3*c, mat, NULL, 0, y, 0); + resind[c++] = (y > 0) ? 2 : 3; } if (z) { globalcoord(res + 3, mat, NULL, 0, 0, z); - resind[1] = (z > 0) ? 4 : 5; + resind[c++] = (z > 0) ? 4 : 5; } - return 2; + // TODO(kylebayes): Should be able to recover multiple contacts here. + return c == 2 ? 2 : 0; } if (dim == 1) { diff --git a/test/benchmark/CMakeLists.txt b/test/benchmark/CMakeLists.txt index 0a23fafe..48a5a369 100644 --- a/test/benchmark/CMakeLists.txt +++ b/test/benchmark/CMakeLists.txt @@ -16,7 +16,7 @@ mujoco_test( ccd_benchmark_test MAIN_TARGET benchmark::benchmark_main - ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers + ADDITIONAL_LINK_LIBRARIES benchmark::benchmark absl::core_headers ccd ) mujoco_test( diff --git a/test/benchmark/ccd_benchmark_test.cc b/test/benchmark/ccd_benchmark_test.cc index 1ea8ce37..a7768dce 100644 --- a/test/benchmark/ccd_benchmark_test.cc +++ b/test/benchmark/ccd_benchmark_test.cc @@ -24,6 +24,9 @@ #include #include "test/fixture.h" +#include "src/engine/engine_collision_convex.h" +#include "src/engine/engine_collision_primitive.h" + namespace mujoco { namespace { @@ -116,11 +119,19 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL BENCHMARK(BM_BoxMesh_LibCCD); void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_BoxBox(benchmark::State& state) { - static TestHarness harness(kBoxBoxPath, "box.xml (BoxBox)"); + static TestHarness harness(kBoxBoxPath, "box.xml (BoxBox)", mjDSBL_NATIVECCD); harness.RunBenchmark(state); } BENCHMARK(BM_BoxBox); +void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_BoxBox_NativeCCD(benchmark::State& state) { + mjCOLLISIONFUNC[mjGEOM_BOX][mjGEOM_BOX] = mjc_Convex; + static TestHarness harness(kBoxBoxPath, "box.xml (NativeCCD)"); + harness.RunBenchmark(state); + mjCOLLISIONFUNC[mjGEOM_BOX][mjGEOM_BOX] = mjc_BoxBox; +} +BENCHMARK(BM_BoxBox_NativeCCD); + void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_Ellipsoid_NativeCCD(benchmark::State& state) { static TestHarness harness(kEllipsoidPath, "ellipsoid.xml (nativeccd)");