From 353a252a231d0fd79cb0dcbc9132aabb2dfc73e2 Mon Sep 17 00:00:00 2001 From: zhouyi Date: Thu, 28 May 2026 11:03:38 +0800 Subject: [PATCH 1/3] [FEATURE] Add filter_link_idx to ContactForceSensor (#2771) Adds `filter_link_idx` parameter to `gs.sensors.ContactForce`, mirroring the existing `ContactSensor.filter_link_idx`. Contacts whose counterpart link is in the filter list are excluded from the reported force. Co-Authored-By: Claude Opus 4.7 --- genesis/engine/sensors/contact_force.py | 29 +++++++++++++++++++++++++ genesis/options/sensors/options.py | 14 ++++++++++++ 2 files changed, 43 insertions(+) diff --git a/genesis/engine/sensors/contact_force.py b/genesis/engine/sensors/contact_force.py index 688b41e360..fada1554e5 100644 --- a/genesis/engine/sensors/contact_force.py +++ b/genesis/engine/sensors/contact_force.py @@ -196,6 +196,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 +227,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,6 +279,16 @@ 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) diff --git a/genesis/options/sensors/options.py b/genesis/options/sensors/options.py index 6ea1223ce3..1c190c7f98 100644 --- a/genesis/options/sensors/options.py +++ b/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)): From 57a8b51421adcd263118e06745cb3e1e55697fef Mon Sep 17 00:00:00 2001 From: zhouyi Date: Thu, 28 May 2026 12:37:55 +0800 Subject: [PATCH 2/3] [BUG FIX] Add filter_link_idx support to non-zerocopy kernel path (#2771) The filter_link_idx feature was only implemented in the zerocopy (PyTorch) path. The quadrants kernel _kernel_get_contacts_forces ignored the filter, so users with GS_ENABLE_ZEROCOPY=0 or backends without zerocopy support would get different sensor readings for the same ContactForce setup. Fix by adding filter_links_idx parameter to the kernel and checking the filter before accumulating forces for each contact-sensor pair. Co-Authored-By: Claude Opus 4.7 --- genesis/engine/sensors/contact_force.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/genesis/engine/sensors/contact_force.py b/genesis/engine/sensors/contact_force.py index fada1554e5..2992a24332 100644 --- a/genesis/engine/sensors/contact_force.py +++ b/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) @@ -302,6 +321,7 @@ def _update_raw_data(cls, shared_metadata: ContactForceSensorMetadata, raw_data_ link_b.contiguous(), links_quat.contiguous(), shared_metadata.links_idx, + shared_metadata.filter_links_idx, raw_data_T, ) From 3120aa4bec8aedb8a2c30596fd329ac79b3558e1 Mon Sep 17 00:00:00 2001 From: zhouyi Date: Thu, 28 May 2026 12:43:24 +0800 Subject: [PATCH 3/3] [BUG FIX] Guard against empty filter_links_idx in quadrants kernel path (#2771) Co-Authored-By: Claude Opus 4.7 --- genesis/engine/sensors/contact_force.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/genesis/engine/sensors/contact_force.py b/genesis/engine/sensors/contact_force.py index 2992a24332..98cf0ba8e3 100644 --- a/genesis/engine/sensors/contact_force.py +++ b/genesis/engine/sensors/contact_force.py @@ -315,13 +315,20 @@ def _update_raw_data(cls, shared_metadata: ContactForceSensorMetadata, raw_data_ 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, - shared_metadata.filter_links_idx, + filter_links, raw_data_T, )