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

Switch mjData.{qH,qLD} from reduced ("C") to full ("M") inertia matrix structure. No performance impact of extra zeros because of existing "simple dof" skipping mechanism.

Browse Source 
PiperOrigin-RevId: 733523931
Change-Id: Ic8d8a152dda5532331c239cb6b4ce7d8d09b7fff
This commit is contained in:
Yuval Tassa
2025-03-04 17:33:48 -08:00
committed by Copybara-Service
parent 51f6aa8b43
commit 0f563ecf31
20 changed files with 95 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_core_smooth_test.cc
+26 -28
View File
@@ -476,7 +476,7 @@ TEST_F(CoreSmoothTest, FactorI) {
int nv = model->nv;
vector<mjtNum> Ldense(nv*nv, 0);
mju_sparse2dense(Ldense.data(), data->qLD, nv, nv,
data->C_rownnz, data->C_rowadr, data->C_colind);
data->M_rownnz, data->M_rowadr, data->M_colind);
for (int i=0; i < nv; i++) {
// set diagonal to 1
Ldense[i*nv+i] = 1;
@@ -485,7 +485,7 @@ TEST_F(CoreSmoothTest, FactorI) {
// dense D matrix
vector<mjtNum> Ddense(nv*nv);
mju_sparse2dense(Ddense.data(), data->qLD, nv, nv,
data->C_rownnz, data->C_rowadr, data->C_colind);
data->M_rownnz, data->M_rowadr, data->M_colind);
for (int i=0; i < nv; i++) {
for (int j=0; j < nv; j++) {
// zero everything except the diagonal
@@ -521,18 +521,17 @@ TEST_F(CoreSmoothTest, SolveLDs) {
int nv = m->nv;
int nM = m->nM;
int nC = m->nC;
// copy M into LD: Legacy format
vector<mjtNum> LDlegacy(nM, 0);
for (int i=0; i < nC; i++) {
LDlegacy[d->mapM2C[i]] = d->qLD[i];
vector<mjtNum> LDlegacy(nM);
for (int i=0; i < nM; i++) {
LDlegacy[d->mapM2M[i]] = d->qLD[i];
}
// compare LD and LDs densified matrices
vector<mjtNum> LDdense(nv*nv);
mju_sparse2dense(LDdense.data(), d->qLD, nv, nv,
d->C_rownnz, d->C_rowadr, d->C_colind);
d->M_rownnz, d->M_rowadr, d->M_colind);
vector<mjtNum> LDdense2(nv*nv);
mj_fullM(m, LDdense2.data(), LDlegacy.data());
@@ -551,7 +550,7 @@ TEST_F(CoreSmoothTest, SolveLDs) {
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);
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
// expect vectors to match up to floating point precision
for (int i=0; i < nv; i++) {
@@ -573,12 +572,11 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) {
int nv = m->nv;
int nM = m->nM;
int nC = m->nC;
// copy LD into LDlegacy: Legacy format
vector<mjtNum> LDlegacy(nM, 0);
for (int i=0; i < nC; i++) {
LDlegacy[d->mapM2C[i]] = d->qLD[i];
vector<mjtNum> LDlegacy(nM);
for (int i=0; i < nM; i++) {
LDlegacy[d->mapM2M[i]] = d->qLD[i];
}
// compare n LD and LDs vector solve
@@ -590,7 +588,7 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) {
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);
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
// expect vectors to match up to floating point precision
for (int i=0; i < nv*n; i++) {
@@ -614,7 +612,7 @@ TEST_F(CoreSmoothTest, SolveM2) {
int nv = m->nv;
vector<mjtNum> sqrtInvD(nv);
for (int i=0; i < nv; i++) {
int diag = d->C_rowadr[i] + d->C_rownnz[i] - 1;
int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1;
sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]);
}
@@ -628,7 +626,7 @@ TEST_F(CoreSmoothTest, SolveM2) {
mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_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++) {
@@ -649,29 +647,29 @@ TEST_F(CoreSmoothTest, FactorIs) {
mjData* d = mj_makeData(m);
mj_forward(m, d);
int nC = m->nC, nM = m->nM, nv = m->nv;
int 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);
for (int i=0; i < nC; i++) {
qLDexpected[i] = qLDlegacy[d->mapM2C[i]]; // mj_factorIs is in-place
vector<mjtNum> qLDexpected(nM);
for (int i=0; i < nM; i++) {
qLDexpected[i] = qLDlegacy[d->mapM2M[i]];
}
// copy qM into qLD: CSR format
vector<mjtNum> qLD(nC);
for (int i=0; i < nC; i++) {
qLD[i] = d->qM[d->mapM2C[i]]; // mj_factorIs is in-place
vector<mjtNum> qLD(nM);
for (int i=0; i < nM; i++) {
qLD[i] = d->qM[d->mapM2M[i]]; // mj_factorI is in-place
}
vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv);
vector<mjtNum> qLDiagInv(nv, 0);
mj_factorI(qLD.data(), qLDiagInv.data(), nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
// expect outputs to match to floating point precision
EXPECT_THAT(qLD, Pointwise(DoubleNear(1e-12), qLDexpected));
@@ -681,12 +679,12 @@ TEST_F(CoreSmoothTest, FactorIs) {
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);
d->M_rownnz, d->M_rowadr, d->M_colind);
PrintMatrix(LDdense.data(), nv, nv, 2, "qLDexpected");
mju_sparse2dense(LDdense.data(), qLDs.data(), nv, nv,
d->C_rownnz, d->C_rowadr, d->C_colind);
PrintMatrix(LDdense.data(), nv, nv, 2);
mju_sparse2dense(LDdense.data(), qLD.data(), nv, nv,
d->M_rownnz, d->M_rowadr, d->M_colind);
PrintMatrix(LDdense.data(), nv, nv, 2, "qLD");
*/
mj_deleteData(d);
test/engine/engine_derivative_test.cc
+2 -2
View File
@@ -436,7 +436,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
Ac[nv*nv + i*nv + i] = -m->dof_damping[i];
}
mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2*nv,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
// A = [dt*Ac; Ac]
mju_transpose(A, Ac, 2*nv, nv);
@@ -464,7 +464,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz,
d->moment_rowadr, d->moment_colind);
mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu,
d->C_rownnz, d->C_rowadr, m->dof_simplenum, d->C_colind);
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
mju_transpose(BcT, Bc, nu, nv);
mju_scl(B, BcT, dt*dt, nu*nv);
mju_scl(B+nu*nv, BcT, dt, nu*nv);
test/engine/engine_support_test.cc
-1
View File
@@ -32,7 +32,6 @@ namespace {
using ::std::vector;
using ::testing::ContainsRegex; // NOLINT
using ::testing::DoubleNear;
using ::testing::ElementsAreArray;
using ::testing::Eq;
using ::testing::MatchesRegex;
using ::testing::NotNull;