From 3325971840e92177ca3a38a539f446cf739b36cc Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 21 Apr 2026 04:02:34 -0700 Subject: [PATCH] Add mj_maxContact API function. PiperOrigin-RevId: 903135055 Change-Id: I5f103c7d51f97e327c923bc567002ace835f5517 --- doc/APIreference/functions.rst | 12 ++ doc/changelog.rst | 3 +- doc/computation/index.rst | 145 +++++++++++++++++--- doc/css/theme_overrides.css | 92 +++++++++++++ doc/includes/references.h | 1 + include/mujoco/mujoco.h | 5 + python/mujoco/functions.cc | 1 + python/mujoco/introspect/functions.py | 26 ++++ src/engine/engine_collision_driver.c | 100 +++++++++++++- src/engine/engine_collision_driver.h | 8 +- test/engine/engine_collision_driver_test.cc | 68 +++++++++ unity/Runtime/Bindings/MjBindings.cs | 3 + wasm/codegen/generated/bindings.cc | 5 + 13 files changed, 448 insertions(+), 21 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index ef49bf39..2e83a385 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1179,6 +1179,18 @@ It is also triggered for :ref:`user sensors` of :ref:`stage`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_maxContact + +Return the maximum number of contacts that can be generated between two geoms. + +If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +indicates that the geoms have a positive margin. + .. _mj_collision: `mj_collision <#mj_collision>`__ diff --git a/doc/changelog.rst b/doc/changelog.rst index 451b8d0c..03b768f7 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,7 +7,8 @@ Upcoming version (not yet released) General ^^^^^^^ - +- Added new :ref:`mj_maxContact` function to get the maximum number of possible contacts returned by + two geoms. - Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. - Refactored ``flexstrain`` equality constraints to be instantiated per cell instead of per flex object, reducing the diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 08eea043..07f9adb5 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1718,9 +1718,36 @@ work, but it pays off at runtime and yields both faster and more stable simulati Pair-wise colliders ^^^^^^^^^^^^^^^^^^^ -The table below provides information about the colliders used for different geom pairs. The second row in each cell -lists the maximum number of contacts generated, possibly with ``multiccd`` enabled. For example, ``Mesh`` / ``Mesh`` -will generate up to 1 contact or with ``multiccd`` up to 4 contacts. +The table below provides information about the colliders used for different geom pairs. These values can be computed +dynamically by the :ref:`mj_maxContact` function. Use the toggles to see the max number of contacts returned with the +parameters :ref:`nativeccd`, :ref:`multiccd`, and +:ref:`margin`. + +.. raw:: html + +
+
+ + nativeccd +
+
+ + multiccd +
+
+ + with margin +
+
.. list-table:: :header-rows: 1 @@ -1744,7 +1771,7 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - | primitive | **1** - | primitive - | **2** + | **4** - | primitive | **4** - | primitive @@ -1787,12 +1814,28 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. | **2** - | CCD | **1** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | primitive | **2** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Ellipsoid @@ -1812,12 +1855,36 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - - - - | CCD - | **1**, **4** - - | CCD - | **1**, **4** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Box @@ -1827,8 +1894,16 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - | primitive | **8** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
4
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Mesh @@ -1837,8 +1912,16 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - - - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
4
+
5
+
+ - | MeshSDF | :ref:`sdf_initpoints ` * - SDF @@ -1851,6 +1934,32 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - | SDF | :ref:`sdf_initpoints ` +.. raw:: html + + + + + .. _Sleeping: Sleeping islands diff --git a/doc/css/theme_overrides.css b/doc/css/theme_overrides.css index 45f94131..a7318056 100644 --- a/doc/css/theme_overrides.css +++ b/doc/css/theme_overrides.css @@ -54,6 +54,98 @@ body[data-theme="dark"] table.docutils:not(.mjcf-attributes) { font-size: 85%; } +.pairwise-toggles { + display: flex; + align-items: center; + gap: 1.5em; + margin-bottom: 0.75em; +} + +.pairwise-toggle-item { + display: flex; + align-items: center; + gap: 0.5em; +} + +.pairwise-switch { + position: relative; + display: inline-block; + width: 36px; + height: 20px; +} + +.pairwise-switch input { + opacity: 0; + width: 0; + height: 0; +} + +.pairwise-slider { + position: absolute; + cursor: pointer; + inset: 0; + background-color: #ccc; + transition: 0.3s; + border-radius: 20px; +} + +.pairwise-slider:before { + content: ""; + position: absolute; + height: 14px; + width: 14px; + left: 3px; + bottom: 3px; + background-color: white; + transition: 0.3s; + border-radius: 50%; +} + +.pairwise-switch input:checked + .pairwise-slider { + background-color: var(--secondary-header-color, #123693); +} + +.pairwise-switch input:checked + .pairwise-slider:before { + transform: translateX(16px); +} + +.multiccd-off, +.multiccd-native, +.multiccd-legacy { + display: none; + margin: 0; +} + +.multiccd-off { + display: inline; +} + +.multiccd-enabled .multiccd-off { + display: none; +} + +.multiccd-enabled.nativeccd-enabled:not(.margin-enabled) .multiccd-native { + display: inline; +} + +.multiccd-enabled:not(.nativeccd-enabled) .multiccd-legacy, +.multiccd-enabled.nativeccd-enabled.margin-enabled .multiccd-legacy { + display: inline; +} + +.margin-show { + display: none; +} + +.margin-enabled .margin-hide { + display: none; +} + +.margin-enabled .margin-show { + display: inline; +} + + .small-centered td, .small-centered th, .table-pairwise td, .table-pairwise th { text-align: center !important; diff --git a/doc/includes/references.h b/doc/includes/references.h index 1e1db78d..4d55e8ba 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3272,6 +3272,7 @@ void mj_passive(const mjModel* m, mjData* d); void mj_subtreeVel(const mjModel* m, mjData* d); void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result); void mj_rnePostConstraint(const mjModel* m, mjData* d); +int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); void mj_collision(const mjModel* m, mjData* d); void mj_makeConstraint(const mjModel* m, mjData* d); void mj_island(const mjModel* m, mjData* d); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index fa67e0bf..31954cfb 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -460,6 +460,11 @@ MJAPI void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result); // RNE with complete data: compute cacc, cfrc_ext, cfrc_int. MJAPI void mj_rnePostConstraint(const mjModel* m, mjData* d); +// Return the maximum number of contacts that can be generated between two geoms. +// If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin. +MJAPI int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); + // Run collision detection. MJAPI void mj_collision(const mjModel* m, mjData* d); diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index d6e0c724..03a1be1b 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -304,6 +304,7 @@ PYBIND11_MODULE(_functions, pymodule) { m, d, flg_acc, result.data()); }); Def(pymodule); + Def(pymodule); Def(pymodule); Def(pymodule); Def(pymodule); diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index f05bd8a5..ee5795f4 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -2407,6 +2407,32 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='RNE with complete data: compute cacc, cfrc_ext, cfrc_int.', )), + ('mj_maxContact', + FunctionDecl( + name='mj_maxContact', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='g1', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='g2', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='has_margin', + type=ValueType(name='int'), + ), + ), + doc='Return the maximum number of contacts that can be generated between two geoms. If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 indicates that the geoms have a positive margin.', # pylint: disable=line-too-long + )), ('mj_collision', FunctionDecl( name='mj_collision', diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index b77164f6..ca271e5a 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -38,7 +38,7 @@ #include "engine/engine_util_spatial.h" -// table of pair-wise collision functions +// table of pairwise collision functions mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { /* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH SDF */ /*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex, mjc_PlaneConvex}, @@ -56,6 +56,104 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { //------------------------------------ utility functions ------------------------------------------ + +// return the maximum number of contacts that can be generated between two geoms +// if has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin +int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin) { + int type1 = m->geom_type[g1]; + int type2 = m->geom_type[g2]; + + if (type1 == mjGEOM_SDF || type2 == mjGEOM_SDF) { + return m->opt.sdf_initpoints; + } + + if (type1 == mjGEOM_HFIELD || type2 == mjGEOM_HFIELD) { + int type = (type1 == mjGEOM_HFIELD) ? type2 : type1; + return (type != mjGEOM_PLANE && type != mjGEOM_HFIELD) ? mjMAXCONPAIR : 0; + } + + // spheres and ellipsoids always generate a single contact + if (type1 == mjGEOM_SPHERE || type1 == mjGEOM_ELLIPSOID || + type2 == mjGEOM_SPHERE || type2 == mjGEOM_ELLIPSOID) { + return 1; + } + + // box-box primitive collider + if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) { + return 8; + } + + // capsule-capsule primitive collider + if (type1 == mjGEOM_CAPSULE && type2 == mjGEOM_CAPSULE) { + return 2; + } + + // capsule-box primitive collider + if ((type1 == mjGEOM_CAPSULE && type2 == mjGEOM_BOX) || + (type1 == mjGEOM_BOX && type2 == mjGEOM_CAPSULE)) { + return 4; + } + + // the remaining plane cases + if (type1 == mjGEOM_PLANE || type2 == mjGEOM_PLANE) { + int type = (type1 == mjGEOM_PLANE) ? type2 : type1; + switch (type) { + case mjGEOM_CAPSULE: + return 2; + case mjGEOM_CYLINDER: + case mjGEOM_BOX: + return 4; + case mjGEOM_MESH: + return 3; + default: + return 0; + } + } + + int is_multiccd = !mjDISABLED(mjDSBL_MULTICCD); + if (!is_multiccd) { + return 1; + } + + if (type1 == mjGEOM_CAPSULE || type2 == mjGEOM_CAPSULE || + type1 == mjGEOM_CYLINDER || type2 == mjGEOM_CYLINDER) { + return 5; + } + + if (mjDISABLED(mjDSBL_NATIVECCD)) { + return is_multiccd ? 5 : 1; // mesh-mesh or mesh-box with libccd + } + + // check margin from model + if (has_margin < 0) { + has_margin = 0; + if (mjENABLED(mjENBL_OVERRIDE)) { + has_margin = m->opt.o_margin > 0.0; + } else { + int npair = m->npair; + int ipair = -1; + for (int k=0; k < npair; k++) { + if ((m->pair_geom1[k] == g1 && m->pair_geom2[k] == g2) || + (m->pair_geom1[k] == g2 && m->pair_geom2[k] == g1)) { + ipair = k; + break; + } + } + + if (ipair > -1) { + has_margin = m->pair_margin[ipair] > 0.0; + } else { + has_margin = m->geom_margin[g1] > 0.0 || m->geom_margin[g2] > 0.0; + } + } + } + + // 4 contacts for mesh-mesh or mesh-box without margins, 5 with margins + return has_margin ? 5 : 4; +} + + // move arena pointer back to the end of the contact array static inline void resetArena(mjData* d) { d->parena = d->ncon * sizeof(mjContact); diff --git a/src/engine/engine_collision_driver.h b/src/engine/engine_collision_driver.h index 9583cf5f..e231eaaf 100644 --- a/src/engine/engine_collision_driver.h +++ b/src/engine/engine_collision_driver.h @@ -18,6 +18,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -26,10 +27,15 @@ extern "C" { // collision function pointers and max contact pairs MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES]; +// return the maximum number of contacts that can be generated between two geoms +// if has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin +MJAPI int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); + // collision detection entry point MJAPI void mj_collision(const mjModel* m, mjData* d); -// applies Separating Axis Theorem for rotated AABBs +// apply the Separating Axis Theorem for rotated AABBs MJAPI int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], const mjtNum xpos1[3], const mjtNum xmat1[9], const mjtNum xpos2[3], const mjtNum xmat2[9], mjtNum margin, diff --git a/test/engine/engine_collision_driver_test.cc b/test/engine/engine_collision_driver_test.cc index 0db2556d..aef1d9fe 100644 --- a/test/engine/engine_collision_driver_test.cc +++ b/test/engine/engine_collision_driver_test.cc @@ -390,5 +390,73 @@ TEST_F(MjCollisionTest, MarginSumming) { mj_deleteModel(m); } +TEST_F(MjCollisionTest, MaxContact) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + ASSERT_THAT(d, NotNull()); + + int mesh = mj_name2id(m, mjOBJ_GEOM, "mesh"); + int box = mj_name2id(m, mjOBJ_GEOM, "box"); + int plane = mj_name2id(m, mjOBJ_GEOM, "plane"); + int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere"); + int capsule = mj_name2id(m, mjOBJ_GEOM, "capsule"); + int ellipsoid = mj_name2id(m, mjOBJ_GEOM, "ellipsoid"); + int cylinder = mj_name2id(m, mjOBJ_GEOM, "cylinder"); + + EXPECT_EQ(mj_maxContact(m, mesh, box, -1), 4); + EXPECT_EQ(mj_maxContact(m, mesh, plane, -1), 3); + EXPECT_EQ(mj_maxContact(m, box, plane, -1), 4); + EXPECT_EQ(mj_maxContact(m, mesh, mesh, -1), 4); + EXPECT_EQ(mj_maxContact(m, box, box, -1), 8); + EXPECT_EQ(mj_maxContact(m, capsule, capsule, -1), 2); + EXPECT_EQ(mj_maxContact(m, capsule, box, -1), 4); + EXPECT_EQ(mj_maxContact(m, capsule, plane, -1), 2); + EXPECT_EQ(mj_maxContact(m, cylinder, plane, -1), 4); + EXPECT_EQ(mj_maxContact(m, sphere, sphere, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, capsule, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, box, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, mesh, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, plane, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, cylinder, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, ellipsoid, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, box, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, mesh, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, plane, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, cylinder, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, capsule, -1), 1); + EXPECT_EQ(mj_maxContact(m, capsule, cylinder, -1), 5); + EXPECT_EQ(mj_maxContact(m, capsule, mesh, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, cylinder, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, box, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, mesh, -1), 5); + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index d5ea2b37..cb4c402f 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6951,6 +6951,9 @@ public static unsafe extern void mj_rne(mjModel_* m, mjData_* d, int flg_acc, do [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_rnePostConstraint(mjModel_* m, mjData_* d); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mj_maxContact(mjModel_* m, int g1, int g2, int has_margin); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_collision(mjModel_* m, mjData_* d); diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index dfe18aea..ae618fd8 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -8845,6 +8845,10 @@ void mj_makeM_wrapper(const MjModel& m, MjData& d) { mj_makeM(m.get(), d.get()); } +int mj_maxContact_wrapper(const MjModel& m, int g1, int g2, int has_margin) { + return mj_maxContact(m.get(), g1, g2, has_margin); +} + void mj_mulJacTVec_wrapper(const MjModel& m, const MjData& d, const val& res, const NumberArray& vec) { UNPACK_VALUE(mjtNum, res); UNPACK_ARRAY(mjtNum, vec); @@ -13173,6 +13177,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mj_local2Global", &mj_local2Global_wrapper); function("mj_makeConstraint", &mj_makeConstraint_wrapper); function("mj_makeM", &mj_makeM_wrapper); + function("mj_maxContact", &mj_maxContact_wrapper); function("mj_mulJacTVec", &mj_mulJacTVec_wrapper); function("mj_mulJacVec", &mj_mulJacVec_wrapper); function("mj_mulM", &mj_mulM_wrapper);