Adds contact point location reporting to ContactSensor (isaac-sim#2842)

jtigue-bdai · akhadke-bdai · kellyguo11 · web-flow · commit 4fefc0dbdeea · 2025-08-14T13:06:41.000-07:00
# Description This PR: - Adds ContactSensorCfg.track_contact_points to toggle tracking of contact point locations between sensor bodies and filtered bodies. - Adds ContactSensorCfg.max_contact_data_per_prim to configure the maximum amount of contacts per sensor body. - Adds ContactSensorData.contact_pos_w data field for tracking contact point locations. Fixes # (issue)  ## Type of change  - New feature (non-breaking change which adds functionality) ## Checklist - [ ] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format` - [ ] I have made corresponding changes to the documentation - [ ] My changes generate no new warnings - [ ] I have added tests that prove my fix is effective or that my feature works - [ ] I have updated the changelog and the corresponding version in the extension's `config/extension.toml` file - [ ] I have added my name to the `CONTRIBUTORS.md` or my name already exists there --------- Signed-off-by: James Tigue <166445701+jtigue-bdai@users.noreply.github.com> Signed-off-by: Kelly Guo <kellyg@nvidia.com> Signed-off-by: ooctipus <zhengyuz@nvidia.com> Co-authored-by: Ashwin Khadke <133695616+akhadke-bdai@users.noreply.github.com> Co-authored-by: Kelly Guo <kellyg@nvidia.com> Co-authored-by: ooctipus <zhengyuz@nvidia.com>
diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml
@@ -1,7 +1,8 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.44.10"
+version = "0.45.0"
+
 
 # Description
 title = "Isaac Lab framework for Robot Learning"
diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst
@@ -1,6 +1,20 @@
 Changelog
 ---------
 
+0.45.0 (2025-08-07)
+~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorCfg.track_contact_points` to toggle tracking of contact
+  point locations between sensor bodies and filtered bodies.
+* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorCfg.max_contact_data_per_prim` to configure the maximum
+  amount of contacts per sensor body.
+* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorData.contact_pos_w` data field for tracking contact point
+  locations.
+
+
 0.44.12 (2025-08-12)
 ~~~~~~~~~~~~~~~~~~~
 
@@ -31,6 +45,7 @@ instantaneous term done count at reset. This let to inaccurate aggregation of te
 happeningduring the traing. Instead we log the episodic term done.
 
 
+
 0.44.9 (2025-07-30)
 ~~~~~~~~~~~~~~~~~~~
 
@@ -330,7 +345,6 @@ Changed
 Added
 ^^^^^
 
-
 * Added unit test for :func:`~isaaclab.utils.math.quat_inv`.
 
 Fixed
diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
@@ -159,6 +159,11 @@ def reset(self, env_ids: Sequence[int] | None = None):
             self._data.last_air_time[env_ids] = 0.0
             self._data.current_contact_time[env_ids] = 0.0
             self._data.last_contact_time[env_ids] = 0.0
+        # 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.
@@ -277,7 +282,9 @@ def _initialize_impl(self):
         # create a rigid prim view for the sensor
         self._body_physx_view = self._physics_sim_view.create_rigid_body_view(body_names_glob)
         self._contact_physx_view = self._physics_sim_view.create_rigid_contact_view(
-            body_names_glob, filter_patterns=filter_prim_paths_glob
+            body_names_glob,
+            filter_patterns=filter_prim_paths_glob,
+            max_contact_data_count=self.cfg.max_contact_data_count_per_prim * len(body_names) * self._num_envs,
         )
         # resolve the true count of bodies
         self._num_bodies = self.body_physx_view.count // self._num_envs
@@ -303,6 +310,19 @@ 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)
+        # -- 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,
+                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)
@@ -357,6 +377,35 @@ 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)
+            )
+            # 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
+            )[env_ids]
+
         # obtain the air time
         if self.cfg.track_air_time:
             # -- time elapsed since last update
diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py
@@ -20,6 +20,22 @@ class ContactSensorCfg(SensorBaseCfg):
     track_pose: bool = False
     """Whether to track the pose of the sensor's origin. Defaults to False."""
 
+    track_contact_points: bool = False
+    """Whether to track the contact point locations. 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.
+
+    This parameter sets the total maximum counts of the simulation across all bodies and environments. The total number
+    of contacts allowed is max_contact_data_count_per_prim*num_envs*num_sensor_bodies.
+
+    .. note::
+
+        If the environment is very contact rich it is suggested to increase this parameter to avoid out of bounds memory
+        errors and loss of contact data leading to inaccurate measurements.
+
+        """
+
     track_air_time: bool = False
     """Whether to track the air/contact time of the bodies (time between contacts). Defaults to False."""
 
@@ -49,6 +65,7 @@ class ContactSensorCfg(SensorBaseCfg):
         single primitive in that environment. If the sensor primitive corresponds to multiple primitives, the
         filtering will not work as expected. Please check :class:`~isaaclab.sensors.contact_sensor.ContactSensor`
         for more details.
+        If track_contact_points is true, then filter_prim_paths_expr cannot be an empty list!
     """
 
     visualizer_cfg: VisualizationMarkersCfg = CONTACT_SENSOR_MARKER_CFG.replace(prim_path="/Visuals/ContactSensor")
diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py
@@ -23,6 +23,21 @@ class ContactSensorData:
         If the :attr:`ContactSensorCfg.track_pose` is False, then this quantity is None.
     """
 
+    contact_pos_w: torch.Tensor | None = None
+    """Average of the positions of contact points 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_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.
+    """
+
     quat_w: torch.Tensor | None = None
     """Orientation of the sensor origin in quaternion (w, x, y, z) in world frame.
 
diff --git a/source/isaaclab/test/sensors/check_contact_sensor.py b/source/isaaclab/test/sensors/check_contact_sensor.py
@@ -20,7 +20,7 @@
 
 # add argparse arguments
 parser = argparse.ArgumentParser(description="Contact Sensor Test Script")
-parser.add_argument("--num_robots", type=int, default=64, help="Number of robots to spawn.")
+parser.add_argument("--num_robots", type=int, default=128, help="Number of robots to spawn.")
 
 # append AppLauncher cli args
 AppLauncher.add_app_launcher_args(parser)
@@ -45,6 +45,7 @@
 import isaaclab.sim as sim_utils
 from isaaclab.assets import Articulation
 from isaaclab.sensors.contact_sensor import ContactSensor, ContactSensorCfg
+from isaaclab.utils.timer import Timer
 
 ##
 # Pre-defined configs
@@ -63,9 +64,8 @@ def design_scene():
     cfg = sim_utils.GroundPlaneCfg()
     cfg.func("/World/defaultGroundPlane", cfg)
     # Lights
-    cfg = sim_utils.SphereLightCfg()
-    cfg.func("/World/Light/GreySphere", cfg, translation=(4.5, 3.5, 10.0))
-    cfg.func("/World/Light/WhiteSphere", cfg, translation=(-4.5, 3.5, 10.0))
+    cfg = sim_utils.DomeLightCfg(intensity=2000)
+    cfg.func("/World/Light/DomeLight", cfg, translation=(-4.5, 3.5, 10.0))
 
 
 """
@@ -103,7 +103,11 @@ def main():
     robot = Articulation(cfg=robot_cfg)
     # Contact sensor
     contact_sensor_cfg = ContactSensorCfg(
-        prim_path="/World/envs/env_.*/Robot/.*_SHANK", track_air_time=True, debug_vis=not args_cli.headless
+        prim_path="/World/envs/env_.*/Robot/.*_FOOT",
+        track_air_time=True,
+        track_contact_points=True,
+        debug_vis=False,  # not args_cli.headless,
+        filter_prim_paths_expr=["/World/defaultGroundPlane/GroundPlane/CollisionPlane"],
     )
     contact_sensor = ContactSensor(cfg=contact_sensor_cfg)
     # filter collisions within each environment instance
@@ -126,6 +130,7 @@ def main():
     sim_dt = decimation * physics_dt
     sim_time = 0.0
     count = 0
+    dt = []
     # Simulate physics
     while simulation_app.is_running():
         # If simulation is stopped, then exit.
@@ -136,14 +141,20 @@ def main():
             sim.step(render=False)
             continue
         # reset
-        if count % 1000 == 0:
+        if count % 1000 == 0 and count != 0:
             # reset counters
             sim_time = 0.0
             count = 0
+            print("=" * 80)
+            print("avg dt real-time", sum(dt) / len(dt))
+            print("=" * 80)
+
             # reset dof state
             joint_pos, joint_vel = robot.data.default_joint_pos, robot.data.default_joint_vel
             robot.write_joint_state_to_sim(joint_pos, joint_vel)
             robot.reset()
+            dt = []
+
         # perform 4 steps
         for _ in range(decimation):
             # apply actions
@@ -159,6 +170,10 @@ def main():
         count += 1
         # update the buffers
         if sim.is_playing():
+            with Timer() as timer:
+                contact_sensor.update(sim_dt, force_recompute=True)
+                dt.append(timer.time_elapsed)
+
             contact_sensor.update(sim_dt, force_recompute=True)
             if count % 100 == 0:
                 print("Sim-time: ", sim_time)
diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py
