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

Specialized box and capsule collision primitives for stable 2D flex interactions.

Browse Source 
PiperOrigin-RevId: 864500749
Change-Id: I52aed6e29569477a8a0b6b5eb55ad483f1077c0e
This commit is contained in:
Alessio Quaglino
2026-02-02 13:35:46 -08:00
committed by Copybara-Service
parent 8344dba11e
commit a54c9f1d78
6 changed files with 353 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
model/flex/pinch.xml
+41
View File
@@ -0,0 +1,41 @@
<mujoco>
<include file="scene.xml"/>
<option timestep="0.002"/>
<worldbody>
<body name="gripper" pos="0 0 0.5">
<joint name="lift" type="slide" axis="0 0 1" damping="50"/>
<geom type="box" size="0.2 0.05 0.02" rgba="0.5 0.5 0.5 1"/> <!-- base -->
<body name="left_finger" pos="-0.1 0 -0.1">
<joint name="left_slide" type="slide" axis="1 0 0" damping="10"/>
<geom type="box" size="0.02 0.1 0.1" rgba="0.8 0.2 0.2 1"/>
</body>
<body name="right_finger" pos="0.1 0 -0.1">
<joint name="right_slide" type="slide" axis="-1 0 0" damping="10"/>
<geom type="box" size="0.02 0.1 0.1" rgba="0.8 0.2 0.2 1"/>
</body>
</body>
<flexcomp name="cloth" type="grid" dim="2" count="9 9 1" spacing="0.05 0.05 0.05"
pos="0 0 0.1" radius="0.01">
<edge equality="true"/>
</flexcomp>
</worldbody>
<equality>
<joint joint1="right_slide" joint2="left_slide"/>
</equality>
<tendon>
<fixed name="grasp">
<joint joint="right_slide" coef="1"/>
<joint joint="left_slide" coef="1"/>
</fixed>
</tendon>
<actuator>
<position name="lift" joint="lift" kp="600" dampratio="1" ctrlrange="-1 1"/>
<position name="grasp" tendon="grasp" kp="200" dampratio="1" ctrlrange="0 1"/>
</actuator>
</mujoco>
src/engine/engine_collision_convex.c
+25 -6
View File
@@ -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;
}
src/engine/engine_collision_driver.c
+18
View File
@@ -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);
src/engine/engine_collision_primitive.c
+158
View File
@@ -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;
}
src/engine/engine_collision_primitive.h
+7
View File
@@ -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,
test/engine/engine_collision_driver_test.cc
+104
View File
@@ -253,5 +253,109 @@ TEST_F(MjCollisionTest, PlaneInBody) {
mj_deleteModel(m);
}
TEST_F(MjCollisionTest, PinchingSucceeds) {
constexpr char xml[] = R"(
<mujoco>
<option timestep="0.002" gravity="0 0 -9.81"/>
<worldbody>
<geom name="floor" type="plane" size="0 0 1"/>
<body name="gripper" pos="0 0 0.5">
<joint name="lift" type="slide" axis="0 0 1" damping="50"/>
<geom type="box" size="0.2 0.05 0.02" rgba="0.5 0.5 0.5 1"/> <!-- base -->
<body name="left_finger" pos="-0.1 0 -0.1">
<joint name="left_slide" type="slide" axis="1 0 0" damping="10"/>
<geom type="box" size="0.02 0.1 0.1" rgba="0.8 0.2 0.2 1"/>
</body>
<body name="right_finger" pos="0.1 0 -0.1">
<joint name="right_slide" type="slide" axis="-1 0 0" damping="10"/>
<geom type="box" size="0.02 0.1 0.1" rgba="0.8 0.2 0.2 1"/>
</body>
</body>
<flexcomp name="cloth" type="grid" dim="2" count="9 9 1" spacing="0.05 0.05 0.05"
pos="0 0 0.1" radius="0.01">
<edge equality="true"/>
</flexcomp>
</worldbody>
<equality>
<joint joint1="right_slide" joint2="left_slide"/>
</equality>
<tendon>
<fixed name="grasp">
<joint joint="right_slide" coef="1"/>
<joint joint="left_slide" coef="1"/>
</fixed>
</tendon>
<actuator>
<position name="lift" joint="lift" kp="600" dampratio="1" ctrlrange="-1 1"/>
<position name="grasp" tendon="grasp" kp="200" dampratio="1" ctrlrange="0 1"/>
</actuator>
</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());
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