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
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>