diff --git a/model/flex/pinch.xml b/model/flex/pinch.xml
new file mode 100644
index 00000000..78d343a6
--- /dev/null
+++ b/model/flex/pinch.xml
@@ -0,0 +1,41 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c
index 1a182531..96f032ae 100644
--- a/src/engine/engine_collision_convex.c
+++ b/src/engine/engine_collision_convex.c
@@ -1477,13 +1477,15 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
int gid[2] = {g1, g2};
mjtGeom type[2];
for (int i=0; i < 2; i++) {
- type[i] = m->geom_type[gid[i]];
+ if (gid[i] < 0) {
+ type[i] = mjGEOM_NONE;
+ } else {
+ type[i] = m->geom_type[gid[i]];
+ }
// set to mjGEOM_NONE if type cannot be processed
- if (type[i] != mjGEOM_SPHERE &&
- type[i] != mjGEOM_CAPSULE &&
- type[i] != mjGEOM_ELLIPSOID &&
- type[i] != mjGEOM_CYLINDER) {
+ if (type[i] != mjGEOM_SPHERE && type[i] != mjGEOM_CAPSULE &&
+ type[i] != mjGEOM_ELLIPSOID && type[i] != mjGEOM_CYLINDER) {
type[i] = mjGEOM_NONE;
}
}
@@ -1582,6 +1584,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
nrm[2] = 0;
processed[i] = 1;
break;
+
default:
// do nothing: only sphere, capsule, ellipsoid and cylinder are processed
break;
@@ -1630,7 +1633,23 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
mjc_setCCDObjFlex(&obj2, f2, e2, -1);
// find contacts
- return mjc_CCDIteration(m, d, &obj1, &obj2, con, 1, margin);
+ int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, 1, margin);
+
+ // fix normals for 2D flex
+ if (ncon && !mjDISABLED(mjDSBL_NATIVECCD)) {
+ // check if either object is a 2D flex
+ int isflex2d = 0;
+ if (f1 >= 0 && m->flex_dim[f1] == 2) isflex2d = 1;
+ if (f2 >= 0 && m->flex_dim[f2] == 2) isflex2d = 1;
+
+ if (isflex2d) {
+ for (int i = 0; i < ncon; i++) {
+ mjc_fixNormal(m, d, con + i, g1, -1);
+ }
+ }
+ }
+
+ return ncon;
}
diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c
index ffd2a535..cce1f8ea 100644
--- a/src/engine/engine_collision_driver.c
+++ b/src/engine/engine_collision_driver.c
@@ -1944,6 +1944,24 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
vertxpos + 3*edata[2], m->flex_radius[f]);
}
+ // 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,
+ 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]);
+ }
+
+ // capsule : triangle
+ 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,
+ 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);
diff --git a/src/engine/engine_collision_primitive.c b/src/engine/engine_collision_primitive.c
index ec0fa814..4ce09120 100644
--- a/src/engine/engine_collision_primitive.c
+++ b/src/engine/engine_collision_primitive.c
@@ -606,3 +606,161 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
return 1;
}
+
+// box : triangle with radius
+int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
+ const mjtNum* mat, const mjtNum* size, const mjtNum* t1,
+ const mjtNum* t2, const mjtNum* t3, mjtNum rt) {
+ int cnt = 0;
+ const mjtNum* vert[3] = {t1, t2, t3};
+
+ for (int i = 0; i < 3; i++) {
+ // map vertex to box local frame
+ mjtNum diff[3], local[3];
+ mju_sub3(diff, vert[i], pos);
+ mju_mulMatTVec3(local, mat, diff);
+
+ // find max penetration / closest face
+ int maxaxis = 0;
+ mjtNum maxval = mju_abs(local[0]) - size[0];
+ for (int j = 1; j < 3; j++) {
+ mjtNum val = mju_abs(local[j]) - size[j];
+ if (val > maxval) {
+ maxval = val;
+ maxaxis = j;
+ }
+ }
+
+ // contact distance: dist = maxval - rt
+ // strictly, we only care if dist < margin
+ if (maxval - rt > margin) {
+ continue;
+ }
+
+ // check if within other dimensions (with margin/radius)
+ int inside = 1;
+ for (int j = 0; j < 3; j++) {
+ if (mju_abs(local[j]) > size[j] + margin + rt) {
+ inside = 0;
+ break;
+ }
+ }
+ if (!inside) {
+ continue;
+ }
+
+ // create contact
+ if (cnt < mjMAXCONPAIR) {
+ // normal in local frame
+ mjtNum nrm_local[3] = {0, 0, 0};
+ nrm_local[maxaxis] = (local[maxaxis] > 0 ? 1 : -1);
+
+ // normal in global frame (from Box to Triangle)
+ mju_mulMatVec3(con[cnt].frame, mat, nrm_local);
+
+ // distance
+ con[cnt].dist = maxval - rt;
+
+ // position: v - nrm * (rt + dist/2)
+ mjtNum offset = rt + con[cnt].dist * 0.5;
+ mji_addScl3(con[cnt].pos, vert[i], con[cnt].frame, -offset);
+
+ // frame details
+ mju_zero3(con[cnt].frame + 3);
+
+ cnt++;
+ }
+ }
+
+ // check box corners against triangle
+ for (int i = 0; i < 8; i++) {
+ if (cnt >= mjMAXCONPAIR) {
+ break;
+ }
+
+ // get corner in local coordinates
+ mjtNum vec[3];
+ vec[0] = (i & 1 ? size[0] : -size[0]);
+ vec[1] = (i & 2 ? size[1] : -size[1]);
+ vec[2] = (i & 4 ? size[2] : -size[2]);
+
+ // get corner in global coordinates relative to box center
+ mjtNum corner[3];
+ mju_mulMatVec3(corner, mat, vec);
+ mju_addTo3(corner, pos);
+
+ // check collision with triangle (radius 0 for corner)
+ if (mjraw_SphereTriangle(con + cnt, margin, corner, 0, t1, t2, t3, rt)) {
+ // mjraw_SphereTriangle normal points from Sphere (Corner) to Triangle.
+ cnt++;
+ }
+ }
+
+ return cnt;
+}
+
+// capsule : triangle with radius
+int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
+ const mjtNum* mat, const mjtNum* size,
+ const mjtNum* t1, const mjtNum* t2, const mjtNum* t3,
+ mjtNum rt) {
+ int cnt = 0;
+ mjtNum radius = size[0];
+ mjtNum len = size[1];
+ mjtNum axis[3] = {mat[2], mat[5], mat[8]};
+ mjtNum p1[3], p2[3];
+
+ // capsule endpoints
+ mju_addScl3(p1, pos, axis, -len);
+ mju_addScl3(p2, pos, axis, len);
+
+ // Check endpoints against triangle
+ cnt += mjraw_SphereTriangle(con + cnt, margin, p1, radius, t1, t2, t3, rt);
+ if (cnt >= mjMAXCONPAIR) return cnt;
+ cnt += mjraw_SphereTriangle(con + cnt, margin, p2, radius, t1, t2, t3, rt);
+ if (cnt >= mjMAXCONPAIR) return cnt;
+
+ // Check triangle vertices against capsule axis (Point-Segment)
+ const mjtNum* vert[3] = {t1, t2, t3};
+ for (int i = 0; i < 3; i++) {
+ // point-segment distance
+ mjtNum vec[3], ab[3];
+ mju_sub3(vec, vert[i], p1);
+ mju_sub3(ab, p2, p1);
+ mjtNum t = mju_dot3(vec, ab) / (4 * len * len); // ab length is 2*len
+
+ // clamp t to [0, 1] segment (only process interior)
+ if (t <= mjMINVAL || t >= 1 - mjMINVAL) {
+ continue;
+ }
+
+ // closest point on segment
+ mjtNum closest[3];
+ mji_addScl3(closest, p1, ab, t);
+
+ // distance vector
+ mju_sub3(vec, vert[i], closest);
+ mjtNum dist = mju_normalize3(vec);
+
+ if (dist > radius + rt + margin) {
+ continue;
+ }
+
+ // con->dist
+ con[cnt].dist = dist - radius - rt;
+
+ // Frame: normal from Capsule to Triangle. 'vec' points Closest->Vert.
+ mji_copy3(con[cnt].frame, vec);
+ mju_zero3(con[cnt].frame + 3);
+
+ // Position: midway between surfaces
+ mji_add3(con[cnt].pos, closest, vert[i]);
+ mji_addToScl3(con[cnt].pos, vec, radius - rt);
+ mju_scl3(con[cnt].pos, con[cnt].pos, 0.5);
+
+ cnt++;
+ if (cnt >= mjMAXCONPAIR) return cnt;
+ }
+
+ return cnt;
+}
diff --git a/src/engine/engine_collision_primitive.h b/src/engine/engine_collision_primitive.h
index 8156905e..5fc675e6 100644
--- a/src/engine/engine_collision_primitive.h
+++ b/src/engine/engine_collision_primitive.h
@@ -46,6 +46,13 @@ int mjraw_CapsuleBox (mjContact* con, mjtNum margin,
int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
const mjtNum* s, mjtNum rs,
const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt);
+int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
+ const mjtNum* mat, const mjtNum* size, const mjtNum* t1,
+ const mjtNum* t2, const mjtNum* t3, mjtNum rt);
+int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
+ const mjtNum* mat, const mjtNum* size,
+ const mjtNum* t1, const mjtNum* t2, const mjtNum* t3,
+ mjtNum rt);
// plane collisions
MJAPI int mjc_PlaneSphere (const mjModel* m, const mjData* d,
diff --git a/test/engine/engine_collision_driver_test.cc b/test/engine/engine_collision_driver_test.cc
index e149b423..e02884af 100644
--- a/test/engine/engine_collision_driver_test.cc
+++ b/test/engine/engine_collision_driver_test.cc
@@ -253,5 +253,109 @@ TEST_F(MjCollisionTest, PlaneInBody) {
mj_deleteModel(m);
}
+TEST_F(MjCollisionTest, PinchingSucceeds) {
+ constexpr char xml[] = R"(
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ )";
+ char error[1024];
+ mjModel* m = LoadModelFromString(xml, error, sizeof(error));
+ ASSERT_THAT(m, NotNull()) << error;
+ mjData* d = mj_makeData(m);
+ ASSERT_THAT(d, NotNull());
+
+ int lift_id = mj_name2id(m, mjOBJ_ACTUATOR, "lift");
+ int grasp_id = mj_name2id(m, mjOBJ_ACTUATOR, "grasp");
+
+ // Phase 1: Lower gripper.
+ // The gripper base starts at z=0.5. The finger has length 0.2 (size 0.1),
+ // extending from z=0.4 to z=0.2 relative to base (center at -0.1).
+ // The cloth is at z=0.1. We need to lower the gripper so the fingertips
+ // reach the cloth. A lift value of -0.35 places the fingertips near z=0.05.
+
+ for (int i = 0; i < 1000; ++i) {
+ d->ctrl[lift_id] = -0.35; // Lower
+ d->ctrl[grasp_id] = 0; // Open
+ mj_step(m, d);
+ }
+
+ // Phase 2: Pinch
+ for (int i = 0; i < 1000; ++i) {
+ d->ctrl[lift_id] = -0.35; // Hold height
+ d->ctrl[grasp_id] = 0.8; // Close (max 1)
+ mj_step(m, d);
+ }
+
+ // Phase 3: Lift
+ for (int i = 0; i < 2000; ++i) {
+ d->ctrl[lift_id] = 0.5; // Lift up
+ d->ctrl[grasp_id] = 0.8; // Keep closed
+ mj_step(m, d);
+ }
+
+ // Check if cloth is lifted
+ // flex verts are in d->flexvert_xpos
+ // original z is ~0.1 (falling to floor ~0.0)
+ // gripper lifted to > 0.5 probably
+
+ // Find average Z of cloth
+ double avg_z = 0;
+ int nvert = m->flex_vertnum[0];
+ for (int i = 0; i < nvert; ++i) {
+ avg_z += d->flexvert_xpos[3 * i + 2];
+ }
+ avg_z /= nvert;
+
+ // If lifted, avg_z should be significantly > 0.1
+ // If failed (slipped), avg_z should be near 0 (floor)
+
+ // Specialized primitives (mjraw_BoxTriangle, mjraw_CapsuleTriangle) should
+ // enable stable pinching, so we expect the cloth to be lifted.
+ EXPECT_GT(avg_z, 0.2) << "Cloth slipped out of gripper!";
+
+ mj_deleteData(d);
+ mj_deleteModel(m);
+}
+
} // namespace
} // namespace mujoco