diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 10920162..d42a4f2e 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -1444,6 +1444,9 @@ 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; @@ -1524,16 +1527,44 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { mjERROR("too many contacts returned by collision function"); } - // remove repeated contacts in box-box + // remove bad and repeated contacts in box-box if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) { // use dim field to mark: -1: bad, 0: good for (int i=0; i < num; i++) { con[i].dim = 0; } - // find bad + // get box info + const mjtNum* pos1 = d->geom_xpos + 3 * g1; + const mjtNum* mat1 = d->geom_xmat + 9 * g1; + const mjtNum* size1 = m->geom_size + 3 * g1; + const mjtNum* pos2 = d->geom_xpos + 3 * g2; + const mjtNum* mat2 = d->geom_xmat + 9 * g2; + const mjtNum* size2 = m->geom_size + 3 * g2; + + // find bad: contacts outside one of the boxes + for (int i=0; i < num; i++) { + // 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)) { + con[i].dim = -1; + } + } + + // find duplicates for (int i=0; i < num-1; i++) { + if (con[i].dim == -1) { + continue; // already marked bad: skip + } for (int j=i+1; j < num; j++) { + if (con[j].dim == -1) { + continue; // already marked bad: skip + } if (con[i].pos[0] == con[j].pos[0] && con[i].pos[1] == con[j].pos[1] && con[i].pos[2] == con[j].pos[2]) { @@ -1546,6 +1577,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { // consolidate good int i = 0; for (int j=0; j < num; j++) { + // good: maybe copy if (con[j].dim == 0) { // different: copy if (i < j) { diff --git a/src/engine/engine_collision_primitive.h b/src/engine/engine_collision_primitive.h index 49946707..8156905e 100644 --- a/src/engine/engine_collision_primitive.h +++ b/src/engine/engine_collision_primitive.h @@ -16,6 +16,7 @@ #define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_PRIMITIVE_H_ #include +#include #include // define and extract geom info @@ -47,32 +48,32 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt); // plane collisions -int mjc_PlaneSphere (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_PlaneCapsule (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_PlaneCylinder (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_PlaneBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneSphere (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneCapsule (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneCylinder (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); // sphere and capsule collisions -int mjc_SphereSphere (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_SphereCapsule (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_SphereCylinder (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_CapsuleCapsule (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereSphere (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereCapsule (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereCylinder (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_CapsuleCapsule (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); // box collisions: from engine_collision_box.c -int mjc_CapsuleBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_SphereBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_BoxBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_CapsuleBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_BoxBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); #ifdef __cplusplus } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 30eb0d2a..29e2e0f8 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -837,6 +837,26 @@ 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 +int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9], + const mjtNum size[3]) { + // 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]) { + return 1; + } + + // inside + return 0; +} + + + // print matrix to screen void mju_printMat(const mjtNum* mat, int nr, int nc) { for (int r=0; r < nr; r++) { diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 32417f2c..c103aba1 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -78,6 +78,10 @@ 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 +MJAPI int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9], + const mjtNum size[3]); + // print matrix MJAPI void mju_printMat(const mjtNum* mat, int nr, int nc); diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index 1e083b36..fb77205d 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -12,6 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +mujoco_test(engine_collision_box_test) +target_link_libraries(engine_collision_box_test fixture gmock) + mujoco_test(engine_collision_convex_test) target_link_libraries(engine_collision_convex_test fixture gmock) diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc new file mode 100644 index 00000000..0247335d --- /dev/null +++ b/test/engine/engine_collision_box_test.cc @@ -0,0 +1,226 @@ +// Copyright 2023 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Tests for engine/engine_collision_box.c. + +#include + +#include +#include +#include +#include +#include "test/fixture.h" +#include "src/engine/engine_collision_primitive.h" +#include "src/engine/engine_util_misc.h" + + +namespace mujoco { +namespace { + +using MjCollisionBoxTest = MujocoTest; +using ::testing::NotNull; + +static const char* const kBad0FilePath = + "engine/testdata/collision_box/boxbox_bad0.xml"; +static const char* const kBad1FilePath = + "engine/testdata/collision_box/boxbox_bad1.xml"; + +TEST_F(MjCollisionBoxTest, BadContacts) { + for (const char* local_path : {kBad0FilePath, kBad1FilePath}) { + const std::string xml_path = GetTestDataFilePath(local_path); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // allocate contact array and matching arrays + mj_markStack(data); + mjContact* con_raw = (mjContact*) mj_stackAllocByte( + data, mjMAXCONPAIR * sizeof(mjContact), alignof(mjContact)); + int* match_raw = mj_stackAllocInt(data, mjMAXCONPAIR); + int* match = mj_stackAllocInt(data, data->ncon); + + int g1 = -1; + int g2 = -1; + for (int c = 0; c < data->ncon; c++) { + mjContact* con = data->contact + c; + int g1new = con->geom[0]; + int g2new = con->geom[1]; + + // not box-box: skip + if (model->geom_type[g1new] != mjGEOM_BOX || + model->geom_type[g2new] != mjGEOM_BOX) { + continue; + } + + // same geom pair: skip + if (g1 == g1new && g2 == g2new) { + continue; + } + + g1 = g1new; + g2 = g2new; + + // call low-level box-box collider + int num = mjc_BoxBox(model, data, con_raw, g1, g2, con->includemargin); + + // allocate and clear arrays marking already matched contacts + mju_zeroInt(match_raw, num); + mju_zeroInt(match, data->ncon); + + // loop over raw contacts, match with contact array using pos + int nmatched = 0; + for (int i = 0; i < num; i++) { + for (int j = 0; j < data->ncon; j++) { + if (!match[j] && + con_raw[i].pos[0] == data->contact[j].pos[0] && + con_raw[i].pos[1] == data->contact[j].pos[1] && + con_raw[i].pos[2] == data->contact[j].pos[2]) { + match_raw[i] = match[j] = 1; + nmatched++; + } + } + } + + // expect some contacts to have been removed + EXPECT_LT(nmatched, num); + + // get box info + const mjtNum* pos1 = data->geom_xpos + 3 * g1; + const mjtNum* mat1 = data->geom_xmat + 9 * g1; + const mjtNum* size1 = model->geom_size + 3 * g1; + const mjtNum* pos2 = data->geom_xpos + 3 * g2; + const mjtNum* mat2 = data->geom_xmat + 9 * g2; + const mjtNum* size2 = model->geom_size + 3 * 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); + + // 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); + + // expect that removed contact was either outside + EXPECT_TRUE(outside); + } + } + } + + mj_freeStack(data); + mj_deleteData(data); + mj_deleteModel(model); + } +} + +static const char* const kDuplicateFilePath = + "engine/testdata/collision_box/boxbox_duplicate.xml"; + +TEST_F(MjCollisionBoxTest, DuplicateContacts) { + const std::string xml_path = GetTestDataFilePath(kDuplicateFilePath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // allocate contact array and matching arrays + mj_markStack(data); + mjContact* con_raw = (mjContact*) mj_stackAllocByte( + data, mjMAXCONPAIR * sizeof(mjContact), alignof(mjContact)); + int* match_raw = mj_stackAllocInt(data, mjMAXCONPAIR); + int* match = mj_stackAllocInt(data, data->ncon); + + + int g1 = -1; + int g2 = -1; + for (int c = 0; c < data->ncon; c++) { + mjContact* con = data->contact + c; + int g1new = con->geom[0]; + int g2new = con->geom[1]; + + // not box-box: skip + if (model->geom_type[g1new] != mjGEOM_BOX || + model->geom_type[g2new] != mjGEOM_BOX) { + continue; + } + + // same geom pair: skip + if (g1 == g1new && g2 == g2new) { + continue; + } + + g1 = g1new; + g2 = g2new; + + // call low-level box-box collider + int num = mjc_BoxBox(model, data, con_raw, g1, g2, con->includemargin); + + // allocate and clear arrays marking already matched contacts + mju_zeroInt(match_raw, num); + mju_zeroInt(match, data->ncon); + + // loop over raw contacts, match with contact array using pos + int nmatched = 0; + for (int i = 0; i < num; i++) { + for (int j = 0; j < data->ncon; j++) { + if (!match[j] && + con_raw[i].pos[0] == data->contact[j].pos[0] && + con_raw[i].pos[1] == data->contact[j].pos[1] && + con_raw[i].pos[2] == data->contact[j].pos[2]) { + match_raw[i] = match[j] = 1; + nmatched++; + } + } + } + + // expect some contacts to have been removed + EXPECT_LT(nmatched, num); + + // loop over raw contacts, find removed + for (int i = 0; i < num; i++) { + if (!match_raw[i]) { + // === check if duplicate + bool duplicate = false; + for (int j = 0; j < num; j++) { + if (duplicate || i == j) { + continue; + } + if (con_raw[i].pos[0] == con_raw[j].pos[0] && + con_raw[i].pos[1] == con_raw[j].pos[1] && + con_raw[i].pos[2] == con_raw[j].pos[2]) { + duplicate = true; + } + } + + // expect that removed contact was duplicated + EXPECT_TRUE(duplicate); + } + } + } + + mj_freeStack(data); + mj_deleteData(data); + mj_deleteModel(model); +} + + +} // namespace +} // namespace mujoco diff --git a/test/engine/testdata/collision_box/boxbox_bad0.xml b/test/engine/testdata/collision_box/boxbox_bad0.xml new file mode 100644 index 00000000..a3e08973 --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_bad0.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/collision_box/boxbox_bad1.xml b/test/engine/testdata/collision_box/boxbox_bad1.xml new file mode 100644 index 00000000..04f16290 --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_bad1.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/collision_box/boxbox_duplicate.xml b/test/engine/testdata/collision_box/boxbox_duplicate.xml new file mode 100644 index 00000000..6582f5cf --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_duplicate.xml @@ -0,0 +1,11 @@ + + + + + + + + + + +