Skip pruning for degenerated contact buckets.

Author: duburcqa

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

@@ -90,6 +90,10 @@ def __init__(self, rigid_solver: "RigidSolver"):
         self._diff_pos_tolerance = 1e-2
         self._diff_normal_tolerance = 1e-2
         self._prune_deep_penetration_ratio = 3.0
+        # Multiplier on the link-pair effective inertia radius squared (invweight_trans / invweight_rot) that defines
+        # the support-polygon area threshold below which the bucket's hull pruning is skipped (all hull-dropped contacts
+        # restored). Catches buckets too thin to absorb the first-order-Taylor bias in perturbed contact normals.
+        self._prune_degenerate_area_ratio = 0.2
 
         self._init_static_config()
         self._use_split_narrowphase = (
@@ -248,6 +252,7 @@ def _init_collision_fields(self) -> None:
             diff_normal_tolerance=self._diff_normal_tolerance,
             contact_pruning_tolerance=self._solver._options.contact_pruning_tolerance or 0.0,
             prune_deep_penetration_ratio=self._prune_deep_penetration_ratio,
+            prune_degenerate_area_ratio=self._prune_degenerate_area_ratio,
         )
         self._init_collision_pair_idx(self._collision_pair_idx)
         self._init_valid_pairs()
@@ -899,6 +904,7 @@ def detection(self) -> None:
         )
         if ran_fused_dedup_coop:
             func_clamp_prune_and_sort_contacts_coop(
+                self._solver.links_info,
                 self._collider_state,
                 self._collider_info,
                 self._solver._rigid_global_info,
@@ -907,6 +913,7 @@ def detection(self) -> None:
             )
         else:
             func_clamp_prune_and_sort_contacts(
+                self._solver.links_info,
                 self._collider_state,
                 self._collider_info,
                 self._solver._rigid_global_info,

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

@@ -538,6 +538,7 @@ def func_rotate_frame(
 
 @qd.kernel(fastcache=True)
 def func_clamp_prune_and_sort_contacts(
+    links_info: array_class.LinksInfo,
     collider_state: array_class.ColliderState,
     collider_info: array_class.ColliderInfo,
     rigid_global_info: array_class.RigidGlobalInfo,
@@ -582,6 +583,7 @@ def func_clamp_prune_and_sort_contacts(
     max_contact_pairs = collider_info.max_contact_pairs[None]
     tol = collider_info.contact_pruning_tolerance[None]
     prune_deep_penetration_ratio = collider_info.prune_deep_penetration_ratio[None]
+    prune_degenerate_area_ratio = collider_info.prune_degenerate_area_ratio[None]
     LP_KEY_STRIDE = gs.qd_float(1.0e7)
     EPS = rigid_global_info.EPS[None]
 
@@ -869,6 +871,49 @@ def func_clamp_prune_and_sort_contacts(
                                     if collider_state.contact_data.penetration[phys_i, i_b] > deep_keep_threshold:
                                         collider_state.contact_keep[i, i_b] = 1
 
+                            # Support-polygon degeneracy gate. When the hull polygon's surface area is too small to
+                            # absorb the first-order-Taylor bias in perturbed contact normals (each contact's bias
+                            # creates a horizontal force ~ N * mc_perturbation^2; if the polygon is thin the moment arms
+                            # align rather than cancel and the LCP drifts), restore every contact the hull dropped, i.e.
+                            # skip pruning for this bucket. Closed-form shoelace area on the hull stack
+                            # contact_hull_stack[b_start..b_start+k); The threshold is the link-pair effective inertia
+                            # radius squared (invweight_trans / invweight_rot): mass cancels out, so the ratio is a
+                            # shape-and-density-distribution quantity.
+                            hull_area = gs.qd_float(0.0)
+                            if k >= 3:
+                                for i in range(k):
+                                    a_pos = collider_state.contact_hull_stack[b_start + i, i_b]
+                                    j = i + 1
+                                    if j == k:
+                                        j = 0
+                                    b_pos = collider_state.contact_hull_stack[b_start + j, i_b]
+                                    au = collider_state.contact_sort_key[a_pos, i_b]
+                                    av = collider_state.contact_proj_v[a_pos, i_b]
+                                    bu = collider_state.contact_sort_key[b_pos, i_b]
+                                    bv = collider_state.contact_proj_v[b_pos, i_b]
+                                    hull_area = hull_area + au * bv - bu * av
+                                hull_area = 0.5 * qd.abs(hull_area)
+                                I_la0 = (la0, i_b) if qd.static(static_rigid_sim_config.batch_links_info) else la0
+                                I_lb0 = (lb0, i_b) if qd.static(static_rigid_sim_config.batch_links_info) else lb0
+                                invw_ratio = gs.qd_float(0.0)
+                                if links_info.is_fixed[I_la0]:
+                                    invw_ratio = links_info.invweight[I_lb0][0] / qd.max(
+                                        links_info.invweight[I_lb0][1], EPS
+                                    )
+                                elif links_info.is_fixed[I_lb0]:
+                                    invw_ratio = links_info.invweight[I_la0][0] / qd.max(
+                                        links_info.invweight[I_la0][1], EPS
+                                    )
+                                else:
+                                    invw_ratio = min(
+                                        links_info.invweight[I_la0][0] / qd.max(links_info.invweight[I_la0][1], EPS),
+                                        links_info.invweight[I_lb0][0] / qd.max(links_info.invweight[I_lb0][1], EPS),
+                                    )
+                                if hull_area < prune_degenerate_area_ratio * invw_ratio:
+                                    for i in range(b_start, b_end):
+                                        if collider_state.contact_keep[i, i_b] == 0:
+                                            collider_state.contact_keep[i, i_b] = 1
+
                     b_start = b_end
 
                 # Phase 3: compact contact_sort_idx by squeezing out dropped slots.
@@ -923,6 +968,7 @@ def func_clamp_prune_and_sort_contacts(
 
 @qd.kernel(fastcache=True)
 def func_clamp_prune_and_sort_contacts_coop(
+    links_info: array_class.LinksInfo,
     collider_state: array_class.ColliderState,
     collider_info: array_class.ColliderInfo,
     rigid_global_info: array_class.RigidGlobalInfo,

genesis/utils/array_class.py

@@ -798,6 +798,7 @@ class ColliderInfo:
     # link-pair contact pruning
     contact_pruning_tolerance: qd.Tensor
     prune_deep_penetration_ratio: qd.Tensor
+    prune_degenerate_area_ratio: qd.Tensor
 
 
 def get_collider_info(solver, n_vert_neighbors, n_valid_pairs, collider_static_config, **kwargs):
@@ -830,6 +831,7 @@ def get_collider_info(solver, n_vert_neighbors, n_valid_pairs, collider_static_c
         diff_normal_tolerance=V_SCALAR_FROM(dtype=gs.qd_float, value=kwargs["diff_normal_tolerance"]),
         contact_pruning_tolerance=V_SCALAR_FROM(dtype=gs.qd_float, value=kwargs["contact_pruning_tolerance"]),
         prune_deep_penetration_ratio=V_SCALAR_FROM(dtype=gs.qd_float, value=kwargs["prune_deep_penetration_ratio"]),
+        prune_degenerate_area_ratio=V_SCALAR_FROM(dtype=gs.qd_float, value=kwargs["prune_degenerate_area_ratio"]),
     )