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

Fix bug in box-sphere collider. Fixes #2206

Browse Source 
PiperOrigin-RevId: 702627603
Change-Id: Ie6015b7fa6f8507459325d692581b0f5f83aa159
This commit is contained in:
Yuval Tassa
2024-12-04 01:18:26 -08:00
committed by Copybara-Service
parent ec9986dcb7
commit 0e7d2ef6df
3 changed files with 42 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+6
View File
@@ -2,6 +2,12 @@
Changelog
=========
Upcoming version (not yet released)
-----------------------------------
Bug fixes
^^^^^^^^^
- Fixed a bug in the box-sphere collider, depth was incorrect for deep penetrations (:github:issue:`2206`).
Version 3.2.6 (Dec 2, 2024)
---------------------------
src/engine/engine_collision_box.c
+7 -7
View File
@@ -38,10 +38,9 @@ static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n)
// raw sphere : box
int mjraw_SphereBox(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2)
{
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
int i, k;
mjtNum tmp[3], center[3], clamped[3], deepest[3], nearest[3];
mjtNum tmp[3], center[3], clamped[3], deepest[3];
mjtNum pos[3];
mjtNum dist, closest;
@@ -62,19 +61,20 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin,
if (dist <= mjMINVAL) {
closest = (size2[0] + size2[1] + size2[2]) * 2;
for (i = 0; i < 6; i++)
if (closest > mju_abs((i % 2 ? 1 : -1)*size2[i / 2] - center[i / 2]))
{
for (i = 0; i < 6; i++) {
if (closest > mju_abs((i % 2 ? 1 : -1)*size2[i / 2] - center[i / 2])) {
closest = mju_abs((i % 2 ? 1 : -1) * size2[i / 2] - center[i / 2]);
k = i;
}
}
mju_zero3(nearest);
mjtNum nearest[3] = {0};
nearest[k / 2] = (k % 2 ? -1 : 1);
mju_copy3(pos, center);
mju_addToScl3(pos, nearest, (size1[0] - closest) / 2);
mju_mulMatVec3(con[0].frame, mat2, nearest);
dist = -closest;
} else {
mju_addToScl3(deepest, tmp, size1[0]);
mju_zero3(pos);
test/engine/engine_collision_box_test.cc
+29
View File
@@ -30,6 +30,7 @@ namespace {
using MjCollisionBoxTest = MujocoTest;
using ::testing::NotNull;
using ::testing::DoubleNear;
static const char* const kBad0FilePath =
"engine/testdata/collision_box/boxbox_bad0.xml";
@@ -243,6 +244,34 @@ TEST_F(MjCollisionBoxTest, DeepPenetration) {
mj_deleteModel(model);
}
TEST_F(MjCollisionBoxTest, BoxSphere) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="plane" type="plane" size="0.05 0.05 0.001"/>
<geom name="box" type="box" pos = "0 0 -0.025" size="0.05 0.05 .025"/>
<body>
<freejoint/>
<geom name="sphere" type="sphere" mass="1" size="0.005"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
for (mjtNum z : {-.015, -.00501, -.005, -.00499, 0.0, 0.004}) {
data->qpos[2] = z;
mj_forward(model, data);
EXPECT_EQ(data->ncon, 2);
EXPECT_THAT(data->contact[0].dist, DoubleNear(data->contact[1].dist, 1e-8));
}
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco