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

Remove contact pruning with box-box collisions in nativeccd and add benchmarks.

Browse Source 
PiperOrigin-RevId: 744732382
Change-Id: I2e20b646c541ee99888f1e964302d1b4363b2dc4
This commit is contained in:
Kyle Bayes
2025-04-07 08:32:58 -07:00
committed by Copybara-Service
parent d7027fb1c0
commit 606f00f802
5 changed files with 48 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+17 -13
View File
@@ -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;
}
src/engine/engine_collision_convex.h
+6 -6
View File
@@ -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,
src/engine/engine_collision_gjk.c
+12 -10
View File
@@ -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) {
test/benchmark/CMakeLists.txt
+1 -1
View File
@@ -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(
test/benchmark/ccd_benchmark_test.cc
+12 -1
View File
@@ -24,6 +24,9 @@
#include <mujoco/mujoco.h>
#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)");