Fix bug related to sleeping static children of mocap bodies.

PiperOrigin-RevId: 831376685
Change-Id: I8891a1d7dc6efc037e9ade5c7e8dd5c35c7e455e

src/engine/engine_sleep.c

@@ -37,6 +37,7 @@ void mj_updateSleepInit(const mjModel* m, mjData* d, int flg_staticawake) {
   const int* tree_asleep   = d->tree_asleep;  // sleep state source of truth
   const int* body_treeid   = m->body_treeid;
   const int* body_parentid = m->body_parentid;
+  const int* body_rootid   = m->body_rootid;
   const int* body_mocapid  = m->body_mocapid;
   const int* dof_bodyid    = m->dof_bodyid;
 
@@ -61,7 +62,7 @@ void mj_updateSleepInit(const mjModel* m, mjData* d, int flg_staticawake) {
   for (int i=0; i < nbody; i++) {
     // static body
     if (body_treeid[i] < 0) {
-      if (body_mocapid[i] >= 0) {
+      if (body_mocapid[body_rootid[i]] >= 0) {
         // mocap body are always awake
         body_awake[i] = mjS_AWAKE;
       } else {

test/engine/engine_sleep_test.cc

@@ -222,6 +222,7 @@ TEST_F(SleepTest, BadWakeTree) {
 }
 
 static const char* const kStaticModel = "engine/testdata/sleep/static.xml";
+static const char* const kMocapcModel = "engine/testdata/sleep/mocap.xml";
 static const char* const kSmoothModel = "engine/testdata/sleep/smooth.xml";
 static const char* const kInitModel = "engine/testdata/sleep/init.xml";
 static const char* const kInitIslandModel =
@@ -234,8 +235,8 @@ static const char* const kSensorModel = "engine/testdata/sleep/sensor.xml";
 // roll out some models with sleeping enabled, valuable under ASAN and MSAN
 TEST_F(SleepTest, KickTires) {
   for (const char* path :
-       {kStaticModel, kInitModel, kInitIslandModel, kSensorModel, kTendonModel,
+       {kStaticModel, kMocapcModel, kInitModel, kInitIslandModel, kSensorModel,
-        kContactModel, kPairModel, kSmoothModel}) {
+        kTendonModel, kContactModel, kPairModel, kSmoothModel}) {
     const std::string xml_path = GetTestDataFilePath(path);
     char error[1024];
     mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
@@ -368,8 +369,9 @@ TEST_F(SleepTest, WakingUnaffectedBySleeping) {
 // unchanged for all trees and frc/acc-dependent arrays to be the same for awake
 // trees.
 TEST_F(SleepTest, SleepingUnaffectedByWaking) {
-  for (const char* path : {kInitModel, kInitIslandModel, kTendonModel,
+  for (const char* path :
-                           kContactModel, kSensorModel, kSmoothModel}) {
+       {kInitModel, kMocapcModel, kInitIslandModel, kTendonModel, kContactModel,
+        kSensorModel, kSmoothModel}) {
     const std::string xml_path = GetTestDataFilePath(path);
     char error[1024];
     mjModel* m = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));

test/engine/testdata/sleep/mocap.xml

@@ -0,0 +1,28 @@
+<mujoco>
+  <custom>
+    <numeric name="duration" data="0.1"/>
+  </custom>
+
+  <option>
+    <flag sleep="enable"/>
+  </option>
+
+  <worldbody>
+    <geom size="1"/>
+
+    <body name="mocap_static" pos="-5 0 0.01" mocap="true">
+      <geom size="1" rgba="1 0 0 1"/>
+      <body pos="0 0 1">
+        <geom size="1" rgba="0 0 1 1"/>
+      </body>
+    </body>
+
+    <body name="mocap_dynamic" pos="5 0 0.01" mocap="true">
+      <geom size="1" rgba="1 0 0 1"/>
+      <body pos="0 0 2">
+        <joint type="slide"/>
+        <geom size="1" rgba="0 0 1 1"/>
+      </body>
+    </body>
+  </worldbody>
+</mujoco>