diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 7e759027..5f2c6acf 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -7502,9 +7502,7 @@ Extraction .. _sensor-contact-subtree2: :at:`subtree1`, :at:`subtree2`: :at-val:`string, optional` - Name of a body whose subtree is participating in a contact. See **matching** :ref:`above `. Note - currently only "entire" subtrees are supported, in the sense that the specified body must be a direct child of the - world. General subtrees could be added in the future. + Name of a body whose subtree is participating in a contact. See **matching** :ref:`above `. .. _sensor-contact-site: diff --git a/doc/changelog.rst b/doc/changelog.rst index 54129662..ea2fa6c4 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -10,6 +10,8 @@ General - Constraint island discovery and construction, previously an experimental feature, is now :ref:`documented` and promoted to default; disable it with :ref:`option/flag/island `. We expect islanding to be a strict improvement over the monolithic constraint solver, please let us know if you experience any issues. +- :ref:`Contact sensor` :at-val:`subtree1/subtree2` specification is now available for any body, not + just direct children of the world. .. admonition:: Breaking API changes :class: attention diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 35472845..8151e126 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -1428,22 +1428,6 @@ class SpecsTest(absltest.TestCase): refname='cam', ), ), - dict( - expected_error='subtree1 must be a child of the world', - sensor_params=dict( - type=mujoco.mjtSensor.mjSENS_CONTACT, - objtype=mujoco.mjtObj.mjOBJ_XBODY, - objname='non_root', - ), - ), - dict( - expected_error='subtree2 must be a child of the world', - sensor_params=dict( - type=mujoco.mjtSensor.mjSENS_CONTACT, - reftype=mujoco.mjtObj.mjOBJ_XBODY, - refname='non_root', - ), - ), ] for params in test_cases: diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index b4acc932..9b345d5a 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -247,7 +247,14 @@ static int checkMatch(const mjModel* m, int body, int geom, mjtObj type, int id) if (type == mjOBJ_SITE) return 1; // already passed site filter test if (type == mjOBJ_GEOM) return id == geom; if (type == mjOBJ_BODY) return id == body; - if (type == mjOBJ_XBODY) return body >= 0 && m->body_rootid[id] == m->body_rootid[body]; + if (type == mjOBJ_XBODY) { + // traverse up the tree from body, return true if we land on id + while (body > id) { + body = m->body_parentid[body]; + } + return body == id; + } + return 0; } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 674143dd..d110e7f3 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -7152,11 +7152,6 @@ void mjCSensor::Compile(void) { throw mjCError(this, "first matching criterion: if set, must be (x)body, geom or site"); } - // check that subtree1 is a full tree - if (objtype == mjOBJ_XBODY && static_cast(obj)->GetParent()->id != 0) { - throw mjCError(this, "subtree1 must be a child of the world"); - } - // check second matching criterion if (reftype != mjOBJ_BODY && reftype != mjOBJ_XBODY && @@ -7165,11 +7160,6 @@ void mjCSensor::Compile(void) { throw mjCError(this, "second matching criterion: if set, must be (x)body or geom"); } - // check that subtree2 is a full tree - if (reftype == mjOBJ_XBODY && static_cast(ref)->GetParent()->id != 0) { - throw mjCError(this, "subtree2 must be a child of the world"); - } - // check for dataspec correctness int dataspec = intprm[0]; if (dataspec <= 0) { diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index bb32a675..c5607223 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -708,8 +708,6 @@ TEST_F(SensorTest, BadContact) { "at most one of (geom1, body1, subtree1, site) can be specified"}, {"geom2='sphere1' body2='body'", "at most one of (geom2, body2, subtree2) can be specified"}, - {"subtree1='non_root'", - "must be a child of the world"} }; for (const auto& test : test_cases) { @@ -870,6 +868,31 @@ TEST_F(SensorTest, ContactSubtree) { mj_deleteModel(model); } +TEST_F(SensorTest, ContactSubtreePartial) { + const string xml_path = + GetTestDataFilePath("engine/testdata/sensor/contact_subtree_partial.xml"); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + mjData* data = mj_makeData(model); + + while (data->time < 0.6) { + mj_step(model, data); + } + + EXPECT_EQ(GetSensor(model, data, "all")[0], 4); + EXPECT_EQ(GetSensor(model, data, "world")[0], 4); + EXPECT_EQ(GetSensor(model, data, "thigh")[0], 4); + EXPECT_EQ(GetSensor(model, data, "shin")[0], 2); + EXPECT_EQ(GetSensor(model, data, "foot")[0], 1); + EXPECT_EQ(GetSensor(model, data, "foot_w")[0], 0); + EXPECT_EQ(GetSensor(model, data, "foot_w2")[0], 1); + + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(SensorTest, ContactNet) { const string xml_path = GetTestDataFilePath("engine/testdata/sensor/contact_net.xml"); diff --git a/test/engine/testdata/sensor/contact_subtree_partial.xml b/test/engine/testdata/sensor/contact_subtree_partial.xml new file mode 100644 index 00000000..dbd0c0e1 --- /dev/null +++ b/test/engine/testdata/sensor/contact_subtree_partial.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + +