diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index d0baa655..2d4ac47f 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1876,69 +1876,6 @@ void mj_makeM(const mjModel* m, mjData* d) { } -// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd -// (legacy implementation) -void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, - mjtNum* qLDiagInv) { - int cnt; - int Madr_kk, Madr_ki; - mjtNum tmp; - - // local copies of key variables - int* dof_Madr = m->dof_Madr; - int* dof_parentid = m->dof_parentid; - int nv = m->nv; - - // copy M into LD - mju_copy(qLD, M, m->nM); - - // dense backward loop over dofs (regular only, simple diagonal already copied) - for (int k=nv-1; k >= 0; k--) { - // get address of M(k,k) - Madr_kk = dof_Madr[k]; - - // check for small/negative numbers on diagonal - if (qLD[Madr_kk] < mjMINVAL) { - mj_warning(d, mjWARN_INERTIA, k); - qLD[Madr_kk] = mjMINVAL; - } - - // skip the rest if simple - if (m->dof_simplenum[k]) { - continue; - } - - // sparse backward loop over ancestors of k (excluding k) - Madr_ki = Madr_kk + 1; - int i = dof_parentid[k]; - while (i >= 0) { - tmp = qLD[Madr_ki] / qLD[Madr_kk]; // tmp = M(k,i) / M(k,k) - - // get number of ancestors of i (including i) - if (i < nv-1) { - cnt = dof_Madr[i+1] - dof_Madr[i]; - } else { - cnt = m->nM - dof_Madr[i+1]; - } - - // M(i,j) -= M(k,j) * tmp - mju_addToScl(qLD+dof_Madr[i], qLD+Madr_ki, -tmp, cnt); - - qLD[Madr_ki] = tmp; // M(k,i) = tmp - - // advance to i's parent - i = dof_parentid[i]; - Madr_ki++; - } - } - - // compute 1/diag(D) - for (int i=0; i < nv; i++) { - qLDiagInv[i] = 1.0 / qLD[dof_Madr[i]]; - } -} - - // sparse L'*D*L factorizaton of the inertia matrix M, assumed spd void mj_factorM(const mjModel* m, mjData* d) { TM_START; @@ -1997,118 +1934,6 @@ void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv, } -// in-place sparse backsubstitution: x = inv(L'*D*L)*x -// (legacy implementation) -void mj_solveLD_legacy(const mjModel* m, mjtNum* restrict x, int n, - const mjtNum* qLD, const mjtNum* qLDiagInv) { - // local copies of key variables - int* dof_Madr = m->dof_Madr; - int* dof_parentid = m->dof_parentid; - int nv = m->nv; - - // single vector - if (n == 1) { - // x <- inv(L') * x; skip simple, exploit sparsity of input vector - for (int i=nv-1; i >= 0; i--) { - if (!m->dof_simplenum[i] && x[i]) { - // init - int Madr_ij = dof_Madr[i]+1; - int j = dof_parentid[i]; - - // traverse ancestors backwards - // read directly from x[i] since i cannot be a parent of itself - while (j >= 0) { - x[j] -= qLD[Madr_ij++]*x[i]; // x(j) -= L(i,j) * x(i) - - // advance to parent - j = dof_parentid[j]; - } - } - } - - // x <- inv(D) * x - for (int i=0; i < nv; i++) { - x[i] *= qLDiagInv[i]; // x(i) /= L(i,i) - } - - // x <- inv(L) * x; skip simple - for (int i=0; i < nv; i++) { - if (!m->dof_simplenum[i]) { - // init - int Madr_ij = dof_Madr[i]+1; - int j = dof_parentid[i]; - - // traverse ancestors backwards - // write directly in x[i] since i cannot be a parent of itself - while (j >= 0) { - x[i] -= qLD[Madr_ij++]*x[j]; // x(i) -= L(i,j) * x(j) - - // advance to parent - j = dof_parentid[j]; - } - } - } - } - - // multiple vectors - else { - int offset; - mjtNum tmp; - - // x <- inv(L') * x; skip simple - for (int i=nv-1; i >= 0; i--) { - if (!m->dof_simplenum[i]) { - // init - int Madr_ij = dof_Madr[i]+1; - int j = dof_parentid[i]; - - // traverse ancestors backwards - while (j >= 0) { - // process all vectors, exploit sparsity - for (offset=0; offset < n*nv; offset+=nv) - if ((tmp = x[i+offset])) { - x[j+offset] -= qLD[Madr_ij]*tmp; // x(j) -= L(i,j) * x(i) - } - - // advance to parent - Madr_ij++; - j = dof_parentid[j]; - } - } - } - - // x <- inv(D) * x - for (int i=0; i < nv; i++) { - for (offset=0; offset < n*nv; offset+=nv) { - x[i+offset] *= qLDiagInv[i]; // x(i) /= L(i,i) - } - } - - // x <- inv(L) * x; skip simple - for (int i=0; i < nv; i++) { - if (!m->dof_simplenum[i]) { - // init - int Madr_ij = dof_Madr[i]+1; - int j = dof_parentid[i]; - - // traverse ancestors backwards - tmp = x[i+offset]; - while (j >= 0) { - // process all vectors - for (offset=0; offset < n*nv; offset+=nv) { - x[i+offset] -= qLD[Madr_ij]*x[j+offset]; // x(i) -= L(i,j) * x(j) - } - - // advance to parent - Madr_ij++; - j = dof_parentid[j]; - } - } - } - } -} - - // in-place sparse backsubstitution: x = inv(L'*D*L)*x (with dof skipping) void mj_solveLD(mjtNum* restrict x, const mjtNum* qLD, const mjtNum* qLDiagInv, int nv, int n, const int* rownnz, const int* rowadr, const int* colind, const int* index) { diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index 0ea923d5..9ecf73d3 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -64,10 +64,6 @@ MJAPI void mj_tendonArmature(const mjModel* m, mjData* d); // make inertia matrix MJAPI void mj_makeM(const mjModel* m, mjData* d); -// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd (legacy implementation) -MJAPI void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, - mjtNum* qLD, mjtNum* qLDiagInv); - // sparse L'*D*L factorizaton of inertia-like matrix (only dofs in index, if given) MJAPI void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv, const int* rownnz, const int* rowadr, const int* colind, const int* index); @@ -75,10 +71,6 @@ MJAPI void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv, // sparse L'*D*L factorizaton of the inertia matrix M, assumed spd MJAPI void mj_factorM(const mjModel* m, mjData* d); -// sparse backsubstitution: x = inv(L'*D*L)*x (legacy implementation) -MJAPI void mj_solveLD_legacy(const mjModel* m, mjtNum* x, int n, - const mjtNum* qLD, const mjtNum* qLDiagInv); - // in-place sparse backsubstitution (only dofs in index, if given): x = inv(L'*D*L)*x // handle n vectors at once MJAPI void mj_solveLD(mjtNum* x, const mjtNum* qLD, const mjtNum* qLDiagInv, int nv, int n, diff --git a/test/benchmark/factorI_benchmark_test.cc b/test/benchmark/factorI_benchmark_test.cc index 73e0af06..88cbf0a6 100644 --- a/test/benchmark/factorI_benchmark_test.cc +++ b/test/benchmark/factorI_benchmark_test.cc @@ -30,7 +30,7 @@ static const int kNumBenchmarkSteps = 50; // ----------------------------- benchmark ------------------------------------ -static void BM_factorI(benchmark::State& state, bool legacy, bool coil) { +static void BM_factorI(benchmark::State& state, bool coil) { static mjModel* m; if (coil) { m = LoadModelFromPath("plugin/elasticity/coil.xml"); @@ -46,21 +46,14 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) { // M: mass matrix in CSR format mjtNum* M = mj_stackAllocNum(d, m->nC); - mju_gather(M, d->qM, m->mapM2M, m->nC); - - // LDlegacy: legacy LD matrix (size nM) - mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM); + mju_copy(M, d->M, m->nC); // benchmark while (state.KeepRunningBatch(kNumBenchmarkSteps)) { for (int i=0; i < kNumBenchmarkSteps; i++) { - if (legacy) { - mj_factorI_legacy(m, d, d->qM, LDlegacy, d->qLDiagInv); - } else { - mju_copy(d->qLD, M, m->nC); - mj_factorI(d->qLD, d->qLDiagInv, m->nv, - m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); - } + mju_copy(d->qLD, M, m->nC); + mj_factorI(d->qLD, d->qLDiagInv, m->nv, + m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); } } @@ -71,31 +64,17 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) { state.SetItemsProcessed(state.iterations()); } -void ABSL_ATTRIBUTE_NO_TAIL_CALL -BM_factorI_COIL_LEGACY(benchmark::State& state) { - MujocoErrorTestGuard guard; - BM_factorI(state, /*legacy=*/true, /*coil=*/true); -} -BENCHMARK(BM_factorI_COIL_LEGACY); - void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_factorI_COIL_CSR(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_factorI(state, /*legacy=*/false, /*coil=*/true); + BM_factorI(state, /*coil=*/true); } BENCHMARK(BM_factorI_COIL_CSR); -void ABSL_ATTRIBUTE_NO_TAIL_CALL -BM_factorI_H100_LEGACY(benchmark::State& state) { - MujocoErrorTestGuard guard; - BM_factorI(state, /*legacy=*/true, /*coil=*/false); -} -BENCHMARK(BM_factorI_H100_LEGACY); - void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_factorI_H100_CSR(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_factorI(state, /*legacy=*/false, /*coil=*/false); + BM_factorI(state, /*coil=*/false); } BENCHMARK(BM_factorI_H100_CSR); diff --git a/test/benchmark/inertia_benchmark_test.cc b/test/benchmark/inertia_benchmark_test.cc index cd204663..257011bf 100644 --- a/test/benchmark/inertia_benchmark_test.cc +++ b/test/benchmark/inertia_benchmark_test.cc @@ -31,12 +31,7 @@ static const int kNumBenchmarkSteps = 50; // ----------------------------- benchmark ------------------------------------ -enum class SolveType { - kLegacy = 0, - kCsr, -}; - -static void BM_solve(benchmark::State& state, SolveType type) { +static void BM_solve(benchmark::State& state) { static mjModel* m; m = LoadModelFromPath("../test/benchmark/testdata/inertia.xml"); @@ -48,10 +43,7 @@ static void BM_solve(benchmark::State& state, SolveType type) { // M: mass matrix in CSR format mjtNum* M = mj_stackAllocNum(d, m->nC); - mju_gather(M, d->qM, m->mapM2M, m->nC); - - // LDlegacy: legacy LD matrix (size nM) - mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM); + mju_copy(M, d->M, m->nC); // arbitrary input vector mjtNum *res = mj_stackAllocNum(d, m->nv); @@ -64,19 +56,11 @@ static void BM_solve(benchmark::State& state, SolveType type) { while (state.KeepRunningBatch(kNumBenchmarkSteps)) { for (int i=0; i < kNumBenchmarkSteps; i++) { mju_copy(res, vec, m->nv); - switch (type) { - case SolveType::kLegacy: - mj_factorI_legacy(m, d, d->qM, LDlegacy, d->qLDiagInv); - mj_solveLD_legacy(m, res, 1, LDlegacy, d->qLDiagInv); - mj_solveM(m, d, res, vec, 1); - break; - case SolveType::kCsr: - mju_copy(d->qLD, M, m->nC); - mj_factorI(d->qLD, d->qLDiagInv, m->nv, - m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); - mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1, - m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); - } + mju_copy(d->qLD, M, m->nC); + mj_factorI(d->qLD, d->qLDiagInv, m->nv, + m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); + mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1, + m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); } } @@ -87,15 +71,9 @@ static void BM_solve(benchmark::State& state, SolveType type) { state.SetItemsProcessed(state.iterations()); } -void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solve_LEGACY(benchmark::State& state) { - MujocoErrorTestGuard guard; - BM_solve(state, SolveType::kLegacy); -} -BENCHMARK(BM_solve_LEGACY); - void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solve_CSR(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_solve(state, SolveType::kCsr); + BM_solve(state); } BENCHMARK(BM_solve_CSR); diff --git a/test/benchmark/solveLD_benchmark_test.cc b/test/benchmark/solveLD_benchmark_test.cc index a6c1e157..e979a1e0 100644 --- a/test/benchmark/solveLD_benchmark_test.cc +++ b/test/benchmark/solveLD_benchmark_test.cc @@ -30,7 +30,7 @@ static const int kNumBenchmarkSteps = 50; // ----------------------------- benchmark ------------------------------------ -static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { +static void BM_solveLD(benchmark::State& state, bool coil) { static mjModel* m; if (coil) { m = LoadModelFromPath("plugin/elasticity/coil.xml"); @@ -51,21 +51,12 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { vec[i] = 0.2 + 0.3*i; } - // scatter into legacy matrix - mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM); - mju_zero(LDlegacy, m->nM); - mju_scatter(LDlegacy, d->qLD, m->mapM2M, m->nC); - // benchmark while (state.KeepRunningBatch(kNumBenchmarkSteps)) { for (int i=0; i < kNumBenchmarkSteps; i++) { mju_copy(res, vec, m->nv); - if (featherstone) { - mj_solveLD_legacy(m, res, 1, LDlegacy, d->qLDiagInv); - } else { - mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1, - m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); - } + mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1, + m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); } } @@ -76,27 +67,15 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { state.SetItemsProcessed(state.iterations()); } -void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_COIL_FS(benchmark::State& state) { - MujocoErrorTestGuard guard; - BM_solveLD(state, /*featherstone=*/true, /*coil=*/true); -} -BENCHMARK(BM_solveLD_COIL_FS); - void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_COIL_CSR(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_solveLD(state, /*featherstone=*/false, /*coil=*/true); + BM_solveLD(state, /*coil=*/true); } BENCHMARK(BM_solveLD_COIL_CSR); -void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_H100_FS(benchmark::State& state) { - MujocoErrorTestGuard guard; - BM_solveLD(state, /*featherstone=*/true, /*coil=*/false); -} -BENCHMARK(BM_solveLD_H100_FS); - void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_solveLD_H100_CSR(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_solveLD(state, /*featherstone=*/false, /*coil=*/false); + BM_solveLD(state, /*coil=*/false); } BENCHMARK(BM_solveLD_H100_CSR); diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 73a4b1e8..6cff4a3a 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -663,22 +663,7 @@ TEST_F(CoreSmoothTest, FactorI) { mj_deleteModel(model); } -// Convert legacy-format symmetric matrix to dense (local helper for tests). -static void legacyToDense(const mjModel* m, mjtNum* dst, const mjtNum* M) { - int adr = 0, nv = m->nv; - mju_zero(dst, nv * nv); - for (int i = 0; i < nv; i++) { - int j = i; - while (j >= 0) { - dst[i * nv + j] = M[adr]; - dst[j * nv + i] = M[adr]; - j = m->dof_parentid[j]; - adr++; - } - } -} - -TEST_F(CoreSmoothTest, SolveLDs) { +TEST_F(CoreSmoothTest, SolveLD) { const std::string xml_path = GetTestDataFilePath(kInertiaPath); char error[1024]; mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); @@ -688,41 +673,24 @@ TEST_F(CoreSmoothTest, SolveLDs) { mj_forward(m, d); int nv = m->nv; - int nM = m->nM; - int nC = m->nC; - // copy M into LD: Legacy format - vector LDlegacy(nM, 0); - mju_scatter(LDlegacy.data(), d->qLD, m->mapM2M, nC); + // arbitrary RHS vector y + vector y(nv); + for (int i = 0; i < nv; i++) y[i] = 20 + 30 * i; + for (int i = 0; i < nv; i += 2) y[i] = 0; - // compare LD and LDs densified matrices - vector LDdense(nv * nv); - mju_sparse2dense(LDdense.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr, - m->M_colind); - vector LDdense2(nv * nv); - legacyToDense(m, LDdense2.data(), LDlegacy.data()); - - // expect lower triangles to match exactly - for (int i = 0; i < nv; i++) { - for (int j = 0; j < i; j++) { - EXPECT_NEAR(LDdense[i * nv + j], LDdense2[i * nv + j], - MjTol(1e-14, 1e-6)); - } - } - - // compare legacy and CSR LD vector solve - vector vec(nv); - vector vec2(nv); - 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; - - mj_solveLD_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv); - mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, + // x = inv(M) * y + vector x = y; + mj_solveLD(x.data(), d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); - // expect vectors to match up to floating point precision + // z = M * x + vector z(nv); + mj_mulM(m, d, z.data(), x.data()); + + // expect z to match y for (int i = 0; i < nv; i++) { - EXPECT_NEAR(vec[i], vec2[i], MjTol(1e-14, 5e-6)); + EXPECT_NEAR(z[i], y[i], MjTol(1e-12, 5e-4)); } mj_deleteData(d); @@ -739,25 +707,25 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) { mj_forward(m, d); int nv = m->nv; - - // copy LD into LDlegacy: Legacy format - vector LDlegacy(m->nM, 0); - mju_scatter(LDlegacy.data(), d->qLD, m->mapM2M, m->nC); - - // compare n LD and LDs vector solve int n = 3; - vector vec(nv * n); - vector vec2(nv * n); - for (int i = 0; i < nv * n; i++) vec[i] = vec2[i] = 2 + 3 * i; - for (int i = 0; i < nv * n; i += 3) vec[i] = vec2[i] = 0; - mj_solveLD_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv); - mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr, + // Y: arbitrary RHS vectors (nv x n) + vector Y(nv * n); + for (int i = 0; i < nv * n; i++) Y[i] = 2 + 3 * i; + for (int i = 0; i < nv * n; i += 3) Y[i] = 0; + + // X = inv(M) * Y + vector X = Y; + mj_solveLD(X.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); - // expect vectors to match up to floating point precision - for (int i = 0; i < nv * n; i++) { - EXPECT_NEAR(vec[i], vec2[i], MjTol(1e-14, 5e-6)); + // verify each vector: Z_i = M * X_i + for (int i = 0; i < n; i++) { + vector z(nv); + mj_mulM(m, d, z.data(), X.data() + i * nv); + for (int j = 0; j < nv; j++) { + EXPECT_NEAR(z[j], Y[i * nv + j], MjTol(1e-12, 5e-4)); + } } mj_deleteData(d); @@ -804,54 +772,7 @@ TEST_F(CoreSmoothTest, SolveM2) { mj_deleteModel(m); } -TEST_F(CoreSmoothTest, FactorIs) { - const std::string xml_path = GetTestDataFilePath(kInertiaPath); - char error[1024]; - mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error; - mjData* d = mj_makeData(m); - mj_forward(m, d); - - int nC = m->nC, nM = m->nM, nv = m->nv; - - // copy qM into into qLDlegacy and factorize - vector qLDlegacy(nM); - mj_factorI_legacy(m, d, d->qM, qLDlegacy.data(), d->qLDiagInv); - - // copy qLDlegacy into qLDexpected: CSR format - vector qLDexpected(nC); - mju_gather(qLDexpected.data(), qLDlegacy.data(), m->mapM2M, nC); - - // copy qM into qLD: CSR format - vector qLD(nC); - mju_gather(qLD.data(), d->qM, m->mapM2M, nC); - - vector qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv); - vector qLDiagInv(nv, 0); - - mj_factorI(qLD.data(), qLDiagInv.data(), nv, m->M_rownnz, m->M_rowadr, - m->M_colind, nullptr); - - // expect outputs to match to floating point precision - EXPECT_THAT(qLD, Pointwise(MjNear(1e-12, 1e-4), qLDexpected)); - EXPECT_THAT(qLDiagInv, Pointwise(MjNear(1e-12, 1e-4), qLDiagInvExpected)); - - /* uncomment for debugging - vector LDdense(nv*nv); - - mju_sparse2dense(LDdense.data(), qLDexpected.data(), nv, nv, - d->C_rownnz, d->C_rowadr, d->C_colind); - PrintMatrix(LDdense.data(), nv, nv, 2); - - mju_sparse2dense(LDdense.data(), qLDs.data(), nv, nv, - d->C_rownnz, d->C_rowadr, d->C_colind); - PrintMatrix(LDdense.data(), nv, nv, 2); - */ - - mj_deleteData(d); - mj_deleteModel(m); -} TEST_F(CoreSmoothTest, FlexVertLengthScaling) { constexpr char xml[] = R"(