From 1e1e6e0293f022af96403b030458f61d3981f821 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Wed, 24 Sep 2025 14:50:07 -0400 Subject: [PATCH 01/33] removed redundant buffer --- .../isaaclab/sensors/contact_sensor/contact_sensor.py | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index aed50d390f8..af6f078011c 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -162,8 +162,6 @@ def reset(self, env_ids: Sequence[int] | None = None): # reset contact positions if self.cfg.track_contact_points: self._data.contact_pos_w[env_ids, :] = torch.nan - # buffer used during contact position aggregation - self._contact_position_aggregate_buffer[env_ids, :] = torch.nan def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: """Find bodies in the articulation based on the name keys. @@ -317,12 +315,6 @@ def _initialize_impl(self): torch.nan, device=self._device, ) - # buffer used during contact position aggregation - self._contact_position_aggregate_buffer = torch.full( - (self._num_bodies * self._num_envs, self.contact_physx_view.filter_count, 3), - torch.nan, - device=self._device, - ) # -- air/contact time between contacts if self.cfg.track_air_time: self._data.last_air_time = torch.zeros(self._num_envs, self._num_bodies, device=self._device) @@ -401,8 +393,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): agg = agg.zero_().index_add_(0, row_ids, pts) / counts.clamp_min(1).unsqueeze(1) agg[counts == 0] = float("nan") - self._contact_position_aggregate_buffer[:] = agg.view(self._num_envs * self.num_bodies, -1, 3) - self._data.contact_pos_w[env_ids] = self._contact_position_aggregate_buffer.view( + self._data.contact_pos_w[env_ids] = agg.view(self._num_envs * self.num_bodies, -1, 3).view( self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3 )[env_ids] From e9c9f47a0053e24c79711535db10459f729b409c Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Wed, 24 Sep 2025 15:18:58 -0400 Subject: [PATCH 02/33] added friction force tracking --- .../sensors/contact_sensor/contact_sensor.py | 36 +++++++++++++++---- .../contact_sensor/contact_sensor_cfg.py | 3 ++ 2 files changed, 32 insertions(+), 7 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index af6f078011c..96b369047a9 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -162,6 +162,9 @@ def reset(self, env_ids: Sequence[int] | None = None): # reset contact positions if self.cfg.track_contact_points: self._data.contact_pos_w[env_ids, :] = torch.nan + # reset friction forces + if self.cfg.track_friction_forces: + self._data.friction_forces_w[env_ids, :] = torch.nan def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: """Find bodies in the articulation based on the name keys. @@ -315,6 +318,13 @@ def _initialize_impl(self): torch.nan, device=self._device, ) + # -- friction forces at contact points + if self.cfg.track_friction_forces: + self._data.friction_forces_w = torch.full( + (self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3), + torch.nan, + device=self._device, + ) # -- air/contact time between contacts if self.cfg.track_air_time: self._data.last_air_time = torch.zeros(self._num_envs, self._num_bodies, device=self._device) @@ -369,11 +379,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): pose[..., 3:] = convert_quat(pose[..., 3:], to="wxyz") self._data.pos_w[env_ids], self._data.quat_w[env_ids] = pose.split([3, 4], dim=-1) - # obtain contact points - if self.cfg.track_contact_points: - _, buffer_contact_points, _, _, buffer_count, buffer_start_indices = ( - self.contact_physx_view.get_contact_data(dt=self._sim_physics_dt) - ) + def unpack_contact_points(contact_points, buffer_count, buffer_start_indices): # unpack the contact points: see RigidContactView.get_contact_data() documentation for details: # https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.RigidContactView.get_net_contact_forces # buffer_count: (N_envs * N_bodies, N_filters), buffer_contact_points: (N_envs * N_bodies, 3) @@ -393,9 +399,25 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): agg = agg.zero_().index_add_(0, row_ids, pts) / counts.clamp_min(1).unsqueeze(1) agg[counts == 0] = float("nan") - self._data.contact_pos_w[env_ids] = agg.view(self._num_envs * self.num_bodies, -1, 3).view( + return agg.view(self._num_envs * self.num_bodies, -1, 3).view( self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3 - )[env_ids] + ) + + # obtain contact points + if self.cfg.track_contact_points: + _, buffer_contact_points, _, _, buffer_count, buffer_start_indices = ( + self.contact_physx_view.get_contact_data(dt=self._sim_physics_dt) + ) + self._data.contact_pos_w[env_ids] = unpack_contact_points( + buffer_contact_points, buffer_count, buffer_start_indices)[env_ids] + + # obtain friction forces + if self.cfg.track_friction_forces: + friction_forces, buffer_count, buffer_start_indices = ( + self.contact_physx_view.get_friction_forces(dt=self._sim_physics_dt) + ) + self._data.friction_forces_w[env_ids] = unpack_contact_points( + friction_forces, buffer_count, buffer_start_indices)[env_ids] # obtain the air time if self.cfg.track_air_time: diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py index c51b09473bb..e230b9fd2be 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py @@ -23,6 +23,9 @@ class ContactSensorCfg(SensorBaseCfg): track_contact_points: bool = False """Whether to track the contact point locations. Defaults to False.""" + track_friction_forces: bool = False + """Whether to track the friction forces at the contact points. Defaults to False.""" + max_contact_data_count_per_prim: int = 4 """The maximum number of contacts across all batches of the sensor to keep track of. Default is 4. From e2a9f4374d161ca015af7b9dd8ef7afa32ddf332 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Wed, 24 Sep 2025 15:25:38 -0400 Subject: [PATCH 03/33] added friction force shape test --- .../contact_sensor/contact_sensor_data.py | 15 +++++ .../test/sensors/test_contact_sensor.py | 56 ++++++++++++------- 2 files changed, 50 insertions(+), 21 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py index 5d08f6058ce..8a1c6c2947c 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py @@ -38,7 +38,22 @@ class ContactSensorData: * If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty, then this quantity is an empty tensor. * If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1, then this quantity will not be calculated. + """ + + friction_forces_w: torch.Tensor | None = None + """Average of the friction forces between sensor body and filter prim in world frame. + + Shape is (N, B, M, 3), where N is the number of sensors, B is number of bodies in each sensor + and M is the number of filtered bodies. + + Collision pairs not in contact will result in NaN. + + Note: + * If the :attr:`ContactSensorCfg.track_friction_forces` is False, then this quantity is None. + * If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty, then this quantity is an empty tensor. + * If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1, then this quantity + will not be calculated. """ quat_w: torch.Tensor | None = None diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index c30a7d2eaf1..a9e405c0c15 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -422,9 +422,9 @@ def _run_contact_sensor_test( scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) scene_cfg.terrain = terrain scene_cfg.shape = shape_cfg - test_contact_position = False + test_contact_data = False if (type(shape_cfg.spawn) is sim_utils.SphereCfg) and (terrain.terrain_type == "plane"): - test_contact_position = True + test_contact_data = True elif track_contact_points: continue @@ -460,7 +460,7 @@ def _run_contact_sensor_test( scene=scene, sim_dt=sim_dt, durations=durations, - test_contact_position=test_contact_position, + test_contact_data=test_contact_data, ) _test_sensor_contact( shape=scene["shape"], @@ -470,7 +470,7 @@ def _run_contact_sensor_test( scene=scene, sim_dt=sim_dt, durations=durations, - test_contact_position=test_contact_position, + test_contact_data=test_contact_data, ) @@ -482,7 +482,7 @@ def _test_sensor_contact( scene: InteractiveScene, sim_dt: float, durations: list[float], - test_contact_position: bool = False, + test_contact_data: bool = False, ): """Test for the contact sensor. @@ -549,8 +549,11 @@ def _test_sensor_contact( expected_last_air_time=expected_last_test_contact_time, dt=duration + sim_dt, ) - if test_contact_position: + + if test_contact_data: _test_contact_position(shape, sensor, mode) + _test_contact_forces(shape, sensor, mode) + # switch the contact mode for 1 dt step before the next contact test begins. shape.write_root_pose_to_sim(root_pose=reset_pose) # perform simulation step @@ -561,6 +564,16 @@ def _test_sensor_contact( expected_last_reset_contact_time = 2 * sim_dt +def _test_contact_forces(shape: RigidObject, sensor: ContactSensor, mode: ContactTestMode) -> None: + if not sensor.cfg.track_friction_forces: + assert sensor._data.friction_forces_w is None + return + + # check shape of the contact_pos_w tensor + num_bodies = sensor.num_bodies + assert sensor._data.friction_forces_w.shape == (sensor.num_instances / num_bodies, num_bodies, 1, 3) + + def _test_contact_position(shape: RigidObject, sensor: ContactSensor, mode: ContactTestMode) -> None: """Test for the contact positions (only implemented for sphere and flat terrain) checks that the contact position is radius distance away from the root of the object @@ -569,22 +582,23 @@ def _test_contact_position(shape: RigidObject, sensor: ContactSensor, mode: Cont sensor: The sensor reporting data to be verified by the contact sensor test. mode: The contact test mode: either contact with ground plane or air time. """ - if sensor.cfg.track_contact_points: - # check shape of the contact_pos_w tensor - num_bodies = sensor.num_bodies - assert sensor._data.contact_pos_w.shape == (sensor.num_instances / num_bodies, num_bodies, 1, 3) - # check contact positions - if mode == ContactTestMode.IN_CONTACT: - contact_position = sensor._data.pos_w + torch.tensor( - [[0.0, 0.0, -shape.cfg.spawn.radius]], device=sensor._data.pos_w.device - ) - assert torch.all( - torch.abs(torch.norm(sensor._data.contact_pos_w - contact_position.unsqueeze(1), p=2, dim=-1)) < 1e-2 - ).item() - elif mode == ContactTestMode.NON_CONTACT: - assert torch.all(torch.isnan(sensor._data.contact_pos_w)).item() - else: + if not sensor.cfg.track_contact_points: assert sensor._data.contact_pos_w is None + return + + # check shape of the contact_pos_w tensor + num_bodies = sensor.num_bodies + assert sensor._data.contact_pos_w.shape == (sensor.num_instances / num_bodies, num_bodies, 1, 3) + # check contact positions + if mode == ContactTestMode.IN_CONTACT: + contact_position = sensor._data.pos_w + torch.tensor( + [[0.0, 0.0, -shape.cfg.spawn.radius]], device=sensor._data.pos_w.device + ) + assert torch.all( + torch.abs(torch.norm(sensor._data.contact_pos_w - contact_position.unsqueeze(1), p=2, dim=-1)) < 1e-2 + ).item() + elif mode == ContactTestMode.NON_CONTACT: + assert torch.all(torch.isnan(sensor._data.contact_pos_w)).item() def _check_prim_contact_state_times( From ac32441254f799bb5ff54bff0c16efc7f5a92a07 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Wed, 24 Sep 2025 15:27:40 -0400 Subject: [PATCH 04/33] added non contact test --- source/isaaclab/test/sensors/test_contact_sensor.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index a9e405c0c15..6d77ffbc824 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -552,7 +552,7 @@ def _test_sensor_contact( if test_contact_data: _test_contact_position(shape, sensor, mode) - _test_contact_forces(shape, sensor, mode) + _test_friction_forces(shape, sensor, mode) # switch the contact mode for 1 dt step before the next contact test begins. shape.write_root_pose_to_sim(root_pose=reset_pose) @@ -564,7 +564,7 @@ def _test_sensor_contact( expected_last_reset_contact_time = 2 * sim_dt -def _test_contact_forces(shape: RigidObject, sensor: ContactSensor, mode: ContactTestMode) -> None: +def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: ContactTestMode) -> None: if not sensor.cfg.track_friction_forces: assert sensor._data.friction_forces_w is None return @@ -572,6 +572,11 @@ def _test_contact_forces(shape: RigidObject, sensor: ContactSensor, mode: Contac # check shape of the contact_pos_w tensor num_bodies = sensor.num_bodies assert sensor._data.friction_forces_w.shape == (sensor.num_instances / num_bodies, num_bodies, 1, 3) + # check friction forces + if mode == ContactTestMode.IN_CONTACT: + pass + elif mode == ContactTestMode.NON_CONTACT: + assert torch.all(torch.isnan(sensor._data.friction_forces_w)).item() def _test_contact_position(shape: RigidObject, sensor: ContactSensor, mode: ContactTestMode) -> None: From 1aad6c1434e90d4de7c63b60cf4a66cc31d884a4 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Thu, 25 Sep 2025 09:39:57 -0400 Subject: [PATCH 05/33] validating non nan --- .../sensors/contact_sensor/contact_sensor.py | 59 ++++++++++--------- .../test/sensors/test_contact_sensor.py | 11 ++-- 2 files changed, 36 insertions(+), 34 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index 96b369047a9..665015dabc3 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -379,45 +379,21 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): pose[..., 3:] = convert_quat(pose[..., 3:], to="wxyz") self._data.pos_w[env_ids], self._data.quat_w[env_ids] = pose.split([3, 4], dim=-1) - def unpack_contact_points(contact_points, buffer_count, buffer_start_indices): - # unpack the contact points: see RigidContactView.get_contact_data() documentation for details: - # https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.RigidContactView.get_net_contact_forces - # buffer_count: (N_envs * N_bodies, N_filters), buffer_contact_points: (N_envs * N_bodies, 3) - counts, starts = buffer_count.view(-1), buffer_start_indices.view(-1) - n_rows, total = counts.numel(), int(counts.sum()) - # default to NaN rows - agg = torch.full((n_rows, 3), float("nan"), device=self._device, dtype=buffer_contact_points.dtype) - if total > 0: - row_ids = torch.repeat_interleave(torch.arange(n_rows, device=self._device), counts) - total = row_ids.numel() - - block_starts = counts.cumsum(0) - counts - deltas = torch.arange(total, device=counts.device) - block_starts.repeat_interleave(counts) - flat_idx = starts[row_ids] + deltas - - pts = buffer_contact_points.index_select(0, flat_idx) - agg = agg.zero_().index_add_(0, row_ids, pts) / counts.clamp_min(1).unsqueeze(1) - agg[counts == 0] = float("nan") - - return agg.view(self._num_envs * self.num_bodies, -1, 3).view( - self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3 - ) - # obtain contact points if self.cfg.track_contact_points: _, buffer_contact_points, _, _, buffer_count, buffer_start_indices = ( self.contact_physx_view.get_contact_data(dt=self._sim_physics_dt) ) - self._data.contact_pos_w[env_ids] = unpack_contact_points( + self._data.contact_pos_w[env_ids] = self._unpack_contact_points( buffer_contact_points, buffer_count, buffer_start_indices)[env_ids] # obtain friction forces if self.cfg.track_friction_forces: - friction_forces, buffer_count, buffer_start_indices = ( - self.contact_physx_view.get_friction_forces(dt=self._sim_physics_dt) + friction_forces, _, buffer_count, buffer_start_indices = ( + self.contact_physx_view.get_friction_data(dt=self._sim_physics_dt) ) - self._data.friction_forces_w[env_ids] = unpack_contact_points( - friction_forces, buffer_count, buffer_start_indices)[env_ids] + self._data.friction_forces_w[env_ids] = self._unpack_contact_points( + friction_forces, buffer_count, buffer_start_indices, avg=False)[env_ids] # obtain the air time if self.cfg.track_air_time: @@ -449,6 +425,31 @@ def unpack_contact_points(contact_points, buffer_count, buffer_start_indices): is_contact, self._data.current_contact_time[env_ids] + elapsed_time.unsqueeze(-1), 0.0 ) + def _unpack_contact_points(self, contact_points, buffer_count, buffer_start_indices, avg=True): + # unpack the contact points: see RigidContactView.get_contact_data() documentation for details: + # https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.RigidContactView.get_net_contact_forces + # buffer_count: (N_envs * N_bodies, N_filters), contact_points: (N_envs * N_bodies, 3) + counts, starts = buffer_count.view(-1), buffer_start_indices.view(-1) + n_rows, total = counts.numel(), int(counts.sum()) + # default to NaN rows + agg = torch.full((n_rows, 3), float("nan"), device=self._device, dtype=contact_points.dtype) + if total > 0: + row_ids = torch.repeat_interleave(torch.arange(n_rows, device=self._device), counts) + total = row_ids.numel() + + block_starts = counts.cumsum(0) - counts + deltas = torch.arange(total, device=counts.device) - block_starts.repeat_interleave(counts) + flat_idx = starts[row_ids] + deltas + + pts = contact_points.index_select(0, flat_idx) + agg = agg.zero_().index_add_(0, row_ids, pts) + agg = agg / counts.unsqueeze(-1) if avg else agg + agg[counts == 0] = float("nan") + + return agg.view(self._num_envs * self.num_bodies, -1, 3).view( + self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3 + ) + def _set_debug_vis_impl(self, debug_vis: bool): # set visibility of markers # note: parent only deals with callbacks. not their visibility diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 6d77ffbc824..c17a1faf4da 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -415,7 +415,7 @@ def _run_contact_sensor_test( """ for device in devices: for terrain in terrains: - for track_contact_points in [True, False]: + for track_contact_data in [True, False]: with build_simulation_context(device=device, dt=sim_dt, add_lighting=True) as sim: sim._app_control_on_stop_handle = None @@ -425,10 +425,10 @@ def _run_contact_sensor_test( test_contact_data = False if (type(shape_cfg.spawn) is sim_utils.SphereCfg) and (terrain.terrain_type == "plane"): test_contact_data = True - elif track_contact_points: + elif track_contact_data: continue - if track_contact_points: + if track_contact_data: if terrain.terrain_type == "plane": filter_prim_paths_expr = [terrain.prim_path + "/terrain/GroundPlane/CollisionPlane"] elif terrain.terrain_type == "generator": @@ -443,7 +443,8 @@ def _run_contact_sensor_test( update_period=0.0, track_air_time=True, history_length=3, - track_contact_points=track_contact_points, + track_contact_points=track_contact_data, + track_friction_forces=track_contact_data, filter_prim_paths_expr=filter_prim_paths_expr, ) scene = InteractiveScene(scene_cfg) @@ -574,7 +575,7 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta assert sensor._data.friction_forces_w.shape == (sensor.num_instances / num_bodies, num_bodies, 1, 3) # check friction forces if mode == ContactTestMode.IN_CONTACT: - pass + assert not torch.any(torch.isnan(sensor._data.friction_forces_w)).item() elif mode == ContactTestMode.NON_CONTACT: assert torch.all(torch.isnan(sensor._data.friction_forces_w)).item() From fa22e4016e54d89c39e88198a7a603040337617b Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Thu, 25 Sep 2025 10:36:23 -0400 Subject: [PATCH 06/33] test to validate vectorized solution --- .../isaaclab/sensors/contact_sensor/contact_sensor.py | 10 ++++++++++ source/isaaclab/test/sensors/test_contact_sensor.py | 10 ++++++++++ 2 files changed, 20 insertions(+) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index 665015dabc3..4c46f2d4481 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -429,6 +429,16 @@ def _unpack_contact_points(self, contact_points, buffer_count, buffer_start_indi # unpack the contact points: see RigidContactView.get_contact_data() documentation for details: # https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.RigidContactView.get_net_contact_forces # buffer_count: (N_envs * N_bodies, N_filters), contact_points: (N_envs * N_bodies, 3) + """ + Vectorizes the following nested loop: + for i in range(self._num_bodies * self._num_envs): + for j in range(self.contact_physx_view.filter_count): + start_index_ij = buffer_start_indices[i, j] + count_ij = buffer_count[i, j] + self._contact_position_aggregate_buffer[i, j, :] = torch.mean( + contact_points[start_index_ij : (start_index_ij + count_ij), :], dim=0 + ) + """ counts, starts = buffer_count.view(-1), buffer_start_indices.view(-1) n_rows, total = counts.numel(), int(counts.sum()) # default to NaN rows diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index c17a1faf4da..7fc6c77018f 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -576,6 +576,16 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta # check friction forces if mode == ContactTestMode.IN_CONTACT: assert not torch.any(torch.isnan(sensor._data.friction_forces_w)).item() + friction_forces, _, buffer_count, buffer_start_indices = sensor.contact_physx_view.get_friction_data(dt=sensor._sim_physics_dt) + for i in range(sensor.num_instances * num_bodies): + for j in range(sensor.contact_physx_view.filter_count): + start_index_ij = buffer_start_indices[i, j] + count_ij = buffer_count[i, j] + force = torch.sum( + friction_forces[start_index_ij : (start_index_ij + count_ij), :], dim=0 + ) + assert torch.allclose(force, sensor._data.friction_forces_w[i, j, :], atol=1e-5) + elif mode == ContactTestMode.NON_CONTACT: assert torch.all(torch.isnan(sensor._data.friction_forces_w)).item() From 0e4f6d4272dfe073034c6bcb3da4edb91cbfe692 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Thu, 25 Sep 2025 11:26:52 -0400 Subject: [PATCH 07/33] formatting --- .../isaaclab/sensors/contact_sensor/contact_sensor.py | 10 ++++++---- source/isaaclab/test/sensors/test_contact_sensor.py | 8 ++++---- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index 4c46f2d4481..415cdd021e4 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -385,15 +385,17 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): self.contact_physx_view.get_contact_data(dt=self._sim_physics_dt) ) self._data.contact_pos_w[env_ids] = self._unpack_contact_points( - buffer_contact_points, buffer_count, buffer_start_indices)[env_ids] + buffer_contact_points, buffer_count, buffer_start_indices + )[env_ids] # obtain friction forces if self.cfg.track_friction_forces: - friction_forces, _, buffer_count, buffer_start_indices = ( - self.contact_physx_view.get_friction_data(dt=self._sim_physics_dt) + friction_forces, _, buffer_count, buffer_start_indices = self.contact_physx_view.get_friction_data( + dt=self._sim_physics_dt ) self._data.friction_forces_w[env_ids] = self._unpack_contact_points( - friction_forces, buffer_count, buffer_start_indices, avg=False)[env_ids] + friction_forces, buffer_count, buffer_start_indices, avg=False + )[env_ids] # obtain the air time if self.cfg.track_air_time: diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 7fc6c77018f..8f2c3e11b43 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -576,14 +576,14 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta # check friction forces if mode == ContactTestMode.IN_CONTACT: assert not torch.any(torch.isnan(sensor._data.friction_forces_w)).item() - friction_forces, _, buffer_count, buffer_start_indices = sensor.contact_physx_view.get_friction_data(dt=sensor._sim_physics_dt) + friction_forces, _, buffer_count, buffer_start_indices = sensor.contact_physx_view.get_friction_data( + dt=sensor._sim_physics_dt + ) for i in range(sensor.num_instances * num_bodies): for j in range(sensor.contact_physx_view.filter_count): start_index_ij = buffer_start_indices[i, j] count_ij = buffer_count[i, j] - force = torch.sum( - friction_forces[start_index_ij : (start_index_ij + count_ij), :], dim=0 - ) + force = torch.sum(friction_forces[start_index_ij : (start_index_ij + count_ij), :], dim=0) assert torch.allclose(force, sensor._data.friction_forces_w[i, j, :], atol=1e-5) elif mode == ContactTestMode.NON_CONTACT: From c78c80c627e6df4f5b0675257371522dc79d346d Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Thu, 25 Sep 2025 11:34:27 -0400 Subject: [PATCH 08/33] updating doc / version --- source/isaaclab/config/extension.toml | 2 +- source/isaaclab/docs/CHANGELOG.rst | 8 ++++++++ source/isaaclab/test/sensors/check_contact_sensor.py | 1 + 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index d2c0e84fecd..cbc2de67560 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.47.1" +version = "0.47.2" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index eb33e88773f..e625b2470af 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,14 @@ Changelog --------- +0.47.2 (2025-10-20) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ +* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorCfg.track_friction_forces` to toggle tracking of friction forces between sensor bodies and filtered bodies. +* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorData.friction_forces_w` data field for tracking friction forces. + 0.47.1 (2025-10-17) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/test/sensors/check_contact_sensor.py b/source/isaaclab/test/sensors/check_contact_sensor.py index 30d2c9be437..431ab2f47f6 100644 --- a/source/isaaclab/test/sensors/check_contact_sensor.py +++ b/source/isaaclab/test/sensors/check_contact_sensor.py @@ -106,6 +106,7 @@ def main(): prim_path="/World/envs/env_.*/Robot/.*_FOOT", track_air_time=True, track_contact_points=True, + track_friction_forces=True, debug_vis=False, # not args_cli.headless, filter_prim_paths_expr=["/World/defaultGroundPlane/GroundPlane/CollisionPlane"], ) From ab095a94d2533f8131b4d4cbac6c7918041618c0 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Thu, 25 Sep 2025 11:43:09 -0400 Subject: [PATCH 09/33] rename func to be more generic --- .../isaaclab/sensors/contact_sensor/contact_sensor.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index 415cdd021e4..7da37c192a2 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -384,7 +384,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): _, buffer_contact_points, _, _, buffer_count, buffer_start_indices = ( self.contact_physx_view.get_contact_data(dt=self._sim_physics_dt) ) - self._data.contact_pos_w[env_ids] = self._unpack_contact_points( + self._data.contact_pos_w[env_ids] = self._unpack_contact_data( buffer_contact_points, buffer_count, buffer_start_indices )[env_ids] @@ -393,7 +393,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): friction_forces, _, buffer_count, buffer_start_indices = self.contact_physx_view.get_friction_data( dt=self._sim_physics_dt ) - self._data.friction_forces_w[env_ids] = self._unpack_contact_points( + self._data.friction_forces_w[env_ids] = self._unpack_contact_data( friction_forces, buffer_count, buffer_start_indices, avg=False )[env_ids] @@ -427,7 +427,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): is_contact, self._data.current_contact_time[env_ids] + elapsed_time.unsqueeze(-1), 0.0 ) - def _unpack_contact_points(self, contact_points, buffer_count, buffer_start_indices, avg=True): + def _unpack_contact_data(self, contact_data, buffer_count, buffer_start_indices, avg=True): # unpack the contact points: see RigidContactView.get_contact_data() documentation for details: # https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.RigidContactView.get_net_contact_forces # buffer_count: (N_envs * N_bodies, N_filters), contact_points: (N_envs * N_bodies, 3) @@ -444,7 +444,7 @@ def _unpack_contact_points(self, contact_points, buffer_count, buffer_start_indi counts, starts = buffer_count.view(-1), buffer_start_indices.view(-1) n_rows, total = counts.numel(), int(counts.sum()) # default to NaN rows - agg = torch.full((n_rows, 3), float("nan"), device=self._device, dtype=contact_points.dtype) + agg = torch.full((n_rows, 3), float("nan"), device=self._device, dtype=contact_data.dtype) if total > 0: row_ids = torch.repeat_interleave(torch.arange(n_rows, device=self._device), counts) total = row_ids.numel() @@ -453,7 +453,7 @@ def _unpack_contact_points(self, contact_points, buffer_count, buffer_start_indi deltas = torch.arange(total, device=counts.device) - block_starts.repeat_interleave(counts) flat_idx = starts[row_ids] + deltas - pts = contact_points.index_select(0, flat_idx) + pts = contact_data.index_select(0, flat_idx) agg = agg.zero_().index_add_(0, row_ids, pts) agg = agg / counts.unsqueeze(-1) if avg else agg agg[counts == 0] = float("nan") From eee482276ef5133d46acc366661203c48e253177 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Thu, 25 Sep 2025 11:48:10 -0400 Subject: [PATCH 10/33] added to contributors --- CONTRIBUTORS.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 0feaabb2c8f..e99eb97144c 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -35,6 +35,7 @@ Guidelines for modifications: * Pascal Roth * Sheikh Dawood * Ossama Ahmed +* Greg Attra ## Contributors From 664056b0bb0d655f4fa1eb31624482b76bf4d490 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Thu, 25 Sep 2025 16:11:16 -0400 Subject: [PATCH 11/33] test to check that friction forces make sense --- .../test/sensors/check_contact_sensor.py | 2 +- .../test/sensors/test_contact_sensor.py | 61 ++++++++++++++++++- 2 files changed, 61 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/test/sensors/check_contact_sensor.py b/source/isaaclab/test/sensors/check_contact_sensor.py index 431ab2f47f6..32c628d7024 100644 --- a/source/isaaclab/test/sensors/check_contact_sensor.py +++ b/source/isaaclab/test/sensors/check_contact_sensor.py @@ -106,7 +106,7 @@ def main(): prim_path="/World/envs/env_.*/Robot/.*_FOOT", track_air_time=True, track_contact_points=True, - track_friction_forces=True, + # track_friction_forces=True, debug_vis=False, # not args_cli.headless, filter_prim_paths_expr=["/World/defaultGroundPlane/GroundPlane/CollisionPlane"], ) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 8f2c3e11b43..b01d1ed5856 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -30,6 +30,8 @@ from isaaclab.terrains import HfRandomUniformTerrainCfg, TerrainGeneratorCfg, TerrainImporterCfg from isaaclab.utils import configclass +from pxr import Gf, UsdPhysics + ## # Custom helper classes. ## @@ -394,6 +396,63 @@ def test_sensor_print(setup_simulation): print(scene.sensors["contact_sensor"]) +@pytest.mark.parametrize("grav_dir", [(-10.0, 0.0, -1.0), (0.0, -10.0, -1.0)]) +@pytest.mark.isaacsim_ci +def test_friction_reporting(setup_simulation, grav_dir): + """ + Test friction force reporting for contact sensors. + + This test places a contact sensor enabled cube onto a ground plane under different gravity directions. + It then compares the normalized friction force dir with the direction of gravity to ensure they are aligned. + """ + sim_dt, _, _, _, carb_settings_iface = setup_simulation + carb_settings_iface.set_bool("/physics/disableContactProcessing", True) + device = "cuda:0" + with build_simulation_context(device=device, dt=sim_dt, add_lighting=False) as sim: + sim._app_control_on_stop_handle = None + + scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = FLAT_TERRAIN_CFG + scene_cfg.shape = CUBE_CFG + + filter_prim_paths_expr = [scene_cfg.terrain.prim_path + "/terrain/GroundPlane/CollisionPlane"] + + scene_cfg.contact_sensor = ContactSensorCfg( + prim_path=scene_cfg.shape.prim_path, + track_pose=True, + debug_vis=False, + update_period=0.0, + track_air_time=True, + history_length=3, + track_friction_forces=True, + filter_prim_paths_expr=filter_prim_paths_expr) + + scene = InteractiveScene(scene_cfg) + + # set custom gravity + UsdPhysics.Scene.Get(scene.stage, "/physicsScene").CreateGravityDirectionAttr().Set(Gf.Vec3f(*grav_dir)) + + sim.reset() + + scene["contact_sensor"].reset() + scene["shape"].write_root_pose_to_sim(root_pose=torch.tensor([0, 0.0, CUBE_CFG.spawn.size[2] / 2.0, 1, 0, 0, 0])) + + # step sim once to compute friction forces + _perform_sim_step(sim, scene, sim_dt) + + friction_force = scene["contact_sensor"]._data.friction_forces_w[0, 0, :] + friction_magnitude = friction_force.norm().item() + + assert friction_magnitude > 1e-6, "Friction forces should be non-zero" + + grav = torch.tensor(grav_dir, device=device) + norm_friction = friction_force / friction_force.norm() + norm_gravity = grav / grav.norm() + dot = torch.dot(norm_friction.squeeze(), norm_gravity) + + assert torch.isclose(torch.abs(dot), torch.tensor(1.0, device=device), atol=1e-2), "Friction force should be roughly opposite gravity direction" + + """ Internal helpers. """ @@ -573,7 +632,7 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta # check shape of the contact_pos_w tensor num_bodies = sensor.num_bodies assert sensor._data.friction_forces_w.shape == (sensor.num_instances / num_bodies, num_bodies, 1, 3) - # check friction forces + # compare friction forces if mode == ContactTestMode.IN_CONTACT: assert not torch.any(torch.isnan(sensor._data.friction_forces_w)).item() friction_forces, _, buffer_count, buffer_start_indices = sensor.contact_physx_view.get_friction_data( From 30a7b5d0dbe4628bfcd5743020250efbe5a5628b Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Thu, 25 Sep 2025 16:12:25 -0400 Subject: [PATCH 12/33] formatting --- .../isaaclab/test/sensors/test_contact_sensor.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index b01d1ed5856..361ad943239 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -21,6 +21,7 @@ import carb import pytest from flaky import flaky +from pxr import Gf, UsdPhysics import isaaclab.sim as sim_utils from isaaclab.assets import RigidObject, RigidObjectCfg @@ -30,8 +31,6 @@ from isaaclab.terrains import HfRandomUniformTerrainCfg, TerrainGeneratorCfg, TerrainImporterCfg from isaaclab.utils import configclass -from pxr import Gf, UsdPhysics - ## # Custom helper classes. ## @@ -425,7 +424,8 @@ def test_friction_reporting(setup_simulation, grav_dir): track_air_time=True, history_length=3, track_friction_forces=True, - filter_prim_paths_expr=filter_prim_paths_expr) + filter_prim_paths_expr=filter_prim_paths_expr, + ) scene = InteractiveScene(scene_cfg) @@ -435,7 +435,9 @@ def test_friction_reporting(setup_simulation, grav_dir): sim.reset() scene["contact_sensor"].reset() - scene["shape"].write_root_pose_to_sim(root_pose=torch.tensor([0, 0.0, CUBE_CFG.spawn.size[2] / 2.0, 1, 0, 0, 0])) + scene["shape"].write_root_pose_to_sim( + root_pose=torch.tensor([0, 0.0, CUBE_CFG.spawn.size[2] / 2.0, 1, 0, 0, 0]) + ) # step sim once to compute friction forces _perform_sim_step(sim, scene, sim_dt) @@ -443,14 +445,16 @@ def test_friction_reporting(setup_simulation, grav_dir): friction_force = scene["contact_sensor"]._data.friction_forces_w[0, 0, :] friction_magnitude = friction_force.norm().item() - assert friction_magnitude > 1e-6, "Friction forces should be non-zero" + assert friction_magnitude > 1e-2, "Friction forces should be non-zero" grav = torch.tensor(grav_dir, device=device) norm_friction = friction_force / friction_force.norm() norm_gravity = grav / grav.norm() dot = torch.dot(norm_friction.squeeze(), norm_gravity) - assert torch.isclose(torch.abs(dot), torch.tensor(1.0, device=device), atol=1e-2), "Friction force should be roughly opposite gravity direction" + assert torch.isclose( + torch.abs(dot), torch.tensor(1.0, device=device), atol=1e-2 + ), "Friction force should be roughly opposite gravity direction" """ From 3d5099cbc5e0790663f7e5126bc7b98111d0b17f Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Thu, 25 Sep 2025 16:14:54 -0400 Subject: [PATCH 13/33] tuned forces in test --- source/isaaclab/test/sensors/test_contact_sensor.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 361ad943239..68b4637de9e 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -395,7 +395,8 @@ def test_sensor_print(setup_simulation): print(scene.sensors["contact_sensor"]) -@pytest.mark.parametrize("grav_dir", [(-10.0, 0.0, -1.0), (0.0, -10.0, -1.0)]) +# minor gravity force in -z to ensure object stays on ground plane +@pytest.mark.parametrize("grav_dir", [(-10.0, 0.0, -.1), (0.0, -10.0, -0.1)]) @pytest.mark.isaacsim_ci def test_friction_reporting(setup_simulation, grav_dir): """ @@ -453,7 +454,7 @@ def test_friction_reporting(setup_simulation, grav_dir): dot = torch.dot(norm_friction.squeeze(), norm_gravity) assert torch.isclose( - torch.abs(dot), torch.tensor(1.0, device=device), atol=1e-2 + torch.abs(dot), torch.tensor(1.0, device=device), atol=1e-4 ), "Friction force should be roughly opposite gravity direction" From 91509ca6ed21c2c0fe68ac5f258ae2989615fe32 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Thu, 25 Sep 2025 16:19:05 -0400 Subject: [PATCH 14/33] adds back friction tracking to check sensor script --- source/isaaclab/test/sensors/check_contact_sensor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/test/sensors/check_contact_sensor.py b/source/isaaclab/test/sensors/check_contact_sensor.py index 32c628d7024..431ab2f47f6 100644 --- a/source/isaaclab/test/sensors/check_contact_sensor.py +++ b/source/isaaclab/test/sensors/check_contact_sensor.py @@ -106,7 +106,7 @@ def main(): prim_path="/World/envs/env_.*/Robot/.*_FOOT", track_air_time=True, track_contact_points=True, - # track_friction_forces=True, + track_friction_forces=True, debug_vis=False, # not args_cli.headless, filter_prim_paths_expr=["/World/defaultGroundPlane/GroundPlane/CollisionPlane"], ) From 23b00ce6eb1288dbdb1efeb333d9ef1ff732b1df Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Fri, 26 Sep 2025 12:56:46 -0400 Subject: [PATCH 15/33] docstring / typing --- .../sensors/contact_sensor/contact_sensor.py | 41 ++++++++++++------- 1 file changed, 26 insertions(+), 15 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index 7da37c192a2..2179b6edf83 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -394,7 +394,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): dt=self._sim_physics_dt ) self._data.friction_forces_w[env_ids] = self._unpack_contact_data( - friction_forces, buffer_count, buffer_start_indices, avg=False + friction_forces, buffer_count, buffer_start_indices, avg=False, default=0.0 )[env_ids] # obtain the air time @@ -427,24 +427,35 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): is_contact, self._data.current_contact_time[env_ids] + elapsed_time.unsqueeze(-1), 0.0 ) - def _unpack_contact_data(self, contact_data, buffer_count, buffer_start_indices, avg=True): - # unpack the contact points: see RigidContactView.get_contact_data() documentation for details: - # https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.RigidContactView.get_net_contact_forces - # buffer_count: (N_envs * N_bodies, N_filters), contact_points: (N_envs * N_bodies, 3) + def _unpack_contact_data(self, contact_data: torch.Tensor, buffer_count: torch.Tensor, buffer_start_indices: torch.Tensor, avg: bool = True, default: float = float("nan")) -> torch.Tensor: """ - Vectorizes the following nested loop: - for i in range(self._num_bodies * self._num_envs): - for j in range(self.contact_physx_view.filter_count): - start_index_ij = buffer_start_indices[i, j] - count_ij = buffer_count[i, j] - self._contact_position_aggregate_buffer[i, j, :] = torch.mean( - contact_points[start_index_ij : (start_index_ij + count_ij), :], dim=0 - ) + Unpacks and aggregates contact data for each (env, body, filter) group. + + This function vectorizes the following nested loop: + for i in range(self._num_bodies * self._num_envs): + for j in range(self.contact_physx_view.filter_count): + start_index_ij = buffer_start_indices[i, j] + count_ij = buffer_count[i, j] + self._contact_position_aggregate_buffer[i, j, :] = torch.mean( + contact_data[start_index_ij : (start_index_ij + count_ij), :], dim=0 + ) + + For more details, see the RigidContactView.get_contact_data() documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.RigidContactView.get_net_contact_forces + + Args: + contact_data (torch.Tensor): Flat tensor of contact data, shape (N_envs * N_bodies, 3). + buffer_count (torch.Tensor): Number of contact points per (env, body, filter), shape (N_envs * N_bodies, N_filters). + buffer_start_indices (torch.Tensor): Start indices for each (env, body, filter), shape (N_envs * N_bodies, N_filters). + avg (bool, optional): If True, average the contact data for each group; if False, sum the data. Defaults to True. + + Returns: + torch.Tensor: Aggregated contact data, shape (N_envs, N_bodies, N_filters, 3). """ counts, starts = buffer_count.view(-1), buffer_start_indices.view(-1) n_rows, total = counts.numel(), int(counts.sum()) # default to NaN rows - agg = torch.full((n_rows, 3), float("nan"), device=self._device, dtype=contact_data.dtype) + agg = torch.full((n_rows, 3), default, device=self._device, dtype=contact_data.dtype) if total > 0: row_ids = torch.repeat_interleave(torch.arange(n_rows, device=self._device), counts) total = row_ids.numel() @@ -456,7 +467,7 @@ def _unpack_contact_data(self, contact_data, buffer_count, buffer_start_indices, pts = contact_data.index_select(0, flat_idx) agg = agg.zero_().index_add_(0, row_ids, pts) agg = agg / counts.unsqueeze(-1) if avg else agg - agg[counts == 0] = float("nan") + agg[counts == 0] = default return agg.view(self._num_envs * self.num_bodies, -1, 3).view( self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3 From 428272cc5093adc7443343eda41699f9cf13c073 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Fri, 26 Sep 2025 12:57:07 -0400 Subject: [PATCH 16/33] formatting --- .../isaaclab/sensors/contact_sensor/contact_sensor.py | 9 ++++++++- source/isaaclab/test/sensors/test_contact_sensor.py | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index 2179b6edf83..7d73ec024bb 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -427,7 +427,14 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): is_contact, self._data.current_contact_time[env_ids] + elapsed_time.unsqueeze(-1), 0.0 ) - def _unpack_contact_data(self, contact_data: torch.Tensor, buffer_count: torch.Tensor, buffer_start_indices: torch.Tensor, avg: bool = True, default: float = float("nan")) -> torch.Tensor: + def _unpack_contact_data( + self, + contact_data: torch.Tensor, + buffer_count: torch.Tensor, + buffer_start_indices: torch.Tensor, + avg: bool = True, + default: float = float("nan"), + ) -> torch.Tensor: """ Unpacks and aggregates contact data for each (env, body, filter) group. diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 68b4637de9e..13c424eb018 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -396,7 +396,7 @@ def test_sensor_print(setup_simulation): # minor gravity force in -z to ensure object stays on ground plane -@pytest.mark.parametrize("grav_dir", [(-10.0, 0.0, -.1), (0.0, -10.0, -0.1)]) +@pytest.mark.parametrize("grav_dir", [(-10.0, 0.0, -0.1), (0.0, -10.0, -0.1)]) @pytest.mark.isaacsim_ci def test_friction_reporting(setup_simulation, grav_dir): """ From 646bdcb8ff36123b578b1cd9493aa99675d2bc77 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Fri, 26 Sep 2025 16:07:23 -0400 Subject: [PATCH 17/33] test comparing elements of physx friction data with reported sensor friction data --- source/isaaclab/test/sensors/test_contact_sensor.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 13c424eb018..65d3be0f7c5 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -443,15 +443,16 @@ def test_friction_reporting(setup_simulation, grav_dir): # step sim once to compute friction forces _perform_sim_step(sim, scene, sim_dt) - friction_force = scene["contact_sensor"]._data.friction_forces_w[0, 0, :] - friction_magnitude = friction_force.norm().item() + # check that forces are being reported match expected friction forces + expected_friction, _, _, _ = scene["contact_sensor"].contact_physx_view.get_friction_data(dt=sim_dt) + reported_friction = scene["contact_sensor"].data.friction_forces_w[0, 0, :] - assert friction_magnitude > 1e-2, "Friction forces should be non-zero" + assert torch.allclose(expected_friction.sum(dim=0), reported_friction[0], atol=1e-4) grav = torch.tensor(grav_dir, device=device) - norm_friction = friction_force / friction_force.norm() + norm_reported_friction = reported_friction / reported_friction.norm() norm_gravity = grav / grav.norm() - dot = torch.dot(norm_friction.squeeze(), norm_gravity) + dot = torch.dot(norm_reported_friction[0], norm_gravity) assert torch.isclose( torch.abs(dot), torch.tensor(1.0, device=device), atol=1e-4 From 058c6dc2f958b42a2d9a1f184d807d998dfdfa67 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Fri, 26 Sep 2025 16:16:56 -0400 Subject: [PATCH 18/33] tighter threshold for comparison --- source/isaaclab/test/sensors/test_contact_sensor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 65d3be0f7c5..4ff22ff986f 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -447,7 +447,7 @@ def test_friction_reporting(setup_simulation, grav_dir): expected_friction, _, _, _ = scene["contact_sensor"].contact_physx_view.get_friction_data(dt=sim_dt) reported_friction = scene["contact_sensor"].data.friction_forces_w[0, 0, :] - assert torch.allclose(expected_friction.sum(dim=0), reported_friction[0], atol=1e-4) + assert torch.allclose(expected_friction.sum(dim=0), reported_friction[0], atol=1e-6) grav = torch.tensor(grav_dir, device=device) norm_reported_friction = reported_friction / reported_friction.norm() From 817ed3f959acbb9ed80dff9ab1267426b27b0a66 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Mon, 29 Sep 2025 10:16:44 -0400 Subject: [PATCH 19/33] resetting to zeros --- .../isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py | 2 +- source/isaaclab/test/sensors/test_contact_sensor.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index 7d73ec024bb..e0688d3f013 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -164,7 +164,7 @@ def reset(self, env_ids: Sequence[int] | None = None): self._data.contact_pos_w[env_ids, :] = torch.nan # reset friction forces if self.cfg.track_friction_forces: - self._data.friction_forces_w[env_ids, :] = torch.nan + self._data.friction_forces_w[env_ids, :] = torch.zeros def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: """Find bodies in the articulation based on the name keys. diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 4ff22ff986f..80ea8260244 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -640,7 +640,7 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta assert sensor._data.friction_forces_w.shape == (sensor.num_instances / num_bodies, num_bodies, 1, 3) # compare friction forces if mode == ContactTestMode.IN_CONTACT: - assert not torch.any(torch.isnan(sensor._data.friction_forces_w)).item() + assert torch.all(torch.is_nonzero(sensor._data.friction_forces_w)).item() friction_forces, _, buffer_count, buffer_start_indices = sensor.contact_physx_view.get_friction_data( dt=sensor._sim_physics_dt ) From a20d58c90513e93d0a0cd2921e8f078ccc2e9659 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Mon, 29 Sep 2025 10:17:51 -0400 Subject: [PATCH 20/33] tests pass --- .../isaaclab/sensors/contact_sensor/contact_sensor.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index e0688d3f013..c768c335193 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -164,7 +164,7 @@ def reset(self, env_ids: Sequence[int] | None = None): self._data.contact_pos_w[env_ids, :] = torch.nan # reset friction forces if self.cfg.track_friction_forces: - self._data.friction_forces_w[env_ids, :] = torch.zeros + self._data.friction_forces_w[env_ids, :] = 0.0 def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: """Find bodies in the articulation based on the name keys. @@ -322,7 +322,7 @@ def _initialize_impl(self): if self.cfg.track_friction_forces: self._data.friction_forces_w = torch.full( (self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3), - torch.nan, + 0.0, device=self._device, ) # -- air/contact time between contacts From d81d3986b07eacdd11502c44f22404a898ac5388 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Mon, 29 Sep 2025 10:40:19 -0400 Subject: [PATCH 21/33] add param to docstring --- .../isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py | 1 + 1 file changed, 1 insertion(+) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index c768c335193..a85ab4a7a62 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -455,6 +455,7 @@ def _unpack_contact_data( buffer_count (torch.Tensor): Number of contact points per (env, body, filter), shape (N_envs * N_bodies, N_filters). buffer_start_indices (torch.Tensor): Start indices for each (env, body, filter), shape (N_envs * N_bodies, N_filters). avg (bool, optional): If True, average the contact data for each group; if False, sum the data. Defaults to True. + default (float, optional): Default value to use for groups with zero contacts. Defaults to NaN. Returns: torch.Tensor: Aggregated contact data, shape (N_envs, N_bodies, N_filters, 3). From 51c4f6346c10c973b600242b1551a6f3d3a6f5a7 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Mon, 29 Sep 2025 10:40:48 -0400 Subject: [PATCH 22/33] comment formatting --- .../sensors/contact_sensor/contact_sensor.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index a85ab4a7a62..2cd16d274af 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -439,13 +439,14 @@ def _unpack_contact_data( Unpacks and aggregates contact data for each (env, body, filter) group. This function vectorizes the following nested loop: - for i in range(self._num_bodies * self._num_envs): - for j in range(self.contact_physx_view.filter_count): - start_index_ij = buffer_start_indices[i, j] - count_ij = buffer_count[i, j] - self._contact_position_aggregate_buffer[i, j, :] = torch.mean( - contact_data[start_index_ij : (start_index_ij + count_ij), :], dim=0 - ) + + for i in range(self._num_bodies * self._num_envs): + for j in range(self.contact_physx_view.filter_count): + start_index_ij = buffer_start_indices[i, j] + count_ij = buffer_count[i, j] + self._contact_position_aggregate_buffer[i, j, :] = torch.mean( + contact_data[start_index_ij : (start_index_ij + count_ij), :], dim=0 + ) For more details, see the RigidContactView.get_contact_data() documentation: https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.RigidContactView.get_net_contact_forces From 6c0441e10eae0fde7d452f8c88fe2a02f3111750 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Mon, 29 Sep 2025 10:46:45 -0400 Subject: [PATCH 23/33] setting gravity in sim cfg --- source/isaaclab/test/sensors/test_contact_sensor.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 80ea8260244..c7ca64a1832 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -27,7 +27,7 @@ from isaaclab.assets import RigidObject, RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sensors import ContactSensor, ContactSensorCfg -from isaaclab.sim import SimulationContext, build_simulation_context +from isaaclab.sim import SimulationContext, SimulationCfg, build_simulation_context from isaaclab.terrains import HfRandomUniformTerrainCfg, TerrainGeneratorCfg, TerrainImporterCfg from isaaclab.utils import configclass @@ -408,7 +408,8 @@ def test_friction_reporting(setup_simulation, grav_dir): sim_dt, _, _, _, carb_settings_iface = setup_simulation carb_settings_iface.set_bool("/physics/disableContactProcessing", True) device = "cuda:0" - with build_simulation_context(device=device, dt=sim_dt, add_lighting=False) as sim: + sim_cfg = SimulationCfg(dt=sim_dt, device=device, gravity=grav_dir) + with build_simulation_context(sim_cfg=sim_cfg, add_lighting=False) as sim: sim._app_control_on_stop_handle = None scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) @@ -430,9 +431,6 @@ def test_friction_reporting(setup_simulation, grav_dir): scene = InteractiveScene(scene_cfg) - # set custom gravity - UsdPhysics.Scene.Get(scene.stage, "/physicsScene").CreateGravityDirectionAttr().Set(Gf.Vec3f(*grav_dir)) - sim.reset() scene["contact_sensor"].reset() From 53aed8a2f9e474df6359643228b6cb21d2de5509 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Mon, 29 Sep 2025 10:48:35 -0400 Subject: [PATCH 24/33] remove unused imports --- source/isaaclab/test/sensors/test_contact_sensor.py | 1 - 1 file changed, 1 deletion(-) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index c7ca64a1832..30201e1d363 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -21,7 +21,6 @@ import carb import pytest from flaky import flaky -from pxr import Gf, UsdPhysics import isaaclab.sim as sim_utils from isaaclab.assets import RigidObject, RigidObjectCfg From 1fbdb77941c08208ad3c36504de89e72eec2a36a Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Mon, 29 Sep 2025 14:35:50 -0400 Subject: [PATCH 25/33] formatter --- source/isaaclab/test/sensors/test_contact_sensor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 30201e1d363..61e05a39d8e 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -26,7 +26,7 @@ from isaaclab.assets import RigidObject, RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sensors import ContactSensor, ContactSensorCfg -from isaaclab.sim import SimulationContext, SimulationCfg, build_simulation_context +from isaaclab.sim import SimulationCfg, SimulationContext, build_simulation_context from isaaclab.terrains import HfRandomUniformTerrainCfg, TerrainGeneratorCfg, TerrainImporterCfg from isaaclab.utils import configclass From 536b4a5a6dc4e7d9b26863326c8b83a2d548b565 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Mon, 20 Oct 2025 13:59:13 -0400 Subject: [PATCH 26/33] updated tests --- source/isaaclab/test/sensors/test_contact_sensor.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 61e05a39d8e..d4afc905432 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -637,7 +637,7 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta assert sensor._data.friction_forces_w.shape == (sensor.num_instances / num_bodies, num_bodies, 1, 3) # compare friction forces if mode == ContactTestMode.IN_CONTACT: - assert torch.all(torch.is_nonzero(sensor._data.friction_forces_w)).item() + assert torch.any(torch.abs(sensor._data.friction_forces_w) > 1e-5).item() friction_forces, _, buffer_count, buffer_start_indices = sensor.contact_physx_view.get_friction_data( dt=sensor._sim_physics_dt ) @@ -649,7 +649,7 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta assert torch.allclose(force, sensor._data.friction_forces_w[i, j, :], atol=1e-5) elif mode == ContactTestMode.NON_CONTACT: - assert torch.all(torch.isnan(sensor._data.friction_forces_w)).item() + assert torch.all(sensor._data.friction_forces_w == 0.0).item() def _test_contact_position(shape: RigidObject, sensor: ContactSensor, mode: ContactTestMode) -> None: From 033760685b79618c55f715400389e050541d4753 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Mon, 20 Oct 2025 16:20:26 -0400 Subject: [PATCH 27/33] removed unhelpful comment --- .../isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py | 1 - 1 file changed, 1 deletion(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index 2cd16d274af..d4128c0e3da 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -463,7 +463,6 @@ def _unpack_contact_data( """ counts, starts = buffer_count.view(-1), buffer_start_indices.view(-1) n_rows, total = counts.numel(), int(counts.sum()) - # default to NaN rows agg = torch.full((n_rows, 3), default, device=self._device, dtype=contact_data.dtype) if total > 0: row_ids = torch.repeat_interleave(torch.arange(n_rows, device=self._device), counts) From 9972b9b7735642f5f0c94978a5e0106a47b066c9 Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Wed, 22 Oct 2025 11:26:08 -0400 Subject: [PATCH 28/33] addressing pr comments --- source/isaaclab/docs/CHANGELOG.rst | 2 ++ .../sensors/contact_sensor/contact_sensor.py | 26 +++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index e625b2470af..ceee33374b5 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -6,9 +6,11 @@ Changelog Added ^^^^^ + * Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorCfg.track_friction_forces` to toggle tracking of friction forces between sensor bodies and filtered bodies. * Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorData.friction_forces_w` data field for tracking friction forces. + 0.47.1 (2025-10-17) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index d4128c0e3da..5c84c36492d 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -384,7 +384,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): _, buffer_contact_points, _, _, buffer_count, buffer_start_indices = ( self.contact_physx_view.get_contact_data(dt=self._sim_physics_dt) ) - self._data.contact_pos_w[env_ids] = self._unpack_contact_data( + self._data.contact_pos_w[env_ids] = self._unpack_contact_buffer_data( buffer_contact_points, buffer_count, buffer_start_indices )[env_ids] @@ -393,7 +393,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): friction_forces, _, buffer_count, buffer_start_indices = self.contact_physx_view.get_friction_data( dt=self._sim_physics_dt ) - self._data.friction_forces_w[env_ids] = self._unpack_contact_data( + self._data.friction_forces_w[env_ids] = self._unpack_contact_buffer_data( friction_forces, buffer_count, buffer_start_indices, avg=False, default=0.0 )[env_ids] @@ -427,7 +427,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): is_contact, self._data.current_contact_time[env_ids] + elapsed_time.unsqueeze(-1), 0.0 ) - def _unpack_contact_data( + def _unpack_contact_buffer_data( self, contact_data: torch.Tensor, buffer_count: torch.Tensor, @@ -448,33 +448,31 @@ def _unpack_contact_data( contact_data[start_index_ij : (start_index_ij + count_ij), :], dim=0 ) - For more details, see the RigidContactView.get_contact_data() documentation: - https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.RigidContactView.get_net_contact_forces + For more details, see the `RigidContactView.get_contact_data() documentation `_. Args: - contact_data (torch.Tensor): Flat tensor of contact data, shape (N_envs * N_bodies, 3). - buffer_count (torch.Tensor): Number of contact points per (env, body, filter), shape (N_envs * N_bodies, N_filters). - buffer_start_indices (torch.Tensor): Start indices for each (env, body, filter), shape (N_envs * N_bodies, N_filters). - avg (bool, optional): If True, average the contact data for each group; if False, sum the data. Defaults to True. - default (float, optional): Default value to use for groups with zero contacts. Defaults to NaN. + contact_data: Flat tensor of contact data, shape (N_envs * N_bodies, 3). + buffer_count: Number of contact points per (env, body, filter), shape (N_envs * N_bodies, N_filters). + buffer_start_indices: Start indices for each (env, body, filter), shape (N_envs * N_bodies, N_filters). + avg: If True, average the contact data for each group; if False, sum the data. Defaults to True. + default: Default value to use for groups with zero contacts. Defaults to NaN. Returns: - torch.Tensor: Aggregated contact data, shape (N_envs, N_bodies, N_filters, 3). + Aggregated contact data, shape (N_envs, N_bodies, N_filters, 3). """ counts, starts = buffer_count.view(-1), buffer_start_indices.view(-1) n_rows, total = counts.numel(), int(counts.sum()) agg = torch.full((n_rows, 3), default, device=self._device, dtype=contact_data.dtype) if total > 0: row_ids = torch.repeat_interleave(torch.arange(n_rows, device=self._device), counts) - total = row_ids.numel() block_starts = counts.cumsum(0) - counts - deltas = torch.arange(total, device=counts.device) - block_starts.repeat_interleave(counts) + deltas = torch.arange(row_ids.numel(), device=counts.device) - block_starts.repeat_interleave(counts) flat_idx = starts[row_ids] + deltas pts = contact_data.index_select(0, flat_idx) agg = agg.zero_().index_add_(0, row_ids, pts) - agg = agg / counts.unsqueeze(-1) if avg else agg + agg = agg / counts.clamp_min(1).unsqueeze(-1) if avg else agg agg[counts == 0] = default return agg.view(self._num_envs * self.num_bodies, -1, 3).view( From 6e6486e30638e30155988bbaec8cb1e9c1b6d17f Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Wed, 22 Oct 2025 13:43:51 -0400 Subject: [PATCH 29/33] using torch.testing / working on test for invalid config --- .../contact_sensor/contact_sensor_data.py | 2 +- .../test/sensors/test_contact_sensor.py | 46 +++++++++++++++++-- 2 files changed, 43 insertions(+), 5 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py index 8a1c6c2947c..dd5e5aa8431 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py @@ -41,7 +41,7 @@ class ContactSensorData: """ friction_forces_w: torch.Tensor | None = None - """Average of the friction forces between sensor body and filter prim in world frame. + """Sum of the friction forces between sensor body and filter prim in world frame. Shape is (N, B, M, 3), where N is the number of sensors, B is number of bodies in each sensor and M is the number of filtered bodies. diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index d4afc905432..74ee1fc0cc4 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -444,16 +444,54 @@ def test_friction_reporting(setup_simulation, grav_dir): expected_friction, _, _, _ = scene["contact_sensor"].contact_physx_view.get_friction_data(dt=sim_dt) reported_friction = scene["contact_sensor"].data.friction_forces_w[0, 0, :] - assert torch.allclose(expected_friction.sum(dim=0), reported_friction[0], atol=1e-6) + torch.testing.assert_close(expected_friction.sum(dim=0), reported_friction[0], atol=1e-6) + # check that friction force direction opposes gravity direction grav = torch.tensor(grav_dir, device=device) norm_reported_friction = reported_friction / reported_friction.norm() norm_gravity = grav / grav.norm() dot = torch.dot(norm_reported_friction[0], norm_gravity) - assert torch.isclose( - torch.abs(dot), torch.tensor(1.0, device=device), atol=1e-4 - ), "Friction force should be roughly opposite gravity direction" + torch.testing.assert_close(torch.abs(dot), torch.tensor(1.0, device=device), atol=1e-4) + + +@pytest.mark.isaacsim_ci +def test_invalid_config(setup_simulation): + sim_dt, _, _, _, carb_settings_iface = setup_simulation + carb_settings_iface.set_bool("/physics/disableContactProcessing", True) + device = "cuda:0" + sim_cfg = SimulationCfg(dt=sim_dt, device=device) + with build_simulation_context(sim_cfg=sim_cfg, add_lighting=False) as sim: + sim._app_control_on_stop_handle = None + + scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = FLAT_TERRAIN_CFG + scene_cfg.shape = CUBE_CFG + + # filter_prim_paths_expr = [scene_cfg.terrain.prim_path + "/terrain/GroundPlane/CollisionPlane"] + + scene_cfg.contact_sensor = ContactSensorCfg( + prim_path=scene_cfg.shape.prim_path, + track_pose=True, + debug_vis=False, + update_period=0.0, + track_air_time=True, + history_length=3, + track_friction_forces=True, + filter_prim_paths_expr=[], + ) + + scene = InteractiveScene(scene_cfg) + + sim.reset() + + scene["contact_sensor"].reset() + scene["shape"].write_root_pose_to_sim( + root_pose=torch.tensor([0, 0.0, CUBE_CFG.spawn.size[2] / 2.0, 1, 0, 0, 0]) + ) + + # step sim once to compute friction forces + _perform_sim_step(sim, scene, sim_dt) """ From 551a24509bc43e8932e1593aa341e7ae0a01beca Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Wed, 22 Oct 2025 14:03:47 -0400 Subject: [PATCH 30/33] config tests --- .../sensors/contact_sensor/contact_sensor.py | 15 ++++++ .../test/sensors/test_contact_sensor.py | 51 +++++++++++++++---- 2 files changed, 56 insertions(+), 10 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index 5c84c36492d..676d6272ff2 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -311,6 +311,21 @@ def _initialize_impl(self): if self.cfg.track_pose: self._data.pos_w = torch.zeros(self._num_envs, self._num_bodies, 3, device=self._device) self._data.quat_w = torch.zeros(self._num_envs, self._num_bodies, 4, device=self._device) + + # check if filter paths are valid + if self.cfg.track_contact_points or self.cfg.track_friction_forces: + if len(self.cfg.filter_prim_paths_expr) == 0: + raise ValueError( + "The 'filter_prim_paths_expr' is empty. Please specify a valid filter pattern to track" + f" {'contact points' if self.cfg.track_contact_points else 'friction forces'}." + ) + if self.cfg.max_contact_data_count_per_prim < 1: + raise ValueError( + f"The 'max_contact_data_count_per_prim' is {self.cfg.max_contact_data_count_per_prim}. " + "Please set it to a value greater than 0 to track" + f" {'contact points' if self.cfg.track_contact_points else 'friction forces'}." + ) + # -- position of contact points if self.cfg.track_contact_points: self._data.contact_pos_w = torch.full( diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index 74ee1fc0cc4..f8b9fc374ac 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -456,7 +456,7 @@ def test_friction_reporting(setup_simulation, grav_dir): @pytest.mark.isaacsim_ci -def test_invalid_config(setup_simulation): +def test_invalid_prim_paths_config(setup_simulation): sim_dt, _, _, _, carb_settings_iface = setup_simulation carb_settings_iface.set_bool("/physics/disableContactProcessing", True) device = "cuda:0" @@ -468,8 +468,6 @@ def test_invalid_config(setup_simulation): scene_cfg.terrain = FLAT_TERRAIN_CFG scene_cfg.shape = CUBE_CFG - # filter_prim_paths_expr = [scene_cfg.terrain.prim_path + "/terrain/GroundPlane/CollisionPlane"] - scene_cfg.contact_sensor = ContactSensorCfg( prim_path=scene_cfg.shape.prim_path, track_pose=True, @@ -481,17 +479,50 @@ def test_invalid_config(setup_simulation): filter_prim_paths_expr=[], ) - scene = InteractiveScene(scene_cfg) + try: + _ = InteractiveScene(scene_cfg) - sim.reset() + sim.reset() - scene["contact_sensor"].reset() - scene["shape"].write_root_pose_to_sim( - root_pose=torch.tensor([0, 0.0, CUBE_CFG.spawn.size[2] / 2.0, 1, 0, 0, 0]) + assert False, "Expected ValueError due to invalid contact sensor configuration." + except ValueError: + pass + + +@pytest.mark.isaacsim_ci +def test_invalid_max_contact_points_config(setup_simulation): + sim_dt, _, _, _, carb_settings_iface = setup_simulation + carb_settings_iface.set_bool("/physics/disableContactProcessing", True) + device = "cuda:0" + sim_cfg = SimulationCfg(dt=sim_dt, device=device) + with build_simulation_context(sim_cfg=sim_cfg, add_lighting=False) as sim: + sim._app_control_on_stop_handle = None + + scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = FLAT_TERRAIN_CFG + scene_cfg.shape = CUBE_CFG + filter_prim_paths_expr = [scene_cfg.terrain.prim_path + "/terrain/GroundPlane/CollisionPlane"] + + scene_cfg.contact_sensor = ContactSensorCfg( + prim_path=scene_cfg.shape.prim_path, + track_pose=True, + debug_vis=False, + update_period=0.0, + track_air_time=True, + history_length=3, + track_friction_forces=True, + filter_prim_paths_expr=filter_prim_paths_expr, + max_contact_data_count_per_prim=0, ) - # step sim once to compute friction forces - _perform_sim_step(sim, scene, sim_dt) + try: + _ = InteractiveScene(scene_cfg) + + sim.reset() + + assert False, "Expected ValueError due to invalid contact sensor configuration." + except ValueError: + pass """ From 1ed5152cac70b53e7dbe10a58c7d250f5c48584d Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Wed, 22 Oct 2025 14:07:08 -0400 Subject: [PATCH 31/33] atol/rtol --- source/isaaclab/test/sensors/test_contact_sensor.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index f8b9fc374ac..c8c61d770c0 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -444,7 +444,7 @@ def test_friction_reporting(setup_simulation, grav_dir): expected_friction, _, _, _ = scene["contact_sensor"].contact_physx_view.get_friction_data(dt=sim_dt) reported_friction = scene["contact_sensor"].data.friction_forces_w[0, 0, :] - torch.testing.assert_close(expected_friction.sum(dim=0), reported_friction[0], atol=1e-6) + torch.testing.assert_close(expected_friction.sum(dim=0), reported_friction[0], atol=1e-6, rtol=1e-5) # check that friction force direction opposes gravity direction grav = torch.tensor(grav_dir, device=device) @@ -452,7 +452,7 @@ def test_friction_reporting(setup_simulation, grav_dir): norm_gravity = grav / grav.norm() dot = torch.dot(norm_reported_friction[0], norm_gravity) - torch.testing.assert_close(torch.abs(dot), torch.tensor(1.0, device=device), atol=1e-4) + torch.testing.assert_close(torch.abs(dot), torch.tensor(1.0, device=device), atol=1e-4, rtol=1e-3) @pytest.mark.isaacsim_ci From a4b0c83789c932f32114668d9012e71a999ba70a Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Wed, 22 Oct 2025 14:11:28 -0400 Subject: [PATCH 32/33] updated docstring --- .../sensors/contact_sensor/contact_sensor_data.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py index dd5e5aa8431..c2b6f814bdd 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py @@ -35,9 +35,10 @@ class ContactSensorData: Note: * If the :attr:`ContactSensorCfg.track_contact_points` is False, then this quantity is None. - * If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty, then this quantity is an empty tensor. - * If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1, then this quantity - will not be calculated. + * If the :attr:`ContactSensorCfg.track_contact_points` is True, a ValueError will be raised if: + * If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty. + * If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1. + will not be calculated. """ friction_forces_w: torch.Tensor | None = None @@ -51,9 +52,10 @@ class ContactSensorData: Note: * If the :attr:`ContactSensorCfg.track_friction_forces` is False, then this quantity is None. - * If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty, then this quantity is an empty tensor. - * If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1, then this quantity - will not be calculated. + * If the :attr:`ContactSensorCfg.track_friction_forces` is True, a ValueError will be raised if: + * If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty. + * If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1. + will not be calculated. """ quat_w: torch.Tensor | None = None From 47b9ce38443a8652d8fd0736ca3deda6acb5ac2d Mon Sep 17 00:00:00 2001 From: Greg Attra Date: Mon, 27 Oct 2025 13:20:12 -0400 Subject: [PATCH 33/33] formatting --- .../sensors/contact_sensor/contact_sensor_data.py | 12 ++++++------ source/isaaclab/test/sensors/test_contact_sensor.py | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py index c2b6f814bdd..529e286269b 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py @@ -36,9 +36,9 @@ class ContactSensorData: * If the :attr:`ContactSensorCfg.track_contact_points` is False, then this quantity is None. * If the :attr:`ContactSensorCfg.track_contact_points` is True, a ValueError will be raised if: - * If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty. - * If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1. - will not be calculated. + * If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty. + * If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1. + will not be calculated. """ friction_forces_w: torch.Tensor | None = None @@ -53,9 +53,9 @@ class ContactSensorData: * If the :attr:`ContactSensorCfg.track_friction_forces` is False, then this quantity is None. * If the :attr:`ContactSensorCfg.track_friction_forces` is True, a ValueError will be raised if: - * If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty. - * If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1. - will not be calculated. + * If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty. + * If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1. + will not be calculated. """ quat_w: torch.Tensor | None = None diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index c8c61d770c0..2876063e6fb 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -703,7 +703,7 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta # check shape of the contact_pos_w tensor num_bodies = sensor.num_bodies - assert sensor._data.friction_forces_w.shape == (sensor.num_instances / num_bodies, num_bodies, 1, 3) + assert sensor._data.friction_forces_w.shape == (sensor.num_instances // num_bodies, num_bodies, 1, 3) # compare friction forces if mode == ContactTestMode.IN_CONTACT: assert torch.any(torch.abs(sensor._data.friction_forces_w) > 1e-5).item() @@ -735,7 +735,7 @@ def _test_contact_position(shape: RigidObject, sensor: ContactSensor, mode: Cont # check shape of the contact_pos_w tensor num_bodies = sensor.num_bodies - assert sensor._data.contact_pos_w.shape == (sensor.num_instances / num_bodies, num_bodies, 1, 3) + assert sensor._data.contact_pos_w.shape == (sensor.num_instances // num_bodies, num_bodies, 1, 3) # check contact positions if mode == ContactTestMode.IN_CONTACT: contact_position = sensor._data.pos_w + torch.tensor(