Circumvent bug in mjc_BoxBox.
The box-box collider sometimes outputs bad contact points which are far outside one of the two boxes. This happens at configurations which are very rare numerically, but rare configurations (e.g. perfect box alignment) are often induced by the designer and the physics, making them actually not-so-rare. Since the box collision functions are very difficult to understand, we circumvent the issue by extending the post processing code to detect and remove such contacts. PiperOrigin-RevId: 589178197 Change-Id: I82b8a888ef04f9196111c960757e3314d0ab6d8e
This commit is contained in:
committed by
Copybara-Service
parent
a006b15958
commit
7942fe957e
@@ -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) {
|
||||
|
||||
@@ -16,6 +16,7 @@
|
||||
#define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_PRIMITIVE_H_
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjexport.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
|
||||
// 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
|
||||
}
|
||||
|
||||
@@ -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++) {
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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 <string>
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#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
|
||||
@@ -0,0 +1,36 @@
|
||||
<mujoco>
|
||||
<!-- This model leads to bad contacts from mjc_BoxBox -->
|
||||
|
||||
<default>
|
||||
<geom rgba="1 1 1 1" margin="1e-3" gap="1e-3"/>
|
||||
<site type="sphere" rgba="0 0 0 0" size="0.001"/>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<light diffuse="1 1 1" pos="-5 -17.5 15" dir="0.24 0.84 -0.48"/>
|
||||
<camera name="fixed" pos="0 -25 8" euler="90 0 0"/>
|
||||
<geom name="ground" pos="0 0 0" quat="1 0 0 0" rgba="0.770 0.670 0.490 1" size="50 50 1" type="plane"/>
|
||||
<body name="block01" pos="-0.9 0 1.5" quat="1 0 0 0">
|
||||
<freejoint/>
|
||||
<geom mass="1" name="geom01" rgba="0.5 0 0 1" size="0.5 0.5 1.5" type="box"/>
|
||||
</body>
|
||||
<body name="block02" pos="-0.7 0 3.5" quat="1 0 0 0">
|
||||
<freejoint/>
|
||||
<geom mass="1" name="geom02" rgba="0.5 0 0.350 1" size="1.5 0.5 0.5" type="box"/>
|
||||
</body>
|
||||
<body name="block03" pos="0.3 0 5.5" quat="1 0 0 0">
|
||||
<freejoint/>
|
||||
<geom mass="1" name="geom03" rgba="0 0.5 0.450 1" size="0.5 0.5 1.5" type="box"/>
|
||||
</body>
|
||||
<body name="block04" pos="0.200 1e-9 7.5" quat="1 0 0 0">
|
||||
<freejoint/>
|
||||
<geom mass="1" name="geom04" rgba="0 0.5 0.099 1" size="1.5 0.5 0.5" type="box"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<contact>
|
||||
<exclude body1="world" body2="block01"/>
|
||||
<exclude body1="block03" body2="block02"/>
|
||||
</contact>
|
||||
|
||||
</mujoco>
|
||||
@@ -0,0 +1,24 @@
|
||||
<mujoco>
|
||||
<!-- This model leads to bad contacts from mjc_BoxBox -->
|
||||
<statistic meansize=".01" center="-.3 -.14 .055" extent=".2"/>
|
||||
|
||||
<worldbody>
|
||||
<light pos="-.3 -.14 1"/>
|
||||
|
||||
<geom type="plane" size="3 3 .01" pos="-0.025 -0.295 0"/>
|
||||
|
||||
<body pos="-.23 -.1 0" euler="0 0 30">
|
||||
<geom type="cylinder" size=".01 .0175" pos="-.09 0 .0175"/>
|
||||
<body pos="0 0 .03">
|
||||
<joint axis="0 1 0"/>
|
||||
<geom type="cylinder" size=".005 .039" zaxis="0 1 0" rgba=".84 .15 .33 1"/>
|
||||
<geom type="box" size=".1 .02 .005" pos="0 0 .01" rgba=".84 .15 .33 1"/>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
<body pos="-.3 -.14 .055" euler="0 0 -30">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".01 .01 .01" rgba=".0 .7 .79 1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,11 @@
|
||||
<mujoco>
|
||||
<!-- This model leads to duplicate contacts from mjc_BoxBox -->
|
||||
|
||||
<worldbody>
|
||||
<geom type="box" size="1 1 1"/>
|
||||
<body pos="0 0 2">
|
||||
<freejoint/>
|
||||
<geom type="box" size="1 1 1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
Reference in New Issue
Block a user