chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Make constraint island discovery on by default.

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+8 -8
View File
@@ -587,6 +587,14 @@ from its default.
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.
.. _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:
: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<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** (*)
doc/XMLschema.rst
+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:`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:: |
doc/changelog.rst
+12
View File
@@ -7,9 +7,16 @@ Upcoming version (not yet released)
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
: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
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)
------------------------------
doc/computation/index.rst
+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
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 <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:
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`,
doc/images/computation/island.svg
+1
View File
File diff suppressed because one or more lines are too long
Side by Side

After

Width:  |  Height:  |  Size: 6.7 KiB

doc/includes/references.h
+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_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)
include/mujoco/mjmodel.h
+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_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;
mjx/mujoco/mjx/_src/solver_test.py
+7
View File
@@ -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):
python/mujoco/bindings_test.py
+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_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)
python/mujoco/introspect/enums.py
+3 -3
View File
@@ -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',
python/mujoco/introspect/enums_test.py
+1 -2
View File
@@ -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
python/mujoco/structs_wrappers.cc
+2
View File
@@ -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
simulate/simulate.cc
+1 -1
View File
@@ -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");
src/engine/engine_forward.c
+1 -1
View File
@@ -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);
src/engine/engine_island.c
+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;
// 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);
src/engine/engine_support.c
+3 -3
View File
@@ -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"
};
src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc
+2 -2
View File
@@ -643,7 +643,6 @@ class ModelWriter {
const std::vector<std::pair<pxr::TfToken, int>> 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);
}
src/experimental/usd/usd_to_mjspec.cc
+4 -4
View File
@@ -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()) {
src/xml/xml_native_reader.cc
+2 -2
View File
@@ -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
}
}
src/xml/xml_native_writer.cc
+1 -1
View File
@@ -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
}
test/benchmark/engine_util_sparse_benchmark_test.cc
+1 -1
View File
@@ -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);
test/benchmark/thread_performance_test.cc
+4 -4
View File
@@ -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<char, 1024> 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);
test/engine/engine_island_test.cc
+2 -2
View File
@@ -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);
}
test/engine/engine_solver_test.cc
+41 -26
View File
@@ -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");
}
}
}
}
test/engine/engine_thread_test.cc
+2 -2
View File
@@ -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);
test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc
-1
View File
@@ -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);
unity/Runtime/Bindings/MjBindings.cs
+3 -3
View File
@@ -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,