chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Improve logic and documentation for sleeping sensors

Browse Source 
PiperOrigin-RevId: 830891115
Change-Id: Ife8309b165c085b54c9f668d825d69ed12a9a8f0
This commit is contained in:
Yuval Tassa
2025-11-11 06:44:53 -08:00
committed by Copybara-Service
parent 3e167e0f0b
commit 22c0b68903
5 changed files with 99 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/programming/simulation.rst
+18 -7
View File
@@ -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<sensor-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<sensor-rangefinder>` sensors are always awake; the sleep state of the site they are attached to
is not relevant to the reported value.
- :ref:`clock<sensor-clock>` sensors are always awake (no associated object).
- :ref:`user<sensor-user>` and :ref:`plugin<sensor-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<sensor-contact>` sensors that have no object specifier (match all contacts).
- :ref:`contact<sensor-contact>` sensors whose only object specifier is static.
- :ref:`contact<sensor-contact>` sensors that use the :at:`site` attribute.
- :ref:`force<sensor-force>` or :ref:`torque<sensor-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.
src/engine/engine_sleep.c
+25 -29
View File
@@ -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;
src/engine/engine_sleep.h
+1 -1
View File
@@ -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
}
test/engine/engine_sleep_test.cc
+21 -2
View File
@@ -15,6 +15,7 @@
// Tests for engine/engine_sleep.c.
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
@@ -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<bool> 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);
}
test/engine/testdata/sleep/sensor.xml
Vendored
+34 -9
View File
@@ -10,30 +10,55 @@
<worldbody>
<light pos="0 -1 3"/>
<geom name="floor" type="plane" size="1 1 .01"/>
<site name="inside" type="box" size=".5 .5 .5" rgba="0 0 0 0"/>
<body name="1" pos="0 0 .1">
<freejoint/>
<geom name="1" type="box" size=".1 .1 .1"/>
<body name="force_static" pos="0 0 .025">
<geom name="0" type="box" size=".15 .15 .025"/>
<site name="force_static"/>
</body>
<body name="2" pos="0 0 .3">
<body name="1" pos="0 0 .15">
<freejoint/>
<geom name="1" type="box" size=".1 .1 .05" pos="0 0 .05"/>
<body name="force_dynamic" pos="0 0 -.05">
<geom name="1_bottom" type="box" size=".1 .1 .05"/>
<site name="force_dynamic"/>
</body>
</body>
<body name="2" pos="0 0 .35">
<freejoint/>
<geom name="2" type="box" size=".1 .1 .1"/>
<site name="2"/>
<site name="2" type="box" size=".11 .11 .11" rgba=".5 .7 .5 .2"/>
</body>
<body name="3" pos="0 0 1.2" euler="10 20 30">
<freejoint/>
<geom name="3" type="box" size=".1 .1 .1"/>
<site name="rf" type="cylinder" size=".01 .03" pos="0 0 -.11" zaxis="0 0 -1"/>
</body>
</worldbody>
<sensor>
<contact geom1="1" geom2="2"/>
<contact geom1="1" geom2="2" reduce="maxforce" data="force"/>
<contact geom1="1" geom2="floor"/>
<contact geom1="1" reduce="maxforce" data="force"/>
<!-- for most sensors, expect match between sleep enabled/disabled -->
<contact subtree1="1" geom2="2"/>
<contact subtree1="1" geom2="2" reduce="maxforce" data="force"/>
<contact subtree1="1" geom2="floor"/>
<contact subtree1="1"/>
<contact subtree1="1" reduce="maxforce" data="force"/>
<insidesite site="inside" objtype="body" objname="3"/>
<framelinvel objtype="body" objname="3"/>
<framelinvel objtype="body" objname="3" reftype="body" refname="2"/>
<gyro site="2"/>
<touch site="2"/>
<clock/>
<rangefinder site="rf"/>
<force site="force_dynamic"/>
<!-- sensors marked with user="1": expect mismatch between sleep enabled/disabled -->
<contact user="1" reduce="maxforce" data="force"/>
<contact user="1" geom1="floor"/>
<contact user="1" site="2"/>
<force user="1" site="force_static"/>
</sensor>
</mujoco>