diff --git a/mjx/mujoco/mjx/_src/collision_convex.py b/mjx/mujoco/mjx/_src/collision_convex.py index 30e64401..ce3e41e2 100644 --- a/mjx/mujoco/mjx/_src/collision_convex.py +++ b/mjx/mujoco/mjx/_src/collision_convex.py @@ -331,10 +331,12 @@ def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact: degenerate_edge_dir = jp.sum(jp.square(edge_dir)) < 1e-6 edge_dir, edge_dist = math.normalize_with_norm(edge_dir) face_edge_normals = convex.face_edge_normal[best_idx][e_idx] - in_edge_voronoi = ((face_edge_normals @ edge_dir) < 0).all() + inside_edge_voronoi_front = ((face_edge_normals @ edge_dir) < 0).all() + inside_edge_voronoi_back = ((face_edge_normals @ edge_dir) > 0).all() + outside_edge_voronoi = ~inside_edge_voronoi_front & ~inside_edge_voronoi_back edge_axis = math.normalize(-edge_closest_pt) # approximate edge axis edge_axis = jp.where( - ~degenerate_edge_dir & in_edge_voronoi, + ~degenerate_edge_dir & inside_edge_voronoi_front, edge_dir, # shallow edge penetration edge_axis, # deep edge penetration ) @@ -348,7 +350,11 @@ def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact: edge_dir_parallel_to_face = ( jp.abs(edge_dir.dot(normal)) > 0.99 ) & ~degenerate_edge_dir - has_edge_contact = (edge_penetration > 0) & ~edge_dir_parallel_to_face + has_edge_contact = ( + (edge_penetration > 0) + & ~edge_dir_parallel_to_face + & ~outside_edge_voronoi + ) # Get the contact info. pos = jp.where(has_edge_contact, pos.at[0].set(edge_pos), pos) diff --git a/mjx/mujoco/mjx/_src/collision_driver_test.py b/mjx/mujoco/mjx/_src/collision_driver_test.py index c914ad5c..858e8795 100644 --- a/mjx/mujoco/mjx/_src/collision_driver_test.py +++ b/mjx/mujoco/mjx/_src/collision_driver_test.py @@ -323,6 +323,8 @@ class CapsuleCollisionTest(parameterized.TestCase): def test_capsule_convex_edge_shallow_tip(self): """Tests shallow edge penetration on the tip of the capsule.""" + # the capsule sphere is inside the edge voronoi region, so there is an + # edge contact xml = self._CAP_EDGE_BOX.replace( '