diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 11a07786..d6a2eb13 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -795,6 +795,15 @@ static void mj_computeSensorPos(const mjModel* m, mjData* d, int i, mjtNum* sens case mjSENS_INSIDESITE: // 1 if object is inside site get_xpos_xmat(d, objtype, objid, i, &xpos, &xmat); + + // for massless bodies with positive subtree mass (e.g., flex parents), + // xipos is the static body frame origin; use subtree_com instead + if (objtype == mjOBJ_BODY && objid > 0 && + m->body_mass[objid] < mjMINVAL && + m->body_subtreemass[objid] >= mjMINVAL) { + xpos = d->subtree_com + 3*objid; + } + sensordata[0] = mju_insideGeom(d->site_xpos + 3*refid, d->site_xmat + 9*refid, m->site_size + 3*refid, diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index 8b477f2c..01aced2e 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -1727,5 +1727,62 @@ TEST_F(SensorTest, TactileSkipTangents) { mj_deleteModel(model); } + +// insidesite uses subtree_com for massless flex parent bodies +TEST_F(SensorTest, InsideSiteFlexBody) { + static constexpr char xml[] = R"( + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + // flex is at origin, site is a large box at origin — should be inside + mj_forward(m, d); + EXPECT_EQ(d->sensordata[0], 1) + << "flex body should be inside the container site"; + + // shift all vertex/node bodies far outside the site via qpos + // each body has 3 slide joints (x, y, z); shift z by +10 + int parent_id = mj_name2id(m, mjOBJ_BODY, "parent"); + for (int b = parent_id + 1; b < m->nbody; b++) { + if (m->body_parentid[b] == parent_id) { + int jadr = m->body_jntadr[b]; + if (jadr >= 0 && m->body_jntnum[b] == 3) { + // z-slide is the 3rd joint + d->qpos[m->jnt_qposadr[jadr + 2]] = 10.0; + } + } + } + mj_forward(m, d); + + // subtree_com should now be far outside; sensor should read 0 + EXPECT_EQ(d->sensordata[0], 0) + << "flex body should be outside the container site after displacement"; + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco