diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index feea786f..00018951 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -499,16 +499,11 @@ Returns the smallest signed distance between two geoms and optionally the segmen Returned distances are bounded from above by ``distmax``. |br| If no collision of distance smaller than ``distmax`` is found, the function will return ``distmax`` and ``fromto``, if given, will be set to (0, 0, 0, 0, 0, 0). -.. admonition:: Positive ``distmax`` values - :class: note + .. admonition:: different (correct) behavior under `nativeccd` + :class: note - .. TODO: b/339596989 - Improve mjc_Convex. - - For some colliders, a large, positive ``distmax`` will result in an accurate measurement. However, for collision - pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely inaccurate. - This is considered a bug to be fixed in a future release. - In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of - `engine_collision_driver.c `__. + As explained in :ref:`Collision Detection`, distances are inaccurate when using the + :ref:`legacy CCD pipeline`, and its use is discouraged. .. _mj_contactForce: diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index 03c1e7fb..08e08878 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -222,16 +222,11 @@ Returns the smallest signed distance between two geoms and optionally the segmen Returned distances are bounded from above by ``distmax``. |br| If no collision of distance smaller than ``distmax`` is found, the function will return ``distmax`` and ``fromto``, if given, will be set to (0, 0, 0, 0, 0, 0). -.. admonition:: Positive ``distmax`` values - :class: note + .. admonition:: different (correct) behavior under `nativeccd` + :class: note - .. TODO: b/339596989 - Improve mjc_Convex. - - For some colliders, a large, positive ``distmax`` will result in an accurate measurement. However, for collision - pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely inaccurate. - This is considered a bug to be fixed in a future release. - In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of - `engine_collision_driver.c `__. + As explained in :ref:`Collision Detection`, distances are inaccurate when using the + :ref:`legacy CCD pipeline`, and its use is discouraged. .. _mj_fullM: diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 3a1b6819..43f159ce 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -582,6 +582,12 @@ from its default. This flag disables the mid-phase collision filtering using a static AABB bounding volume hierarchy (a BVH binary tree). If disabled, all geoms pairs that are allowed to collide are checked for collisions. +.. _option-flag-nativeccd: + +:at:`nativeccd`: :at-val:`[disable, enable], "enable"` + This flag enables the native convex collision detection pipeline instead of using the + `libccd library `__, see :ref:`convex collisions` for more details. + .. _option-flag-eulerdamp: :at:`eulerdamp`: :at-val:`[disable, enable], "enable"` @@ -635,14 +641,12 @@ from its default. .. _option-flag-multiccd: -:at:`multiccd`: :at-val:`[disable, enable], "disable"` |nbsp| |nbsp| |nbsp| (experimental feature) +:at:`multiccd`: :at-val:`[disable, enable], "disable"` This flag enables multiple-contact collision detection for geom pairs that use a general-purpose convex-convex - collider e.g., mesh-mesh collisions. This can be useful when the contacting geoms have a flat surface, and the + collider e.g., mesh-mesh collisions. This can be useful when the contacting geoms have a flat surface and the single contact point generated by the convex-convex collider cannot accurately capture the surface contact, leading - to instabilities that typically manifest as sliding or wobbling. Multiple contact points are found by rotating the - two geoms by ±1e-3 radians around the tangential axes and re-running the collision function. If a new contact is - detected it is added, allowing for up to 4 additional contact points. This feature is currently considered - experimental, and both the behavior and the way it is activated may change in the future. + to instabilities that typically manifest as sliding or wobbling. The implementation of this feature depends on the + selected convex collision pipeline, see :ref:`convex collisions` for more details. .. _option-flag-island: @@ -652,12 +656,6 @@ from its default. allows for `island visualization `__. In a future release, the constraint solver will exploit the disjoint nature of constraint islands. -.. _option-flag-nativeccd: - -:at:`nativeccd`: :at-val:`[disable, enable], "disable"` - This flag enables the native convex collision detection pipeline instead of using the - `libccd library `__. - .. _compiler: **compiler** (*) @@ -6977,16 +6975,11 @@ pipeline. These 3 sensors share some common properties: to geom-geom penetration) will be reported by :ref:`sensor/distance`. In order to determine collision properties of non-penetrating geom pairs, a positive :at:`cutoff` is required. - .. admonition:: Positive cutoff values + .. admonition:: different (correct) behavior under `nativeccd` :class: note - .. TODO: b/339596989 - Improve mjc_Convex. - - For some colliders, a positive :at:`cutoff` will result in an accurate measurement. However, for collision - pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely inaccurate. - This is considered a bug to be fixed in a future release. - In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of - `engine_collision_driver.c `__. + As explained in :ref:`Collision Detection`, distances are inaccurate when using the + :ref:`legacy CCD pipeline`, and its use is discouraged. :at:`geom1`, :at:`geom2`, :at:`body1`, :at:`body2` For all 3 collision sensor types, the two colliding geoms can be specified explicitly using the :at:`geom1` and diff --git a/doc/changelog.rst b/doc/changelog.rst index d44b3591..4dc07ba4 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -23,6 +23,23 @@ Feature promotion `__ flexes for modeling deformable gripper pads. + +- .. image:: images/computation/ccd_light.gif + :width: 20% + :align: right + :class: only-light + + .. image:: images/computation/ccd_dark.gif + :width: 20% + :align: right + :class: only-dark + + The native convex collision detection pipeline introduced in 3.2.3 and enabled by the + :ref:`nativeccd` flag, is now the default. See the section on + :ref:`Convex Collision Detection` for more details. + + **Migration:** If the new pipeline breaks your workflow, set :ref:`nativeccd` to "disable". + General ^^^^^^^ - Add support for custom plots in the MuJoCo viewer by exposing a ``viewport`` property, a ``set_figures`` method, @@ -36,6 +53,8 @@ General .. admonition:: Breaking API changes :class: attention + - As mentioned above, the native convex collision detection pipeline is now the default, which may break some + workflows. In this case, set :ref:`nativeccd` to "disable" to restore the old behavior. - Added :ref:`mjs_setDeepCopy` API function. When the deep copy flag is 0, attaching a model will not copy it to the parent, so the original references to the child can be used to modify the parent after attachment. The default behavior is to perform such a shallow copy. The old behavior of creating a deep copy of the child model while @@ -184,8 +203,8 @@ General 1. The Newton solver no longer requires ``nv*nv`` memory allocation, allowing for much larger models. See e.g., `100_humanoids.xml `__. - Two quadratic-memory allocations still remain to be fully sparsified: ``mjData.actuator_moment`` and the matrices used - by the PGS solver. + Two quadratic-memory allocations still remain to be fully sparsified: ``mjData.actuator_moment`` and the matrices + used by the PGS solver. 2. Removed the :at:`solid` and :at:`membrane` plugins and moved the associated computations into the engine. See `3D example model `__ and `2D example model `__ for examples of flex objects diff --git a/doc/computation/index.rst b/doc/computation/index.rst index f0c0a180..da2ecf53 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1539,27 +1539,106 @@ Filtering Checking ~~~~~~~~ +Detailed collision checking, also known as *near-phase* or narrow-phase_ collision detection, is performed by functions +that depend on the geom types in the pair. The table of narrow-phase collision functions can be inspected at the top of +`engine_collision_driver.c `__ +and exposed to users who wish to install their own colliders as :ref:`mjCOLLISIONFUNC`. MuJoCo supports several +primitive geometric shapes: plane, sphere, capsule, cylinder, ellipsoid, and box. It also supports triangulated meshes and +height-fields. -Detailed collision checking is performed by functions that depend on the geom types in the pair. MuJoCo supports several -primitive geometric shapes: plane, sphere, capsule, cylinder, ellipsoid, box. It also supports triangulated meshes and -height fields. +.. _narrow-phase: https://en.wikipedia.org/wiki/Collision_detection#Narrow_phase -We have chosen to limit collision detection to *convex* geoms. All primitive types are convex. Height fields are not -convex but internally they are treated as unions of triangular prisms (using custom collision pruning beyond the filters -described above). Meshes specified by the user can be non-convex, and are rendered as such. For collision purposes -however they are replaced with their convex hulls. Mesh collisions are based on the Minkowski Portal Refinement (MPR) -algorithm as implemented in `libccd `__. It has tolerance and maximum iteration -parameters exposed as ``mjModel.opt.ccd_tolerance`` and ``mjModel.opt.ccd_iterations`` respectively. MPR operates on the -convex hull implicitly, however pre-computing that hull can substantially improve performance for large meshes. The -model compiler does that by default, using the `qhull `__ library. +With the notable exception of :ref:`SDF plugins` (see documentation therein), collision detection is limited to +*convex* geoms. All primitive types are convex. Height-fields are not convex but internally they are treated as a +collection of triangular prisms (using custom collision pruning beyond the filters described above). Meshes specified by +the user can be non-convex, and are rendered as such. For collision purposes however they are replaced with their convex +hulls (visualized with the 'H' key in :ref:`simulate `), computed by the `qhull `__ +library. + +.. _coCCD: + +Convex collisions +^^^^^^^^^^^^^^^^^ +All collisions involving pairs of geoms that do not have an analytic collider (e.g., meshes), are handled by one of two +general-purpose convex collision detection (CCD) pipelines: + +native pipeline (default) + The native CCD pipeline ("nativeccd") is implemented natively in MuJoCo, based on the Gilbert-Johnson-Keerthi and + Expanding Polytope algorithms (GJK_ / EPA_). The native pipeline is both faster and more robust than the MPR-based + pipeline. + +libccd pipeline (legacy) + This legacy pipeline is based on the libccd_ library, and uses Minkowski Portal Refinement (MPR_). It is activated by + disabling the :ref:`nativeccd` flag. + +.. _libccd: https://github.com/danfis/libccd +.. _MPR: https://en.wikipedia.org/wiki/Minkowski_Portal_Refinement +.. _GJK: https://en.wikipedia.org/wiki/Gilbert%E2%80%93Johnson%E2%80%93Keerthi_distance_algorithm +.. _EPA: http://scroll.stanford.edu/courses/cs468-01-fall/Papers/van-den-bergen.pdf + +Both pipelines are controlled by a tolerance (in units of distance) and maximum iteration parameters exposed as +``mjOption.ccd_tolerance`` (:ref:`ccd_tolerance`) and ``mjOption.ccd_iterations`` +(:ref:`ccd_iterations`), respectively. + +.. _coMultiCCD: + +Multiple contacts +^^^^^^^^^^^^^^^^^ +Some colliders can return more than one contact per colliding pair to model line or surface contacts, as when two flat +objects touch. For example the capsule-plane and box-plane colliders can return up to two or four contacts, +respectively. Standard general-purpose convex collision algorithms like MPR and GJK always return a single contact +point, which is problematic for surface contact scenarios (e.g., box-stacking). Both of MuJoCo's CCD pipelines can +return multiple points per contacting pair ("multiccd"). This behavior is controlled by the +:ref:`multiccd` flag, but is implemented in different ways with different trade-offs: + +libccd pipeline (legacy) + Multiple contact points are found by rotating the two geoms by ±1e-3 radians around the tangential axes and + re-running the collision routine. If a new contact is detected it is added, allowing for up to 4 additional contact + points. This method is effective, but increases the cost of each collision call by a factor of 5. + +native pipeline + Native multiccd discovers multiple contacts using a novel analysis of the contacting surfaces at the solution, + avoiding full re-runs of the collision routine, and is thus effectively "free". Note that native multiccd currently + does not support positive contact margins. If one of the two geoms has a positive margin, native multiccd will fall + back to legacy algorithm. + +.. _coDistance: + +Geom distance +^^^^^^^^^^^^^ + +.. image:: ../images/computation/ccd_light.gif + :width: 25% + :align: right + :class: only-light + +.. image:: ../images/computation/ccd_dark.gif + :width: 25% + :align: right + :class: only-dark + +The narrow-phase collision functions described :ref:`above` drive the :ref:`mj_geomDistance` function and +associated :ref:`collision-sensors`. Due to the limitations of MPR, the legacy pipeline will return incorrect values +(top) except at very small distances relative to the geom sizes, and is discouraged for this use case. In +contrast, the GJK-based native pipeline (bottom), computes the correct values at all distances. + +Convex decomposition +^^^^^^^^^^^^^^^^^^^^ In order to model a non-convex object other than a height field, the user must decompose it into a union of convex geoms -(which can be primitive shapes or meshes) and attach them to the same body. Open tools like the `CoACD library -`__ can be used outside MuJoCo to automate this process. Finally, all built-in -collision functions can be replaced with custom callbacks. This can be used to incorporate a general-purpose "triangle -soup" collision detector for example. However we do not recommend such an approach. Pre-processing the geometry and -representing it as a union of convex geoms takes some work, but it pays off at runtime and yields both faster and more -stable simulation. +(which can be primitive shapes or meshes) and attach them to the same body. A height-field is essentially a shape that +is automatically-decomposed into prisms + +Open mesh-decomposition tools like the +`CoACD library `__ can be used outside MuJoCo to automate this process. Finally, +all built-in collision functions can be replaced with custom callbacks. This can be used to incorporate a +general-purpose "triangle soup" collision detector for example. However we do not recommend such an approach. +Pre-processing the geometry and representing it as a union of convex geoms takes some work, but it pays off at runtime +and yields both faster and more stable simulation. + +The exception to this rule are :ref:`SDF plugins` (see documentation therein), which in +`certain cases `__ can be efficient, +but other requirements and limitations. .. _Pipeline: diff --git a/doc/images/computation/ccd_dark.gif b/doc/images/computation/ccd_dark.gif new file mode 100644 index 00000000..61a47d49 Binary files /dev/null and b/doc/images/computation/ccd_dark.gif differ diff --git a/doc/images/computation/ccd_light.gif b/doc/images/computation/ccd_light.gif new file mode 100644 index 00000000..c12354be Binary files /dev/null and b/doc/images/computation/ccd_light.gif differ diff --git a/doc/includes/references.h b/doc/includes/references.h index 7609d25e..ae0c13c6 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -429,8 +429,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator mjDSBL_AUTORESET = 1<<15, // automatic reset when numerical issues are detected + mjDSBL_NATIVECCD = 1<<16, // native convex collision detection - mjNDISABLE = 16 // number of disable flags + mjNDISABLE = 17 // number of disable flags } mjtDisableBit; typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_OVERRIDE = 1<<0, // override contact parameters @@ -440,9 +441,8 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags // experimental features: mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection mjENBL_ISLAND = 1<<5, // constraint island discovery - mjENBL_NATIVECCD = 1<<6, // native convex collision detection - mjNENABLE = 7 // number of enable flags + mjNENABLE = 6 // number of enable flags } mjtEnableBit; typedef enum mjtJoint_ { // type of degree of freedom mjJNT_FREE = 0, // global position and orientation (quat) (7) diff --git a/doc/mjx.rst b/doc/mjx.rst index b8d882ae..98fb36b3 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -66,9 +66,9 @@ directly from the top-level ``mjx`` module. Structs ------- -Before running MJX functions on an accelerator device, structs must be copied onto the device via the ``mjx.put_model`` and ``mjx.put_data`` -functions. Placing an :ref:`mjModel` on device yields an ``mjx.Model``. Placing an :ref:`mjData` on device yields -an ``mjx.Data``: +Before running MJX functions on an accelerator device, structs must be copied onto the device via the ``mjx.put_model`` +and ``mjx.put_data`` functions. Placing an :ref:`mjModel` on device yields an ``mjx.Model``. Placing an :ref:`mjData` on +device yields an ``mjx.Data``: .. code-block:: python @@ -86,12 +86,10 @@ These MJX variants mirror their MuJoCo counterparts but have a few key differenc express domain randomization (in the case of ``mjx.Model``) or high-throughput simulation for reinforcement learning (in the case of ``mjx.Data``). #. Numpy arrays in ``mjx.Model`` and ``mjx.Data`` are structural fields that control the output of JIT compilation. - Modifying these arrays will force JAX to recompile MJX functions. As an example, - ``jnt_limited`` is a numpy array passed by reference from :ref:`mjModel`, which determines if joint limit - constraints should be applied. If ``jnt_limited`` is modified, JAX will - re-compile MJX functions. - On the other hand, ``jnt_range`` is a JAX array that can be modified at runtime, and will only apply to joints with limits - as specified by the ``jnt_limited`` field. + Modifying these arrays will force JAX to recompile MJX functions. As an example, ``jnt_limited`` is a numpy array + passed by reference from :ref:`mjModel`, which determines if joint limit constraints should be applied. If + ``jnt_limited`` is modified, JAX will re-compile MJX functions. On the other hand, ``jnt_range`` is a JAX array that + can be modified at runtime, and will only apply to joints with limits as specified by the ``jnt_limited`` field. Neither ``mjx.Model`` nor ``mjx.Data`` are meant to be constructed manually. An ``mjx.Data`` may be created by calling @@ -110,9 +108,9 @@ Using ``mjx.make_data`` may be preferable when constructing batched ``mjx.Data`` Functions --------- -MuJoCo functions are exposed as MJX functions of the same name, but following -`PEP 8 `__-compliant names. Most of the :ref:`main simulation ` and -some of the :ref:`sub-components ` for forward simulation are available from the top-level ``mjx`` module. +MuJoCo functions are exposed as MJX functions of the same name, but following `PEP 8 +`__-compliant names. Most of the :ref:`main simulation ` and some of +the :ref:`sub-components ` for forward simulation are available from the top-level ``mjx`` module. MJX functions are not `JIT compiled `__ by default -- we leave it to the user to JIT MJX functions, or JIT their own functions that reference MJX functions. See the @@ -225,7 +223,8 @@ The following features are **fully supported** in MJX: - ``PLANE``, ``HFIELD``, ``SPHERE``, ``CAPSULE``, ``BOX``, ``MESH`` are fully implemented. ``ELLIPSOID`` and ``CYLINDER`` are implemented but only collide with other primitives, note that ``BOX`` is implemented as a mesh. * - :ref:`Constraint ` - - ``EQUALITY``, ``LIMIT_JOINT``, ``CONTACT_FRICTIONLESS``, ``CONTACT_PYRAMIDAL``, ``CONTACT_ELLIPTIC``, ``FRICTION_DOF``, ``FRICTION_TENDON`` + - ``EQUALITY``, ``LIMIT_JOINT``, ``CONTACT_FRICTIONLESS``, ``CONTACT_PYRAMIDAL``, ``CONTACT_ELLIPTIC``, + ``FRICTION_DOF``, ``FRICTION_TENDON`` * - :ref:`Equality ` - ``CONNECT``, ``WELD``, ``JOINT``, ``TENDON`` * - :ref:`Integrator ` @@ -315,22 +314,19 @@ Single scene simulation carefully optimized for CPU. MJX works best when simulating thousands or tens of thousands of scenes in parallel. Collisions between large meshes - MJX supports collisions between convex mesh geometries. However the convex collision algorithms - in MJX are implemented differently than in MuJoCo. MJX uses a branchless version of the - `Separating Axis Test `__ - (SAT) to determine if geometries are colliding with convex meshes, while MuJoCo uses the Minkowski Portal Refinement (MPR) - algorithm as implemented in `libccd `__. - SAT works well for smaller meshes but suffers in both runtime and memory for larger meshes. + MJX supports collisions between convex mesh geometries. However the convex collision algorithms in MJX are implemented + differently than in MuJoCo. MJX uses a branchless version of the `Separating Axis Test + `__ + (SAT) to determine if geometries are colliding with convex meshes, while MuJoCo uses either MPR or GJK/EPA, see + :ref:`Collision Detection` for more details. SAT works well for smaller meshes but suffers in both runtime + and memory for larger meshes. - For - collisions with convex meshes and primitives, the convex decompositon of the mesh should have - roughly **200 vertices or less** for reasonable performance. For convex-convex collisions, - the convex mesh should have roughly **fewer than 32 vertices**. We recommend using - :ref:`maxhullvert` in the MuJoCo compiler to achieve desired convex mesh properties. - With careful - tuning, MJX can simulate scenes with mesh collisions -- see the MJX - `shadow hand `__ - config for an example. Speeding up mesh collision detection is an active area of development for MJX. + For collisions with convex meshes and primitives, the convex decompositon of the mesh should have roughly **200 + vertices or less** for reasonable performance. For convex-convex collisions, the convex mesh should have roughly + **fewer than 32 vertices**. We recommend using :ref:`maxhullvert` in the MuJoCo compiler to + achieve desired convex mesh properties. With careful tuning, MJX can simulate scenes with mesh collisions -- see the + MJX `shadow hand `__ config + for an example. Speeding up mesh collision detection is an active area of development for MJX. Large, complex scenes with many contacts Accelerators exhibit poor performance for @@ -392,13 +388,12 @@ For MJX to perform well, some configuration parameters should be adjusted from t of 10% to 20%, as long as the dense matrices can fit on the device. Broadphase - While MuJoCo handles broadphase culling out of the box, MJX requires additional parameters. For an approximate version of - broadphase, use the experimental custom numeric parameters - ``max_contact_points`` and ``max_geom_pairs``. ``max_contact_points`` caps the number of contact points - sent to the solver for each condim type. ``max_geom_pairs`` caps the total number of geom-pairs sent to - respective collision functions for each geom-type pair. As an example, the - `shadow hand `__ - environment makes use of these parameters. + While MuJoCo handles broadphase culling out of the box, MJX requires additional parameters. For an approximate version + of broadphase, use the experimental custom numeric parameters ``max_contact_points`` and ``max_geom_pairs``. + ``max_contact_points`` caps the number of contact points sent to the solver for each condim type. ``max_geom_pairs`` + caps the total number of geom-pairs sent to respective collision functions for each geom-type pair. As an example, the + `shadow hand `__ environment + makes use of these parameters. GPU performance --------------- diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index 1963b213..763c43c3 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -293,6 +293,7 @@ The plugins in the `sensor/ `__ for details. +.. _exSDF: sdf """ diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 9c5fcb3b..d7ce62fc 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -64,8 +64,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator mjDSBL_AUTORESET = 1<<15, // automatic reset when numerical issues are detected + mjDSBL_NATIVECCD = 1<<16, // native convex collision detection - mjNDISABLE = 16 // number of disable flags + mjNDISABLE = 17 // number of disable flags } mjtDisableBit; @@ -77,9 +78,8 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags // experimental features: mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection mjENBL_ISLAND = 1<<5, // constraint island discovery - mjENBL_NATIVECCD = 1<<6, // native convex collision detection - mjNENABLE = 7 // number of enable flags + mjNENABLE = 6 // number of enable flags } mjtEnableBit; diff --git a/mjx/mujoco/mjx/_src/collision_driver_test.py b/mjx/mujoco/mjx/_src/collision_driver_test.py index 6bff2e8c..f33cbb07 100644 --- a/mjx/mujoco/mjx/_src/collision_driver_test.py +++ b/mjx/mujoco/mjx/_src/collision_driver_test.py @@ -60,7 +60,6 @@ def _collide( mujoco.mj_resetDataKeyframe(m, d, keyframe) dx = mjx.put_data(m, d) - m.opt.enableflags |= mujoco.mjtEnableBit.mjENBL_NATIVECCD mujoco.mj_step(m, d) collision_jit_fn = jax.jit(mjx.collision) kinematics_jit_fn = jax.jit(mjx.kinematics) diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 47182aa5..5c5d7911 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -938,7 +938,7 @@ Euler integrator, semi-implicit in velocity. self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1 << 0) self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1 << 1) self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1 << 2) - self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 7) + self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 6) self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0) self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1) self.assertEqual(mujoco.mjtGeom.mjGEOM_SPHERE, 2) diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index 32caae09..2b35d71d 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -43,7 +43,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDSBL_MIDPHASE', 8192), ('mjDSBL_EULERDAMP', 16384), ('mjDSBL_AUTORESET', 32768), - ('mjNDISABLE', 16), + ('mjDSBL_NATIVECCD', 65536), + ('mjNDISABLE', 17), ]), )), ('mjtEnableBit', @@ -57,8 +58,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjENBL_INVDISCRETE', 8), ('mjENBL_MULTICCD', 16), ('mjENBL_ISLAND', 32), - ('mjENBL_NATIVECCD', 64), - ('mjNENABLE', 7), + ('mjNENABLE', 6), ]), )), ('mjtJoint', diff --git a/python/mujoco/introspect/enums_test.py b/python/mujoco/introspect/enums_test.py index fde6bf7d..fe1981a9 100644 --- a/python/mujoco/introspect/enums_test.py +++ b/python/mujoco/introspect/enums_test.py @@ -44,8 +44,7 @@ class EnumsTest(absltest.TestCase): ('mjENBL_INVDISCRETE', 1<<3), ('mjENBL_MULTICCD', 1<<4), ('mjENBL_ISLAND', 1<<5), - ('mjENBL_NATIVECCD', 1<<6), - ('mjNENABLE', 7))) + ('mjNENABLE', 6))) # values mostly increment by one with occasional overrides def test_mjtGeom(self): # pylint: disable=invalid-name diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 2dc048e6..8ba003aa 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -33,38 +33,38 @@ // call libccd or nativeccd to recover penetration info static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) { - if (mjENABLED(mjENBL_NATIVECCD)) { - mjCCDConfig config; - mjCCDStatus status; - - // set config - config.max_iterations = ccd->max_iterations, - config.tolerance = ccd->mpr_tolerance, - config.max_contacts = 1; - config.dist_cutoff = 0; // no geom distances needed - - mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); - if (dist < 0) { - if (depth) *depth = -dist; - if (dir) { - mju_sub3(dir->v, status.x1, status.x2); - mju_normalize3(dir->v); - } - if (pos) { - pos->v[0] = 0.5 * (status.x1[0] + status.x2[0]); - pos->v[1] = 0.5 * (status.x1[1] + status.x2[1]); - pos->v[2] = 0.5 * (status.x1[2] + status.x2[2]); - } - return 0; - } - if (depth) *depth = 0; - if (dir) mju_zero3(dir->v); - if (pos) mju_zero3(dir->v); - return 1; + // fallback to MPR + if (mjDISABLED(mjDSBL_NATIVECCD)) { + return ccdMPRPenetration(obj1, obj2, ccd, depth, dir, pos); } - // fallback to MPR - return ccdMPRPenetration(obj1, obj2, ccd, depth, dir, pos); + mjCCDConfig config; + mjCCDStatus status; + + // set config + config.max_iterations = ccd->max_iterations, + config.tolerance = ccd->mpr_tolerance, + config.max_contacts = 1; + config.dist_cutoff = 0; // no geom distances needed + + mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); + if (dist < 0) { + if (depth) *depth = -dist; + if (dir) { + mju_sub3(dir->v, status.x1, status.x2); + mju_normalize3(dir->v); + } + if (pos) { + pos->v[0] = 0.5 * (status.x1[0] + status.x2[0]); + pos->v[1] = 0.5 * (status.x1[1] + status.x2[1]); + pos->v[2] = 0.5 * (status.x1[2] + status.x2[2]); + } + return 0; + } + if (depth) *depth = 0; + if (dir) mju_zero3(dir->v); + if (pos) mju_zero3(dir->v); + return 1; } @@ -791,7 +791,7 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) { // find convex-convex collision static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, mjCCDObj* obj2, mjContact* con, int max_contacts, mjtNum margin) { - if (mjENABLED(mjENBL_NATIVECCD)) { + if (!mjDISABLED(mjDSBL_NATIVECCD)) { mjCCDConfig config; mjCCDStatus status; @@ -928,7 +928,7 @@ int mjc_Convex(const mjModel* m, const mjData* d, int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, max_contacts, margin); // nativeccd supports multi Box-Box collision directly - if (mjENABLED(mjENBL_NATIVECCD) && singlePass(&obj1, &obj2)) { + if (!mjDISABLED(mjDSBL_NATIVECCD) && singlePass(&obj1, &obj2)) { return ncon; } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index cf4fed4b..7ccc9ef9 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -61,7 +61,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = { "Sensor", "Midphase", "Eulerdamp", - "AutoReset" + "AutoReset", + "NativeCCD" }; @@ -72,8 +73,7 @@ const char* mjENABLESTRING[mjNENABLE] = { "Fwdinv", "InvDiscrete", "MultiCCD", - "Island", - "NativeCCD" + "Island" }; @@ -1425,7 +1425,7 @@ mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, } // use nativecdd if flag is enabled - if (mjENABLED(mjENBL_NATIVECCD)) { + if (!mjDISABLED(mjDSBL_NATIVECCD)) { if (func == mjc_Convex || func == mjc_BoxBox) { return mj_geomDistanceCCD(m, d, g1, g2, distmax, fromto); } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index da2c68d3..1925ebef 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1118,6 +1118,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READDSBL("midphase", mjDSBL_MIDPHASE) READDSBL("eulerdamp", mjDSBL_EULERDAMP) READDSBL("autoreset", mjDSBL_AUTORESET) + READDSBL("nativeccd", mjDSBL_NATIVECCD) #undef READDSBL #define READENBL(NAME, MASK) \ @@ -1131,7 +1132,6 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READENBL("invdiscrete", mjENBL_INVDISCRETE) READENBL("multiccd", mjENBL_MULTICCD) READENBL("island", mjENBL_ISLAND) - READENBL("nativeccd", mjENBL_NATIVECCD) #undef READENBL } } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 8cb0ae1c..1bbe7a54 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1028,6 +1028,7 @@ void mjXWriter::Option(XMLElement* root) { WRITEDSBL("midphase", mjDSBL_MIDPHASE) WRITEDSBL("eulerdamp", mjDSBL_EULERDAMP) WRITEDSBL("autoreset", mjDSBL_AUTORESET) + WRITEDSBL("nativeccd", mjDSBL_NATIVECCD) #undef WRITEDSBL #define WRITEENBL(NAME, MASK) \ @@ -1039,7 +1040,6 @@ void mjXWriter::Option(XMLElement* root) { WRITEENBL("invdiscrete", mjENBL_INVDISCRETE) WRITEENBL("multiccd", mjENBL_MULTICCD) WRITEENBL("island", mjENBL_ISLAND) - WRITEENBL("nativeccd", mjENBL_NATIVECCD) #undef WRITEENBL } diff --git a/test/benchmark/ccd_benchmark_test.cc b/test/benchmark/ccd_benchmark_test.cc index aaf09359..f9da456b 100644 --- a/test/benchmark/ccd_benchmark_test.cc +++ b/test/benchmark/ccd_benchmark_test.cc @@ -42,11 +42,11 @@ static const char kMixedPath[] = class TestHarness { public: - TestHarness(const char* xml_path, std::string label, int enable_flags = 0) { + TestHarness(const char* xml_path, std::string label, int disable_flags = 0) { // Fail test if there are any mujoco errors MujocoErrorTestGuard guard; model_ = LoadModelFromPath(xml_path); - model_->opt.enableflags |= enable_flags; + model_->opt.disableflags |= disable_flags; data_ = mj_makeData(model_); for (int i=0; i < kNumWarmupSteps; i++) { mj_step(model_, data_); @@ -100,43 +100,43 @@ class TestHarness { void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_BoxMesh_NativeCCD(benchmark::State& state) { - static TestHarness harness(kBoxMeshPath, "boxmesh.xml (nativeccd)", - mjENBL_NATIVECCD); + static TestHarness harness(kBoxMeshPath, "boxmesh.xml (nativeccd)"); harness.RunBenchmark(state); } BENCHMARK(BM_BoxMesh_NativeCCD); void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_BoxMesh_LibCCD(benchmark::State& state) { - static TestHarness harness(kBoxMeshPath, "boxmesh.xml (libccd)"); + static TestHarness harness(kBoxMeshPath, "boxmesh.xml (libccd)", + mjDSBL_NATIVECCD); harness.RunBenchmark(state); } BENCHMARK(BM_BoxMesh_LibCCD); void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_Ellipsoid_NativeCCD(benchmark::State& state) { - static TestHarness harness(kEllipsoidPath, "ellipsoid.xml (nativeccd)", - mjENBL_NATIVECCD); + static TestHarness harness(kEllipsoidPath, "ellipsoid.xml (nativeccd)"); harness.RunBenchmark(state); } BENCHMARK(BM_Ellipsoid_NativeCCD); void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_Ellipsoid_LibCCD(benchmark::State& state) { - static TestHarness harness(kEllipsoidPath, "ellipsoid.xml (libccd)"); + static TestHarness harness(kEllipsoidPath, "ellipsoid.xml (libccd)", + mjDSBL_NATIVECCD); harness.RunBenchmark(state); } BENCHMARK(BM_Ellipsoid_LibCCD); void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_Mixed_NativeCCD(benchmark::State& state) { - static TestHarness harness(kMixedPath, "mixed.xml (nativeccd)", - mjENBL_NATIVECCD); + static TestHarness harness(kMixedPath, "mixed.xml (nativeccd)"); harness.RunBenchmark(state); } BENCHMARK(BM_Mixed_NativeCCD); void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_Mixed_LibCCD(benchmark::State& state) { - static TestHarness harness(kMixedPath, "mixed.xml (libccd)"); + static TestHarness harness(kMixedPath, "mixed.xml (libccd)", + mjDSBL_NATIVECCD); harness.RunBenchmark(state); } BENCHMARK(BM_Mixed_LibCCD); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index df392f11..1da58995 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -160,7 +160,8 @@ public enum mjtDisableBit : int{ mjDSBL_MIDPHASE = 8192, mjDSBL_EULERDAMP = 16384, mjDSBL_AUTORESET = 32768, - mjNDISABLE = 16, + mjDSBL_NATIVECCD = 65536, + mjNDISABLE = 17, } public enum mjtEnableBit : int{ mjENBL_OVERRIDE = 1, @@ -169,8 +170,7 @@ public enum mjtEnableBit : int{ mjENBL_INVDISCRETE = 8, mjENBL_MULTICCD = 16, mjENBL_ISLAND = 32, - mjENBL_NATIVECCD = 64, - mjNENABLE = 7, + mjNENABLE = 6, } public enum mjtJoint : int{ mjJNT_FREE = 0,