From 22c0b689033682bb0d085d8d8493b88f41680058 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 11 Nov 2025 06:44:53 -0800 Subject: [PATCH] Improve logic and documentation for sleeping sensors PiperOrigin-RevId: 830891115 Change-Id: Ife8309b165c085b54c9f668d825d69ed12a9a8f0 --- doc/programming/simulation.rst | 25 +++++++++---- src/engine/engine_sleep.c | 54 +++++++++++++-------------- src/engine/engine_sleep.h | 2 +- test/engine/engine_sleep_test.cc | 23 +++++++++++- test/engine/testdata/sleep/sensor.xml | 43 ++++++++++++++++----- 5 files changed, 99 insertions(+), 48 deletions(-) diff --git a/doc/programming/simulation.rst b/doc/programming/simulation.rst index aa4fec64..0b35a16d 100644 --- a/doc/programming/simulation.rst +++ b/doc/programming/simulation.rst @@ -1018,13 +1018,24 @@ Notes associated velocities or forces, as described above. **Sleeping sensors** - The computation of sensor values are skipped if the objects associated with it are asleep or static, so the last - valid computed values remain untouched. This is straightforward for most sensors, but :ref:`contact` - sensors are an exception. Because contact sensors report contacts that occurred in the current timestep, and - sleeping implies that some contacts are not computed, skipping the sensor computation is not always possible. - For example, consider a scene with free bodies on the floor, some of which may be asleep. A sensor which reports - information from contacts of body A (with anything else), will keep reporting the same thing when the body goes to - sleep. However a sensor that reports all world contacts will report something different when some bodies go to sleep. + For most sensors, we can skip the computation of their values when their associated objects are asleep, reporting the + value that was computed when those objects were last awake. Some sensors are always awake, but disabling sleep will + not affect their computed values: + + - :ref:`rangefinder` sensors are always awake; the sleep state of the site they are attached to + is not relevant to the reported value. + - :ref:`clock` sensors are always awake (no associated object). + - :ref:`user` and :ref:`plugin` sensors are always awake. + + Some sensors are always awake, yet disabling sleep may affect their computed value. These are sensors that explicitly + depend on the presence of contacts, yet the contacts that were present when they were last awake are not sufficient + to determine their current value: + + - :ref:`contact` sensors that have no object specifier (match all contacts). + - :ref:`contact` sensors whose only object specifier is static. + - :ref:`contact` sensors that use the :at:`site` attribute. + - :ref:`force` or :ref:`torque` sensors attached to a static body (e.g., a weight sensor + on the floor). **Provisional choices** Some implementation choices are provisional and subject to change. diff --git a/src/engine/engine_sleep.c b/src/engine/engine_sleep.c index 0c8d55a6..acaf820e 100644 --- a/src/engine/engine_sleep.c +++ b/src/engine/engine_sleep.c @@ -657,54 +657,29 @@ static mjtSleepState mj_equalitySleepState(const mjModel* m, const mjData* d, in } -// return sleep state of sensor i +// return sleep state of sensor i (AWAKE or ASLEEP, never STATIC) static mjtSleepState mj_sensorSleepState(const mjModel* m, const mjData* d, int i) { mjtSensor type = m->sensor_type[i]; mjtObj objtype = m->sensor_objtype[i]; - int objid = m->sensor_objid[i]; mjtObj reftype = m->sensor_reftype[i]; - int refid = m->sensor_refid[i]; - - // get sleep state of the primary and reference objects - mjtSleepState s_obj = mj_sleepState(m, d, objtype, objid); - mjtSleepState s_ref = mj_sleepState(m, d, reftype, refid); // special handling for specific sensor types switch (type) { - // USER and PLUGIN sensors are always awake + // USER and PLUGIN sensors: always awake case mjSENS_USER: case mjSENS_PLUGIN: return mjS_AWAKE; - // sensors that use sites to define a volume are always awake - case mjSENS_INSIDESITE: - case mjSENS_TOUCH: - return mjS_AWAKE; - - // contact sensors + // contact sensors with site specifiers: always awake case mjSENS_CONTACT: // site used to define a volume: always awake if (objtype == mjOBJ_SITE || reftype == mjOBJ_SITE) { return mjS_AWAKE; } - - // for contact sensors UNKNOWN means undefined, so the AWAKE returned by mj_sleepState is wrong - - // if both are UNKNOWN (all contacts), return ASLEEP iff everything is alseep - if (objtype == mjOBJ_UNKNOWN && reftype == mjOBJ_UNKNOWN) { - return d->ntree_awake == 0 ? mjS_ASLEEP : mjS_AWAKE; - } - - // if only one is UNKNOWN, return state of other object - if (objtype == mjOBJ_UNKNOWN) { - return s_ref; - } else if (reftype == mjOBJ_UNKNOWN) { - return s_obj; - } break; - // sensors whose value depends on objects other than the two they are attached to are always awake + // rangefinder output does not depend on sleep state: always awake case mjSENS_RANGEFINDER: return mjS_AWAKE; @@ -712,6 +687,24 @@ static mjtSleepState mj_sensorSleepState(const mjModel* m, const mjData* d, int break; } + // get sleep state of the primary and reference objects + mjtSleepState s_obj = mj_sleepState(m, d, objtype, m->sensor_objid[i]); + mjtSleepState s_ref = mj_sleepState(m, d, reftype, m->sensor_refid[i]); + + // special handling for UNKNOWN objects + + // if both are UNKNOWN, return AWAKE + if (objtype == mjOBJ_UNKNOWN && reftype == mjOBJ_UNKNOWN) { + return mjS_AWAKE; + } + + // if one is UNKNOWN, return the other's sleep state (if STATIC, return AWAKE) + if (objtype == mjOBJ_UNKNOWN) { + return s_ref == mjS_ASLEEP ? mjS_ASLEEP : mjS_AWAKE; + } else if (reftype == mjOBJ_UNKNOWN) { + return s_obj == mjS_ASLEEP ? mjS_ASLEEP : mjS_AWAKE; + } + // if either object is awake, return AWAKE if (s_obj == mjS_AWAKE || s_ref == mjS_AWAKE) { return mjS_AWAKE; @@ -757,6 +750,9 @@ mjtSleepState mj_sleepState(const mjModel* m, const mjData* d, mjtObj type, int // always awake case mjOBJ_FLEX: + return mjS_AWAKE; + + // undefined sleep state, return AWAKE case mjOBJ_UNKNOWN: return mjS_AWAKE; diff --git a/src/engine/engine_sleep.h b/src/engine/engine_sleep.h index 6bb65616..4908d7c3 100644 --- a/src/engine/engine_sleep.h +++ b/src/engine/engine_sleep.h @@ -54,7 +54,7 @@ int mj_wakeEquality(const mjModel* m, mjData* d); int mj_sleep(const mjModel* m, mjData* d); // return sleep state of object i -mjtSleepState mj_sleepState(const mjModel* m, const mjData* d, mjtObj type, int i); +MJAPI mjtSleepState mj_sleepState(const mjModel* m, const mjData* d, mjtObj type, int i); #ifdef __cplusplus } diff --git a/test/engine/engine_sleep_test.cc b/test/engine/engine_sleep_test.cc index 2a7c1ad2..5d3e6164 100644 --- a/test/engine/engine_sleep_test.cc +++ b/test/engine/engine_sleep_test.cc @@ -15,6 +15,7 @@ // Tests for engine/engine_sleep.c. #include +#include #include #include @@ -32,6 +33,7 @@ using ::testing::IsNull; using ::testing::HasSubstr; using ::testing::NotNull; using ::std::string; +using ::std::vector; using SleepTest = MujocoTest; @@ -375,6 +377,9 @@ TEST_F(SleepTest, SleepingUnaffectedByWaking) { const int kCompare = 10; // number of comparisons per rollout + // for some sensors, the comparison is expected to fail (at least once) + vector sensor_mismatch(m->nsensor, false); + // TODO: b/457674312 - Add support for RK4. for (mjtIntegrator integrator : {mjINT_EULER, mjINT_IMPLICITFAST, mjINT_IMPLICIT}) { @@ -464,8 +469,15 @@ TEST_F(SleepTest, SleepingUnaffectedByWaking) { int adr = m->sensor_adr[i]; auto data1 = AsVector(d_sleep->sensordata + adr, dim); auto data2 = AsVector(d_nosleep->sensordata + adr, dim); - EXPECT_EQ(data1, data2) - << " sensor " << i << " at time " << d_sleep->time; + if (m->nuser_sensor == 1 && m->sensor_user[i] == 1) { + // user=1 means sensor value cannot be determined at sleep time + sensor_mismatch[i] = sensor_mismatch[i] || (data1 != data2); + EXPECT_EQ(mj_sleepState(m, d_sleep, mjOBJ_SENSOR, i), mjS_AWAKE); + } else { + // otherwise expect perfect match + EXPECT_EQ(data1, data2) + << " sensor " << i << " at time " << d_sleep->time; + } } // ==== compare arrays for awake dofs only ==== @@ -480,6 +492,13 @@ TEST_F(SleepTest, SleepingUnaffectedByWaking) { } } + for (int i = 0; i < m->nsensor; i++) { + if (m->nuser_sensor == 1 && m->sensor_user[i] == 1) { + EXPECT_TRUE(sensor_mismatch[i]) + << "contact sensor " << i << " comparison was expected to fail"; + } + } + mj_deleteData(d_nosleep); mj_deleteData(d_sleep); } diff --git a/test/engine/testdata/sleep/sensor.xml b/test/engine/testdata/sleep/sensor.xml index 40ab01bc..1065780b 100644 --- a/test/engine/testdata/sleep/sensor.xml +++ b/test/engine/testdata/sleep/sensor.xml @@ -10,30 +10,55 @@ + - - - + + + - + + + + + + + + + + - + + - - - - + + + + + + + + + + + + + + + + + +