From a4e49f2dff3e7f26b6cd003e51097ace8f4d9732 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 12 May 2026 09:43:07 -0700 Subject: [PATCH] Margin and gap redesign (breaking change) PiperOrigin-RevId: 914329812 Change-Id: I905665e4c1965bdb8b90587e5b1277cfbfe5cce0 --- doc/XMLreference.rst | 51 +- doc/changelog.rst | 51 + doc/computation/index.rst | 55 +- doc/images/modeling/margin_gap_dark.svg | 887 ++++++++++++++++++ doc/images/modeling/margin_gap_light.svg | 886 +++++++++++++++++ doc/includes/references.h | 20 +- doc/modeling.rst | 3 +- .../usd/mjcPhysics/collisionAPI.h | 11 +- include/mujoco/mjdata.h | 2 +- include/mujoco/mjmodel.h | 12 +- include/mujoco/mjspec.h | 6 +- mjx/mujoco/mjx/_src/collision_driver.py | 5 +- mjx/mujoco/mjx/_src/types.py | 2 +- .../mujoco_warp/_src/collision_convex.py | 14 +- .../mujoco_warp/_src/collision_core.py | 11 +- .../mujoco_warp/_src/collision_driver.py | 40 +- .../mjx/third_party/mujoco_warp/_src/types.py | 9 +- .../third_party/mujoco_warp/_src/util_pkg.py | 20 +- model/adhesion/active_adhesion.xml | 12 +- python/mujoco/introspect/structs.py | 20 +- src/engine/engine_collision_driver.c | 117 +-- src/experimental/platform/ux/gui_spec.cc | 6 +- .../usd/mjcPhysics/generatedSchema.usda | 4 +- src/experimental/usd/mjcPhysics/schema.usda | 4 +- test/engine/engine_collision_gjk_test.cc | 4 +- .../testdata/collision_box/boxbox_bad0.xml | 2 +- .../collision_primitive/sphere_cylinder.xml | 2 +- test/engine/testdata/solver/model.xml | 2 +- test/testdata/model.xml | 2 +- 29 files changed, 2082 insertions(+), 178 deletions(-) create mode 100644 doc/images/modeling/margin_gap_dark.svg create mode 100644 doc/images/modeling/margin_gap_light.svg diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 55c220b7..d136291a 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -376,8 +376,8 @@ adjust it properly through the XML. :at:`o_margin`: :at-val:`real, "0"` This attribute replaces the margin parameter of all active contact pairs when :ref:`Contact override ` is enabled. Otherwise MuJoCo uses the element-specific margin attribute of :ref:`geom` or - :ref:`pair` depending on how the contact pair was generated. See also :ref:`Collision` in the - Computation chapter. The related gap parameter does not have a global override. + :ref:`pair` depending on how the contact pair was generated. See :ref:`margin and gap` in + the Computation chapter. The related gap parameter does not have a global override. .. _option-o_solref: .. _option-o_solimp: @@ -2702,18 +2702,20 @@ helps clarify the role of bodies and geoms in MuJoCo. .. _body-geom-margin: :at:`margin`: :at-val:`real, "0"` - Distance threshold below which contacts are detected and included in the global array mjData.contact. This however - does not mean that contact force will be generated. A contact is considered active only if the distance between the - two geom surfaces is below margin-gap. Recall that constraint impedance can be a function of distance, as explained - in :ref:`CSolver`. The quantity this function is applied to is the distance between - the two geoms minus the margin plus the gap. + Geometric inflation of the geom surface for the purpose of contact force generation. When the distance between two + geom surfaces is below ``margin``, the contact is considered active and contact forces are generated. The constraint + impedance can be a function of distance, as explained in :ref:`CSolver`. The quantity this function is applied to is + the distance between the two geoms minus the ``margin``. See :ref:`margin and gap`. .. _body-geom-gap: :at:`gap`: :at-val:`real, "0"` - This attribute is used to enable the generation of inactive contacts, i.e., contacts that are ignored by the - constraint solver but are included in mjData.contact for the purpose of custom computations. When this value is - positive, geom distances between margin and margin-gap correspond to such inactive contacts. + Additional contact detection buffer beyond ``margin``. When this value is positive, contacts are detected at + distance ``margin + gap`` but forces are only generated at distance ``margin``. Contacts with distance between + ``margin`` and ``margin + gap`` are included in ``mjData.contact`` as inactive contacts (with ``efc_address`` = -1). + These inactive contacts can be used for custom computations, for example by :ref:`adhesion` + actuators which use contacts in the gap zone to generate adhesive forces without producing contact forces. + See :ref:`margin and gap`. .. _body-geom-fromto: @@ -4110,14 +4112,15 @@ friction can only be created with this element. .. _contact-pair-margin: :at:`margin`: :at-val:`real, "0"` - Distance threshold below which contacts are detected and included in the global array mjData.contact. + Geometric inflation for the purpose of contact force generation. Contacts are detected at distance ``margin + gap`` + and forces are generated at distance ``margin``. .. _contact-pair-gap: :at:`gap`: :at-val:`real, "0"` - This attribute is used to enable the generation of inactive contacts, i.e., contacts that are ignored by the - constraint solver but are included in mjData.contact for the purpose of custom computations. When this value is - positive, geom distances between margin and margin-gap correspond to such inactive contacts. + Additional contact detection buffer beyond ``margin``. When this value is positive, contacts with distance between + ``margin`` and ``margin + gap`` are included in ``mjData.contact`` as inactive contacts but no contact forces are + generated. .. _contact-exclude: @@ -6286,16 +6289,16 @@ This element has nine custom attributes in addition to the common attributes: This element defines an active adhesion actuator which injects forces at contacts in the normal direction, see illustration video. The model shown in the video can be found `here -`_ and includes inline annotations. The transmission target -is a :el:`body`, and adhesive forces are injected into all contacts involving geoms which belong to this body. The force -is divided equally between multiple contacts. When the :at:`gap` attribute is not used, this actuator requires active -contacts and cannot apply a force at a distance, more like the active adhesion on the feet of geckos and insects rather -than an industrial vacuum gripper. In order to enable "suction at a distance", "inflate" the body's geoms by -:at:`margin` and add a corresponding :at:`gap` which activates contacts only after :at:`gap` penetration distance. This -will create a layer around the geom where contacts are detected but are inactive, and can be used for -applying the adhesive force. In the video above, such inactive contacts are blue, while active contacts are orange. -An adhesion actuator's length is always 0. :at:`ctrlrange` is required and must also be nonnegative (no repulsive forces -are allowed). The underlying :el:`general` attributes are set as follows: +`_ and includes inline annotations. The transmission +target is a :el:`body`, and adhesive forces are injected into all contacts involving geoms which belong to this body. +The force is divided equally between multiple contacts. When the :ref:`gap` attribute is not used, this +actuator requires active contacts and cannot apply a force at a distance, more like the active adhesion on the feet of +geckos and insects rather than an industrial vacuum gripper. In order to enable "suction at a distance", set the +:ref:`gap` attribute of the body's geoms to a positive value. This creates a layer around each geom where +contacts are detected but no contact forces are generated, and the adhesive force can act across this gap. In the video +above, such inactive contacts are blue, while active contacts are orange. An adhesion actuator's length is always 0. +:at:`ctrlrange` is required and must also be nonnegative (no repulsive forces are allowed). The underlying :el:`general` +attributes are set as follows: =========== ======= =========== ======== Attribute Setting Attribute Setting diff --git a/doc/changelog.rst b/doc/changelog.rst index e408b4fa..437cdac0 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,57 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- + +.. admonition:: Breaking API changes + :class: attention + + - The semantics of the contact ``margin`` and ``gap`` parameters have been redesigned for conceptual clarity and + consistency with `NVIDIA Newton `__. See the new + :ref:`margin and gap` documentation section for details. + + Previously, ``margin`` controlled the *detection threshold* (contacts exist when ``dist < margin``) and ``gap`` + was subtracted from it to produce the *force threshold* (forces generated when ``dist < margin - gap``). This was + unintuitive: users expected ``margin`` to mean geometric inflation and ``gap`` to mean a spatial gap. + + Under the new semantics, ``margin`` is the geometric inflation of the geom surface and ``gap`` is an additional + detection buffer beyond the inflated surface: + + - **Detection**: contacts are created when ``dist < margin + gap``. + - **Force generation**: constraint forces are applied when ``dist < margin``. + - **Inactive contacts**: contacts with ``margin < dist ≤ margin + gap`` are included in ``mjData.contact`` but + generate no force (``efc_address = -1``). This is useful for :ref:`adhesion` actuators and + custom callbacks. + + With the default values ``margin = 0``, ``gap = 0``, the behavior is unchanged. + + .. image:: images/modeling/margin_gap_light.svg + :width: 80% + :align: center + :class: only-light + + .. image:: images/modeling/margin_gap_dark.svg + :width: 80% + :align: center + :class: only-dark + + | + + **Migration:** Models that use the default ``gap="0"`` (the vast majority) require no changes. For models with + ``gap > 0``, apply the following transformation to preserve identical behavior: + + .. code-block:: + + margin_new = margin_old - gap_old + gap_new = gap_old + + For example, a geom with the old attributes ``margin="0.1" gap="0.1"`` should be changed to + ``margin="0" gap="0.1"``. + + Negative ``margin`` values are now permitted (corresponding to ``gap > margin`` under the old semantics). The + constraint ``margin + gap >= 0`` should be maintained to ensure valid collision detection. + Version 3.8.1 (May 11, 2026) ---------------------------- diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 8ef76cb8..059558ee 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -909,7 +909,7 @@ In addition to the above quantities which are computed online, each contact has model definition. .. list-table:: - :widths: 1 5 + :widths: 2 6 :header-rows: 1 * - Parameter @@ -920,15 +920,53 @@ model definition. - Vector of friction coefficients with dimensionality ``condim-1``. See below for semantics of the specific coefficients. * - ``margin`` - - The distance margin used to determine if the contact should be included in the global contact array - ``mjData.contact``. + - The geometric inflation of the geom surfaces. Contacts are detected when the distance is below + ``margin + gap``, and contact forces are generated when the distance is below ``margin``. * - ``gap`` - - For custom computations it is sometimes convenient to include contacts in ``mjData.contact`` but not generate - contact forces. This is what ``gap`` does: contact forces are generated only when the normal distance is below - (margin - gap). + - An additional detection buffer beyond ``margin``. Contacts with distance between ``margin`` and + ``margin + gap`` are included in ``mjData.contact`` as inactive contacts, but no contact forces are generated. + This is useful for action-at-a-distance effects, for example by :ref:`adhesion` actuators. * - ``solref`` and ``solimp`` - :ref:`Solver ` parameters, explained later. +.. _coMarginGap: + +margin and gap +^^^^^^^^^^^^^^ + +Each geom has a ``margin`` and a ``gap`` parameter, defined in the table above. The values for both parameters are +:ref:`summed` when considering contact between the two geoms. Together they define three regimes of contact +detection and force generation, illustrated in the figure below. + +.. image:: ../images/modeling/margin_gap_light.svg + :width: 90% + :align: center + :class: only-light + +.. image:: ../images/modeling/margin_gap_dark.svg + :width: 90% + :align: center + :class: only-dark + +The distance between two geom surfaces determines which regime applies: + +- **No contact** (distance > ``margin + gap``): The geom surfaces, including their gap buffers, are not overlapping. + No contact is generated. + +- **Inactive contact** (``margin`` < distance ≤ ``margin + gap``): A contact is detected and included in + ``mjData.contact``, but no contact force is generated (``efc_address = -1``). These contacts can be used for custom + computations, for example by :ref:`adhesion` actuators. + +- **Active contact** (distance ≤ ``margin``): The contact is active and constraint forces are generated. The constraint + impedance function is applied to the quantity ``distance - margin``, which is non-positive in this regime. + +Negative ``margin`` values, corresponding "shrinkgage" of the geometric shape, are permitted. In this case +``margin + gap >= 0`` must be maintained for collision detection to work correctly. + +.. _coCondim: + +condim +^^^^^^ The contact friction cone can be either elliptic or pyramidal. This is a global setting determined by the choice of constraint solver: the elliptic solvers work with elliptic cones, while the pyramidal solvers work with pyramidal cones, as defined later. The ``condim`` parameter determines the contact type, and has the following meaning: @@ -961,6 +999,11 @@ Note that condim cannot be 2 or 5. This is because the two tangential directions treated as pairs. The friction coefficients within a pair can be different though, which can be used to model skating for example. +.. _coCones: + +Friction cones +^^^^^^^^^^^^^^ + Now we describe the friction cones and the corresponding Jacobians more formally. In this section only, let :math:`f` denote the vector of constraint forces for a single contact (as opposed to the system-level vector of constraint forces), :math:`\mu` the vector of friction coefficients, and :math:`n` the contact dimensionality condim. For diff --git a/doc/images/modeling/margin_gap_dark.svg b/doc/images/modeling/margin_gap_dark.svg new file mode 100644 index 00000000..adddae01 --- /dev/null +++ b/doc/images/modeling/margin_gap_dark.svg @@ -0,0 +1,887 @@ + + + + + + + + 2026-05-12T11:57:09.015687 + image/svg+xml + + + Matplotlib v3.10.6, https://matplotlib.org/ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/modeling/margin_gap_light.svg b/doc/images/modeling/margin_gap_light.svg new file mode 100644 index 00000000..93f162c0 --- /dev/null +++ b/doc/images/modeling/margin_gap_light.svg @@ -0,0 +1,886 @@ + + + + + + + + 2026-05-12T11:57:08.937609 + image/svg+xml + + + Matplotlib v3.10.6, https://matplotlib.org/ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/includes/references.h b/doc/includes/references.h index c226304b..3034e6c9 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -108,7 +108,7 @@ struct mjContact_ { // result of collision detection functions mjtNum frame[9]; // normal is in [0-2], points from geom[0] to geom[1] // contact parameters set by mj_collideGeoms - mjtNum includemargin; // include if dist`-:ref:`gap`. + constraint becomes active; for contacts this margin is :ref:`margin`. Limit and contact constraints are active when :math:`r < 0` (penetration). For frictional constraints, see :ref:`Friction`. @@ -516,6 +516,7 @@ are as follows: **margin**, **gap** The sum of the two geom margins (or gaps respectively) is used. The geom priority is ignored here, because the margin and gap are distance properties and a one-sided specification makes little sense. + See :ref:`margin and gap`. .. _solmixing: diff --git a/include/mujoco/experimental/usd/mjcPhysics/collisionAPI.h b/include/mujoco/experimental/usd/mjcPhysics/collisionAPI.h index 1203ad5a..3aee44ed 100644 --- a/include/mujoco/experimental/usd/mjcPhysics/collisionAPI.h +++ b/include/mujoco/experimental/usd/mjcPhysics/collisionAPI.h @@ -321,8 +321,8 @@ class MjcPhysicsCollisionAPI : public UsdAPISchemaBase { // --------------------------------------------------------------------- // // MARGIN // --------------------------------------------------------------------- // - /// Distance threshold below which contacts are detected and included in the - /// global array mjData.contact. + /// Geometric inflation of the geom surface for the purpose of contact force + /// generation. /// /// | || /// | -- | -- | @@ -346,11 +346,8 @@ class MjcPhysicsCollisionAPI : public UsdAPISchemaBase { // --------------------------------------------------------------------- // // GAP // --------------------------------------------------------------------- // - /// This attribute is used to enable the generation of inactive contacts, - /// i.e., contacts that are ignored by the constraint solver but are included - /// in mjData.contact for the purpose of custom computations. When this value - /// is positive, geom distances between margin and margin-gap correspond to - /// such inactive contacts. + /// Additional contact detection buffer beyond margin. Contacts are detected + /// at distance margin + gap but forces are only generated at distance margin. /// /// | || /// | -- | -- | diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index f98de218..d092a27d 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -130,7 +130,7 @@ struct mjContact_ { // result of collision detection functions mjtNum frame[9]; // normal is in [0-2], points from geom[0] to geom[1] // contact parameters set by mj_collideGeoms - mjtNum includemargin; // include if dist Dict[FunctionKey, Contact]: if ip.size > 0: # pair contacts get their params from m.pair_* fields params.append(( - m.pair_margin[ip] - m.pair_gap[ip], + m.pair_margin[ip], jp.clip(m.pair_friction[ip], min=eps), m.pair_solref[ip], m.pair_solreffriction[ip], @@ -284,7 +284,6 @@ def _contact_groups(m: Model, d: Data) -> Dict[FunctionKey, Contact]: if geom1.size > 0 and geom2.size > 0: # other contacts get their params from geom fields margin = m.geom_margin[geom1] + m.geom_margin[geom2] - gap = m.geom_gap[geom1] + m.geom_gap[geom2] solmix1, solmix2 = m.geom_solmix[geom1], m.geom_solmix[geom2] mix = solmix1 / (solmix1 + solmix2) mix = jp.where((solmix1 < eps) & (solmix2 < eps), 0.5, mix) @@ -315,7 +314,7 @@ def _contact_groups(m: Model, d: Data) -> Dict[FunctionKey, Contact]: # unpack 5d friction: friction = friction[:, [0, 0, 1, 2, 2]] - params.append((margin - gap, friction, solref, solreffriction, solimp)) + params.append((margin, friction, solref, solreffriction, solimp)) params = map(jp.concatenate, zip(*params)) includemargin, friction, solref, solreffriction, solimp = params diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index e29d4aca..bc51e6e5 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -942,7 +942,7 @@ class Contact(PyTreeNode): dist: distance between nearest points; neg: penetration pos: position of contact point: midpoint between geoms (3,) frame: normal is in [0-2] (9,) - includemargin: include if dist= 0.0 and pairid[1] == -1: - return 0 + if wp.static(_NEW_GAP_SEMANTICS): + if dist >= gap and pairid[1] == -1: + return 0 + else: + if dist >= 0.0 and pairid[1] == -1: + return 0 # CCD operates on margin-inflated shapes (support() inflates each geom by # 0.5 * margin). The returned dist is therefore relative to the inflated diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py index 96da38e0..41feb836 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py @@ -22,6 +22,7 @@ import warp as wp from mujoco.mjx.third_party.mujoco_warp._src.math import safe_div from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINMU +from mujoco.mjx.third_party.mujoco_warp._src.types import _NEW_GAP_SEMANTICS from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import ContactType from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType @@ -197,14 +198,15 @@ def write_contact( Returns 1 if the contact is active (dist < margin), 0 otherwise. """ active = dist_in < margin_in + detected = dist_in < margin_in + gap_in # skip contact and no collision sensor - if (pairid_in[0] == -2 or not active) and pairid_in[1] == -1: + if (pairid_in[0] == -2 or not detected) and pairid_in[1] == -1: return 0 contact_type = 0 - if pairid_in[0] >= -1 and active: + if pairid_in[0] >= -1 and detected: contact_type |= ContactType.CONSTRAINT if pairid_in[1] >= 0: @@ -217,7 +219,10 @@ def write_contact( contact_frame_out[cid] = frame_in contact_geom_out[cid] = geoms_in contact_worldid_out[cid] = worldid_in - includemargin = margin_in - gap_in + if wp.static(_NEW_GAP_SEMANTICS): + includemargin = margin_in + else: + includemargin = margin_in - gap_in contact_includemargin_out[cid] = includemargin contact_dim_out[cid] = condim_in contact_friction_out[cid] = friction_in diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py index c87b21f1..df75adc7 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py @@ -271,13 +271,14 @@ def _obb_filter( return True -def _broadphase_filter(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int): +def _broadphase_filter(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int, ngeom_gap: int): @wp.func def func( # Model: geom_aabb: wp.array3d[wp.vec3], geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], # Data in: geom_xpos_in: wp.array2d[wp.vec3], geom_xmat_in: wp.array2d[wp.mat33], @@ -299,21 +300,25 @@ def _broadphase_filter(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound rbound1, rbound2 = geom_rbound[rbound_id, geom1], geom_rbound[rbound_id, geom2] # kernel_analyzer: ignore margin_id = worldid % ngeom_margin if wp.static(ngeom_margin > 1) else 0 margin1, margin2 = geom_margin[margin_id, geom1], geom_margin[margin_id, geom2] # kernel_analyzer: ignore + gap_id = worldid % ngeom_gap if wp.static(ngeom_gap > 1) else 0 + gap1, gap2 = geom_gap[gap_id, geom1], geom_gap[gap_id, geom2] # kernel_analyzer: ignore + effective_margin1 = margin1 + gap1 + effective_margin2 = margin2 + gap2 xpos1, xpos2 = geom_xpos_in[worldid, geom1], geom_xpos_in[worldid, geom2] xmat1, xmat2 = geom_xmat_in[worldid, geom1], geom_xmat_in[worldid, geom2] if rbound1 == 0.0 or rbound2 == 0.0: if wp.static(opt_broadphase_filter & BroadphaseFilter.PLANE): - return _plane_filter(rbound1, rbound2, margin1, margin2, xpos1, xpos2, xmat1, xmat2) + return _plane_filter(rbound1, rbound2, effective_margin1, effective_margin2, xpos1, xpos2, xmat1, xmat2) else: if wp.static(opt_broadphase_filter & BroadphaseFilter.SPHERE): - if not _sphere_filter(rbound1, rbound2, margin1, margin2, xpos1, xpos2): + if not _sphere_filter(rbound1, rbound2, effective_margin1, effective_margin2, xpos1, xpos2): return False if wp.static(opt_broadphase_filter & BroadphaseFilter.AABB): - if not _aabb_filter(center1, center2, size1, size2, margin1, margin2, xpos1, xpos2, xmat1, xmat2): + if not _aabb_filter(center1, center2, size1, size2, effective_margin1, effective_margin2, xpos1, xpos2, xmat1, xmat2): return False if wp.static(opt_broadphase_filter & BroadphaseFilter.OBB): - if not _obb_filter(center1, center2, size1, size2, margin1, margin2, xpos1, xpos2, xmat1, xmat2): + if not _obb_filter(center1, center2, size1, size2, effective_margin1, effective_margin2, xpos1, xpos2, xmat1, xmat2): return False return True @@ -377,6 +382,7 @@ def _sap_project(opt_broadphase: int): ngeom: int, geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], # Data in: geom_xpos_in: wp.array2d[wp.vec3], nworld_in: int, @@ -397,7 +403,7 @@ def _sap_project(opt_broadphase: int): # geom is a plane rbound = MJ_MAXVAL - radius = rbound + geom_margin[worldid % geom_margin.shape[0], geomid] + radius = rbound + geom_margin[worldid % geom_margin.shape[0], geomid] + geom_gap[worldid % geom_gap.shape[0], geomid] center = wp.dot(direction_in, xpos) sort_index_out[worldid, geomid] = geomid @@ -443,7 +449,7 @@ def _sap_range( @cache_kernel -def _sap_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int): +def _sap_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int, ngeom_gap: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Model: @@ -452,6 +458,7 @@ def _sap_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: i geom_aabb: wp.array3d[wp.vec3], geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], nxn_pairid: wp.array[wp.vec2i], # Data in: geom_xpos_in: wp.array2d[wp.vec3], @@ -502,8 +509,8 @@ def _sap_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: i continue if ( - wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin))( - geom_aabb, geom_rbound, geom_margin, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid + wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin, ngeom_gap))( + geom_aabb, geom_rbound, geom_margin, geom_gap, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid ) or pairid[1] >= 0 ): @@ -586,7 +593,7 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): wp.launch( kernel=_sap_project(m.opt.broadphase), dim=(d.nworld, m.ngeom), - inputs=[m.ngeom, m.geom_rbound, m.geom_margin, d.geom_xpos, d.nworld, direction], + inputs=[m.ngeom, m.geom_rbound, m.geom_margin, m.geom_gap, d.geom_xpos, d.nworld, direction], outputs=[ projection_lower.reshape((-1, m.ngeom)), projection_upper, @@ -622,7 +629,7 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): # assumes each geom has 5 other geoms (batched over all worlds) nsweep = 5 * nworldgeom wp.launch( - kernel=_sap_broadphase(m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0]), + kernel=_sap_broadphase(m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0], m.geom_gap.shape[0]), dim=nsweep, inputs=[ m.ngeom, @@ -630,6 +637,7 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): m.geom_aabb, m.geom_rbound, m.geom_margin, + m.geom_gap, m.nxn_pairid, d.geom_xpos, d.geom_xmat, @@ -644,7 +652,7 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): @cache_kernel -def _nxn_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int): +def _nxn_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int, ngeom_gap: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Model: @@ -652,6 +660,7 @@ def _nxn_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: i geom_aabb: wp.array3d[wp.vec3], geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], nxn_geom_pair: wp.array[wp.vec2i], nxn_pairid: wp.array[wp.vec2i], # Data in: @@ -672,8 +681,8 @@ def _nxn_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: i geom2 = geom[1] if ( - wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin))( - geom_aabb, geom_rbound, geom_margin, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid + wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin, ngeom_gap))( + geom_aabb, geom_rbound, geom_margin, geom_gap, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid ) or nxn_pairid[elementid][1] >= 0 ): @@ -709,13 +718,14 @@ def nxn_broadphase(m: Model, d: Data, ctx: CollisionContext): `contype`/`conaffinity`, parent-child relationships, and explicit `` tags. """ wp.launch( - _nxn_broadphase(m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0]), + _nxn_broadphase(m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0], m.geom_gap.shape[0]), dim=(d.nworld, m.nxn_geom_pair_filtered.shape[0]), inputs=[ m.geom_type, m.geom_aabb, m.geom_rbound, m.geom_margin, + m.geom_gap, m.nxn_geom_pair_filtered, m.nxn_pairid_filtered, d.geom_xpos, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index 7a4c8242..01c974c0 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py @@ -17,6 +17,7 @@ import enum from typing import Callable import mujoco +from mujoco.mjx.third_party.mujoco_warp._src.util_pkg import check_version import numpy as np import warp as wp @@ -26,6 +27,8 @@ MJ_MINIMP = mujoco.mjMINIMP # minimum constraint impedance MJ_MAXIMP = mujoco.mjMAXIMP # maximum constraint impedance MJ_MAXCONPAIR = mujoco.mjMAXCONPAIR MJ_MINMU = mujoco.mjMINMU # minimum friction +# True if MuJoCo >= 3.9.0 (new margin/gap semantics: includemargin = margin) +_NEW_GAP_SEMANTICS = check_version("mujoco>=3.9.0") # maximum size (by number of edges) of an horizon in EPA algorithm MJ_MAX_EPAHORIZON = 24 # maximum average number of trianglarfaces EPA can insert at each iteration @@ -974,7 +977,7 @@ class Model: geom_quat: local orientation offset rel. to body (*, ngeom, 4) geom_friction: friction for (slide, spin, roll) (*, ngeom, 3) geom_margin: detect contact if dist tuple[tuple[int, int | str], ...]: """Parse a version string into comparable components. - Both '.' and '-' are treated as separators. Each component is wrapped in a - tuple: (0, int) for numeric parts, (-1, str) for non-numeric. A (0, 0) - sentinel is appended so that stable releases sort above pre-release suffixes - during Python tuple comparison (e.g., 1.2.3 >= 1.2.3.dev). Non-numeric - components are compared lexicographically (e.g., b >= a). + Dot-separated components form the version. Hyphen-separated suffixes (e.g., + "-foo3") are treated as local build identifiers and stripped before + parsing. Each component is wrapped in a tuple: (0, int) for numeric parts, + (-1, str) for non-numeric. A (0, 0) sentinel is appended so that stable + releases sort above pre-release suffixes during Python tuple comparison + (e.g., 1.2.3 >= 1.2.3.dev). Non-numeric components are compared + lexicographically (e.g., b >= a). Args: - version_str: Version string like "3.5.0" or "3.5.0.dev869102767". + version_str: Version string like "3.5.0", "3.5.0.dev869102767", or + "3.9.0-foo3". Returns: Tuple of (type_order, value) pairs for comparison, where type_order is 0 for integers and -1 for strings, followed by a (0, 0) sentinel. """ - # Split on both '.' and '-' - parts = re.split(r"[.\-]", version_str) + # Strip local build identifier (e.g., "3.9.0-foo3" -> "3.9.0") + version_str = version_str.split("-", 1)[0] + parts = version_str.split(".") return tuple([(0, int(p)) if p.isdigit() else (-1, p) for p in parts] + [(0, 0)]) diff --git a/model/adhesion/active_adhesion.xml b/model/adhesion/active_adhesion.xml index db99db98..8678311c 100644 --- a/model/adhesion/active_adhesion.xml +++ b/model/adhesion/active_adhesion.xml @@ -22,11 +22,11 @@ - + @@ -87,9 +87,9 @@ frictionless point particles. In order to make them stick to the sphere we give the sphere priority 2, to force condim=3. - Also note the sphere has a margin+gap of 3cm as opposed to the 1cm of the arm box. + Also note the sphere has a gap of 3cm as opposed to the 1cm of the arm box. --> - + diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 5ed73b61..a9ee204a 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -2118,7 +2118,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='detect contact if distgeom_margin[nodeid1] + m->geom_margin[nodeid2]); + mjtNum gap = m->geom_gap[nodeid1] + m->geom_gap[nodeid2]; - if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin)) { + if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin + gap)) { if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2, d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1, d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2, - margin, NULL, NULL, &initialize)) { + margin + gap, NULL, NULL, &initialize)) { mj_collideGeomPair(m, d, nodeid1, nodeid2, merged, startadr, pairadr); if (mark_active) { d->bvh_active[node1 + bvhadr1] = 1; @@ -913,15 +914,16 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, // both are leaves if (isleaf1 && isleaf2) { mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->flex_margin[f2]); + mjtNum gap = m->geom_gap[nodeid1] + m->flex_gap[f2]; if (!filterBitmask(m->geom_contype[nodeid1], m->geom_conaffinity[nodeid1], m->flex_contype[f2], m->flex_conaffinity[f2]) && - !filterSphereBox(d->geom_xpos + 3*nodeid1, m->geom_rbound[nodeid1] + margin, + !filterSphereBox(d->geom_xpos + 3*nodeid1, m->geom_rbound[nodeid1] + margin + gap, bvh2 + 6*node2)) { if (mj_collideOBB(m->geom_aabb + 6*nodeid1, bvh2 + 6*node2, d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1, NULL, NULL, - margin, NULL, NULL, &initialize)) { + margin + gap, NULL, NULL, &initialize)) { // collide unless geom is plane or SDF (handled separately) if (m->geom_type[nodeid1] != mjGEOM_PLANE && m->geom_type[nodeid1] != mjGEOM_SDF) { @@ -1058,7 +1060,8 @@ static void makeAAMM(const mjModel* m, mjData* d, // process all body geoms (body is collidable, should have geoms) for (int i=0; i < body_geomnum; i++) { int geom = m->body_geomadr[body]+i; - mjtNum margin = override_margin ? override_margin : m->geom_margin[geom]; + mjtNum margin = override_margin ? override_margin + : m->geom_margin[geom] + m->geom_gap[geom]; mjtNum _aamm[6]; const mjtNum* aabb = m->geom_aabb + 6*geom; @@ -1124,7 +1127,8 @@ static void makeAAMM(const mjModel* m, mjData* d, } // correct for flex radius and margin - mjtNum margin = override_margin ? override_margin : m->flex_margin[f]; + mjtNum margin = override_margin ? override_margin + : m->flex_margin[f] + m->flex_gap[f]; mjtNum bound = m->flex_radius[f] + margin; aamm[0] -= bound; aamm[1] -= bound; @@ -1513,8 +1517,8 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) { //----------------------------- narrow-phase collision detection ----------------------------------- -// compute contact condim, gap, solref, solimp, friction -static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap, +// compute contact condim, solref, solimp, friction +static void mj_contactParam(const mjModel* m, int* condim, mjtNum* solref, mjtNum* solimp, mjtNum* friction, int g1, int g2, int f1, int f2) { mjtNum fri[3]; @@ -1522,7 +1526,6 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap, // get parameters from geom1 or flex1 int priority1 = (f1 < 0) ? m->geom_priority[g1] : m->flex_priority[f1]; int condim1 = (f1 < 0) ? m->geom_condim[g1] : m->flex_condim[f1]; - mjtNum gap1 = (f1 < 0) ? m->geom_gap[g1] : m->flex_gap[f1]; mjtNum solmix1 = (f1 < 0) ? m->geom_solmix[g1] : m->flex_solmix[f1]; const mjtNum* solref1 = (f1 < 0) ? m->geom_solref+g1*mjNREF : m->flex_solref+f1*mjNREF; const mjtNum* solimp1 = (f1 < 0) ? m->geom_solimp+g1*mjNIMP : m->flex_solimp+f1*mjNIMP; @@ -1531,15 +1534,11 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap, // get parameters from geom2 or flex2 int priority2 = (f2 < 0) ? m->geom_priority[g2] : m->flex_priority[f2]; int condim2 = (f2 < 0) ? m->geom_condim[g2] : m->flex_condim[f2]; - mjtNum gap2 = (f2 < 0) ? m->geom_gap[g2] : m->flex_gap[f2]; mjtNum solmix2 = (f2 < 0) ? m->geom_solmix[g2] : m->flex_solmix[f2]; const mjtNum* solref2 = (f2 < 0) ? m->geom_solref+g2*mjNREF : m->flex_solref+f2*mjNREF; const mjtNum* solimp2 = (f2 < 0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP; const mjtNum* friction2 = (f2 < 0) ? m->geom_friction+g2*3 : m->flex_friction+f2*3; - // gap: add - *gap = gap1 + gap2; - // different priority: copy from item with higher priority if (priority1 > priority2) { *condim = condim1; @@ -1719,8 +1718,15 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) { margin = mj_assignMargin(m, m->pair_margin[ipair]); } + // set gap: dynamic or pair + if (ipair < 0) { + gap = m->geom_gap[g1] + m->geom_gap[g2]; + } else { + gap = m->pair_gap[ipair]; + } + // bounding sphere filter - if (mj_filterSphere(m, d, g1, g2, margin)) { + if (mj_filterSphere(m, d, g1, g2, margin + gap)) { return; } @@ -1733,7 +1739,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) { } // call collision detector to generate contacts - num = collisionFunc(m, d, con, g1, g2, margin); + num = collisionFunc(m, d, con, g1, g2, margin + gap); // check contacts if (!num) { @@ -1746,15 +1752,14 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) { mjERROR("too many contacts returned by collision function"); } - // set condim, gap, solref, solimp, friction: dynamic + // set condim, solref, solimp, friction: dynamic if (ipair < 0) { - mj_contactParam(m, &condim, &gap, solref, solimp, friction, g1, g2, -1, -1); + mj_contactParam(m, &condim, solref, solimp, friction, g1, g2, -1, -1); } - // set condim, gap, solref, solimp, friction: pair + // set condim, solref, solimp, friction: pair else { condim = m->pair_dim[ipair]; - gap = m->pair_gap[ipair]; mju_copy(solref, m->pair_solref+mjNREF*ipair, mjNREF); mju_copy(solimp, m->pair_solimp+mjNIMP*ipair, mjNIMP); mju_copy(friction, m->pair_friction+5*ipair, 5); @@ -1778,7 +1783,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) { con[i].vert[1] = -1; // set remaining contact parameters - mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction); + mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); } // add to ncon @@ -1801,9 +1806,10 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]); int condim; int flex_vertnum = m->flex_vertnum[f]; - mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5]; + mjtNum gap = m->geom_gap[g] + m->flex_gap[f]; + mjtNum solref[mjNREF], solimp[mjNIMP], friction[5]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f); + mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f); // collide all flex vertices with plane for (int i=0; i < flex_vertnum; i++) { @@ -1814,7 +1820,7 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { mjtNum dist = mju_dot3(dif, nrm); // no contact - if (dist > margin + radius) { + if (dist > margin + gap + radius) { continue; } @@ -1835,7 +1841,7 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { con.vert[1] = i; // set remaining contact parameters - mj_setContact(m, &con, condim, margin-gap, solref, solreffriction, solimp, friction); + mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction); // add to mjData, abort if too many contacts if (mj_addContact(m, d, &con)) { @@ -1854,10 +1860,11 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { // prepare contact parameters (same for all contacts) mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]); + mjtNum gap = m->geom_gap[g] + m->flex_gap[f]; int condim; - mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5]; + mjtNum solref[mjNREF], solimp[mjNIMP], friction[5]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f); + mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f); // allocate temporary contact array on stack (zero-initialized) mj_markStack(d); @@ -1865,7 +1872,7 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { memset(con, 0, mjMAXCONPAIR * sizeof(mjContact)); // call batched flex-SDF collision - int num = mjc_FlexSDF(m, d, con, g, f, margin); + int num = mjc_FlexSDF(m, d, con, g, f, margin + gap); // add contacts to mjData for (int i = 0; i < num; i++) { @@ -1880,7 +1887,7 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { con[i].vert[1] = -1; // set remaining contact parameters - mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction); + mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); // add to mjData, abort if too many contacts if (mj_addContact(m, d, con + i)) { @@ -1949,9 +1956,9 @@ void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { int condim; int flex_elemnum = m->flex_elemnum[f]; mjtNum radius = m->flex_radius[f]; - mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5]; + mjtNum solref[mjNREF], solimp[mjNIMP], friction[5]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f, f); + mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f, f); condim = 1; // process all elements @@ -2048,13 +2055,14 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) { // test a geom and an elem for collision, add to contact list void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]); + mjtNum gap = m->geom_gap[g] + m->flex_gap[f]; int dim = m->flex_dim[f], type = m->geom_type[g]; int num; // bounding sphere test: only if midphase is disabled if (mjDISABLED(mjDSBL_MIDPHASE)) { int eglobal = m->flex_elemadr[f] + e; - if (filterSphereBox(d->geom_xpos+3*g, m->geom_rbound[g]+margin, + if (filterSphereBox(d->geom_xpos+3*g, m->geom_rbound[g]+margin+gap, d->flexelem_aabb+6*eglobal)) { return; } @@ -2087,17 +2095,17 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // call raw primitive for corresponding geom type if (type == mjGEOM_SPHERE) { - num = mjraw_SphereCapsule(con, margin, + num = mjraw_SphereCapsule(con, margin + gap, d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, pos, mat, size); } else if (type == mjGEOM_CAPSULE) { - num = mjraw_CapsuleCapsule(con, margin, + num = mjraw_CapsuleCapsule(con, margin + gap, d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, pos, mat, size); } else { - num = mjraw_CapsuleBox(con, margin, + num = mjraw_CapsuleBox(con, margin + gap, pos, mat, size, d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g); @@ -2110,13 +2118,13 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // heightfield : elem else if (type == mjGEOM_HFIELD) { - num = mjc_HFieldElem(m, d, con, g, f, e, margin); + num = mjc_HFieldElem(m, d, con, g, f, e, margin + gap); } // sphere : triangle else if (type == mjGEOM_SPHERE && dim == 2) { const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; - num = mjraw_SphereTriangle(con, margin, + num = mjraw_SphereTriangle(con, margin + gap, d->geom_xpos+3*g, m->geom_size[3*g], vertxpos + 3*edata[0], vertxpos + 3*edata[1], vertxpos + 3*edata[2], m->flex_radius[f]); @@ -2125,7 +2133,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // box : triangle else if (type == mjGEOM_BOX && dim == 2) { const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f]; - num = mjraw_BoxTriangle(con, margin, d->geom_xpos + 3 * g, + num = mjraw_BoxTriangle(con, margin + gap, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g, m->geom_size + 3 * g, vertxpos + 3 * edata[0], vertxpos + 3 * edata[1], vertxpos + 3 * edata[2], m->flex_radius[f]); @@ -2135,14 +2143,14 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { else if (type == mjGEOM_CAPSULE && dim == 2) { const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f]; num = mjraw_CapsuleTriangle( - con, margin, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g, + con, margin + gap, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g, m->geom_size + 3 * g, vertxpos + 3 * edata[0], vertxpos + 3 * edata[1], vertxpos + 3 * edata[2], m->flex_radius[f]); } // general geom : elem else { - num = mjc_ConvexElem(m, d, con, g, -1, -1, -1, f, e, margin); + num = mjc_ConvexElem(m, d, con, g, -1, -1, -1, f, e, margin + gap); } // check contacts @@ -2153,9 +2161,9 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // get contact parameters int condim; - mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum friction[5], solref[mjNREF], solimp[mjNIMP]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f); + mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f); // add contacts for (int i=0; i < num; i++) { @@ -2170,7 +2178,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { con[i].vert[1] = -1; // set remaining contact parameters - mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction); + mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); } // add to ncon @@ -2184,17 +2192,19 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // test two elems for collision, add to contact list void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2) { mjtNum margin = mj_assignMargin(m, m->flex_margin[f1] + m->flex_margin[f2]); + mjtNum gap = m->flex_gap[f1] + m->flex_gap[f2]; int dim1 = m->flex_dim[f1], dim2 = m->flex_dim[f2]; int num; - // ignore margin in self-collisions + // ignore margin and gap in self-collisions if (f1 == f2) { margin = 0; + gap = 0; } // bounding box filter (not applied in midphase) if (filterBox(d->flexelem_aabb+6*(m->flex_elemadr[f1]+e1), - d->flexelem_aabb+6*(m->flex_elemadr[f2]+e2), margin)) { + d->flexelem_aabb+6*(m->flex_elemadr[f2]+e2), margin + gap)) { return; } @@ -2231,12 +2241,12 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 pos2, mat2, size2); // raw primitive - num = mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2); + num = mjraw_CapsuleCapsule(con, margin + gap, pos1, mat1, size1, pos2, mat2, size2); } // general convex collision else { - num = mjc_ConvexElem(m, d, con, -1, f1, e1, -1, f2, e2, margin); + num = mjc_ConvexElem(m, d, con, -1, f1, e1, -1, f2, e2, margin + gap); } // check contacts @@ -2247,14 +2257,9 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 // get contact parameters int condim; - mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum friction[5], solref[mjNREF], solimp[mjNIMP]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f1, f2); - - // ignore gap in self collision, since margin is ignored - if (f1 == f2) { - gap = 0; - } + mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f1, f2); // add contacts for (int i=0; i < num; i++) { @@ -2269,7 +2274,7 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 con[i].vert[1] = -1; // set remaining contact parameters - mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction); + mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); } // add to ncon @@ -2336,9 +2341,9 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { // get contact parameters int condim; - mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum friction[5], solref[mjNREF], solimp[mjNIMP]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f, f); + mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f, f); // add contacts for (int i=0; i < num; i++) { diff --git a/src/experimental/platform/ux/gui_spec.cc b/src/experimental/platform/ux/gui_spec.cc index d845ab09..a723cbfe 100644 --- a/src/experimental/platform/ux/gui_spec.cc +++ b/src/experimental/platform/ux/gui_spec.cc @@ -421,7 +421,7 @@ void ElementSpecGui(mjsElement* element, SpecEditor* editor) { FIELD(solref, "solver reference"); FIELD(solimp, "solver impedance"); FIELD(margin, "margin for contact detection"); - FIELD(gap, "include in solver if dist < margin-gap"); + FIELD(gap, "additional contact detection buffer"); FIELD(mass, "used to compute density"); FIELD(density, "used to compute mass and inertia from volume or surface"); FIELD(typeinertia, "selects between surface and volume inertia"); @@ -530,7 +530,7 @@ void ElementSpecGui(mjsElement* element, SpecEditor* editor) { FIELD(solref, "solver reference"); FIELD(solimp, "solver impedance"); FIELD(margin, "margin for contact detection"); - FIELD(gap, "include in solver if distncon, 1); - EXPECT_LT(data->contact[0].efc_address, 0); + EXPECT_GE(data->contact[0].efc_address, 0); mj_deleteData(data); mj_deleteModel(model); diff --git a/test/engine/testdata/collision_box/boxbox_bad0.xml b/test/engine/testdata/collision_box/boxbox_bad0.xml index a3e08973..8d436c6f 100644 --- a/test/engine/testdata/collision_box/boxbox_bad0.xml +++ b/test/engine/testdata/collision_box/boxbox_bad0.xml @@ -2,7 +2,7 @@ - + diff --git a/test/engine/testdata/collision_primitive/sphere_cylinder.xml b/test/engine/testdata/collision_primitive/sphere_cylinder.xml index 3023d060..0e17f2dd 100644 --- a/test/engine/testdata/collision_primitive/sphere_cylinder.xml +++ b/test/engine/testdata/collision_primitive/sphere_cylinder.xml @@ -16,7 +16,7 @@ - + diff --git a/test/engine/testdata/solver/model.xml b/test/engine/testdata/solver/model.xml index 82bc2906..1f98808d 100644 --- a/test/engine/testdata/solver/model.xml +++ b/test/engine/testdata/solver/model.xml @@ -43,7 +43,7 @@ - + diff --git a/test/testdata/model.xml b/test/testdata/model.xml index 9bd11249..595eaae4 100644 --- a/test/testdata/model.xml +++ b/test/testdata/model.xml @@ -43,7 +43,7 @@ - +