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

Fix insidesite sensor for massless flex parent bodies.

Browse Source 
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
This commit is contained in:
Alessio Quaglino
2026-05-01 08:26:21 -07:00
committed by Copybara-Service
parent f7d31e06f0
commit 8287d9d152
2 changed files with 66 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_sensor.c
+9
View File
@@ -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
+57
View File
@@ -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