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
-1
View File
@@ -979,7 +979,6 @@ struct mjModel_ {
int* flex_matid; // material id for rendering (nflex x 1)
int* flex_group; // group for visibility (nflex x 1)
int* flex_interp; // interpolation (0: vertex, 1: nodes) (nflex x 1)
int* flex_bandwidth; // precomputed solver bandwidth (nflex x 1)
int* flex_cellnum; // finite cell num per dimension (nflex x 3)
int* flex_nodeadr; // first node address (nflex x 1)
int* flex_nodenum; // number of nodes (nflex x 1)
-1
View File
@@ -455,7 +455,6 @@
X ( int, flex_matid, nflex, 1 ) \
X ( int, flex_group, nflex, 1 ) \
X ( int, flex_interp, nflex, 1 ) \
X ( int, flex_bandwidth, nflex, 1 ) \
X ( int, flex_cellnum, nflex, 3 ) \
X ( int, flex_nodeadr, nflex, 1 ) \
X ( int, flex_nodenum, nflex, 1 ) \