From 164fa11551cb7389879f23c79500f61a66d9dc80 Mon Sep 17 00:00:00 2001
From: Ashwin Khadke <133695616+akhadke-bdai@users.noreply.github.com>
Date: Tue, 10 Jun 2025 09:31:35 -0400
Subject: [PATCH 01/10] Updating contact sensor to report contact points (#109)

---
 .../sensors/contact_sensor/contact_sensor.py  | 37 +++++++++++++++++++
 .../contact_sensor/contact_sensor_cfg.py      |  4 ++
 .../contact_sensor/contact_sensor_data.py     | 13 +++++++
 .../test/sensors/check_contact_sensor.py      | 11 ++++--
 4 files changed, 61 insertions(+), 4 deletions(-)

diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
index ff1a0ea5f1e..ac2dd050b92 100644
--- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
+++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
@@ -160,6 +160,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.
@@ -304,6 +309,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)
@@ -348,6 +366,25 @@ 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._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
+            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(
+                        buffer_contact_points[start_index_ij : (start_index_ij + count_ij), :], dim=0
+                    )
+            # reshape from [num_env*num_bodies, num_filter_shapes, 3] to [num_env, num_bodies, num_filter_shapes, 3]
+            self._data.contact_pos_w[env_ids] = self._contact_position_aggregate_buffer.view(
+                -1, 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
index fcaa6bc1220..3e2bca2658c 100644
--- 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,9 @@ 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."""
+
     track_air_time: bool = False
     """Whether to track the air/contact time of the bodies (time between contacts). Defaults to False."""
 
@@ -49,6 +52,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
index cd01630af61..d42ee11d585 100644
--- 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,19 @@ 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.
+    """
+
     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
index 5b3b13161b9..5d29a5cf05c 100644
--- a/source/isaaclab/test/sensors/check_contact_sensor.py
+++ b/source/isaaclab/test/sensors/check_contact_sensor.py
@@ -63,9 +63,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 +102,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=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

From 51574557283c80039ce72fb9bf87711bddb8d6b7 Mon Sep 17 00:00:00 2001
From: James Tigue <jtigue@theaiinstitute.com>
Date: Wed, 2 Jul 2025 11:17:39 -0400
Subject: [PATCH 02/10] update test to pytest

---
 .../test/sensors/test_contact_sensor.py       | 151 ++++++++++++++----
 1 file changed, 124 insertions(+), 27 deletions(-)

diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py
index 0e3ad136315..2cc00799191 100644
--- a/source/isaaclab/test/sensors/test_contact_sensor.py
+++ b/source/isaaclab/test/sensors/test_contact_sensor.py
@@ -412,33 +412,101 @@ 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
+                    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,
+                    )
+
+                    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 + "/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(
@@ -449,6 +517,7 @@ def _test_sensor_contact(
     scene: InteractiveScene,
     sim_dt: float,
     durations: list[float],
+    test_contact_position: bool = False,
 ):
     """Test for the contact sensor.
 
@@ -515,6 +584,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
@@ -525,6 +596,32 @@ def _test_sensor_contact(
         expected_last_reset_contact_time = 2 * sim_dt
 
 
+def _test_contact_position(self, 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
+        self.assertEqual(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,

From 8a7f581c1d47e5d7785fb905d449d3a270fbe6b8 Mon Sep 17 00:00:00 2001
From: James Tigue <jtigue@theaiinstitute.com>
Date: Wed, 2 Jul 2025 14:38:34 -0400
Subject: [PATCH 03/10] fix implementation and documentation

---
 source/isaaclab/config/extension.toml         |  2 +-
 source/isaaclab/docs/CHANGELOG.rst            | 14 ++++++
 .../sensors/contact_sensor/contact_sensor.py  |  6 ++-
 .../contact_sensor/contact_sensor_cfg.py      |  3 ++
 .../contact_sensor/contact_sensor_data.py     |  2 +
 .../test/sensors/test_contact_sensor.py       | 44 ++-----------------
 6 files changed, 27 insertions(+), 44 deletions(-)

diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml
index cf769e34cfb..e4d413d35c4 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.40.11"
+version = "0.41.0"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"
diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst
index 7dfe075a3a4..54ef7d0144f 100644
--- a/source/isaaclab/docs/CHANGELOG.rst
+++ b/source/isaaclab/docs/CHANGELOG.rst
@@ -1,6 +1,20 @@
 Changelog
 ---------
 
+0.41.0 (2025-07-2)
+~~~~~~~~~~~~~~~~~~
+
+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.40.11 (2025-06-27)
 ~~~~~~~~~~~~~~~~~~~~
 
diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
index ac2dd050b92..9d2b0d597f8 100644
--- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
+++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
@@ -283,7 +283,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),
         )
         # resolve the true count of bodies
         self._num_bodies = self.body_physx_view.count // self._num_envs
@@ -369,7 +371,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
         # 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._dt)
+                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
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 3e2bca2658c..4e687928e89 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."""
 
+    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."""
+
     track_air_time: bool = False
     """Whether to track the air/contact time of the bodies (time between contacts). Defaults to False."""
 
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 d42ee11d585..6717d0c0dd1 100644
--- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py
+++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py
@@ -34,6 +34,8 @@ 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.
     """
 
     quat_w: torch.Tensor | None = None
diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py
index 2cc00799191..3742bdfa466 100644
--- a/source/isaaclab/test/sensors/test_contact_sensor.py
+++ b/source/isaaclab/test/sensors/test_contact_sensor.py
@@ -416,44 +416,6 @@ def _run_contact_sensor_test(
                 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,
-                    )
-
                     scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False)
                     scene_cfg.terrain = terrain
                     scene_cfg.shape = shape_cfg
@@ -465,7 +427,7 @@ def _run_contact_sensor_test(
 
                     if track_contact_points:
                         if terrain.terrain_type == "plane":
-                            filter_prim_paths_expr = [terrain.prim_path + "/GroundPlane/CollisionPlane"]
+                            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:
@@ -596,7 +558,7 @@ def _test_sensor_contact(
         expected_last_reset_contact_time = 2 * sim_dt
 
 
-def _test_contact_position(self, shape: RigidObject, sensor: ContactSensor, mode: ContactTestMode) -> None:
+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:
@@ -607,7 +569,7 @@ def _test_contact_position(self, shape: RigidObject, sensor: ContactSensor, mode
     if sensor.cfg.track_contact_points:
         # check shape of the contact_pos_w tensor
         num_bodies = sensor.num_bodies
-        self.assertEqual(sensor._data.contact_pos_w.shape, (sensor.num_instances / num_bodies, num_bodies, 1, 3))
+        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(

From 6c31703a33f77026c1df04396e2899c585230f99 Mon Sep 17 00:00:00 2001
From: James Tigue <jtigue@theaiinstitute.com>
Date: Fri, 11 Jul 2025 11:21:32 -0400
Subject: [PATCH 04/10] vectorize contact points

---
 .../sensors/contact_sensor/contact_sensor.py     | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
index 9d2b0d597f8..7cbd1375795 100644
--- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
+++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
@@ -285,7 +285,7 @@ def _initialize_impl(self):
         self._contact_physx_view = self._physics_sim_view.create_rigid_contact_view(
             body_names_glob,
             filter_patterns=filter_prim_paths_glob,
-            max_contact_data_count=self.cfg.max_contact_data_count_per_prim * len(body_names),
+            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
@@ -375,6 +375,20 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
             )
             # 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, max_count = buffer_count.view(-1), int(buffer_count.max())
+            if max_count > 0:
+                rel = torch.arange(max_count, device=counts.device).unsqueeze(0).expand(counts.size(0), max_count)
+                # 1) pull out all points → (n_env*n_bodies, max_count, and mask out invalid slots (r ≥ counts[k])
+                pts = buffer_contact_points[buffer_start_indices.view(-1).unsqueeze(1) + rel]
+                pts = pts * (rel < counts.unsqueeze(1)).unsqueeze(2)
+                # zero out invalid rows # 2) sum & divide → (n_env*n_bodies*n_filter, 3) → reshape: (n_env*n_bodies, n_filter, 3)
+                self._contact_position_aggregate_buffer[:] = (pts.sum(dim=1) / counts.unsqueeze(1)).view(
+                    self._num_envs * self.num_bodies, self.contact_physx_view.filter_count, 3
+                )
+            else:
+                self._contact_position_aggregate_buffer[:] = float("nan")
+
             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]

From 985a74349fe72bf5f7f1e9cd28fd83e717176046 Mon Sep 17 00:00:00 2001
From: James Tigue <jtigue@theaiinstitute.com>
Date: Fri, 11 Jul 2025 13:10:52 -0400
Subject: [PATCH 05/10] apply vectorization and timing in check

---
 .../sensors/contact_sensor/contact_sensor.py   | 14 +++++++-------
 .../test/sensors/check_contact_sensor.py       | 18 +++++++++++++++---
 2 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
index 7cbd1375795..f886001bcea 100644
--- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
+++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
@@ -389,13 +389,13 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
             else:
                 self._contact_position_aggregate_buffer[:] = float("nan")
 
-            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(
-                        buffer_contact_points[start_index_ij : (start_index_ij + count_ij), :], dim=0
-                    )
+            # 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(
+            #             buffer_contact_points[start_index_ij : (start_index_ij + count_ij), :], dim=0
+            #         )
             # reshape from [num_env*num_bodies, num_filter_shapes, 3] to [num_env, num_bodies, num_filter_shapes, 3]
             self._data.contact_pos_w[env_ids] = self._contact_position_aggregate_buffer.view(
                 -1, self._num_bodies, self.contact_physx_view.filter_count, 3
diff --git a/source/isaaclab/test/sensors/check_contact_sensor.py b/source/isaaclab/test/sensors/check_contact_sensor.py
index 5d29a5cf05c..30d2c9be437 100644
--- 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
@@ -105,7 +106,7 @@ def main():
         prim_path="/World/envs/env_.*/Robot/.*_FOOT",
         track_air_time=True,
         track_contact_points=True,
-        debug_vis=not args_cli.headless,
+        debug_vis=False,  # not args_cli.headless,
         filter_prim_paths_expr=["/World/defaultGroundPlane/GroundPlane/CollisionPlane"],
     )
     contact_sensor = ContactSensor(cfg=contact_sensor_cfg)
@@ -129,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.
@@ -139,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
@@ -162,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)

From 135280d3d889c393f2c3ed3eff15f5449fc84921 Mon Sep 17 00:00:00 2001
From: James Tigue <jtigue@theaiinstitute.com>
Date: Fri, 18 Jul 2025 15:28:20 -0400
Subject: [PATCH 06/10] remove commented code

---
 .../isaaclab/sensors/contact_sensor/contact_sensor.py      | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
index f886001bcea..6ffa5e8252e 100644
--- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
+++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
@@ -389,13 +389,6 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
             else:
                 self._contact_position_aggregate_buffer[:] = float("nan")
 
-            # 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(
-            #             buffer_contact_points[start_index_ij : (start_index_ij + count_ij), :], dim=0
-            #         )
             # reshape from [num_env*num_bodies, num_filter_shapes, 3] to [num_env, num_bodies, num_filter_shapes, 3]
             self._data.contact_pos_w[env_ids] = self._contact_position_aggregate_buffer.view(
                 -1, self._num_bodies, self.contact_physx_view.filter_count, 3

From e64a84bed6443e063808f23bb92eb9f9a35bc3f8 Mon Sep 17 00:00:00 2001
From: James Tigue <jtigue@theaiinstitute.com>
Date: Fri, 18 Jul 2025 15:37:02 -0400
Subject: [PATCH 07/10] fix log lint

---
 source/isaaclab/docs/CHANGELOG.rst | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst
index f0ecfc05357..a413b1b8099 100644
--- a/source/isaaclab/docs/CHANGELOG.rst
+++ b/source/isaaclab/docs/CHANGELOG.rst
@@ -1,6 +1,7 @@
 Changelog
 ---------
 
+
 0.41.0 (2025-07-2)
 ~~~~~~~~~~~~~~~~~~
 
@@ -13,7 +14,7 @@ Added
   amount of contacts per sensor body.
 * Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorData.contact_pos_w` data field for tracking contact point
   locations.
-  
+
 
 0.40.22 (2025-07-11)
 ~~~~~~~~~~~~~~~~~~~~

From 9a64b8a81feb35a793f683627cfcad4d03ad3386 Mon Sep 17 00:00:00 2001
From: James Tigue <jtigue@theaiinstitute.com>
Date: Mon, 11 Aug 2025 09:12:02 -0400
Subject: [PATCH 08/10] adjust the contact points calcualtion and add notes for
 max_contact_data_count_per_prim

---
 .../sensors/contact_sensor/contact_sensor.py  | 33 ++++++++++---------
 .../contact_sensor/contact_sensor_cfg.py      | 12 ++++++-
 2 files changed, 29 insertions(+), 16 deletions(-)

diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
index 4e92afc464e..ba2a019ef64 100644
--- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
+++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py
@@ -385,22 +385,25 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
             # 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, max_count = buffer_count.view(-1), int(buffer_count.max())
-            if max_count > 0:
-                rel = torch.arange(max_count, device=counts.device).unsqueeze(0).expand(counts.size(0), max_count)
-                # 1) pull out all points → (n_env*n_bodies, max_count, and mask out invalid slots (r ≥ counts[k])
-                pts = buffer_contact_points[buffer_start_indices.view(-1).unsqueeze(1) + rel]
-                pts = pts * (rel < counts.unsqueeze(1)).unsqueeze(2)
-                # zero out invalid rows # 2) sum & divide → (n_env*n_bodies*n_filter, 3) → reshape: (n_env*n_bodies, n_filter, 3)
-                self._contact_position_aggregate_buffer[:] = (pts.sum(dim=1) / counts.unsqueeze(1)).view(
-                    self._num_envs * self.num_bodies, self.contact_physx_view.filter_count, 3
-                )
-            else:
-                self._contact_position_aggregate_buffer[:] = float("nan")
-
-            # reshape from [num_env*num_bodies, num_filter_shapes, 3] to [num_env, num_bodies, num_filter_shapes, 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(
-                -1, self._num_bodies, self.contact_physx_view.filter_count, 3
+                self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3
             )[env_ids]
 
         # obtain the air time
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 4e687928e89..c51b09473bb 100644
--- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py
+++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py
@@ -24,7 +24,17 @@ class ContactSensorCfg(SensorBaseCfg):
     """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."""
+    """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."""

From 0e84bd4641c800a8ea44570a1421c3be1f817b86 Mon Sep 17 00:00:00 2001
From: James Tigue <166445701+jtigue-bdai@users.noreply.github.com>
Date: Thu, 14 Aug 2025 14:16:16 -0400
Subject: [PATCH 09/10] Format

Signed-off-by: James Tigue <166445701+jtigue-bdai@users.noreply.github.com>
---
 source/isaaclab/docs/CHANGELOG.rst | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst
index edcc29eb724..675b46eb587 100644
--- a/source/isaaclab/docs/CHANGELOG.rst
+++ b/source/isaaclab/docs/CHANGELOG.rst
@@ -13,8 +13,8 @@ Added
   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)
 ~~~~~~~~~~~~~~~~~~~
 

From ea932ebc59cf7392acf19e1901d597155f355fa6 Mon Sep 17 00:00:00 2001
From: James Tigue <166445701+jtigue-bdai@users.noreply.github.com>
Date: Thu, 14 Aug 2025 14:17:29 -0400
Subject: [PATCH 10/10] Format

Signed-off-by: James Tigue <166445701+jtigue-bdai@users.noreply.github.com>
---
 source/isaaclab/docs/CHANGELOG.rst | 1 -
 1 file changed, 1 deletion(-)

diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst
index 675b46eb587..dbd4a090bec 100644
--- a/source/isaaclab/docs/CHANGELOG.rst
+++ b/source/isaaclab/docs/CHANGELOG.rst
@@ -174,7 +174,6 @@ Changed
 0.42.26 (2025-06-29)
 ~~~~~~~~~~~~~~~~~~~~
 
-
 Added
 ^^^^^