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

Use only lower triangle in CSR back-substitution.

Browse Source 
PiperOrigin-RevId: 711686157
Change-Id: I4fc98cdfb927e5608ce3a99ea34890ce556fcfd7
This commit is contained in:
Yuval Tassa
2025-01-03 02:32:54 -08:00
committed by Copybara-Service
parent 6c880eb0fa
commit 923f75fd23
2 changed files with 10 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+10 -7
View File
@@ -1578,16 +1578,19 @@ void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv
const int* rownnz, const int* rowadr, const int* diagind, const int* diagnum,
const int* colind) {
// x <- L^-T x
for (int i=nv-2; i >= 0; i--) {
// skip diagonal (simple) rows
if (diagnum[i]) {
for (int i=nv-1; i > 0; i--) {
// skip diagonal (simple) rows, exploit sparsity of input vector
if (diagnum[i] || x[i] == 0) {
continue;
}
int d1 = diagind[i] + 1;
int nnz = rownnz[i] - d1;
int adr = rowadr[i] + d1;
x[i] -= mju_dotSparse(qLDs+adr, x, nnz, colind+adr, /*flg_unc1=*/0);
int d = diagind[i];
int adr_i = rowadr[i];
mjtNum x_i = x[i];
for (int j=0; j < d; j++) {
int adr = adr_i + j;
x[colind[adr]] -= qLDs[adr] * x_i;
}
}
// x(i) /= D(i,i)
test/engine/engine_core_smooth_test.cc
-60
View File
@@ -456,53 +456,6 @@ TEST_F(CoreSmoothTest, FactorI) {
mj_deleteModel(model);
}
// in-place sparse backsubstitution: x = inv(L'*D*L)*x
// like mj_solveLD, but using the CSR representation of L
// variant that only uses the lower triangle of qLDs
static void mj_solveLDsLower(mjtNum* x, const mjtNum* qLDs,
const mjtNum* qLDiagInv, int nv, const int* rownnz,
const int* rowadr, const int* diagind,
const int* diagnum, const int* colind,
int* scratch) {
int* marker = scratch;
for (int i=1; i < nv; i++) {
marker[i] = rowadr[i] + diagind[i] - 1;
}
// x <- L^-T x
for (int i=nv-2; i >= 0; i--) {
// skip diagonal (simple) rows
if (diagnum[i]) {
continue;
}
for (int j=i+1; j < nv; j++) {
if (colind[marker[j]] == i) {
x[i] -= qLDs[marker[j]--] * x[j];
}
}
}
// x(i) /= D(i,i)
for (int i=0; i < nv; i++) {
x[i] *= qLDiagInv[i];
}
// x <- L^-1 x
for (int i=1; i < nv; i++) {
// skip diagonal (simple) rows
if (diagnum[i]) {
i += diagnum[i] - 1; // when iterating forward we can skip ahead
continue;
}
int d = diagind[i];
int adr = rowadr[i];
x[i] -= mju_dotSparse(qLDs+adr, x, d, colind+adr, /*flg_unc1=*/0);
}
}
TEST_F(CoreSmoothTest, SolveLDs) {
const std::string xml_path = GetTestDataFilePath(kInertiaPath);
char error[1024];
@@ -537,7 +490,6 @@ TEST_F(CoreSmoothTest, SolveLDs) {
for (int i=0; i < nv; i++) vec[i] = vec2[i] = 20 + 30*i;
for (int i=0; i < nv; i+=2) vec[i] = vec2[i] = 0;
// use upper triangle
mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv);
mj_solveLDs(vec2.data(), LDs.data(), d->qLDiagInv, nv,
d->C_rownnz, d->C_rowadr, d->C_diag, m->dof_simplenum,
@@ -548,18 +500,6 @@ TEST_F(CoreSmoothTest, SolveLDs) {
EXPECT_FLOAT_EQ(vec[i], vec2[i]);
}
// don't use use upper triangle
mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv);
vector<int> scratch(nv);
mj_solveLDsLower(vec2.data(), LDs.data(), d->qLDiagInv, nv, d->C_rownnz,
d->C_rowadr, d->C_diag, m->dof_simplenum, d->C_colind,
scratch.data());
// expect vectors to match up to floating point precision
for (int i=0; i < nv; i++) {
EXPECT_FLOAT_EQ(vec[i], vec2[i]);
}
mj_deleteData(d);
mj_deleteModel(m);
}