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

Avoid duplicate contact points in compound bodies.

Browse Source 
Sometimes broadphase returns the same body pair multiple times. In those cases, avoid creating every contact twice.

PiperOrigin-RevId: 499869235
Change-Id: I58606168b0723d40b4fdd43b1fafad79bf35d76d
This commit is contained in:
Nimrod Gileadi
2023-01-05 07:27:19 -08:00
committed by Copybara-Service
parent 0c463366a0
commit f85f3fba2f
2 changed files with 46 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_driver.c
+9 -2
View File
@@ -51,7 +51,7 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
//----------------------------- collision detection entry point ------------------------------------
void mj_collision(const mjModel* m, mjData* d) {
int g1, g2, signature, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr;
int g1, g2, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr;
int nexclude = m->nexclude, npair = m->npair, nbodypair = ((m->nbody-1)*m->nbody)/2;
int *broadphasepair = 0;
mjMARKSTACK;
@@ -77,6 +77,7 @@ void mj_collision(const mjModel* m, mjData* d) {
// call broadphase collision detector
broadphasepair = (int*)mj_stackAlloc(d, (m->nbody*(m->nbody-1))/2);
nbodypair = mj_broadphase(m, d, broadphasepair, (m->nbody*(m->nbody-1))/2);
unsigned int last_signature = -1;
// loop over body pairs (broadphase or all)
for (int i=0; i<nbodypair; i++) {
@@ -85,7 +86,13 @@ void mj_collision(const mjModel* m, mjData* d) {
b2 = broadphasepair[i] & 0xFFFF;
// compute signature for this body pair
signature = ((b1+1)<<16) + (b2+1);
unsigned int signature = ((b1+1)<<16) + (b2+1);
// pairs come sorted by signature, but may not be unique
// if signature is repeated, skip it
if (signature == last_signature) {
continue;
}
last_signature = signature;
// merge predefined pairs
merged = 0;
test/engine/engine_collision_driver_test.cc
+37
View File
@@ -31,6 +31,7 @@ using MjCollisionTest = MujocoTest;
using GeomPair = std::pair<std::string, std::string>;
using ::testing::IsEmpty;
using ::testing::ElementsAre;
using ::testing::NotNull;
// Returns a sorted list of pairs of colliding geom names, where each pair of
// geom names is sorted.
@@ -108,5 +109,41 @@ TEST_F(MjCollisionTest, ZeroedHessian) {
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjCollisionTest, ContactCount) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom type="plane" size="5 5 .01"/>
</body>
<body pos="0 0 0.9">
<freejoint/>
<geom type="sphere" size="1" pos="-1 -1 0"/>
<geom type="sphere" size="1" pos="-1 1 0"/>
<geom type="sphere" size="1" pos=" 1 -1 0"/>
<geom type="sphere" size="1" pos=" 1 1 0"/>
<geom type="sphere" size="1" pos="-2 -2 0"/>
<geom type="sphere" size="1" pos="-2 2 0"/>
<geom type="sphere" size="1" pos=" 2 -2 0"/>
<geom type="sphere" size="1" pos=" 2 2 0"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
mjData* d = mj_makeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m, d);
// there are 8 spheres, all touching the floor
EXPECT_EQ(d->ncon, 8);
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco