From ec94bb49aab990a5eae40f21fab3ca257f389df5 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 26 Aug 2025 04:26:30 -0700 Subject: [PATCH] Make constraint island discovery on by default. Also fix latent bug in mjData serialization. PiperOrigin-RevId: 799505349 Change-Id: I299e7cc8133fa2ec0b339a7ff3d02543e06778b1 --- doc/XMLreference.rst | 16 ++--- doc/XMLschema.rst | 4 +- doc/changelog.rst | 12 ++++ doc/computation/index.rst | 39 +++++++++-- doc/images/computation/island.svg | 1 + doc/includes/references.h | 6 +- include/mujoco/mjmodel.h | 6 +- mjx/mujoco/mjx/_src/solver_test.py | 7 ++ python/mujoco/bindings_test.py | 2 +- python/mujoco/introspect/enums.py | 6 +- python/mujoco/introspect/enums_test.py | 3 +- python/mujoco/structs_wrappers.cc | 2 + simulate/simulate.cc | 2 +- src/engine/engine_forward.c | 2 +- src/engine/engine_island.c | 4 +- src/engine/engine_support.c | 6 +- .../usd/plugins/mjcf/mujoco_to_usd.cc | 4 +- src/experimental/usd/usd_to_mjspec.cc | 8 +-- src/xml/xml_native_reader.cc | 4 +- src/xml/xml_native_writer.cc | 2 +- .../engine_util_sparse_benchmark_test.cc | 2 +- test/benchmark/thread_performance_test.cc | 8 +-- test/engine/engine_island_test.cc | 4 +- test/engine/engine_solver_test.cc | 67 ++++++++++++------- test/engine/engine_thread_test.cc | 4 +- .../usd/mjcPhysics/mjc_physics_scene_test.cc | 1 - unity/Runtime/Bindings/MjBindings.cs | 6 +- 27 files changed, 147 insertions(+), 81 deletions(-) create mode 100644 doc/images/computation/island.svg diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 0b0d1bee..7e759027 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -587,6 +587,14 @@ from its default. This flag enables the native convex collision detection pipeline instead of using the `libccd library `__, see :ref:`convex collisions` for more details. +.. _option-flag-island: + +:at:`island`: :at-val:`[disable, enable], "enable"` + This flag enables discovery and construction of constraint islands: disjoint sets of constraints and + degrees-of-freedom that do not interact and can be solved independently. Islanding is not yet supported by the PGS + solver. See :ref:`soIsland` for more details. The :ref:`mjVIS_ISLAND ` enables + `island visualization `__. + .. _option-flag-eulerdamp: :at:`eulerdamp`: :at-val:`[disable, enable], "enable"` @@ -647,14 +655,6 @@ from its default. 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: - -:at:`island`: :at-val:`[disable, enable], "disable"` - This flag enables discovery of constraint islands: disjoint sets of constraints and - degrees-of-freedom that do not interact. The flag currently has no effect on the physics pipeline, but enabling it - allows for `island visualization `__. - In a future release, the constraint solver will exploit the disjoint nature of constraint islands. - .. _compiler: **compiler** (*) diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 45504b7c..a733318d 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -37,9 +37,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`sensor` | :ref:`midphase` | :ref:`eulerdamp` | :ref:`autoreset` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`override` | :ref:`energy` | :ref:`fwdinv` | :ref:`invdiscrete` | | +| | | | :ref:`nativeccd` | :ref:`island` | :ref:`override` | :ref:`energy` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`multiccd` | :ref:`island` | :ref:`nativeccd` | | | +| | | | :ref:`fwdinv` | :ref:`invdiscrete` | :ref:`multiccd` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | mujoco |br| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 1adedb24..54129662 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,9 +7,16 @@ Upcoming version (not yet released) General ^^^^^^^ +- Constraint island discovery and construction, previously an experimental feature, is now :ref:`documented` + and promoted to default; disable it with :ref:`option/flag/island `. We expect islanding to be + a strict improvement over the monolithic constraint solver, please let us know if you experience any issues. + .. admonition:: Breaking API changes :class: attention + - The promotion of islanding to default involved removing the enable flag ``mjENBL_ISLAND`` and + converting it to a disable flag :ref:`mjDSBL_ISLAND `. + - The update of ``mjData.qacc_warmstart`` was moved from the end of the solver call (:ref:`mj_fwdConstraint`) to the end of :ref:`mj_step`, and is now updated with all other state variables. This change makes :ref:`mj_forward` fully idempotent. @@ -43,6 +50,11 @@ MJX - ``ten_length`` was moved from ``mjx.Data._impl.ten_length`` to a public field ``mjx.Data.ten_length``. +Bug fixes +^^^^^^^^^ +- Fixed a latent bug where MjData objects were not serialized correctly by the Python bindings when islanding was + enabled. + Version 3.3.5 (August 8, 2025) ------------------------------ diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 20fc4d84..8a17981c 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1362,15 +1362,45 @@ representations of the constraint Jacobian and related matrices. axes. The right panel illustrates our solution to this problem. We still update one contact at a time, but within a contact we update along non-orthogonal axes adapted to the constraint surface, as follows. First, we optimize the quadratic cost along the ray from the tip of the cone through the current solution. Then we slice the cone with a - hyperplane passing through the current solution and orthogonal to the contact normal. This yields an ellipsoid -which - can be up to 5-dimensional given our contact model. Now we optimize the quadratic cost within this ellipsoid. This is - an instance of quadratically constrained quadratic programming (QCQP). Since there is only one scalar constraint + hyperplane passing through the current solution and orthogonal to the contact normal. This yields an ellipsoid which + can be up to 5-dimensional, given our contact model. Now we optimize the quadratic cost within this ellipsoid. This + is an instance of quadratically constrained quadratic programming (QCQP). Since there is only one scalar constraint (however nonlinear it may be), the dual is a scalar optimization problem over the unknown Lagrange multiplier. We solve this problem with Newton's method applied until convergence -- which in practice takes less than 10 iterations, and involves small matrices. Overall this algorithm has similar behavior to PGS for pyramidal cones, but it can handle elliptic cones without approximating them. It does more work per contact, however the contact dimensionality is smaller, and these two factors roughly balance each other. +.. _soIsland: + +Constraint islands +~~~~~~~~~~~~~~~~~~ + +.. image:: ../images/computation/island.svg + :width: 58% + :align: right + +Consider the abstract graph defined by degrees of freedom (dofs) and constraints. A vertex is all the dofs in a single +kinematic subtree; an edge is a constraint (a contact or equality) between two bodies belonging to different subtrees. A +*constraint island* is a disjoint sub-graph which can be solved for independently, because constraint forces cannot +propagate between islands. Constraint island discovery and construction ("islanding") invloves finding the disjoint +subgraphs and reordering both the dofs and constraints to make them memory-contiguous. This amounts to a +block-diagonalization of the constraint Jacobian :math:`J`, as illustrated in the figure. On the left is the monolithic +Jacobian of size :math:`\nc \times \nv`, where we use the :ref:`corresponding ` size names from MuJoCo's data +structures ``mjData.nefc`` and ``mjModel.nv``. On the right is the block-diagonalized Jacobian with 3 islands that can +be solved indepenently. Note that islanding also identifies unconstrained dofs, so ``mjData.nidof``, the total number of +dofs in all islands, might be smaller than ``mjModel.nv``. While islanding is not free (see implementation in +`engine_island.c `__), it is worth the +effort: + +- Different islands require different numbers of iterations to converge, and a monolithic solve would run for the + number required by the slowest island. +- Unconstrained dofs are completely untouched by the solver, which otherwise needs to discover that they are unaffected. +- Solving separate islands can be multi-threaded. + +Islanding is not yet supported by the PGS solver. + + .. _soParameters: Parameters @@ -1694,7 +1724,8 @@ The stages below compute quantities that depend on the generalized positions ``m 7. Compute the sparse factorization of the joint-space inertia matrix: :ref:`mj_factorM` 8. Construct the list of active contacts. This includes both broad-phase and near-phase collision detection: :ref:`mj_collision` -9. Construct the constraint Jacobian and compute the constraint residuals: :ref:`mj_makeConstraint` +9. Construct the constraint Jacobian, compute the constraint residuals, construct islands: :ref:`mj_makeConstraint`, + ``mj_island`` (not yet exposed in the API) 10. Compute the actuator lengths and moment arms: :ref:`mj_transmission` 11. Compute the matrices and vectors needed by the constraint solvers: :ref:`mj_projectConstraint` 12. Compute sensor data that only depends on position, and the potential energy if enabled: :ref:`mj_sensorPos`, diff --git a/doc/images/computation/island.svg b/doc/images/computation/island.svg new file mode 100644 index 00000000..9c757d42 --- /dev/null +++ b/doc/images/computation/island.svg @@ -0,0 +1 @@ +nefcnvnidof diff --git a/doc/includes/references.h b/doc/includes/references.h index 04dce769..5fcf3c44 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -464,8 +464,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags 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 + mjDSBL_ISLAND = 1<<17, // constraint island discovery - mjNDISABLE = 17 // number of disable flags + mjNDISABLE = 18 // number of disable flags } mjtDisableBit; typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_OVERRIDE = 1<<0, // override contact parameters @@ -474,9 +475,8 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics // experimental features: mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection - mjENBL_ISLAND = 1<<5, // constraint island discovery - mjNENABLE = 6 // number of enable flags + mjNENABLE = 5 // 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 85849b96..9971c1b9 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -66,8 +66,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags 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 + mjDSBL_ISLAND = 1<<17, // constraint island discovery - mjNDISABLE = 17 // number of disable flags + mjNDISABLE = 18 // number of disable flags } mjtDisableBit; @@ -78,9 +79,8 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics // experimental features: mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection - mjENBL_ISLAND = 1<<5, // constraint island discovery - mjNENABLE = 6 // number of enable flags + mjNENABLE = 5 // number of enable flags } mjtEnableBit; diff --git a/mjx/mujoco/mjx/_src/solver_test.py b/mjx/mujoco/mjx/_src/solver_test.py index 928d6a9b..e5d5adec 100644 --- a/mjx/mujoco/mjx/_src/solver_test.py +++ b/mjx/mujoco/mjx/_src/solver_test.py @@ -58,6 +58,13 @@ class SolverTest(parameterized.TestCase): m.opt.solver = solver_ m.opt.cone = cone m.opt.iterations = iterations + + # with islanding on, MuJoCo CG converges *much* faster, + # too fast for the low-iteration comparison to be meaningful, + # so we disable islanding at low iteration count + if iterations < 5: + m.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_ISLAND + d = mujoco.MjData(m) def cost(qacc): diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 5131e73e..827182b1 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -946,7 +946,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, 5) 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 c867525c..b67471fa 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -44,7 +44,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDSBL_EULERDAMP', 16384), ('mjDSBL_AUTORESET', 32768), ('mjDSBL_NATIVECCD', 65536), - ('mjNDISABLE', 17), + ('mjDSBL_ISLAND', 131072), + ('mjNDISABLE', 18), ]), )), ('mjtEnableBit', @@ -57,8 +58,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjENBL_FWDINV', 4), ('mjENBL_INVDISCRETE', 8), ('mjENBL_MULTICCD', 16), - ('mjENBL_ISLAND', 32), - ('mjNENABLE', 6), + ('mjNENABLE', 5), ]), )), ('mjtJoint', diff --git a/python/mujoco/introspect/enums_test.py b/python/mujoco/introspect/enums_test.py index fe1981a9..f2e3c3c0 100644 --- a/python/mujoco/introspect/enums_test.py +++ b/python/mujoco/introspect/enums_test.py @@ -43,8 +43,7 @@ class EnumsTest(absltest.TestCase): ('mjENBL_FWDINV', 1<<2), ('mjENBL_INVDISCRETE', 1<<3), ('mjENBL_MULTICCD', 1<<4), - ('mjENBL_ISLAND', 1<<5), - ('mjNENABLE', 6))) + ('mjNENABLE', 5))) # values mostly increment by one with occasional overrides def test_mjtGeom(self): # pylint: disable=invalid-name diff --git a/python/mujoco/structs_wrappers.cc b/python/mujoco/structs_wrappers.cc index 7c1ec9b0..b909cc2f 100644 --- a/python/mujoco/structs_wrappers.cc +++ b/python/mujoco/structs_wrappers.cc @@ -736,6 +736,7 @@ void MjDataWrapper::Serialize(std::ostream& output) const { X(nA); X(nefc); X(nisland); + X(nidof); X(time); X(energy); #undef X @@ -815,6 +816,7 @@ MjDataWrapper MjDataWrapper::Deserialize(std::istream& input) { X(nA); X(nefc); X(nisland); + X(nidof); X(time); X(energy); #undef X diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 30795dca..555ab68c 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -647,7 +647,7 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d, } // add islands if enabled - if (mjENABLED(mjENBL_ISLAND)) { + if (!mjDISABLED(mjDSBL_ISLAND) && d->nisland > 0) { mju::sprintf_arr(tmp, "\n%d", d->nisland); mju::strcat_arr(content, tmp); mju::strcat_arr(title, "\nIslands"); diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 2d4044c8..d1de115b 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -723,7 +723,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) { mju_zeroInt(d->solver_niter, mjNISLAND); // check if islands are supported - int islands_supported = mjENABLED(mjENBL_ISLAND) && + int islands_supported = !mjDISABLED(mjDSBL_ISLAND) && nisland > 0 && m->opt.noslip_iterations == 0 && (m->opt.solver == mjSOL_CG || m->opt.solver == mjSOL_NEWTON); diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 06d4f7d7..0284a730 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -411,7 +411,7 @@ void mj_island(const mjModel* m, mjData* d) { int nv = m->nv, nefc = d->nefc, ntree=m->ntree; // no constraints: quick return - if (!mjENABLED(mjENBL_ISLAND) || !nefc) { + if (mjDISABLED(mjDSBL_ISLAND) || !nefc) { d->nisland = d->nidof = 0; return; } @@ -454,7 +454,7 @@ void mj_island(const mjModel* m, mjData* d) { return; } - // count ni: total number of dofs in islands + // count nidof: total number of dofs in islands int nidof = 0; for (int i=0; i < nv; i++) { nidof += (tree_island[m->dof_treeid[i]] >= 0); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index a68d6a22..596ba714 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -62,7 +62,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = { "Midphase", "Eulerdamp", "AutoReset", - "NativeCCD" + "NativeCCD", + "Island" }; @@ -72,8 +73,7 @@ const char* mjENABLESTRING[mjNENABLE] = { "Energy", "Fwdinv", "InvDiscrete", - "MultiCCD", - "Island" + "MultiCCD" }; diff --git a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc index 26ec2414..2a9ae0c8 100644 --- a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc +++ b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc @@ -643,7 +643,6 @@ class ModelWriter { const std::vector> enable_flags = { {MjcPhysicsTokens->mjcFlagMulticcd, mjENBL_MULTICCD}, - {MjcPhysicsTokens->mjcFlagIsland, mjENBL_ISLAND}, {MjcPhysicsTokens->mjcFlagFwdinv, mjENBL_FWDINV}, {MjcPhysicsTokens->mjcFlagEnergy, mjENBL_ENERGY}, {MjcPhysicsTokens->mjcFlagOverride, mjENBL_OVERRIDE}, @@ -669,7 +668,8 @@ class ModelWriter { {MjcPhysicsTokens->mjcFlagMidphase, mjDSBL_MIDPHASE}, {MjcPhysicsTokens->mjcFlagEulerdamp, mjDSBL_EULERDAMP}, {MjcPhysicsTokens->mjcFlagAutoreset, mjDSBL_AUTORESET}, - {MjcPhysicsTokens->mjcFlagNativeccd, mjDSBL_NATIVECCD}}; + {MjcPhysicsTokens->mjcFlagNativeccd, mjDSBL_NATIVECCD}, + {MjcPhysicsTokens->mjcFlagIsland, mjDSBL_ISLAND}}; for (const auto &[token, flag] : disable_flags) { create_flag_attr(token, flag, false); } diff --git a/src/experimental/usd/usd_to_mjspec.cc b/src/experimental/usd/usd_to_mjspec.cc index fed6ceda..94e009f8 100644 --- a/src/experimental/usd/usd_to_mjspec.cc +++ b/src/experimental/usd/usd_to_mjspec.cc @@ -617,6 +617,10 @@ void ParseUsdPhysicsScene(mjSpec* spec, mjc_physics_scene.GetAutoResetFlagAttr().Get(&autoreset_flag); spec->option.disableflags |= (!autoreset_flag ? mjDSBL_AUTORESET : 0); + bool island_flag; + mjc_physics_scene.GetIslandFlagAttr().Get(&island_flag); + spec->option.disableflags |= (!island_flag ? mjDSBL_ISLAND : 0); + bool override_flag; mjc_physics_scene.GetOverrideFlagAttr().Get(&override_flag); spec->option.enableflags |= (override_flag ? mjENBL_OVERRIDE : 0); @@ -637,10 +641,6 @@ void ParseUsdPhysicsScene(mjSpec* spec, mjc_physics_scene.GetMultiCCDFlagAttr().Get(&multiccd_flag); spec->option.enableflags |= (multiccd_flag ? mjENBL_MULTICCD : 0); - bool island_flag; - mjc_physics_scene.GetIslandFlagAttr().Get(&island_flag); - spec->option.enableflags |= (island_flag ? mjENBL_ISLAND : 0); - // Compiler attributes auto auto_limits_attr = mjc_physics_scene.GetAutoLimitsAttr(); if (auto_limits_attr.HasAuthoredValue()) { diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index f99d255c..d729e8c3 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -121,7 +121,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"flag", "?", "23", "constraint", "equality", "frictionloss", "limit", "contact", "passive", "gravity", "clampctrl", "warmstart", "filterparent", "actuation", "refsafe", "sensor", "midphase", "eulerdamp", "autoreset", - "override", "energy", "fwdinv", "invdiscrete", "multiccd", "island", "nativeccd"}, + "nativeccd", "island", "override", "energy", "fwdinv", "invdiscrete", "multiccd"}, {">"}, {"size", "*", "14", "memory", "njmax", "nconmax", "nstack", "nuserdata", "nkey", @@ -1199,6 +1199,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READDSBL("eulerdamp", mjDSBL_EULERDAMP) READDSBL("autoreset", mjDSBL_AUTORESET) READDSBL("nativeccd", mjDSBL_NATIVECCD) + READDSBL("island", mjDSBL_ISLAND) #undef READDSBL #define READENBL(NAME, MASK) \ @@ -1211,7 +1212,6 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READENBL("fwdinv", mjENBL_FWDINV) READENBL("invdiscrete", mjENBL_INVDISCRETE) READENBL("multiccd", mjENBL_MULTICCD) - READENBL("island", mjENBL_ISLAND) #undef READENBL } } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 35b0666c..6a998a6e 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1056,6 +1056,7 @@ void mjXWriter::Option(XMLElement* root) { WRITEDSBL("eulerdamp", mjDSBL_EULERDAMP) WRITEDSBL("autoreset", mjDSBL_AUTORESET) WRITEDSBL("nativeccd", mjDSBL_NATIVECCD) + WRITEDSBL("island", mjDSBL_ISLAND) #undef WRITEDSBL #define WRITEENBL(NAME, MASK) \ @@ -1066,7 +1067,6 @@ void mjXWriter::Option(XMLElement* root) { WRITEENBL("fwdinv", mjENBL_FWDINV) WRITEENBL("invdiscrete", mjENBL_INVDISCRETE) WRITEENBL("multiccd", mjENBL_MULTICCD) - WRITEENBL("island", mjENBL_ISLAND) #undef WRITEENBL } diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index 02bfab1e..1537c398 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -587,7 +587,7 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { // force use of sparse matrices, Newton solver, no islands m->opt.jacobian = mjJAC_SPARSE; m->opt.solver = mjSOL_NEWTON; - m->opt.enableflags &= ~mjENBL_ISLAND; + m->opt.disableflags |= mjDSBL_ISLAND; mjData* d = mj_makeData(m); diff --git a/test/benchmark/thread_performance_test.cc b/test/benchmark/thread_performance_test.cc index d0fb153a..4b6183cc 100644 --- a/test/benchmark/thread_performance_test.cc +++ b/test/benchmark/thread_performance_test.cc @@ -32,8 +32,8 @@ void BM_StepHumanoid200(benchmark::State& state) { mjModel* model = mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size()); - model->opt.solver = mjSOL_CG; // use CG solver - model->opt.enableflags |= mjENBL_ISLAND; // enable islands + model->opt.solver = mjSOL_CG; // use CG solver + model->opt.disableflags &= ~mjDSBL_ISLAND; // enable islands mjData* data = mj_makeData(model); mjThreadPool* threadpool = mju_threadPoolCreate(10); @@ -69,8 +69,8 @@ void BM_Step22Humanoids(benchmark::State& state) { std::array error; mjModel* model = mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size()); - model->opt.solver = mjSOL_CG; // use CG solver - model->opt.enableflags |= mjENBL_ISLAND; // enable islands + model->opt.solver = mjSOL_CG; // use CG solver + model->opt.disableflags &= ~mjDSBL_ISLAND; // enable islands mjData* data = mj_makeData(model); mjThreadPool* threadpool = mju_threadPoolCreate(10); diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index 4a6b1865..8075f863 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -381,12 +381,12 @@ TEST_F(IslandTest, IslandFlex) { mjData* data1 = mj_makeData(model); mjData* data2 = mj_makeData(model); - model->opt.enableflags |= mjENBL_ISLAND; + model->opt.disableflags &= ~mjDSBL_ISLAND; while (data1->time < 0.2) { mj_step(model, data1); } - model->opt.enableflags &= ~mjENBL_ISLAND; + model->opt.disableflags |= mjDSBL_ISLAND; while (data2->time < 0.2) { mj_step(model, data2); } diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 66e9c3fd..2d586d23 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -28,7 +28,7 @@ namespace { using ::testing::DoubleNear; using ::testing::NotNull; -using ::std::abs; +using ::testing::Pointwise; using ::std::max; using SolverTest = MujocoTest; @@ -83,10 +83,10 @@ TEST_F(SolverTest, IslandsEquivalent) { mj_getState(model, data_noisland, state, mjSTATE_INTEGRATION); mj_setState(model, data_island, state, mjSTATE_INTEGRATION); - model->opt.enableflags |= mjENBL_ISLAND; // enable islands + model->opt.disableflags &= ~mjDSBL_ISLAND; // enable islands mj_forward(model, data_island); - model->opt.enableflags &= ~mjENBL_ISLAND; // disable islands + model->opt.disableflags |= mjDSBL_ISLAND; // disable islands mj_forward(model, data_noisland); auto time = std::to_string(data_noisland->time); @@ -113,43 +113,58 @@ TEST_F(SolverTest, IslandsEquivalent) { mj_deleteModel(model); } -// compare accelerations produced by CG solver with and without islands +// compare accelerations produced by CG/Newton solver with and without islands TEST_F(SolverTest, IslandsEquivalentForward) { const std::string xml_path = GetTestDataFilePath(kModelPath); char error[1024]; mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; - model->opt.solver = mjSOL_CG; // use CG solver + int nv = model->nv; model->opt.tolerance = 0; // set tolerance to 0 model->opt.ls_tolerance = 0; // set ls_tolerance to 0 - mjtNum rtol = 3e-6; - mjData* data_island = mj_makeData(model); mjData* data_noisland = mj_makeData(model); - for (bool coldstart : {true, false}) { - mj_resetDataKeyframe(model, data_island, 0); - mj_resetDataKeyframe(model, data_noisland, 0); + for (bool warmstart : {false, true}) { + for (mjtJacobian jacobian : {mjJAC_DENSE, mjJAC_SPARSE}) { + for (mjtSolver solver : {mjSOL_CG, mjSOL_NEWTON}) { + for (mjtCone cone : {mjCONE_PYRAMIDAL, mjCONE_ELLIPTIC}) { + if (warmstart) { + model->opt.disableflags &= ~mjDSBL_WARMSTART; + } else { + model->opt.disableflags |= mjDSBL_WARMSTART; + } + model->opt.jacobian = jacobian; + model->opt.solver = solver; + model->opt.cone = cone; - if (coldstart) { - model->opt.disableflags |= mjDSBL_WARMSTART; - } else { - model->opt.disableflags &= ~mjDSBL_WARMSTART; - } + // disable islands, reset and step both datas to populate warmstart + model->opt.disableflags |= mjDSBL_ISLAND; + mj_resetDataKeyframe(model, data_island, 0); + mj_resetDataKeyframe(model, data_noisland, 0); + mj_step(model, data_island); + mj_step(model, data_noisland); - model->opt.enableflags &= ~mjENBL_ISLAND; // disable islands - mj_forward(model, data_noisland); + // forward with islands disabled + mj_forward(model, data_noisland); - model->opt.enableflags |= mjENBL_ISLAND; // enable islands - mj_forward(model, data_island); - for (int j = 0; j < model->nv; j++) { - mjtNum scale = 0.5 * max(2.0, abs(data_noisland->qacc[j]) + - abs(data_island->qacc[j])); - EXPECT_THAT(data_noisland->qacc[j], - DoubleNear(data_island->qacc[j], scale * rtol)) - << "dof: " << j << '\n' - << "rtol: " << scale * rtol; + // forward with islands enabled + model->opt.disableflags &= ~mjDSBL_ISLAND; // enable islands + mj_forward(model, data_island); + + mjtNum scale = 0.5 * (mju_norm(data_noisland->qacc, nv) + + mju_norm(data_island->qacc, nv)); + mjtNum tol = scale * (solver == mjSOL_CG ? 1e-6 : 1e-8); + EXPECT_THAT(AsVector(data_island->qacc, nv), + Pointwise(DoubleNear(scale * tol), + AsVector(data_noisland->qacc, nv))) + << "warmstart: " << warmstart << '\n' + << "jacobian: " << (jacobian ? "sparse" : "dense") << '\n' + << "solver: " << (solver == mjSOL_CG ? "CG" : "Newton") << '\n' + << "cone: " << (cone == 1 ? "elliptic" : "pyramidal"); + } + } } } diff --git a/test/engine/engine_thread_test.cc b/test/engine/engine_thread_test.cc index 4a68956d..7d428c06 100644 --- a/test/engine/engine_thread_test.cc +++ b/test/engine/engine_thread_test.cc @@ -93,12 +93,12 @@ TEST_F(ThreadTest, IslandSingleAndMultiThreadedMatch) { mjModel* model = mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size()); model->opt.solver = mjSOL_CG; // use CG solver - model->opt.enableflags |= mjENBL_ISLAND; // enable islands + model->opt.disableflags &= ~mjDSBL_ISLAND; // enable islands mjModel* model_threaded = mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size()); model_threaded->opt.solver = mjSOL_CG; // use CG solver - model_threaded->opt.enableflags |= mjENBL_ISLAND; // enable islands + model_threaded->opt.disableflags &= ~mjDSBL_ISLAND; // enable islands mjData* data = mj_makeData(model); mjData* data_threaded = mj_makeData(model_threaded); diff --git a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc index 5748f5bb..ebf8c20f 100644 --- a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc +++ b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc @@ -180,7 +180,6 @@ TEST_F(MjcPhysicsSceneTest, TestDefaults) { mjENBL_INVDISCRETE); EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MultiCCDFlag, mjENBL_MULTICCD); - EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(IslandFlag, mjENBL_ISLAND); mj_deleteModel(default_model); mj_deleteSpec(empty_spec); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 634adb40..27b553d5 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -179,7 +179,8 @@ public enum mjtDisableBit : int{ mjDSBL_EULERDAMP = 16384, mjDSBL_AUTORESET = 32768, mjDSBL_NATIVECCD = 65536, - mjNDISABLE = 17, + mjDSBL_ISLAND = 131072, + mjNDISABLE = 18, } public enum mjtEnableBit : int{ mjENBL_OVERRIDE = 1, @@ -187,8 +188,7 @@ public enum mjtEnableBit : int{ mjENBL_FWDINV = 4, mjENBL_INVDISCRETE = 8, mjENBL_MULTICCD = 16, - mjENBL_ISLAND = 32, - mjNENABLE = 6, + mjNENABLE = 5, } public enum mjtJoint : int{ mjJNT_FREE = 0,