Fix insidesite sensor for massless flex parent bodies.

mjSENS_INSIDESITE uses xipos to test containment, but for massless flex
parent bodies xipos equals the static body frame origin and does not
track the actual flex position.

Fix: in the INSIDESITE sensor case, when the object is a massless body
with positive subtree mass (i.e., a flex parent), use subtree_com
instead of xipos. This correctly reflects the mass-weighted centroid of
the flex child bodies without changing the global semantics of xipos.
PiperOrigin-RevId: 908745144
Change-Id: I527f0efb7b419345188411c9da883505156ebeea

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,

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"(
+  <mujoco>
+    <option gravity="0 0 0"/>
+    <worldbody>
+      <body name="parent">
+        <flexcomp name="soft" type="grid" count="3 3 3"
+                  radius="0.01" dim="3" mass="1">
+            <elasticity young="5e4" poisson="0.2"/>
+        </flexcomp>
+      </body>
+
+      <!-- large site centered at origin, should contain the flex -->
+      <site name="container" type="box" size="2 2 2"/>
+    </worldbody>
+
+    <sensor>
+      <insidesite name="inside" site="container"
+                  objtype="body" objname="parent"/>
+    </sensor>
+  </mujoco>
+  )";
+
+  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