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

Remove legacy factorI/solveLD and clean up benchmarks/tests

Browse Source 
Deletes mj_factorI_legacy and mj_solveLD_legacy from the engine and headers.
Updates factorI, solveLD, and inertia benchmarks to remove legacy targets
and only benchmark CSR.
Rewrites engine_core_smooth_test to verify CSR solver against mj_mulM
instead of legacy solver.

PiperOrigin-RevId: 942507341
Change-Id: I4281decb7018cfa3e46cb446efd5fe1179f6ae1f
This commit is contained in:
Yuval Tassa
2026-07-04 08:33:56 -07:00
committed by Copybara-Service
parent 10d6c01dce
commit 7c6f519879
6 changed files with 49 additions and 375 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_core_smooth_test.cc
+29 -108
View File
@@ -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<mjtNum> LDlegacy(nM, 0);
mju_scatter(LDlegacy.data(), d->qLD, m->mapM2M, nC);
// arbitrary RHS vector y
vector<mjtNum> 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<mjtNum> LDdense(nv * nv);
mju_sparse2dense(LDdense.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr,
m->M_colind);
vector<mjtNum> 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<mjtNum> vec(nv);
vector<mjtNum> 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<mjtNum> 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<mjtNum> 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<mjtNum> 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<mjtNum> vec(nv * n);
vector<mjtNum> 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<mjtNum> 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<mjtNum> 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<mjtNum> 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<mjtNum> qLDlegacy(nM);
mj_factorI_legacy(m, d, d->qM, qLDlegacy.data(), d->qLDiagInv);
// copy qLDlegacy into qLDexpected: CSR format
vector<mjtNum> qLDexpected(nC);
mju_gather(qLDexpected.data(), qLDlegacy.data(), m->mapM2M, nC);
// copy qM into qLD: CSR format
vector<mjtNum> qLD(nC);
mju_gather(qLD.data(), d->qM, m->mapM2M, nC);
vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv);
vector<mjtNum> 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<mjtNum> 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"(