Extend filter_link_idx to the solver-contact probe sensors

ContactProbe, ContactDepthProbe, and KinematicTaxel now accept
filter_link_idx, mirroring the Contact/ContactForce semantics: a contact
with the sensor link is dropped when its counterpart link is in the
filter list. The three share the counterpart check via a new
_func_link_is_filtered device helper threaded into both contact-depth
query funcs and their kernels, and a SolverContactProbeSensorOptionsMixin
so the point-cloud tactile sensors (Elastomer/Proximity), which sample a
point cloud rather than solver contacts, do not expose the option.

Factor the shared build-time padding into misc.append_filter_links_idx
(now used by Contact, ContactForce, and the tactile sensors) and the
range validation into options._validate_filter_link_idx. The filter
table keeps at least one column so the no-filter case is still a valid
kernel argument. Adds test_contact_probe_sensor_filter_link_idx.

Author: Milotrince

genesis/engine/sensors/contact_force.py

@@ -9,7 +9,13 @@
 from genesis.options.sensors import Contact as ContactSensorOptions
 from genesis.options.sensors import ContactForce as ContactForceSensorOptions
 from genesis.utils.geom import inv_transform_by_quat, qd_inv_transform_by_quat, transform_by_quat
-from genesis.utils.misc import concat_with_tensor, make_tensor_field, qd_to_torch, tensor_to_array
+from genesis.utils.misc import (
+    append_filter_links_idx,
+    concat_with_tensor,
+    make_tensor_field,
+    qd_to_torch,
+    tensor_to_array,
+)
 from genesis.utils.ring_buffer import TensorRingBuffer
 
 from .base_sensor import RigidSensorMetadataMixin, RigidSensorMixin, Sensor, SimpleSensor, SimpleSensorMetadata
@@ -121,15 +127,10 @@ def build(self):
             self._shared_metadata.expanded_links_idx, link_idx, expand=(1,), dim=0
         )
 
-        num_sensors, cur_num_filter_links = self._shared_metadata.filter_links_idx.shape
-        max_num_filter_links = max(cur_num_filter_links, len(self._options.filter_link_idx))
-        filter_links_idx = torch.full((num_sensors + 1, max_num_filter_links), -1, dtype=gs.tc_int, device=gs.device)
-        filter_links_idx[:num_sensors, :cur_num_filter_links] = self._shared_metadata.filter_links_idx
-        filter_links_idx[num_sensors, : len(self._options.filter_link_idx)] = torch.tensor(
-            self._options.filter_link_idx, dtype=gs.tc_int, device=gs.device
+        num_sensors = self._shared_metadata.filter_links_idx.shape[0]
+        self._shared_metadata.filter_links_idx = append_filter_links_idx(
+            self._shared_metadata.filter_links_idx, self._options.filter_link_idx
         )
-        self._shared_metadata.filter_links_idx = filter_links_idx
-
         if len(self._options.filter_link_idx) > 0:
             self._shared_metadata.filtered_sensor_idx = concat_with_tensor(
                 self._shared_metadata.filtered_sensor_idx, num_sensors, expand=(1,), dim=0
@@ -256,15 +257,10 @@ def build(self):
             self._shared_metadata.max_force, self._options.max_force, expand=(1, 3)
         )
 
-        num_sensors, cur_num_filter_links = self._shared_metadata.filter_links_idx.shape
-        max_num_filter_links = max(cur_num_filter_links, len(self._options.filter_link_idx))
-        filter_links_idx = torch.full((num_sensors + 1, max_num_filter_links), -1, dtype=gs.tc_int, device=gs.device)
-        filter_links_idx[:num_sensors, :cur_num_filter_links] = self._shared_metadata.filter_links_idx
-        filter_links_idx[num_sensors, : len(self._options.filter_link_idx)] = torch.tensor(
-            self._options.filter_link_idx, dtype=gs.tc_int, device=gs.device
+        num_sensors = self._shared_metadata.filter_links_idx.shape[0]
+        self._shared_metadata.filter_links_idx = append_filter_links_idx(
+            self._shared_metadata.filter_links_idx, self._options.filter_link_idx
         )
-        self._shared_metadata.filter_links_idx = filter_links_idx
-
         if len(self._options.filter_link_idx) > 0:
             self._shared_metadata.filtered_sensor_idx = concat_with_tensor(
                 self._shared_metadata.filtered_sensor_idx, num_sensors, expand=(1,), dim=0

genesis/engine/sensors/kinematic_tactile.py

@@ -13,7 +13,7 @@
 from genesis.options.sensors import ContactDepthProbe as ContactDepthProbeOptions
 from genesis.options.sensors import ContactProbe as ContactProbeOptions
 from genesis.options.sensors import KinematicTaxel as KinematicTaxelOptions
-from genesis.utils.misc import concat_with_tensor, make_tensor_field, tensor_to_array
+from genesis.utils.misc import append_filter_links_idx, concat_with_tensor, make_tensor_field, tensor_to_array
 
 from .base_sensor import RigidSensorMetadataMixin, RigidSensorMixin, SimpleSensor, SimpleSensorMetadata
 from .probe import (
@@ -33,13 +33,25 @@
     from .sensor_manager import SensorManager
 
 
+@qd.func
+def _func_link_is_filtered(filter_links_idx: qd.types.ndarray(), i_s: int, link: int):
+    """Whether ``link`` is in sensor ``i_s``'s filter list. ``-1`` padding entries never match a real link index."""
+    is_filtered = False
+    for i_f in range(filter_links_idx.shape[-1]):
+        if filter_links_idx[i_s, i_f] == link:
+            is_filtered = True
+    return is_filtered
+
+
 @qd.func
 def _func_query_contact_depth_penetration(
     i_b: int,
     probe_pos: qd.types.vector(3),
     probe_radius_gt: float,
     probe_radius_m: float,
     sensor_link_idx: int,
+    filter_links_idx: qd.types.ndarray(),
+    i_s: int,
     geoms_info: array_class.GeomsInfo,
     geoms_state: array_class.GeomsState,
     collider_state: array_class.ColliderState,
@@ -50,7 +62,8 @@ def _func_query_contact_depth_penetration(
 
     Returns ``(max_pen_gt, max_pen_m)`` from a single SDF pass: both penetrations come from the same ``sd`` per contact
     via ``pen = radius - sd``. Callers that do not need the noised-radius branch pass ``probe_radius_m ==
-    probe_radius_gt`` and ignore the second return.
+    probe_radius_gt`` and ignore the second return. Contacts whose counterpart link is in sensor ``i_s``'s
+    ``filter_links_idx`` list are skipped.
     """
     max_pen_gt = gs.qd_float(0.0)
     max_pen_m = gs.qd_float(0.0)
@@ -65,9 +78,10 @@ def _func_query_contact_depth_penetration(
 
         for side in qd.static(range(2)):
             c_link = c_link_a if side == 0 else c_link_b
+            c_link_other = c_link_b if side == 0 else c_link_a
             i_g = c_geom_b if side == 0 else c_geom_a
 
-            if c_link == sensor_link_idx:
+            if c_link == sensor_link_idx and not _func_link_is_filtered(filter_links_idx, i_s, c_link_other):
                 g_pos = geoms_state.pos[i_g, i_b]
                 g_quat = geoms_state.quat[i_g, i_b]
                 sd = sdf.sdf_func_world_local(geoms_info, sdf_info, probe_pos, i_g, g_pos, g_quat)
@@ -88,6 +102,8 @@ def _func_query_contact_depth(
     probe_radius_gt: float,
     probe_radius_m: float,
     sensor_link_idx: int,
+    filter_links_idx: qd.types.ndarray(),
+    i_s: int,
     geoms_info: array_class.GeomsInfo,
     geoms_state: array_class.GeomsState,
     rigid_global_info: array_class.RigidGlobalInfo,
@@ -101,7 +117,8 @@ def _func_query_contact_depth(
 
     Returns ``(max_pen_gt, contact_link_gt, contact_normal_gt, max_pen_m, contact_link_m, contact_normal_m)``. AABB
     pre-filter expands by ``max(probe_radius_gt, probe_radius_m)`` so neither branch is silently skipped. Callers
-    without a noised radius pass ``probe_radius_m == probe_radius_gt``.
+    without a noised radius pass ``probe_radius_m == probe_radius_gt``. Contacts whose counterpart link is in sensor
+    ``i_s``'s ``filter_links_idx`` list are skipped.
     """
     max_pen_gt = gs.qd_float(0.0)
     contact_link_gt = gs.qd_int(-1)
@@ -123,9 +140,14 @@ def _func_query_contact_depth(
         # Check if either side of this contact involves the sensor link.
         for side in qd.static(range(2)):
             c_link = c_link_a if side == 0 else c_link_b
+            c_link_other = c_link_b if side == 0 else c_link_a
             i_g = c_geom_b if side == 0 else c_geom_a
 
-            if c_link == sensor_link_idx and func_point_in_geom_aabb(geoms_state, i_g, i_b, probe_pos, aabb_expansion):
+            if (
+                c_link == sensor_link_idx
+                and not _func_link_is_filtered(filter_links_idx, i_s, c_link_other)
+                and func_point_in_geom_aabb(geoms_state, i_g, i_b, probe_pos, aabb_expansion)
+            ):
                 g_pos = geoms_state.pos[i_g, i_b]
                 g_quat = geoms_state.quat[i_g, i_b]
                 sd = sdf.sdf_func_world_local(geoms_info, sdf_info, probe_pos, i_g, g_pos, g_quat)
@@ -139,11 +161,11 @@ def _func_query_contact_depth(
                     )
                     if pen_gt > max_pen_gt and pen_gt > eps:
                         max_pen_gt = pen_gt
-                        contact_link_gt = c_link_b if side == 0 else c_link_a
+                        contact_link_gt = c_link_other
                         contact_normal_gt = normal
                     if pen_m > max_pen_m and pen_m > eps:
                         max_pen_m = pen_m
-                        contact_link_m = c_link_b if side == 0 else c_link_a
+                        contact_link_m = c_link_other
                         contact_normal_m = normal
 
     return max_pen_gt, contact_link_gt, contact_normal_gt, max_pen_m, contact_link_m, contact_normal_m
@@ -161,6 +183,7 @@ def _kernel_kinematic_taxel(
     shear_scalar: qd.types.ndarray(),
     twist_scalar: qd.types.ndarray(),
     links_idx: qd.types.ndarray(),
+    filter_links_idx: qd.types.ndarray(),
     sensor_cache_start: qd.types.ndarray(),
     sensor_probe_start: qd.types.ndarray(),
     n_probes_per_sensor: qd.types.ndarray(),
@@ -211,6 +234,8 @@ def _kernel_kinematic_taxel(
             probe_radius,
             probe_radius_m,
             sensor_link_idx,
+            filter_links_idx,
+            i_s,
             geoms_info,
             geoms_state,
             rigid_global_info,
@@ -305,6 +330,7 @@ def _kernel_contact_depth_probe(
     probe_radii: qd.types.ndarray(),
     probe_radii_noise: qd.types.ndarray(),
     links_idx: qd.types.ndarray(),
+    filter_links_idx: qd.types.ndarray(),
     sensor_cache_start: qd.types.ndarray(),
     sensor_probe_start: qd.types.ndarray(),
     collider_state: array_class.ColliderState,
@@ -343,6 +369,8 @@ def _kernel_contact_depth_probe(
             probe_radius,
             probe_radius_m,
             sensor_link_idx,
+            filter_links_idx,
+            i_s,
             geoms_info,
             geoms_state,
             collider_state,
@@ -368,6 +396,9 @@ def __init__(
     def build(self):
         super().build()
         self._shared_metadata.solver.collider.activate_sdf()
+        self._shared_metadata.filter_links_idx = append_filter_links_idx(
+            self._shared_metadata.filter_links_idx, self._options.filter_link_idx
+        )
 
     def _draw_debug_probes(self, context: "RasterizerContext", get_is_contact: Callable[[object], object]):
         for obj in self._debug_objects:
@@ -392,7 +423,8 @@ def _draw_debug_probes(self, context: "RasterizerContext", get_is_contact: Calla
 
 @dataclass
 class ContactDepthProbeMetadata(ProbeSensorMetadataMixin, RigidSensorMetadataMixin, SimpleSensorMetadata):
-    pass
+    # (n_sensors, max_num_filter_links); unused slots and empty filters are -1. See `append_filter_links_idx`.
+    filter_links_idx: torch.Tensor = make_tensor_field((0, 0), dtype_factory=lambda: gs.tc_int)
 
 
 class ContactDepthProbeSensor(
@@ -433,6 +465,7 @@ def _update_current_timestep_data(
             shared_metadata.probe_radii,
             shared_metadata.probe_radii_noise,
             shared_metadata.links_idx,
+            shared_metadata.filter_links_idx,
             shared_metadata.sensor_cache_start,
             shared_metadata.sensor_probe_start,
             solver.collider._collider_state,
@@ -522,6 +555,8 @@ class KinematicTaxelMetadata(ProbesWithNormalSensorMetadataMixin, RigidSensorMet
     normal_exponent: torch.Tensor = make_tensor_field((0,))
     shear_scalar: torch.Tensor = make_tensor_field((0,))
     twist_scalar: torch.Tensor = make_tensor_field((0,))
+    # (n_sensors, max_num_filter_links); unused slots and empty filters are -1. See `append_filter_links_idx`.
+    filter_links_idx: torch.Tensor = make_tensor_field((0, 0), dtype_factory=lambda: gs.tc_int)
 
 
 class KinematicTaxelSensor(
@@ -597,6 +632,7 @@ def _update_current_timestep_data(
             shared_metadata.shear_scalar,
             shared_metadata.twist_scalar,
             shared_metadata.links_idx,
+            shared_metadata.filter_links_idx,
             shared_metadata.sensor_cache_start,
             shared_metadata.sensor_probe_start,
             shared_metadata.n_probes_per_sensor,

genesis/options/sensors/options.py

@@ -61,6 +61,16 @@ def _check_len_match(value, expected_len: int, name: str, ref_name: str):
         )
 
 
+def _validate_filter_link_idx(scene: "Scene", filter_link_idx, sensor_name: str):
+    """Check that a contact sensor's ``filter_link_idx`` are valid global (solver-space) rigid link indices."""
+    if filter_link_idx:
+        n_links = scene.sim.rigid_solver.n_links
+        if np.any(np.array(filter_link_idx) < 0) or np.any(np.array(filter_link_idx) >= n_links):
+            gs.raise_exception(
+                f"{sensor_name} filter_link_idx should be in range [0, {n_links}). Got {filter_link_idx}"
+            )
+
+
 class SensorOptions(Options, Generic[SensorT]):
     """
     Base class for all sensor options.
@@ -313,12 +323,7 @@ class Contact(RigidSensorOptionsMixin["ContactSensor"], SimpleSensorOptions["Con
 
     def validate_scene(self, scene: "Scene"):
         super().validate_scene(scene)
-        if self.filter_link_idx:
-            n_links = scene.sim.rigid_solver.n_links
-            if np.any(np.array(self.filter_link_idx) < 0) or np.any(np.array(self.filter_link_idx) >= n_links):
-                gs.raise_exception(
-                    f"Contact sensor filter_link_idx should be in range [0, {n_links}). Got {self.filter_link_idx}"
-                )
+        _validate_filter_link_idx(scene, self.filter_link_idx, "Contact sensor")
 
 
 class ContactForce(RigidSensorOptionsMixin["ContactForceSensor"], SimpleSensorOptions["ContactForceSensor"]):
@@ -358,12 +363,7 @@ def model_post_init(self, context: Any) -> None:
 
     def validate_scene(self, scene: "Scene"):
         super().validate_scene(scene)
-        if self.filter_link_idx:
-            n_links = scene.sim.rigid_solver.n_links
-            if np.any(np.array(self.filter_link_idx) < 0) or np.any(np.array(self.filter_link_idx) >= n_links):
-                gs.raise_exception(
-                    f"ContactForce sensor filter_link_idx should be in range [0, {n_links}). Got {self.filter_link_idx}"
-                )
+        _validate_filter_link_idx(scene, self.filter_link_idx, "ContactForce sensor")
 
 
 class TemperatureProperties(NamedTuple):

genesis/options/sensors/tactile.py

@@ -8,6 +8,7 @@
     NonNegativeFloat,
     NonNegativeInt,
     NumericType,
+    OptionalIArrayType,
     PositiveFloat,
     PositiveInt,
     UnitIntervalVec4Type,
@@ -22,9 +23,11 @@
     RigidSensorOptionsMixin,
     SensorT,
     SimpleSensorOptions,
+    _validate_filter_link_idx,
 )
 
 if TYPE_CHECKING:
+    from genesis.engine.scene import Scene
     from genesis.engine.sensors.kinematic_tactile import (
         ContactDepthProbeSensor,
         ContactProbeSensor,
@@ -58,6 +61,25 @@ class TactileProbeSensorOptionsMixin(ProbeSensorOptionsMixin[SensorT]):
     debug_contact_color: UnitIntervalVec4Type = (1.0, 0.2, 0.0, 0.8)
 
 
+class SolverContactProbeSensorOptionsMixin(TactileProbeSensorOptionsMixin[SensorT]):
+    """
+    Options for tactile probe sensors that resolve depth against the physics solver's active contact pairs (as
+    opposed to a sampled point cloud), and can therefore scope which contacts count by the counterpart link.
+
+    Parameters
+    ----------
+    filter_link_idx : array-like[int], optional
+        Global rigid link indices (solver link space). Contacts with the sensor link where the other participant is
+        one of these links are ignored (they contribute no penetration/force). Default is empty (no filtering).
+    """
+
+    filter_link_idx: OptionalIArrayType = Field(default_factory=tuple)
+
+    def validate_scene(self, scene: "Scene"):
+        super().validate_scene(scene)
+        _validate_filter_link_idx(scene, self.filter_link_idx, f"{type(self).__name__} sensor")
+
+
 class PointCloudTactileSensorMixin(TactileProbeSensorOptionsMixin[SensorT]):
     """
     Parameters
@@ -85,7 +107,7 @@ class PointCloudTactileSensorMixin(TactileProbeSensorOptionsMixin[SensorT]):
 class ContactProbe(
     RigidSensorOptionsMixin["ContactProbeSensor"],
     SimpleSensorOptions["ContactProbeSensor"],
-    TactileProbeSensorOptionsMixin["ContactProbeSensor"],
+    SolverContactProbeSensorOptionsMixin["ContactProbeSensor"],
 ):
     """
     Returns boolean contact per probe based on the contact depth threshold.
@@ -102,7 +124,7 @@ class ContactProbe(
 class ContactDepthProbe(
     RigidSensorOptionsMixin["ContactDepthProbeSensor"],
     SimpleSensorOptions["ContactDepthProbeSensor"],
-    TactileProbeSensorOptionsMixin["ContactDepthProbeSensor"],
+    SolverContactProbeSensorOptionsMixin["ContactDepthProbeSensor"],
 ):
     """
     Returns contact depth in meters per probe.
@@ -112,7 +134,7 @@ class ContactDepthProbe(
 class KinematicTaxel(
     RigidSensorOptionsMixin["KinematicTaxelSensor"],
     SimpleSensorOptions["KinematicTaxelSensor"],
-    TactileProbeSensorOptionsMixin["KinematicTaxelSensor"],
+    SolverContactProbeSensorOptionsMixin["KinematicTaxelSensor"],
     ProbesWithNormalSensorOptionsMixin["KinematicTaxelSensor"],
 ):
     """

genesis/utils/misc.py

@@ -458,6 +458,22 @@ def _default_factory():
     return field(default_factory=_default_factory)
 
 
+def append_filter_links_idx(filter_links_idx: torch.Tensor, filter_link_idx: Sequence[int]) -> torch.Tensor:
+    """Append one contact sensor's filter-link indices as a new row of the shared ``(n_sensors, max_filter_links)``
+    table, growing the column count to fit and back-filling unused slots (and empty filters) with ``-1``.
+
+    ``-1`` is a sentinel that never matches a real link index, so kernels can scan every column unconditionally. The
+    table keeps at least one column so that even an all-empty table stays a valid (non-zero-dim) kernel argument.
+    """
+    n_sensors, cur_max = filter_links_idx.shape
+    new_max = max(cur_max, len(filter_link_idx), 1)
+    out = torch.full((n_sensors + 1, new_max), -1, dtype=gs.tc_int, device=gs.device)
+    out[:n_sensors, :cur_max] = filter_links_idx
+    if len(filter_link_idx) > 0:
+        out[n_sensors, : len(filter_link_idx)] = torch.tensor(filter_link_idx, dtype=gs.tc_int, device=gs.device)
+    return out
+
+
 def try_get_display_size() -> tuple[int | None, int | None, float | None]:
     """
     Try to connect to display if it exists and get the screen size.