Margin and gap redesign (breaking change)

PiperOrigin-RevId: 914329812
Change-Id: I905665e4c1965bdb8b90587e5b1277cfbfe5cce0
This commit is contained in:
Yuval Tassa
2026-05-12 09:43:07 -07:00
committed by Copybara-Service
parent 7f5be412a0
commit a4e49f2dff
29 changed files with 2082 additions and 178 deletions
+61 -56
View File
@@ -882,12 +882,13 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
// both are leaves
if (isleaf1 && isleaf2) {
mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->geom_margin[nodeid2]);
mjtNum gap = m->geom_gap[nodeid1] + m->geom_gap[nodeid2];
if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin)) {
if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin + gap)) {
if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2,
d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1,
d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2,
margin, NULL, NULL, &initialize)) {
margin + gap, NULL, NULL, &initialize)) {
mj_collideGeomPair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
if (mark_active) {
d->bvh_active[node1 + bvhadr1] = 1;
@@ -913,15 +914,16 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
// both are leaves
if (isleaf1 && isleaf2) {
mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->flex_margin[f2]);
mjtNum gap = m->geom_gap[nodeid1] + m->flex_gap[f2];
if (!filterBitmask(m->geom_contype[nodeid1], m->geom_conaffinity[nodeid1],
m->flex_contype[f2], m->flex_conaffinity[f2]) &&
!filterSphereBox(d->geom_xpos + 3*nodeid1, m->geom_rbound[nodeid1] + margin,
!filterSphereBox(d->geom_xpos + 3*nodeid1, m->geom_rbound[nodeid1] + margin + gap,
bvh2 + 6*node2)) {
if (mj_collideOBB(m->geom_aabb + 6*nodeid1, bvh2 + 6*node2,
d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1,
NULL, NULL,
margin, NULL, NULL, &initialize)) {
margin + gap, NULL, NULL, &initialize)) {
// collide unless geom is plane or SDF (handled separately)
if (m->geom_type[nodeid1] != mjGEOM_PLANE &&
m->geom_type[nodeid1] != mjGEOM_SDF) {
@@ -1058,7 +1060,8 @@ static void makeAAMM(const mjModel* m, mjData* d,
// process all body geoms (body is collidable, should have geoms)
for (int i=0; i < body_geomnum; i++) {
int geom = m->body_geomadr[body]+i;
mjtNum margin = override_margin ? override_margin : m->geom_margin[geom];
mjtNum margin = override_margin ? override_margin
: m->geom_margin[geom] + m->geom_gap[geom];
mjtNum _aamm[6];
const mjtNum* aabb = m->geom_aabb + 6*geom;
@@ -1124,7 +1127,8 @@ static void makeAAMM(const mjModel* m, mjData* d,
}
// correct for flex radius and margin
mjtNum margin = override_margin ? override_margin : m->flex_margin[f];
mjtNum margin = override_margin ? override_margin
: m->flex_margin[f] + m->flex_gap[f];
mjtNum bound = m->flex_radius[f] + margin;
aamm[0] -= bound;
aamm[1] -= bound;
@@ -1513,8 +1517,8 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
//----------------------------- narrow-phase collision detection -----------------------------------
// compute contact condim, gap, solref, solimp, friction
static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap,
// compute contact condim, solref, solimp, friction
static void mj_contactParam(const mjModel* m, int* condim,
mjtNum* solref, mjtNum* solimp, mjtNum* friction,
int g1, int g2, int f1, int f2) {
mjtNum fri[3];
@@ -1522,7 +1526,6 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap,
// get parameters from geom1 or flex1
int priority1 = (f1 < 0) ? m->geom_priority[g1] : m->flex_priority[f1];
int condim1 = (f1 < 0) ? m->geom_condim[g1] : m->flex_condim[f1];
mjtNum gap1 = (f1 < 0) ? m->geom_gap[g1] : m->flex_gap[f1];
mjtNum solmix1 = (f1 < 0) ? m->geom_solmix[g1] : m->flex_solmix[f1];
const mjtNum* solref1 = (f1 < 0) ? m->geom_solref+g1*mjNREF : m->flex_solref+f1*mjNREF;
const mjtNum* solimp1 = (f1 < 0) ? m->geom_solimp+g1*mjNIMP : m->flex_solimp+f1*mjNIMP;
@@ -1531,15 +1534,11 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap,
// get parameters from geom2 or flex2
int priority2 = (f2 < 0) ? m->geom_priority[g2] : m->flex_priority[f2];
int condim2 = (f2 < 0) ? m->geom_condim[g2] : m->flex_condim[f2];
mjtNum gap2 = (f2 < 0) ? m->geom_gap[g2] : m->flex_gap[f2];
mjtNum solmix2 = (f2 < 0) ? m->geom_solmix[g2] : m->flex_solmix[f2];
const mjtNum* solref2 = (f2 < 0) ? m->geom_solref+g2*mjNREF : m->flex_solref+f2*mjNREF;
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: add
*gap = gap1 + gap2;
// different priority: copy from item with higher priority
if (priority1 > priority2) {
*condim = condim1;
@@ -1719,8 +1718,15 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) {
margin = mj_assignMargin(m, m->pair_margin[ipair]);
}
// set gap: dynamic or pair
if (ipair < 0) {
gap = m->geom_gap[g1] + m->geom_gap[g2];
} else {
gap = m->pair_gap[ipair];
}
// bounding sphere filter
if (mj_filterSphere(m, d, g1, g2, margin)) {
if (mj_filterSphere(m, d, g1, g2, margin + gap)) {
return;
}
@@ -1733,7 +1739,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) {
}
// call collision detector to generate contacts
num = collisionFunc(m, d, con, g1, g2, margin);
num = collisionFunc(m, d, con, g1, g2, margin + gap);
// check contacts
if (!num) {
@@ -1746,15 +1752,14 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) {
mjERROR("too many contacts returned by collision function");
}
// set condim, gap, solref, solimp, friction: dynamic
// set condim, solref, solimp, friction: dynamic
if (ipair < 0) {
mj_contactParam(m, &condim, &gap, solref, solimp, friction, g1, g2, -1, -1);
mj_contactParam(m, &condim, solref, solimp, friction, g1, g2, -1, -1);
}
// set condim, gap, solref, solimp, friction: pair
// set condim, solref, solimp, friction: pair
else {
condim = m->pair_dim[ipair];
gap = m->pair_gap[ipair];
mju_copy(solref, m->pair_solref+mjNREF*ipair, mjNREF);
mju_copy(solimp, m->pair_solimp+mjNIMP*ipair, mjNIMP);
mju_copy(friction, m->pair_friction+5*ipair, 5);
@@ -1778,7 +1783,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) {
con[i].vert[1] = -1;
// set remaining contact parameters
mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction);
mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction);
}
// add to ncon
@@ -1801,9 +1806,10 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int 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];
mjtNum gap = m->geom_gap[g] + m->flex_gap[f];
mjtNum solref[mjNREF], solimp[mjNIMP], friction[5];
mjtNum solreffriction[mjNREF] = {0};
mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f);
mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f);
// collide all flex vertices with plane
for (int i=0; i < flex_vertnum; i++) {
@@ -1814,7 +1820,7 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) {
mjtNum dist = mju_dot3(dif, nrm);
// no contact
if (dist > margin + radius) {
if (dist > margin + gap + radius) {
continue;
}
@@ -1835,7 +1841,7 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) {
con.vert[1] = i;
// set remaining contact parameters
mj_setContact(m, &con, condim, margin-gap, solref, solreffriction, solimp, friction);
mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction);
// add to mjData, abort if too many contacts
if (mj_addContact(m, d, &con)) {
@@ -1854,10 +1860,11 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) {
// prepare contact parameters (same for all contacts)
mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]);
mjtNum gap = m->geom_gap[g] + m->flex_gap[f];
int condim;
mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5];
mjtNum solref[mjNREF], solimp[mjNIMP], friction[5];
mjtNum solreffriction[mjNREF] = {0};
mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f);
mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f);
// allocate temporary contact array on stack (zero-initialized)
mj_markStack(d);
@@ -1865,7 +1872,7 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) {
memset(con, 0, mjMAXCONPAIR * sizeof(mjContact));
// call batched flex-SDF collision
int num = mjc_FlexSDF(m, d, con, g, f, margin);
int num = mjc_FlexSDF(m, d, con, g, f, margin + gap);
// add contacts to mjData
for (int i = 0; i < num; i++) {
@@ -1880,7 +1887,7 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) {
con[i].vert[1] = -1;
// set remaining contact parameters
mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction);
mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction);
// add to mjData, abort if too many contacts
if (mj_addContact(m, d, con + i)) {
@@ -1949,9 +1956,9 @@ void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
int condim;
int flex_elemnum = m->flex_elemnum[f];
mjtNum radius = m->flex_radius[f];
mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5];
mjtNum solref[mjNREF], solimp[mjNIMP], friction[5];
mjtNum solreffriction[mjNREF] = {0};
mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f, f);
mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f, f);
condim = 1;
// process all elements
@@ -2048,13 +2055,14 @@ 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, m->geom_margin[g] + m->flex_margin[f]);
mjtNum gap = m->geom_gap[g] + m->flex_gap[f];
int dim = m->flex_dim[f], type = m->geom_type[g];
int num;
// bounding sphere test: only if midphase is disabled
if (mjDISABLED(mjDSBL_MIDPHASE)) {
int eglobal = m->flex_elemadr[f] + e;
if (filterSphereBox(d->geom_xpos+3*g, m->geom_rbound[g]+margin,
if (filterSphereBox(d->geom_xpos+3*g, m->geom_rbound[g]+margin+gap,
d->flexelem_aabb+6*eglobal)) {
return;
}
@@ -2087,17 +2095,17 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
// call raw primitive for corresponding geom type
if (type == mjGEOM_SPHERE) {
num = mjraw_SphereCapsule(con, margin,
num = mjraw_SphereCapsule(con, margin + gap,
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g,
pos, mat, size);
}
else if (type == mjGEOM_CAPSULE) {
num = mjraw_CapsuleCapsule(con, margin,
num = mjraw_CapsuleCapsule(con, margin + gap,
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g,
pos, mat, size);
}
else {
num = mjraw_CapsuleBox(con, margin,
num = mjraw_CapsuleBox(con, margin + gap,
pos, mat, size,
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g);
@@ -2110,13 +2118,13 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
// heightfield : elem
else if (type == mjGEOM_HFIELD) {
num = mjc_HFieldElem(m, d, con, g, f, e, margin);
num = mjc_HFieldElem(m, d, con, g, f, e, margin + gap);
}
// sphere : triangle
else if (type == mjGEOM_SPHERE && dim == 2) {
const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
num = mjraw_SphereTriangle(con, margin,
num = mjraw_SphereTriangle(con, margin + gap,
d->geom_xpos+3*g, m->geom_size[3*g],
vertxpos + 3*edata[0], vertxpos + 3*edata[1],
vertxpos + 3*edata[2], m->flex_radius[f]);
@@ -2125,7 +2133,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
// box : triangle
else if (type == mjGEOM_BOX && dim == 2) {
const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f];
num = mjraw_BoxTriangle(con, margin, d->geom_xpos + 3 * g,
num = mjraw_BoxTriangle(con, margin + gap, d->geom_xpos + 3 * g,
d->geom_xmat + 9 * g, m->geom_size + 3 * g,
vertxpos + 3 * edata[0], vertxpos + 3 * edata[1],
vertxpos + 3 * edata[2], m->flex_radius[f]);
@@ -2135,14 +2143,14 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
else if (type == mjGEOM_CAPSULE && dim == 2) {
const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f];
num = mjraw_CapsuleTriangle(
con, margin, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g,
con, margin + gap, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g,
m->geom_size + 3 * g, vertxpos + 3 * edata[0], vertxpos + 3 * edata[1],
vertxpos + 3 * edata[2], m->flex_radius[f]);
}
// general geom : elem
else {
num = mjc_ConvexElem(m, d, con, g, -1, -1, -1, f, e, margin);
num = mjc_ConvexElem(m, d, con, g, -1, -1, -1, f, e, margin + gap);
}
// check contacts
@@ -2153,9 +2161,9 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
// get contact parameters
int condim;
mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP];
mjtNum friction[5], solref[mjNREF], solimp[mjNIMP];
mjtNum solreffriction[mjNREF] = {0};
mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f);
mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f);
// add contacts
for (int i=0; i < num; i++) {
@@ -2170,7 +2178,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
con[i].vert[1] = -1;
// set remaining contact parameters
mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction);
mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction);
}
// add to ncon
@@ -2184,17 +2192,19 @@ 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, m->flex_margin[f1] + m->flex_margin[f2]);
mjtNum gap = m->flex_gap[f1] + m->flex_gap[f2];
int dim1 = m->flex_dim[f1], dim2 = m->flex_dim[f2];
int num;
// ignore margin in self-collisions
// ignore margin and gap in self-collisions
if (f1 == f2) {
margin = 0;
gap = 0;
}
// bounding box filter (not applied in midphase)
if (filterBox(d->flexelem_aabb+6*(m->flex_elemadr[f1]+e1),
d->flexelem_aabb+6*(m->flex_elemadr[f2]+e2), margin)) {
d->flexelem_aabb+6*(m->flex_elemadr[f2]+e2), margin + gap)) {
return;
}
@@ -2231,12 +2241,12 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
pos2, mat2, size2);
// raw primitive
num = mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
num = mjraw_CapsuleCapsule(con, margin + gap, pos1, mat1, size1, pos2, mat2, size2);
}
// general convex collision
else {
num = mjc_ConvexElem(m, d, con, -1, f1, e1, -1, f2, e2, margin);
num = mjc_ConvexElem(m, d, con, -1, f1, e1, -1, f2, e2, margin + gap);
}
// check contacts
@@ -2247,14 +2257,9 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
// get contact parameters
int condim;
mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP];
mjtNum friction[5], solref[mjNREF], solimp[mjNIMP];
mjtNum solreffriction[mjNREF] = {0};
mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f1, f2);
// ignore gap in self collision, since margin is ignored
if (f1 == f2) {
gap = 0;
}
mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f1, f2);
// add contacts
for (int i=0; i < num; i++) {
@@ -2269,7 +2274,7 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
con[i].vert[1] = -1;
// set remaining contact parameters
mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction);
mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction);
}
// add to ncon
@@ -2336,9 +2341,9 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) {
// get contact parameters
int condim;
mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP];
mjtNum friction[5], solref[mjNREF], solimp[mjNIMP];
mjtNum solreffriction[mjNREF] = {0};
mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f, f);
mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f, f);
// add contacts
for (int i=0; i < num; i++) {
+3 -3
View File
@@ -421,7 +421,7 @@ void ElementSpecGui(mjsElement* element, SpecEditor* editor) {
FIELD(solref, "solver reference");
FIELD(solimp, "solver impedance");
FIELD(margin, "margin for contact detection");
FIELD(gap, "include in solver if dist < margin-gap");
FIELD(gap, "additional contact detection buffer");
FIELD(mass, "used to compute density");
FIELD(density, "used to compute mass and inertia from volume or surface");
FIELD(typeinertia, "selects between surface and volume inertia");
@@ -530,7 +530,7 @@ void ElementSpecGui(mjsElement* element, SpecEditor* editor) {
FIELD(solref, "solver reference");
FIELD(solimp, "solver impedance");
FIELD(margin, "margin for contact detection");
FIELD(gap, "include in solver if dist<margin-gap");
FIELD(gap, "additional contact detection buffer");
FIELD(dim, "element dimensionality");
FIELD(radius, "radius around primitive element");
FIELD(size, "vertex bounding box half sizes in qpos0");
@@ -626,7 +626,7 @@ void ElementSpecGui(mjsElement* element, SpecEditor* editor) {
FIELD(solreffriction, "solver reference, frictional directions");
FIELD(solimp, "solver impedance");
FIELD(margin, "margin for contact detection");
FIELD(gap, "include in solver if dist<margin-gap");
FIELD(gap, "additional contact detection buffer");
FIELD(friction, "full contact friction");
FIELD(info, "message appended to errors");
break;
@@ -312,7 +312,7 @@ class "MjcCollisionAPI" (
)
uniform double mjc:gap = 0 (
displayName = "Gap"
doc = "This attribute is used to enable the generation of inactive contacts, i.e., contacts that are ignored by the constraint solver but are included in mjData.contact for the purpose of custom computations. When this value is positive, geom distances between margin and margin-gap correspond to such inactive contacts."
doc = "Additional contact detection buffer beyond margin. Contacts are detected at distance margin + gap but forces are only generated at distance margin."
)
uniform int mjc:group = 0 (
displayName = "Group"
@@ -320,7 +320,7 @@ class "MjcCollisionAPI" (
)
uniform double mjc:margin = 0 (
displayName = "Margin"
doc = "Distance threshold below which contacts are detected and included in the global array mjData.contact."
doc = "Geometric inflation of the geom surface for the purpose of contact force generation."
)
uniform int mjc:priority = 0 (
displayName = "Priority"
+2 -2
View File
@@ -738,7 +738,7 @@ class "MjcCollisionAPI"
string apiName = "Margin"
}
displayName = "Margin"
doc = """Distance threshold below which contacts are detected and included in the global array mjData.contact."""
doc = """Geometric inflation of the geom surface for the purpose of contact force generation."""
)
uniform double mjc:gap = 0.0 (
@@ -746,7 +746,7 @@ class "MjcCollisionAPI"
string apiName = "Gap"
}
displayName = "Gap"
doc = """This attribute is used to enable the generation of inactive contacts, i.e., contacts that are ignored by the constraint solver but are included in mjData.contact for the purpose of custom computations. When this value is positive, geom distances between margin and margin-gap correspond to such inactive contacts."""
doc = """Additional contact detection buffer beyond margin. Contacts are detected at distance margin + gap but forces are only generated at distance margin."""
)
}