From b5f9628dd7a7d149614b9d309c702d705331d349 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 18 Aug 2023 10:01:57 -0700 Subject: [PATCH] Reduce `mjMAXCONPAIR` to 50. Use only the first `mjMAXCONPAIR` contacts in SDF-mesh collisions after sorting them using the penetration depth. PiperOrigin-RevId: 558176217 Change-Id: Ia1ee12bd5ceb81e4c52f143831ad00426e8cf174 --- doc/changelog.rst | 1 - include/mujoco/mjmodel.h | 2 +- src/engine/engine_collision_sdf.c | 63 +++++++++++++++++++++------- src/engine/engine_ray.c | 4 +- unity/Runtime/Bindings/MjBindings.cs | 2 +- 5 files changed, 52 insertions(+), 20 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 27eac1f0..6981d1fc 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -17,7 +17,6 @@ General - Added new SDF plugin for defining implicit geometries. The plugin must define methods computing an SDF and its gradient at query points See the :ref:`documentation` for more details. - - Increased ``mjMAXCONPAIR`` to 100. .. youtube:: Vc1tq0fFvQA :align: right diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index a9bd828c..f988580a 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -23,7 +23,7 @@ #define mjMINMU 1E-5 // minimum friction coefficient #define mjMINIMP 0.0001 // minimum constraint impedance #define mjMAXIMP 0.9999 // maximum constraint impedance -#define mjMAXCONPAIR 100 // maximum number of contacts per geom pair +#define mjMAXCONPAIR 50 // maximum number of contacts per geom pair #define mjMAXTREEDEPTH 50 // maximum bounding volume hierarchy depth #define mjMAXVFS 2000 // maximum number of files in virtual file system #define mjMAXVFSNAME 1000 // maximum filename size in virtual file system diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index fb261fd5..a5d1dba4 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -20,6 +20,7 @@ #include #include #include "engine/engine_collision_primitive.h" +#include "engine/engine_crossplatform.h" #include "engine/engine_io.h" #include "engine/engine_plugin.h" #include "engine/engine_util_blas.h" @@ -29,7 +30,7 @@ #define MAXSDFFACE 1300 - +#define MAXMESHPNT 500 //---------------------------- primitives sdf --------------------------------------------- @@ -77,7 +78,7 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p case mjGEOM_SDF: return p->sdf_distance(x, d, i); default: - mju_error("sdf collisions not available for geom type %d", type); + mjERROR("sdf collisions not available for geom type %d", type); return 0; } } @@ -166,7 +167,7 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d, p->sdf_gradient(gradient, x, d, i); break; default: - mju_error("sdf collisions not available for geom type %d", type); + mjERROR("sdf collisions not available for geom type %d", type); } } @@ -185,7 +186,7 @@ mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjt return mju_max(geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]), geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1])); default: - mju_error("SDF type not available"); + mjERROR("SDF type not available"); return 0; } } @@ -223,7 +224,7 @@ void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, geomGradient(gradient, m, d, s->plugin[0], s->id[0], point[0], s->geomtype[0]); break; default: - mju_error("SDF type not available"); + mjERROR("SDF type not available"); } } @@ -233,9 +234,9 @@ static const mjpPlugin* getSDF(const mjModel* m, int id) { const int nslot = mjp_pluginCount(); const int slot = m->plugin[instance]; const mjpPlugin* sdf = mjp_getPluginAtSlotUnsafe(slot, nslot); - if (!sdf) mju_error("invalid plugin slot: %d", slot); + if (!sdf) mjERROR("invalid plugin slot: %d", slot); if (!(sdf->capabilityflags & mjPLUGIN_SDF)) { - mju_error("Plugin is not a signed distance field at slot %d", slot); + mjERROR("Plugin is not a signed distance field at slot %d", slot); } return sdf; } @@ -270,6 +271,20 @@ static void undoTransformation(const mjModel* m, const mjData* d, int g, //---------------------------- narrow phase ----------------------------------------------- +// comparison function for contact sorting +quicksortfunc(distcompare, dist, i1, i2) { + mjtNum d1 = ((mjtNum*)dist)[*(int*)i1]; + mjtNum d2 = ((mjtNum*)dist)[*(int*)i2]; + + if (d1 < d2) { + return -1; + } else if (d1 == d2) { + return 0; + } else { + return 1; + } +} + // check if the collision point already exists static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) { for (int i = 0; i < cnt; i++) { @@ -482,7 +497,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g, // recursive call for (int i=0; i<2; i++) { if (child[2*node+i] != -1) { - if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); + if (nstack >= max_stack) mjERROR("BVH stack depth exceeded."); stack[nstack].node = child[2*node+i]; nstack++; } @@ -505,11 +520,11 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g mjtNum* pos1 = d->geom_xpos + 3 * g1; mjtNum* mat1 = d->geom_xmat + 9 * g1; - mjtNum offset[3], rotation[9]; - mjtNum corners[9], points[3*MAXSDFFACE], x[3], dist; + mjtNum offset[3], rotation[9], corners[9], x[3], depth; + mjtNum points[3*MAXSDFFACE], dist[MAXMESHPNT], candidate[3*MAXMESHPNT]; int vertadr = m->mesh_vertadr[m->geom_dataid[g1]]; int faceadr = m->mesh_faceadr[m->geom_dataid[g1]]; - int cnt=0, npoints=0, n0=0, faces[MAXSDFFACE]={-1}; + int cnt=0, npoints=0, ncandidate=0, n0=0, faces[MAXSDFFACE]={-1}, index[MAXMESHPNT]; // get sdf plugin int instance = m->geom_plugin[g2]; @@ -558,11 +573,26 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g x[2] = (corners[2]+corners[5]+corners[8])/3; // SHOULD NOT OCCUR - if (cnt==mjMAXCONPAIR) mju_error("mjc_MeshSDF: too many contact points"); + if (ncandidate==MAXMESHPNT) mjERROR("too many contact points"); // Frank-Wolfe - dist = stepFrankWolfe(x, corners, 3, m, &sdf, (mjData*)d); - cnt = addContact(points, con, x, pos2true, sdf_quat, dist, cnt, m, &sdf, (mjData*)d); + depth = stepFrankWolfe(x, corners, 3, m, &sdf, (mjData*)d); + + // store candidate if there is penetration + if (depth<0) { + mju_copy3(candidate + 3*ncandidate, x); + index[ncandidate] = ncandidate; + dist[ncandidate++] = depth; + } + } + + // sort contacts using depth + mjQUICKSORT(index, ncandidate, sizeof(int), distcompare, dist); + + // add only the first mjMAXCONPAIR pairs + for (int i=0; igeom_type[g2] != mjGEOM_SDF) { - mju_error("geom is not an SDF"); + mjERROR("geom is not an SDF"); } // compute transformations from/to g1 to/from g2 @@ -688,6 +718,9 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m dist = stepGradient(x, m, &sdf, (mjData*)d); sdf.type = mjSDFTYPE_AVERAGE; cnt = addContact(contacts, con, x, pos2true, squat2, dist, cnt, m, &sdf, (mjData*)d); + + // SHOULD NOT OCCUR + if (cnt>mjMAXCONPAIR) mjERROR("too many contact points"); } return cnt; diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 5363d68c..32c2c6d3 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -740,9 +740,9 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g, const int nslot = mjp_pluginCount(); const int slot = m->plugin[instance]; const mjpPlugin* sdf = mjp_getPluginAtSlotUnsafe(slot, nslot); - if (!sdf) mju_error("invalid plugin slot: %d", slot); + if (!sdf) mjERROR("invalid plugin slot: %d", slot); if (!(sdf->capabilityflags & mjPLUGIN_SDF)) { - mju_error("Plugin is not a sign distance field at slot %d", slot); + mjERROR("Plugin is not a sign distance field at slot %d", slot); } // reset counter diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 3b01e5ab..035ea508 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -39,7 +39,7 @@ public const double mjMAXVAL = 10000000000.0; public const double mjMINMU = 1e-05; public const double mjMINIMP = 0.0001; public const double mjMAXIMP = 0.9999; -public const int mjMAXCONPAIR = 100; +public const int mjMAXCONPAIR = 50; public const int mjMAXTREEDEPTH = 50; public const int mjMAXVFS = 2000; public const int mjMAXVFSNAME = 1000;