Add culling of speculative contacts in Kamino (#3779)

Author: chschuma-disney

newton/_src/solvers/kamino/_src/geometry/contacts.py

@@ -112,6 +112,12 @@
 Set to `1e-5`.
 """
 
+DEFAULT_CULL_SPECULATIVE_CONTACTS: bool = True
+"""
+The global default for culling speculative contacts (with positive
+margin-shifted distance) during Newton->Kamino conversion.
+"""
+
 
 ###
 # Types
@@ -856,13 +862,16 @@ def _assert_has_data(self):
 def make_convert_contacts_newton_to_kamino(
     friction_mix_mode: MaterialMixMode = MaterialMixMode.AVERAGE,
     restitution_mix_mode: MaterialMixMode = MaterialMixMode.MIN,
+    cull_speculative: bool = DEFAULT_CULL_SPECULATIVE_CONTACTS,
 ):
     """
     Generates a kernel to convert Newton contacts to the Kamino format.
 
     Args:
         friction_mix_mode: The mixing mode to use for friction.
         restitution_mix_mode: The mixing mode to use for restitution.
+        cull_speculative: If ``True``, skip speculative contacts, i.e., contacts
+            with positive margin-shifted distance.
 
     Returns:
         A kernel function that converts Newton contacts to the Kamino format.
@@ -984,6 +993,12 @@ def _convert_contacts_newton_to_kamino(
         # and the per-shape surface thicknesses stored in rigid_contact_margin*.
         distance = contact_surface_separation(p0_world, p1_world, normal, margin_0, margin_1)
 
+        # Cull speculative contacts whose margin-shifted surfaces have not yet
+        # made contact (positive gap distance).
+        if wp.static(cull_speculative):
+            if distance > 0.0:
+                return
+
         # Ensure static body is always Kamino A, dynamic body is Kamino B
         if bid_1 < 0:
             # shape1 is world-static → make it Kamino A, shape0 becomes Kamino B.
@@ -1287,6 +1302,7 @@ def convert_contacts_newton_to_kamino(
     convert_forces: bool = False,
     friction_mix_mode: Literal["average", "multiply", "max", "min"] = "average",
     restitution_mix_mode: Literal["average", "multiply", "max", "min"] = "min",
+    cull_speculative_contacts: bool = DEFAULT_CULL_SPECULATIVE_CONTACTS,
 ):
     """
     Converts Newton's :class:`Contacts` to Kamino's :class:`ContactsKamino` format.
@@ -1328,6 +1344,9 @@ def convert_contacts_newton_to_kamino(
             The mixing mode to use for contact friction. Defaults to `"average"`.
         restitution_mix_mode:
             The mixing mode to use for contact restitution. Defaults to `"min"`.
+        cull_speculative_contacts:
+            If ``True`` (the default), drop speculative contacts (contacts with
+            positive margin-shifted distance).
     """
     # Skip conversion if there are no contacts to convert or no capacity to store them.
     if contacts_out.model_max_contacts_host == 0 or contacts_in.rigid_contact_max == 0:
@@ -1373,6 +1392,7 @@ def convert_contacts_newton_to_kamino(
     _convert_contacts_newton_to_kamino = make_convert_contacts_newton_to_kamino(
         friction_mix_mode=MaterialMixMode.from_string(friction_mix_mode),
         restitution_mix_mode=MaterialMixMode.from_string(restitution_mix_mode),
+        cull_speculative=cull_speculative_contacts,
     )
 
     # Launch the conversion kernel to convert Newton contacts to Kamino's format.
@@ -1569,6 +1589,12 @@ def convert_contacts_kamino_to_newton(
                 "construct `ContactsKamino` with `remappable=True`."
             )
 
+        # Speculative-contact culling (and capacity truncation) can leave active
+        # Newton contacts without a matching Kamino contact, so the kernel below
+        # never writes their wrench. Zero the rigid force region first so those
+        # slots report zero instead of stale prior-frame data.
+        contacts_out_force[: contacts_out.rigid_contact_max].zero_()
+
         # Launch the kernel to fill in solver-specific
         # contact attributes for already populated contacts.
         wp.launch(

newton/_src/solvers/kamino/tests/test_geometry_contacts.py

@@ -189,6 +189,34 @@ def build_dynamic_static_sphere_scene(
     return builder
 
 
+def build_box_on_plane_scene(
+    box_z: float,
+    margin: float = 0.0,
+    gap: float = 0.0,
+    half_extent: float = 0.25,
+) -> ModelBuilder:
+    """Construct a single free box hovering above / resting on a static ground plane.
+
+    The box centre is placed at ``box_z`` so that its bottom face sits at
+    ``box_z - half_extent`` above the ground surface (``z = 0``).
+    """
+    cfg = ModelBuilder.ShapeConfig(margin=margin, gap=gap)
+    builder = ModelBuilder()
+    builder.begin_world()
+    body = builder.add_link(xform=wp.transform(p=wp.vec3(0.0, 0.0, box_z), q=wp.quat_identity()))
+    builder.add_shape_box(body, hx=half_extent, hy=half_extent, hz=half_extent, cfg=cfg)
+    joint = builder.add_joint_free(
+        parent=-1,
+        child=body,
+        parent_xform=wp.transform_identity(),
+        child_xform=wp.transform_identity(),
+    )
+    builder.add_articulation([joint])
+    builder.add_ground_plane(cfg=cfg)
+    builder.end_world()
+    return builder
+
+
 ###
 # Module configs
 ###
@@ -1473,6 +1501,120 @@ def test_09_multi_world_per_world_capacity_not_starved(self):
         bid_AB = kamino.bid_AB.numpy()[:2]
         self.assertTrue(np.all(bid_AB[:, 1] >= 0))
 
+    def test_10_speculative_contacts_culled(self):
+        """Speculative box-plane contacts are dropped by default.
+
+        Both configurations use a non-zero gap so that the collision detector
+        reports contacts while the box still hovers above the rest offset.
+        """
+        half_extent = 0.25
+
+        for margin, gap, box_z in (
+            (0.0, 0.1, half_extent + 0.05),
+            (0.02, 0.1, half_extent + 0.08),
+        ):
+            with self.subTest(margin=margin, gap=gap):
+                scene = build_box_on_plane_scene(box_z, margin=margin, gap=gap, half_extent=half_extent)
+                model = scene.finalize(self.default_device)
+                state = model.state()
+                collision_pipeline = newton.CollisionPipeline(model, rigid_contact_max=_NEWTON_CONTACT_CAPACITY)
+                contacts = collision_pipeline.contacts()
+                collision_pipeline.collide(state, contacts)
+                nc = int(contacts.rigid_contact_count.numpy()[0])
+                self.assertGreater(nc, 0, "Detector must report the hovering box as a (speculative) contact")
+
+                # Sanity: with culling disabled every converted contact is speculative.
+                kamino_off = ContactsKamino(capacity=contacts.rigid_contact_max, device=self.default_device)
+                convert_contacts_newton_to_kamino(model, state, contacts, kamino_off, cull_speculative_contacts=False)
+                n_off = int(kamino_off.model_active_contacts.numpy()[0])
+                self.assertEqual(n_off, nc, "Disabling culling must keep every detected contact")
+                w = kamino_off.gapfunc.numpy()[:n_off, 3]
+                self.assertTrue(np.all(w > 0.0), f"Expected speculative contacts (w > 0), got {w}")
+
+                # Default conversion culls all of them.
+                kamino_on = ContactsKamino(capacity=contacts.rigid_contact_max, device=self.default_device)
+                convert_contacts_newton_to_kamino(model, state, contacts, kamino_on)
+                self.assertEqual(
+                    int(kamino_on.model_active_contacts.numpy()[0]),
+                    0,
+                    "Speculative contacts must be culled by default",
+                )
+
+    def test_11_genuine_contacts_kept(self):
+        """Genuine box-plane contacts (gapfunc.w <= 0) survive culling.
+
+        Covers the full ``(gap, margin)`` matrix with the box placed at or below
+        the rest offset so no contact is speculative.
+        """
+        half_extent = 0.25
+
+        for margin, gap, box_z in (
+            (0.0, 0.0, half_extent - 0.01),
+            (0.0, 0.1, half_extent - 0.01),
+            (0.02, 0.0, half_extent + 0.02),
+            (0.02, 0.1, half_extent + 0.02),
+        ):
+            with self.subTest(margin=margin, gap=gap):
+                scene = build_box_on_plane_scene(box_z, margin=margin, gap=gap, half_extent=half_extent)
+                model = scene.finalize(self.default_device)
+                state = model.state()
+                collision_pipeline = newton.CollisionPipeline(model, rigid_contact_max=_NEWTON_CONTACT_CAPACITY)
+                contacts = collision_pipeline.contacts()
+                collision_pipeline.collide(state, contacts)
+                nc = int(contacts.rigid_contact_count.numpy()[0])
+                self.assertGreater(nc, 0, "Detector must report the penetrating box as a contact")
+
+                kamino = ContactsKamino(capacity=contacts.rigid_contact_max, device=self.default_device)
+                convert_contacts_newton_to_kamino(model, state, contacts, kamino)
+                n_kamino = int(kamino.model_active_contacts.numpy()[0])
+                self.assertEqual(n_kamino, nc, "Genuine contacts must not be culled")
+                w = kamino.gapfunc.numpy()[:n_kamino, 3]
+                self.assertTrue(np.all(w <= 1e-6), f"Expected genuine contacts (w <= 0), got {w}")
+
+    def test_12_culled_contacts_report_zero_force(self):
+        """Culled contacts report zero force through the K->N existing path.
+
+        Regression: the existing-contacts write-back only touches surviving
+        (remapped) contacts, so culled slots must be zeroed rather than left
+        holding stale prior-frame force data.
+        """
+        half_extent = 0.25
+
+        # Box hovering within the detection gap -> every contact is speculative.
+        scene = build_box_on_plane_scene(half_extent + 0.05, margin=0.0, gap=0.1, half_extent=half_extent)
+        scene.request_contact_attributes("force")
+        model = scene.finalize(self.default_device)
+        state = model.state()
+        collision_pipeline = newton.CollisionPipeline(model, rigid_contact_max=_NEWTON_CONTACT_CAPACITY)
+        contacts = collision_pipeline.contacts()
+        collision_pipeline.collide(state, contacts)
+        nc = int(contacts.rigid_contact_count.numpy()[0])
+        self.assertGreater(nc, 0)
+
+        # Seed stale prior-frame force data on every active contact slot.
+        force_np = contacts.force.numpy()
+        force_np[:nc] = 9.0
+        contacts.force.assign(force_np)
+
+        # N->K culls every (speculative) contact.
+        kamino = ContactsKamino(
+            capacity=contacts.rigid_contact_max,
+            device=self.default_device,
+            remappable=True,
+        )
+        convert_contacts_newton_to_kamino(model, state, contacts, kamino, convert_forces=True)
+        self.assertEqual(int(kamino.model_active_contacts.numpy()[0]), 0, "All contacts should be culled")
+
+        # K->N existing path must not leave stale force on the culled contacts.
+        convert_contacts_kamino_to_newton(model, state, kamino, contacts, clear_output=False, convert_forces=True)
+
+        force_after = contacts.force.numpy()[:nc]
+        np.testing.assert_array_equal(
+            force_after,
+            np.zeros_like(force_after),
+            err_msg="Culled contacts must not retain stale force data",
+        )
+
 
 ###
 # Test execution