Skip pruning for degenerated contact buckets.

Author: duburcqa

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

@@ -89,6 +89,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 = (
@@ -240,6 +244,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()
@@ -860,6 +865,7 @@ def detection(self) -> None:
             )
 
         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]
 
@@ -739,23 +741,23 @@ def func_clamp_prune_and_sort_contacts(
                             vy = mnz * ux - mnx * uz
                             vz = mnx * uy - mny * ux
 
-                            # Project bucket contacts to (u, v). sort_key holds u, contact_proj_v holds v. Both
-                            # are indexed by bucket-logical position so the (u, v) sort below can read them without
-                            # another indirection.
+                            # Project bucket contacts to (u, v). sort_key holds u, contact_proj_v holds v. Both are
+                            # indexed by bucket-logical position so the (u, v) sort below can read them without another
+                            # indirection.
                             for i in range(b_start, b_end):
                                 phys_i = collider_state.contact_sort_idx[i, i_b]
                                 p_i = collider_state.contact_data.pos[phys_i, i_b]
                                 collider_state.contact_sort_key[i, i_b] = p_i[0] * ux + p_i[1] * uy + p_i[2] * uz
                                 collider_state.contact_proj_v[i, i_b] = p_i[0] * vx + p_i[1] * vy + p_i[2] * vz
 
-                            # Sort bucket positions lexicographically by (u, v), with a tolerance on u so that
-                            # contacts whose u values differ only by float noise (or by sub-millimeter physics noise
-                            # from MPR perturbations) sort by v. Without the tolerance, the wrong point pops from a
-                            # 3-collinear triplet when the corner and the mid-edge have u values that differ by a
-                            # few microns and the mid-edge happens to sort first.
+                            # Sort bucket positions lexicographically by (u, v), with a tolerance on u so that contacts
+                            # whose u values differ only by float noise (or by sub-millimeter physics noise from MPR
+                            # perturbations) sort by v. Without the tolerance, the wrong point pops from a 3-collinear
+                            # triplet when the corner and the mid-edge have u values that differ by a few microns and
+                            # the mid-edge happens to sort first.
                             #
-                            # The permutation lives in contact_keep[b_start..b_end). contact_keep is rewritten with
-                            # the final keep flags below before this bucket exits, so reusing it as scratch is safe.
+                            # The permutation lives in contact_keep[b_start..b_end). contact_keep is rewritten with the
+                            # final keep flags below before this bucket exits, so reusing it as scratch is safe.
                             sort_u_tol = gs.qd_float(1e-3) * qd.sqrt(max_in_plane_r2)
                             for i in range(b_start, b_end):
                                 collider_state.contact_keep[i, i_b] = i
@@ -774,10 +776,9 @@ def func_clamp_prune_and_sort_contacts(
                                     j -= 1
                                 collider_state.contact_keep[j + 1, i_b] = ci
 
-                            # Collinearity threshold for hull pops, scaled to the bucket extent. A pure
-                            # "cross <= 0" check fails on numerically-near-collinear edge points (cross is a tiny
-                            # positive epsilon from float roundoff), so genuine midpoints would survive as spurious
-                            # hull vertices.
+                            # Collinearity threshold for hull pops, scaled to the bucket extent. A pure "cross <= 0"
+                            # check fails on numerically-near-collinear edge points (cross is a tiny positive epsilon
+                            # from float roundoff), so genuine midpoints would survive as spurious  hull vertices.
                             hull_collinear_tol = tol * max_in_plane_r2
 
                             # Andrew's monotone chain. The (u, v) permutation lives in contact_keep; the hull stack
@@ -828,13 +829,12 @@ def func_clamp_prune_and_sort_contacts(
                                         k -= 1
                                     else:
                                         break
-                                # The closing iteration of the upper hull visits the leftmost point, which already
-                                # sits at stack[b_start] from the lower hull. Skipping that push, plus the
-                                # k < b_size guard, bounds k to b_size and keeps the write index within
-                                # max_contact_pairs even for buckets where the lower-hull pass already kept all
-                                # b_size points (downward-convex layouts: every lex-sorted triple makes a left turn
-                                # so nothing gets popped, then the upper-hull pass tries to push a duplicate of an
-                                # already-kept lower-hull vertex).
+                                # The closing iteration of the upper hull visits the leftmost point, which already sits
+                                # at stack[b_start] from the lower hull. Skipping that push, plus the k < b_size guard,
+                                # bounds k to b_size and keeps the write index within max_contact_pairs even for buckets
+                                # where the lower-hull pass already kept all b_size points (downward-convex layouts:
+                                # every lex-sorted triple makes a left turn so nothing gets popped, then the upper-hull
+                                # pass tries to push a duplicate of an already-kept lower-hull vertex).
                                 if ci != collider_state.contact_hull_stack[b_start, i_b] and k < b_size:
                                     collider_state.contact_hull_stack[b_start + k, i_b] = ci
                                     k += 1
@@ -851,12 +851,12 @@ def func_clamp_prune_and_sort_contacts(
                             # average. The support-polygon argument says interior contacts are wrench-redundant only
                             # when ALL contacts share the same normal and penetration; a contact with substantially
                             # higher penetration than the hull's average represents a distinct physical support (the
-                            # body of a fork resting beyond its tines, the deep middle of a long body) and dropping
-                            # it lets the body sink into the surface. The 3x factor is well above the typical ~1.x
+                            # body of a fork resting beyond its tines, the deep middle of a long body) and dropping it
+                            # lets the body sink into the surface. The 3x factor is well above the typical ~1.x
                             # penetration spread on transient/rocking faces (so non-uniform-penetration buckets like
-                            # irregular mesh contacts keep only the hull) but well below the deep interior
-                            # penetrations seen when a non-flat body rests inside its convex envelope (so genuine
-                            # deep supports are restored).
+                            # irregular mesh contacts keep only the hull) but well below the deep interior penetrations
+                            # seen when a non-flat body rests inside its convex envelope (so genuine deep supports are
+                            # restored).
                             hull_pen_max = gs.qd_float(0.0)
                             for hk in range(k):
                                 survivor = collider_state.contact_hull_stack[b_start + hk, i_b]
@@ -871,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.

genesis/utils/array_class.py

@@ -792,6 +792,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):
@@ -824,6 +825,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"]),
     )