Fix bug related to sleeping static children of mocap bodies.

PiperOrigin-RevId: 831376685 Change-Id: I8891a1d7dc6efc037e9ade5c7e8dd5c35c7e455e
2025-11-12 07:19:08 -08:00
parent 7b52d3034e
commit b8512b9d1f
3 changed files with 36 additions and 5 deletions
@@ -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 {