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

Use banded solver for implicit flex integration.

Browse Source 
The flex interpolation stiffness matrix within the implicit/implicitfast solvers is now built and factorized in a banded format instead of a dense one. This involves:
-   Calculating the bandwidth based on the sparsity of the mass/damping matrix and the connectivity within flex cells.
-   Allocating and populating a banded matrix `H`.
-   Using `mju_cholFactorBand` and `mju_cholSolveBand` for factorization and solving.
This change improves performance for flexes with many DOFs but local coupling.

PiperOrigin-RevId: 901297952
Change-Id: I3efe06353d1903ea65ab30dc49685cede228bb68
This commit is contained in:
Alessio Quaglino
2026-04-17 07:59:34 -07:00
committed by Copybara-Service
parent 6c7ed66781
commit b16383dfaf
13 changed files with 239 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_derivative_test.cc
+15 -5
View File
@@ -1473,6 +1473,17 @@ void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name,
}
}
// Helper: assemble flex stiffness into dense matrix via banded addH
// This wraps the banded API and converts to dense for test verification.
static void addH_dense(mjModel* m, mjData* d, mjtNum* H_dense,
const int* dof_indices, int ndof, mjtNum h) {
// use full bandwidth (ndof) for exact dense equivalence
std::vector<mjtNum> H_band(ndof * ndof, 0);
mjd_flexInterp_addH(m, d, H_band.data(), dof_indices, ndof, ndof, h);
// convert banded to dense (lower triangle), then symmetrize
mju_band2Dense(H_dense, H_band.data(), ndof, ndof, 0, 1);
}
// compare analytic and fin-diff d_qfrc_passive/d_qvel for flex interp
// Combined test for verify mjd_flexInterp_mulK (stiffness) and damping
TEST_F(DerivativeTest, FlexInterpDerivatives) {
@@ -1525,7 +1536,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
for (int i = 0; i < nv; i++) dof_indices[i] = i;
// assemble K into H
mjd_flexInterp_addH(model, data, H.data(), dof_indices.data(), nv, 1.0);
addH_dense(model, data, H.data(), dof_indices.data(), nv, 1.0);
// restore damping
model->flex_damping[0] = save_damping;
@@ -1618,10 +1629,10 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
for (int i = 0; i < nv; i++) dof_indices[i] = i;
vector<mjtNum> H1(nv * nv, 0);
mjd_flexInterp_addH(model, data, H1.data(), dof_indices.data(), nv, 1.0);
addH_dense(model, data, H1.data(), dof_indices.data(), nv, 1.0);
vector<mjtNum> H2(nv * nv, 0);
mjd_flexInterp_addH(model, data, H2.data(), dof_indices.data(), nv, 0.5);
addH_dense(model, data, H2.data(), dof_indices.data(), nv, 0.5);
vector<mjtNum> D(nv * nv);
for (int i = 0; i < nv * nv; i++) {
@@ -1695,8 +1706,7 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
for (int i = 0; i < nv; i++) dof_indices[i] = i;
// h=1, damping=0 => adds K to H
mjd_flexInterp_addH(model, data, H_approx.data(), dof_indices.data(), nv,
1.0);
addH_dense(model, data, H_approx.data(), dof_indices.data(), nv, 1.0);
// 2. Compute Finite Difference Jacobian (Ground Truth)
// qfrc_passive = -dV/dq