From b8512b9d1fa3033f732f43036e9e7f00654f97de Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 12 Nov 2025 07:19:08 -0800 Subject: [PATCH] Fix bug related to sleeping static children of mocap bodies. PiperOrigin-RevId: 831376685 Change-Id: I8891a1d7dc6efc037e9ade5c7e8dd5c35c7e455e --- src/engine/engine_sleep.c | 3 ++- test/engine/engine_sleep_test.cc | 10 ++++++---- test/engine/testdata/sleep/mocap.xml | 28 ++++++++++++++++++++++++++++ 3 files changed, 36 insertions(+), 5 deletions(-) create mode 100644 test/engine/testdata/sleep/mocap.xml diff --git a/src/engine/engine_sleep.c b/src/engine/engine_sleep.c index acaf820e..5c235901 100644 --- a/src/engine/engine_sleep.c +++ b/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 { diff --git a/test/engine/engine_sleep_test.cc b/test/engine/engine_sleep_test.cc index 5d3e6164..88067eff 100644 --- a/test/engine/engine_sleep_test.cc +++ b/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, - kContactModel, kPairModel, kSmoothModel}) { + {kStaticModel, kMocapcModel, kInitModel, kInitIslandModel, kSensorModel, + 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, - kContactModel, kSensorModel, kSmoothModel}) { + for (const char* path : + {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)); diff --git a/test/engine/testdata/sleep/mocap.xml b/test/engine/testdata/sleep/mocap.xml new file mode 100644 index 00000000..61d55394 --- /dev/null +++ b/test/engine/testdata/sleep/mocap.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + +