Adds contact point location reporting to ContactSensor (#2842)

# 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)

<!-- As a practice, it is recommended to open an issue to have
discussions on the proposed pull request.
This makes it easier for the community to keep track of what is being
developed or added, and if a given feature
is demanded by more than one party. -->

## Type of change

<!-- As you go through the list, delete the ones that are not
applicable. -->

- 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>

source/isaaclab/config/extension.toml

 [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"

source/isaaclab/docs/CHANGELOG.rst

 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

source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py

@@ -159,6 +159,11 @@ class ContactSensor(SensorBase):
             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 @@ class ContactSensor(SensorBase):
         # 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 @@ class ContactSensor(SensorBase):
         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 @@ class ContactSensor(SensorBase):
             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

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

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.
 

source/isaaclab/test/sensors/check_contact_sensor.py

@@ -20,7 +20,7 @@ from isaaclab.app import AppLauncher
 
 # 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 @@ from isaacsim.core.utils.viewports import set_camera_view
 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)

source/isaaclab/test/sensors/test_contact_sensor.py

@@ -414,33 +414,63 @@ def _run_contact_sensor_test(
     """
     for device in devices:
         for terrain in terrains:
-            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
-                scene_cfg.contact_sensor = ContactSensorCfg(
-                    prim_path=shape_cfg.prim_path,
-                    track_pose=True,
-                    debug_vis=False,
-                    update_period=0.0,
-                    track_air_time=True,
-                    history_length=3,
-                )
-                scene = InteractiveScene(scene_cfg)
-
-                # Check that contact processing is enabled
-                assert not carb_settings_iface.get("/physics/disableContactProcessing")
-
-                # Play the simulator
-                sim.reset()
-
-                _test_sensor_contact(
-                    scene["shape"], scene["contact_sensor"], ContactTestMode.IN_CONTACT, sim, scene, sim_dt, durations
-                )
-                _test_sensor_contact(
-                    scene["shape"], scene["contact_sensor"], ContactTestMode.NON_CONTACT, sim, scene, sim_dt, durations
-                )
+            for track_contact_points 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
+                    if (type(shape_cfg.spawn) is sim_utils.SphereCfg) and (terrain.terrain_type == "plane"):
+                        test_contact_position = True
+                    elif track_contact_points:
+                        continue
+
+                    if track_contact_points:
+                        if terrain.terrain_type == "plane":
+                            filter_prim_paths_expr = [terrain.prim_path + "/terrain/GroundPlane/CollisionPlane"]
+                        elif terrain.terrain_type == "generator":
+                            filter_prim_paths_expr = [terrain.prim_path + "/terrain/mesh"]
+                    else:
+                        filter_prim_paths_expr = []
+
+                    scene_cfg.contact_sensor = ContactSensorCfg(
+                        prim_path=shape_cfg.prim_path,
+                        track_pose=True,
+                        debug_vis=False,
+                        update_period=0.0,
+                        track_air_time=True,
+                        history_length=3,
+                        track_contact_points=track_contact_points,
+                        filter_prim_paths_expr=filter_prim_paths_expr,
+                    )
+                    scene = InteractiveScene(scene_cfg)
+
+                    # Play the simulation
+                    sim.reset()
+
+                    # Run contact time and air time tests.
+                    _test_sensor_contact(
+                        shape=scene["shape"],
+                        sensor=scene["contact_sensor"],
+                        mode=ContactTestMode.IN_CONTACT,
+                        sim=sim,
+                        scene=scene,
+                        sim_dt=sim_dt,
+                        durations=durations,
+                        test_contact_position=test_contact_position,
+                    )
+                    _test_sensor_contact(
+                        shape=scene["shape"],
+                        sensor=scene["contact_sensor"],
+                        mode=ContactTestMode.NON_CONTACT,
+                        sim=sim,
+                        scene=scene,
+                        sim_dt=sim_dt,
+                        durations=durations,
+                        test_contact_position=test_contact_position,
+                    )
 
 
 def _test_sensor_contact(
@@ -451,6 +481,7 @@ def _test_sensor_contact(
     scene: InteractiveScene,
     sim_dt: float,
     durations: list[float],
+    test_contact_position: bool = False,
 ):
     """Test for the contact sensor.
 
@@ -517,6 +548,8 @@ def _test_sensor_contact(
                 expected_last_air_time=expected_last_test_contact_time,
                 dt=duration + sim_dt,
             )
+        if test_contact_position:
+            _test_contact_position(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
@@ -527,6 +560,32 @@ def _test_sensor_contact(
         expected_last_reset_contact_time = 2 * sim_dt
 
 
+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
+    Args:
+        shape: The contact prim used for the contact sensor test.
+        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:
+        assert sensor._data.contact_pos_w is None
+
+
 def _check_prim_contact_state_times(
     sensor: ContactSensor,
     expected_air_time: float,