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

Switch mj_solveM_island to use CSR representation

Browse Source 
PiperOrigin-RevId: 717684720
Change-Id: I7d74a5d4aef4aa5b0c0acddf88c5b8591d63e88d
This commit is contained in:
Yuval Tassa
2025-01-20 18:34:58 -08:00
committed by Copybara-Service
parent a5ab7a9515
commit 2624d524ba
4 changed files with 80 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_core_smooth_test.cc
+46 -41
View File
@@ -353,58 +353,63 @@ TEST_F(CoreSmoothTest, RefsiteConservesMomentum) {
static const char* const kIlslandEfcPath =
"engine/testdata/island/island_efc.xml";
static const char* const kModelPath =
"testdata/model.xml";
TEST_F(CoreSmoothTest, SolveMIsland) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
int nv = model->nv;
for (auto model_path : {kModelPath, kIlslandEfcPath}) {
const std::string xml_path = GetTestDataFilePath(model_path);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
int nv = model->nv;
// allocate vec, fill with arbitrary values, copy to sol
mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv);
mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv);
for (int i=0; i < nv; i++) {
vec[i] = 0.2 + 0.3*i;
}
mju_copy(res, vec, nv);
// allocate vec, fill with arbitrary values, copy to sol
mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv);
mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv);
for (int i=0; i < nv; i++) {
vec[i] = 0.2 + 0.3*i;
}
mju_copy(res, vec, nv);
// simulate for 0.2 seconds
mj_resetData(model, data);
while (data->time < 0.2) {
mj_step(model, data);
}
mj_forward(model, data);
if (model->nkey > 0) mj_resetDataKeyframe(model, data, 0);
// divide by mass matrix: sol = M^-1 * vec
mj_solveM(model, data, res, res, 1);
// iterate over islands
for (int i=0; i < data->nisland; i++) {
// allocate dof vectors for island
int dofnum = data->island_dofnum[i];
mjtNum* res_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum);
// copy values into sol_i
int* dofind = data->island_dofind + data->island_dofadr[i];
for (int j=0; j < dofnum; j++) {
res_i[j] = vec[dofind[j]];
for (int i=0; i < 6; i++) {
mj_step(model, data);
}
// divide by mass matrix, for this island
mj_solveM_island(model, data, res_i, i);
mj_forward(model, data);
// expect corresponding values to match
for (int j=0; j < dofnum; j++) {
EXPECT_THAT(res_i[j], DoubleNear(res[dofind[j]], 1e-12));
// divide by mass matrix: sol = M^-1 * vec
mj_solveM(model, data, res, res, 1);
// iterate over islands
for (int i=0; i < data->nisland; i++) {
// allocate dof vectors for island
int dofnum = data->island_dofnum[i];
mjtNum* res_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum);
// copy values into sol_i
int* dofind = data->island_dofind + data->island_dofadr[i];
for (int j=0; j < dofnum; j++) {
res_i[j] = vec[dofind[j]];
}
// divide by mass matrix, for this island
mj_solveM_island(model, data, res_i, i);
// expect corresponding values to match
for (int j=0; j < dofnum; j++) {
EXPECT_THAT(res_i[j], DoubleNear(res[dofind[j]], 1e-14));
}
mju_free(res_i);
}
mju_free(res_i);
mju_free(res);
mju_free(vec);
mj_deleteData(data);
mj_deleteModel(model);
}
mju_free(res);
mju_free(vec);
mj_deleteData(data);
mj_deleteModel(model);
}
static const char* const kInertiaPath = "engine/testdata/inertia.xml";