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

Move island-specific sparse matrices from arena to stack.

Browse Source 
PiperOrigin-RevId: 923850345
Change-Id: I9683d7554b15b7cd8c45a8dce7814640aa266452
This commit is contained in:
Yuval Tassa
2026-05-30 03:07:31 -07:00
committed by Copybara-Service
parent 5d782a2bb8
commit 96bf8aea81
11 changed files with 122 additions and 402 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_island_test.cc
-133
View File
@@ -400,139 +400,6 @@ TEST_F(IslandTest, IslandFlex) {
mj_deleteModel(model);
}
static const char* const k2H100Path = "engine/testdata/island/2humanoid100.xml";
TEST_F(IslandTest, IslandJacobian) {
for (const char* local_path : {kIlslandEfcPath, k2H100Path}) {
const std::string xml_path = GetTestDataFilePath(local_path);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
int jac0 = m->opt.jacobian;
mjData* d = mj_makeData(m);
for (mjtNum t_stop : {0.0, 0.2, 2.0}) {
while (d->time < t_stop) {
mj_step(m, d);
}
for (mjtJacobian jac : {mjJAC_DENSE, mjJAC_SPARSE}) {
m->opt.jacobian = jac;
mj_forward(m, d);
int nv = m->nv;
int nefc = d->nefc;
int nisland = d->nisland;
int nidof = d->nidof;
mjtNum* J = (mjtNum*)mju_malloc(sizeof(mjtNum) * nefc * nv);
mjtNum* iJ = (mjtNum*)mju_malloc(sizeof(mjtNum) * nefc * nidof);
// get local dense Jacobian
if (jac == mjJAC_DENSE) {
mju_copy(J, d->efc_J, nefc * nv);
mju_copy(iJ, d->iefc_J, nefc * nidof);
} else {
mju_sparse2dense(J, d->efc_J, nefc, nv, d->efc_J_rownnz,
d->efc_J_rowadr, d->efc_J_colind);
}
// compare random access in efc_J to contiguous memory in iefc_J
for (int island=0; island < nisland; island++) {
int idof = d->island_idofadr[island];
int iefc = d->island_iefcadr[island];
int nefc_island = d->island_nefc[island];
int nv_island = d->island_nv[island];
// === test J
// get pointer to J_island, dense (nefc_island x nv_island) submatrix
mjtNum* J_island;
if (jac == mjJAC_DENSE) {
// point to starting address of island in efc_J
J_island = iJ + iefc * nidof;
} else {
// dense copy of island in iJ (here used as scratch)
mju_sparse2dense(iJ, d->iefc_J, nefc_island, nv_island,
d->iefc_J_rownnz + iefc,
d->iefc_J_rowadr + iefc,
d->iefc_J_colind);
J_island = iJ;
}
// sequential memory in J_island equals random access memory in J
for (int i=0; i < nefc_island; i++) {
for (int j=0; j < nv_island; j++) {
int efc = d->map_iefc2efc[iefc + i];
int dof = d->map_idof2dof[idof + j];
EXPECT_EQ(J_island[i * nv_island + j], J[efc * nv + dof]);
}
}
}
mju_free(iJ);
mju_free(J);
}
// reset opt.jacobian to initial value
m->opt.jacobian = jac0;
}
mj_deleteData(d);
mj_deleteModel(m);
}
}
TEST_F(IslandTest, IslandInertia) {
for (const char* local_path : {kIlslandEfcPath, k2H100Path}) {
const std::string xml_path = GetTestDataFilePath(local_path);
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
int nv = m->nv;
mjData* d = mj_makeData(m);
mjtNum* M = (mjtNum*)mju_malloc(sizeof(mjtNum) * nv * nv);
for (mjtNum t_stop : {0.0, 0.2, 2.0}) {
while (d->time < t_stop) {
mj_step(m, d);
}
mj_forward(m, d);
int nisland = d->nisland;
// get dense inertia (lower only)
mj_fullM(m, M, d->qM);
// compare iM sub-matrix to full M
for (int island=0; island < nisland; island++) {
int nvi = d->island_nv[island];
mjtNum* Mi = (mjtNum*)mju_malloc(sizeof(mjtNum) * nvi * nvi);
int adr = d->island_idofadr[island];
mju_sparse2dense(Mi, d->iM, nvi, nvi,
d->iM_rownnz + adr,
d->iM_rowadr + adr,
d->iM_colind);
// compare Mi to M (lower triangle only)
for (int i=0; i < nvi; i++) {
for (int j=0; j <= i; j++) {
int dofi = d->map_idof2dof[adr + j];
int dofj = d->map_idof2dof[adr + i];
EXPECT_EQ(Mi[i * nvi + j], M[dofi * nv + dofj]);
}
}
mju_free(Mi);
}
}
mju_free(M);
mj_deleteData(d);
mj_deleteModel(m);
}
}
TEST_F(IslandTest, IslandEfcElliptic) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
char error[1024];