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

Refactor mj_fullM. This change is part of the deprecation of mjData.qM.

Browse Source 
PiperOrigin-RevId: 925669464
Change-Id: I4889c66591bc1df4c31135a13776052aad491f7a
This commit is contained in:
Yuval Tassa
2026-06-02 17:22:15 -07:00
committed by Copybara-Service
parent 49211a05c5
commit 7b9b88060e
15 changed files with 49 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_core_smooth_test.cc
+19 -4
View File
@@ -233,13 +233,13 @@ TEST_F(CoreSmoothTest, TendonArmature) {
// get full M, includes both CRB and tendon inertia
vector<mjtNum> M(nv*nv);
mj_fullM(m, M.data(), d->qM);
mj_fullM(m, d, M.data());
// put only CRB inertia in M2
mj_crb(m, d);
mju_scatter(d->qM, d->M, m->mapM2M, m->nC);
vector<mjtNum> M2(nv*nv);
mj_fullM(m, M2.data(), d->qM);
mj_fullM(m, d, M2.data());
vector<mjtNum> ten_J(nv); // tendon Jacobian
vector<mjtNum> ten_M(nv*nv); // tendon inertia
@@ -681,7 +681,7 @@ TEST_F(CoreSmoothTest, FactorI) {
// dense M matrix
vector<mjtNum> Mexpected(nv*nv);
mj_fullM(model, Mexpected.data(), data->qM);
mj_fullM(model, data, Mexpected.data());
// expect matrices to match to floating point precision
EXPECT_THAT(M, Pointwise(MjNear(1e-12, 1e-5), Mexpected));
@@ -690,6 +690,21 @@ 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) {
const std::string xml_path = GetTestDataFilePath(kInertiaPath);
char error[1024];
@@ -712,7 +727,7 @@ TEST_F(CoreSmoothTest, SolveLDs) {
mju_sparse2dense(LDdense.data(), d->qLD, nv, nv,
m->M_rownnz, m->M_rowadr, m->M_colind);
vector<mjtNum> LDdense2(nv*nv);
mj_fullM(m, LDdense2.data(), LDlegacy.data());
legacyToDense(m, LDdense2.data(), LDlegacy.data());
// expect lower triangles to match exactly
for (int i=0; i < nv; i++) {
test/engine/engine_derivative_test.cc
+1 -1
View File
@@ -848,7 +848,7 @@ TEST_F(DerivativeTest, LinearSystemInverse) {
// expect that acceleration derivatives are the mass matrix
vector<mjtNum> DfDa_expect(nv*nv, 0);
mj_fullM(model, DfDa_expect.data(), data->qM);
mj_fullM(model, data, DfDa_expect.data());
EXPECT_THAT(DfDa, Pointwise(DoubleNear(eps), DfDa_expect));
// expect that sensor derivatives w.r.t position only see sensor 1 at dof 0
test/engine/engine_support_test.cc
+3 -3
View File
@@ -608,13 +608,13 @@ TEST_F(InertiaTest, FullM) {
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
int nv = m->nv;
// forward dynamics, populate qM and qLD
// forward dynamics, populate M and qLD
mjData* d = mj_makeData(m);
mj_forward(m, d);
// get dense mass matrix from M using mju_sym2dense
// get dense mass matrix from M using mj_fullM
vector<mjtNum> M(nv * nv);
mju_sym2dense(M.data(), d->M, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
mj_fullM(m, d, M.data());
// get dense mass matrix from M using mju_sparse2dense
vector<mjtNum> M_CSR(nv * nv);