From 0bffd744f90df5b2d9bf1085ef2044bec46554ce Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 16 Aug 2024 14:07:53 -0700 Subject: [PATCH] Add nativeccd flag. When this flag is enabled, general convex collision detection is handled natively, as opposed to using libccd. PiperOrigin-RevId: 663859009 Change-Id: Ife18bb1de62f2154e4725fdf764484a18586b836 --- doc/XMLreference.rst | 22 +++++++++++++--------- doc/XMLschema.rst | 2 +- doc/changelog.rst | 27 +++++++++++++++------------ doc/includes/references.h | 3 ++- include/mujoco/mjmodel.h | 3 ++- introspect/enums.py | 3 ++- introspect/enums_test.py | 3 ++- python/mujoco/bindings_test.py | 2 +- src/engine/engine_collision_convex.c | 24 ++++++++++++++++-------- src/engine/engine_support.c | 3 ++- src/xml/xml_native_reader.cc | 5 +++-- src/xml/xml_native_writer.cc | 1 + unity/Runtime/Bindings/MjBindings.cs | 3 ++- 13 files changed, 62 insertions(+), 39 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index e9c3f0fb..bcd283e8 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -623,13 +623,13 @@ from its default. .. _option-flag-multiccd: :at:`multiccd`: :at-val:`[disable, enable], "disable"` |nbsp| |nbsp| |nbsp| (experimental feature) - This flag enables multiple-contact collision detection for geom pairs that use the general-purpose convex-convex - collider based on :ref:`libccd ` 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. + 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 + 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. .. _option-flag-island: @@ -639,15 +639,19 @@ 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** (*) ~~~~~~~~~~~~~~~~ -This element is used to set options for the built-in parser and compiler. After parsing and compilation it no longer has -any effect. The settings here are global and apply to the entire model. +This element is used to set options for the built-in parser and compiler. After parsing and compilation it no longer +has any effect. The settings here are global and apply to the entire model. .. _compiler-autolimits: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index ae9f43c7..5adf0c74 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -39,7 +39,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`override` | :ref:`energy` | :ref:`fwdinv` | :ref:`invdiscrete` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`multiccd` | :ref:`island` | | | | +| | | | :ref:`multiccd` | :ref:`island` | :ref:`nativeccd` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | mujoco |br| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 54b706c6..1fd5ebf1 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,28 +7,31 @@ Upcoming version (not yet released) General ^^^^^^^ -1. Added :ref:`mjSpec` option for creating a texture from a buffer. -2. :ref:`shellinertia ` is now supported by all geom types. -3. Added support for :ref:`attaching` keyframes. +1. Added the :ref:`nativeccd` flag. When this flag is enabled, general convex collision + detection is handled natively, as opposed to using `libccd `__. This feature is in + early stages of testing. +2. Added :ref:`mjSpec` option for creating a texture from a buffer. +3. :ref:`shellinertia ` is now supported by all geom types. +4. Added support for :ref:`attaching` keyframes. MJX ^^^ -4. Added ``efc_pos`` to ``mjx.Data`` (:github:issue:`1388`). -5. Added position-dependent sensors: ``MAGNETOMETER``, ``JOINTPOS``, ``ACTUATORPOS``, ``BALLQUAT``, ``FRAMEPOS``, +5. Added ``efc_pos`` to ``mjx.Data`` (:github:issue:`1388`). +6. Added position-dependent sensors: ``MAGNETOMETER``, ``JOINTPOS``, ``ACTUATORPOS``, ``BALLQUAT``, ``FRAMEPOS``, ``FRAMEXAXIS``, ``FRAMEYAXIS``, ``FRAMEZAXIS``, ``SUBTREECOM``, ``CLOCK``. -6. Changed default policy to avoid placing unused (MuJoCo-only) arrays on device. -7. Added ``device`` parameter to ``mjx.make_data`` to bring it to parity with ``mjx.put_model`` and ``mjx.put_data``. +7. Changed default policy to avoid placing unused (MuJoCo-only) arrays on device. +8. Added ``device`` parameter to ``mjx.make_data`` to bring it to parity with ``mjx.put_model`` and ``mjx.put_data``. Bug fixes ^^^^^^^^^ -8. Fixed a performance regression introduced in 3.1.7 in mesh Bounding Volume Hierarchies (:github:issue:`1875`, - contribution by :github:user:`michael-ahn`). -9. Fixed a bug wherein, for models that have both muscles and stateless actuators and used one of the implicit - integrators, wrong derivatives would be computed. +9. Fixed a performance regression introduced in 3.1.7 in mesh Bounding Volume Hierarchies (:github:issue:`1875`, + contribution by :github:user:`michael-ahn`). +10. Fixed a bug wherein, for models that have both muscles and stateless actuators and used one of the implicit + integrators, wrong derivatives would be computed. Python bindings ^^^^^^^^^^^^^^^ -10. Added support for engine plugins in :ref:`mjSpec` (:github:issue:`1903`). +11. Added support for engine plugins in :ref:`mjSpec` (:github:issue:`1903`). Version 3.2.2 (Aug 8, 2024) diff --git a/doc/includes/references.h b/doc/includes/references.h index 267059b8..f0bc5390 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -430,8 +430,9 @@ 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 = 6 // number of enable flags + mjNENABLE = 7 // 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/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 5f3fe4bd..37e346b7 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -75,8 +75,9 @@ 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 = 6 // number of enable flags + mjNENABLE = 7 // number of enable flags } mjtEnableBit; diff --git a/introspect/enums.py b/introspect/enums.py index f040505c..22e88ae3 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -57,7 +57,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjENBL_INVDISCRETE', 8), ('mjENBL_MULTICCD', 16), ('mjENBL_ISLAND', 32), - ('mjNENABLE', 6), + ('mjENBL_NATIVECCD', 64), + ('mjNENABLE', 7), ]), )), ('mjtJoint', diff --git a/introspect/enums_test.py b/introspect/enums_test.py index fe1981a9..fde6bf7d 100644 --- a/introspect/enums_test.py +++ b/introspect/enums_test.py @@ -44,7 +44,8 @@ class EnumsTest(absltest.TestCase): ('mjENBL_INVDISCRETE', 1<<3), ('mjENBL_MULTICCD', 1<<4), ('mjENBL_ISLAND', 1<<5), - ('mjNENABLE', 6))) + ('mjENBL_NATIVECCD', 1<<6), + ('mjNENABLE', 7))) # values mostly increment by one with occasional overrides def test_mjtGeom(self): # pylint: disable=invalid-name diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index aa88d784..52b3efd1 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -870,7 +870,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, 6) + self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 7) self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0) self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1) self.assertEqual(mujoco.mjtGeom.mjGEOM_SPHERE, 2) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 942e6172..e74b29d3 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -23,14 +23,24 @@ #include #include #include +#include "engine/engine_collision_gjk.h" #include "engine/engine_collision_primitive.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "engine/engine_util_spatial.h" -// the LibCCD penetration function we use (ccdMPRPenetration or ccdGJKPenetration) -#define _mjCCDPENETRATION ccdMPRPenetration + +// call LibCCD or GJK to recover penetration info +static int mjc_penetration(const mjModel* m, const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) { + if (mjENABLED(mjENBL_NATIVECCD)) { + return mj_gjkPenetration(obj1, obj2, ccd, depth, dir, pos); + } else { + return ccdMPRPenetration(obj1, obj2, ccd, depth, dir, pos); + } +} + // ccd center function void mjccd_center(const void *obj, ccd_vec3_t *center) { @@ -294,7 +304,7 @@ static int mjc_MPRIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, mjContact* con, mjtNum margin) { ccd_vec3_t dir, pos; ccd_real_t depth; - if (_mjCCDPENETRATION(obj1, obj2, ccd, &depth, &dir, &pos) == 0) { + if (mjc_penetration(m, obj1, obj2, ccd, &depth, &dir, &pos) == 0) { // contact is found but normal is undefined if (ccdVec3Eq(&dir, ccd_vec3_origin)) { return 0; @@ -806,8 +816,8 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, continue; } - // run MPR, save contact - if (_mjCCDPENETRATION(&obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0 + // run penetration function, save contact + if (mjc_penetration(m, &obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0 && !ccdVec3Eq(&dirccd, ccd_vec3_origin)) { // fill in contact data, transform to global coordinates con[cnt].dist = -depth; @@ -1257,7 +1267,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, } // run MPR, save contact - if (_mjCCDPENETRATION(&obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0) { + if (mjc_penetration(m, &obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0) { if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) { // fill in contact data, transform to global coordinates con[cnt].dist = -depth; @@ -1289,5 +1299,3 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, return cnt; } - -#undef _mjCCDPENETRATION diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index adc9f247..07bf9ed5 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -69,7 +69,8 @@ const char* mjENABLESTRING[mjNENABLE] = { "Fwdinv", "InvDiscrete", "MultiCCD", - "Island" + "Island", + "NativeCCD" }; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index d9576e7d..85a5f5d2 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -116,10 +116,10 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { "solver", "iterations", "ls_iterations", "noslip_iterations", "mpr_iterations", "sdf_iterations", "sdf_initpoints", "actuatorgroupdisable"}, {"<"}, - {"flag", "?", "22", "constraint", "equality", "frictionloss", "limit", "contact", + {"flag", "?", "23", "constraint", "equality", "frictionloss", "limit", "contact", "passive", "gravity", "clampctrl", "warmstart", "filterparent", "actuation", "refsafe", "sensor", "midphase", "eulerdamp", "autoreset", - "override", "energy", "fwdinv", "invdiscrete", "multiccd", "island"}, + "override", "energy", "fwdinv", "invdiscrete", "multiccd", "island", "nativeccd"}, {">"}, {"size", "*", "14", "memory", "njmax", "nconmax", "nstack", "nuserdata", "nkey", @@ -1137,6 +1137,7 @@ 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 8d48cbce..3c610bb3 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1011,6 +1011,7 @@ 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/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index d7da8318..3f26168a 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -167,7 +167,8 @@ public enum mjtEnableBit : int{ mjENBL_INVDISCRETE = 8, mjENBL_MULTICCD = 16, mjENBL_ISLAND = 32, - mjNENABLE = 6, + mjENBL_NATIVECCD = 64, + mjNENABLE = 7, } public enum mjtJoint : int{ mjJNT_FREE = 0,