From f2a967348c64197b9a29fb9e1c7dd9879984e10b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 14 Dec 2023 13:29:22 -0800 Subject: [PATCH] Fix bug introduced in 7942fe957ea046750f936ac002d49425e31d9dab 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 --- src/engine/engine_collision_driver.c | 18 ++++---- src/engine/engine_util_misc.c | 40 ++++++++++++++---- src/engine/engine_util_misc.h | 6 ++- test/engine/engine_collision_box_test.cc | 42 ++++++++++++++----- .../testdata/collision_box/boxbox_deep.xml | 11 +++++ 5 files changed, 90 insertions(+), 27 deletions(-) create mode 100644 test/engine/testdata/collision_box/boxbox_deep.xml diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index d42a4f2e..f6132b74 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -1444,9 +1444,6 @@ static void mj_makeCapsule(const mjModel* m, mjData* d, int f, const int vid[2], // test two geoms for collision, apply filters, add to contact list void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { - // relative distance (1%) outside of which box-box contacts are removed - static mjtNum kBoxRemoveMargin = 1.01; - TM_START; int num, type1, type2, condim; @@ -1547,11 +1544,16 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { // box sizes with margin mjtNum sz1[3] = {size1[0] + margin, size1[1] + margin, size1[2] + margin}; mjtNum sz2[3] = {size2[0] + margin, size2[1] + margin, size2[2] + margin}; - mju_scl3(sz1, sz1, kBoxRemoveMargin); - mju_scl3(sz2, sz2, kBoxRemoveMargin); - // mark as bad if outside box - if (mju_outsideBox(con[i].pos, pos1, mat1, sz1) || - mju_outsideBox(con[i].pos, pos2, mat2, sz2)) { + + // relative distance from surface (1%) outside of which box-box contacts are removed + static mjtNum kRemoveRatio = 1.01; + + // is the contact outside: 1, inside: -1, within the removal width: 0 + int out1 = mju_outsideBox(con[i].pos, pos1, mat1, sz1, kRemoveRatio); + int out2 = mju_outsideBox(con[i].pos, pos2, mat2, sz2, kRemoveRatio); + + // mark as bad if outside one box and not inside the other box + if ((out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1)) { con[i].dim = -1; } } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 29e2e0f8..37eca9e4 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -837,21 +837,47 @@ mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t) -// return 1 if point is outside box given by pos, mat, size +// return 1 if point is outside box given by pos, mat, size * inflate +// return -1 if point is inside box given by pos, mat, size / inflate +// return 0 if point is between the inflated and deflated boxes int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9], - const mjtNum size[3]) { + const mjtNum size[3], mjtNum inflate) { + // check inflation coefficient + if (inflate < 1) { + mjERROR("inflation coefficient must be >= 1") + } + // vector from pos to point, projected to box frame mjtNum vec[3] = {point[0]-pos[0], point[1]-pos[1], point[2]-pos[2]}; mju_rotVecMatT(vec, vec, mat); - // outside - if (vec[0] > size[0] || vec[0] < -size[0] || - vec[1] > size[1] || vec[1] < -size[1] || - vec[2] > size[2] || vec[2] < -size[2]) { + // big: inflated box + mjtNum big[3] = {size[0], size[1], size[2]}; + if (inflate > 1) { + mju_scl3(big, big, inflate); + } + + // check if outside big box + if (vec[0] > big[0] || vec[0] < -big[0] || + vec[1] > big[1] || vec[1] < -big[1] || + vec[2] > big[2] || vec[2] < -big[2]) { return 1; } - // inside + // quick return if no inflation + if (inflate == 1) { + return -1; + } + + // check if inside small (deflated) box + mjtNum small[3] = {size[0]/inflate, size[1]/inflate, size[2]/inflate}; + if (vec[0] < small[0] && vec[0] > -small[0] && + vec[1] < small[1] && vec[1] > -small[1] && + vec[2] < small[2] && vec[2] > -small[2]) { + return -1; + } + + // within margin between small and big box return 0; } diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index c103aba1..cb317258 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -78,9 +78,11 @@ MJAPI void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, // integrate spring-damper analytically, return pos(dt) MJAPI mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum Kp, mjtNum Kv, mjtNum dt); -// return 1 if point is outside box given by pos, mat, size +// return 1 if point is outside box given by pos, mat, size * inflate +// return -1 if point is inside box given by pos, mat, size / inflate +// return 0 if point is between the inflated and deflated boxes MJAPI int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9], - const mjtNum size[3]); + const mjtNum size[3], mjtNum inflate); // print matrix MJAPI void mju_printMat(const mjtNum* mat, int nr, int nc); diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc index 0247335d..5477189d 100644 --- a/test/engine/engine_collision_box_test.cc +++ b/test/engine/engine_collision_box_test.cc @@ -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 diff --git a/test/engine/testdata/collision_box/boxbox_deep.xml b/test/engine/testdata/collision_box/boxbox_deep.xml new file mode 100644 index 00000000..65f14e5f --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_deep.xml @@ -0,0 +1,11 @@ + + + + + + + + + + +