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.47.1"
+version = "0.47.2"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"

source/isaaclab/docs/CHANGELOG.rst

@@ -1,6 +1,16 @@
 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)
 ~~~~~~~~~~~~~~~~~~~
 

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.
@@ -310,17 +311,33 @@ 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(
                 (self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3),
                 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 +399,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_buffer_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_buffer_data(
+                friction_forces, buffer_count, buffer_start_indices, avg=False, default=0.0
             )[env_ids]
 
         # obtain the air time
@@ -436,6 +442,58 @@ 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_buffer_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_contact_data>`_.
+
+        Args:
+            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:
+            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)
+
+            block_starts = counts.cumsum(0) - 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.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(
+            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

@@ -35,10 +35,27 @@ 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
+    """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.
+
+    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.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

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