Adds friction force reporting to ContactSensor

CONTRIBUTORS.md

@@ -35,6 +35,7 @@ Guidelines for modifications:
 * Pascal Roth
 * Sheikh Dawood
 * Ossama Ahmed
+* Greg Attra
 
 ## Contributors
 

source/isaaclab/config/extension.toml

@@ -1,7 +1,7 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.46.2"
+version = "0.46.3"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"

source/isaaclab/docs/CHANGELOG.rst

@@ -1,6 +1,15 @@
 Changelog
 ---------
 
+0.46.3 (2025-09-25)
+~~~~~~~~~~~~~~~~~~~
+
+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.46.2 (2025-09-13)
 ~~~~~~~~~~~~~~~~~~~

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,61 @@ 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())
+        # 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)
+            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

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.
 

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

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"],
     )