Allow flex sleeping

PiperOrigin-RevId: 917817500 Change-Id: Ia3bd5e52e7c2eaa3f70c81352d82c130b1d357f6
2026-05-19 07:14:45 -07:00
parent ddc4c54d16
commit f712eed4ce
21 changed files with 660 additions and 57 deletions
@@ -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
@@ -0,0 +1,23 @@
+<mujoco>
+  <option integrator="implicitfast" viscosity="2" sleep_tolerance="0.1">
+    <flag sleep="enable"/>
+  </option>
+
+  <worldbody>
+    <geom type="plane" size="5 5 .01"/>
+
+    <!-- constrained flex on the floor, will go to sleep quickly -->
+    <body name="flex_body" pos="0 0 0.01">
+      <flexcomp name="f1" type="grid" dim="1" count="3 1 1"
+                spacing=".05 .05 .05" radius=".01" mass="1">
+        <edge equality="true" solref="0.15 0.2"/>
+      </flexcomp>
+    </body>
+
+    <!-- ball that will collide with the flex -->
+    <body name="ball" pos="0 0.01 0.5">
+      <freejoint/>
+      <geom type="sphere" size=".05"/>
+    </body>
+  </worldbody>
+</mujoco>
@@ -0,0 +1,21 @@
+<mujoco>
+  <custom>
+    <numeric name="duration" data="2"/>
+  </custom>
+
+  <option integrator="implicitfast" viscosity="10"
+          sleep_tolerance="0.01">
+    <flag sleep="enable" gravity="disable"/>
+  </option>
+
+  <worldbody>
+    <body name="flex_body">
+      <freejoint/>
+      <inertial pos="0 0 0" mass="1" diaginertia="0.01 0.01 0.01"/>
+      <flexcomp name="f1" type="grid" dim="1" count="3 1 1"
+                spacing=".05 .05 .05" radius=".01" mass="1">
+        <edge equality="true" solref="0.15 0.2" stiffness="0" damping="0"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+</mujoco>
@@ -0,0 +1,22 @@
+<mujoco>
+  <option integrator="implicitfast" viscosity="1" sleep_tolerance="1e-2">
+    <flag sleep="enable"/>
+  </option>
+
+  <worldbody>
+    <geom type="plane" size="5 5 .01"/>
+
+    <!-- mocap body with a sphere -->
+    <body name="mocap_obstacle" pos="0 0 -.02" mocap="true">
+      <geom type="sphere" size=".04"/>
+    </body>
+
+    <!-- constrained flex on the floor, will go to sleep -->
+    <body name="flex_body" pos="0 0 0.01">
+      <flexcomp name="f1" type="grid" dim="1" count="3 1 1"
+                spacing=".05 .05 .05" radius=".01" mass="1">
+        <edge equality="true" solref="0.15 0.2"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+</mujoco>
@@ -0,0 +1,15 @@
+<mujoco>
+  <option integrator="implicitfast" viscosity="10"
+          sleep_tolerance="0.01">
+    <flag sleep="enable" gravity="disable"/>
+  </option>
+
+  <worldbody>
+    <body name="flex_body">
+      <flexcomp name="f1" type="grid" dim="1" count="3 1 1"
+                spacing=".05 .05 .05" radius=".01" mass="1">
+        <edge equality="false" stiffness="100" damping="10"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+</mujoco>
@@ -0,0 +1,16 @@
+<mujoco>
+  <option>
+    <flag sleep="enable" constraint="disable" contact="disable" gravity="disable"/>
+  </option>
+
+  <worldbody>
+    <body name="flex_body">
+      <freejoint/>
+      <inertial pos="0 0 0" mass="1" diaginertia="0.01 0.01 0.01"/>
+      <flexcomp name="f1" type="grid" dim="1" count="3 1 1"
+                spacing=".05 .05 .05" radius=".01" mass="1">
+        <edge equality="true" solref="0.15 0.2"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+</mujoco>
@@ -0,0 +1,22 @@
+<mujoco>
+  <option integrator="implicitfast" viscosity="2" sleep_tolerance="0.1">
+    <flag sleep="enable"/>
+  </option>
+
+  <worldbody>
+    <geom type="plane" size="5 5 .01"/>
+
+    <!-- static body (no joint) with a sphere touching the flex -->
+    <body name="static_obstacle" pos="0 0 0.05">
+      <geom type="sphere" size=".04"/>
+    </body>
+
+    <!-- constrained flex on the floor, will go to sleep -->
+    <body name="flex_body" pos="0 0 0.01">
+      <flexcomp name="f1" type="grid" dim="1" count="3 1 1"
+                spacing=".05 .05 .05" radius=".01" mass="1">
+        <edge equality="true" solref="0.15 0.2"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+</mujoco>
@@ -0,0 +1,89 @@
+<mujoco>
+  <include file="scene.xml"/>
+
+  <option cone="elliptic" solver="CG" sleep_tolerance=".01">
+    <flag sleep="enable"/>
+  </option>
+
+  <default>
+    <default class="gripper">
+      <geom type="box" size="0.02 0.15 0.02" rgba="0.5 0.5 0.5 1"
+            solref="0.001 1" friction="2 0.1 0.1"/>
+      <joint type="slide" axis="0 0 1" range="-1 1"/>
+      <default class="finger">
+        <geom size="0.01 0.06 0.03"/>
+        <joint type="slide" range="0 0.1"/>
+      </default>
+    </default>
+  </default>
+
+  <worldbody>
+    <body name="gripper_solid" pos="0.2 0.45 0.2">
+      <joint name="lift_solid" class="gripper"/>
+      <geom class="gripper"/>
+      <body name="left_finger_r" pos="0.05 -0.1 0" childclass="finger">
+        <joint name="left_slide_r" axis="0 1 0"/>
+        <geom rgba="0.8 0.2 0.2 1"/>
+      </body>
+      <body name="right_finger_r" pos="0.05 0.1 0" childclass="finger">
+        <joint name="right_slide_r" axis="0 -1 0"/>
+        <geom rgba="0.8 0.2 0.2 1"/>
+      </body>
+    </body>
+
+    <body name="gripper_hollow" pos="0.2 0 0.2">
+      <joint name="lift_hollow" class="gripper"/>
+      <geom class="gripper"/>
+      <body name="left_finger_s" pos="0.05 -0.1 0" childclass="finger">
+        <joint name="left_slide_s" axis="0 1 0"/>
+        <geom rgba="0.2 0.8 0.2 1"/>
+      </body>
+      <body name="right_finger_s" pos="0.05 0.1 0" childclass="finger">
+        <joint name="right_slide_s" axis="0 -1 0"/>
+        <geom rgba="0.2 0.8 0.2 1"/>
+      </body>
+    </body>
+
+    <body name="solid" pos=".32 0 .2">
+      <flexcomp
+        origin="0 0 0" count="8 2 12" cellcount="2 1 2"
+        type="grid" name="soft_mesh" dim="3" spacing=".025 .05 .025"
+        mass="0.43" radius="0.005" dof="trilinear" rgba="0.7 0.9 0.7 1">
+        <edge equality="strain" solref="0.001 1"/>
+        <contact selfcollide="none"
+              internal="false"
+              friction="2.0 0.5 0.01"
+              solref="0.001 1"
+              solimp="0.99 0.999 0.001"/>
+      </flexcomp>
+    </body>
+
+    <body name="hollow" pos=".32 .45 .2">
+      <flexcomp
+        origin="0 0 0" count="8 2 12" cellcount="2 1 2"
+        type="grid" name="soft_mesh_2" dim="3" spacing=".025 .05 .025"
+        mass="0.43" radius="0.005" dof="trilinear" rgba="0.9 0.7 0.7 1">
+        <elasticity young="1e5" poisson="0.3" damping="0" elastic2d="bend" thickness="0.03"/>
+        <edge equality="strain" solref="0.001 1" solimp="0.99 0.999 0.001"/>
+        <contact selfcollide="none"
+              internal="false"
+              friction="2.0 0.5 0.01"
+              solref="0.001 1"
+              solimp="0.99 0.999 0.001"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+
+  <equality>
+    <joint joint1="right_slide_r" joint2="left_slide_r"/>
+    <joint joint1="right_slide_s" joint2="left_slide_s"/>
+  </equality>
+
+  <actuator>
+    <position name="lift_r" joint="lift_solid" kp="400" dampratio="1" ctrlrange="-1 1"/>
+    <position name="grasp_r" joint="left_slide_r" kp="10" dampratio="1" ctrlrange="0 5"/>
+
+    <position name="lift_s" joint="lift_hollow" kp="400" dampratio="1" ctrlrange="-1 1"/>
+    <position name="grasp_s" joint="left_slide_s" kp="10" dampratio="1" ctrlrange="0 5"/>
+  </actuator>
+</mujoco>
@@ -0,0 +1,26 @@
+<mujoco>
+  <compiler meshdir="asset" texturedir="asset"/>
+
+  <statistic meansize=".05"/>
+
+  <visual>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <quality shadowsize="4096"/>
+    <map stiffness="700" shadowscale="0.5" fogstart="1" fogend="15" zfar="40" haze="1"/>
+  </visual>
+
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="512"/>
+    <texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
+      width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
+
+    <material name="matplane" reflectance="0.3" texture="texplane" texrepeat="10 10" texuniform="true"/>
+  </asset>
+
+  <worldbody>
+    <light diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 2.0" dir="0 0 -1" castshadow="false"/>
+    <light directional="true" diffuse=".8 .8 .8" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
+
+    <geom name="ground" type="plane" size="0 0 1" pos="0 0 0" quat="1 0 0 0" material="matplane" condim="1"/>
+  </worldbody>
+</mujoco>