[FEATURE] Add analytical sphere-sphere narrow-phase contact

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

@@ -18,18 +18,14 @@ def func_capsule_capsule_contact(
     rigid_global_info: array_class.RigidGlobalInfo,
 ):
     """
-    Analytical capsule-capsule collision detection.
+    Analytical capsule-capsule collision detection 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_global_info.EPS[None]
 
@@ -98,6 +94,61 @@ 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,
+    ga_quat,
+    gb_pos,
+    gb_quat,
+    geoms_info: array_class.GeomsInfo,
+    rigid_global_info: array_class.RigidGlobalInfo,
+):
+    """
+    Analytical sphere-sphere collision detection from the two sphere centers ga_pos and gb_pos and their radii
+    (the orientations are unused since a sphere is rotation-invariant).
+
+    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 (no EPA, which fails to converge on the smoothly-curved sphere-sphere
+    Minkowski boundary).
+    """
+    EPS = rigid_global_info.EPS[None]
+
+    radius_a = geoms_info.data[i_ga][0]
+    radius_b = geoms_info.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)
+
+        # Compute contact normal (from B to A)
+        if dist > EPS:
+            normal_unit = diff / dist
+        # else: centers are coincident, keep the arbitrary default direction
+
+        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,
@@ -110,17 +161,14 @@ def func_sphere_capsule_contact(
     rigid_global_info: array_class.RigidGlobalInfo,
 ):
     """
-    Analytical sphere-capsule collision detection.
+    Analytical sphere-capsule collision detection 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_global_info.EPS[None]
 

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

@@ -542,12 +542,18 @@ def _compute_collision_pair_idx(self):
             # 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.
+            #
+            # Sphere-sphere is exempt: it is handled by the closed-form analytic path (func_sphere_sphere_contact
+            # forward, func_differentiable_sphere_contact backward), which is exactly differentiable and never routes
+            # through diff_gjk's EPA. Ellipsoid-involving smooth pairs still fall through to diff_gjk and remain
+            # unsupported.
             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."
                     )
@@ -1129,6 +1135,7 @@ def backward(self, dL_dposition, dL_dnormal, dL_dpenetration):
             self._collider_state,
             self._collider_info,
             self._gjk._gjk_info,
+            self._solver._rigid_global_info,
             self._collider_state.diff_contact_input,
         )
 

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

@@ -413,6 +413,30 @@ def func_add_diff_contact_input(
         ]
 
 
+@qd.func
+def func_add_diff_contact_input_sphere(
+    i_ga,
+    i_gb,
+    i_b,
+    collider_state: array_class.ColliderState,
+    collider_info: array_class.ColliderInfo,
+):
+    """Register a differentiable-contact-input entry for an analytically handled sphere-sphere contact.
+
+    Unlike func_add_diff_contact_input, which stores a GJK Minkowski triangle, the sphere-sphere contact is
+    reconstructed in closed form in the backward pass (func_differentiable_sphere_contact) directly from the geom
+    centres and radii, so only the geom identities and a self-referential ref_id are needed here. Must be called at
+    the same index as the matching func_add_contact, i.e. before n_contacts is incremented.
+    """
+    i_c = collider_state.n_contacts[i_b]
+    if i_c < collider_info.max_candidate_contacts[None]:
+        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
+        # Single, self-referential contact: ref_id == i_c marks it as its own reference in the backward pass.
+        collider_state.diff_contact_input.ref_id[i_b, i_c] = i_c
+        collider_state.diff_contact_input.ref_penetration[i_b, i_c] = 0.0
+
+
 @qd.func
 def func_compute_geom_rbound(
     i_g,

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

@@ -748,6 +748,43 @@ def func_compute_minkowski_point(
 # ------------------------------ Differentiable functions ------------------------------------
 # These functions have minimal number of branches to align backward pass with forward pass.
 # --------------------------------------------------------------------------------------------
+@qd.func
+def func_differentiable_sphere_contact(
+    geoms_state: array_class.GeomsState,
+    geoms_info: array_class.GeomsInfo,
+    rigid_global_info: array_class.RigidGlobalInfo,
+    i_ga,
+    i_gb,
+    i_b,
+):
+    """
+    Closed-form differentiable sphere-sphere contact. The contact is a smooth function of the two geom centres, so
+    gradients flow directly through [geoms_state.pos] - unlike the Minkowski-triangle reconstruction in
+    func_differentiable_contact, which is degenerate for two smoothly-curved surfaces (where EPA never converges).
+    This must match func_sphere_sphere_contact in capsule_contact.py.
+    """
+    EPS = rigid_global_info.EPS[None]
+
+    radius_a = geoms_info.data[i_ga][0]
+    radius_b = geoms_info.data[i_gb][0]
+    delta = geoms_state.pos[i_ga, i_b] - geoms_state.pos[i_gb, i_b]
+    dist_sq = delta.dot(delta)
+
+    # At coincident centres the distance is a non-differentiable cusp: the normal (delta / dist) and the distance
+    # itself (whose gradient is also delta / dist) are both 0/0 there, and reverse-mode turns that into 0 * NaN = NaN.
+    # Guard the sqrt so it only runs when the centres are separated; otherwise fall back to the same arbitrary
+    # direction and constant penetration as func_sphere_sphere_contact (the contact is degenerate there anyway).
+    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 = geoms_state.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_contact(
     geoms_state: array_class.GeomsState,
@@ -862,6 +899,40 @@ def func_differentiable_contact(
     return contact_pos, contact_normal, penetration, weight
 
 
+@qd.func
+def func_dispatch_differentiable_contact(
+    geoms_state: array_class.GeomsState,
+    geoms_info: array_class.GeomsInfo,
+    rigid_global_info: array_class.RigidGlobalInfo,
+    diff_contact_input: array_class.DiffContactInput,
+    gjk_info: array_class.GJKInfo,
+    i_ga,
+    i_gb,
+    i_b,
+    i_c,
+    ref_penetration,
+):
+    """
+    Dispatch a differentiable contact to the closed-form sphere-sphere reconstruction
+    (func_differentiable_sphere_contact) when both geoms are spheres, else the general Minkowski-triangle
+    reconstruction (func_differentiable_contact). Shared by the reference and non-reference contact passes in
+    func_narrow_phase_diff_convex_vs_convex (narrowphase.py).
+    """
+    contact_pos = gs.qd_vec3(0.0, 0.0, 0.0)
+    contact_normal = gs.qd_vec3(0.0, 0.0, 0.0)
+    penetration = gs.qd_float(0.0)
+    weight = gs.qd_float(0.0)
+    if geoms_info.type[i_ga] == gs.GEOM_TYPE.SPHERE and geoms_info.type[i_gb] == gs.GEOM_TYPE.SPHERE:
+        contact_pos, contact_normal, penetration, weight = func_differentiable_sphere_contact(
+            geoms_state, geoms_info, rigid_global_info, i_ga, i_gb, i_b
+        )
+    else:
+        contact_pos, contact_normal, penetration, weight = func_differentiable_contact(
+            geoms_state, diff_contact_input, gjk_info, i_ga, i_gb, i_b, i_c, ref_penetration
+        )
+    return contact_pos, contact_normal, penetration, weight
+
+
 @qd.func
 def func_plane_normal(v1, v2, v3):
     """

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

@@ -24,6 +24,7 @@
 from .contact import (
     func_add_contact,
     func_add_diff_contact_input,
+    func_add_diff_contact_input_sphere,
     func_apply_smooth_refinement,
     func_compute_geom_pair_scale_mj,
     func_compute_geom_pair_scale,
@@ -1789,6 +1790,17 @@ def func_convex_convex_contact(
                         geoms_info,
                         rigid_global_info,
                     )
+                elif geoms_info.type[i_ga] == gs.GEOM_TYPE.SPHERE and geoms_info.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,
+                        ga_quat_current,
+                        gb_pos_current,
+                        gb_quat_current,
+                        geoms_info,
+                        rigid_global_info,
+                    )
                 elif geoms_info.type[i_ga] == gs.GEOM_TYPE.SPHERE and geoms_info.type[i_gb] == gs.GEOM_TYPE.CAPSULE:
                     is_col, normal, contact_pos, penetration = capsule_contact.func_sphere_capsule_contact(
                         i_ga,
@@ -2045,6 +2057,17 @@ def func_convex_convex_contact(
             if i_detection == 0:
                 is_col_0, normal_0, penetration_0, contact_pos_0 = is_col, normal, penetration, contact_pos
                 if is_col_0:
+                    # Sphere-sphere is handled by the closed-form analytic branch above, which never populates a
+                    # GJK Minkowski triangle. In differentiable mode, register a diff-contact-input entry (consumed
+                    # by func_differentiable_sphere_contact) so the backward pass reconstructs the contact in closed
+                    # form instead of from a degenerate triangle. Must run at the same contact index as
+                    # func_add_contact, i.e. before it increments n_contacts.
+                    if qd.static(static_rigid_sim_config.requires_grad):
+                        if (
+                            geoms_info.type[i_ga] == gs.GEOM_TYPE.SPHERE
+                            and geoms_info.type[i_gb] == gs.GEOM_TYPE.SPHERE
+                        ):
+                            func_add_diff_contact_input_sphere(i_ga, i_gb, i_b, collider_state, collider_info)
                     func_add_contact(
                         i_ga,
                         i_gb,
@@ -2208,6 +2231,17 @@ def _func_multicontact_run_detection(
             geoms_info,
             rigid_global_info,
         )
+    elif geoms_info.type[i_ga] == gs.GEOM_TYPE.SPHERE and geoms_info.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,
+            ga_quat,
+            gb_pos,
+            gb_quat,
+            geoms_info,
+            rigid_global_info,
+        )
     elif geoms_info.type[i_ga] == gs.GEOM_TYPE.SPHERE and geoms_info.type[i_gb] == gs.GEOM_TYPE.CAPSULE:
         is_col, normal, contact_pos, penetration = capsule_contact.func_sphere_capsule_contact(
             i_ga,
@@ -2877,6 +2911,17 @@ def _func_narrowphase_contact0(
                     geoms_info,
                     rigid_global_info,
                 )
+            elif geoms_info.type[i_ga] == gs.GEOM_TYPE.SPHERE and geoms_info.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,
+                    ga_quat,
+                    gb_pos,
+                    gb_quat,
+                    geoms_info,
+                    rigid_global_info,
+                )
             elif geoms_info.type[i_ga] == gs.GEOM_TYPE.SPHERE and geoms_info.type[i_gb] == gs.GEOM_TYPE.CAPSULE:
                 is_col, normal, contact_pos, penetration = capsule_contact.func_sphere_capsule_contact(
                     i_ga,
@@ -3146,6 +3191,7 @@ def func_narrow_phase_diff_convex_vs_convex(
     collider_state: array_class.ColliderState,
     collider_info: array_class.ColliderInfo,
     gjk_info: array_class.GJKInfo,
+    rigid_global_info: array_class.RigidGlobalInfo,
     # FIXME: Passing nested data structure as input argument is not supported for now.
     diff_contact_input: array_class.DiffContactInput,
 ):
@@ -3160,8 +3206,17 @@ def func_narrow_phase_diff_convex_vs_convex(
 
             if is_ref:
                 ref_penetration = -1.0
-                contact_pos, contact_normal, penetration, weight = diff_gjk.func_differentiable_contact(
-                    geoms_state, diff_contact_input, gjk_info, i_ga, i_gb, i_b, i_c, ref_penetration
+                contact_pos, contact_normal, penetration, weight = diff_gjk.func_dispatch_differentiable_contact(
+                    geoms_state,
+                    geoms_info,
+                    rigid_global_info,
+                    diff_contact_input,
+                    gjk_info,
+                    i_ga,
+                    i_gb,
+                    i_b,
+                    i_c,
+                    ref_penetration,
                 )
                 collider_state.diff_contact_input.ref_penetration[i_b, i_c] = penetration
 
@@ -3190,8 +3245,17 @@ def func_narrow_phase_diff_convex_vs_convex(
 
             if not is_ref:
                 ref_penetration = collider_state.diff_contact_input.ref_penetration[i_b, ref_id]
-                contact_pos, contact_normal, penetration, weight = diff_gjk.func_differentiable_contact(
-                    geoms_state, diff_contact_input, gjk_info, i_ga, i_gb, i_b, i_c, ref_penetration
+                contact_pos, contact_normal, penetration, weight = diff_gjk.func_dispatch_differentiable_contact(
+                    geoms_state,
+                    geoms_info,
+                    rigid_global_info,
+                    diff_contact_input,
+                    gjk_info,
+                    i_ga,
+                    i_gb,
+                    i_b,
+                    i_c,
+                    ref_penetration,
                 )
 
                 func_set_contact(