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

Add mj_maxContact API function.

Browse Source 
PiperOrigin-RevId: 903135055
Change-Id: I5f103c7d51f97e327c923bc567002ace835f5517
This commit is contained in:
Kyle Bayes
2026-04-21 04:02:34 -07:00
committed by Copybara-Service
parent bc5883e82f
commit 3325971840
13 changed files with 448 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+12
View File
@@ -1179,6 +1179,18 @@ It is also triggered for :ref:`user sensors<sensor-user>` of :ref:`stage<sensor-
The computed force arrays ``cfrc_int`` and ``cfrc_ext`` currently suffer from a know bug, they do not take into account
the effect of spatial tendons, see :issue:`832`.
.. _mj_maxContact:
`mj_maxContact <#mj_maxContact>`__
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
.. 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>`__
doc/changelog.rst
+2 -1
View File
@@ -7,7 +7,8 @@ Upcoming version (not yet released)
General
^^^^^^^
- Added new :ref:`mj_maxContact<mj_maxContact>` function to get the maximum number of possible contacts returned by
two geoms.
- Added :ref:`multi-cell support<body-flexcomp-cellnum>` 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
doc/computation/index.rst
+127 -18
View File
@@ -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<option-flag-nativeccd>`, :ref:`multiccd<option-flag-multiccd>`, and
:ref:`margin<body-geom-margin>`.
.. raw:: html
<div class="pairwise-toggles">
<div class="pairwise-toggle-item">
<label class="pairwise-switch">
<input type="checkbox" id="nativeccd-checkbox" checked>
<span class="pairwise-slider"></span>
</label>
<span>nativeccd</span>
</div>
<div class="pairwise-toggle-item">
<label class="pairwise-switch">
<input type="checkbox" id="multiccd-checkbox">
<span class="pairwise-slider"></span>
</label>
<span>multiccd</span>
</div>
<div class="pairwise-toggle-item">
<label class="pairwise-switch">
<input type="checkbox" id="margin-checkbox">
<span class="pairwise-slider"></span>
</label>
<span>with margin</span>
</div>
</div>
.. 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
<div class="line">CCD</div>
<div class="line">
<div class="multiccd-off"><strong>1</strong></div>
<div class="multiccd-native"><strong>5</strong></div>
<div class="multiccd-legacy"><strong>5</strong></div>
</div>
- | primitive
| **2**
- | CCD
| **1**, **4**
-
.. raw:: html
<div class="line">CCD</div>
<div class="line">
<div class="multiccd-off"><strong>1</strong></div>
<div class="multiccd-native"><strong>5</strong></div>
<div class="multiccd-legacy"><strong>5</strong></div>
</div>
- | SDF
| :ref:`sdf_initpoints <option-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
<div class="line">CCD</div>
<div class="line">
<div class="multiccd-off"><strong>1</strong></div>
<div class="multiccd-native"><strong>5</strong></div>
<div class="multiccd-legacy"><strong>5</strong></div>
</div>
-
.. raw:: html
<div class="line">CCD</div>
<div class="line">
<div class="multiccd-off"><strong>1</strong></div>
<div class="multiccd-native"><strong>5</strong></div>
<div class="multiccd-legacy"><strong>5</strong></div>
</div>
-
.. raw:: html
<div class="line">CCD</div>
<div class="line">
<div class="multiccd-off"><strong>1</strong></div>
<div class="multiccd-native"><strong>5</strong></div>
<div class="multiccd-legacy"><strong>5</strong></div>
</div>
- | SDF
| :ref:`sdf_initpoints <option-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
<div class="line">CCD</div>
<div class="line">
<div class="multiccd-off"><strong>1</strong></div>
<div class="multiccd-native"><strong>4</strong></div>
<div class="multiccd-legacy"><strong>5</strong></div>
</div>
- | SDF
| :ref:`sdf_initpoints <option-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
<div class="line">CCD</div>
<div class="line">
<div class="multiccd-off"><strong>1</strong></div>
<div class="multiccd-native"><strong>4</strong></div>
<div class="multiccd-legacy"><strong>5</strong></div>
</div>
- | MeshSDF
| :ref:`sdf_initpoints <option-sdf_initpoints>`
* - SDF
@@ -1851,6 +1934,32 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
- | SDF
| :ref:`sdf_initpoints <option-sdf_initpoints>`
.. raw:: html
<script>
const pairwiseToggles = () => {
const table = document.querySelector('.table-pairwise');
const toggles = [
{id: 'nativeccd-checkbox', cls: 'nativeccd-enabled'},
{id: 'multiccd-checkbox', cls: 'multiccd-enabled'},
{id: 'margin-checkbox', cls: 'margin-enabled'}
];
toggles.forEach(toggle => {
const cb = document.getElementById(toggle.id);
if (cb.checked) {
table.classList.add(toggle.cls);
}
cb.addEventListener('change', () => {
table.classList.toggle(toggle.cls, this.checked);
});
});
};
pairwiseToggles();
</script>
.. _Sleeping:
Sleeping islands
doc/css/theme_overrides.css
+92
View File
@@ -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;
doc/includes/references.h
+1
View File
@@ -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);
include/mujoco/mujoco.h
+5
View File
@@ -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);
python/mujoco/functions.cc
+1
View File
@@ -304,6 +304,7 @@ PYBIND11_MODULE(_functions, pymodule) {
m, d, flg_acc, result.data());
});
Def<traits::mj_rnePostConstraint>(pymodule);
Def<traits::mj_maxContact>(pymodule);
Def<traits::mj_collision>(pymodule);
Def<traits::mj_makeConstraint>(pymodule);
Def<traits::mj_island>(pymodule);
python/mujoco/introspect/functions.py
+26
View File
@@ -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',
src/engine/engine_collision_driver.c
+99 -1
View File
@@ -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);
src/engine/engine_collision_driver.h
+7 -1
View File
@@ -18,6 +18,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#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,
test/engine/engine_collision_driver_test.cc
+68
View File
@@ -390,5 +390,73 @@ TEST_F(MjCollisionTest, MarginSumming) {
mj_deleteModel(m);
}
TEST_F(MjCollisionTest, MaxContact) {
constexpr char xml[] = R"(
<mujoco>
<option>
<flag multiccd="enable"/>
</option>
<asset>
<mesh name="smallbox"
vertex="-1 -1 -1 1 -1 -1 1 1 -1
1 1 1 1 -1 1 -1 1 -1
-1 1 1 -1 -1 1"/>
</asset>
<worldbody>
<geom name="mesh" type="mesh" mesh="smallbox"/>
<geom name="box" type="box" size="1 1 1"/>
<geom name="plane" type="plane" size="1 1 1"/>
<geom name="sphere" type="sphere" size="1"/>
<geom name="capsule" type="capsule" size="1 1"/>
<geom name="ellipsoid" type="ellipsoid" size="1 1 1"/>
<geom name="cylinder" type="cylinder" size="1 1"/>
</worldbody>
</mujoco>
)";
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
unity/Runtime/Bindings/MjBindings.cs
+3
View File
@@ -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);
wasm/codegen/generated/bindings.cc
+5
View File
@@ -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);