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

Rename inertia factorization routines:

Browse Source 
```
mj_factorI -> mj_factorI_legacy
mj_solveLD -> mj_solveLD_legacy
mj_factorIs -> mj_factorI
mj_solveLDs -> mj_solveLD
```

PiperOrigin-RevId: 728246367
Change-Id: I3bc7804cb96faac2ae5419ba9abfc3c1648ed4c6
This commit is contained in:
Yuval Tassa
2025-02-18 09:40:43 -08:00
committed by Copybara-Service
parent 982330d15a
commit b516edae1b
8 changed files with 59 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+15 -16
View File
@@ -1401,7 +1401,9 @@ void mj_crb(const mjModel* m, mjData* d) {
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd // 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 cnt;
int Madr_kk, Madr_ki; int Madr_kk, Madr_ki;
mjtNum tmp; mjtNum tmp;
@@ -1469,16 +1471,15 @@ void mj_factorM(const mjModel* m, mjData* d) {
for (int i=0; i < nC; i++) { for (int i=0; i < nC; i++) {
d->qLD[i] = d->qM[d->mapM2C[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); TM_ADD(mjTIMER_POS_INERTIA);
} }
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd
// like mj_factorI, but using CSR representation void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv,
void mj_factorIs(mjtNum* mat, mjtNum* diaginv, int nv, const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) {
const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) {
// backward loop over rows // backward loop over rows
for (int k=nv-1; k >= 0; k--) { for (int k=nv-1; k >= 0; k--) {
// get row k's address, diagonal index, inverse diagonal value // 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 // in-place sparse backsubstitution: x = inv(L'*D*L)*x
// L is in lower triangle of qLD; D is on diagonal of qLD // (legacy implementation)
// handle n vectors at once void mj_solveLD_legacy(const mjModel* m, mjtNum* restrict x, int n,
void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n, const mjtNum* qLD, const mjtNum* qLDiagInv) {
const mjtNum* qLD, const mjtNum* qLDiagInv) {
// local copies of key variables // local copies of key variables
int* dof_Madr = m->dof_Madr; int* dof_Madr = m->dof_Madr;
int* dof_parentid = m->dof_parentid; 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 // in-place sparse backsubstitution: x = inv(L'*D*L)*x
// like mj_solveLD, but using the CSR representation of L void mj_solveLD(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, int n,
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) {
const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) {
// x <- L^-T x // x <- L^-T x
for (int i=nv-1; i > 0; i--) { for (int i=nv-1; i > 0; i--) {
// skip diagonal rows // 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) { if (x != y) {
mju_copy(x, y, n*m->nv); mju_copy(x, y, n*m->nv);
} }
mj_solveLDs(x, d->qLD, d->qLDiagInv, m->nv, n, mj_solveLD(x, d->qLD, d->qLDiagInv, m->nv, n,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); 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* qLD = d->qLD;
const mjtNum* qLDiagInv = d->qLDiagInv; const mjtNum* qLDiagInv = d->qLDiagInv;
if (island < 0) { if (island < 0) {
mj_solveLDs(x, qLD, qLDiagInv, m->nv, 1, mj_solveLD(x, qLD, qLDiagInv, m->nv, 1,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
return; return;
} }
src/engine/engine_core_smooth.h
+10 -10
View File
@@ -48,25 +48,25 @@ MJAPI void mj_transmission(const mjModel* m, mjData* d);
// composite rigid body inertia algorithm // composite rigid body inertia algorithm
MJAPI void mj_crb(const mjModel* m, mjData* d); MJAPI void mj_crb(const mjModel* m, mjData* d);
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd (legacy implementation)
MJAPI void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNum* qLDiagInv); 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 // sparse L'*D*L factorizaton of inertia-like matrix
// like mj_factorI, but using CSR representation MJAPI void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv,
MJAPI void mj_factorIs(mjtNum* mat, mjtNum* diaginv, int nv, const int* rownnz, const int* rowadr, const int* diagnum, const int* colind);
const int* rownnz, const int* rowadr, const int* diagnum, const int* colind);
// sparse L'*D*L factorizaton of the inertia matrix M, assumed spd // sparse L'*D*L factorizaton of the inertia matrix M, assumed spd
MJAPI void mj_factorM(const mjModel* m, mjData* d); MJAPI void mj_factorM(const mjModel* m, mjData* d);
// sparse backsubstitution: x = inv(L'*D*L)*x // sparse backsubstitution: x = inv(L'*D*L)*x (legacy implementation)
MJAPI void mj_solveLD(const mjModel* m, mjtNum* x, int n, MJAPI void mj_solveLD_legacy(const mjModel* m, mjtNum* x, int n,
const mjtNum* qLD, const mjtNum* qLDiagInv); const mjtNum* qLD, const mjtNum* qLDiagInv);
// in-place sparse backsubstitution: x = inv(L'*D*L)*x // in-place sparse backsubstitution: x = inv(L'*D*L)*x
// handle n vectors at once // handle n vectors at once
MJAPI void mj_solveLDs(mjtNum* x, const mjtNum* qLDs, const mjtNum* qLDiagInv, int nv, int n, 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); const int* rownnz, const int* rowadr, const int* diagnum, const int* colind);
// sparse backsubstitution: x = inv(L'*D*L)*y, use factorization in d // 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); MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n);
src/engine/engine_forward.c
+6 -6
View File
@@ -803,14 +803,14 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
} }
// factorize in-place // 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 // solve
mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv); mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
mju_copy(qacc, qfrc, m->nv); mju_copy(qacc, qfrc, m->nv);
mj_solveLDs(qacc, d->qH, d->qHDiagInv, nv, 1, mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
} }
// advance state and time // advance state and time
@@ -992,13 +992,13 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
} }
// factorize in-place // 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 // solve for qacc: (qM - dt*qDeriv) * qacc = qfrc
mju_copy(qacc, qfrc, nv); mju_copy(qacc, qfrc, nv);
mj_solveLDs(qacc, d->qH, d->qHDiagInv, nv, 1, mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
} else { } else {
mjERROR("integrator must be implicit or implicitfast"); mjERROR("integrator must be implicit or implicitfast");
test/benchmark/factorI_benchmark_test.cc
+3 -3
View File
@@ -56,11 +56,11 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) {
while (state.KeepRunningBatch(kNumBenchmarkSteps)) { while (state.KeepRunningBatch(kNumBenchmarkSteps)) {
for (int i=0; i < kNumBenchmarkSteps; i++) { for (int i=0; i < kNumBenchmarkSteps; i++) {
if (legacy) { if (legacy) {
mj_factorI(m, d, d->qM, LDlegacy, d->qLDiagInv); mj_factorI_legacy(m, d, d->qM, LDlegacy, d->qLDiagInv);
} else { } else {
mju_copy(d->qLD, M, m->nC); mju_copy(d->qLD, M, m->nC);
mj_factorIs(d->qLD, d->qLDiagInv, m->nv, mj_factorI(d->qLD, d->qLDiagInv, m->nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
} }
} }
} }
test/benchmark/inertia_benchmark_test.cc
+6 -6
View File
@@ -67,16 +67,16 @@ static void BM_solve(benchmark::State& state, SolveType type) {
mju_copy(res, vec, m->nv); mju_copy(res, vec, m->nv);
switch (type) { switch (type) {
case SolveType::kLegacy: case SolveType::kLegacy:
mj_factorI(m, d, d->qM, LDlegacy, d->qLDiagInv); mj_factorI_legacy(m, d, d->qM, LDlegacy, d->qLDiagInv);
mj_solveLD(m, res, 1, LDlegacy, d->qLDiagInv); mj_solveLD_legacy(m, res, 1, LDlegacy, d->qLDiagInv);
mj_solveM(m, d, res, vec, 1); mj_solveM(m, d, res, vec, 1);
break; break;
case SolveType::kCsr: case SolveType::kCsr:
mju_copy(d->qLD, M, m->nC); mju_copy(d->qLD, M, m->nC);
mj_factorIs(d->qLD, d->qLDiagInv, m->nv, mj_factorI(d->qLD, d->qLDiagInv, m->nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
mj_solveLDs(res, d->qLD, d->qLDiagInv, m->nv, 1, mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
} }
} }
} }
test/benchmark/solveLD_benchmark_test.cc
+3 -3
View File
@@ -62,10 +62,10 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
for (int i=0; i < kNumBenchmarkSteps; i++) { for (int i=0; i < kNumBenchmarkSteps; i++) {
mju_copy(res, vec, m->nv); mju_copy(res, vec, m->nv);
if (featherstone) { if (featherstone) {
mj_solveLD(m, res, 1, LDlegacy, d->qLDiagInv); mj_solveLD_legacy(m, res, 1, LDlegacy, d->qLDiagInv);
} else { } else {
mj_solveLDs(res, d->qLD, d->qLDiagInv, m->nv, 1, mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
} }
} }
} }
test/engine/engine_core_smooth_test.cc
+11 -11
View File
@@ -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++) vec[i] = vec2[i] = 20 + 30*i;
for (int i=0; i < nv; i+=2) vec[i] = vec2[i] = 0; for (int i=0; i < nv; i+=2) vec[i] = vec2[i] = 0;
mj_solveLD(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv); mj_solveLD_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv);
mj_solveLDs(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
// expect vectors to match up to floating point precision // expect vectors to match up to floating point precision
for (int i=0; i < nv; i++) { 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++) vec[i] = vec2[i] = 2 + 3*i;
for (int i=0; i < nv*n; i+=3) vec[i] = vec2[i] = 0; 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_solveLD_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv);
mj_solveLDs(vec2.data(), d->qLD, d->qLDiagInv, nv, n, mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
// expect vectors to match up to floating point precision // expect vectors to match up to floating point precision
for (int i=0; i < nv*n; i++) { for (int i=0; i < nv*n; i++) {
@@ -627,8 +627,8 @@ TEST_F(CoreSmoothTest, SolveM2) {
vector<mjtNum> res(nv*n); vector<mjtNum> res(nv*n);
mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n); mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n);
mj_solveLDs(vec2.data(), d->qLD, d->qLDiagInv, nv, n, mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); 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) // 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++) { for (int i=0; i < n; i++) {
@@ -653,7 +653,7 @@ TEST_F(CoreSmoothTest, FactorIs) {
// copy qM into into qLDlegacy and factorize // copy qM into into qLDlegacy and factorize
vector<mjtNum> qLDlegacy(nM); vector<mjtNum> 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 // copy qLDlegacy into qLDexpected: CSR format
vector<mjtNum> qLDexpected(nC); vector<mjtNum> qLDexpected(nC);
@@ -670,8 +670,8 @@ TEST_F(CoreSmoothTest, FactorIs) {
vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv); vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv);
vector<mjtNum> qLDiagInv(nv, 0); vector<mjtNum> qLDiagInv(nv, 0);
mj_factorIs(qLD.data(), qLDiagInv.data(), nv, mj_factorI(qLD.data(), qLDiagInv.data(), nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
// expect outputs to match to floating point precision // expect outputs to match to floating point precision
EXPECT_THAT(qLD, Pointwise(DoubleNear(1e-12), qLDexpected)); EXPECT_THAT(qLD, Pointwise(DoubleNear(1e-12), qLDexpected));
test/engine/engine_derivative_test.cc
+5 -5
View File
@@ -339,7 +339,7 @@ TEST_F(DerivativeTest, StepSkip) {
int nq = model->nq; int nq = model->nq;
int nv = model->nv; 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; model->opt.disableflags |= mjDSBL_WARMSTART;
for (const mjtIntegrator integrator : {mjINT_EULER, 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[i*nv + i] = -m->jnt_stiffness[i];
Ac[nv*nv + i*nv + i] = -m->dof_damping[i]; Ac[nv*nv + i*nv + i] = -m->dof_damping[i];
} }
mj_solveLDs(Ac, d->qH, d->qHDiagInv, nv, 2*nv, mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2*nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
// A = [dt*Ac; Ac] // A = [dt*Ac; Ac]
mju_transpose(A, Ac, 2*nv, nv); 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); mjtNum *BcT = mj_stackAllocNum(d, nv*nu);
mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz, mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz,
d->moment_rowadr, d->moment_colind); d->moment_rowadr, d->moment_colind);
mj_solveLDs(Bc, d->qH, d->qHDiagInv, nv, nu, mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind); d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
mju_transpose(BcT, Bc, nu, nv); mju_transpose(BcT, Bc, nu, nv);
mju_scl(B, BcT, dt*dt, nu*nv); mju_scl(B, BcT, dt*dt, nu*nv);
mju_scl(B+nu*nv, BcT, dt, nu*nv); mju_scl(B+nu*nv, BcT, dt, nu*nv);