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

Replace margin/gap max with sum.

Browse Source 
PiperOrigin-RevId: 868842521
Change-Id: Ia4a4ca5c6821c5a7b3ba7b0af7f71d981c2866c1
This commit is contained in:
Yuval Tassa
2026-02-11 14:21:37 -08:00
committed by Copybara-Service
parent 336e1026a4
commit 6af0d4c823
9 changed files with 61 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+3
View File
@@ -77,6 +77,9 @@ General
- Removed ``getdir`` from the ``mjpResourceProvider`` struct. All Resource Providers now use the same shared
implementation.
- When combining the ``margin`` and ``gap`` :ref:`parameters<CContact>` of two geoms to obtain the parameters
of a contact, the respective values are now **summed** rather than than taking the maximum. This allows geom
margins to be a proper "inflation" of the geom.
- Camera frustum visualization is now triggered by setting :ref:`resolution<body-camera-resolution>` to values larger
than 1. Relatedly, frustum visualization also works for :ref:`orthographic<body-camera-projection>` cameras.
doc/modeling.rst
+1 -1
View File
@@ -490,7 +490,7 @@ are as follows:
an individual geom. This is why MuJoCo does not allow anisotropic friction in the individual geom specifications, but
only in the explicit contact pair specifications.
**margin**, **gap**
The maximum of the two geom margins (or gaps respectively) is used. The geom priority is ignored here, because the
The sum of the two geom margins (or gaps respectively) is used. The geom priority is ignored here, because the
margin and gap are distance properties and a one-sided specification makes little sense.
.. _solmixing:
mjx/mujoco/mjx/_src/collision_driver.py
+2 -2
View File
@@ -283,8 +283,8 @@ def _contact_groups(m: Model, d: Data) -> Dict[FunctionKey, Contact]:
))
if geom1.size > 0 and geom2.size > 0:
# other contacts get their params from geom fields
margin = jp.maximum(m.geom_margin[geom1], m.geom_margin[geom2])
gap = jp.maximum(m.geom_gap[geom1], m.geom_gap[geom2])
margin = m.geom_margin[geom1] + m.geom_margin[geom2]
gap = m.geom_gap[geom1] + m.geom_gap[geom2]
solmix1, solmix2 = m.geom_solmix[geom1], m.geom_solmix[geom2]
mix = solmix1 / (solmix1 + solmix2)
mix = jp.where((solmix1 < eps) & (solmix2 < eps), 0.5, mix)
mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py
Vendored
+1 -1
View File
@@ -91,7 +91,7 @@ def _hfield_filter(
r2 = geom_rbound[rbound_id, g2]
# TODO(team): margin?
margin = wp.max(geom_margin[margin_id, g1], geom_margin[margin_id, g2])
margin = geom_margin[margin_id, g1] + geom_margin[margin_id, g2]
# box-sphere test: horizontal plane
for i in range(2):
mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py
Vendored
+5 -5
View File
@@ -101,18 +101,18 @@ def _plane_filter(
if size1 == 0.0:
# geom1 is a plane
dist = wp.dot(xpos2 - xpos1, wp.vec3(xmat1[0, 2], xmat1[1, 2], xmat1[2, 2]))
return dist <= size2 + wp.max(margin1, margin2)
return dist <= size2 + margin1 + margin2
elif size2 == 0.0:
# geom2 is a plane
dist = wp.dot(xpos1 - xpos2, wp.vec3(xmat2[0, 2], xmat2[1, 2], xmat2[2, 2]))
return dist <= size1 + wp.max(margin1, margin2)
return dist <= size1 + margin1 + margin2
return True
@wp.func
def _sphere_filter(size1: float, size2: float, margin1: float, margin2: float, xpos1: wp.vec3, xpos2: wp.vec3) -> bool:
bound = size1 + size2 + wp.max(margin1, margin2)
bound = size1 + size2 + margin1 + margin2
dif = xpos2 - xpos1
dist_sq = wp.dot(dif, dif)
return dist_sq <= bound * bound
@@ -141,7 +141,7 @@ def _aabb_filter(
center1 = xmat1 @ center1 + xpos1
center2 = xmat2 @ center2 + xpos2
margin = wp.max(margin1, margin2)
margin = margin1 + margin2
max_x1 = -MJ_MAXVAL
max_y1 = -MJ_MAXVAL
@@ -236,7 +236,7 @@ def _obb_filter(
xmat2: wp.mat33,
) -> bool:
"""Oriented bounding boxes collision (see Gottschalk et al.), see mj_collideOBB."""
margin = wp.max(margin1, margin2)
margin = margin1 + margin2
xcenter = mat23()
normal = mat63()
mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py
Vendored
+2 -2
View File
@@ -552,8 +552,8 @@ def contact_params(
solreffriction = wp.vec2(0.0, 0.0)
solimp = mix * geom_solimp[solimp_id, g1] + (1.0 - mix) * geom_solimp[solimp_id, g2]
# geom priority is ignored
margin = wp.max(geom_margin[margin_id, g1], geom_margin[margin_id, g2])
gap = wp.max(geom_gap[gap_id, g1], geom_gap[gap_id, g2])
margin = geom_margin[margin_id, g1] + geom_margin[margin_id, g2]
gap = geom_gap[gap_id, g1] + geom_gap[gap_id, g2]
friction = vec5(
wp.max(MJ_MINMU, friction[0]),
src/engine/engine_collision_driver.c
+11 -16
View File
@@ -689,8 +689,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
if (isbody1 && isbody2) {
// both are leaves
if (isleaf1 && isleaf2) {
mjtNum maxmargin = mju_max(m->geom_margin[nodeid1], m->geom_margin[nodeid2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->geom_margin[nodeid2]);
if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin)) {
if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2,
@@ -708,8 +707,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
}
// if no intersection at intermediate levels, stop
mjtNum maxmargin = mju_max(m->body_margin[bf1], m->body_margin[bf2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
mjtNum margin = mj_assignMargin(m, m->body_margin[bf1] + m->body_margin[bf2]);
if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2,
d->xipos + 3*bf1, d->ximat + 9*bf1,
d->xipos + 3*bf2, d->ximat + 9*bf2,
@@ -722,8 +720,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
else if (isbody1 && !isbody2) {
// both are leaves
if (isleaf1 && isleaf2) {
mjtNum maxmargin = mju_max(m->geom_margin[nodeid1], m->flex_margin[f2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->flex_margin[f2]);
if (!filterBitmask(m->geom_contype[nodeid1], m->geom_conaffinity[nodeid1],
m->flex_contype[f2], m->flex_conaffinity[f2]) &&
@@ -747,8 +744,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
}
// if no intersection at intermediate levels, stop
mjtNum maxmargin = mju_max(m->body_margin[bf1], m->flex_margin[f2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
mjtNum margin = mj_assignMargin(m, m->body_margin[bf1] + m->flex_margin[f2]);
if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2,
d->xipos + 3*bf1, d->ximat + 9*bf1,
NULL, NULL,
@@ -776,8 +772,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
}
// if no intersection at intermediate levels, stop
mjtNum maxmargin = mju_max(m->flex_margin[f1], m->flex_margin[f2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
mjtNum margin = mj_assignMargin(m, m->flex_margin[f1] + m->flex_margin[f2]);
if (filterBox(bvh1 + 6*node1, bvh2 + 6*node2, margin)) {
continue;
}
@@ -1330,8 +1325,8 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap,
const mjtNum* solimp2 = (f2 < 0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP;
const mjtNum* friction2 = (f2 < 0) ? m->geom_friction+g2*3 : m->flex_friction+f2*3;
// gap: max
*gap = mju_max(gap1, gap2);
// gap: add
*gap = gap1 + gap2;
// different priority: copy from item with higher priority
if (priority1 > priority2) {
@@ -1513,7 +1508,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) {
// set margin: dynamic or pair
if (ipair < 0) {
margin = mj_assignMargin(m, mju_max(m->geom_margin[g1], m->geom_margin[g2]));
margin = mj_assignMargin(m, m->geom_margin[g1] + m->geom_margin[g2]);
} else {
margin = mj_assignMargin(m, m->pair_margin[ipair]);
}
@@ -1668,7 +1663,7 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) {
mjtNum nrm[3] = {mat[2], mat[5], mat[8]};
// prepare contact parameters (same for all vertices)
mjtNum margin = mj_assignMargin(m, mju_max(m->geom_margin[g], m->flex_margin[f]));
mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]);
int condim;
int flex_vertnum = m->flex_vertnum[f];
mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5];
@@ -1869,7 +1864,7 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) {
// test a geom and an elem for collision, add to contact list
void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
mjtNum margin = mj_assignMargin(m, mju_max(m->geom_margin[g], m->flex_margin[f]));
mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]);
int dim = m->flex_dim[f], type = m->geom_type[g];
int num;
@@ -2005,7 +2000,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
// test two elems for collision, add to contact list
void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2) {
mjtNum margin = mj_assignMargin(m, mju_max(m->flex_margin[f1], m->flex_margin[f2]));
mjtNum margin = mj_assignMargin(m, m->flex_margin[f1] + m->flex_margin[f2]);
int dim1 = m->flex_dim[f1], dim2 = m->flex_dim[f2];
int num;
test/engine/engine_collision_driver_test.cc
+33
View File
@@ -357,5 +357,38 @@ TEST_F(MjCollisionTest, PinchingSucceeds) {
mj_deleteModel(m);
}
TEST_F(MjCollisionTest, MarginSumming) {
// Two spheres with size 0.1, placed 0.21 apart (distance of 0.01).
// With margin summing, margin1 + margin2 = 0.00999 + 0.00999 = 0.01998 > 0.01
// so a contact should be generated.
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom name="sphere1" type="sphere" size=".1" margin="0.00999"/>
<joint type="slide" axis="1 0 0"/>
</body>
<body pos=".21 0 0">
<geom name="sphere2" type="sphere" size=".1" margin="0.00999"/>
<joint type="slide" axis="1 0 0"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
ASSERT_THAT(d, NotNull());
mj_fwdPosition(m, d);
// With margin summing, we expect 1 contact
EXPECT_EQ(d->ncon, 1);
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco
test/engine/engine_solver_test.cc
+3 -3
View File
@@ -57,15 +57,15 @@ TEST_F(SolverTest, IslandsEquivalent) {
mjData* data_noisland = mj_makeData(model);
// Below are 3 tolerances associated with 3 different iteration counts,
// they are only moderately tight, 10x higher than x86-64 failure on Linux,
// i.e. in that case the test fails with rtol smaller than {5e-3, 5e-4, 5e-5}.
// they are only moderately tight, 12x higher than x86-64 failure on Linux,
// i.e. in that case the test fails with rtol smaller than {6e-3, 6e-4, 6e-5}.
// The point of this test is to show that CG convergence is actually not very
// precise, simply changing whether islands are used changes the solution by
// quite a lot, even at high iteration count and zero {ls_}tolerance.
// Increasing the iteration count higher than 60 does not improve convergence.
constexpr int kNumTol = 3;
mjtNum maxiter[kNumTol] = {30, 40, 60};
mjtNum rtol[kNumTol] = {5e-2, 5e-3, 5e-4};
mjtNum rtol[kNumTol] = {6e-2, 6e-3, 6e-4};
for (int i = 0; i < kNumTol; ++i) {
model->opt.iterations = maxiter[i];