Replace the banded Cholesky solver for implicit flex interpolation

with a preconditioned Conjugate Gradient (CG) solver that operates
on the full system matrix.

The previous approach extracted flex DOFs into a reduced banded system,
factored it separately, and overwrote the global solve. This required
precomputed bandwidth (makeFlexBandwidth), parent-joint detection,
coupling corrections, and a FlexInterpContext struct — and only worked
for standalone flex trees without parent joints.

The new CG solver uses the already-factored global system (M - h*qDeriv)
as a preconditioner and adds the flex stiffness contribution via
matrix-free products (mjd_flexInterp_mulKD/mulK). This handles any
kinematic configuration — including flexes attached to articulated
chains or with parent joints — without sparsity pattern restrictions.

Before (`bunny_multicell`):
```
 Simulation time      : 50.80 s
 Steps per second     : 197
 Realtime factor      : 0.20 x
 Time per step        : 5080.3 µs

 CG iters / step      : 3.16
 Contacts / step      : 31.04
 Constraints / step   : 124.15
 Degrees of freedom   : 178
 Dynamic memory usage : 0.4% of 100M
```

After:
```
 Simulation time      : 9.52 s
 Steps per second     : 1051
 Realtime factor      : 1.05 x
 Time per step        : 951.7 µs

 CG iters / step      : 3.21
 Contacts / step      : 30.90
 Constraints / step   : 123.61
 Degrees of freedom   : 178
 Dynamic memory usage : 0.3% of 100M
```

PiperOrigin-RevId: 913758038
Change-Id: If5aa617b2d535c86aec9bd71c9e0003a2b38bdd7
This commit is contained in:
Alessio Quaglino
2026-05-11 10:09:37 -07:00
committed by Copybara-Service
parent 5d818306ef
commit f9f1db1e0a
12 changed files with 152 additions and 422 deletions
+2 -2
View File
@@ -16,9 +16,9 @@
<mujoco model="Trilinear">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="Euler"/>
<option solver="CG" tolerance="1e-6" integrator="implicitfast"/>
<size memory="100M"/>
<size memory="10M"/>
<visual>
<map stiffness="100"/>