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

Circumvent bug in mjc_BoxBox.

Browse Source 
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:
Yuval Tassa
2023-12-08 10:38:39 -08:00
committed by Copybara-Service
parent a006b15958
commit 7942fe957e
9 changed files with 381 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/CMakeLists.txt
+3
View File
@@ -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)
test/engine/engine_collision_box_test.cc
+226
View File
@@ -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
test/engine/testdata/collision_box/boxbox_bad0.xml
Vendored
+36
View File
@@ -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>
test/engine/testdata/collision_box/boxbox_bad1.xml
Vendored
+24
View File
@@ -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>
test/engine/testdata/collision_box/boxbox_duplicate.xml
Vendored
+11
View File
@@ -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>