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

Implicit flex elasticity in the CG constraint solver via an effective metric

Browse Source 
This CL replaces the post-hoc implicit flex correction (`flexInterp_cgsolve`) with a **linearly-implicit effective metric** `M̃ = M + (h² + h·damping)·K` carried by the CG constraint solver itself. Contact/friction forces and implicit flex elasticity are now computed against one consistent metric, instead of the solver seeing `M` and a post-solve correction changing `qacc` behind its back.

Gate (unchanged semantics): `solver="CG"` + implicit/implicitfast integrator + pyramidal cones + flex stiffness present. Newton and PGS are untouched. `solver="CG"` remains the user-facing contract — the factorization is an implementation detail of the preconditioner.

### What's in the metric

- **mjData `efm_*`** (arena, efc-like lifetime/skip semantics; built in `mj_fwdPosition`, value-refreshed in `mj_fwdVelocity`): the per-step stiffness CSR `efm_B_*`, its reverse-Cholesky factor `efm_dofid` + `efm_L_*` (nested-dissection ordered, separators-first for the reverse factorization), and the smooth-force shift `efm_c = h·K·qvel`.
- **`mjd_flexStiff_assemble`** now assembles stretch (Gauss–Newton), standard dim-2 bending, and — via the cached corotated stiffness `d->flexelem_krot` — interp stiffness (all node bodies on simple sliders: point Jacobian is I₃, `flex_centered` not required; fixed nodes drop like pins) into one dof-level CSR. `mjd_effMulAdd`/`mjd_effSolve` apply the metric, with matrix-free operator fallbacks where assembly does not apply.
- **mjModel `efm0_*`** (`nefm0dof`/`nefm0L`): the constant part of the metric factor — currently the dim-2 bending factor, computed once in `mj_setConst` — so bending-only models pay zero per-step factorization cost. Naming mirrors mjData's `efm_*` with the standard `0`-suffix (reference/constant) idiom, and is deliberately not bending-specific: future constant contributors extend it without renames.
- The solver consumes the metric through pre-shifted `qfrc_smooth` and the metric products `Ma`/`Mv`/`Mgrad`; `qacc_smooth` becomes the unconstrained minimizer of the implicit dynamics, which makes the no-constraint shortcut and the warmstart choice consistent by construction.
- **`mj_inverse` adds `B·qacc − c`**, making inverse dynamics discrete-consistent with the gated forward dynamics — exact, since the gated path has no qDeriv term (new test `ForwardTest.GatedFlexInverseConsistency`).

### Performance

All numbers: ms/step over the same 2000-step window, models as shipped on each side (old code with the old model settings vs this CL with the new ones).

The new solver path activates on exactly two shipped models — the ponchos, the only flex models that need an implicit integrator (poncho on Euler degenerates to >200 ms/step). For them, this CL trades speed for consistency: the implicit bending solve now runs inside every solver iteration, where the contact solve can see the stiffness, instead of once after the solve. Solver iterations drop because the curvature is visible, but each iteration pays for the implicit solve:

| model | before | after | solver iters/step |
|---|---|---|---|
| poncho | 2.47 | 3.30 (1.33×) | 16.8 → 11.8 |
| poncho_edgeequality | 1.96 | 2.72 (1.39×) | 13.2 → 10.0 |

What that price buys: contact forces consistent with the implicit elasticity (previously the post-hoc correction changed `qacc` after the constraint solve), discrete-consistent inverse dynamics, and the removal of the post-hoc special case from the integration path. Raising poncho's timestep from 2 to 5 ms leaves its per-step cost nearly flat, so the consistency price can be recovered by taking fewer steps where accuracy allows.

Every other flex model was measured stable on Euler at its shipped timestep and switches to it (these models predate the post-hoc integrator; implicit was never load-bearing for them). They end up equal or faster than before: bunny_multicell 0.47 → 0.40, trampoline 0.28 → 0.25, plate 1.02 → 0.99, pancake 0.34 → 0.33.

Finally, the per-step factorization makes configurations practical that the old code could only integrate explicitly: implicit stretch elasticity (`elastic2d="stretch"`/`"both"`, dim-3 solids) and factorized interp stiffness. No before/after exists for these — stock has no implicit treatment of stretch at all.

### Behavior changes

- With the post-hoc correction deleted, interp/bending models running `solver="Newton"` (or elliptic cones, or islands) now integrate flex elasticity **explicitly** (previously: post-hoc implicit). Affects e.g. `gripper_trilinear` (stable, and faster, but different semantics). Follow-up options: Newton-side metric support, or a documented fallback.
- With the gate on, `mj_forward` outputs are timestep-dependent for gated models (they answer the linearly-implicit discrete problem); `qacc_smooth` and `mj_inverse` change accordingly. Non-gated models are bit-identical (full suite green throughout).

### Validation

- 1737/1737 tests, including new: `FlexStretchDerivatives` (FD-validated GN operator), `FlexStiffAssemble`/`FlexStiffAssembleInterp` (CSR ≡ operators), `GatedFlexInverseConsistency` (fails pre-change), equivalence tests vs the old post-hoc treatment (bending matches to 2e-11).
- Fingerprint discipline throughout: bending-only models bit-exact across every refactor; permutation/kernel changes verified iteration-identical.

### Known follow-ups (not in this CL)

3×3-block sparse Cholesky kernel (the numeric factorization is index-bound; projected ~3× on the factor); mjModel persistence of the factor's symbolic pattern (rest-pose ND makes sizes compile-time); the general effective-metric mode (all solvers, all PSD-safe force classes, behind an enable flag).

PiperOrigin-RevId: 948561856
Change-Id: I8b8e32ebd0428042af71647d0470d10773bf6daf
This commit is contained in:
Alessio Quaglino
2026-07-15 14:56:57 -07:00
committed by Copybara-Service
parent f0fa3d8260
commit ea230a950c
39 changed files with 2501 additions and 203 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/includes/references.h
+24
View File
@@ -111,6 +111,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 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 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
@@ -200,6 +204,7 @@ typedef struct mjData_ {
// computed by mj_fwdPosition/mj_flex
mjtNum* flexvert_xpos; // Cartesian flex vertex positions (nflexvert x 3)
mjtNum* flexelem_aabb; // flex element bounding boxes (center, size) (nflexelem x 6)
mjtNum* flexelem_krot; // corotated element stiffness (implicit only) (nflexstiffness x 1)
mjtNum* flexedge_J; // flex edge Jacobian (nJfe x 1)
mjtNum* flexedge_length; // flex edge lengths (nflexedge x 1)
mjtNum* flexvert_J; // flex vertex Jacobian (nJfv x 2)
@@ -369,6 +374,18 @@ typedef struct mjData_ {
mjtNum* efc_vel; // velocity in constraint space: J*qvel (nefc x 1)
mjtNum* efc_aref; // reference pseudo-acceleration (nefc x 1)
// computed by mj_fwdPosition/mj_invPosition when the implicit effective metric M+K is active
mjtNum* efm_c; // smooth-force shift h*K*qvel (nv x 1)
int* efm_K_rownnz; // effective-stiffness CSR row nonzeros (nv x 1)
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)
//-------------------- arena-allocated: POSITION, VELOCITY, CONTROL/ACCELERATION dependent
// computed by mj_fwdConstraint/mj_inverse
@@ -584,6 +601,8 @@ typedef struct mjModel_ {
mjtSize nflexelemdata; // number of element vertex ids in all flexes
mjtSize nflexstiffness; // number of stiffness parameters in all flexes
mjtSize nflexbending; // number of bending parameters in all flexes
mjtSize nefm0dof; // number of dofs covered by the constant metric factor
mjtSize nefm0L; // number of non-zeros in the constant metric factor
mjtSize nflexelemedge; // number of element edge ids in all flexes
mjtSize nflexshelldata; // number of shell fragment vertex ids in all flexes
mjtSize nflexevpair; // number of element-vertex pairs in all flexes
@@ -902,6 +921,11 @@ typedef struct mjModel_ {
mjtNum* flex_size; // vertex bounding box half sizes in qpos0 (nflex x 3)
mjtNum* flex_stiffness; // finite element stiffness matrix (nflexstiffness x 1)
mjtNum* flex_bending; // bending stiffness (nflexbending x 1)
int* efm0_dofid; // constant metric factor row->dof address (nefm0dof x 1)
int* efm0_L_rownnz; // constant metric factor row nonzeros (nefm0dof x 1)
int* efm0_L_rowadr; // constant metric factor row addresses (nefm0dof x 1)
int* efm0_L_colind; // constant metric factor column indices (nefm0L x 1)
mjtNum* efm0_L; // factor of M + (dt^2+dt*d)*K_bend (nefm0L x 1)
mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1)
mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1)
mjtNum* flex_edgedamping; // edge damping (nflex x 1)
include/mujoco/mjdata.h
+17
View File
@@ -135,6 +135,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 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 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
@@ -224,6 +228,7 @@ typedef struct mjData_ {
// computed by mj_fwdPosition/mj_flex
mjtNum* flexvert_xpos; // Cartesian flex vertex positions (nflexvert x 3)
mjtNum* flexelem_aabb; // flex element bounding boxes (center, size) (nflexelem x 6)
mjtNum* flexelem_krot; // corotated element stiffness (implicit only) (nflexstiffness x 1)
mjtNum* flexedge_J; // flex edge Jacobian (nJfe x 1)
mjtNum* flexedge_length; // flex edge lengths (nflexedge x 1)
mjtNum* flexvert_J; // flex vertex Jacobian (nJfv x 2)
@@ -393,6 +398,18 @@ typedef struct mjData_ {
mjtNum* efc_vel; // velocity in constraint space: J*qvel (nefc x 1)
mjtNum* efc_aref; // reference pseudo-acceleration (nefc x 1)
// computed by mj_fwdPosition/mj_invPosition when the implicit effective metric M+K is active
mjtNum* efm_c; // smooth-force shift h*K*qvel (nv x 1)
int* efm_K_rownnz; // effective-stiffness CSR row nonzeros (nv x 1)
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)
//-------------------- arena-allocated: POSITION, VELOCITY, CONTROL/ACCELERATION dependent
// computed by mj_fwdConstraint/mj_inverse
include/mujoco/mjmodel.h
+7
View File
@@ -272,6 +272,8 @@ typedef struct mjModel_ {
mjtSize nflexelemdata; // number of element vertex ids in all flexes
mjtSize nflexstiffness; // number of stiffness parameters in all flexes
mjtSize nflexbending; // number of bending parameters in all flexes
mjtSize nefm0dof; // number of dofs covered by the constant metric factor
mjtSize nefm0L; // number of non-zeros in the constant metric factor
mjtSize nflexelemedge; // number of element edge ids in all flexes
mjtSize nflexshelldata; // number of shell fragment vertex ids in all flexes
mjtSize nflexevpair; // number of element-vertex pairs in all flexes
@@ -590,6 +592,11 @@ typedef struct mjModel_ {
mjtNum* flex_size; // vertex bounding box half sizes in qpos0 (nflex x 3)
mjtNum* flex_stiffness; // finite element stiffness matrix (nflexstiffness x 1)
mjtNum* flex_bending; // bending stiffness (nflexbending x 1)
int* efm0_dofid; // constant metric factor row->dof address (nefm0dof x 1)
int* efm0_L_rownnz; // constant metric factor row nonzeros (nefm0dof x 1)
int* efm0_L_rowadr; // constant metric factor row addresses (nefm0dof x 1)
int* efm0_L_colind; // constant metric factor column indices (nefm0L x 1)
mjtNum* efm0_L; // factor of M + (dt^2+dt*d)*K_bend (nefm0L x 1)
mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1)
mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1)
mjtNum* flex_edgedamping; // edge damping (nflex x 1)
include/mujoco/mjxmacro.h
+27 -1
View File
@@ -189,6 +189,8 @@
X( nflexelemdata ) \
X( nflexstiffness ) \
X( nflexbending ) \
X( nefm0dof ) \
X( nefm0L ) \
X( nflexelemedge ) \
X( nflexshelldata ) \
X( nflexevpair ) \
@@ -501,6 +503,11 @@
X ( mjtNum, flex_size, nflex, 3 ) \
X ( mjtNum, flex_stiffness, nflexstiffness, 1 ) \
X ( mjtNum, flex_bending, nflexbending, 1 ) \
X ( int, efm0_dofid, nefm0dof, 1 ) \
X ( int, efm0_L_rownnz, nefm0dof, 1 ) \
X ( int, efm0_L_rowadr, nefm0dof, 1 ) \
X ( int, efm0_L_colind, nefm0L, 1 ) \
X ( mjtNum, efm0_L, nefm0L, 1 ) \
X ( mjtNum, flex_damping, nflex, 1 ) \
X ( mjtNum, flex_edgestiffness, nflex, 1 ) \
X ( mjtNum, flex_edgedamping, nflex, 1 ) \
@@ -866,6 +873,7 @@
X ( mjtNum, cinert, nbody, 10 ) \
X ( mjtNum, flexvert_xpos, nflexvert, 3 ) \
X ( mjtNum, flexelem_aabb, nflexelem, 6 ) \
X ( mjtNum, flexelem_krot, nflexstiffness, 1 ) \
X ( mjtNum, flexedge_J, nJfe, 1 ) \
X ( mjtNum, flexedge_length, nflexedge, 1 ) \
X ( mjtNum, flexvert_J, nJfv, 2 ) \
@@ -995,11 +1003,25 @@
X ( mjtNum, ifrc_constraint, MJ_D(nidof), 1 )
// array fields of mjData that live in d->arena
#define MJDATA_ARENA_POINTERS_EFM \
X ( mjtNum, efm_c, MJ_M(nv), 1 ) \
X ( int, efm_K_rownnz, MJ_M(nv), 1 ) \
X ( int, efm_K_rowadr, MJ_M(nv), 1 ) \
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 )
#define MJDATA_ARENA_POINTERS \
MJDATA_ARENA_POINTERS_CONTACT \
MJDATA_ARENA_POINTERS_SOLVER \
MJDATA_ARENA_POINTERS_DUAL \
MJDATA_ARENA_POINTERS_ISLAND
MJDATA_ARENA_POINTERS_ISLAND \
MJDATA_ARENA_POINTERS_EFM
// scalar fields of mjData
@@ -1021,6 +1043,10 @@
X( int, nl ) \
X( int, nefc ) \
X( int, nJ ) \
X( int, efm_active ) \
X( int, nefmK ) \
X( int, nefmdof ) \
X( int, nefmL ) \
X( int, nY ) \
X( int, nA ) \
X( int, nisland ) \
model/flex/bunny.xml
+1 -1
View File
@@ -16,7 +16,7 @@
<mujoco model="Trilinear">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="Euler"/>
<size memory="100M"/>
model/flex/bunny_multicell.xml
+1 -1
View File
@@ -16,7 +16,7 @@
<mujoco model="Trilinear">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" integrator="implicitfast"/>
<option solver="CG" tolerance="1e-6" integrator="Euler"/>
<size memory="10M"/>
model/flex/bunny_quadratic.xml
+1 -1
View File
@@ -16,7 +16,7 @@
<mujoco model="Trilinear">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".0005" integrator="implicitfast"/>
<option solver="CG" tolerance="1e-6" timestep=".0005" integrator="Euler"/>
<size memory="100M"/>
model/flex/bunny_with_uv.xml
+48 -48
View File
@@ -1,48 +1,48 @@
<!-- Copyright 2024 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="Trilinear">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
<size memory="100M"/>
<visual>
<map stiffness="100"/>
</visual>
<asset>
<texture name="texsponge" type="2d" file="sponge.png"/>
<material name="matsponge" texture="texsponge" specular="0.3"/>
</asset>
<worldbody>
<body>
<joint name="press" type="slide" axis="0 0 1" damping="500"/>
<geom type="box" size=".02 .2 .2" pos="0 0 .5"/>
</body>
<flexcomp type="mesh" file="bunny_with_uv.obj" pos="0 0 .1" dim="2" euler="90 0 0"
radius=".001" material="matsponge" mass=".05" name="softbody" dof="trilinear">
<elasticity young="1e3" poisson="0.1" damping="0.001" elastic2d="none"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
<actuator>
<position name="press" joint="press" gear="-1 0 0 0 0 0" ctrlrange="-1 1" kp="1000"/>
</actuator>
</mujoco>
<!-- Copyright 2024 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="Trilinear">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="Euler"/>
<size memory="100M"/>
<visual>
<map stiffness="100"/>
</visual>
<asset>
<texture name="texsponge" type="2d" file="sponge.png"/>
<material name="matsponge" texture="texsponge" specular="0.3"/>
</asset>
<worldbody>
<body>
<joint name="press" type="slide" axis="0 0 1" damping="500"/>
<geom type="box" size=".02 .2 .2" pos="0 0 .5"/>
</body>
<flexcomp type="mesh" file="bunny_with_uv.obj" pos="0 0 .1" dim="2" euler="90 0 0"
radius=".001" material="matsponge" mass=".05" name="softbody" dof="trilinear">
<elasticity young="1e3" poisson="0.1" damping="0.001" elastic2d="none"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
<actuator>
<position name="press" joint="press" gear="-1 0 0 0 0 0" ctrlrange="-1 1" kp="1000"/>
</actuator>
</mujoco>
model/flex/jelly.xml
+1 -1
View File
@@ -16,7 +16,7 @@
<mujoco model="Jelly">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="Euler"/>
<size memory="10M"/>
model/flex/pancake.xml
+1 -1
View File
@@ -16,7 +16,7 @@
<mujoco model="Pancake">
<include file="scene.xml"/>
<option timestep="0.001" solver="CG" tolerance="1e-6" integrator="implicitfast"/>
<option timestep="0.001" solver="CG" tolerance="1e-6" integrator="Euler"/>
<size memory="10M"/>
model/flex/plate.xml
+1 -1
View File
@@ -16,7 +16,7 @@
<mujoco model="Plate">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" integrator="implicitfast"/>
<option solver="CG" tolerance="1e-6" integrator="Euler"/>
<size memory="10M"/>
model/flex/quadratic.xml
+1 -1
View File
@@ -16,7 +16,7 @@
<mujoco model="Quadratic">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="Euler"/>
<size memory="10M"/>
model/flex/sphere_trilinear.xml
+1 -1
View File
@@ -16,7 +16,7 @@
<mujoco model="Trilinear flex spheres">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep="1e-3" integrator="implicitfast"/>
<option solver="CG" tolerance="1e-6" timestep="1e-3" integrator="Euler"/>
<size memory="10M"/>
model/flex/trampoline.xml
+1 -1
View File
@@ -16,7 +16,7 @@
<mujoco model="Trampoline">
<include file="scene.xml"/>
<option timestep="0.001" solver="CG" tolerance="1e-6" integrator="implicitfast"/>
<option timestep="0.001" solver="CG" tolerance="1e-6" integrator="Euler"/>
<size memory="10M"/>
model/flex/trilinear.xml
+1 -1
View File
@@ -16,7 +16,7 @@
<mujoco model="Trilinear">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="Euler"/>
<size memory="10M"/>
python/mujoco/functions.cc
+1
View File
@@ -1697,6 +1697,7 @@ PYBIND11_MODULE(_functions, pymodule, pybind11::mod_gil_not_used()) {
MJDATA_ARENA_POINTERS_SOLVER
MJDATA_ARENA_POINTERS_DUAL
MJDATA_ARENA_POINTERS_ISLAND
MJDATA_ARENA_POINTERS_EFM
#undef X
};
python/mujoco/introspect/structs.py
+158
View File
@@ -1078,6 +1078,16 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='mjtSize'),
doc='number of bending parameters in all flexes',
),
StructFieldDecl(
name='nefm0dof',
type=ValueType(name='mjtSize'),
doc='number of dofs covered by the constant metric factor',
),
StructFieldDecl(
name='nefm0L',
type=ValueType(name='mjtSize'),
doc='number of non-zeros in the constant metric factor',
),
StructFieldDecl(
name='nflexelemedge',
type=ValueType(name='mjtSize'),
@@ -3115,6 +3125,46 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='bending stiffness',
array_extent=('nflexbending',),
),
StructFieldDecl(
name='efm0_dofid',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='constant metric factor row->dof address',
array_extent=('nefm0dof',),
),
StructFieldDecl(
name='efm0_L_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='constant metric factor row nonzeros',
array_extent=('nefm0dof',),
),
StructFieldDecl(
name='efm0_L_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='constant metric factor row addresses',
array_extent=('nefm0dof',),
),
StructFieldDecl(
name='efm0_L_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='constant metric factor column indices',
array_extent=('nefm0L',),
),
StructFieldDecl(
name='efm0_L',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='factor of M + (dt^2+dt*d)*K_bend',
array_extent=('nefm0L',),
),
StructFieldDecl(
name='flex_damping',
type=PointerType(
@@ -5632,6 +5682,26 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='int'),
doc='number of non-zeros in constraint Jacobian',
),
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
),
StructFieldDecl(
name='nefmK',
type=ValueType(name='int'),
doc='number of non-zeros in effective-stiffness CSR',
),
StructFieldDecl(
name='nefmdof',
type=ValueType(name='int'),
doc='number of rows in effective-metric factor',
),
StructFieldDecl(
name='nefmL',
type=ValueType(name='int'),
doc='number of non-zeros in the effective-metric factor',
),
StructFieldDecl(
name='nY',
type=ValueType(name='int'),
@@ -6031,6 +6101,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='flex element bounding boxes (center, size)',
array_extent=('nflexelem', 6),
),
StructFieldDecl(
name='flexelem_krot',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='corotated element stiffness (implicit only)',
array_extent=('nflexstiffness',),
),
StructFieldDecl(
name='flexedge_J',
type=PointerType(
@@ -6855,6 +6933,86 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='reference pseudo-acceleration',
array_extent=('nefc',),
),
StructFieldDecl(
name='efm_c',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='smooth-force shift h*K*qvel',
array_extent=('nv',),
),
StructFieldDecl(
name='efm_K_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='effective-stiffness CSR row nonzeros',
array_extent=('nv',),
),
StructFieldDecl(
name='efm_K_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='effective-stiffness CSR row addresses',
array_extent=('nv',),
),
StructFieldDecl(
name='efm_K_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='effective-stiffness CSR column indices',
array_extent=('nefmK',),
),
StructFieldDecl(
name='efm_K_val',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='effective-stiffness CSR values',
array_extent=('nefmK',),
),
StructFieldDecl(
name='efm_dofid',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='factor row -> dof address',
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',
array_extent=('nefmL',),
),
StructFieldDecl(
name='efc_b',
type=PointerType(
python/mujoco/structs.cc
+1
View File
@@ -893,6 +893,7 @@ This is useful for example when the MJB is not available as a file on disk.)"));
MJDATA_ARENA_POINTERS_SOLVER
MJDATA_ARENA_POINTERS_DUAL
MJDATA_ARENA_POINTERS_ISLAND
MJDATA_ARENA_POINTERS_EFM
#undef MJ_M
#define MJ_M(x) (x)
src/engine/engine_derivative.c
+1138
View File
File diff suppressed because it is too large Load Diff
src/engine/engine_derivative.h
+33
View File
@@ -65,6 +65,39 @@ MJAPI void mjd_flexInterp_cacheKrot(const mjModel* m, mjData* d, mjtNum* K_rot_o
MJAPI void mjd_flexBend_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,
mjtNum s1, mjtNum s2);
// compute res += scale * K_stretch * vec for standard (non-interp) flex stretch,
// K_stretch the Gauss-Newton Hessian of the passive stretch force at the current state
// scale = s1 + s2 * flex_damping[f] per flex
MJAPI void mjd_flexStretch_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,
mjtNum s1, mjtNum s2);
// assemble the standard-flex implicit stiffness (s1 + s2*damping)*(K_bend + K_stretch) into
// dof-level CSR; phase 1 (colind==NULL) fills rownnz/rowadr and returns total nnz, phase 2
// fills colind/val. Interp flexes are assembled iff Krot (mjd_flexInterp_cacheKrot cache) is
// non-NULL and the centered fast path applies (check mjd_flexInterpAssemblable first).
MJAPI int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr,
int* colind, mjtNum* val, mjtNum s1, mjtNum s2,
int flg_bend, int flg_stretch, const mjtNum* Krot);
// can all interp flexes be assembled to dof-level CSR? (centered fast path everywhere)
MJAPI mjtBool mjd_flexInterpAssemblable(const mjModel* m);
// does any flex contribute assemblable implicit stiffness? (existence check)
MJAPI mjtBool mjd_flexStiff_any(const mjModel* m, int flg_interp);
// implicit effective metric Mtilde = M + (h^2+h*d)*K: per-step arena object (see mjdata.h efm_*)
// build (or deactivate, active==0); the gate decision belongs to the caller
MJAPI void mjd_effBuild(const mjModel* m, mjData* d, int active, int flg_factor);
// refresh the metric's smooth-force shift c = h*K*qvel (values only, velocity stage)
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
MJAPI void mjd_effSolve(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* b);
#ifdef __cplusplus
}
src/engine/engine_forward.c
+65 -121
View File
@@ -168,6 +168,11 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
mj_transmission(m, d);
TM_ADD(mjTIMER_POS_KINEMATICS);
// implicit effective metric Mtilde = M + K: build (or deactivate) for this step. Arena
// lifetime and skip semantics mirror the constraint data: built once per position stage,
// value-refreshed in the velocity stage, consumed downstream.
mjd_effBuild(m, d, mj_flexCG(m), /*flg_factor=*/1);
TM_END1(mjTIMER_POSITION);
}
@@ -214,6 +219,8 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
// add bias force due to tendon armature
mj_tendonBias(m, d, d->qfrc_bias);
mjd_effShift(m, d);
TM_END(mjTIMER_VELOCITY);
}
@@ -786,6 +793,18 @@ void mj_fwdAcceleration(const mjModel* m, mjData* d) {
// qfrc_smooth += project(xfrc_applied)
mj_xfrcAccumulate(m, d, d->qfrc_smooth);
// implicit effective metric (built in mj_fwdPosition): the smooth acceleration is that of
// the linearly-implicit dynamics, (M + K)*qacc_smooth = qfrc_smooth + c, so the constraint
// solver, the no-constraint shortcut and the warmstart all see one consistent metric.
if (d->efm_active) {
mj_markStack(d);
mjtNum* qfrc_eff = mjSTACKALLOC(d, nv, mjtNum);
mju_add(qfrc_eff, d->qfrc_smooth, d->efm_c, nv);
mjd_effSolve(m, d, d->qacc_smooth, qfrc_eff);
mj_freeStack(d);
return;
}
// copy for in-place solve: qacc_smooth = qfrc_smooth
if (!sleep_filter) {
mju_copy(d->qacc_smooth, d->qfrc_smooth, nv);
@@ -900,6 +919,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
mju_zero(d->qfrc_constraint, nv);
// no constraints: copy unconstrained acc, clear forces, return
// (with the effective metric active, qacc_smooth is already the implicit answer)
if (!nefc) {
mju_copy(d->qacc, d->qacc_smooth, nv);
mju_zeroInt(d->solver_niter, mjNISLAND);
@@ -921,7 +941,16 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
mju_zeroInt(d->solver_niter, mjNISLAND);
// check if islands are supported
int islands_supported = !mjDISABLED(mjDSBL_ISLAND) && nisland > 0;
// 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
// 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
// island's dofs, gather/scatter its island-local vectors around the covered-compact
// factor solves (the factors themselves need no change), and enable the metric path
// (flg_flex) for the flex-containing island alone.
int islands_supported = !mjDISABLED(mjDSBL_ISLAND) && nisland > 0 && !mj_flexCG(m);
// run solver over constraint islands
if (islands_supported) {
@@ -1298,7 +1327,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
// return 1 if any flex needs implicit stiffness treatment (interp or bending)
static int flex_has_implicit_stiffness(const mjModel* m) {
static mjtBool flex_has_implicit_stiffness(const mjModel* m) {
for (int f=0; f < m->nflex; f++) {
if (m->flex_rigid[f]) {
continue;
@@ -1316,120 +1345,32 @@ static int flex_has_implicit_stiffness(const mjModel* m) {
m->flex_bendingadr[f] >= 0) {
return 1;
}
// standard flex with stretch
if (!m->flex_interp[f] && m->flex_dim[f] >= 2 &&
m->flex_stiffnessadr[f] >= 0 &&
m->flex_stiffness[m->flex_stiffnessadr[f]] != 0) {
return 1;
}
}
return 0;
}
// preconditioned CG solve for implicit flex interp
// solves (M - h*qDeriv - (h^2+h*d)*K) * qacc = qfrc - h*K*qvel
// where K is the flex stiffness, using the already-factored standard system
// (M - h*qDeriv) as a preconditioner
static void flexInterp_cgsolve(const mjModel* m, mjData* d,
mjtNum* qacc, const mjtNum* qfrc, int nv) {
mjtNum h = m->opt.timestep;
int implicit = (m->opt.integrator == mjINT_IMPLICIT);
mj_markStack(d);
// allocate CG work vectors
mjtNum* rhs = mjSTACKALLOC(d, nv, mjtNum);
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* temp = mjSTACKALLOC(d, nv, mjtNum);
// precompute K_rot cache: same layout as m->flex_stiffness
int krot_size = m->nflexstiffness;
mjtNum* K_rot_cache = mjSTACKALLOC(d, krot_size, mjtNum);
mju_zero(K_rot_cache, krot_size);
mjd_flexInterp_cacheKrot(m, d, K_rot_cache);
// build RHS: rhs = qfrc
mju_copy(rhs, qfrc, nv);
// flex_interp velocity correction: rhs += h*K_interp*qvel (K_interp is NSD)
mjd_flexInterp_mul(m, d, rhs, d->qvel, h, 0, K_rot_cache);
// standard flex bending velocity correction: rhs -= h*K_bend*qvel
mjd_flexBend_mul(m, d, rhs, d->qvel, -h, 0); // rhs -= h*K_bend*v
// --- helper: compute Ap = A*x ---
// A*x = (M - h*qDeriv)*x - (h^2+h*d)*K_interp*x + (h^2+h*d)*K_bend*x
#define FLEX_CG_MATVEC(Ap_out, x_in) \
mju_mulMatVecSparse(Ap_out, d->qDeriv, x_in, nv, m->D_rownnz, m->D_rowadr, \
m->D_colind, NULL); \
mju_mulSymVecSparse(temp, d->M, x_in, nv, m->M_rownnz, m->M_rowadr, \
m->M_colind); \
mju_addScl(Ap_out, temp, Ap_out, -h, nv); \
mjd_flexInterp_mul(m, d, Ap_out, x_in, -(h*h), -h, K_rot_cache); \
mjd_flexBend_mul(m, d, Ap_out, x_in, h*h, h)
// --- helper: preconditioner solve z = (M - h*qDeriv)^{-1} * r ---
#define FLEX_CG_PRECOND(z_out, r_in) \
if (implicit) { \
mju_solveLUSparse(z_out, d->qLU, r_in, nv, m->D_rownnz, m->D_rowadr, \
m->D_diag, m->D_colind, NULL); \
} else { \
mju_copy(z_out, r_in, nv); \
mj_solveLD(z_out, d->qH, d->qHDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, \
m->M_colind, NULL); \
}
// initial residual: r = rhs - A*qacc
FLEX_CG_MATVEC(Ap, qacc);
mju_sub(r, rhs, Ap, nv);
// check if already converged
mjtNum rnorm = mju_dot(r, r, nv);
mjtNum tol = 1e-10 * mju_dot(rhs, rhs, nv);
if (rnorm < tol || rnorm < mjMINVAL) {
mj_freeStack(d);
return;
}
// z = precond(r), p = z
FLEX_CG_PRECOND(z, r);
mju_copy(p, z, nv);
mjtNum rz = mju_dot(r, z, nv);
// CG iterations
int maxiter = 50;
for (int iter=0; iter < maxiter; iter++) {
FLEX_CG_MATVEC(Ap, p);
// alpha = rz / dot(p, Ap)
mjtNum pAp = mju_dot(p, Ap, nv);
if (mju_abs(pAp) < mjMINVAL) break;
mjtNum alpha = rz / pAp;
// qacc += alpha * p
mju_addToScl(qacc, p, alpha, nv);
// r -= alpha * Ap
mju_addToScl(r, Ap, -alpha, nv);
// check convergence
rnorm = mju_dot(r, r, nv);
if (rnorm < tol || rnorm < mjMINVAL) break;
// z = precond(r)
FLEX_CG_PRECOND(z, r);
// beta = rz_new / rz
mjtNum rz_new = mju_dot(r, z, nv);
mjtNum beta = rz_new / mju_max(mjMINVAL, rz);
// p = z + beta * p
mju_addScl(p, z, p, beta, nv);
rz = rz_new;
}
#undef FLEX_CG_MATVEC
#undef FLEX_CG_PRECOND
mj_freeStack(d);
// implicit-flex solve gate: with the CG solver, an implicit integrator and flex stiffness
// present, the CG solve carries the implicit flex stiffness itself -- K = (h^2+h*d) times the flex stiffness enters
// the objective/gradient/linesearch, and the preconditioned gradient becomes (M+K)\grad by
// linear matrix-free CG against the existing M factor (the in-solver form of the old post-hoc
// flexInterp_cgsolve treatment, no factorization anywhere); mj_implicitSkip then folds the
// implicit flex force of the solver's qacc into qfrc. When active with islands
// enabled, mj_fwdConstraint forces a monolithic solve (flex mesh coupling is invisible to
// constraint islanding). solver="Newton" keeps its exact-factorization semantics untouched.
// Models outside the gate integrate flex elasticity explicitly.
int mj_flexCG(const mjModel* m) {
return m->opt.solver == mjSOL_CG &&
(m->opt.integrator == mjINT_IMPLICIT || m->opt.integrator == mjINT_IMPLICITFAST) &&
m->opt.cone != mjCONE_ELLIPTIC && !mjENABLED(mjENBL_SLEEP) &&
flex_has_implicit_stiffness(m);
}
@@ -1454,11 +1395,16 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
}
// check for flex_interp that needs implicit treatment
int has_flex_stiffness = !sleep_filter && flex_has_implicit_stiffness(m);
// implicit flex stiffness is carried by the constraint solver (see mj_flexCG): use the
// solver's qacc directly. The qDeriv treatment is skipped for these models -- flex damping
// is already implicit inside the solve (the s2 terms of B), joint damping and other velocity
// derivatives integrate explicitly. This avoids both the qDeriv machinery and the
// sequential flex-vs-qDeriv splitting. Models outside the gate (non-Newton solver, elliptic
// cones, islands, sleep) integrate flex elasticity explicitly.
int flexcg = !sleep_filter && mj_flexCG(m);
// factorization
if (!skipfactor) {
if (!skipfactor && !flexcg) {
// implicit
if (m->opt.integrator == mjINT_IMPLICIT) {
// compute analytical derivative qDeriv
@@ -1495,7 +1441,10 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
}
// standard sparse solve
if (m->opt.integrator == mjINT_IMPLICIT) {
if (flexcg) {
// constraint solver's qacc already carries the implicit flex force
mju_copy(qacc, d->qacc, m->nv);
} else if (m->opt.integrator == mjINT_IMPLICIT) {
mju_solveLUSparse(qacc, d->qLU, qfrc, nv, m->D_rownnz, m->D_rowadr, m->D_diag, m->D_colind,
dof_awake_ind);
} else {
@@ -1508,16 +1457,11 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, m->M_colind, dof_awake_ind);
}
// flex: CG correction for implicit flex stiffness
if (has_flex_stiffness) {
flexInterp_cgsolve(m, d, qacc, qfrc, m->nv);
}
// implicitfast: local unsymmetric solve for standalone free bodies
// adds the bias (gyroscopic) derivative, dropped from the global symmetric solve; the
// 6x6 block of M - h*D is decoupled from the rest of the system (D sparsity is tree-local),
// so overwriting these rows of qacc leaves all other DOFs unaffected
if (m->opt.integrator == mjINT_IMPLICITFAST) {
if (m->opt.integrator == mjINT_IMPLICITFAST && !flexcg) {
for (int j=0; j < m->njnt; j++) {
mjtNum A[36];
if (!mjd_freeMhat(m, d, j, m->opt.timestep, A)) {
src/engine/engine_forward.h
+4
View File
@@ -64,6 +64,10 @@ MJAPI void mj_implicit(const mjModel *m, mjData *d);
// fully implicit in velocity, possibly skipping factorization
MJAPI void mj_implicitSkip(const mjModel *m, mjData *d, int skipfactor);
// implicit-flex solve gate: with solver=CG, an implicit integrator and flex stiffness present,
// the constraint solve is dispatched to the flex-augmented primal solver (monolithic,
// pyramidal cones only); solver=Newton keeps its exact-factorization semantics
int mj_flexCG(const mjModel* m);
//-------------------------------- solver components -----------------------------------------------
src/engine/engine_inverse.c
+13
View File
@@ -70,6 +70,9 @@ void mj_invPosition(const mjModel* m, mjData* d) {
mj_transmission(m, d);
TM_ADD(mjTIMER_POS_KINEMATICS);
// implicit effective metric: multiply-only build (no factorization) for the inverse
mjd_effBuild(m, d, mj_flexCG(m), /*flg_factor=*/0);
TM_END1(mjTIMER_POSITION);
}
@@ -177,6 +180,9 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
mj_solveM(m, d, qacc, qfrc, 1);
mj_freeStack(d);
// refresh the effective-metric velocity shift
mjd_effShift(m, d);
}
@@ -262,6 +268,13 @@ void mj_inverseSkip(const mjModel* m, mjData* d,
mjtNum* Ma = mjSTACKALLOC(d, nv, mjtNum);
mj_mulM(m, d, Ma, d->qacc);
// implicit effective metric (built in mj_invPosition): the forward dynamics solved
// (M+K)*qacc = qfrc + c + J'*f, so the discrete-consistent inverse adds K*qacc - c
if (d->efm_active) {
mjd_effMulAdd(m, d, Ma, d->qacc);
mju_subFrom(Ma, d->efm_c, nv);
}
// qfrc_inverse += Ma - qfrc_passive - qfrc_constraint
for (int i=0; i < nv; i++) {
d->qfrc_inverse[i] += Ma[i] - d->qfrc_passive[i] - d->qfrc_constraint[i];
src/engine/engine_io.c
+11 -3
View File
@@ -230,7 +230,8 @@ void mj_makeModel(mjModel** dest,
mjtSize nbvhdynamic, mjtSize noct, mjtSize njnt, mjtSize ntree, mjtSize nM, mjtSize nB,
mjtSize nC, mjtSize nD, mjtSize ngeom, mjtSize nsite, mjtSize ncam, mjtSize nlight,
mjtSize nflex, mjtSize nflexnode, mjtSize nflexvert, mjtSize nflexedge, mjtSize nflexelem,
mjtSize nflexelemdata, mjtSize nflexstiffness, mjtSize nflexbending, mjtSize nflexelemedge,
mjtSize nflexelemdata, mjtSize nflexstiffness, mjtSize nflexbending,
mjtSize nefm0dof, mjtSize nefm0L, mjtSize nflexelemedge,
mjtSize nflexshelldata, mjtSize nflexevpair, mjtSize nflextexcoord, mjtSize nJfe, mjtSize nJfv,
mjtSize nmesh, mjtSize nmeshvert, mjtSize nmeshnormal, mjtSize nmeshtexcoord, mjtSize nmeshface,
mjtSize nmeshgraph, mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap,
@@ -324,6 +325,8 @@ void mj_makeModel(mjModel** dest,
m->nflexelemdata = nflexelemdata;
m->nflexstiffness = nflexstiffness;
m->nflexbending = nflexbending;
m->nefm0dof = nefm0dof;
m->nefm0L = nefm0L;
m->nflexelemedge = nflexelemedge;
m->nflexshelldata = nflexshelldata;
m->nflexevpair = nflexevpair;
@@ -436,7 +439,7 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
src->nbvhdynamic, src->noct, src->njnt, src->ntree, src->nM, src->nB, src->nC, src->nD,
src->ngeom, src->nsite, src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert,
src->nflexedge, src->nflexelem, src->nflexelemdata, src->nflexstiffness,
src->nflexbending, src->nflexelemedge, src->nflexshelldata, src->nflexevpair,
src->nflexbending, src->nefm0dof, src->nefm0L, src->nflexelemedge, src->nflexshelldata, src->nflexevpair,
src->nflextexcoord, src->nJfe, src->nJfv, src->nmesh, src->nmeshvert, src->nmeshnormal,
src->nmeshtexcoord, src->nmeshface, src->nmeshgraph, src->nmeshpoly, src->nmeshpolyvert,
src->nmeshpolymap, src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
@@ -615,7 +618,8 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
sizes[56], sizes[57], sizes[58], sizes[59], sizes[60], sizes[61], sizes[62],
sizes[63], sizes[64], sizes[65], sizes[66], sizes[67], sizes[68], sizes[69],
sizes[70], sizes[71], sizes[72], sizes[73], sizes[74], sizes[75], sizes[76],
sizes[77], sizes[78], sizes[79], sizes[80], sizes[81]);
sizes[77], sizes[78], sizes[79], sizes[80], sizes[81],
sizes[82], sizes[83]);
// mj_makeModel may fail if the input buffer has invalid sizes
if (!m) {
@@ -1343,6 +1347,10 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
d->nA = 0;
d->nisland = 0;
d->nidof = 0;
d->efm_active = 0;
d->nefmK = 0;
d->nefmdof = 0;
d->nefmL = 0;
// clear global properties
d->time = 0;
src/engine/engine_io.h
+2 -1
View File
@@ -53,7 +53,8 @@ void mj_makeModel(mjModel** dest,
mjtSize nbvhdynamic, mjtSize noct, mjtSize njnt, mjtSize ntree, mjtSize nM, mjtSize nB,
mjtSize nC, mjtSize nD, mjtSize ngeom, mjtSize nsite, mjtSize ncam, mjtSize nlight,
mjtSize nflex, mjtSize nflexnode, mjtSize nflexvert, mjtSize nflexedge, mjtSize nflexelem,
mjtSize nflexelemdata, mjtSize nflexstiffness, mjtSize nflexbending, mjtSize nflexelemedge,
mjtSize nflexelemdata, mjtSize nflexstiffness, mjtSize nflexbending,
mjtSize nefm0dof, mjtSize nefm0L, mjtSize nflexelemedge,
mjtSize nflexshelldata, mjtSize nflexevpair, mjtSize nflextexcoord, mjtSize nJfe, mjtSize nJfv,
mjtSize nmesh, mjtSize nmeshvert, mjtSize nmeshnormal, mjtSize nmeshtexcoord, mjtSize nmeshface,
mjtSize nmeshgraph, mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap,
src/engine/engine_island.c
+41
View File
@@ -371,6 +371,47 @@ static int findEdges(const mjModel* m, const mjData* d,
}
}
// flex stiffness couples all vertices (nodes for interpolated flexes) of a flex without any
// constraint row representing the coupling: union the trees of every stiffness-active flex
// (star around the first dynamic tree). This keeps the partition valid when the implicit
// effective metric (mj_flexCG) carries the stiffness inside the constraint solve. Awake
// trees only: sleeping trees must stay out of islands (mj_sleep invariant, matching the
// constraint filter); waking a flex as a unit remains the wake machinery's job.
for (int f=0; f < m->nflex; f++) {
// mirror the stiffness-activity conditions of engine_derivative's flexStiff_active /
// flexInterp_processed: deformable dim>=2 flex with bending or nonzero stiffness
if (m->flex_rigid[f] || m->flex_dim[f] < 2) {
continue;
}
int sadr = m->flex_stiffnessadr[f];
if (m->flex_bendingadr[f] < 0 && (sadr < 0 || m->flex_stiffness[sadr] == 0)) {
continue;
}
int num, adr;
const int* bodyid;
if (m->flex_interp[f]) {
num = m->flex_nodenum[f];
adr = m->flex_nodeadr[f];
bodyid = m->flex_nodebodyid;
} else {
num = m->flex_vertnum[f];
adr = m->flex_vertadr[f];
bodyid = m->flex_vertbodyid;
}
int tree1 = -1;
for (int j=0; j < num; j++) {
int treeid = m->body_treeid[bodyid[adr+j]];
if (treeid < 0 || treeid == tree1 || !d->tree_awake[treeid]) {
continue;
}
if (tree1 < 0) {
tree1 = treeid;
} else {
nnz += addEdge(rownnz, colind, tree_tree, ntree, tree1, treeid);
}
}
}
return nnz;
}
src/engine/engine_setconst.c
+164
View File
@@ -23,11 +23,13 @@
#include <mujoco/mjsan.h> // IWYU pragma: keep
#include "engine/engine_core_smooth.h"
#include "engine/engine_core_util.h"
#include "engine/engine_derivative.h"
#include "engine/engine_forward.h"
#include "engine/engine_io.h"
#include "engine/engine_memory.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_solve.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_sparse.h"
#include "engine/engine_util_spatial.h"
@@ -1320,6 +1322,165 @@ static void setSpring(mjModel* m, mjData* d) {
// entry point: set all remaining constant fields of mjModel, except for lengthrange
// constant part of the implicit effective metric factor (currently dim-2 bending): sparse
// reverse-Cholesky of M + (h^2 + h*damping)*K_bend over
// the dofs of unpinned vertices of standard dim-2 flexes with bending. The matrix is constant
// (flat-rest bending stiffness, point masses), so the factor is computed here once and reused
// by the implicit-flex constraint solve every step. Bending couples only same-coordinate dofs,
// so the pattern is three interleaved copies of the vertex flap adjacency. Row order (flex
// order, vertex order, coordinate fastest) and the resulting fill count must match the
// compiler's symbolic sizing (checked below).
static void setEfm0Factor(mjModel* m, mjData* d) {
int nbd = m->nefm0dof;
if (!nbd) {
return;
}
mj_markStack(d);
mjtNum h = m->opt.timestep;
// enumerate covered vertices: compact slot per unpinned vertex of qualifying flexes
// (filter matches the compiler's sizing and, for bending, flexStiff_active in
// engine_derivative.c: bending data exists only for dim-2 flexes)
int* vslot = mjSTACKALLOC(d, m->nflexvert > 0 ? m->nflexvert : 1, int);
for (int i=0; i < m->nflexvert; i++) {
vslot[i] = -1;
}
int nfree = 0;
for (int f=0; f < m->nflex; f++) {
if (m->flex_interp[f] || m->flex_rigid[f] || m->flex_dim[f] != 2 ||
m->flex_bendingadr[f] < 0) {
continue;
}
for (int lv=0; lv < m->flex_vertnum[f]; lv++) {
int gv = m->flex_vertadr[f] + lv;
if (m->body_dofnum[m->flex_vertbodyid[gv]] == 3) {
vslot[gv] = nfree;
nfree++;
}
}
}
if (3*nfree != nbd) {
mj_freeStack(d);
mjERROR("constant metric factor dof count mismatch: compiler sized %d, engine found %d",
nbd, 3*nfree);
}
// fill row -> dof address (row 3*slot + k, coordinate fastest)
for (int gv=0; gv < m->nflexvert; gv++) {
if (vslot[gv] >= 0) {
int da = m->body_dofadr[m->flex_vertbodyid[gv]];
for (int k=0; k < 3; k++) {
m->efm0_dofid[3*vslot[gv] + k] = da + k;
}
}
}
// assemble the bending-only stiffness K = (h^2 + h*damping)*K_bend over all dofs with the
// shared stencil walker from engine_derivative: bending values are configuration-independent
// and stretch/interp are gated off, so the call is valid at set-constants time (d is used
// for stack scratch only)
int nv = m->nv;
int* K_rownnz = mjSTACKALLOC(d, nv, int);
int* K_rowadr = mjSTACKALLOC(d, nv, int);
int nK = mjd_flexStiff_assemble(m, d, K_rownnz, K_rowadr, NULL, NULL, h*h, h,
/*flg_bend=*/1, /*flg_stretch=*/0, NULL);
int* K_colind = mjSTACKALLOC(d, nK > 0 ? nK : 1, int);
mjtNum* K_val = mjSTACKALLOC(d, nK > 0 ? nK : 1, mjtNum);
mjd_flexStiff_assemble(m, d, K_rownnz, K_rowadr, K_colind, K_val, h*h, h, 1, 0, NULL);
// inverse map: dof address -> compact factor row (monotone: slots follow dof order)
int* dofrow = mjSTACKALLOC(d, nv, int);
for (int i=0; i < nv; i++) {
dofrow[i] = -1;
}
for (int r=0; r < nbd; r++) {
dofrow[m->efm0_dofid[r]] = r;
}
// compact B to covered rows, keeping same-coordinate entries only: bending blocks are
// isotropic (q * I3), so the off-coordinate entries of assemble's 3x3 block pattern are
// structurally zero and dropping them preserves the pattern the compiler sized.
// H = M + (h^2+h*d)*K_bend in compact dof indices: lower CSR (values) + upper CSR (pattern)
int nHl = 0, nHu = 0;
for (int r=0; r < nbd; r++) {
int dof = m->efm0_dofid[r];
for (int c=0; c < K_rownnz[dof]; c++) {
int rc = dofrow[K_colind[K_rowadr[dof] + c]];
if (rc < 0 || (rc - r) % 3 != 0) continue; // uncovered or off-coordinate
if (rc < r) nHl++;
else if (rc > r) nHu++;
}
}
nHl += nbd; // diagonals: always present, also for rows without bending entries
int* Hl_rownnz = mjSTACKALLOC(d, nbd, int);
int* Hl_rowadr = mjSTACKALLOC(d, nbd, int);
int* Hl_colind = mjSTACKALLOC(d, nHl, int);
mjtNum* Hl_val = mjSTACKALLOC(d, nHl, mjtNum);
int* Hu_rownnz = mjSTACKALLOC(d, nbd, int);
int* Hu_rowadr = mjSTACKALLOC(d, nbd, int);
int* Hu_colind = mjSTACKALLOC(d, nHu > 0 ? nHu : 1, int);
int ladr = 0, uadr = 0;
for (int r=0; r < nbd; r++) {
int dof = m->efm0_dofid[r];
Hl_rowadr[r] = ladr;
Hu_rowadr[r] = uadr;
mjtNum diag = 0;
// B columns are dof-ascending, so filtered lower/upper columns stay ascending
for (int c=0; c < K_rownnz[dof]; c++) {
int adr = K_rowadr[dof] + c;
int rc = dofrow[K_colind[adr]];
if (rc < 0 || (rc - r) % 3 != 0) continue;
if (rc < r) {
Hl_colind[ladr] = rc;
Hl_val[ladr++] = K_val[adr];
} else if (rc > r) {
Hu_colind[uadr++] = rc;
} else {
diag = K_val[adr];
}
}
// diagonal last: point mass + armature + bending diagonal
Hl_colind[ladr] = r;
Hl_val[ladr++] = m->body_mass[m->dof_bodyid[dof]] + m->dof_armature[dof] + diag;
Hl_rownnz[r] = ladr - Hl_rowadr[r];
Hu_rownnz[r] = uadr - Hu_rowadr[r];
}
// symbolic factorization: counting phase (from the upper-triangle pattern)
int* LT_rownnz = mjSTACKALLOC(d, nbd, int);
int* LT_rowadr = mjSTACKALLOC(d, nbd, int);
int nnz = mju_cholFactorSymbolic(NULL, m->efm0_L_rownnz, m->efm0_L_rowadr,
NULL, LT_rownnz, LT_rowadr, NULL,
Hu_rownnz, Hu_rowadr, Hu_colind, nbd, d);
if (nnz != m->nefm0L) {
mj_freeStack(d);
mjERROR("constant metric factor size mismatch: compiler sized %d, symbolic found %d",
(int)m->nefm0L, nnz);
}
// symbolic factorization: filling phase
int* LT_colind = mjSTACKALLOC(d, nnz, int);
int* LT_map = mjSTACKALLOC(d, nnz, int);
mju_cholFactorSymbolic(m->efm0_L_colind, m->efm0_L_rownnz, m->efm0_L_rowadr,
LT_colind, LT_rownnz, LT_rowadr, LT_map,
Hu_rownnz, Hu_rowadr, Hu_colind, nbd, d);
// numeric factorization
int rank = mju_cholFactorNumeric(m->efm0_L, nbd, mjMINVAL,
m->efm0_L_rownnz, m->efm0_L_rowadr, m->efm0_L_colind,
LT_rownnz, LT_rowadr, LT_colind, LT_map,
Hl_val, Hl_rownnz, Hl_rowadr, Hl_colind, d);
if (rank != nbd) {
mj_freeStack(d);
mjERROR("constant metric factor is rank-deficient (%d of %d)", rank, nbd);
}
mj_freeStack(d);
}
void mj_setConst(mjModel* m, mjData* d) {
// compute npolygonmax and nmeshdegmax
m->npolygonmax = 0;
@@ -1354,6 +1515,9 @@ void mj_setConst(mjModel* m, mjData* d) {
// set quantities that depend qpos_spring
setSpring(m, d);
// precompute the constant part of the implicit effective metric factor
setEfm0Factor(m, d);
}
src/engine/engine_solver.c
+44 -6
View File
@@ -24,6 +24,7 @@
#include <mujoco/mjsan.h> // IWYU pragma: keep
#include "engine/engine_core_constraint.h"
#include "engine/engine_core_smooth.h"
#include "engine/engine_derivative.h"
#include "engine/engine_core_util.h"
#include "engine/engine_memory.h"
#include "engine/engine_macro.h"
@@ -1068,6 +1069,13 @@ typedef struct {
mjtNum* L; // Cholesky factor (nL x 1)
mjtNum* Lcone; // Cholesky factor with cone contributions (nL x 1)
// implicit effective metric for flex: Mtilde = M + K (built per step in mj_fwdPosition): when
// active, ctx.qfrc_smooth is pre-shifted by +c and Ma/Mv carry the B term, so the stock
// objective/gradient/linesearch formulas below operate in the Mtilde metric unchanged
int flg_flex; // effective metric active for this solve
const mjModel* fm; // model, for the metric calls
mjData* fd; // data, for the metric calls
// globals
mjtNum cost; // constraint + Gauss cost
mjtNum quadGauss[3]; // quadratic polynomial for Gauss cost
@@ -1331,10 +1339,29 @@ static void PrimalAllocate(const mjModel* m, mjData* d, mjPrimalContext* ctx, in
// sparse: compute Jacobian transpose
if (is_sparse) {
int offset = ctx->J_rowadr[0];
mju_transposeSparse(ctx->JT, ctx->J + offset, nefc, nv,
ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper,
ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind + offset);
if (nefc) {
int offset = ctx->J_rowadr[0];
mju_transposeSparse(ctx->JT, ctx->J + offset, nefc, nv,
ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper,
ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind + offset);
} else {
// no constraints (reachable via the flex CG dispatch): valid empty transpose structures
mju_zeroInt(ctx->JT_rownnz, nv);
mju_zeroInt(ctx->JT_rowadr, nv);
mju_zeroInt(ctx->JT_rowsuper, nv);
}
}
// implicit effective metric (built in mj_fwdPosition): route Ma/Mv/Mgrad through the
// metric operators and shift the smooth force, so the stock objective/gradient/linesearch
// formulas operate in the Mtilde = M+K metric
if (ctx->island < 0 && !is_elliptic && !flg_Newton && d->efm_active) {
ctx->flg_flex = 1;
ctx->fm = m;
ctx->fd = d;
mjtNum* qfrc_eff = mjSTACKALLOC(d, nv, mjtNum);
mju_add(qfrc_eff, ctx->qfrc_smooth, d->efm_c, nv);
ctx->qfrc_smooth = qfrc_eff;
}
}
@@ -1399,6 +1426,11 @@ 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);
}
// CG: Mgrad = M \ grad
else {
mju_copy(ctx->Mgrad, ctx->grad, nv);
@@ -1843,9 +1875,12 @@ static mjtNum PrimalSearch(mjPrimalContext* ctx, mjtNum tolerance, mjtNum ls_ite
mjtNum gtol = tolerance * snorm / ctx->scale;
mjtNum slopescl = ctx->scale / snorm;
// compute Mv = M * v
// compute Mv = Mtilde * v
mju_mulSymVecSparse(ctx->Mv, ctx->M, ctx->search, nv,
ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind);
if (ctx->flg_flex) {
mjd_effMulAdd(ctx->fm, ctx->fd, ctx->Mv, ctx->search);
}
// compute Jv = J * search (dense or sparse)
if (!ctx->is_sparse) {
@@ -2321,9 +2356,12 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i
int nefc = ctx.nefc;
int* oldstate = ctx.oldstate;
// compute Ma = M * qacc
// compute Ma = Mtilde * qacc
mju_mulSymVecSparse(ctx.Ma, ctx.M, ctx.qacc, nv,
ctx.M_rownnz, ctx.M_rowadr, ctx.M_colind);
if (ctx.flg_flex) {
mjd_effMulAdd(m, d, ctx.Ma, ctx.qacc);
}
// compute Jaref = J * qacc - aref (dense or sparse)
src/engine/engine_util_solve.c
+16 -6
View File
@@ -192,16 +192,19 @@ int mju_cholFactorSymbolic(int* restrict L_colind, int* restrict L_rownnz, int*
int* restrict LT_rowadr, int* restrict LT_map,
const int* rownnz, const int* rowadr, const int* colind, int n,
mjData* d) {
mj_markStack(d);
int* restrict parent = mjSTACKALLOC(d, n, int);
int* restrict flag = mjSTACKALLOC(d, n, int);
// d supplies stack scratch; if NULL, scratch is heap-allocated
if (d) {
mj_markStack(d);
}
int* restrict parent = d ? mjSTACKALLOC(d, n, int) : (int*) mju_malloc(sizeof(int)*n);
int* restrict flag = d ? mjSTACKALLOC(d, n, int) : (int*) mju_malloc(sizeof(int)*n);
int* restrict cursor = NULL;
int* LT_write = NULL;
// filling phase: initialize write positions
if (L_colind) {
cursor = mjSTACKALLOC(d, n, int);
LT_write = mjSTACKALLOC(d, n, int);
cursor = d ? mjSTACKALLOC(d, n, int) : (int*) mju_malloc(sizeof(int)*n);
LT_write = d ? mjSTACKALLOC(d, n, int) : (int*) mju_malloc(sizeof(int)*n);
for (int r = 0; r < n; r++) {
cursor[r] = L_rowadr[r] + L_rownnz[r] - 2; // end of row r (before diagonal)
LT_write[r] = LT_rowadr[r]; // start of LT row r
@@ -270,7 +273,14 @@ int mju_cholFactorSymbolic(int* restrict L_colind, int* restrict L_rownnz, int*
}
}
mj_freeStack(d);
if (d) {
mj_freeStack(d);
} else {
mju_free(parent);
mju_free(flag);
mju_free(cursor);
mju_free(LT_write);
}
// counting phase: compute row addresses, add up total non-zeros
int nnz = 0;
src/engine/engine_util_solve.h
+1
View File
@@ -41,6 +41,7 @@ MJAPI int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
// if L_colind is NULL, perform counting logic (fill rownnz/rowadr arrays and return total nnz)
// if L_colind is not NULL, assume rownnz/rowadr are precomputed and fill colind/map arrays
// reads pattern from upper triangle
// d may be NULL: scratch is then heap-allocated
// based on ldl_symbolic from 'Algorithm 8xx: a concise sparse Cholesky factorization package'
MJAPI int mju_cholFactorSymbolic(int* L_colind, int* L_rownnz, int* L_rowadr,
int* LT_colind, int* LT_rownnz, int* LT_rowadr, int* LT_map,
src/user/user_model.cc
+83 -1
View File
@@ -27,6 +27,7 @@
#include <filesystem> // NOLINT(build/c++17)
#include <functional>
#include <mutex>
#include <set>
#include <sstream>
#include <string>
#include <string_view>
@@ -52,6 +53,7 @@
#include "engine/engine_setconst.h"
#include "engine/engine_support.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_solve.h"
#include "engine/engine_util_misc.h"
#include "user/user_api.h"
#include "user/user_objects.h"
@@ -1212,6 +1214,8 @@ void mjCModel::Clear() {
nflexelemdata = 0;
nflexstiffness = 0;
nflexbending = 0;
nefm0dof = 0;
nefm0L = 0;
nflexelemedge = 0;
nflexshelldata = 0;
nflexevpair = 0;
@@ -2245,6 +2249,83 @@ void mjCModel::SetSizes() {
continue;
}
// bending factor sizes: symbolic reverse-Cholesky count on the (M + K_bend) pattern.
// Bending couples only same-coordinate dofs of unpinned flap vertices, so the pattern is
// three interleaved copies of the vertex flap adjacency. The count must match the symbolic
// factorization performed in mj_setConst (asserted there).
if (flexes_[i]->dim == 2 && !flexes_[i]->bending.empty()) {
const mjCFlex* fl = flexes_[i];
int nvrt = fl->nvert;
// unpinned vertices -> compact slots (vertex enumeration order)
std::vector<int> slot(nvrt, -1);
int nfree = 0;
for (int v=0; v < nvrt; v++) {
if (!bodies_[fl->vertbodyid[v]]->joints.empty()) {
slot[v] = nfree++;
}
}
if (!nfree) {
continue;
}
// vertex adjacency from 4-vertex flap stencils (self excluded; diagonal is implicit)
std::vector<std::set<int>> adj(nfree);
for (const auto& flap : fl->flaps) {
if (flap.vertices[3] < 0) {
continue;
}
for (int a=0; a < 4; a++) {
int sa = slot[flap.vertices[a]];
if (sa < 0) continue;
for (int b=0; b < 4; b++) {
int sb = slot[flap.vertices[b]];
if (sb >= 0 && sb != sa) {
adj[sa].insert(sb);
}
}
}
}
// dof-level upper-triangle pattern: row 3*s+k has columns {3*t+k : t > s, t in adj(s)}
int n = 3*nfree;
std::vector<std::vector<int>> upper(n);
for (int s=0; s < nfree; s++) {
for (int t : adj[s]) {
if (t > s) {
for (int k=0; k < 3; k++) {
upper[3*s+k].push_back(3*t+k);
}
}
}
}
for (auto& row : upper) {
std::sort(row.begin(), row.end());
}
// flatten the pattern to CSR and count fill with the engine's symbolic factorization
// (d == NULL: no mjData exists yet, scratch is heap-allocated)
std::vector<int> u_rownnz(n), u_rowadr(n), u_colind;
int u_nnz = 0;
for (int r=0; r < n; r++) {
u_nnz += (int)upper[r].size();
}
u_colind.reserve(u_nnz);
for (int r=0; r < n; r++) {
u_rownnz[r] = (int)upper[r].size();
u_rowadr[r] = (int)u_colind.size();
u_colind.insert(u_colind.end(), upper[r].begin(), upper[r].end());
}
std::vector<int> L_rownnz(n), L_rowadr(n), LT_rownnz(n), LT_rowadr(n);
mjtSize nnz = mju_cholFactorSymbolic(NULL, L_rownnz.data(), L_rowadr.data(),
NULL, LT_rownnz.data(), LT_rowadr.data(), NULL,
u_rownnz.data(), u_rowadr.data(), u_colind.data(),
n, NULL);
nefm0dof += n;
nefm0L += nnz;
}
// count number of non-zero elements in the edge Jacobian matrix
for (const auto& edge : flexes_[i]->edge) {
mjCBody* b1 = bodies_[flexes_[i]->vertbodyid[edge.first]];
@@ -5279,7 +5360,8 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
nq, nv, nu, nactuator, nout, na,
nbody, nbvh, nbvhstatic, nbvhdynamic, noct, njnt, ntree, nM, nB, nC,
nD, ngeom, nsite, ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem,
nflexelemdata, nflexstiffness, nflexbending, nflexelemedge, nflexshelldata,
nflexelemdata, nflexstiffness, nflexbending, nefm0dof, nefm0L,
nflexelemedge, nflexshelldata,
nflexevpair, nflextexcoord, nJfe, nJfv, nmesh, nmeshvert, nmeshnormal, nmeshtexcoord,
nmeshface, nmeshgraph, nmeshpoly, nmeshpolyvert, nmeshpolymap, nskin, nskinvert,
nskintexvert, nskinface, nskinbone, nskinbonevert, nhfield, nhfielddata, ntex,
src/user/user_model.h
+2
View File
@@ -100,6 +100,8 @@ class mjCModel_ : public mjsElement {
mjtSize nflexelemdata; // number of element vertex ids in all flexes
mjtSize nflexstiffness; // number of stiffness parameters in all flexes
mjtSize nflexbending; // number of bending parameters in all flexes
mjtSize nefm0dof; // number of dofs covered by the bending factor
mjtSize nefm0L; // number of non-zeros in the bending factor
mjtSize nflexelemedge; // number of element edges in all flexes
mjtSize nflexshelldata; // number of shell fragment vertex ids in all flexes
mjtSize nflexevpair; // number of element-vertex pairs in all flexes
test/engine/engine_derivative_test.cc
+372
View File
@@ -30,6 +30,7 @@
#include "src/engine/engine_forward.h"
#include "src/engine/engine_io.h"
#include "src/engine/engine_util_blas.h"
#include "src/engine/engine_util_sparse.h"
#include "test/fixture.h"
namespace mujoco {
@@ -1771,5 +1772,376 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
<< "Jacobian approximation should differ from FD when deformed";
}
// Helper: assemble the standard-flex stretch stiffness into a dense matrix,
// column-by-column using mjd_flexStretch_mul with scale (s1 + s2*damping).
static void stretchK_dense(mjModel* m, mjData* d, mjtNum* K, int nv,
mjtNum s1, mjtNum s2) {
std::vector<mjtNum> e_i(nv, 0);
std::vector<mjtNum> col(nv, 0);
for (int i = 0; i < nv; i++) {
mju_zero(e_i.data(), nv);
mju_zero(col.data(), nv);
e_i[i] = 1.0;
mjd_flexStretch_mul(m, d, col.data(), e_i.data(), s1, s2);
for (int j = 0; j < nv; j++) {
K[j * nv + i] = col[j];
}
}
}
// verify mjd_flexStretch_mul (Gauss-Newton Hessian of the standard-flex
// stretch force) against finite differences of qfrc_passive, plus symmetry,
// positive semi-definiteness and (s1, s2) scale linearity. The model covers
// both element edge tables (dim=2 triangles and dim=3 tets) and a pinned
// vertex (zero-dof body guard).
TEST_F(DerivativeTest, FlexStretchDerivatives) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit"/>
<worldbody>
<flexcomp name="cloth" type="grid" count="4 4 1" spacing="0.1 0.1 0.1"
radius=".01" dim="2" mass="1" pos="0 0 1">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" thickness="0.01"
elastic2d="stretch" damping="50"/>
<pin id="0"/>
</flexcomp>
<flexcomp name="solid" type="grid" count="3 3 3" spacing="0.1 0.1 0.1"
radius=".01" dim="3" mass="1" pos="1 0 1">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" damping="10"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
ASSERT_EQ(model->nq, nv); // all slide dofs
MjDataPtr data = MakeData(model);
// deform both flexes deterministically. Keep the strain small: the operator
// is the Gauss-Newton Hessian, exact to O(strain) (the geometric term is
// dropped, see FlexInterpDerivativesDeformed for the analogous property).
for (int i = 0; i < nv; i++) {
data->qpos[i] += 5e-4 * (mju_Halton(i, 2) - 0.5);
}
mj_forward(model.get(), data.get());
// part 1: FD verification of K*vec against qfrc_passive (qvel = 0, so the
// kD elongation term vanishes and qfrc_passive is the pure stretch spring)
{
std::vector<mjtNum> vec(nv), res(nv, 0);
for (int i = 0; i < nv; i++) {
vec[i] = mju_Halton(i, 2) - 0.5;
}
mjd_flexStretch_mul(model.get(), data.get(), res.data(), vec.data(), 1, 0);
mjtNum eps = MjTol(1e-7, 1e-4);
mjData* data_perturbed = mj_copyData(NULL, model.get(), data.get());
mju_addToScl(data_perturbed->qpos, vec.data(), eps, nv);
mj_forward(model.get(), data_perturbed);
// qfrc_passive = -dV/dq => -(qfrc_new - qfrc)/eps ~= K * vec.
// Compare max error against the force scale: the operator omits the
// geometric (stress-proportional) term, so the residual is O(strain) of
// the overall scale and individual near-zero entries are not meaningful.
mjtNum max_err = 0, scale = 0;
for (int i = 0; i < nv; ++i) {
mjtNum fd =
-(data_perturbed->qfrc_passive[i] - data->qfrc_passive[i]) / eps;
max_err = mju_max(max_err, mju_abs(res[i] - fd));
scale = mju_max(scale, mju_abs(fd));
}
EXPECT_GT(scale, 1.0) << "test should exercise nontrivial stiffness";
EXPECT_LT(max_err, MjTol(5e-3, 5e-2) * scale)
<< "stretch stiffness mismatch: max_err " << max_err
<< " at force scale " << scale;
mj_deleteData(data_perturbed);
}
// part 2: symmetry and positive semi-definiteness of the assembled K
{
std::vector<mjtNum> K(nv * nv, 0);
stretchK_dense(model.get(), data.get(), K.data(), nv, 1, 0);
mjtNum max_asymmetry = 0;
for (int i = 0; i < nv; i++) {
for (int j = 0; j < i; j++) {
max_asymmetry =
mju_max(max_asymmetry, mju_abs(K[i * nv + j] - K[j * nv + i]));
}
}
EXPECT_THAT(max_asymmetry, MjNear(0, 1e-10, 5e-4))
<< "K_stretch is not symmetric";
for (int trial = 0; trial < 5; trial++) {
std::vector<mjtNum> v(nv);
for (int i = 0; i < nv; i++) {
v[i] = mju_Halton(i + trial * nv, 3) - 0.5;
}
mjtNum vKv = 0;
for (int i = 0; i < nv; i++) {
for (int j = 0; j < nv; j++) {
vKv += v[i] * K[i * nv + j] * v[j];
}
}
EXPECT_GE(vKv, MjTol(-1e-8, -1e-5)) << "K_stretch is not PSD";
}
}
// part 3: (s1, s2) scale linearity across flexes with different damping:
// mul(s1, s2) == s1*mul(1, 0) + s2*mul(0, 1)
{
std::vector<mjtNum> vec(nv), a(nv, 0), b(nv, 0), c(nv, 0);
for (int i = 0; i < nv; i++) {
vec[i] = mju_Halton(i, 5) - 0.5;
}
mjtNum h = 1e-3;
mjd_flexStretch_mul(model.get(), data.get(), a.data(), vec.data(),
h * h, h);
mjd_flexStretch_mul(model.get(), data.get(), b.data(), vec.data(), 1, 0);
mjd_flexStretch_mul(model.get(), data.get(), c.data(), vec.data(), 0, 1);
for (int i = 0; i < nv; i++) {
EXPECT_THAT(a[i], MjNear(h * h * b[i] + h * c[i], 1e-12, 1e-5))
<< "scale linearity mismatch at DOF " << i;
}
}
}
// verify mjd_flexStiff_assemble against the matrix-free operators: the
// assembled CSR applied to test vectors must reproduce mjd_flexBend_mul +
// mjd_flexStretch_mul at the same state
TEST_F(DerivativeTest, FlexStiffAssemble) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit"/>
<worldbody>
<flexcomp name="cloth" type="grid" count="4 4 1" spacing="0.1 0.1 0.1"
radius=".01" dim="2" mass="1" pos="0 0 1">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" thickness="0.01"
elastic2d="both" damping="7"/>
</flexcomp>
<flexcomp name="solid" type="grid" count="3 3 3" spacing="0.1 0.1 0.1"
radius=".01" dim="3" mass="1" pos="1 0 1">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" damping="10"/>
<pin id="0"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
MjDataPtr data = MakeData(model);
// deform deterministically
for (int i = 0; i < nv; i++) {
data->qpos[i] += 2e-3 * (mju_Halton(i, 2) - 0.5);
}
mj_forward(model.get(), data.get());
// assemble both terms with a mixed (s1, s2) scale
mjtNum s1 = 4e-6, s2 = 2e-3;
std::vector<int> rownnz(nv), rowadr(nv);
int nnz = mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(),
rowadr.data(), NULL, NULL, s1, s2,
/*flg_bend=*/1, /*flg_stretch=*/1, NULL);
ASSERT_GT(nnz, 0);
std::vector<int> colind(nnz);
std::vector<mjtNum> val(nnz);
mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(),
colind.data(), val.data(), s1, s2, /*flg_bend=*/1,
/*flg_stretch=*/1, NULL);
// compare CSR apply vs operators on test vectors
for (int trial = 0; trial < 3; trial++) {
std::vector<mjtNum> vec(nv), res_op(nv, 0), res_csr(nv, 0);
for (int i = 0; i < nv; i++) {
vec[i] = mju_Halton(i + trial*nv, 3) - 0.5;
}
mjd_flexBend_mul(model.get(), data.get(), res_op.data(), vec.data(), s1,
s2);
mjd_flexStretch_mul(model.get(), data.get(), res_op.data(), vec.data(), s1,
s2);
for (int i = 0; i < nv; i++) {
mjtNum sum = 0;
for (int k = 0; k < rownnz[i]; k++) {
sum += val[rowadr[i] + k]*vec[colind[rowadr[i] + k]];
}
res_csr[i] = sum;
}
for (int i = 0; i < nv; i++) {
EXPECT_THAT(res_csr[i], MjNear(res_op[i], 1e-12, 2e-5))
<< "assembly/operator mismatch at DOF " << i << " trial " << trial;
}
}
}
// verify the interp assembly mode: with the K_rot cache supplied, the assembled CSR applied
// to test vectors must reproduce mjd_flexInterp_mul, whose sign convention is negated
TEST_F(DerivativeTest, FlexStiffAssembleInterp) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit"/>
<worldbody>
<flexcomp name="soft" type="grid" count="4 4 4" spacing="0.1 0.1 0.1"
radius=".01" dim="3" mass="1" pos="0 0 1" dof="trilinear">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" damping="2"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
MjDataPtr data = MakeData(model);
ASSERT_EQ(mjd_flexInterpAssemblable(model.get()), 1);
// deform deterministically, refresh kinematics, cache the corotated stiffness
for (int i = 0; i < nv; i++) {
data->qpos[i] += 2e-3 * (mju_Halton(i, 2) - 0.5);
}
mj_forward(model.get(), data.get());
std::vector<mjtNum> krot(model->nflexstiffness, 0);
mjd_flexInterp_cacheKrot(model.get(), data.get(), krot.data());
// assemble interp only
mjtNum s1 = 4e-6, s2 = 2e-3;
std::vector<int> rownnz(nv), rowadr(nv);
int nnz = mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(),
NULL, NULL, s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0,
krot.data());
ASSERT_GT(nnz, 0);
std::vector<int> colind(nnz);
std::vector<mjtNum> val(nnz);
mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(),
colind.data(), val.data(), s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0,
krot.data());
// compare CSR apply vs the operator called with negated scales (its convention)
for (int trial = 0; trial < 3; trial++) {
std::vector<mjtNum> vec(nv), res_op(nv, 0), res_csr(nv, 0);
for (int i = 0; i < nv; i++) {
vec[i] = mju_Halton(i + trial*nv, 3) - 0.5;
}
mjd_flexInterp_mul(model.get(), data.get(), res_op.data(), vec.data(), -s1, -s2,
krot.data());
for (int i = 0; i < nv; i++) {
mjtNum sum = 0;
for (int k = 0; k < rownnz[i]; k++) {
sum += val[rowadr[i] + k]*vec[colind[rowadr[i] + k]];
}
res_csr[i] = sum;
}
for (int i = 0; i < nv; i++) {
EXPECT_THAT(res_csr[i], MjNear(res_op[i], 1e-12, 2e-5))
<< "interp assembly/operator mismatch at DOF " << i << " trial " << trial;
}
}
}
// 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) {
// relative residual of (M+K)x - b after mjd_effSolve
auto solve_residual = [](const mjModel* m, mjData* d) {
int nv = m->nv;
std::vector<mjtNum> b(nv), x(nv), r(nv);
for (int i = 0; i < nv; i++) {
b[i] = mju_Halton(i, 3) - 0.5;
}
mjd_effSolve(m, d, x.data(), b.data());
mju_mulSymVecSparse(r.data(), d->M, x.data(), nv, m->M_rownnz, m->M_rowadr, m->M_colind);
mjd_effMulAdd(m, d, r.data(), x.data());
mju_subFrom(r.data(), b.data(), nv);
return mju_norm(r.data(), nv) / mju_norm(b.data(), nv);
};
// stretch + bending cloth on world: per-step factor, exact
static const char* const kXmlBoth = R"(
<mujoco>
<option solver="CG" integrator="implicitfast"/>
<worldbody>
<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 1" dof="full">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e3" poisson="0.2" damping="0.1" elastic2d="both" thickness="0.01"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXmlBoth, 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);
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));
// bending-only cloth: no CSR or per-step factor, constant factor covers, exact
static const char* const kXmlBend = R"(
<mujoco>
<option solver="CG" integrator="implicitfast"/>
<worldbody>
<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 1" dof="full">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e3" poisson="0.2" damping="0.1" elastic2d="bend" thickness="0.01"/>
</flexcomp>
</worldbody>
</mujoco>
)";
model = LoadModelFromString(kXmlBend, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
data = MakeData(model);
mj_forward(model.get(), data.get());
ASSERT_GE(data->efm_active, 1);
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));
// cloth under a jointed parent: M couples across the covered block, not exact,
// the refinement path must still meet its tolerance
static const char* const kXmlMoving = 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>
)";
model = LoadModelFromString(kXmlMoving, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
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);
}
} // namespace
} // namespace mujoco
test/engine/engine_forward_test.cc
+51 -6
View File
@@ -2933,7 +2933,7 @@ TEST_F(ForwardTest, FlexTrilinearInstability) {
TEST_F(ForwardTest, FlexDampingRigidMotion) {
constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0" timestep="0.01" integrator="implicitfast"/>
<option gravity="0 0 0" timestep="0.01" integrator="implicitfast" solver="CG"/>
<worldbody>
<flexcomp name="flex" type="grid" count="3 3 3" spacing="0.1 0.1 0.1"
pos="0 0 0" euler="45 45 45" radius="0.01" dim="3" mass="1" dof="trilinear">
@@ -2984,7 +2984,7 @@ TEST_F(ForwardTest, FlexDampingRigidMotion) {
TEST_F(ForwardTest, FlexParentCoupling) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit" timestep="0.01"/>
<option integrator="implicit" timestep="0.01" solver="CG"/>
<worldbody>
<body name="parent" pos="0 0 0">
<freejoint/>
@@ -3034,14 +3034,17 @@ TEST_F(ForwardTest, FlexParentCoupling) {
if (diff > max_diff) max_diff = diff;
}
EXPECT_LT(max_diff, MjTol(2e-5, 1.5e-2))
// tolerance rebaselined 2e-5 -> 5e-5 for the in-solver implicit flex treatment: implicit
// and explicit flex damping legitimately differ at O(h*damping*K/M) in this comparison, and
// the in-solver form lands at ~3e-5 where the old post-hoc operator landed just under 2e-5
EXPECT_LT(max_diff, MjTol(5e-5, 1.5e-2))
<< "Implicit integrator should match Euler at small timestep";
}
TEST_F(ForwardTest, TrilinearPinnedParentWithFreejoint) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast"/>
<option integrator="implicitfast" solver="CG"/>
<worldbody>
<body>
<joint type="free"/>
@@ -3601,7 +3604,7 @@ TEST_F(ImplicitIntegratorTest, FlexContactEnergy) {
TEST_F(ImplicitIntegratorTest, BendingDampingDecaysEnergy) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0" timestep="0.001" integrator="implicitfast">
<option gravity="0 0 0" timestep="0.001" integrator="implicitfast" solver="CG">
<flag energy="enable"/>
</option>
<worldbody>
@@ -3661,7 +3664,7 @@ TEST_F(ImplicitIntegratorTest, BendingDampingDecaysEnergy) {
TEST_F(ImplicitIntegratorTest, InterpStretchEnergy) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0" timestep="0.001" integrator="implicitfast">
<option gravity="0 0 0" timestep="0.001" integrator="implicitfast" solver="CG">
<flag energy="enable"/>
</option>
<worldbody>
@@ -3707,4 +3710,46 @@ TEST_F(ImplicitIntegratorTest, InterpStretchEnergy) {
}
} // namespace
// with the implicit effective metric active, inverse dynamics must recover the applied force
// (zero here): the forward solve is (M+B)*qacc = qfrc_smooth + c + J'*f and the inverse adds
// the same B*qacc - c terms. This is the fwd/inv consistency fence for the flex-CG dispatch.
TEST_F(ForwardTest, GatedFlexInverseConsistency) {
static const char* const kXml = R"(
<mujoco>
<option solver="CG" integrator="implicitfast" tolerance="1e-14"/>
<worldbody>
<flexcomp name="cloth" type="grid" count="6 6 1" spacing="0.1 0.1 0.1"
radius=".01" dim="2" mass="1" pos="0 0 1">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" thickness="0.01"
elastic2d="both" damping="0.5"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
int nv = model->nv;
// deform and settle a few steps under gravity
for (int i=0; i < nv; i++) {
data->qvel[i] = 0.1 * (mju_Halton(i, 3) - 0.5);
}
for (int step=0; step < 50; step++) {
mj_step(model.get(), data.get());
}
// forward then inverse at the same state
mj_forward(model.get(), data.get());
mj_inverse(model.get(), data.get());
// no applied forces: the inverse must return ~zero, at the scale of the passive forces
mjtNum scale = 1 + mju_norm(data->qfrc_passive, nv);
EXPECT_LT(mju_norm(data->qfrc_inverse, nv), 1e-6 * scale);
}
} // namespace mujoco
test/engine/engine_island_test.cc
+33
View File
@@ -373,6 +373,39 @@ TEST_F(IslandTest, IslandFlex) {
mj_deleteModel(model);
}
// stiffness couples all vertices of a flex: one contact anywhere on the flex
// must pull every vertex tree (and the contacting body) into a single island
TEST_F(IslandTest, FlexStiffnessUnionsTrees) {
static const char xml[] = R"(
<mujoco>
<option solver="Newton"/>
<worldbody>
<flexcomp name="cloth" type="grid" count="4 4 1" spacing="0.1 0.1 0.1"
radius=".005" dim="2" mass="0.5" pos="0 0 1" dof="full">
<contact selfcollide="none"/>
<elasticity young="1e3" poisson="0.2" damping="0.1" elastic2d="both" thickness="0.01"/>
</flexcomp>
<body pos="0.1 0.1 0.96">
<freejoint/>
<geom type="sphere" size="0.05"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
// the sphere penetrates the cloth at one corner
ASSERT_GT(data->ncon, 0);
// one island containing every dof: 16 vertices and the free sphere
EXPECT_EQ(data->nisland, 1);
EXPECT_EQ(data->nidof, model->nv);
}
TEST_F(IslandTest, IslandEfcElliptic) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
char error[1024];
unity/Runtime/Bindings/MjBindings.cs
+22
View File
@@ -1047,6 +1047,8 @@ public unsafe struct mjModel_ {
public UInt64 nflexelemdata;
public UInt64 nflexstiffness;
public UInt64 nflexbending;
public UInt64 nefm0dof;
public UInt64 nefm0L;
public UInt64 nflexelemedge;
public UInt64 nflexshelldata;
public UInt64 nflexevpair;
@@ -1328,6 +1330,11 @@ public unsafe struct mjModel_ {
public double* flex_size;
public double* flex_stiffness;
public double* flex_bending;
public int* efm0_dofid;
public int* efm0_L_rownnz;
public int* efm0_L_rowadr;
public int* efm0_L_colind;
public double* efm0_L;
public double* flex_damping;
public double* flex_edgestiffness;
public double* flex_edgedamping;
@@ -5695,6 +5702,10 @@ public unsafe struct mjData_ {
public int nl;
public int nefc;
public int nJ;
public int efm_active;
public int nefmK;
public int nefmdof;
public int nefmL;
public int nY;
public int nA;
public int nisland;
@@ -5750,6 +5761,7 @@ public unsafe struct mjData_ {
public double* cinert;
public double* flexvert_xpos;
public double* flexelem_aabb;
public double* flexelem_krot;
public double* flexedge_J;
public double* flexedge_length;
public double* flexvert_J;
@@ -5853,6 +5865,16 @@ public unsafe struct mjData_ {
public double* efc_AR;
public double* efc_vel;
public double* efc_aref;
public double* efm_c;
public int* efm_K_rownnz;
public int* efm_K_rowadr;
public int* efm_K_colind;
public double* efm_K_val;
public int* efm_dofid;
public int* efm_L_rownnz;
public int* efm_L_rowadr;
public int* efm_L_colind;
public double* efm_L;
public double* efc_b;
public double* iefc_aref;
public int* iefc_state;
wasm/codegen/generated/bindings.cc
+22
View File
@@ -4583,12 +4583,24 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("efc_state", &MjData::efc_state)
.property("efc_type", &MjData::efc_type)
.property("efc_vel", &MjData::efc_vel)
.property("efm_K_colind", &MjData::efm_K_colind)
.property("efm_K_rowadr", &MjData::efm_K_rowadr)
.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)
.property("energy", &MjData::energy)
.property("eq_active", &MjData::eq_active)
.property("flexedge_J", &MjData::flexedge_J)
.property("flexedge_length", &MjData::flexedge_length)
.property("flexedge_velocity", &MjData::flexedge_velocity)
.property("flexelem_aabb", &MjData::flexelem_aabb)
.property("flexelem_krot", &MjData::flexelem_krot)
.property("flexvert_J", &MjData::flexvert_J)
.property("flexvert_length", &MjData::flexvert_length)
.property("flexvert_xpos", &MjData::flexvert_xpos)
@@ -4645,6 +4657,9 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("ncon", &MjData::ncon, &MjData::set_ncon, reference())
.property("ne", &MjData::ne, &MjData::set_ne, reference())
.property("nefc", &MjData::nefc, &MjData::set_nefc, reference())
.property("nefmK", &MjData::nefmK, &MjData::set_nefmK, reference())
.property("nefmL", &MjData::nefmL, &MjData::set_nefmL, reference())
.property("nefmdof", &MjData::nefmdof, &MjData::set_nefmdof, reference())
.property("nf", &MjData::nf, &MjData::set_nf, reference())
.property("nidof", &MjData::nidof, &MjData::set_nidof, reference())
.property("nisland", &MjData::nisland, &MjData::set_nisland, reference())
@@ -4875,6 +4890,11 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("dof_solimp", &MjModel::dof_solimp)
.property("dof_solref", &MjModel::dof_solref)
.property("dof_treeid", &MjModel::dof_treeid)
.property("efm0_L", &MjModel::efm0_L)
.property("efm0_L_colind", &MjModel::efm0_L_colind)
.property("efm0_L_rowadr", &MjModel::efm0_L_rowadr)
.property("efm0_L_rownnz", &MjModel::efm0_L_rownnz)
.property("efm0_dofid", &MjModel::efm0_dofid)
.property("eq_active0", &MjModel::eq_active0)
.property("eq_data", &MjModel::eq_data)
.property("eq_obj1id", &MjModel::eq_obj1id)
@@ -5129,6 +5149,8 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("nbvhstatic", &MjModel::nbvhstatic, &MjModel::set_nbvhstatic, reference())
.property("ncam", &MjModel::ncam, &MjModel::set_ncam, reference())
.property("nconmax", &MjModel::nconmax, &MjModel::set_nconmax, reference())
.property("nefm0L", &MjModel::nefm0L, &MjModel::set_nefm0L, reference())
.property("nefm0dof", &MjModel::nefm0dof, &MjModel::set_nefm0dof, reference())
.property("nemax", &MjModel::nemax, &MjModel::set_nemax, reference())
.property("neq", &MjModel::neq, &MjModel::set_neq, reference())
.property("nexclude", &MjModel::nexclude, &MjModel::set_nexclude, reference())
wasm/codegen/generated/bindings.h
+84
View File
@@ -3879,6 +3879,18 @@ struct MjModel {
void set_nflexbending(int value) {
ptr_->nflexbending = static_cast<mjtSize>(value);
}
int nefm0dof() const {
return static_cast<int>(ptr_->nefm0dof);
}
void set_nefm0dof(int value) {
ptr_->nefm0dof = static_cast<mjtSize>(value);
}
int nefm0L() const {
return static_cast<int>(ptr_->nefm0L);
}
void set_nefm0L(int value) {
ptr_->nefm0L = static_cast<mjtSize>(value);
}
int nflexelemedge() const {
return static_cast<int>(ptr_->nflexelemedge);
}
@@ -4914,6 +4926,21 @@ struct MjModel {
emscripten::val flex_bending() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nflexbending, ptr_->flex_bending));
}
emscripten::val efm0_dofid() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefm0dof, ptr_->efm0_dofid));
}
emscripten::val efm0_L_rownnz() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefm0dof, ptr_->efm0_L_rownnz));
}
emscripten::val efm0_L_rowadr() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefm0dof, ptr_->efm0_L_rowadr));
}
emscripten::val efm0_L_colind() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefm0L, ptr_->efm0_L_colind));
}
emscripten::val efm0_L() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefm0L, ptr_->efm0_L));
}
emscripten::val flex_damping() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_damping));
}
@@ -6656,6 +6683,30 @@ struct MjData {
void set_nJ(int value) {
ptr_->nJ = value;
}
int efm_active() const {
return ptr_->efm_active;
}
void set_efm_active(int value) {
ptr_->efm_active = value;
}
int nefmK() const {
return ptr_->nefmK;
}
void set_nefmK(int value) {
ptr_->nefmK = value;
}
int nefmdof() const {
return ptr_->nefmdof;
}
void set_nefmdof(int value) {
ptr_->nefmdof = value;
}
int nefmL() const {
return ptr_->nefmL;
}
void set_nefmL(int value) {
ptr_->nefmL = value;
}
int nY() const {
return ptr_->nY;
}
@@ -6860,6 +6911,9 @@ struct MjData {
emscripten::val flexelem_aabb() const {
return emscripten::val(emscripten::typed_memory_view(model->nflexelem * 6, ptr_->flexelem_aabb));
}
emscripten::val flexelem_krot() const {
return emscripten::val(emscripten::typed_memory_view(model->nflexstiffness, ptr_->flexelem_krot));
}
emscripten::val flexedge_J() const {
return emscripten::val(emscripten::typed_memory_view(model->nJfe, ptr_->flexedge_J));
}
@@ -7167,6 +7221,36 @@ struct MjData {
emscripten::val efc_aref() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefc, ptr_->efc_aref));
}
emscripten::val efm_c() const {
return emscripten::val(emscripten::typed_memory_view(model->nv, ptr_->efm_c));
}
emscripten::val efm_K_rownnz() const {
return emscripten::val(emscripten::typed_memory_view(model->nv, ptr_->efm_K_rownnz));
}
emscripten::val efm_K_rowadr() const {
return emscripten::val(emscripten::typed_memory_view(model->nv, ptr_->efm_K_rowadr));
}
emscripten::val efm_K_colind() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefmK, ptr_->efm_K_colind));
}
emscripten::val efm_K_val() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefmK, ptr_->efm_K_val));
}
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));
}
emscripten::val efc_b() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nefc, ptr_->efc_b));
}
wasm/codegen/generators/constants.py
+7
View File
@@ -339,6 +339,13 @@ MJDATA_SIZES: tuple[str, ...] = (
"efc_state",
"efc_type",
"efc_vel",
"efm_K_colind",
"efm_K_val",
"efm_dofid",
"efm_L",
"efm_L_colind",
"efm_L_rowadr",
"efm_L_rownnz",
"iLDiagInv",
"iM_rowadr",
"iM_rownnz",