Refactor sparse Cholesky factorization into symbolic and numeric phases.

The new symbolic function is a generalization of the function it replaces. In this CL it takes two unused temp arrays. The actual change in behavior happens in the followup.

New benchmark test output below ("L" is 2 humanoids and 100 free objects, "XL" is 100 humanoids). Note that `symbolic` is only ever called once per Newton iteration, while `numeric` is sometimes called multiple times (when the rank-1 update fails), hence timing them separately is valuable.

```
Benchmark               Time(ns)        CPU(ns)     Iterations
--------------------------------------------------------------
BM_old_L_mean              84382          84703          19547  11.807k items/s
BM_symbolic_L_mean         16345          16381          88414  61.055k items/s
BM_numeric_L_mean          10986          10994         120000  90.999k items/s
BM_old_XL_mean           1241208        1244212           1200  803.924 items/s
BM_symbolic_XL_mean       130917         131042          12720  7.631k items/s
BM_numeric_XL_mean         77004          76767          21116  13.029k items/s
```

PiperOrigin-RevId: 846704054
Change-Id: Ib0c365724d63bf2b81606ca5353756a6496c3a26
This commit is contained in:
Yuval Tassa
2025-12-19 06:11:02 -08:00
committed by Copybara-Service
parent d1fd11bccd
commit 45b0153067
7 changed files with 12305 additions and 92 deletions
+19 -3
View File
@@ -36,9 +36,25 @@ MJAPI int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus);
MJAPI int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
int* rownnz, const int* rowadr, int* colind, mjData* d);
// precount row non-zeros of reverse-Cholesky factor L, return total
MJAPI int mju_cholFactorCount(int* L_rownnz, const int* rownnz, const int* rowadr,
const int* colind, int n, mjData* d);
// symbolic reverse-Cholesky: compute both L (CSR) and LT (CSC) structures
// 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
// 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,
const int* rownnz, const int* rowadr, const int* colind,
int n, mjData* d);
// numeric reverse-Cholesky: compute L values given fixed sparsity pattern, returns rank
// L_colind must already contain the correct sparsity pattern (from mju_cholFactorSymbolic)
// LT_map[k] gives index in L for LT_colind[k]
MJAPI int mju_cholFactorNumeric(mjtNum* L, int n, mjtNum mindiag,
const int* L_rownnz, const int* L_rowadr, const int* L_colind,
const int* LT_rownnz, const int* LT_rowadr, const int* LT_colind,
const int* LT_map, const mjtNum* H,
const int* H_rownnz, const int* H_rowadr, const int* H_colind,
mjData* d);
// sparse reverse-order Cholesky solve
void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n,