Add mj_maxContact API function.
PiperOrigin-RevId: 903135055 Change-Id: I5f103c7d51f97e327c923bc567002ace835f5517
This commit is contained in:
committed by
Copybara-Service
parent
bc5883e82f
commit
3325971840
@@ -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 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`.
|
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 <#mj_collision>`__
|
`mj_collision <#mj_collision>`__
|
||||||
|
|||||||
+2
-1
@@ -7,7 +7,8 @@ Upcoming version (not yet released)
|
|||||||
|
|
||||||
General
|
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
|
- 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.
|
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
|
- Refactored ``flexstrain`` equality constraints to be instantiated per cell instead of per flex object, reducing the
|
||||||
|
|||||||
+127
-18
@@ -1718,9 +1718,36 @@ work, but it pays off at runtime and yields both faster and more stable simulati
|
|||||||
Pair-wise colliders
|
Pair-wise colliders
|
||||||
^^^^^^^^^^^^^^^^^^^
|
^^^^^^^^^^^^^^^^^^^
|
||||||
|
|
||||||
The table below provides information about the colliders used for different geom pairs. The second row in each cell
|
The table below provides information about the colliders used for different geom pairs. These values can be computed
|
||||||
lists the maximum number of contacts generated, possibly with ``multiccd`` enabled. For example, ``Mesh`` / ``Mesh``
|
dynamically by the :ref:`mj_maxContact` function. Use the toggles to see the max number of contacts returned with the
|
||||||
will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
|
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::
|
.. list-table::
|
||||||
:header-rows: 1
|
:header-rows: 1
|
||||||
@@ -1744,7 +1771,7 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
|
|||||||
- | primitive
|
- | primitive
|
||||||
| **1**
|
| **1**
|
||||||
- | primitive
|
- | primitive
|
||||||
| **2**
|
| **4**
|
||||||
- | primitive
|
- | primitive
|
||||||
| **4**
|
| **4**
|
||||||
- | primitive
|
- | primitive
|
||||||
@@ -1787,12 +1814,28 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
|
|||||||
| **2**
|
| **2**
|
||||||
- | CCD
|
- | CCD
|
||||||
| **1**
|
| **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
|
- | primitive
|
||||||
| **2**
|
| **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
|
- | SDF
|
||||||
| :ref:`sdf_initpoints <option-sdf_initpoints>`
|
| :ref:`sdf_initpoints <option-sdf_initpoints>`
|
||||||
* - Ellipsoid
|
* - Ellipsoid
|
||||||
@@ -1812,12 +1855,36 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
|
|||||||
-
|
-
|
||||||
-
|
-
|
||||||
-
|
-
|
||||||
- | CCD
|
-
|
||||||
| **1**, **4**
|
.. raw:: html
|
||||||
- | CCD
|
|
||||||
| **1**, **4**
|
<div class="line">CCD</div>
|
||||||
- | CCD
|
<div class="line">
|
||||||
| **1**, **4**
|
<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
|
- | SDF
|
||||||
| :ref:`sdf_initpoints <option-sdf_initpoints>`
|
| :ref:`sdf_initpoints <option-sdf_initpoints>`
|
||||||
* - Box
|
* - Box
|
||||||
@@ -1827,8 +1894,16 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
|
|||||||
-
|
-
|
||||||
- | primitive
|
- | primitive
|
||||||
| **8**
|
| **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
|
- | SDF
|
||||||
| :ref:`sdf_initpoints <option-sdf_initpoints>`
|
| :ref:`sdf_initpoints <option-sdf_initpoints>`
|
||||||
* - Mesh
|
* - 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
|
- | MeshSDF
|
||||||
| :ref:`sdf_initpoints <option-sdf_initpoints>`
|
| :ref:`sdf_initpoints <option-sdf_initpoints>`
|
||||||
* - SDF
|
* - SDF
|
||||||
@@ -1851,6 +1934,32 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
|
|||||||
- | SDF
|
- | SDF
|
||||||
| :ref:`sdf_initpoints <option-sdf_initpoints>`
|
| :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:
|
||||||
|
|
||||||
Sleeping islands
|
Sleeping islands
|
||||||
|
|||||||
@@ -54,6 +54,98 @@ body[data-theme="dark"] table.docutils:not(.mjcf-attributes) {
|
|||||||
font-size: 85%;
|
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,
|
.small-centered td, .small-centered th,
|
||||||
.table-pairwise td, .table-pairwise th {
|
.table-pairwise td, .table-pairwise th {
|
||||||
text-align: center !important;
|
text-align: center !important;
|
||||||
|
|||||||
@@ -3272,6 +3272,7 @@ void mj_passive(const mjModel* m, mjData* d);
|
|||||||
void mj_subtreeVel(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_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result);
|
||||||
void mj_rnePostConstraint(const mjModel* m, mjData* d);
|
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_collision(const mjModel* m, mjData* d);
|
||||||
void mj_makeConstraint(const mjModel* m, mjData* d);
|
void mj_makeConstraint(const mjModel* m, mjData* d);
|
||||||
void mj_island(const mjModel* m, mjData* d);
|
void mj_island(const mjModel* m, mjData* d);
|
||||||
|
|||||||
@@ -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.
|
// RNE with complete data: compute cacc, cfrc_ext, cfrc_int.
|
||||||
MJAPI void mj_rnePostConstraint(const mjModel* m, mjData* d);
|
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.
|
// Run collision detection.
|
||||||
MJAPI void mj_collision(const mjModel* m, mjData* d);
|
MJAPI void mj_collision(const mjModel* m, mjData* d);
|
||||||
|
|
||||||
|
|||||||
@@ -304,6 +304,7 @@ PYBIND11_MODULE(_functions, pymodule) {
|
|||||||
m, d, flg_acc, result.data());
|
m, d, flg_acc, result.data());
|
||||||
});
|
});
|
||||||
Def<traits::mj_rnePostConstraint>(pymodule);
|
Def<traits::mj_rnePostConstraint>(pymodule);
|
||||||
|
Def<traits::mj_maxContact>(pymodule);
|
||||||
Def<traits::mj_collision>(pymodule);
|
Def<traits::mj_collision>(pymodule);
|
||||||
Def<traits::mj_makeConstraint>(pymodule);
|
Def<traits::mj_makeConstraint>(pymodule);
|
||||||
Def<traits::mj_island>(pymodule);
|
Def<traits::mj_island>(pymodule);
|
||||||
|
|||||||
@@ -2407,6 +2407,32 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='RNE with complete data: compute cacc, cfrc_ext, cfrc_int.',
|
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',
|
('mj_collision',
|
||||||
FunctionDecl(
|
FunctionDecl(
|
||||||
name='mj_collision',
|
name='mj_collision',
|
||||||
|
|||||||
@@ -38,7 +38,7 @@
|
|||||||
#include "engine/engine_util_spatial.h"
|
#include "engine/engine_util_spatial.h"
|
||||||
|
|
||||||
|
|
||||||
// table of pair-wise collision functions
|
// table of pairwise collision functions
|
||||||
mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
|
mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
|
||||||
/* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH SDF */
|
/* 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},
|
/*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 ------------------------------------------
|
//------------------------------------ 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
|
// move arena pointer back to the end of the contact array
|
||||||
static inline void resetArena(mjData* d) {
|
static inline void resetArena(mjData* d) {
|
||||||
d->parena = d->ncon * sizeof(mjContact);
|
d->parena = d->ncon * sizeof(mjContact);
|
||||||
|
|||||||
@@ -18,6 +18,7 @@
|
|||||||
#include <mujoco/mjdata.h>
|
#include <mujoco/mjdata.h>
|
||||||
#include <mujoco/mjexport.h>
|
#include <mujoco/mjexport.h>
|
||||||
#include <mujoco/mjmodel.h>
|
#include <mujoco/mjmodel.h>
|
||||||
|
#include <mujoco/mjtnum.h>
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
extern "C" {
|
extern "C" {
|
||||||
@@ -26,10 +27,15 @@ extern "C" {
|
|||||||
// collision function pointers and max contact pairs
|
// collision function pointers and max contact pairs
|
||||||
MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES];
|
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
|
// collision detection entry point
|
||||||
MJAPI void mj_collision(const mjModel* m, mjData* d);
|
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],
|
MJAPI int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
|
||||||
const mjtNum xpos1[3], const mjtNum xmat1[9],
|
const mjtNum xpos1[3], const mjtNum xmat1[9],
|
||||||
const mjtNum xpos2[3], const mjtNum xmat2[9], mjtNum margin,
|
const mjtNum xpos2[3], const mjtNum xmat2[9], mjtNum margin,
|
||||||
|
|||||||
@@ -390,5 +390,73 @@ TEST_F(MjCollisionTest, MarginSumming) {
|
|||||||
mj_deleteModel(m);
|
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
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|||||||
@@ -6951,6 +6951,9 @@ public static unsafe extern void mj_rne(mjModel_* m, mjData_* d, int flg_acc, do
|
|||||||
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
|
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
|
||||||
public static unsafe extern void mj_rnePostConstraint(mjModel_* m, mjData_* d);
|
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)]
|
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
|
||||||
public static unsafe extern void mj_collision(mjModel_* m, mjData_* d);
|
public static unsafe extern void mj_collision(mjModel_* m, mjData_* d);
|
||||||
|
|
||||||
|
|||||||
@@ -8845,6 +8845,10 @@ void mj_makeM_wrapper(const MjModel& m, MjData& d) {
|
|||||||
mj_makeM(m.get(), d.get());
|
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) {
|
void mj_mulJacTVec_wrapper(const MjModel& m, const MjData& d, const val& res, const NumberArray& vec) {
|
||||||
UNPACK_VALUE(mjtNum, res);
|
UNPACK_VALUE(mjtNum, res);
|
||||||
UNPACK_ARRAY(mjtNum, vec);
|
UNPACK_ARRAY(mjtNum, vec);
|
||||||
@@ -13173,6 +13177,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
|||||||
function("mj_local2Global", &mj_local2Global_wrapper);
|
function("mj_local2Global", &mj_local2Global_wrapper);
|
||||||
function("mj_makeConstraint", &mj_makeConstraint_wrapper);
|
function("mj_makeConstraint", &mj_makeConstraint_wrapper);
|
||||||
function("mj_makeM", &mj_makeM_wrapper);
|
function("mj_makeM", &mj_makeM_wrapper);
|
||||||
|
function("mj_maxContact", &mj_maxContact_wrapper);
|
||||||
function("mj_mulJacTVec", &mj_mulJacTVec_wrapper);
|
function("mj_mulJacTVec", &mj_mulJacTVec_wrapper);
|
||||||
function("mj_mulJacVec", &mj_mulJacVec_wrapper);
|
function("mj_mulJacVec", &mj_mulJacVec_wrapper);
|
||||||
function("mj_mulM", &mj_mulM_wrapper);
|
function("mj_mulM", &mj_mulM_wrapper);
|
||||||
|
|||||||
Reference in New Issue
Block a user