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

Refactor islands to be memory contiguous.

Browse Source 
PiperOrigin-RevId: 755803476
Change-Id: I41972b07e0d5ef5d0117c94f565b93367b87458b
This commit is contained in:
Yuval Tassa
2025-05-07 05:05:34 -07:00
committed by Copybara-Service
parent 449de73430
commit ecb769fc3a
30 changed files with 1742 additions and 1116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_support_test.cc
-71
View File
@@ -830,77 +830,6 @@ TEST_F(InertiaTest, mulM2) {
mj_deleteModel(model);
}
static const char* const kIlslandEfcPath =
"engine/testdata/island/island_efc.xml";
TEST_F(SupportTest, MulMIsland) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
// allocate vec, fill with arbitrary values
mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nv);
for (int i=0; i < model->nv; i++) {
vec[i] = 0.2 + 0.3*i;
}
// simulate for 0.2 seconds
mj_resetData(model, data);
while (data->time < 0.2) {
mj_step(model, data);
}
mj_forward(model, data);
// multiply by Mass matrix: Mvec = M * vec
mjtNum* Mvec = (mjtNum*) mju_malloc(sizeof(mjtNum)*data->nefc);
mj_mulM(model, data, Mvec, vec);
// iterate over islands
for (int i=0; i < data->nisland; i++) {
// allocate dof vectors for island
int dofnum = data->island_dofnum[i];
mjtNum* vec_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum);
mjtNum* Mvec_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum);
// copy values into vec_i
int* dofind = data->island_dofind + data->island_dofadr[i];
for (int j=0; j < dofnum; j++) {
vec_i[j] = vec[dofind[j]];
}
// === compressed: use vec_i
// multiply by Jacobian, for this island
int flg_vecunc = 0;
mj_mulM_island(model, data, Mvec_i, vec_i, i, flg_vecunc);
// expect corresponding values to match
for (int j=0; j < dofnum; j++) {
EXPECT_THAT(Mvec_i[j], DoubleNear(Mvec[dofind[j]], 1e-12));
}
// === uncompressed: use vec
mju_zero(Mvec_i, dofnum); // clear output
// multiply by Jacobian, for this island
flg_vecunc = 1;
mj_mulM_island(model, data, Mvec_i, vec, i, flg_vecunc);
// expect corresponding values to match
for (int j=0; j < dofnum; j++) {
EXPECT_THAT(Mvec_i[j], DoubleNear(Mvec[dofind[j]], 1e-12));
}
mju_free(vec_i);
mju_free(Mvec_i);
}
mju_free(Mvec);
mju_free(vec);
mj_deleteData(data);
mj_deleteModel(model);
}
static constexpr char GeomDistanceTestingModel[] = R"(
<mujoco>
<option>