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

Allow flex sleeping

Browse Source 
PiperOrigin-RevId: 917817500
Change-Id: Ia3bd5e52e7c2eaa3f70c81352d82c130b1d357f6
This commit is contained in:
Yuval Tassa
2026-05-19 07:14:45 -07:00
committed by Copybara-Service
parent ddc4c54d16
commit f712eed4ce
21 changed files with 660 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_sleep_test.cc
+225
View File
@@ -600,5 +600,230 @@ TEST_F(SleepTest, InitIslandFail) {
"the first tree that could not be slept."));
}
// Test that a constrained flex eventually goes to sleep.
TEST_F(SleepTest, FlexEdgeSleep) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/sleep/flex_edge.xml");
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
EXPECT_GT(m->flex_edgeequality[0], 0);
mjData* d = mj_makeData(m);
// give initial velocity
d->qvel[0] = 0.1;
// step until body goes to sleep
for (int step = 0; step < 2000; step++) {
mj_step(m, d);
if (d->ntree_awake == 0) break;
}
EXPECT_EQ(d->ntree_awake, 0) << "flex did not go to sleep";
mj_deleteData(d);
mj_deleteModel(m);
}
// Test that a constraint-free flex never sleeps.
TEST_F(SleepTest, FlexNoConstraintNeverSleeps) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/sleep/flex_nocnstr.xml");
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
EXPECT_EQ(m->flex_edgeequality[0], 0);
mjData* d = mj_makeData(m);
// step for a while
for (int step = 0; step < 500; step++) {
mj_step(m, d);
}
// tree should have AUTO_NEVER policy
EXPECT_EQ(m->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_GT(d->ntree_awake, 0) << "constraint-free flex should not sleep";
mj_deleteData(d);
mj_deleteModel(m);
}
// Test that mj_sleepState returns correct values for flex objects.
TEST_F(SleepTest, FlexSleepState) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/sleep/flex_state.xml");
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
// initially awake
EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, 0), mjS_AWAKE);
// step until flex goes to sleep
for (int step = 0; step < 1000; step++) {
mj_step(m, d);
if (d->ntree_awake == 0) break;
}
// flex should be asleep
EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, 0), mjS_ASLEEP);
mj_deleteData(d);
mj_deleteModel(m);
}
// Test that a sleeping flex wakes on contact with a falling ball,
// then both go back to sleep after the ball rolls off.
TEST_F(SleepTest, FlexWakeContact) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/sleep/flex_contact.xml");
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
int flexid = mj_name2id(m, mjOBJ_FLEX, "f1");
ASSERT_GE(flexid, 0);
// phase 1: flex settles and goes to sleep
for (int step = 0; step < 5000; step++) {
mj_step(m, d);
if (mj_sleepState(m, d, mjOBJ_FLEX, flexid) == mjS_ASLEEP) break;
}
ASSERT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, flexid), mjS_ASLEEP)
<< "flex did not go to sleep";
// phase 2: ball hits flex, flex wakes up
for (int step = 0; step < 5000; step++) {
mj_step(m, d);
if (mj_sleepState(m, d, mjOBJ_FLEX, flexid) == mjS_AWAKE) break;
}
EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, flexid), mjS_AWAKE)
<< "flex should have been woken by ball contact";
// phase 3: ball rolls off, everything goes back to sleep
for (int step = 0; step < 10000; step++) {
mj_step(m, d);
if (d->ntree_awake == 0) break;
}
EXPECT_EQ(d->ntree_awake, 0) << "all trees should be asleep again";
mj_deleteData(d);
mj_deleteModel(m);
}
// Test full sleep/wake lifecycle with two grippers and two flex objects.
TEST_F(SleepTest, HollowVsSolidSleep) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/sleep/hollow_vs_solid.xml");
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
// look up flex IDs
int solid_flex = mj_name2id(m, mjOBJ_FLEX, "soft_mesh");
int hollow_flex = mj_name2id(m, mjOBJ_FLEX, "soft_mesh_2");
ASSERT_GE(solid_flex, 0);
ASSERT_GE(hollow_flex, 0);
// look up actuator IDs
int grasp_r = mj_name2id(m, mjOBJ_ACTUATOR, "grasp_r");
int grasp_s = mj_name2id(m, mjOBJ_ACTUATOR, "grasp_s");
ASSERT_GE(grasp_r, 0);
ASSERT_GE(grasp_s, 0);
// phase 1: everything starts awake
mj_forward(m, d);
EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, solid_flex), mjS_AWAKE);
EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex), mjS_AWAKE);
// phase 2: both flexes go to sleep within 500 steps
for (int step = 0; step < 500; step++) {
mj_step(m, d);
}
ASSERT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, solid_flex), mjS_ASLEEP)
<< "solid flex did not go to sleep";
ASSERT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex), mjS_ASLEEP)
<< "hollow flex did not go to sleep";
// phase 3: close gripper_solid (grasp_r), it wakes soft_mesh_2 (hollow)
d->ctrl[grasp_r] = 1;
for (int step = 0; step < 2000; step++) {
mj_step(m, d);
if (mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex) == mjS_AWAKE) break;
}
EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex), mjS_AWAKE)
<< "hollow flex should be woken by gripper_solid";
EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, solid_flex), mjS_ASLEEP)
<< "solid flex should still be asleep";
// phase 4: close gripper_hollow (grasp_s), it wakes soft_mesh (solid)
d->ctrl[grasp_s] = 1;
for (int step = 0; step < 2000; step++) {
mj_step(m, d);
if (mj_sleepState(m, d, mjOBJ_FLEX, solid_flex) == mjS_AWAKE) break;
}
EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, solid_flex), mjS_AWAKE)
<< "solid flex should be woken by gripper_hollow";
// phase 5: open gripper_solid (grasp_r=0), hollow flex goes back to sleep
d->ctrl[grasp_r] = 0;
for (int step = 0; step < 2000; step++) {
mj_step(m, d);
if (mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex) == mjS_ASLEEP) break;
}
EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, hollow_flex), mjS_ASLEEP)
<< "hollow flex should go back to sleep after gripper release";
// phase 6: open gripper_hollow (grasp_s=0), solid flex goes back to sleep
d->ctrl[grasp_s] = 0;
for (int step = 0; step < 2000; step++) {
mj_step(m, d);
if (mj_sleepState(m, d, mjOBJ_FLEX, solid_flex) == mjS_ASLEEP) break;
}
EXPECT_EQ(mj_sleepState(m, d, mjOBJ_FLEX, solid_flex), mjS_ASLEEP)
<< "solid flex should go back to sleep after gripper release";
mj_deleteData(d);
mj_deleteModel(m);
}
// Test that a sleeping flex touching a non-world static body doesn't crash.
TEST_F(SleepTest, FlexStaticContact) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/sleep/flex_static.xml");
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
// step until flex goes to sleep, should not throw
for (int step = 0; step < 1000; step++) {
mj_step(m, d);
}
mj_deleteData(d);
mj_deleteModel(m);
}
// Test that a sleeping flex near a mocap body doesn't crash.
TEST_F(SleepTest, FlexMocapContact) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/sleep/flex_mocap.xml");
char error[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
for (int step = 0; step < 200; step++) {
mj_step(m, d);
}
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco