diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 9f73b092..df95d868 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1401,7 +1401,9 @@ void mj_crb(const mjModel* m, mjData* d) { // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd -void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv) { +// (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; @@ -1469,16 +1471,15 @@ void mj_factorM(const mjModel* m, mjData* d) { for (int i=0; i < nC; i++) { d->qLD[i] = d->qM[d->mapM2C[i]]; } - mj_factorIs(d->qLD, d->qLDiagInv, m->nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); TM_ADD(mjTIMER_POS_INERTIA); } // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd -// like mj_factorI, but using CSR representation -void mj_factorIs(mjtNum* mat, mjtNum* diaginv, int nv, - const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) { +void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv, + const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) { // backward loop over rows for (int k=nv-1; k >= 0; k--) { // get row k's address, diagonal index, inverse diagonal value @@ -1510,10 +1511,9 @@ void mj_factorIs(mjtNum* mat, mjtNum* diaginv, int nv, // in-place sparse backsubstitution: x = inv(L'*D*L)*x -// L is in lower triangle of qLD; D is on diagonal of qLD -// handle n vectors at once -void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, - const mjtNum* qLD, const mjtNum* qLDiagInv) { +// (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; @@ -1624,9 +1624,8 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, // in-place sparse backsubstitution: x = inv(L'*D*L)*x -// like mj_solveLD, but using the CSR representation of L -void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, int n, - const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) { +void mj_solveLD(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, int n, + const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) { // x <- L^-T x for (int i=nv-1; i > 0; i--) { // skip diagonal rows @@ -1713,8 +1712,8 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) { if (x != y) { mju_copy(x, y, n*m->nv); } - mj_solveLDs(x, d->qLD, d->qLDiagInv, m->nv, n, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_solveLD(x, d->qLD, d->qLDiagInv, m->nv, n, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } @@ -1725,8 +1724,8 @@ void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int const mjtNum* qLD = d->qLD; const mjtNum* qLDiagInv = d->qLDiagInv; if (island < 0) { - mj_solveLDs(x, qLD, qLDiagInv, m->nv, 1, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_solveLD(x, qLD, qLDiagInv, m->nv, 1, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); return; } diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index a710416c..7f1b9fb3 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -48,25 +48,25 @@ MJAPI void mj_transmission(const mjModel* m, mjData* d); // composite rigid body inertia algorithm MJAPI void mj_crb(const mjModel* m, mjData* d); -// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd -MJAPI void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv); +// 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 -// like mj_factorI, but using CSR representation -MJAPI void mj_factorIs(mjtNum* mat, mjtNum* diaginv, int nv, - const int* rownnz, const int* rowadr, const int* diagnum, const int* colind); +MJAPI void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv, + const int* rownnz, const int* rowadr, const int* diagnum, const int* colind); // 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 -MJAPI void mj_solveLD(const mjModel* m, mjtNum* x, int n, - const mjtNum* qLD, const mjtNum* qLDiagInv); +// 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: x = inv(L'*D*L)*x // handle n vectors at once -MJAPI void mj_solveLDs(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, int n, - const int* rownnz, const int* rowadr, const int* diagnum, const int* colind); +MJAPI void mj_solveLD(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, int n, + const int* rownnz, const int* rowadr, const int* diagnum, const int* colind); // sparse backsubstitution: x = inv(L'*D*L)*y, use factorization in d MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 34fd4da9..2bb06790 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -803,14 +803,14 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { } // factorize in-place - mj_factorIs(d->qH, d->qHDiagInv, nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_factorI(d->qH, d->qHDiagInv, nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } // solve mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv); mju_copy(qacc, qfrc, m->nv); - mj_solveLDs(qacc, d->qH, d->qHDiagInv, nv, 1, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } // advance state and time @@ -992,13 +992,13 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { } // factorize in-place - mj_factorIs(d->qH, d->qHDiagInv, nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_factorI(d->qH, d->qHDiagInv, nv, d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } // solve for qacc: (qM - dt*qDeriv) * qacc = qfrc mju_copy(qacc, qfrc, nv); - mj_solveLDs(qacc, d->qH, d->qHDiagInv, nv, 1, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } else { mjERROR("integrator must be implicit or implicitfast"); diff --git a/test/benchmark/factorI_benchmark_test.cc b/test/benchmark/factorI_benchmark_test.cc index c1dedbd9..e434df5d 100644 --- a/test/benchmark/factorI_benchmark_test.cc +++ b/test/benchmark/factorI_benchmark_test.cc @@ -56,11 +56,11 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) { while (state.KeepRunningBatch(kNumBenchmarkSteps)) { for (int i=0; i < kNumBenchmarkSteps; i++) { if (legacy) { - mj_factorI(m, d, d->qM, LDlegacy, d->qLDiagInv); + mj_factorI_legacy(m, d, d->qM, LDlegacy, d->qLDiagInv); } else { mju_copy(d->qLD, M, m->nC); - mj_factorIs(d->qLD, d->qLDiagInv, m->nv, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_factorI(d->qLD, d->qLDiagInv, m->nv, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } } } diff --git a/test/benchmark/inertia_benchmark_test.cc b/test/benchmark/inertia_benchmark_test.cc index df230fc8..76cd73a4 100644 --- a/test/benchmark/inertia_benchmark_test.cc +++ b/test/benchmark/inertia_benchmark_test.cc @@ -67,16 +67,16 @@ static void BM_solve(benchmark::State& state, SolveType type) { mju_copy(res, vec, m->nv); switch (type) { case SolveType::kLegacy: - mj_factorI(m, d, d->qM, LDlegacy, d->qLDiagInv); - mj_solveLD(m, res, 1, LDlegacy, d->qLDiagInv); + 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_factorIs(d->qLD, d->qLDiagInv, m->nv, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); - mj_solveLDs(res, d->qLD, d->qLDiagInv, m->nv, 1, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_factorI(d->qLD, d->qLDiagInv, m->nv, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } } } diff --git a/test/benchmark/solveLD_benchmark_test.cc b/test/benchmark/solveLD_benchmark_test.cc index 434d5d2e..436e39ca 100644 --- a/test/benchmark/solveLD_benchmark_test.cc +++ b/test/benchmark/solveLD_benchmark_test.cc @@ -62,10 +62,10 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) { for (int i=0; i < kNumBenchmarkSteps; i++) { mju_copy(res, vec, m->nv); if (featherstone) { - mj_solveLD(m, res, 1, LDlegacy, d->qLDiagInv); + mj_solveLD_legacy(m, res, 1, LDlegacy, d->qLDiagInv); } else { - mj_solveLDs(res, d->qLD, d->qLDiagInv, m->nv, 1, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); } } } diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 05e9348e..d0c5fa00 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -549,9 +549,9 @@ 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; - mj_solveLD(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv); - mj_solveLDs(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_solveLD_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv); + mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // expect vectors to match up to floating point precision for (int i=0; i < nv; i++) { @@ -588,9 +588,9 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) { 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(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv); - mj_solveLDs(vec2.data(), d->qLD, d->qLDiagInv, nv, n, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_solveLD_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv); + mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // expect vectors to match up to floating point precision for (int i=0; i < nv*n; i++) { @@ -627,8 +627,8 @@ TEST_F(CoreSmoothTest, SolveM2) { vector res(nv*n); mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n); - mj_solveLDs(vec2.data(), d->qLD, d->qLDiagInv, nv, n, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // expect equality of dot(v, M^-1 * v) and dot(M^-1/2 * v, M^-1/2 * v) for (int i=0; i < n; i++) { @@ -653,7 +653,7 @@ TEST_F(CoreSmoothTest, FactorIs) { // copy qM into into qLDlegacy and factorize vector qLDlegacy(nM); - mj_factorI(m, d, d->qM, qLDlegacy.data(), d->qLDiagInv); + mj_factorI_legacy(m, d, d->qM, qLDlegacy.data(), d->qLDiagInv); // copy qLDlegacy into qLDexpected: CSR format vector qLDexpected(nC); @@ -670,8 +670,8 @@ TEST_F(CoreSmoothTest, FactorIs) { vector qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv); vector qLDiagInv(nv, 0); - mj_factorIs(qLD.data(), qLDiagInv.data(), nv, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_factorI(qLD.data(), qLDiagInv.data(), nv, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // expect outputs to match to floating point precision EXPECT_THAT(qLD, Pointwise(DoubleNear(1e-12), qLDexpected)); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 7aaf6ad3..ccb93526 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -339,7 +339,7 @@ TEST_F(DerivativeTest, StepSkip) { int nq = model->nq; int nv = model->nv; - // disable warmstarts so we don't need to save qacc_warmstart + // disable warm-starts so we don't need to save qacc_warmstart model->opt.disableflags |= mjDSBL_WARMSTART; for (const mjtIntegrator integrator : {mjINT_EULER, @@ -436,8 +436,8 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { Ac[i*nv + i] = -m->jnt_stiffness[i]; Ac[nv*nv + i*nv + i] = -m->dof_damping[i]; } - mj_solveLDs(Ac, d->qH, d->qHDiagInv, nv, 2*nv, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2*nv, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); // A = [dt*Ac; Ac] mju_transpose(A, Ac, 2*nv, nv); @@ -464,8 +464,8 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { mjtNum *BcT = mj_stackAllocNum(d, nv*nu); mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz, d->moment_rowadr, d->moment_colind); - mj_solveLDs(Bc, d->qH, d->qHDiagInv, nv, nu, - d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); + mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu, + d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); mju_transpose(BcT, Bc, nu, nv); mju_scl(B, BcT, dt*dt, nu*nv); mju_scl(B+nu*nv, BcT, dt, nu*nv);