[FEATURE] Add filter_link_idx to ContactForceSensor (#2771)

genesis/engine/sensors/contact_force.py

@@ -30,12 +30,31 @@ def _kernel_get_contacts_forces(
     link_b: qd.types.ndarray(),
     links_quat: qd.types.ndarray(),
     sensors_link_idx: qd.types.ndarray(),
+    filter_links_idx: qd.types.ndarray(),
     output: qd.types.ndarray(),
 ):
     for i_c, i_s, i_b in qd.ndrange(link_a.shape[-1], sensors_link_idx.shape[-1], output.shape[-1]):
         contact_data_link_a = link_a[i_b, i_c]
         contact_data_link_b = link_b[i_b, i_c]
         if contact_data_link_a == sensors_link_idx[i_s] or contact_data_link_b == sensors_link_idx[i_s]:
+            # Determine the "other" link for filter check: the one that is NOT the sensor's link.
+            if contact_data_link_a == sensors_link_idx[i_s]:
+                other_link = contact_data_link_b
+            else:
+                other_link = contact_data_link_a
+
+            # Skip if the other participant is in this sensor's filter list.
+            is_filtered = False
+            for jf in range(filter_links_idx.shape[-1]):
+                filter_link = filter_links_idx[i_s, jf]
+                if filter_link == -1:
+                    break
+                if filter_link == other_link:
+                    is_filtered = True
+                    break
+            if is_filtered:
+                continue
+
             j_s = i_s * 3  # per-sensor output dimension is 3
 
             quat_a = qd.Vector.zero(gs.qd_float, 4)
@@ -196,6 +215,10 @@ class ContactForceSensorMetadata(RigidSensorMetadataMixin, SimpleSensorMetadata)
 
     min_force: torch.Tensor = make_tensor_field((0, 3))
     max_force: torch.Tensor = make_tensor_field((0, 3))
+    # (num_contact_force_sensors, max_num_filter_links); unused slots are -1.
+    filter_links_idx: torch.Tensor = make_tensor_field((0, 0), dtype_factory=lambda: gs.tc_int)
+    # Indices into links_idx of sensors that have at least one filter link.
+    filtered_sensor_idx: torch.Tensor = make_tensor_field((0,), dtype_factory=lambda: gs.tc_int)
 
 
 class ContactForceSensor(
@@ -223,6 +246,21 @@ def build(self):
             self._shared_metadata.max_force, self._options.max_force, expand=(1, 3)
         )
 
+        # === filter_link_idx handling ===
+        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
+        )
+        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
+            )
+
     def _get_return_format(self) -> tuple[int, ...]:
         return (3,)
 
@@ -260,19 +298,37 @@ def _update_raw_data(cls, shared_metadata: ContactForceSensorMetadata, raw_data_
             force_mask_a = link_a[:, None] == shared_metadata.links_idx[None, :, None]
             force_mask_b = link_b[:, None] == shared_metadata.links_idx[None, :, None]
             force_mask = force_mask_b.to(dtype=gs.tc_float) - force_mask_a.to(dtype=gs.tc_float)
+            # Apply filter_link_idx: zero out contacts where the counterpart link is in the filter list.
+            if shared_metadata.filtered_sensor_idx.numel() > 0:
+                filt = shared_metadata.filtered_sensor_idx
+                sub_filter = shared_metadata.filter_links_idx[filt][None, :, None, :]
+                filtered_a = (link_b[:, None, :, None] == sub_filter).any(dim=-1)
+                filtered_b = (link_a[:, None, :, None] == sub_filter).any(dim=-1)
+                sub_mask_a = force_mask_a[:, filt, :]
+                sub_mask_b = force_mask_b[:, filt, :]
+                sub_filter_mask = (sub_mask_a & filtered_a) | (sub_mask_b & filtered_b)
+                force_mask[:, filt, :] = force_mask[:, filt, :].masked_fill(sub_filter_mask, 0.0)
             sensors_force = (force_mask[..., None] * force[:, None]).sum(dim=2)
             sensors_quat = links_quat[:, shared_metadata.links_idx]
             n_envs = max(shared_metadata.solver.n_envs, 1)
             result = inv_transform_by_quat(sensors_force, sensors_quat)  # (B, n_sensors, 3)
             raw_data_T[:] = result.permute(1, 2, 0).reshape(-1, n_envs)
         else:
             raw_data_T.zero_()
+            # quadrants may not handle 0-size ndarray dimensions; use a dummy placeholder so the kernel
+            # still compiles and launches. The filter loop immediately exits at the -1 sentinel.
+            filter_links = shared_metadata.filter_links_idx
+            if filter_links.shape[-1] == 0:
+                filter_links = torch.full(
+                    (filter_links.shape[0], 1), -1, dtype=gs.tc_int, device=gs.device
+                )
             _kernel_get_contacts_forces(
                 force.contiguous(),
                 link_a.contiguous(),
                 link_b.contiguous(),
                 links_quat.contiguous(),
                 shared_metadata.links_idx,
+                filter_links,
                 raw_data_T,
             )
 

genesis/options/sensors/options.py

@@ -280,6 +280,10 @@ class ContactForce(RigidSensorOptionsMixin["ContactForceSensor"], SimpleSensorOp
 
     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 excluded from the reported force. Default is empty (no
+        filtering).
     min_force : float | array-like[float, float, float], optional
         The minimum detectable absolute force per each axis. Values below this will be treated as 0. Default is 0.
     max_force : float | array-like[float, float, float], optional
@@ -290,6 +294,7 @@ class ContactForce(RigidSensorOptionsMixin["ContactForceSensor"], SimpleSensorOp
         The scale factor for the debug force arrow. Defaults to 0.01.
     """
 
+    filter_link_idx: OptionalIArrayType = Field(default_factory=tuple)
     resolution: LaxVec3FType = 0.0
 
     min_force: LaxNonNegativeUnboundedVec3FType = 0.0
@@ -298,6 +303,15 @@ class ContactForce(RigidSensorOptionsMixin["ContactForceSensor"], SimpleSensorOp
     debug_color: UnitIntervalVec4Type = (1.0, 0.0, 1.0, 0.5)
     debug_scale: PositiveFloat = 0.01
 
+    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}"
+                )
+
     def model_post_init(self, context: Any) -> None:
         super().model_post_init(context)
         if np.any(np.array(self.max_force) <= np.array(self.min_force)):