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
+6 -6
View File
@@ -1134,18 +1134,18 @@ void mjd_flexInterp_mulKD(const mjModel* m, mjData* d, mjtNum* res, const mjtNum
}
// add (h^2 + h*damping) * J'*K*J to banded matrix H, for all interpolated flexes
// H: banded ndof x nband matrix (lower triangle, band storage)
// dof_indices: maps local indices to global DOFs
void mjd_flexInterp_addH(const mjModel* m, mjData* d, mjtNum* H, const int* dof_indices,
int ndof, int nband, mjtNum h) {
mjd_flexInterp_kernel(m, d, mjFLEXOP_ADDH, H, NULL, h * h, h, dof_indices, ndof, nband);
// compute res += h * J'*K*J * vec, for all interpolated flexes (stiffness only, no damping)
void mjd_flexInterp_mulK(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h) {
// s1=h, s2=0 => scale = h (no damping contribution)
mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, h, 0, NULL, 0, 0);
}
// add (d qfrc_actuator / d qvel) to qDeriv
void mjd_actuator_vel(const mjModel* m, mjData* d) {
int nu = m->nu;