From 00823dff0cfdda7e97dbec985b9ff944014a47bb Mon Sep 17 00:00:00 2001 From: camevor Date: Wed, 9 Apr 2025 17:27:15 +0200 Subject: [PATCH 01/10] Capsule-box collision --- mujoco_warp/_src/collision_primitive.py | 391 ++++++++++++++++++++++-- 1 file changed, 374 insertions(+), 17 deletions(-) diff --git a/mujoco_warp/_src/collision_primitive.py b/mujoco_warp/_src/collision_primitive.py index fdec4e3f..6dc01e2e 100644 --- a/mujoco_warp/_src/collision_primitive.py +++ b/mujoco_warp/_src/collision_primitive.py @@ -23,6 +23,7 @@ from .types import GeomType from .types import Model +_MINVAL = 1e-15 _TINY_VAL = 1e-6 @@ -344,45 +345,48 @@ def plane_cylinder( @wp.func -def sphere_box( - sphere: Geom, - box: Geom, - worldid: int, - d: Data, +def _sphere_box_raw( + sphere_pos: wp.vec3, + sphere_size: float, + box_pos: wp.vec3, + box_rot: wp.mat33, + box_size: wp.vec3, margin: float, geom_indices: wp.vec2i, + worldid: int, + d: Data, ): - center = wp.transpose(box.rot) @ (sphere.pos - box.pos) + center = wp.transpose(box_rot) @ (sphere_pos - box_pos) - clamped = wp.max(-box.size, wp.min(box.size, center)) + clamped = wp.max(-box_size, wp.min(box_size, center)) clamped_dir, dist = normalize_with_norm(clamped - center) - if dist - sphere.size[0] > margin: + if dist - sphere_size > margin: return # sphere center inside box if dist <= _TINY_VAL: - closest = 2.0 * (box.size[0] + box.size[1] + box.size[2]) + closest = 2.0 * (box_size[0] + box_size[1] + box_size[2]) k = wp.int32(0) for i in range(6): - face_dist = wp.abs(wp.where(i % 2, 1.0, -1.0) * box.size[i / 2] - center[i / 2]) + face_dist = wp.abs(wp.where(i % 2, 1.0, -1.0) * box_size[i / 2] - center[i / 2]) if closest > face_dist: closest = face_dist k = i nearest = wp.vec3(0.0) nearest[k / 2] = wp.where(k % 2, -1.0, 1.0) - pos = center + nearest * (sphere.size[0] - closest) / 2.0 - contact_normal = box.rot @ nearest - contact_dist = -closest - sphere.size[0] + pos = center + nearest * (sphere_size - closest) / 2.0 + contact_normal = box_rot @ nearest + contact_dist = -closest - sphere_size else: - deepest = center + clamped_dir * sphere.size[0] + deepest = center + clamped_dir * sphere_size pos = 0.5 * (clamped + deepest) - contact_normal = box.rot @ clamped_dir - contact_dist = dist - sphere.size[0] + contact_normal = box_rot @ clamped_dir + contact_dist = dist - sphere_size - contact_pos = box.pos + box.rot @ pos + contact_pos = box_pos + box_rot @ pos write_contact( d, contact_dist, @@ -394,6 +398,357 @@ def sphere_box( ) +@wp.func +def sphere_box( + sphere: Geom, + box: Geom, + worldid: int, + d: Data, + margin: float, + geom_indices: wp.vec2i, +): + _sphere_box_raw( + sphere.pos, + sphere.size[0], + box.pos, + box.rot, + box.size, + margin, + geom_indices, + worldid, + d, + ) + + +@wp.func +def capsule_box( + cap: Geom, + box: Geom, + worldid: int, + d: Data, + margin: float, + geom_indices: wp.vec2i, +): + """Calculates contacts between a capsule and a box.""" + # Ported from the mjc implementation + pos = wp.transpose(box.rot) @ (cap.pos - box.pos) + axis = wp.vec3(cap.rot[0, 2], cap.rot[1, 2], cap.rot[2, 2]) + halfaxis = axis * cap.size[1] + axisdir = ( + wp.int32(axis[0] > 0.0) + 2 * wp.int32(axis[1] > 0.0) + 4 * wp.int32(axis[2] > 0.0) + ) + + bestdistmax = margin + 2.0 * ( + cap.size[0] + cap.size[1] + box.size[0] + box.size[1] + box.size[2] + ) + + # keep track of closest point + bestdist = wp.float32(bestdistmax) + bestsegmentpos = wp.float32(-12) + cltype = wp.int32(-4) + clface = wp.int32(-12) + + # check if face closest + for i in range(-1, 2, 2): + axisTip = pos + wp.float32(i) * halfaxis + boxPoint = wp.vec3(axisTip) + + n_out = wp.int32(0) + ax_out = wp.int32(-1) + + for j in range(3): + if boxPoint[j] < -box.size[j]: + n_out += 1 + ax_out = j + boxPoint[j] = -box.size[j] + elif boxPoint[j] > box.size[j]: + n_out += 1 + ax_out = j + boxPoint[j] = box.size[j] + + if n_out > 1: + continue + + dist = wp.length_sq(boxPoint - axisTip) + + if dist < bestdist: + bestdist = dist + bestsegmentpos = wp.float32(i) + cltype = -2 + i + clface = ax_out + + # check edge edge + + clcorner = wp.int32(-123) # which corner is the closest + cledge = wp.int32(-123) # which axis + bestboxpos = wp.float32(0.0) + + for i in range(8): + for j in range(3): + if i & (1 << j) != 0: + continue + + # c1<6 means that closest point on the box is at the lower end or in the middle of the edge + + c2 = wp.int32(-123) + # trick to get a corner + box_pt = wp.cw_mul( + wp.vec3( + wp.where(i & 1, 1.0, -1.0), + wp.where(i & 2, 1.0, -1.0), + wp.where(i & 4, 1.0, -1.0), + ), + box.size, + ) + box_pt[j] = 0.0 + + # box_pt is the starting point on the box + # box_dir is the direction along the "j"-th axis + # pos is the capsule's center + # halfaxis is the capsule direction + + # find closest point between capsule and the edge + dif = box_pt - pos + + u = -box.size[j] * dif[j] + v = wp.dot(halfaxis, dif) + ma = box.size[j] * box.size[j] + mb = -box.size[j] * halfaxis[j] + mc = cap.size[1] * cap.size[1] + det = ma * mc - mb * mb + if wp.abs(det) < _MINVAL: + continue + + idet = 1.0 / det + # sX : X=1 means middle of segment. X=0 or 2 one or the other end + + x1 = wp.float32((mc * u - mb * v) * idet) + x2 = wp.float32((ma * v - mb * u) * idet) + + s1 = wp.int32(1) + s2 = wp.int32(1) + + if x1 > 1: + x1 = 1.0 + s1 = 2 + x2 = (v - mb) / mc + elif x1 < -1: + x1 = -1.0 + s1 = 0 + x2 = (v + mb) / mc + + x2_over = x2 > 1.0 + if x2_over or x2 < -1.0: + if x2_over: + x2 = 1.0 + s2 = 2 + x1 = (u - mb) / ma + else: + x2 = -1.0 + s2 = 0 + x1 = (u + mb) / ma + + if x1 > 1: + x1 = 1.0 + s1 = 2 + elif x1 < -1: + x1 = -1.0 + s1 = 0 + + dif -= halfaxis * x2 + dif[j] += box.size[j] * x1 + + ct = s1 * 3 + s2 + + dif_sq = wp.length_sq(dif) + if dif_sq < bestdist - _MINVAL: + bestdist = dif_sq + bestsegmentpos = x2 + bestboxpos = x1 + # ct<6 means that closest point on the box is at the lower end or in the middle of the edge + c2 = ct / 6 + # cltype /3 == 0 means the lower corner is closest to the capsule + # cltype /3 == 2 means the upper corner is closest to the capsule + # cltype /3 == 1 means the middle of the edge is closest to the capsule + # cltype %3 == 0 means the lower corner is closest to the box + # cltype %3 == 2 means the upper corner is closest to the box + # cltype %3 == 1 means the middle of the capsule is closest to the box + # note that edges include corners + + clcorner = i + (1 << j) * c2 # which corner is the closest + cledge = j # which axis + cltype = ct # save clamped info + + best = wp.float32(0.0) + l = wp.float32(0.0) + + p = wp.vec2(pos.x, pos.y) + dd = wp.vec2(halfaxis.x, halfaxis.y) + s = wp.vec2(box.size.x, box.size.y) + secondpos = wp.float32(-4.0) + + l = wp.length_sq(dd) + + uu = dd.x * s.y + vv = dd.y * s.x + # w = dd.x * p.y - dd.y * p.x + w_neg = dd.x * p.y - dd.y * p.x < 0 + + best = wp.float32(-1.0) + + ee1 = uu - vv + ee2 = uu + vv + + if wp.abs(ee1) > best: + best = wp.abs(ee1) + c1 = wp.where((ee1 < 0) == w_neg, 0, 3) + + if wp.abs(ee2) > best: + best = wp.abs(ee2) + c1 = wp.where((ee2 > 0) == w_neg, 1, 2) + + if cltype == -4: # invalid type + return + + if cltype >= 0 and cltype / 3 != 1: # closest to a corner of the box + c1 = axisdir ^ clcorner + # hack to find the relative orientation of capsule and corner + # there are 2 cases: + # 1: pointing to or away from the corner + # 2: oriented along a face or an edge + + # case 1: no chance of additional contact + if c1 != 0 and c1 != 7: + if c1 == 1 or c1 == 2 or c1 == 4: + mul = 1 + else: + mul = -1 + c1 = 7 - c1 + + # "de" and "dp" distance from first closest point on the capsule to both ends of it + # mul is a direction along the capsule's axis + + if c1 == 1: + ax = 0 + ax1 = 1 + ax2 = 2 + elif c1 == 2: + ax = 1 + ax1 = 2 + ax2 = 0 + elif c1 == 4: + ax = 2 + ax1 = 0 + ax2 = 1 + + if axis[ax]*axis[ax] > 0.5: # second point along the edge of the box + m = 2.0 * box.size[ax] / wp.abs(halfaxis[ax]) + secondpos = min(1.0 - wp.float32(mul) * bestsegmentpos, m) + else: # second point along a face of the box + # check for overshoot again + m = 2.0 * min( + box.size[ax1] / wp.abs(halfaxis[ax1]), + box.size[ax2] / wp.abs(halfaxis[ax2]) + ) + secondpos = -min(1.0 + wp.float32(mul) * bestsegmentpos, m) + secondpos *= wp.float32(mul) + + elif cltype >= 0 and cltype / 3 == 1: # we are on box's edge + # hacks to find the relative orientation of capsule and edge + # there are 2 cases: + # c1= 2^n: edge and capsule are oriented in a T configuration (no more contacts + # c1!=2^n: oriented in a cross X configuration + c1 = axisdir ^ clcorner # same trick + + c1 &= 7 - (1 << cledge) # even more hacks + + if c1 == 1 or c1 == 2 or c1 == 4: + if cledge == 0: + ax1 = 1 + ax2 = 2 + if cledge == 1: + ax1 = 2 + ax2 = 0 + if cledge == 2: + ax1 = 0 + ax2 = 1 + ax = cledge + + # Then it finds with which face the capsule has a lower angle and switches the axis names + if wp.abs(axis[ax1]) > wp.abs(axis[ax2]): + ax1 = ax2 + ax2 = 3 - ax - ax1 + + # keep track of the axis orientation (mul will tell us which direction along the capsule to + # find the second point) you can notice all other references to the axis "halfaxis" are with + # absolute value + + if c1 & (1 << ax2): + mul = 1 + secondpos = 1.0 - bestsegmentpos + else: + mul = -1 + secondpos = 1.0 + bestsegmentpos + + # now we have to find out whether we point towards the opposite side or towards one of the + # sides and also find the farthest point along the capsule that is above the box + + e1 = 2.0 * box.size[ax2] / wp.abs(halfaxis[ax2]) + secondpos = min(e1, secondpos) + + if ((axisdir & (1 << ax)) != 0) == ((c1 & (1 << ax2)) != 0): # that is insane + e2 = 1.0 - bestboxpos + else: + e2 = 1.0 + bestboxpos + + e1 = box.size[ax] * e2 / wp.abs(halfaxis[ax]) + + secondpos = min(e1, secondpos) + secondpos *= wp.float32(mul) + + elif cltype < 0: + # similarly we handle the case when one capsule's end is closest to a face of the box + # and find where is the other end pointing to and clamping to the farthest point + # of the capsule that's above the box + # if the closest point is inside the box there's no need for a second point + + if clface != -1: + mul = wp.where(cltype == -3, 1, -1) + secondpos = 2.0 + + tmp1 = pos - halfaxis * wp.float32(mul) + + for i in range(3): + if i != clface: + ha_r = wp.float32(mul) / halfaxis[i] + e1 = (box.size[i] - tmp1[i]) * ha_r + if 0 < e1 and e1 < secondpos: + secondpos = e1 + + e1 = (-box.size[i] - tmp1[i]) * ha_r + if 0 < e1 and e1 < secondpos: + secondpos = e1 + + secondpos *= wp.float32(mul) + + # skip: + # create sphere in original orientation at first contact point + s1_pos_l = pos + halfaxis * bestsegmentpos + s1_pos_g = box.rot @ s1_pos_l + box.pos + + # collide with sphere + _sphere_box_raw( + s1_pos_g, cap.size[0], box.pos, box.rot, box.size, margin, geom_indices, worldid, d + ) + + if secondpos > -3: # secondpos was modified + s2_pos_l = pos + halfaxis * (secondpos + bestsegmentpos) + s2_pos_g = box.rot @ s2_pos_l + box.pos + _sphere_box_raw( + s2_pos_g, cap.size[0], box.pos, box.rot, box.size, margin, geom_indices, worldid, d + ) + + @wp.kernel def _primitive_narrowphase( m: Model, @@ -434,6 +789,8 @@ def _primitive_narrowphase( sphere_box(geom1, geom2, worldid, d, margin, geoms) elif type1 == int(GeomType.PLANE.value) and type2 == int(GeomType.CYLINDER.value): plane_cylinder(geom1, geom2, worldid, d, margin, geoms) + elif type1 == int(GeomType.CAPSULE.value) and type2 == int(GeomType.BOX.value): + capsule_box(geom1, geom2, worldid, d, margin, geoms) def primitive_narrowphase(m: Model, d: Data): From 3948e0fad75c97e695a851a6bb16570028c97838 Mon Sep 17 00:00:00 2001 From: camevor Date: Wed, 9 Apr 2025 18:13:14 +0200 Subject: [PATCH 02/10] Add capsule-box test --- mujoco_warp/_src/collision_driver_test.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/mujoco_warp/_src/collision_driver_test.py b/mujoco_warp/_src/collision_driver_test.py index 0b86e9d6..b18e9559 100644 --- a/mujoco_warp/_src/collision_driver_test.py +++ b/mujoco_warp/_src/collision_driver_test.py @@ -171,6 +171,21 @@ class CollisionTest(parameterized.TestCase): """ + _CAPSULE_BOX = """ + + + + + + + + + + + + + + """ @parameterized.parameters( (_BOX_PLANE), @@ -181,6 +196,7 @@ class CollisionTest(parameterized.TestCase): (_SPHERE_CAPSULE), (_SPHERE_BOX), (_CAPSULE_CAPSULE), + (_CAPSULE_BOX), (_PLANE_CYLINDER_1), (_PLANE_CYLINDER_2), (_PLANE_CYLINDER_3), From a624aa785f2b9bf4f6c882241b57781de1bacf38 Mon Sep 17 00:00:00 2001 From: camevor Date: Wed, 9 Apr 2025 18:13:52 +0200 Subject: [PATCH 03/10] Ruff format --- mujoco_warp/_src/collision_primitive.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/mujoco_warp/_src/collision_primitive.py b/mujoco_warp/_src/collision_primitive.py index 6dc01e2e..eb970627 100644 --- a/mujoco_warp/_src/collision_primitive.py +++ b/mujoco_warp/_src/collision_primitive.py @@ -641,14 +641,13 @@ def capsule_box( ax1 = 0 ax2 = 1 - if axis[ax]*axis[ax] > 0.5: # second point along the edge of the box + if axis[ax] * axis[ax] > 0.5: # second point along the edge of the box m = 2.0 * box.size[ax] / wp.abs(halfaxis[ax]) secondpos = min(1.0 - wp.float32(mul) * bestsegmentpos, m) else: # second point along a face of the box # check for overshoot again m = 2.0 * min( - box.size[ax1] / wp.abs(halfaxis[ax1]), - box.size[ax2] / wp.abs(halfaxis[ax2]) + box.size[ax1] / wp.abs(halfaxis[ax1]), box.size[ax2] / wp.abs(halfaxis[ax2]) ) secondpos = -min(1.0 + wp.float32(mul) * bestsegmentpos, m) secondpos *= wp.float32(mul) @@ -745,7 +744,15 @@ def capsule_box( s2_pos_l = pos + halfaxis * (secondpos + bestsegmentpos) s2_pos_g = box.rot @ s2_pos_l + box.pos _sphere_box_raw( - s2_pos_g, cap.size[0], box.pos, box.rot, box.size, margin, geom_indices, worldid, d + s2_pos_g, + cap.size[0], + box.pos, + box.rot, + box.size, + margin, + geom_indices, + worldid, + d, ) From d5953424db1d52a51290b8034fcb9d8cd7146046 Mon Sep 17 00:00:00 2001 From: camevor Date: Thu, 10 Apr 2025 19:45:17 +0200 Subject: [PATCH 04/10] Change capsule-box test to break symmetry Originally, the capsule axis was at a 45 deg angle and intersected the cube, with the symmetry being broken differently from mjc (contact with same distance but different position & frame). --- mujoco_warp/_src/collision_driver_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mujoco_warp/_src/collision_driver_test.py b/mujoco_warp/_src/collision_driver_test.py index b18e9559..59e6bd0a 100644 --- a/mujoco_warp/_src/collision_driver_test.py +++ b/mujoco_warp/_src/collision_driver_test.py @@ -175,7 +175,7 @@ class CollisionTest(parameterized.TestCase): - + From bef7dc245fad49dd2987934956db9bb649b5be96 Mon Sep 17 00:00:00 2001 From: camevor Date: Fri, 11 Apr 2025 10:43:06 +0200 Subject: [PATCH 05/10] Re-add capsule-box tests --- mujoco_warp/_src/collision_driver_test.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/mujoco_warp/_src/collision_driver_test.py b/mujoco_warp/_src/collision_driver_test.py index f006b2bb..0c99a57b 100644 --- a/mujoco_warp/_src/collision_driver_test.py +++ b/mujoco_warp/_src/collision_driver_test.py @@ -224,6 +224,28 @@ class CollisionTest(parameterized.TestCase): """, + "capsule_box_cap": """ + + + + + + + + + + """, + "capsule_box_edge": """ + + + + + + + + + + """, } @parameterized.parameters(_FIXTURES.keys()) From 84fe78aeae765a68bb23ee97e8ff111d468362f7 Mon Sep 17 00:00:00 2001 From: camevor Date: Fri, 11 Apr 2025 11:39:11 +0200 Subject: [PATCH 06/10] Remove redundant MINVAL constant --- mujoco_warp/_src/collision_primitive.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mujoco_warp/_src/collision_primitive.py b/mujoco_warp/_src/collision_primitive.py index c87e19bd..afeadaec 100644 --- a/mujoco_warp/_src/collision_primitive.py +++ b/mujoco_warp/_src/collision_primitive.py @@ -19,11 +19,11 @@ from .math import closest_segment_to_segment_points from .math import make_frame from .math import normalize_with_norm +from .types import MJ_MINVAL from .types import Data from .types import GeomType from .types import Model -_MINVAL = 1e-15 _TINY_VAL = 1e-6 @@ -643,7 +643,7 @@ def capsule_box( mb = -box.size[j] * halfaxis[j] mc = cap.size[1] * cap.size[1] det = ma * mc - mb * mb - if wp.abs(det) < _MINVAL: + if wp.abs(det) < MJ_MINVAL: continue idet = 1.0 / det @@ -688,7 +688,7 @@ def capsule_box( ct = s1 * 3 + s2 dif_sq = wp.length_sq(dif) - if dif_sq < bestdist - _MINVAL: + if dif_sq < bestdist - MJ_MINVAL: bestdist = dif_sq bestsegmentpos = x2 bestboxpos = x1 From dac4be3ef22a09b56f76de69590164ae337e0f59 Mon Sep 17 00:00:00 2001 From: camevor Date: Wed, 23 Apr 2025 16:02:55 +0200 Subject: [PATCH 07/10] Fix lost collision() call --- mujoco_warp/_src/collision_driver_test.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/mujoco_warp/_src/collision_driver_test.py b/mujoco_warp/_src/collision_driver_test.py index 366f6353..0a8ba5c0 100644 --- a/mujoco_warp/_src/collision_driver_test.py +++ b/mujoco_warp/_src/collision_driver_test.py @@ -268,7 +268,10 @@ class CollisionTest(parameterized.TestCase): @parameterized.parameters(_FIXTURES.keys()) def test_collision(self, fixture): """Tests convex collision with different geometries.""" - _, mjd, _, d = test_util.fixture(xml=self._FIXTURES[fixture]) + mjm, mjd, m, d = test_util.fixture(xml=self._FIXTURES[fixture]) + + mujoco.mj_collision(mjm, mjd) + mjwarp.collision(m, d) for i in range(mjd.ncon): actual_dist = mjd.contact.dist[i] From af225f2ca0e28ebd909ae7800d3e3414f8da633e Mon Sep 17 00:00:00 2001 From: camevor Date: Thu, 24 Apr 2025 12:02:00 +0200 Subject: [PATCH 08/10] Clean up comments - Add explanations - Rephrase vague comments - Remove obsolete comments --- mujoco_warp/_src/collision_primitive.py | 87 +++++++++++-------------- 1 file changed, 39 insertions(+), 48 deletions(-) diff --git a/mujoco_warp/_src/collision_primitive.py b/mujoco_warp/_src/collision_primitive.py index a340dffa..ffef2655 100644 --- a/mujoco_warp/_src/collision_primitive.py +++ b/mujoco_warp/_src/collision_primitive.py @@ -911,10 +911,10 @@ def capsule_box( geoms: wp.vec2i, ): """Calculates contacts between a capsule and a box.""" - # Ported from the mjc implementation + # Based on the mjc implementation pos = wp.transpose(box.rot) @ (cap.pos - box.pos) axis = wp.vec3(cap.rot[0, 2], cap.rot[1, 2], cap.rot[2, 2]) - halfaxis = axis * cap.size[1] + halfaxis = axis * cap.size[1] # halfaxis is the capsule direction axisdir = ( wp.int32(axis[0] > 0.0) + 2 * wp.int32(axis[1] > 0.0) + 4 * wp.int32(axis[2] > 0.0) ) @@ -926,10 +926,22 @@ def capsule_box( # keep track of closest point bestdist = wp.float32(bestdistmax) bestsegmentpos = wp.float32(-12) + + # cltype: encoded collision configuration + # cltype / 3 == 0 : lower corner is closest to the capsule + # == 2 : upper corner is closest to the capsule + # == 1 : middle of the edge is closest to the capsule + # cltype % 3 == 0 : lower corner is closest to the box + # == 2 : upper corner is closest to the box + # == 1 : middle of the capsule is closest to the box cltype = wp.int32(-4) + + # clface: index of the closest face of the box to the capsule + # -1: no face is closest (edge or corner is closest) + # 0, 1, 2: index of the axis perpendicular to the closest face clface = wp.int32(-12) - # check if face closest + # first: consider cases where a face of the box is closest for i in range(-1, 2, 2): axisTip = pos + wp.float32(i) * halfaxis boxPoint = wp.vec3(axisTip) @@ -958,8 +970,7 @@ def capsule_box( cltype = -2 + i clface = ax_out - # check edge edge - + # second: consider cases where an edge of the box is closest clcorner = wp.int32(-123) # which corner is the closest cledge = wp.int32(-123) # which axis bestboxpos = wp.float32(0.0) @@ -969,10 +980,9 @@ def capsule_box( if i & (1 << j) != 0: continue - # c1<6 means that closest point on the box is at the lower end or in the middle of the edge - c2 = wp.int32(-123) - # trick to get a corner + + # box_pt is the starting point (corner) on the box box_pt = wp.cw_mul( wp.vec3( wp.where(i & 1, 1.0, -1.0), @@ -983,11 +993,6 @@ def capsule_box( ) box_pt[j] = 0.0 - # box_pt is the starting point on the box - # box_dir is the direction along the "j"-th axis - # pos is the capsule's center - # halfaxis is the capsule direction - # find closest point between capsule and the edge dif = box_pt - pos @@ -1039,6 +1044,7 @@ def capsule_box( dif -= halfaxis * x2 dif[j] += box.size[j] * x1 + # encode relative positions of the closest points ct = s1 * 3 + s2 dif_sq = wp.length_sq(dif) @@ -1046,19 +1052,12 @@ def capsule_box( bestdist = dif_sq bestsegmentpos = x2 bestboxpos = x1 - # ct<6 means that closest point on the box is at the lower end or in the middle of the edge + # ct<6 means closest point on box is at lower end or middle of edge c2 = ct / 6 - # cltype /3 == 0 means the lower corner is closest to the capsule - # cltype /3 == 2 means the upper corner is closest to the capsule - # cltype /3 == 1 means the middle of the edge is closest to the capsule - # cltype %3 == 0 means the lower corner is closest to the box - # cltype %3 == 2 means the upper corner is closest to the box - # cltype %3 == 1 means the middle of the capsule is closest to the box - # note that edges include corners - - clcorner = i + (1 << j) * c2 # which corner is the closest - cledge = j # which axis - cltype = ct # save clamped info + + clcorner = i + (1 << j) * c2 # index of closest box corner + cledge = j # axis index of closest box edge + cltype = ct # encoded collision configuration best = wp.float32(0.0) l = wp.float32(0.0) @@ -1072,7 +1071,6 @@ def capsule_box( uu = dd.x * s.y vv = dd.y * s.x - # w = dd.x * p.y - dd.y * p.x w_neg = dd.x * p.y - dd.y * p.x < 0 best = wp.float32(-1.0) @@ -1093,13 +1091,11 @@ def capsule_box( if cltype >= 0 and cltype / 3 != 1: # closest to a corner of the box c1 = axisdir ^ clcorner - # hack to find the relative orientation of capsule and corner - # there are 2 cases: - # 1: pointing to or away from the corner - # 2: oriented along a face or an edge - - # case 1: no chance of additional contact - if c1 != 0 and c1 != 7: + # Calculate relative orientation between capsule and corner + # There are two possible configurations: + # 1. Capsule axis points toward/away from corner + # 2. Capsule axis aligns with a face or edge + if c1 != 0 and c1 != 7: # create second contact point if c1 == 1 or c1 == 2 or c1 == 4: mul = 1 else: @@ -1134,15 +1130,14 @@ def capsule_box( secondpos *= wp.float32(mul) elif cltype >= 0 and cltype / 3 == 1: # we are on box's edge - # hacks to find the relative orientation of capsule and edge - # there are 2 cases: - # c1= 2^n: edge and capsule are oriented in a T configuration (no more contacts - # c1!=2^n: oriented in a cross X configuration - c1 = axisdir ^ clcorner # same trick - - c1 &= 7 - (1 << cledge) # even more hacks + # Calculate relative orientation between capsule and edge + # Two possible configurations: + # - T configuration: c1 = 2^n (no additional contacts) + # - X configuration: c1 != 2^n (potential additional contacts) + c1 = axisdir ^ clcorner + c1 &= 7 - (1 << cledge) # mask out edge axis to determine configuration - if c1 == 1 or c1 == 2 or c1 == 4: + if c1 == 1 or c1 == 2 or c1 == 4: # create second contact point if cledge == 0: ax1 = 1 ax2 = 2 @@ -1159,10 +1154,7 @@ def capsule_box( ax1 = ax2 ax2 = 3 - ax - ax1 - # keep track of the axis orientation (mul will tell us which direction along the capsule to - # find the second point) you can notice all other references to the axis "halfaxis" are with - # absolute value - + # mul determines direction along capsule axis for second contact point if c1 & (1 << ax2): mul = 1 secondpos = 1.0 - bestsegmentpos @@ -1176,7 +1168,7 @@ def capsule_box( e1 = 2.0 * box.size[ax2] / wp.abs(halfaxis[ax2]) secondpos = min(e1, secondpos) - if ((axisdir & (1 << ax)) != 0) == ((c1 & (1 << ax2)) != 0): # that is insane + if ((axisdir & (1 << ax)) != 0) == ((c1 & (1 << ax2)) != 0): e2 = 1.0 - bestboxpos else: e2 = 1.0 + bestboxpos @@ -1192,7 +1184,7 @@ def capsule_box( # of the capsule that's above the box # if the closest point is inside the box there's no need for a second point - if clface != -1: + if clface != -1: # create second contact point mul = wp.where(cltype == -3, 1, -1) secondpos = 2.0 @@ -1211,7 +1203,6 @@ def capsule_box( secondpos *= wp.float32(mul) - # skip: # create sphere in original orientation at first contact point s1_pos_l = pos + halfaxis * bestsegmentpos s1_pos_g = box.rot @ s1_pos_l + box.pos From 9a449f464be6e643c59cc21abbef9f0e562d62d5 Mon Sep 17 00:00:00 2001 From: camevor Date: Thu, 24 Apr 2025 14:44:33 +0200 Subject: [PATCH 09/10] Add more capsule<>box tests --- mujoco_warp/_src/collision_driver_test.py | 31 ++++++++++++++++++++--- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/mujoco_warp/_src/collision_driver_test.py b/mujoco_warp/_src/collision_driver_test.py index 1dc6aaed..7b6e7956 100644 --- a/mujoco_warp/_src/collision_driver_test.py +++ b/mujoco_warp/_src/collision_driver_test.py @@ -225,30 +225,53 @@ class CollisionTest(parameterized.TestCase): """, - "capsule_box_cap": """ + "capsule_box_edge": """ - + """, - "capsule_box_edge": """ + "capsule_box_corner": """ + + + + + + + + + + """, + "capsule_box_face_tip": """ - + """, + "capsule_box_face_flat": """ + + + + + + + + + + """, } + # Temporarily disabled # "box_mesh": """ # From 709c5f79c9271e93d654f47ca97557d4ca42defa Mon Sep 17 00:00:00 2001 From: camevor Date: Thu, 24 Apr 2025 14:52:50 +0200 Subject: [PATCH 10/10] Add strict contact count check --- mujoco_warp/_src/collision_driver_test.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/mujoco_warp/_src/collision_driver_test.py b/mujoco_warp/_src/collision_driver_test.py index 7b6e7956..2df62836 100644 --- a/mujoco_warp/_src/collision_driver_test.py +++ b/mujoco_warp/_src/collision_driver_test.py @@ -271,7 +271,6 @@ class CollisionTest(parameterized.TestCase): """, } - # Temporarily disabled # "box_mesh": """ # @@ -295,6 +294,10 @@ def test_collision(self, fixture): """Tests convex collision with different geometries.""" mjm, mjd, m, d = test_util.fixture(xml=self._FIXTURES[fixture]) + # Exempt GJK collisions from exact contact count check + # because GJK generates more contacts + allow_different_contact_count = False + mujoco.mj_collision(mjm, mjd) mjwarp.collision(m, d) @@ -302,7 +305,6 @@ def test_collision(self, fixture): actual_dist = mjd.contact.dist[i] actual_pos = mjd.contact.pos[i] actual_frame = mjd.contact.frame[i] - # This is because Gjk generates more contact result = False for j in range(d.ncon.numpy()[0]): test_dist = d.contact.dist.numpy()[j] @@ -316,6 +318,9 @@ def test_collision(self, fixture): break np.testing.assert_equal(result, True, f"Contact {i} not found in Gjk results") + if not allow_different_contact_count: + self.assertEqual(d.ncon.numpy()[0], mjd.ncon) + def test_contact_exclude(self): """Tests contact exclude.""" mjm = mujoco.MjModel.from_xml_string("""