Make constraint island discovery on by default.

Also fix latent bug in mjData serialization.

PiperOrigin-RevId: 799505349
Change-Id: I299e7cc8133fa2ec0b339a7ff3d02543e06778b1
This commit is contained in:
Yuval Tassa
2025-08-26 04:26:30 -07:00
committed by Copybara-Service
parent 4cf5c37211
commit ec94bb49aa
27 changed files with 147 additions and 81 deletions
+8 -8
View File
@@ -587,6 +587,14 @@ from its default.
This flag enables the native convex collision detection pipeline instead of using the This flag enables the native convex collision detection pipeline instead of using the
`libccd library <https://github.com/danfis/libccd>`__, see :ref:`convex collisions<coCCD>` for more details. `libccd library <https://github.com/danfis/libccd>`__, see :ref:`convex collisions<coCCD>` 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 <mjtVisFlag>` enables
`island visualization <https://youtu.be/Vc1tq0fFvQA>`__.
.. _option-flag-eulerdamp: .. _option-flag-eulerdamp:
:at:`eulerdamp`: :at-val:`[disable, enable], "enable"` :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 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<coCCD>` for more details. selected convex collision pipeline, see :ref:`convex collisions<coCCD>` 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 <https://youtu.be/Vc1tq0fFvQA>`__.
In a future release, the constraint solver will exploit the disjoint nature of constraint islands.
.. _compiler: .. _compiler:
**compiler** (*) **compiler** (*)
+2 -2
View File
@@ -37,9 +37,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`sensor<option-flag-sensor>` | :ref:`midphase<option-flag-midphase>` | :ref:`eulerdamp<option-flag-eulerdamp>` | :ref:`autoreset<option-flag-autoreset>` | | | | | | :ref:`sensor<option-flag-sensor>` | :ref:`midphase<option-flag-midphase>` | :ref:`eulerdamp<option-flag-eulerdamp>` | :ref:`autoreset<option-flag-autoreset>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`override<option-flag-override>` | :ref:`energy<option-flag-energy>` | :ref:`fwdinv<option-flag-fwdinv>` | :ref:`invdiscrete<option-flag-invdiscrete>` | | | | | | :ref:`nativeccd<option-flag-nativeccd>` | :ref:`island<option-flag-island>` | :ref:`override<option-flag-override>` | :ref:`energy<option-flag-energy>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`multiccd<option-flag-multiccd>` | :ref:`island<option-flag-island>` | :ref:`nativeccd<option-flag-nativeccd>` | | | | | | | :ref:`fwdinv<option-flag-fwdinv>` | :ref:`invdiscrete<option-flag-invdiscrete>` | :ref:`multiccd<option-flag-multiccd>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| mujoco |br| |L| | | .. table:: | | mujoco |br| |L| | | .. table:: |
+12
View File
@@ -7,9 +7,16 @@ Upcoming version (not yet released)
General General
^^^^^^^ ^^^^^^^
- Constraint island discovery and construction, previously an experimental feature, is now :ref:`documented<soIsland>`
and promoted to default; disable it with :ref:`option/flag/island <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 .. admonition:: Breaking API changes
:class: attention :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 <mjtDisableBit>`.
- The update of ``mjData.qacc_warmstart`` was moved from the end of the solver call (:ref:`mj_fwdConstraint`) to the - 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` end of :ref:`mj_step`, and is now updated with all other state variables. This change makes :ref:`mj_forward`
fully idempotent. 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``. - ``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) Version 3.3.5 (August 8, 2025)
------------------------------ ------------------------------
+35 -4
View File
@@ -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 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 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 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 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 can be up to 5-dimensional, given our contact model. Now we optimize the quadratic cost within this ellipsoid. This
an instance of quadratically constrained quadratic programming (QCQP). Since there is only one scalar constraint 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 (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, 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 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 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. 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 <Framework>` 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 <https://github.com/google-deepmind/mujoco/blob/main/src/engine/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: .. _soParameters:
Parameters 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` 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: 8. Construct the list of active contacts. This includes both broad-phase and near-phase collision detection:
:ref:`mj_collision` :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` 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` 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`, 12. Compute sensor data that only depends on position, and the potential energy if enabled: :ref:`mj_sensorPos`,
File diff suppressed because one or more lines are too long

After

Width:  |  Height:  |  Size: 6.7 KiB

+3 -3
View File
@@ -464,8 +464,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags
mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator
mjDSBL_AUTORESET = 1<<15, // automatic reset when numerical issues are detected mjDSBL_AUTORESET = 1<<15, // automatic reset when numerical issues are detected
mjDSBL_NATIVECCD = 1<<16, // native convex collision detection 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; } mjtDisableBit;
typedef enum mjtEnableBit_ { // enable optional feature bitflags typedef enum mjtEnableBit_ { // enable optional feature bitflags
mjENBL_OVERRIDE = 1<<0, // override contact parameters 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 mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics
// experimental features: // experimental features:
mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection 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; } mjtEnableBit;
typedef enum mjtJoint_ { // type of degree of freedom typedef enum mjtJoint_ { // type of degree of freedom
mjJNT_FREE = 0, // global position and orientation (quat) (7) mjJNT_FREE = 0, // global position and orientation (quat) (7)
+3 -3
View File
@@ -66,8 +66,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags
mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator
mjDSBL_AUTORESET = 1<<15, // automatic reset when numerical issues are detected mjDSBL_AUTORESET = 1<<15, // automatic reset when numerical issues are detected
mjDSBL_NATIVECCD = 1<<16, // native convex collision detection 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; } mjtDisableBit;
@@ -78,9 +79,8 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags
mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics
// experimental features: // experimental features:
mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection 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; } mjtEnableBit;
+7
View File
@@ -58,6 +58,13 @@ class SolverTest(parameterized.TestCase):
m.opt.solver = solver_ m.opt.solver = solver_
m.opt.cone = cone m.opt.cone = cone
m.opt.iterations = iterations 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) d = mujoco.MjData(m)
def cost(qacc): def cost(qacc):
+1 -1
View File
@@ -946,7 +946,7 @@ Euler integrator, semi-implicit in velocity.
self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1 << 0) self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1 << 0)
self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1 << 1) self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1 << 1)
self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1 << 2) 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_PLANE, 0)
self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1) self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1)
self.assertEqual(mujoco.mjtGeom.mjGEOM_SPHERE, 2) self.assertEqual(mujoco.mjtGeom.mjGEOM_SPHERE, 2)
+3 -3
View File
@@ -44,7 +44,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjDSBL_EULERDAMP', 16384), ('mjDSBL_EULERDAMP', 16384),
('mjDSBL_AUTORESET', 32768), ('mjDSBL_AUTORESET', 32768),
('mjDSBL_NATIVECCD', 65536), ('mjDSBL_NATIVECCD', 65536),
('mjNDISABLE', 17), ('mjDSBL_ISLAND', 131072),
('mjNDISABLE', 18),
]), ]),
)), )),
('mjtEnableBit', ('mjtEnableBit',
@@ -57,8 +58,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjENBL_FWDINV', 4), ('mjENBL_FWDINV', 4),
('mjENBL_INVDISCRETE', 8), ('mjENBL_INVDISCRETE', 8),
('mjENBL_MULTICCD', 16), ('mjENBL_MULTICCD', 16),
('mjENBL_ISLAND', 32), ('mjNENABLE', 5),
('mjNENABLE', 6),
]), ]),
)), )),
('mjtJoint', ('mjtJoint',
+1 -2
View File
@@ -43,8 +43,7 @@ class EnumsTest(absltest.TestCase):
('mjENBL_FWDINV', 1<<2), ('mjENBL_FWDINV', 1<<2),
('mjENBL_INVDISCRETE', 1<<3), ('mjENBL_INVDISCRETE', 1<<3),
('mjENBL_MULTICCD', 1<<4), ('mjENBL_MULTICCD', 1<<4),
('mjENBL_ISLAND', 1<<5), ('mjNENABLE', 5)))
('mjNENABLE', 6)))
# values mostly increment by one with occasional overrides # values mostly increment by one with occasional overrides
def test_mjtGeom(self): # pylint: disable=invalid-name def test_mjtGeom(self): # pylint: disable=invalid-name
+2
View File
@@ -736,6 +736,7 @@ void MjDataWrapper::Serialize(std::ostream& output) const {
X(nA); X(nA);
X(nefc); X(nefc);
X(nisland); X(nisland);
X(nidof);
X(time); X(time);
X(energy); X(energy);
#undef X #undef X
@@ -815,6 +816,7 @@ MjDataWrapper MjDataWrapper::Deserialize(std::istream& input) {
X(nA); X(nA);
X(nefc); X(nefc);
X(nisland); X(nisland);
X(nidof);
X(time); X(time);
X(energy); X(energy);
#undef X #undef X
+1 -1
View File
@@ -647,7 +647,7 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
} }
// add islands if enabled // add islands if enabled
if (mjENABLED(mjENBL_ISLAND)) { if (!mjDISABLED(mjDSBL_ISLAND) && d->nisland > 0) {
mju::sprintf_arr(tmp, "\n%d", d->nisland); mju::sprintf_arr(tmp, "\n%d", d->nisland);
mju::strcat_arr(content, tmp); mju::strcat_arr(content, tmp);
mju::strcat_arr(title, "\nIslands"); mju::strcat_arr(title, "\nIslands");
+1 -1
View File
@@ -723,7 +723,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
mju_zeroInt(d->solver_niter, mjNISLAND); mju_zeroInt(d->solver_niter, mjNISLAND);
// check if islands are supported // check if islands are supported
int islands_supported = mjENABLED(mjENBL_ISLAND) && int islands_supported = !mjDISABLED(mjDSBL_ISLAND) &&
nisland > 0 && nisland > 0 &&
m->opt.noslip_iterations == 0 && m->opt.noslip_iterations == 0 &&
(m->opt.solver == mjSOL_CG || m->opt.solver == mjSOL_NEWTON); (m->opt.solver == mjSOL_CG || m->opt.solver == mjSOL_NEWTON);
+2 -2
View File
@@ -411,7 +411,7 @@ void mj_island(const mjModel* m, mjData* d) {
int nv = m->nv, nefc = d->nefc, ntree=m->ntree; int nv = m->nv, nefc = d->nefc, ntree=m->ntree;
// no constraints: quick return // no constraints: quick return
if (!mjENABLED(mjENBL_ISLAND) || !nefc) { if (mjDISABLED(mjDSBL_ISLAND) || !nefc) {
d->nisland = d->nidof = 0; d->nisland = d->nidof = 0;
return; return;
} }
@@ -454,7 +454,7 @@ void mj_island(const mjModel* m, mjData* d) {
return; return;
} }
// count ni: total number of dofs in islands // count nidof: total number of dofs in islands
int nidof = 0; int nidof = 0;
for (int i=0; i < nv; i++) { for (int i=0; i < nv; i++) {
nidof += (tree_island[m->dof_treeid[i]] >= 0); nidof += (tree_island[m->dof_treeid[i]] >= 0);
+3 -3
View File
@@ -62,7 +62,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = {
"Midphase", "Midphase",
"Eulerdamp", "Eulerdamp",
"AutoReset", "AutoReset",
"NativeCCD" "NativeCCD",
"Island"
}; };
@@ -72,8 +73,7 @@ const char* mjENABLESTRING[mjNENABLE] = {
"Energy", "Energy",
"Fwdinv", "Fwdinv",
"InvDiscrete", "InvDiscrete",
"MultiCCD", "MultiCCD"
"Island"
}; };
@@ -643,7 +643,6 @@ class ModelWriter {
const std::vector<std::pair<pxr::TfToken, int>> enable_flags = { const std::vector<std::pair<pxr::TfToken, int>> enable_flags = {
{MjcPhysicsTokens->mjcFlagMulticcd, mjENBL_MULTICCD}, {MjcPhysicsTokens->mjcFlagMulticcd, mjENBL_MULTICCD},
{MjcPhysicsTokens->mjcFlagIsland, mjENBL_ISLAND},
{MjcPhysicsTokens->mjcFlagFwdinv, mjENBL_FWDINV}, {MjcPhysicsTokens->mjcFlagFwdinv, mjENBL_FWDINV},
{MjcPhysicsTokens->mjcFlagEnergy, mjENBL_ENERGY}, {MjcPhysicsTokens->mjcFlagEnergy, mjENBL_ENERGY},
{MjcPhysicsTokens->mjcFlagOverride, mjENBL_OVERRIDE}, {MjcPhysicsTokens->mjcFlagOverride, mjENBL_OVERRIDE},
@@ -669,7 +668,8 @@ class ModelWriter {
{MjcPhysicsTokens->mjcFlagMidphase, mjDSBL_MIDPHASE}, {MjcPhysicsTokens->mjcFlagMidphase, mjDSBL_MIDPHASE},
{MjcPhysicsTokens->mjcFlagEulerdamp, mjDSBL_EULERDAMP}, {MjcPhysicsTokens->mjcFlagEulerdamp, mjDSBL_EULERDAMP},
{MjcPhysicsTokens->mjcFlagAutoreset, mjDSBL_AUTORESET}, {MjcPhysicsTokens->mjcFlagAutoreset, mjDSBL_AUTORESET},
{MjcPhysicsTokens->mjcFlagNativeccd, mjDSBL_NATIVECCD}}; {MjcPhysicsTokens->mjcFlagNativeccd, mjDSBL_NATIVECCD},
{MjcPhysicsTokens->mjcFlagIsland, mjDSBL_ISLAND}};
for (const auto &[token, flag] : disable_flags) { for (const auto &[token, flag] : disable_flags) {
create_flag_attr(token, flag, false); create_flag_attr(token, flag, false);
} }
+4 -4
View File
@@ -617,6 +617,10 @@ void ParseUsdPhysicsScene(mjSpec* spec,
mjc_physics_scene.GetAutoResetFlagAttr().Get(&autoreset_flag); mjc_physics_scene.GetAutoResetFlagAttr().Get(&autoreset_flag);
spec->option.disableflags |= (!autoreset_flag ? mjDSBL_AUTORESET : 0); 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; bool override_flag;
mjc_physics_scene.GetOverrideFlagAttr().Get(&override_flag); mjc_physics_scene.GetOverrideFlagAttr().Get(&override_flag);
spec->option.enableflags |= (override_flag ? mjENBL_OVERRIDE : 0); spec->option.enableflags |= (override_flag ? mjENBL_OVERRIDE : 0);
@@ -637,10 +641,6 @@ void ParseUsdPhysicsScene(mjSpec* spec,
mjc_physics_scene.GetMultiCCDFlagAttr().Get(&multiccd_flag); mjc_physics_scene.GetMultiCCDFlagAttr().Get(&multiccd_flag);
spec->option.enableflags |= (multiccd_flag ? mjENBL_MULTICCD : 0); 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 // Compiler attributes
auto auto_limits_attr = mjc_physics_scene.GetAutoLimitsAttr(); auto auto_limits_attr = mjc_physics_scene.GetAutoLimitsAttr();
if (auto_limits_attr.HasAuthoredValue()) { if (auto_limits_attr.HasAuthoredValue()) {
+2 -2
View File
@@ -121,7 +121,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"flag", "?", "23", "constraint", "equality", "frictionloss", "limit", "contact", {"flag", "?", "23", "constraint", "equality", "frictionloss", "limit", "contact",
"passive", "gravity", "clampctrl", "warmstart", "passive", "gravity", "clampctrl", "warmstart",
"filterparent", "actuation", "refsafe", "sensor", "midphase", "eulerdamp", "autoreset", "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", {"size", "*", "14", "memory", "njmax", "nconmax", "nstack", "nuserdata", "nkey",
@@ -1199,6 +1199,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
READDSBL("eulerdamp", mjDSBL_EULERDAMP) READDSBL("eulerdamp", mjDSBL_EULERDAMP)
READDSBL("autoreset", mjDSBL_AUTORESET) READDSBL("autoreset", mjDSBL_AUTORESET)
READDSBL("nativeccd", mjDSBL_NATIVECCD) READDSBL("nativeccd", mjDSBL_NATIVECCD)
READDSBL("island", mjDSBL_ISLAND)
#undef READDSBL #undef READDSBL
#define READENBL(NAME, MASK) \ #define READENBL(NAME, MASK) \
@@ -1211,7 +1212,6 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
READENBL("fwdinv", mjENBL_FWDINV) READENBL("fwdinv", mjENBL_FWDINV)
READENBL("invdiscrete", mjENBL_INVDISCRETE) READENBL("invdiscrete", mjENBL_INVDISCRETE)
READENBL("multiccd", mjENBL_MULTICCD) READENBL("multiccd", mjENBL_MULTICCD)
READENBL("island", mjENBL_ISLAND)
#undef READENBL #undef READENBL
} }
} }
+1 -1
View File
@@ -1056,6 +1056,7 @@ void mjXWriter::Option(XMLElement* root) {
WRITEDSBL("eulerdamp", mjDSBL_EULERDAMP) WRITEDSBL("eulerdamp", mjDSBL_EULERDAMP)
WRITEDSBL("autoreset", mjDSBL_AUTORESET) WRITEDSBL("autoreset", mjDSBL_AUTORESET)
WRITEDSBL("nativeccd", mjDSBL_NATIVECCD) WRITEDSBL("nativeccd", mjDSBL_NATIVECCD)
WRITEDSBL("island", mjDSBL_ISLAND)
#undef WRITEDSBL #undef WRITEDSBL
#define WRITEENBL(NAME, MASK) \ #define WRITEENBL(NAME, MASK) \
@@ -1066,7 +1067,6 @@ void mjXWriter::Option(XMLElement* root) {
WRITEENBL("fwdinv", mjENBL_FWDINV) WRITEENBL("fwdinv", mjENBL_FWDINV)
WRITEENBL("invdiscrete", mjENBL_INVDISCRETE) WRITEENBL("invdiscrete", mjENBL_INVDISCRETE)
WRITEENBL("multiccd", mjENBL_MULTICCD) WRITEENBL("multiccd", mjENBL_MULTICCD)
WRITEENBL("island", mjENBL_ISLAND)
#undef WRITEENBL #undef WRITEENBL
} }
@@ -587,7 +587,7 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) {
// force use of sparse matrices, Newton solver, no islands // force use of sparse matrices, Newton solver, no islands
m->opt.jacobian = mjJAC_SPARSE; m->opt.jacobian = mjJAC_SPARSE;
m->opt.solver = mjSOL_NEWTON; m->opt.solver = mjSOL_NEWTON;
m->opt.enableflags &= ~mjENBL_ISLAND; m->opt.disableflags |= mjDSBL_ISLAND;
mjData* d = mj_makeData(m); mjData* d = mj_makeData(m);
+4 -4
View File
@@ -32,8 +32,8 @@ void BM_StepHumanoid200(benchmark::State& state) {
mjModel* model = mjModel* model =
mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size()); mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
model->opt.solver = mjSOL_CG; // use CG solver model->opt.solver = mjSOL_CG; // use CG solver
model->opt.enableflags |= mjENBL_ISLAND; // enable islands model->opt.disableflags &= ~mjDSBL_ISLAND; // enable islands
mjData* data = mj_makeData(model); mjData* data = mj_makeData(model);
mjThreadPool* threadpool = mju_threadPoolCreate(10); mjThreadPool* threadpool = mju_threadPoolCreate(10);
@@ -69,8 +69,8 @@ void BM_Step22Humanoids(benchmark::State& state) {
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = mjModel* model =
mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size()); mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
model->opt.solver = mjSOL_CG; // use CG solver model->opt.solver = mjSOL_CG; // use CG solver
model->opt.enableflags |= mjENBL_ISLAND; // enable islands model->opt.disableflags &= ~mjDSBL_ISLAND; // enable islands
mjData* data = mj_makeData(model); mjData* data = mj_makeData(model);
mjThreadPool* threadpool = mju_threadPoolCreate(10); mjThreadPool* threadpool = mju_threadPoolCreate(10);
+2 -2
View File
@@ -381,12 +381,12 @@ TEST_F(IslandTest, IslandFlex) {
mjData* data1 = mj_makeData(model); mjData* data1 = mj_makeData(model);
mjData* data2 = mj_makeData(model); mjData* data2 = mj_makeData(model);
model->opt.enableflags |= mjENBL_ISLAND; model->opt.disableflags &= ~mjDSBL_ISLAND;
while (data1->time < 0.2) { while (data1->time < 0.2) {
mj_step(model, data1); mj_step(model, data1);
} }
model->opt.enableflags &= ~mjENBL_ISLAND; model->opt.disableflags |= mjDSBL_ISLAND;
while (data2->time < 0.2) { while (data2->time < 0.2) {
mj_step(model, data2); mj_step(model, data2);
} }
+41 -26
View File
@@ -28,7 +28,7 @@ namespace {
using ::testing::DoubleNear; using ::testing::DoubleNear;
using ::testing::NotNull; using ::testing::NotNull;
using ::std::abs; using ::testing::Pointwise;
using ::std::max; using ::std::max;
using SolverTest = MujocoTest; using SolverTest = MujocoTest;
@@ -83,10 +83,10 @@ TEST_F(SolverTest, IslandsEquivalent) {
mj_getState(model, data_noisland, state, mjSTATE_INTEGRATION); mj_getState(model, data_noisland, state, mjSTATE_INTEGRATION);
mj_setState(model, data_island, 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); mj_forward(model, data_island);
model->opt.enableflags &= ~mjENBL_ISLAND; // disable islands model->opt.disableflags |= mjDSBL_ISLAND; // disable islands
mj_forward(model, data_noisland); mj_forward(model, data_noisland);
auto time = std::to_string(data_noisland->time); auto time = std::to_string(data_noisland->time);
@@ -113,43 +113,58 @@ TEST_F(SolverTest, IslandsEquivalent) {
mj_deleteModel(model); 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) { TEST_F(SolverTest, IslandsEquivalentForward) {
const std::string xml_path = GetTestDataFilePath(kModelPath); const std::string xml_path = GetTestDataFilePath(kModelPath);
char error[1024]; char error[1024];
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << 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.tolerance = 0; // set tolerance to 0
model->opt.ls_tolerance = 0; // set ls_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_island = mj_makeData(model);
mjData* data_noisland = mj_makeData(model); mjData* data_noisland = mj_makeData(model);
for (bool coldstart : {true, false}) { for (bool warmstart : {false, true}) {
mj_resetDataKeyframe(model, data_island, 0); for (mjtJacobian jacobian : {mjJAC_DENSE, mjJAC_SPARSE}) {
mj_resetDataKeyframe(model, data_noisland, 0); 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) { // disable islands, reset and step both datas to populate warmstart
model->opt.disableflags |= mjDSBL_WARMSTART; model->opt.disableflags |= mjDSBL_ISLAND;
} else { mj_resetDataKeyframe(model, data_island, 0);
model->opt.disableflags &= ~mjDSBL_WARMSTART; mj_resetDataKeyframe(model, data_noisland, 0);
} mj_step(model, data_island);
mj_step(model, data_noisland);
model->opt.enableflags &= ~mjENBL_ISLAND; // disable islands // forward with islands disabled
mj_forward(model, data_noisland); mj_forward(model, data_noisland);
model->opt.enableflags |= mjENBL_ISLAND; // enable islands // forward with islands enabled
mj_forward(model, data_island); model->opt.disableflags &= ~mjDSBL_ISLAND; // enable islands
for (int j = 0; j < model->nv; j++) { mj_forward(model, data_island);
mjtNum scale = 0.5 * max(2.0, abs(data_noisland->qacc[j]) +
abs(data_island->qacc[j])); mjtNum scale = 0.5 * (mju_norm(data_noisland->qacc, nv) +
EXPECT_THAT(data_noisland->qacc[j], mju_norm(data_island->qacc, nv));
DoubleNear(data_island->qacc[j], scale * rtol)) mjtNum tol = scale * (solver == mjSOL_CG ? 1e-6 : 1e-8);
<< "dof: " << j << '\n' EXPECT_THAT(AsVector(data_island->qacc, nv),
<< "rtol: " << scale * rtol; 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");
}
}
} }
} }
+2 -2
View File
@@ -93,12 +93,12 @@ TEST_F(ThreadTest, IslandSingleAndMultiThreadedMatch) {
mjModel* model = mjModel* model =
mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size()); mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
model->opt.solver = mjSOL_CG; // use CG solver 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 = mjModel* model_threaded =
mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size()); mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
model_threaded->opt.solver = mjSOL_CG; // use CG solver 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 = mj_makeData(model);
mjData* data_threaded = mj_makeData(model_threaded); mjData* data_threaded = mj_makeData(model_threaded);
@@ -180,7 +180,6 @@ TEST_F(MjcPhysicsSceneTest, TestDefaults) {
mjENBL_INVDISCRETE); mjENBL_INVDISCRETE);
EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MultiCCDFlag, EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MultiCCDFlag,
mjENBL_MULTICCD); mjENBL_MULTICCD);
EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(IslandFlag, mjENBL_ISLAND);
mj_deleteModel(default_model); mj_deleteModel(default_model);
mj_deleteSpec(empty_spec); mj_deleteSpec(empty_spec);
+3 -3
View File
@@ -179,7 +179,8 @@ public enum mjtDisableBit : int{
mjDSBL_EULERDAMP = 16384, mjDSBL_EULERDAMP = 16384,
mjDSBL_AUTORESET = 32768, mjDSBL_AUTORESET = 32768,
mjDSBL_NATIVECCD = 65536, mjDSBL_NATIVECCD = 65536,
mjNDISABLE = 17, mjDSBL_ISLAND = 131072,
mjNDISABLE = 18,
} }
public enum mjtEnableBit : int{ public enum mjtEnableBit : int{
mjENBL_OVERRIDE = 1, mjENBL_OVERRIDE = 1,
@@ -187,8 +188,7 @@ public enum mjtEnableBit : int{
mjENBL_FWDINV = 4, mjENBL_FWDINV = 4,
mjENBL_INVDISCRETE = 8, mjENBL_INVDISCRETE = 8,
mjENBL_MULTICCD = 16, mjENBL_MULTICCD = 16,
mjENBL_ISLAND = 32, mjNENABLE = 5,
mjNENABLE = 6,
} }
public enum mjtJoint : int{ public enum mjtJoint : int{
mjJNT_FREE = 0, mjJNT_FREE = 0,