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 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/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index aed50d390f8..d4128c0e3da 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -162,8 +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 - # buffer used during contact position aggregation - self._contact_position_aggregate_buffer[env_ids, :] = torch.nan + # reset friction forces + if self.cfg.track_friction_forces: + 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. @@ -317,10 +318,11 @@ 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, + # -- 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), + 0.0, device=self._device, ) # -- air/contact time between contacts @@ -382,28 +384,17 @@ 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) ) - # 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") - - 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._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3 + self._data.contact_pos_w[env_ids] = self._unpack_contact_data( + 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 + ) + self._data.friction_forces_w[env_ids] = self._unpack_contact_data( + friction_forces, buffer_count, buffer_start_indices, avg=False, default=0.0 )[env_ids] # obtain the air time @@ -436,6 +427,60 @@ 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: + """ + 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. + 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). + """ + 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) + 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[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 + ) + 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/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. 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/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"], ) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index c30a7d2eaf1..d4afc905432 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, build_simulation_context +from isaaclab.sim import SimulationCfg, SimulationContext, build_simulation_context from isaaclab.terrains import HfRandomUniformTerrainCfg, TerrainGeneratorCfg, TerrainImporterCfg from isaaclab.utils import configclass @@ -394,6 +394,68 @@ def test_sensor_print(setup_simulation): print(scene.sensors["contact_sensor"]) +# minor gravity force in -z to ensure object stays on ground plane +@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): + """ + 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" + 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) + 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) + + 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) + + # 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 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() + 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" + + """ Internal helpers. """ @@ -415,20 +477,20 @@ 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 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 - elif track_contact_points: + test_contact_data = True + 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 +505,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) @@ -460,7 +523,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 +533,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 +545,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 +612,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_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) # perform simulation step @@ -561,6 +627,31 @@ def _test_sensor_contact( expected_last_reset_contact_time = 2 * sim_dt +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 + + # 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) + # compare friction forces + if mode == ContactTestMode.IN_CONTACT: + 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 + ) + 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(sensor._data.friction_forces_w == 0.0).item() + + 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 +660,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(