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

Replace the flex metric factorization with a block preconditioner

Browse Source 
Every step, the flex block of the implicit effective metric M + K was
factorized by sparse Cholesky, because K depends on the configuration. On
model/flex/bag.xml, added here, that is roughly half the step, against a
comparable share for the constraint solve it exists to accelerate.

Keep only the metric's per-vertex 3x3 diagonal blocks, prefactored. Neither
consumer needs the exact inverse: the CG constraint solver only wants a
preconditioner, and qacc_smooth can come from an iterative solve using those
blocks. They are O(n) to build and to apply, but weaker, so CG runs about twice
the iterations and qacc_smooth becomes an iteration rather than a direct solve.
Net, the bag model steps roughly twice as fast.

The preconditioner, by metric state. Inactive, meaning no flex elasticity or an
explicit integrator: M^-1, unchanged. Bending only (nefmK == 0): M^-1 plus the
exact constant bending factor from mj_setConst on the dofs it covers,
unchanged; that factor is built at model compile time and costs nothing per
step. Per-step stiffness: M^-1 plus the 3x3 blocks, where before it was a
per-step sparse Cholesky, or, when M couples across the flex block, an inner
PCG of up to 50 iterations run once per outer CG iteration.

Only models carrying per-step stretch stiffness change in wall-clock. Both
ponchos hold their timing and take slightly fewer CG iterations than before,
because the preconditioner is now symmetric: it applies M^-1 and the covered
blocks to disjoint sets of dofs, where previously the two overlapped and the
operator was not symmetric, which PCG requires.

mjd_effSolve is the accurate solve of (M + K)x = b; what used to carry that
name only preconditions and is now mjd_effPrec. Its CG guarded the division by
pAp with mjMINVAL, an absolute floor on a quantity that scales with the square
of the right-hand side, so a small b aborted the solve while the curvature was
healthy: four flex models were quietly left short of tolerance. For an SPD
metric the guard is positivity, and with that the same solves converge. The qacc_smooth call site in
mj_fwdAcceleration is textually unchanged but now reaches the iterative solve,
which converges on opt.tolerance rather than a hardcoded threshold, floored in
mjUSESINGLE builds where the squared target is unreachable in float. Reaching
the iteration cap names the ill-conditioned flex stiffness and then reports it
through mjWARN_INERTIA, rather than returning an under-converged result.
Covered dofs are located by walking the covered rows of the stiffness matrix,
as they need not be 3-aligned from dof 0: any joint declared before a flexcomp
shifts them.

mjData.efm_L_rownnz, efm_L_rowadr and efm_L_colind described the sparsity of
the deleted factorization and are removed: left NULL with nonzero mjxmacro
extents they made the Python bindings hand back uninitialized arrays.
efm_active loses the value 2 for the same reason, nothing selects a solve path
on preconditioner exactness any more. Both are recorded under breaking changes.

model/flex/bag.xml is added because no shipped model carried per-step stretch
stiffness. The ponchos are bending-only and trampoline.xml uses an explicit
integrator, so the metric never activates there. It is excluded from
WriteReadCompareTest: stretch stiffness amplifies rest geometry that XML rounds
on save.
This commit is contained in:
Alessio
2026-07-29 14:19:58 +01:00
parent d400914e97
commit 55d13aec5f
16 changed files with 4914 additions and 485 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+28
View File
@@ -2,6 +2,34 @@
Changelog
=========
Upcoming version (not yet released)
-----------------------------------
Engine
^^^^^^
- Replaced the per-step sparse Cholesky factorization of the flex block of the implicit effective metric M + K with
its prefactored per-vertex 3x3 diagonal blocks. The blocks precondition the CG constraint solver and drive an
iterative solve for ``qacc_smooth``, which now converges on :ref:`tolerance<option-tolerance>` rather than a fixed
threshold. Flexes with :ref:`elastic2d<flex-elasticity-elastic2d>` stretch stiffness step roughly twice as fast;
bending-only flexes keep the exact constant factor and are unchanged.
.. admonition:: Breaking API changes
:class: attention
- Removed ``mjData.efm_L_rownnz``, ``mjData.efm_L_rowadr`` and ``mjData.efm_L_colind``. They described the sparsity
of the effective-metric Cholesky factor, which no longer exists; ``mjData.efm_L`` now holds dense 3x3 blocks,
9 numbers per covered vertex. ``mjData.efm_active`` no longer takes the value 2: nothing selects a solve path on
preconditioner exactness, so it is now a plain 0/1 flag.
Models
^^^^^^
- Added `bag <https://github.com/google-deepmind/mujoco/blob/main/model/flex/bag.xml>`__ example model: a cloth bag,
held open by pinning the ring of vertices around its mouth, catching the standard humanoid dropped in from above.
Unlike the poncho models, which are bending-only, this model exercises the 2D
:ref:`stretch<flex-elasticity-elastic2d>` elasticity of a flex.
Version 3.11.0 (July 27, 2026)
------------------------------
doc/includes/references.h
+5 -8
View File
@@ -112,10 +112,10 @@ typedef struct mjData_ {
int nl; // number of limit constraints
int nefc; // number of constraints
int nJ; // number of non-zeros in constraint Jacobian
int efm_active; // implicit effective metric M+K: 0 inactive, 1 active, 2 active + preconditioner exact
int efm_active; // implicit effective metric M+K is active (see mjd_effBuild)
int nefmK; // number of non-zeros in effective-stiffness CSR
int nefmdof; // number of rows in effective-metric factor
int nefmL; // number of non-zeros in the effective-metric factor
int nefmdof; // number of 3x3 blocks in the effective-metric preconditioner
int nefmL; // size of the effective-metric block storage (9*nefmdof)
int nY; // number of non-zeros in constraint inverse inertia square root
int nA; // number of non-zeros in constraint inverse inertia matrix
int nisland; // number of detected constraint islands
@@ -382,11 +382,8 @@ typedef struct mjData_ {
int* efm_K_rowadr; // effective-stiffness CSR row addresses (nv x 1)
int* efm_K_colind; // effective-stiffness CSR column indices (nefmK x 1)
mjtNum* efm_K_val; // effective-stiffness CSR values (nefmK x 1)
int* efm_dofid; // factor row -> dof address (nefmdof x 1)
int* efm_L_rownnz; // factor row nonzeros (nefmdof x 1)
int* efm_L_rowadr; // factor row addresses (nefmdof x 1)
int* efm_L_colind; // factor column indices (nefmL x 1)
mjtNum* efm_L; // Cholesky factor of diag(M)+K, covered dofs (nefmL x 1)
int* efm_dofid; // block k -> dof address of its vertex triple (nefmdof x 1)
mjtNum* efm_L; // factored 3x3 diagonal blocks of M+K (nefmL x 1)
//-------------------- arena-allocated: POSITION, VELOCITY, CONTROL/ACCELERATION dependent
include/mujoco/mjdata.h
+5 -8
View File
@@ -136,10 +136,10 @@ typedef struct mjData_ {
int nl; // number of limit constraints
int nefc; // number of constraints
int nJ; // number of non-zeros in constraint Jacobian
int efm_active; // implicit effective metric M+K: 0 inactive, 1 active, 2 active + preconditioner exact
int efm_active; // implicit effective metric M+K is active (see mjd_effBuild)
int nefmK; // number of non-zeros in effective-stiffness CSR
int nefmdof; // number of rows in effective-metric factor
int nefmL; // number of non-zeros in the effective-metric factor
int nefmdof; // number of 3x3 blocks in the effective-metric preconditioner
int nefmL; // size of the effective-metric block storage (9*nefmdof)
int nY; // number of non-zeros in constraint inverse inertia square root
int nA; // number of non-zeros in constraint inverse inertia matrix
int nisland; // number of detected constraint islands
@@ -406,11 +406,8 @@ typedef struct mjData_ {
int* efm_K_rowadr; // effective-stiffness CSR row addresses (nv x 1)
int* efm_K_colind; // effective-stiffness CSR column indices (nefmK x 1)
mjtNum* efm_K_val; // effective-stiffness CSR values (nefmK x 1)
int* efm_dofid; // factor row -> dof address (nefmdof x 1)
int* efm_L_rownnz; // factor row nonzeros (nefmdof x 1)
int* efm_L_rowadr; // factor row addresses (nefmdof x 1)
int* efm_L_colind; // factor column indices (nefmL x 1)
mjtNum* efm_L; // Cholesky factor of diag(M)+K, covered dofs (nefmL x 1)
int* efm_dofid; // block k -> dof address of its vertex triple (nefmdof x 1)
mjtNum* efm_L; // factored 3x3 diagonal blocks of M+K (nefmL x 1)
//-------------------- arena-allocated: POSITION, VELOCITY, CONTROL/ACCELERATION dependent
include/mujoco/mjxmacro.h
-3
View File
@@ -1015,9 +1015,6 @@
X ( int, efm_K_colind, MJ_D(nefmK), 1 ) \
X ( mjtNum, efm_K_val, MJ_D(nefmK), 1 ) \
X ( int, efm_dofid, MJ_D(nefmdof), 1 ) \
X ( int, efm_L_rownnz, MJ_D(nefmdof), 1 ) \
X ( int, efm_L_rowadr, MJ_D(nefmdof), 1 ) \
X ( int, efm_L_colind, MJ_D(nefmL), 1 ) \
X ( mjtNum, efm_L, MJ_D(nefmL), 1 )
model/flex/asset/bag.obj
+4551
View File
File diff suppressed because it is too large Load Diff
model/flex/bag.xml
+74
View File
@@ -0,0 +1,74 @@
<!-- Copyright 2026 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco model="Bag">
<compiler angle="radian" meshdir="asset"/>
<option timestep="0.001" jacobian="sparse" integrator="implicitfast" solver="CG"
iterations="2000" tolerance="1e-4"/>
<statistic extent="2.5" center="0 0 1"/>
<visual>
<map force="0.1" zfar="30"/>
<global offwidth="2560" offheight="1440" elevation="-20" azimuth="120"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0"
width="512" height="512"/>
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
<material name="matplane" reflectance="0.3" texture="texplane" texrepeat="10 10"
texuniform="true"/>
<model name="humanoid" file="../humanoid/humanoid.xml"/>
</asset>
<worldbody>
<geom name="ground" type="plane" size="0 0 1" material="matplane" condim="1"/>
<!-- Both lights are directional. A positional light inside the bag's closed volume (as in
model/flex/scene.xml, which puts one at z=2) blacks out the cloth around it. -->
<light directional="true" diffuse=".7 .7 .7" specular=".1 .1 .1" pos="0 0 4" dir="0 0 -1"/>
<light directional="true" diffuse=".4 .4 .4" specular=".1 .1 .1" pos="3 -3 4" dir="-.6 .6 -.6"/>
<!-- The bag, scaled up so the humanoid fits and held open by pinning the ring of vertices
around its mouth. It hangs well above the floor, so the cloth catches the humanoid. -->
<body name="bag_container" euler="1.57 0 0" pos="0 0 1.5">
<flexcomp name="bag" type="mesh" file="bag.obj" dim="2" scale="3.0 3.09 3.0"
radius="0.003" mass="5" rgba="0.6 0.6 0.62 0.6">
<elasticity young="3e6" poisson="0.3" thickness="0.002" elastic2d="stretch" damping="5e-3"/>
<pin id="193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212
213 214 215 216 239 240 241 242 243 244 245 246 247 248 249 250
251 252 253 254 255 256 257 258 259 260 267 268 269 270 271 272
645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660
661 662 663 664 665 666 667 668 671 672 673 675 677 678 679 682
687 688 689 690 711 712 713 714 715 716 717 718 719 720 721 722
723 724 725 726 727 728 729 730 755 756 758 761 762 764 766 770
772 773 803 804 805 806 813 815 819 820 821 822 1003 1005 1006
1009 1010 1013 1014 1015 1017 1019 1021 1024 1025 1051 1052
1054 1056 1059 1061 1063 1065 1066 1069 1070"/>
<!-- priority=1: the flex parameters win outright instead of averaging with the
humanoid's softer solref/solimp, which let limbs sink ~35mm into the cloth. -->
<contact selfcollide="none" contype="1" conaffinity="1" priority="1"
solref="0.004 1" solimp="0.99 0.999 0.001"/>
</flexcomp>
</body>
<!-- The humanoid, dropped above the bag's mouth; its torso sits at z=1.282 in its
own model. It is the stock 40kg humanoid, hence the heavy, stiff cloth. -->
<frame pos="0 0 1.618">
<attach model="humanoid" body="torso" prefix="humanoid_"/>
</frame>
</worldbody>
</mujoco>
python/mujoco/introspect/structs.py
+5 -29
View File
@@ -5737,7 +5737,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
StructFieldDecl(
name='efm_active',
type=ValueType(name='int'),
doc='implicit effective metric M+K: 0 inactive, 1 active, 2 active + preconditioner exact', # pylint: disable=line-too-long
doc='implicit effective metric M+K is active (see mjd_effBuild)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='nefmK',
@@ -5747,12 +5747,12 @@ STRUCTS: Mapping[str, StructDecl] = dict([
StructFieldDecl(
name='nefmdof',
type=ValueType(name='int'),
doc='number of rows in effective-metric factor',
doc='number of 3x3 blocks in the effective-metric preconditioner', # pylint: disable=line-too-long
),
StructFieldDecl(
name='nefmL',
type=ValueType(name='int'),
doc='number of non-zeros in the effective-metric factor',
doc='size of the effective-metric block storage (9*nefmdof)',
),
StructFieldDecl(
name='nY',
@@ -7038,39 +7038,15 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='factor row -> dof address',
doc='block k -> dof address of its vertex triple',
array_extent=('nefmdof',),
),
StructFieldDecl(
name='efm_L_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='factor row nonzeros',
array_extent=('nefmdof',),
),
StructFieldDecl(
name='efm_L_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='factor row addresses',
array_extent=('nefmdof',),
),
StructFieldDecl(
name='efm_L_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='factor column indices',
array_extent=('nefmL',),
),
StructFieldDecl(
name='efm_L',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='Cholesky factor of diag(M)+K, covered dofs',
doc='factored 3x3 diagonal blocks of M+K',
array_extent=('nefmL',),
),
StructFieldDecl(
src/engine/engine_derivative.c
+165 -411
View File
@@ -33,7 +33,6 @@
#include "engine/engine_util_sparse.h"
//------------------------- derivatives of spatial algebra -----------------------------------------
@@ -1336,7 +1335,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d,
}
// compute res += (s1 + s2*damping) * J'*K*J * vec, for all interpolated flexes
// K_rot_cache: if non-NULL, use pre-cached K_rot (same layout as m->flex_stiffness)
void mjd_flexInterp_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,
@@ -1352,7 +1350,6 @@ void mjd_flexInterp_cacheKrot(const mjModel* m, mjData* d, mjtNum* K_rot_out) {
}
// compute res += scale * K_bend * vec for standard (non-interp) flex bending
// scale = s1 + s2 * flex_damping[f] per flex
// for stiffness+damping: s1=h^2, s2=h => scale = h^2 + h*damping
@@ -2043,7 +2040,6 @@ int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr
// add (d qfrc_actuator / d qvel) to qDeriv
void mjd_actuator_vel(const mjModel* m, mjData* d) {
int nactuator = m->nactuator;
@@ -2901,403 +2897,191 @@ void mjd_effMulAdd(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec)
}
// z = P \ r with P = M everywhere except the flex block: per-step factor where present, else
// block-Jacobi, plus the constant mj_setConst bending factor on its covered dofs
static void effPrecond(const mjModel* m, mjData* d, mjtNum* z, const mjtNum* r,
mjtNum* psr, mjtNum* psz, mjtNum* bfr, mjtNum* bfz) {
// Build and factor the per-vertex 3x3 diagonal blocks of the flex part of (M + K), stored in
// d->efm_L, 9 numbers per covered vertex: O(n) to build and apply, approximate where the sparse
// factorization it replaces was exact. Both consumers use the blocks as a preconditioner: the CG
// constraint solver (Mgrad = Mtilde \ grad) and the qacc_smooth PCG in mjd_effSolve, which
// supplies the accuracy.
static void effBlocks(const mjModel* m, mjData* d) {
int nv = m->nv;
mju_copy(z, r, nv);
mj_solveLD(z, d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, m->M_colind, NULL);
// precomputed bending factor (mj_setConst): exact (M + K_bend)^-1 on covered dofs.
// Skipped when the per-step factor exists: it covers these rows and is applied last,
// so this solve would be overwritten
// covered dofs come in contiguous triples (the 3 slide dofs of one flex point), but the first
// one need not be at a multiple of 3: any joint declared before the flex shifts them. Walk the
// covered rows rather than striding the dof index, which would straddle point boundaries.
int nb = 0;
for (int i = 0; i < nv; ) {
if (d->efm_K_rownnz[i]) { nb++; i += 3; } else { i++; }
}
d->nefmdof = 0;
mjtNum* B = (mjtNum*) effAlloc(d, sizeof(mjtNum)*9*(nb > 0 ? nb : 1), _Alignof(mjtNum));
int* adr = (int*) effAlloc(d, sizeof(int)*(nb > 0 ? nb : 1), _Alignof(int));
int k = 0;
for (int i = 0; i < nv; ) {
if (!d->efm_K_rownnz[i]) {
i++;
continue;
}
mjtNum* Bk = B + 9*k;
mju_zero(Bk, 9);
for (int r = 0; r < 3; r++) {
int row = i + r;
for (int a = m->M_rowadr[row]; a < m->M_rowadr[row] + m->M_rownnz[row]; a++) {
int c = m->M_colind[a];
if (c >= i && c < i+3) Bk[3*r + (c-i)] += d->M[a];
}
for (int a = d->efm_K_rowadr[row]; a < d->efm_K_rowadr[row] + d->efm_K_rownnz[row]; a++) {
int c = d->efm_K_colind[a];
if (c >= i && c < i+3) Bk[3*r + (c-i)] += d->efm_K_val[a];
}
}
mju_cholFactor(Bk, 3, mjMINVAL);
adr[k++] = i;
i += 3;
}
d->efm_L = B;
d->efm_dofid = adr;
d->nefmdof = nb;
d->nefmL = 9*nb;
}
// Apply the metric preconditioner: the per-step 3x3 blocks when they exist, else the constant
// bending factor from mj_setConst, on the dofs they cover; M^-1 on all other dofs. PCG requires
// symmetry, so covered and uncovered dofs must not see each other: zeroing the covered entries
// of the right-hand side before the qLD sweep keeps the uncovered rows from reading them.
static void effBlockApply(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* b) {
int nv = m->nv;
int nbd = m->nefm0dof;
if (nbd && !d->nefmdof) {
for (int i=0; i < nbd; i++) {
bfr[i] = r[m->efm0_dofid[i]];
int flg_bend = nbd && !d->nefmdof;
mj_markStack(d);
mjtNum* rhs = mjSTACKALLOC(d, nv, mjtNum);
mju_copy(rhs, b, nv); // b may alias x, which the sweep below overwrites
// dofs no factor covers
mju_copy(x, rhs, nv);
for (int k = 0; k < d->nefmdof; k++) {
mju_zero(x + d->efm_dofid[k], 3);
}
if (flg_bend) {
for (int i = 0; i < nbd; i++) {
x[m->efm0_dofid[i]] = 0;
}
}
mj_solveLD(x, d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, m->M_colind, NULL);
// per-step stiffness: 3x3 blocks
for (int k = 0; k < d->nefmdof; k++) {
int i = d->efm_dofid[k];
mju_cholSolve(x + i, d->efm_L + 9*k, rhs + i, 3);
}
// bending-only: exact (M + K_bend)^-1 on the dofs the constant factor covers
if (flg_bend) {
mjtNum* bfr = mjSTACKALLOC(d, nbd, mjtNum);
mjtNum* bfz = mjSTACKALLOC(d, nbd, mjtNum);
for (int i = 0; i < nbd; i++) {
bfr[i] = rhs[m->efm0_dofid[i]];
}
mju_cholSolveSparse(bfz, m->efm0_L, bfr, nbd,
m->efm0_L_rownnz, m->efm0_L_rowadr, m->efm0_L_colind);
for (int i=0; i < nbd; i++) {
z[m->efm0_dofid[i]] = bfz[i];
}
}
// per-step factor: exact (diag(M) + K)^-1 on its covered dofs, applied last
if (d->nefmdof) {
int n = d->nefmdof;
for (int i=0; i < n; i++) {
psr[i] = r[d->efm_dofid[i]];
}
mju_cholSolveSparse(psz, d->efm_L, psr, n,
d->efm_L_rownnz, d->efm_L_rowadr, d->efm_L_colind);
for (int i=0; i < n; i++) {
z[d->efm_dofid[i]] = psz[i];
for (int i = 0; i < nbd; i++) {
x[m->efm0_dofid[i]] = bfz[i];
}
}
mj_freeStack(d);
}
// solve x = Mtilde \ b, where Mtilde is this step's effective metric:
// efm_active == 0: Mtilde = M one sparse LD solve, no elasticity anywhere
// efm_active == 2: Mtilde = M + K exact direct solve, blockdiag(qLD, flex factor);
// exactness conditions in mjd_effBuild
// efm_active == 1: Mtilde = M + K iterative: x0 = M \ b ignores the elasticity, then
// matrix-free PCG on the residual, preconditioned by
// effPrecond (tolerance/cap match the old post-hoc)
// iteration cap for the metric solve
#define mjEFF_MAXITER 100
// accurate solve of (M + K) x = b by PCG with the 3x3 block preconditioner, converging the
// relative residual to opt.tolerance; used for qacc_smooth. Reaching mjEFF_MAXITER means the
// metric is too ill-conditioned for the blocks: warn (mjWARN_INERTIA, worst-residual dof) and
// return x under-converged.
void mjd_effSolve(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* b) {
if (!d->efm_active) {
mjd_effPrec(m, d, x, b);
return;
}
int nv = m->nv;
mj_markStack(d);
mjtNum* r = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* z = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* p = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* Ap = mjSTACKALLOC(d, nv, mjtNum);
mju_copy(r, b, nv); // before zeroing x: b may alias x
mju_zero(x, nv);
mjtNum bn = mju_dot(r, r, nv);
if (bn > mjMINVAL) {
// converge the relative residual to opt.tolerance; both sides are squared norms
#ifdef mjUSESINGLE
// float cannot reach a 1e-8 relative residual (eps ~1.2e-7): without a floor every step of
// every covered model would run to mjEFF_MAXITER and then warn.
mjtNum tolerance = mju_max(m->opt.tolerance, 1e-6);
#else
mjtNum tolerance = m->opt.tolerance;
#endif
mjtNum tol = tolerance*tolerance*bn;
int capped = 1; // cleared by either exit below; still set means the cap was reached
effBlockApply(m, d, z, r);
mju_copy(p, z, nv);
mjtNum rz = mju_dot(r, z, nv);
for (int it = 0; it < mjEFF_MAXITER; it++) {
mju_mulSymVecSparse(Ap, d->M, p, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
mjd_effMulAdd(m, d, Ap, p);
mjtNum pAp = mju_dot(p, Ap, nv);
// curvature breakdown: the metric has no curvature along p, so no further progress is
// possible and x is the best available. Not a budget failure, so it does not warn.
if (pAp <= 0) { capped = 0; break; }
mjtNum alpha = rz/pAp;
mju_addToScl(x, p, alpha, nv);
mju_addToScl(r, Ap, -alpha, nv);
if (mju_dot(r, r, nv) < tol) { capped = 0; break; }
effBlockApply(m, d, z, r);
mjtNum rznew = mju_dot(r, z, nv);
mju_addScl(p, z, p, rznew/rz, nv);
rz = rznew;
}
// Ran out of iterations with the residual still above tolerance: the metric is too
// ill-conditioned for the blocks to solve within the budget. Blame the dof carrying the
// largest residual.
// mjWARN_INERTIA is the closest existing warning (reusing it avoids an ABI addition), but on
// its own it points the user at their inertia when the cause is the flex stiffness, so say so
// first. Gate on the same first-time condition mj_warning uses, or a model that fails every
// step would print this thousands of times a second.
if (capped && mju_dot(r, r, nv) >= tol) {
int worst = 0;
for (int i = 1; i < nv; i++) {
if (mju_abs(r[i]) > mju_abs(r[worst])) worst = i;
}
if (!d->warning[mjWARN_INERTIA].number) {
mju_warning("Flex stiffness is too ill-conditioned for the effective-metric block "
"preconditioner: the M+K solve ran out of iterations at a relative residual "
"of %.2e and qacc_smooth is under-converged. Reported as a singular inertia "
"below, because M+K is the effective inertia.",
mju_sqrt(mju_dot(r, r, nv)/bn));
}
mj_warning(d, mjWARN_INERTIA, worst);
}
}
mj_freeStack(d);
}
void mjd_effPrec(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* b) {
int nv = m->nv;
// inactive metric: x = M \ b
if (!d->efm_active) {
if (x != b) {
mju_copy(x, b, nv);
}
mj_solveLD(x, d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, m->M_colind, NULL);
// active metric: the prefactored 3x3 blocks are the preconditioner
if (d->efm_active) {
effBlockApply(m, d, x, b);
return;
}
// exact preconditioner: blockdiag(qLD, flex factor) is (M+K)^-1, solve directly
if (d->efm_active == 2) {
mj_markStack(d);
mjtNum* psr = mjSTACKALLOC(d, d->nefmdof > 0 ? d->nefmdof : 1, mjtNum);
mjtNum* psz = mjSTACKALLOC(d, d->nefmdof > 0 ? d->nefmdof : 1, mjtNum);
int nbd0 = m->nefm0dof;
mjtNum* bfr = mjSTACKALLOC(d, nbd0 > 0 ? nbd0 : 1, mjtNum);
mjtNum* bfz = mjSTACKALLOC(d, nbd0 > 0 ? nbd0 : 1, mjtNum);
effPrecond(m, d, x, b, psr, psz, bfr, bfz);
mj_freeStack(d);
return;
}
// general path: warm start from M \ b, refine below
// inactive metric: x = M \ b, which is exact
if (x != b) {
mju_copy(x, b, nv);
}
mj_solveLD(x, d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, m->M_colind, NULL);
mj_markStack(d);
mjtNum* r = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* z = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* p = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* Ap = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* psr = mjSTACKALLOC(d, d->nefmdof > 0 ? d->nefmdof : 1, mjtNum);
mjtNum* psz = mjSTACKALLOC(d, d->nefmdof > 0 ? d->nefmdof : 1, mjtNum);
int nbd = m->nefm0dof;
mjtNum* bfr = mjSTACKALLOC(d, nbd > 0 ? nbd : 1, mjtNum);
mjtNum* bfz = mjSTACKALLOC(d, nbd > 0 ? nbd : 1, mjtNum);
// r = b - (M+K)*x
mju_mulSymVecSparse(Ap, d->M, x, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
mjd_effMulAdd(m, d, Ap, x);
mju_sub(r, b, Ap, nv);
// relative tolerance on the residual
mjtNum tol = 1e-10 * mju_dot(b, b, nv);
if (mju_dot(r, r, nv) < tol) {
mj_freeStack(d);
return;
}
effPrecond(m, d, z, r, psr, psz, bfr, bfz);
mju_copy(p, z, nv);
mjtNum rz = mju_dot(r, z, nv);
for (int k=0; k < 50; k++) {
mju_mulSymVecSparse(Ap, d->M, p, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
mjd_effMulAdd(m, d, Ap, p);
mjtNum pAp = mju_dot(p, Ap, nv);
if (pAp < mjMINVAL) {
break;
}
mjtNum alpha = rz/pAp;
mju_addToScl(x, p, alpha, nv);
mju_addToScl(r, Ap, -alpha, nv);
if (mju_dot(r, r, nv) < tol) {
break;
}
effPrecond(m, d, z, r, psr, psz, bfr, bfz);
mjtNum rznew = mju_dot(r, z, nv);
mju_addScl(p, z, p, rznew/rz, nv);
rz = rznew;
}
mj_freeStack(d);
}
// geometric nested-dissection ordering for the per-step factor: recursive coordinate bisection
// with adjacency-detected separators, emitted ancestors-first (the reverse-Cholesky convention)
typedef struct {
const mjtNum* pos; // block positions (3 x nblk)
const int* B_rownnz; // dof-level B pattern, for block adjacency
const int* B_rowadr;
const int* B_colind;
const int* dofid; // block -> first dof address (3 dofs per block)
const int* dof2c; // dof -> compact index (pre-permutation)
int* work; // block id work array (nblk x 1)
int* stamp; // current-range stamp per block (nblk x 1)
int* side; // bisection side per block (valid when stamped)(nblk x 1)
int stampctr; // running range id
int* scratch; // side-1 gather scratch (nblk x 1)
int* perm; // output: block emission order (nblk x 1)
int nperm; // emitted count
} mjEffND;
static void effNDOrder(mjEffND* nd, int lo, int hi) {
int nblk = hi - lo;
if (nblk <= 16) {
for (int i=lo; i < hi; i++) {
nd->perm[nd->nperm++] = nd->work[i];
}
return;
}
// widest axis of the range's bounding box, split at the mean coordinate
mjtNum bmin[3] = {mjMAXVAL, mjMAXVAL, mjMAXVAL}, bmax[3] = {-mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
mjtNum mean[3] = {0, 0, 0};
for (int i=lo; i < hi; i++) {
const mjtNum* p = nd->pos + 3*nd->work[i];
for (int x=0; x < 3; x++) {
bmin[x] = p[x] < bmin[x] ? p[x] : bmin[x];
bmax[x] = p[x] > bmax[x] ? p[x] : bmax[x];
mean[x] += p[x];
}
}
int axis = 0;
for (int x=1; x < 3; x++) {
if (bmax[x] - bmin[x] > bmax[axis] - bmin[axis]) {
axis = x;
}
}
mjtNum split = mean[axis] / nblk;
// stamp the range, assign sides
int id = ++nd->stampctr, n0 = 0;
for (int i=lo; i < hi; i++) {
int b = nd->work[i];
nd->stamp[b] = id;
nd->side[b] = nd->pos[3*b + axis] > split;
n0 += !nd->side[b];
}
// degenerate split (coincident positions): fall back to an arbitrary halving
if (n0 == 0 || n0 == nblk) {
for (int i=lo; i < hi; i++) {
nd->side[nd->work[i]] = (i - lo) >= nblk/2;
}
}
// emit the separator (side-0 blocks adjacent to side 1) first; compact A in place and
// side-1 blocks via the scratch list (in-place would clobber unread entries)
int na = 0, nb = 0;
for (int i=lo; i < hi; i++) {
int b = nd->work[i];
if (nd->side[b]) {
nd->scratch[nb++] = b;
continue;
}
// side 0: separator iff adjacent to side 1 (block adjacency via the first dof's B row)
int dof = nd->dofid[3*b];
int adr = nd->B_rowadr[dof], nnz = nd->B_rownnz[dof], sep = 0;
for (int k=0; k < nnz; k++) {
int cc = nd->dof2c[nd->B_colind[adr + k]];
if (cc >= 0) {
int nbr = cc/3;
if (nd->stamp[nbr] == id && nd->side[nbr]) {
sep = 1;
break;
}
}
}
if (sep) {
nd->perm[nd->nperm++] = b;
} else {
nd->work[lo + na++] = b;
}
}
for (int i=0; i < nb; i++) {
nd->work[lo + na + i] = nd->scratch[i];
}
effNDOrder(nd, lo, lo + na);
effNDOrder(nd, lo + na, lo + na + nb);
}
// per-step sparse factor of the flex block of (M + K): reverse-Cholesky over the covered dofs,
// nested-dissection ordered. M enters as its diagonal there -- exact for free vertices;
// parent-coupled vertices make this a preconditioner, refined to tolerance by mjd_effSolve.
// Exact zeros are dropped from the off-diagonal pattern (bending couples same-coordinate dofs
// only). The matrix is SPD by construction, so rank deficiency can only mean a degenerate
// model (near-zero mass and stiffness on a covered dof) and is a hard error.
static void effFactor(const mjModel* m, mjData* d) {
int nv = m->nv;
const int* B_rownnz = d->efm_K_rownnz;
const int* B_rowadr = d->efm_K_rowadr;
const int* B_colind = d->efm_K_colind;
const mjtNum* B_val = d->efm_K_val;
mj_markStack(d);
// compact dof map over covered rows (ascending, so compact indices stay sorted)
int* dof2c = mjSTACKALLOC(d, nv, int);
int n = 0;
for (int i=0; i < nv; i++) {
dof2c[i] = B_rownnz[i] ? n++ : -1;
}
int* dofid = mjSTACKALLOC(d, n, int);
for (int i=0; i < nv; i++) {
if (dof2c[i] >= 0) {
dofid[dof2c[i]] = i;
}
}
// nested-dissection reordering of the covered blocks (one block = 3 dofs of one point)
int nblk = n/3;
int* nd_perm = mjSTACKALLOC(d, nblk, int);
{
int* nd_work = mjSTACKALLOC(d, nblk, int);
int* nd_stamp = mjSTACKALLOC(d, nblk, int);
int* nd_side = mjSTACKALLOC(d, nblk, int);
int* nd_scr = mjSTACKALLOC(d, nblk, int);
mjtNum* bpos = mjSTACKALLOC(d, 3*nblk, mjtNum);
for (int b=0; b < nblk; b++) {
nd_work[b] = b;
nd_stamp[b] = 0;
mju_copy3(bpos + 3*b, d->xpos + 3*m->dof_bodyid[dofid[3*b]]);
}
mjEffND nd;
nd.pos = bpos;
nd.B_rownnz = B_rownnz;
nd.B_rowadr = B_rowadr;
nd.B_colind = B_colind;
nd.dofid = dofid;
nd.dof2c = dof2c;
nd.work = nd_work;
nd.stamp = nd_stamp;
nd.side = nd_side;
nd.stampctr = 0;
nd.scratch = nd_scr;
nd.perm = nd_perm;
nd.nperm = 0;
effNDOrder(&nd, 0, nblk);
}
// apply the permutation to the compact indexing; the permuted dofid persists on the arena
int* psdofid = EFMALLOC(int, n);
for (int r=0; r < nblk; r++) {
psdofid[3*r] = dofid[3*nd_perm[r]];
psdofid[3*r+1] = dofid[3*nd_perm[r] + 1];
psdofid[3*r+2] = dofid[3*nd_perm[r] + 2];
}
for (int i=0; i < n; i++) {
dof2c[psdofid[i]] = i;
}
// H = diag(M) + K in compact indices: lower CSR (values, diagonal last) + upper CSR (pattern)
int nHl = 0, nHu = 0;
for (int c=0; c < n; c++) {
int adr = B_rowadr[psdofid[c]], nnzB = B_rownnz[psdofid[c]];
for (int k=0; k < nnzB; k++) {
int cc = dof2c[B_colind[adr + k]];
if (B_val[adr + k] == 0 && cc != c) {
continue;
}
if (cc <= c) {
nHl++;
} else {
nHu++;
}
}
}
int* Hl_rownnz = mjSTACKALLOC(d, n, int);
int* Hl_rowadr = mjSTACKALLOC(d, n, int);
int* Hl_colind = mjSTACKALLOC(d, nHl, int);
mjtNum* Hl_val = mjSTACKALLOC(d, nHl, mjtNum);
int* Hu_rownnz = mjSTACKALLOC(d, n, int);
int* Hu_rowadr = mjSTACKALLOC(d, n, int);
int* Hu_colind = mjSTACKALLOC(d, nHu > 0 ? nHu : 1, int);
int maxrow = 0;
for (int c=0; c < n; c++) {
maxrow = B_rownnz[psdofid[c]] > maxrow ? B_rownnz[psdofid[c]] : maxrow;
}
int* rind = mjSTACKALLOC(d, maxrow, int);
mjtNum* rval = mjSTACKALLOC(d, maxrow, mjtNum);
int ladr = 0, uadr = 0;
for (int c=0; c < n; c++) {
int i = psdofid[c];
Hl_rowadr[c] = ladr;
Hu_rowadr[c] = uadr;
// gather the row in permuted compact indices, then sort (columns are no longer monotone)
int adr = B_rowadr[i], nnzB = B_rownnz[i], nr = 0;
for (int k=0; k < nnzB; k++) {
int cc = dof2c[B_colind[adr + k]];
if (B_val[adr + k] == 0 && cc != c) {
continue;
}
rind[nr] = cc;
rval[nr++] = B_val[adr + k];
}
for (int k=1; k < nr; k++) {
int ci = rind[k];
mjtNum vi = rval[k];
int j = k - 1;
while (j >= 0 && rind[j] > ci) {
rind[j+1] = rind[j];
rval[j+1] = rval[j];
j--;
}
rind[j+1] = ci;
rval[j+1] = vi;
}
for (int k=0; k < nr; k++) {
if (rind[k] < c) {
Hl_colind[ladr] = rind[k];
Hl_val[ladr++] = rval[k];
} else if (rind[k] == c) {
Hl_colind[ladr] = c;
Hl_val[ladr++] = rval[k] + d->M[m->M_rowadr[i] + m->M_rownnz[i] - 1];
} else {
Hu_colind[uadr++] = rind[k];
}
}
Hl_rownnz[c] = ladr - Hl_rowadr[c];
Hu_rownnz[c] = uadr - Hu_rowadr[c];
}
// symbolic factorization: counting phase, then filling phase
int* L_rownnz = EFMALLOC(int, n);
int* L_rowadr = EFMALLOC(int, n);
int* LT_rownnz = mjSTACKALLOC(d, n, int);
int* LT_rowadr = mjSTACKALLOC(d, n, int);
int nnz = mju_cholFactorSymbolic(NULL, L_rownnz, L_rowadr, NULL, LT_rownnz, LT_rowadr, NULL,
Hu_rownnz, Hu_rowadr, Hu_colind, n, d);
int* L_colind = EFMALLOC(int, nnz);
int* LT_colind = mjSTACKALLOC(d, nnz, int);
int* LT_map = mjSTACKALLOC(d, nnz, int);
mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz, LT_rowadr, LT_map,
Hu_rownnz, Hu_rowadr, Hu_colind, n, d);
// numeric factorization
mjtNum* L = EFMALLOC(mjtNum, nnz);
int rank = mju_cholFactorNumeric(L, n, mjMINVAL, L_rownnz, L_rowadr, L_colind,
LT_rownnz, LT_rowadr, LT_colind, LT_map,
Hl_val, Hl_rownnz, Hl_rowadr, Hl_colind, d);
mj_freeStack(d);
if (rank != n) {
mjERROR("effective metric factorization is rank-deficient (%d of %d): "
"degenerate mass or stiffness in the flex block", rank, n);
}
d->nefmdof = n;
d->nefmL = nnz;
d->efm_dofid = psdofid;
d->efm_L_rownnz = L_rownnz;
d->efm_L_rowadr = L_rowadr;
d->efm_L_colind = L_colind;
d->efm_L = L;
}
@@ -3358,7 +3142,7 @@ void mjd_effBuild(const mjModel* m, mjData* d, int active, int flg_factor) {
// solve (the stiff flex block stops being iterated on). Consumers that only multiply
// (inverse dynamics) skip it.
if (flg_factor) {
effFactor(m, d);
effBlocks(m, d);
}
} else {
@@ -3367,36 +3151,6 @@ void mjd_effBuild(const mjModel* m, mjData* d, int active, int flg_factor) {
}
d->efm_active = 1;
// preconditioner exactness (efm_active == 2): when every dof the stiffness touches sits on
// a simple slider body (diagonal M row, no kinematic children), M has no coupling across
// the covered block, so blockdiag(qLD, flex factor) is exactly (M+K)^-1 and mjd_effSolve
// skips the refinement. Interp flexes outside the assembled CSR act only through the
// matrix-free operator (no factor rows), which breaks exactness.
int exact = d->nefmK ? (d->nefmdof > 0) : 1;
for (int f=0; exact && f < m->nflex; f++) {
if (!krot && flexInterp_processed(m, f)) {
exact = 0;
}
}
if (exact && d->nefmK) {
for (int i=0; i < nv; i++) {
if (d->efm_K_rownnz[i] && m->body_simple[m->dof_bodyid[i]] != 2) {
exact = 0;
break;
}
}
} else if (exact) {
for (int i=0; i < m->nefm0dof; i++) {
if (m->body_simple[m->dof_bodyid[m->efm0_dofid[i]]] != 2) {
exact = 0;
break;
}
}
}
if (exact) {
d->efm_active = 2;
}
// fill the shift with the current velocity (refreshed again in the velocity stage)
mjd_effShift(m, d);
}
src/engine/engine_derivative.h
+7 -1
View File
@@ -97,9 +97,15 @@ MJAPI void mjd_effShift(const mjModel* m, mjData* d);
// res += B*vec (the stiffness part of the metric; caller supplies the M part)
MJAPI void mjd_effMulAdd(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
// x = (M + B)^-1 b to 1e-10 relative; x = M^-1 b when the metric is inactive
// solve (M + B) x = b by PCG preconditioned with mjd_effPrec, to opt.tolerance on the relative
// residual; x = M^-1 b when the metric is inactive. Warns (mjWARN_INERTIA) if the iteration cap
// is reached before convergence, in which case x is returned under-converged.
MJAPI void mjd_effSolve(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* b);
// apply the metric preconditioner: x ~= (M + B)^-1 b, a cheap fixed linear operator, NOT a solve.
// Exact only when the metric is inactive (x = M^-1 b); otherwise approximate by construction.
MJAPI void mjd_effPrec(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* b);
#ifdef __cplusplus
}
src/engine/engine_forward.c
+1 -1
View File
@@ -1082,7 +1082,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
// check if islands are supported
// TODO: support islands with the implicit effective metric and remove the mj_flexCG
// condition. It is here because the metric machinery is monolithic: the efm_c shift and
// the Ma/Mv/Mgrad operators (mjd_effMulAdd, mjd_effSolve) act on global dof vectors with
// the Ma/Mv/Mgrad operators (mjd_effMulAdd, mjd_effPrec) act on global dof vectors with
// no island-local form. Discovery is already handled: findEdges unions the trees of every
// stiffness-active flex, so a flex always lands in one island together with everything it
// touches. Removal therefore needs only the solver side: apply the efm_c shift to that
src/engine/engine_solver.c
+1 -1
View File
@@ -1428,7 +1428,7 @@ static void PrimalUpdateMgrad(mjPrimalContext* ctx, int flg_Newton) {
// CG: Mgrad = Mtilde \ grad
else if (ctx->flg_flex) {
mjd_effSolve(ctx->fm, ctx->fd, ctx->Mgrad, ctx->grad);
mjd_effPrec(ctx->fm, ctx->fd, ctx->Mgrad, ctx->grad);
}
// CG: Mgrad = M \ grad
test/engine/engine_derivative_test.cc
+70 -8
View File
@@ -2104,9 +2104,11 @@ TEST_F(DerivativeTest, FlexStiffAssembleInterp) {
}
}
// mjd_effSolve: exact-preconditioner fast path solves (M+K)x = b directly; the general
// refinement path stays within its tolerance when exactness does not hold
TEST_F(DerivativeTest, EffSolveExact) {
// mjd_effSolve drives (M+K)x = b to opt.tolerance on the relative residual,
// for every metric coverage case. It is a PCG whose preconditioner
// (mjd_effPrec) is only approximate, so the accuracy comes from the iteration
// and not from the preconditioner being exact.
TEST_F(DerivativeTest, EffSolve) {
// relative residual of (M+K)x - b after mjd_effSolve
auto solve_residual = [](const mjModel* m, mjData* d) {
int nv = m->nv;
@@ -2142,8 +2144,7 @@ TEST_F(DerivativeTest, EffSolveExact) {
ASSERT_GE(data->efm_active, 1);
EXPECT_GT(data->nefmK, 0);
EXPECT_GT(data->nefmdof, 0);
EXPECT_EQ(data->efm_active, 2);
EXPECT_LT(solve_residual(model.get(), data.get()), MjTol(1e-10, 1e-6));
EXPECT_LT(solve_residual(model.get(), data.get()), MjTol(1e-8, 1e-4));
// bending-only cloth: no CSR or per-step factor, constant factor covers, exact
static const char* const kXmlBend = R"(
@@ -2166,8 +2167,7 @@ TEST_F(DerivativeTest, EffSolveExact) {
EXPECT_EQ(data->nefmK, 0);
EXPECT_EQ(data->nefmdof, 0);
EXPECT_GT(model->nefm0dof, 0);
EXPECT_EQ(data->efm_active, 2);
EXPECT_LT(solve_residual(model.get(), data.get()), MjTol(1e-10, 1e-6));
EXPECT_LT(solve_residual(model.get(), data.get()), MjTol(1e-8, 1e-4));
// cloth under a jointed parent: M couples across the covered block, not exact,
// the refinement path must still meet its tolerance
@@ -2192,9 +2192,71 @@ TEST_F(DerivativeTest, EffSolveExact) {
data = MakeData(model);
mj_forward(model.get(), data.get());
ASSERT_GE(data->efm_active, 1);
EXPECT_EQ(data->efm_active, 1);
EXPECT_LT(solve_residual(model.get(), data.get()), 1e-4);
}
// A cloth with per-step stretch stiffness, used by the two tests below.
static const char* const kStretchCloth = R"(
<mujoco>
<option solver="CG" integrator="implicitfast"/>
<worldbody>
<body name="base" pos="0 0 1">
<joint type="slide" axis="0 0 1"/>
<geom type="sphere" size=".01" mass="1" contype="0" conaffinity="0"/>
<flexcomp name="cloth" type="grid" count="6 6 1" spacing="0.05 0.05 0.05"
radius=".005" dim="2" mass="0.5" pos="0 0 0" dof="full">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e3" poisson="0.2" damping="0.1" elastic2d="both"
thickness="0.01"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
// An unreachable tolerance drives mjd_effSolve to its iteration cap; it must
// report that rather than return an under-converged qacc_smooth silently.
TEST_F(DerivativeTest, EffSolveCapWarns) {
char error[1024];
MjModelPtr model = LoadModelFromString(kStretchCloth, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
model->opt.tolerance = 0; // unreachable: the PCG can never meet it
MjDataPtr data = MakeData(model);
MockWarningHandler warning_handler;
// both: the specific cause, then the mjWARN_INERTIA it is reported through
warning_handler.ExpectWarnings("Flex stiffness is too ill-conditioned");
warning_handler.ExpectWarnings("Inertia matrix is too close to singular");
mj_forward(model.get(), data.get());
testing::Mock::VerifyAndClearExpectations(&warning_handler);
}
// PCG requires a symmetric preconditioner. mjd_effPrec must satisfy
// u.P(v) == v.P(u); it did not when the covered and uncovered dofs shared a
// kinematic tree, which is what the flex-under-a-slider model here exercises.
TEST_F(DerivativeTest, EffPrecIsSymmetric) {
char error[1024];
MjModelPtr model = LoadModelFromString(kStretchCloth, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
ASSERT_GE(data->efm_active, 1);
int nv = model->nv;
std::vector<mjtNum> u(nv), v(nv), Pu(nv), Pv(nv);
for (int trial = 0; trial < 5; trial++) {
for (int i = 0; i < nv; i++) {
u[i] = mju_Halton(i + trial*nv, 2) - 0.5;
v[i] = mju_Halton(i + trial*nv, 5) - 0.5;
}
mjd_effPrec(model.get(), data.get(), Pu.data(), u.data());
mjd_effPrec(model.get(), data.get(), Pv.data(), v.data());
mjtNum a = mju_dot(v.data(), Pu.data(), nv);
mjtNum b = mju_dot(u.data(), Pv.data(), nv);
EXPECT_THAT(a, MjNear(b, 1e-10, 1e-4))
<< "preconditioner is not symmetric, trial " << trial;
}
}
} // namespace
} // namespace mujoco
test/xml/xml_write_read_test.cc
+2
View File
@@ -65,6 +65,8 @@ std::vector<std::string> GetWriteReadTestModels() {
absl::StrContains(xml, "fromto_convex") ||
absl::StrContains(xml, "cube_skin") ||
absl::StrContains(xml, "cube_3x3x3") ||
// flex_stiffness: stretch amplifies geometry XML rounds on save
absl::StrContains(xml, "flex/bag") ||
// exclude files that fail since we do not save pinned flex nodes
absl::StrContains(xml, "gripper_trilinear") ||
absl::StrContains(xml, "strain") ||
wasm/codegen/generated/bindings.cc
-3
View File
@@ -4609,9 +4609,6 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("efm_K_rownnz", &MjData::efm_K_rownnz)
.property("efm_K_val", &MjData::efm_K_val)
.property("efm_L", &MjData::efm_L)
.property("efm_L_colind", &MjData::efm_L_colind)
.property("efm_L_rowadr", &MjData::efm_L_rowadr)
.property("efm_L_rownnz", &MjData::efm_L_rownnz)
.property("efm_active", &MjData::efm_active, &MjData::set_efm_active, reference())
.property("efm_c", &MjData::efm_c)
.property("efm_dofid", &MjData::efm_dofid)
wasm/codegen/generated/bindings.h
-9
View File
@@ -7314,15 +7314,6 @@ struct MjData {
emscripten::val efm_dofid() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefmdof, ptr_->efm_dofid));
}
emscripten::val efm_L_rownnz() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefmdof, ptr_->efm_L_rownnz));
}
emscripten::val efm_L_rowadr() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefmdof, ptr_->efm_L_rowadr));
}
emscripten::val efm_L_colind() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefmL, ptr_->efm_L_colind));
}
emscripten::val efm_L() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefmL, ptr_->efm_L));
}
wasm/codegen/generators/constants.py
-3
View File
@@ -343,9 +343,6 @@ MJDATA_SIZES: tuple[str, ...] = (
"efm_K_val",
"efm_dofid",
"efm_L",
"efm_L_colind",
"efm_L_rowadr",
"efm_L_rownnz",
"iLDiagInv",
"iM_rowadr",
"iM_rownnz",