[FEATURE] Add analytical sphere-sphere collision detection.

genesis/engine/solvers/rigid/collider/capsule_contact.py

@@ -13,16 +13,14 @@ def func_capsule_capsule_contact(
     """
     Analytical capsule-capsule collision detection.
 
+    The contact derives from the two capsule poses (ga_pos/ga_quat, gb_pos/gb_quat) and radii. The poses are passed
+    in rather than read from the geom state so the multi-contact loop can perturb them.
+
     A capsule is defined as a line segment plus a radius (swept sphere).
     Collision between two capsules reduces to:
       1. Find closest points on the two line segments (analytical)
       2. Check if distance < sum of radii
       3. Compute contact point and normal
-
-    Parameters
-    ----------
-    ga_pos, ga_quat : Position and orientation of capsule A (may be perturbed for multi-contact).
-    gb_pos, gb_quat : Position and orientation of capsule B (may be perturbed for multi-contact).
     """
     EPS = rigid_info.EPS[None]
 
@@ -91,22 +89,69 @@ def func_capsule_capsule_contact(
     return is_col, normal_unit, contact_pos, penetration
 
 
+@qd.func
+def func_sphere_sphere_contact(
+    i_ga, i_gb, ga_pos, gb_pos, dyn_info: array_class.DynInfo, rigid_info: array_class.RigidInfo
+):
+    """
+    Analytical sphere-sphere collision detection.
+
+    The contact derives from the two sphere centers and radii. The centers are passed in rather than read from the
+    geom state so the multi-contact loop can perturb them.
+
+    A sphere-sphere collision is a closed form:
+      1. Distance between the two centers
+      2. Check if distance < sum of radii
+      3. Compute contact point and normal
+
+    This avoids routing the pair through the iterative MPR/GJK+EPA path. It is also exactly differentiable, where
+    EPA fails to converge on the smoothly-curved sphere-sphere Minkowski boundary.
+    """
+    EPS = rigid_info.EPS[None]
+
+    radius_a = dyn_info.geoms.data[i_ga][0]
+    radius_b = dyn_info.geoms.data[i_gb][0]
+
+    # Vector from sphere B center to sphere A center (normal points into geom A, i.e. B to A).
+    diff = ga_pos - gb_pos
+    dist_sq = diff.dot(diff)
+    combined_radius = radius_a + radius_b
+    combined_radius_sq = combined_radius * combined_radius
+
+    is_col = False
+    normal_unit = qd.Vector([1.0, 0.0, 0.0], dt=gs.qd_float)
+    contact_pos = qd.Vector.zero(gs.qd_float, 3)
+    penetration = gs.qd_float(0.0)
+    if dist_sq < combined_radius_sq:
+        is_col = True
+        dist = qd.sqrt(dist_sq)
+
+        # Coincident centers keep the arbitrary default direction.
+        if dist > EPS:
+            normal_unit = diff / dist
+
+        penetration = combined_radius - dist
+        # Contact position at midpoint between the two surfaces
+        contact_pos = ga_pos - (radius_a - 0.5 * penetration) * normal_unit
+
+    return is_col, normal_unit, contact_pos, penetration
+
+
 @qd.func
 def func_sphere_capsule_contact(
     i_ga, i_gb, ga_pos, ga_quat, gb_pos, gb_quat, dyn_info: array_class.DynInfo, rigid_info: array_class.RigidInfo
 ):
     """
     Analytical sphere-capsule collision detection.
 
+    The contact derives from the sphere center ga_pos, the capsule pose (gb_pos, gb_quat), and their radii (the
+    sphere orientation is unused). The poses are passed in rather than read from the geom state so the multi-contact
+    loop can perturb them.
+
     A sphere-capsule collision reduces to:
       1. Find closest point on the capsule's line segment to sphere center
       2. Check if distance < sum of radii
       3. Compute contact point and normal
-
-    Parameters
-    ----------
-    ga_pos, ga_quat : Position and orientation of geom A (may be perturbed for multi-contact).
-    gb_pos, gb_quat : Position and orientation of geom B (may be perturbed for multi-contact).
     """
     EPS = rigid_info.EPS[None]
 

genesis/engine/solvers/rigid/collider/collider.py

@@ -521,13 +521,15 @@ def _compute_collision_pair_idx(self):
             # Differentiable contact detection (diff_gjk) reconstructs each contact from a triangular face of the
             # Minkowski difference. A sphere or ellipsoid has no flat facet, so a pair of them yields an everywhere
             # smoothly curved Minkowski boundary on which EPA never converges, and no contact is ever generated -
-            # the bodies silently tunnel. Faceted partners (box, mesh) and the analytical plane branch are unaffected.
+            # the bodies silently tunnel. Faceted partners (box, mesh) and the analytical plane branch are unaffected,
+            # and sphere-sphere pairs are reconstructed in closed form (func_differentiable_sphere_contact).
             if self._solver._requires_grad:
                 is_smooth_a = (valid_type_a == gs.GEOM_TYPE.SPHERE) | (valid_type_a == gs.GEOM_TYPE.ELLIPSOID)
                 is_smooth_b = (valid_type_b == gs.GEOM_TYPE.SPHERE) | (valid_type_b == gs.GEOM_TYPE.ELLIPSOID)
-                if np.any(both_convex & ~specialized & is_smooth_a & is_smooth_b):
+                is_sphere_sphere = (valid_type_a == gs.GEOM_TYPE.SPHERE) & (valid_type_b == gs.GEOM_TYPE.SPHERE)
+                if np.any(both_convex & ~specialized & is_smooth_a & is_smooth_b & ~is_sphere_sphere):
                     gs.raise_exception(
-                        "Differentiable contact detection is not supported for sphere-sphere, sphere-ellipsoid or "
+                        "Differentiable contact detection is not supported for sphere-ellipsoid or "
                         "ellipsoid-ellipsoid collision pairs (requires_grad=True). Approximate them with a faceted "
                         "geometry (e.g. a convex mesh) or disable requires_grad."
                     )
@@ -972,10 +974,11 @@ def detection(self) -> None:
                 self._solver._errno,
             )
 
-        # Plane-convex contacts come from analytic paths that leave diff_contact_input unfilled; populate it here so
-        # the differentiable narrow-phase reverse can reconstruct them (see kernel_fill_diff_contact_input_plane).
+        # Plane-convex and sphere-sphere contacts come from analytic paths that leave diff_contact_input unfilled;
+        # populate it here so the differentiable narrow-phase reverse can reconstruct them (see
+        # kernel_fill_diff_contact_input_analytic).
         if self._solver.rigid_config.requires_grad:
-            narrowphase.kernel_fill_diff_contact_input_plane(
+            narrowphase.kernel_fill_diff_contact_input_analytic(
                 self._solver.dyn_state, self._collider_state, self._solver.dyn_info, self._solver.rigid_config
             )
 
@@ -1129,6 +1132,7 @@ def backward_narrowphase(self):
             self._collider_state,
             self._collider_state.diff_contact_input,
             self._solver.dyn_info,
+            self._solver.rigid_info,
             self._collider_info,
             self._solver.rigid_config,
             self._solver._errno,

genesis/engine/solvers/rigid/collider/diff_gjk.py

@@ -853,6 +853,40 @@ def func_plane_contact_frame(
     return normal, radius
 
 
+@qd.func
+def func_differentiable_sphere_contact(
+    i_ga, i_gb, i_b, dyn_state: array_class.DynState, dyn_info: array_class.DynInfo, rigid_info: array_class.RigidInfo
+):
+    """Differentiable sphere-sphere contact reconstruction.
+
+    The contact is a smooth closed form of the two geom centers, so gradients flow directly through
+    [dyn_state.geoms.pos]; the Minkowski-triangle reconstruction of func_differentiable_contact is degenerate for two
+    smoothly-curved surfaces, on which EPA never converges. Must match func_sphere_sphere_contact in
+    capsule_contact.py.
+
+    At coincident centers the distance is a non-differentiable cusp: the normal (delta / dist) and the distance's own
+    gradient (also delta / dist) are both 0/0 there, which reverse-mode turns into NaN. The sqrt only runs when the
+    centers are separated; otherwise the same arbitrary direction and constant penetration as the forward hold (the
+    contact is degenerate there anyway).
+    """
+    EPS = rigid_info.EPS[None]
+
+    radius_a = dyn_info.geoms.data[i_ga][0]
+    radius_b = dyn_info.geoms.data[i_gb][0]
+    delta = dyn_state.geoms.pos[i_ga, i_b] - dyn_state.geoms.pos[i_gb, i_b]
+    dist_sq = delta.dot(delta)
+
+    contact_normal = qd.Vector([1.0, 0.0, 0.0], dt=gs.qd_float)
+    penetration = radius_a + radius_b
+    if dist_sq > EPS * EPS:
+        dist = qd.sqrt(dist_sq)
+        contact_normal = delta / dist
+        penetration = radius_a + radius_b - dist
+
+    contact_pos = dyn_state.geoms.pos[i_ga, i_b] - (radius_a - 0.5 * penetration) * contact_normal
+    return contact_pos, contact_normal, penetration, gs.qd_float(1.0)
+
+
 @qd.func
 def func_differentiable_plane_contact(
     i_ga,
@@ -873,7 +907,7 @@ def func_differentiable_plane_contact(
 
     [i_ga] is the plane geom and [i_gb] the convex geom. [core_local] (box vertex / sphere center / capsule nearest
     endpoint, in the convex geom's local frame) is the pose-independent witness stored by
-    kernel_fill_diff_contact_input_plane; [radius] and the plane direction come from the geoms info. Gradients flow to
+    kernel_fill_diff_contact_input_analytic; [radius] and the plane direction come from the geoms info. Gradients flow to
     both geom poses through the geoms state pos / quat. For a sphere, [core_local] is the local origin, so the
     orientation gradient is zero, matching the rotation-invariant forward contact.
     """

genesis/engine/solvers/rigid/collider/narrowphase.py

@@ -1860,6 +1860,13 @@ def func_convex_convex_contact(
                         dyn_info,
                         rigid_info,
                     )
+                elif (
+                    dyn_info.geoms.type[i_ga] == gs.GEOM_TYPE.SPHERE
+                    and dyn_info.geoms.type[i_gb] == gs.GEOM_TYPE.SPHERE
+                ):
+                    is_col, normal, contact_pos, penetration = capsule_contact.func_sphere_sphere_contact(
+                        i_ga, i_gb, ga_pos_current, gb_pos_current, dyn_info, rigid_info
+                    )
                 elif (
                     dyn_info.geoms.type[i_ga] == gs.GEOM_TYPE.SPHERE
                     and dyn_info.geoms.type[i_gb] == gs.GEOM_TYPE.CAPSULE
@@ -2246,6 +2253,10 @@ def _func_multicontact_run_detection(
         is_col, normal, contact_pos, penetration = capsule_contact.func_capsule_capsule_contact(
             i_ga, i_gb, ga_pos, ga_quat, gb_pos, gb_quat, dyn_info, rigid_info
         )
+    elif dyn_info.geoms.type[i_ga] == gs.GEOM_TYPE.SPHERE and dyn_info.geoms.type[i_gb] == gs.GEOM_TYPE.SPHERE:
+        is_col, normal, contact_pos, penetration = capsule_contact.func_sphere_sphere_contact(
+            i_ga, i_gb, ga_pos, gb_pos, dyn_info, rigid_info
+        )
     elif dyn_info.geoms.type[i_ga] == gs.GEOM_TYPE.SPHERE and dyn_info.geoms.type[i_gb] == gs.GEOM_TYPE.CAPSULE:
         is_col, normal, contact_pos, penetration = capsule_contact.func_sphere_capsule_contact(
             i_ga, i_gb, ga_pos, ga_quat, gb_pos, gb_quat, dyn_info, rigid_info
@@ -2833,6 +2844,10 @@ def _func_narrowphase_contact0(
                 is_col, normal, contact_pos, penetration = capsule_contact.func_capsule_capsule_contact(
                     i_ga, i_gb, ga_pos, ga_quat, gb_pos, gb_quat, dyn_info, rigid_info
                 )
+            elif dyn_info.geoms.type[i_ga] == gs.GEOM_TYPE.SPHERE and dyn_info.geoms.type[i_gb] == gs.GEOM_TYPE.SPHERE:
+                is_col, normal, contact_pos, penetration = capsule_contact.func_sphere_sphere_contact(
+                    i_ga, i_gb, ga_pos, gb_pos, dyn_info, rigid_info
+                )
             elif dyn_info.geoms.type[i_ga] == gs.GEOM_TYPE.SPHERE and dyn_info.geoms.type[i_gb] == gs.GEOM_TYPE.CAPSULE:
                 is_col, normal, contact_pos, penetration = capsule_contact.func_sphere_capsule_contact(
                     i_ga, i_gb, ga_pos, ga_quat, gb_pos, gb_quat, dyn_info, rigid_info
@@ -3051,6 +3066,7 @@ def func_narrow_phase_diff_convex_vs_convex(
     collider_state: array_class.ColliderState,
     diff_contact_input: array_class.DiffContactInput,
     dyn_info: array_class.DynInfo,
+    rigid_info: array_class.RigidInfo,
     collider_info: array_class.ColliderInfo,
     rigid_config: qd.template(),
     errno: qd.Tensor,
@@ -3074,6 +3090,13 @@ def func_narrow_phase_diff_convex_vs_convex(
                     contact_pos, contact_normal, penetration, weight = diff_gjk.func_differentiable_plane_contact(
                         i_ga, i_gb, i_b, i_c, dyn_state, diff_contact_input, dyn_info
                     )
+                elif (
+                    dyn_info.geoms.type[i_ga] == gs.GEOM_TYPE.SPHERE
+                    and dyn_info.geoms.type[i_gb] == gs.GEOM_TYPE.SPHERE
+                ):
+                    contact_pos, contact_normal, penetration, weight = diff_gjk.func_differentiable_sphere_contact(
+                        i_ga, i_gb, i_b, dyn_state, dyn_info, rigid_info
+                    )
                 else:
                     contact_pos, contact_normal, penetration, weight = diff_gjk.func_differentiable_contact(
                         i_ga, i_gb, i_b, i_c, ref_penetration, dyn_state, diff_contact_input, collider_info
@@ -3128,29 +3151,36 @@ def func_narrow_phase_diff_convex_vs_convex(
 
 
 @qd.kernel(fastcache=True)
-def kernel_fill_diff_contact_input_plane(
+def kernel_fill_diff_contact_input_analytic(
     dyn_state: array_class.DynState,
     collider_state: array_class.ColliderState,
     dyn_info: array_class.DynInfo,
     rigid_config: qd.template(),
 ):
-    """Populate diff_contact_input for plane-convex contacts.
+    """Populate diff_contact_input for the contacts of analytic detection paths.
 
-    The analytic plane paths (func_plane_box_contact, the plane branch of func_convex_convex_contact) leave
-    diff_contact_input unfilled, so the differentiable narrow-phase reverse would have nothing to reconstruct. Both
-    paths share the convention contact_pos = v - 0.5 * penetration * normal with
+    The analytic paths leave diff_contact_input unfilled, so the differentiable narrow-phase reverse would have
+    nothing to reconstruct. The plane paths (func_plane_box_contact, the plane branch of func_convex_convex_contact)
+    share the convention contact_pos = v - 0.5 * penetration * normal with
     normal = -normalize(R(quat_plane) @ plane_local_dir), so the convex support point is recovered as
     v = contact_pos + 0.5 * penetration * normal, and its pose-independent "core" (box vertex / sphere center /
     capsule nearest endpoint) as v - radius * normal, stored in the convex geom's local frame. PLANE is the smallest
-    GEOM_TYPE so it is always geom_a after the canonical type-ordered swap.
+    GEOM_TYPE so it is always geom_a after the canonical type-ordered swap. A sphere-sphere contact
+    (func_sphere_sphere_contact) is reconstructed in closed form from the geom centers and radii alone, so only the
+    geom identities and a self-referential ref_id are stored.
     """
     _B = collider_state.active_buffer.shape[1]
     qd.loop_config(serialize=rigid_config.para_level < gs.PARA_LEVEL.PARTIAL)
     for i_c, i_b in qd.ndrange(collider_state.contact_data.pos.shape[0], _B):
         if i_c < collider_state.n_contacts[i_b]:
             i_ga = collider_state.contact_data.geom_a[i_c, i_b]
             i_gb = collider_state.contact_data.geom_b[i_c, i_b]
-            if dyn_info.geoms.type[i_ga] == gs.GEOM_TYPE.PLANE:
+            if dyn_info.geoms.type[i_ga] == gs.GEOM_TYPE.SPHERE and dyn_info.geoms.type[i_gb] == gs.GEOM_TYPE.SPHERE:
+                collider_state.diff_contact_input.geom_a[i_b, i_c] = i_ga
+                collider_state.diff_contact_input.geom_b[i_b, i_c] = i_gb
+                collider_state.diff_contact_input.ref_id[i_b, i_c] = i_c
+                collider_state.diff_contact_input.valid[i_b, i_c] = 1
+            elif dyn_info.geoms.type[i_ga] == gs.GEOM_TYPE.PLANE:
                 trans_convex = dyn_state.geoms.pos[i_gb, i_b]
                 quat_convex = dyn_state.geoms.quat[i_gb, i_b]