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

Fix bug introduced in 7942fe957e

Browse Source 
That change removed some spurious contacts returned by mjc_BoxBox, but also removed some desirable contacts that occur during very deep penetration of two boxes (when one box is completely inside another box). This is now fixed.

PiperOrigin-RevId: 591036252
Change-Id: I84b51f2179fd6fe29618a4e908e86934c6fc6941
This commit is contained in:
Yuval Tassa
2023-12-14 13:29:22 -08:00
committed by Copybara-Service
parent 9f6be3135e
commit f2a967348c
5 changed files with 90 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_collision_box_test.cc
+32 -10
View File
@@ -94,7 +94,7 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
}
// expect some contacts to have been removed
EXPECT_LT(nmatched, num);
EXPECT_LT(nmatched, num) << local_path;
// get box info
const mjtNum* pos1 = data->geom_xpos + 3 * g1;
@@ -103,23 +103,27 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
const mjtNum* pos2 = data->geom_xpos + 3 * g2;
const mjtNum* mat2 = data->geom_xmat + 9 * g2;
const mjtNum* size2 = model->geom_size + 3 * g2;
mjtNum margin = mju_max(model->geom_margin[g1], model->geom_margin[g2]);
// loop over raw contacts, find removed
for (int i = 0; i < num; i++) {
if (!match_raw[i]) {
// === check if outside
// get margin and adjusted sizes
const mjtNum kBoxRemoveMargin = 1.01;
mjtNum sz1[3], sz2[3];
mju_scl3(sz1, size1, kBoxRemoveMargin);
mju_scl3(sz2, size2, kBoxRemoveMargin);
mjtNum sz1[3] = {size1[0]+margin, size1[1]+margin, size1[2]+margin};
mjtNum sz2[3] = {size2[0]+margin, size2[1]+margin, size2[2]+margin};
// is contact outside one of the boxes
bool outside = mju_outsideBox(con_raw[i].pos, pos1, mat1, sz1) ||
mju_outsideBox(con_raw[i].pos, pos2, mat2, sz2);
// relative distance (1%) outside of which contacts are removed
static mjtNum kRatio = 1.01;
// expect that removed contact was either outside
// is the contact outside: 1, inside: -1, within the removal width: 0
int out1 = mju_outsideBox(con_raw[i].pos, pos1, mat1, sz1, kRatio);
int out2 = mju_outsideBox(con_raw[i].pos, pos2, mat2, sz2, kRatio);
// mark as bad if outside one box and not inside the other box
bool outside = (out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1);
// expect that removed contact was outside
EXPECT_TRUE(outside);
}
}
@@ -222,5 +226,23 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
}
static const char* const kDeepFilePath =
"engine/testdata/collision_box/boxbox_deep.xml";
TEST_F(MjCollisionBoxTest, DeepPenetration) {
const std::string xml_path = GetTestDataFilePath(kDeepFilePath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
mj_forward(model, data);
// expect 4 contact
EXPECT_EQ(data->ncon ,4);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/testdata/collision_box/boxbox_deep.xml
Vendored
+11
View File
@@ -0,0 +1,11 @@
<mujoco>
<!-- Box deeply penetrating another box -->
<worldbody>
<geom type="box" size="1 1 1" rgba=".5 .5 .5 .5"/>
<body pos=".1 .2 .3">
<freejoint/>
<geom type="box" size=".2 .2 .2"/>
</body>
</worldbody>
</mujoco>