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

Fix bugs in engine_island.c

Browse Source 
PiperOrigin-RevId: 562519702
Change-Id: I3075e539ccc1cef92485152721a9344f180aa4b1
This commit is contained in:
Yuval Tassa
2023-09-04 04:05:46 -07:00
committed by Copybara-Service
parent 89f47c33e4
commit feb8aa2fbb
3 changed files with 145 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_island_test.cc
+24 -24
View File
@@ -350,33 +350,33 @@ TEST_F(IslandTest, IslandEfc) {
mj_step(model, data);
}
// sizes
int nv = model->nv;
int nefc = data->nefc;
int nisland = data->nisland;
// expect island structure to correspond to comment at top of xml
EXPECT_EQ(nisland, 3);
EXPECT_EQ(data->ne, 1);
EXPECT_EQ(data->nisland, 4);
EXPECT_EQ(data->ne, 4);
EXPECT_EQ(data->nf, 2);
EXPECT_EQ(data->nl, 1);
EXPECT_EQ(nefc, 24);
EXPECT_THAT(AsVector(data->dof_island, nv),
ElementsAre(0, -1, 0, 0, 0, 1, 2, 0, 1, 1, 1, 1, 1, 1));
EXPECT_THAT(AsVector(data->island_dofnum, nisland), ElementsAre(5, 7, 1));
EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 5, 12));
EXPECT_THAT(AsVector(data->island_dofind, nv),
ElementsAre(0, 2, 3, 4, 7, 5, 8, 9, 10, 11, 12, 13, 6, -1));
EXPECT_THAT(AsVector(data->dof_islandind, nv),
ElementsAre(0, -1, 1, 2, 3, 0, 0, 4, 1, 2, 3, 4, 5, 6));
EXPECT_THAT(AsVector(data->efc_island, nefc),
ElementsAre(0, 1, 2, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1));
EXPECT_THAT(AsVector(data->island_efcnum, nisland), ElementsAre(6, 17, 1));
EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 6, 23));
EXPECT_THAT(AsVector(data->island_efcind, nefc),
ElementsAre(0, 3, 4, 5, 6, 7, 1, 8, 9, 10, 11, 12, 13, 14, 15, 16,
17, 18, 19, 20, 21, 22, 23, 2));
EXPECT_EQ(data->nefc, 27);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(IslandTest, IslandEfcElliptic) {
const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
model->opt.cone = mjCONE_ELLIPTIC;
while (data->time < 0.2) {
mj_step(model, data);
}
mj_forward(model, data);
EXPECT_EQ(data->nisland, 4);
EXPECT_EQ(data->ne, 4);
EXPECT_EQ(data->nf, 2);
EXPECT_EQ(data->nl, 1);
EXPECT_EQ(data->nefc, 22);
mj_deleteData(data);
mj_deleteModel(model);
test/engine/testdata/island/island_efc.xml
Vendored
+8 -2
View File
@@ -1,10 +1,10 @@
<!--
test model for constraint islanding:
- 1 equality
- 4 equalities (1:joint, 3:connect)
- 1 limit
- 2 friction constraints
- 1 unconstrained dof
- 3 islands, 2 with non-contiguous dofs
- 4 islands, 2 with non-contiguous dofs
-->
<mujoco>
@@ -64,9 +64,15 @@
<freejoint/>
<geom type="box" size=".03 .03 .03" pos="0.01 0.01 0.01"/>
</body>
<body pos="-.5 0 .3" name="connect">
<freejoint/>
<geom type="box" size=".05 .05 .05"/>
</body>
</worldbody>
<equality>
<joint joint1="hinge1" joint2="hinge2"/>
<connect body1="connect" body2="world" anchor="-.05 -.05 .05"/>
</equality>
</mujoco>