Adds contact point location reporting to ContactSensor

source/isaaclab/config/extension.toml

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

source/isaaclab/docs/CHANGELOG.rst

@@ -1,9 +1,25 @@
 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.22 (2025-07-11)
 ~~~~~~~~~~~~~~~~~~~~
 
+
 Added
 ^^^^^
 
@@ -144,7 +160,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 @@ 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,32 @@ 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, 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]
+            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

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

@@ -20,6 +20,12 @@ 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."""
+
     track_air_time: bool = False
     """Whether to track the air/contact time of the bodies (time between contacts). Defaults to False."""
 
@@ -49,6 +55,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 @@
 
 # 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)

source/isaaclab/test/sensors/test_contact_sensor.py

@@ -412,33 +412,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(
@@ -449,6 +479,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 +546,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 +558,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,