Extends `ContactSensorData` by `force_matrix_w_history` attribute (#2916)

This is a follow-up to #1746 by @lukasfro — thanks for the great work
there!

I’ve been using this feature and found it really helpful. Since the only
remaining request was to resolve merge conflicts and the PR has been
quiet for a bit, I went ahead and rebased it on the latest `main` to
help move things forward.

I branched directly off @lukasfro’s [original
branch](https://github.com/lukasfro/IsaacLab/tree/feature/force_matrix_w_history)
to preserve their authorship, and only applied the changes needed to
resolve the conflicts.

Of course, if @lukasfro prefers to continue the original PR, I’m more
than happy to close this. Just hoping to be helpful and support getting
this great addition merged.

All credit for the original work goes to @lukasfro.

Fixes #1720

- New feature (non-breaking change which adds functionality)

- [x] 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
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

---------
Signed-off-by: Kelly Guo <kellyg@nvidia.com>
Co-authored-by: lukasfro <lukas.froehlich@swiss-mile.com>
Co-authored-by: Kelly Guo <kellyg@nvidia.com>

CONTRIBUTORS.md

@@ -85,6 +85,7 @@ Guidelines for modifications:
 * Lionel Gulich
 * Louis Le Lay
 * Lorenz Wellhausen
+* Lukas Fröhlich
 * Manuel Schweiger
 * Masoud Moghani
 * Michael Gussert

source/isaaclab/config/extension.toml

 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.42.28"
+version = "0.42.29"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"

source/isaaclab/docs/CHANGELOG.rst

 Changelog
 ---------
 
-0.42.28 (2025-07-18)
+0.42.29 (2025-07-18)
 ~~~~~~~~~~~~~~~~~~~~
 
 Added
@@ -11,7 +11,7 @@ Added
 * Remove on prim deletion callback workaround
 
 
-0.42.27 (2025-07-21)
+0.42.28 (2025-07-21)
 ~~~~~~~~~~~~~~~~~~~~
 
 Fixed
@@ -20,7 +20,7 @@ Fixed
 * Fixed rendering preset mode regression.
 
 
-0.42.26 (2025-07-22)
+0.42.27 (2025-07-22)
 ~~~~~~~~~~~~~~~~~~~~
 
 Changed
@@ -29,7 +29,7 @@ Changed
 * Updated teleop scripts to print to console vs omni log.
 
 
-0.42.25 (2025-07-17)
+0.42.26 (2025-07-17)
 ~~~~~~~~~~~~~~~~~~~~
 
 Changed
@@ -38,6 +38,15 @@ Changed
 * Updated test_pink_ik.py test case to pytest format.
 
 
+0.42.25 (2025-06-25)
+~~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added :attr:`omni.isaac.lab.sensors.ContactSensorData.force_matrix_w_history` that tracks the history of the filtered contact forces in the world frame.
+
+
 0.42.24 (2025-06-25)
 ~~~~~~~~~~~~~~~~~~~~
 

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

@@ -149,11 +149,10 @@ class ContactSensor(SensorBase):
         # reset accumulative data buffers
         self._data.net_forces_w[env_ids] = 0.0
         self._data.net_forces_w_history[env_ids] = 0.0
-        if self.cfg.history_length > 0:
-            self._data.net_forces_w_history[env_ids] = 0.0
         # reset force matrix
         if len(self.cfg.filter_prim_paths_expr) != 0:
             self._data.force_matrix_w[env_ids] = 0.0
+            self._data.force_matrix_w_history[env_ids] = 0.0
         # reset the current air time
         if self.cfg.track_air_time:
             self._data.current_air_time[env_ids] = 0.0
@@ -311,11 +310,18 @@ class ContactSensor(SensorBase):
             self._data.last_contact_time = torch.zeros(self._num_envs, self._num_bodies, device=self._device)
             self._data.current_contact_time = torch.zeros(self._num_envs, self._num_bodies, device=self._device)
         # force matrix: (num_envs, num_bodies, num_filter_shapes, 3)
+        # force matrix history: (num_envs, history_length, num_bodies, num_filter_shapes, 3)
         if len(self.cfg.filter_prim_paths_expr) != 0:
             num_filters = self.contact_physx_view.filter_count
             self._data.force_matrix_w = torch.zeros(
                 self._num_envs, self._num_bodies, num_filters, 3, device=self._device
             )
+            if self.cfg.history_length > 0:
+                self._data.force_matrix_w_history = torch.zeros(
+                    self._num_envs, self.cfg.history_length, self._num_bodies, num_filters, 3, device=self._device
+                )
+            else:
+                self._data.force_matrix_w_history = self._data.force_matrix_w.unsqueeze(1)
 
     def _update_buffers_impl(self, env_ids: Sequence[int]):
         """Fills the buffers of the sensor data."""
@@ -330,7 +336,7 @@ class ContactSensor(SensorBase):
         self._data.net_forces_w[env_ids, :, :] = net_forces_w.view(-1, self._num_bodies, 3)[env_ids]
         # update contact force history
         if self.cfg.history_length > 0:
-            self._data.net_forces_w_history[env_ids, 1:] = self._data.net_forces_w_history[env_ids, :-1].clone()
+            self._data.net_forces_w_history[env_ids] = self._data.net_forces_w_history[env_ids].roll(1, dims=1)
             self._data.net_forces_w_history[env_ids, 0] = self._data.net_forces_w[env_ids]
 
         # obtain the contact force matrix
@@ -341,6 +347,9 @@ class ContactSensor(SensorBase):
             force_matrix_w = self.contact_physx_view.get_contact_force_matrix(dt=self._sim_physics_dt)
             force_matrix_w = force_matrix_w.view(-1, self._num_bodies, num_filters, 3)
             self._data.force_matrix_w[env_ids] = force_matrix_w[env_ids]
+            if self.cfg.history_length > 0:
+                self._data.force_matrix_w_history[env_ids] = self._data.force_matrix_w_history[env_ids].roll(1, dims=1)
+                self._data.force_matrix_w_history[env_ids, 0] = self._data.force_matrix_w[env_ids]
 
         # obtain the pose of the sensor origin
         if self.cfg.track_pose:

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

@@ -59,7 +59,19 @@ class ContactSensorData:
     """The normal contact forces filtered between the sensor bodies and filtered bodies 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.
+    and M is the number of filtered bodies.
+
+    Note:
+        If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty, then this quantity is None.
+    """
+
+    force_matrix_w_history: torch.Tensor | None = None
+    """The normal contact forces filtered between the sensor bodies and filtered bodies in world frame.
+
+    Shape is (N, T, B, M, 3), where N is the number of sensors, T is the configured history length,
+    B is number of bodies in each sensor and M is the number of filtered bodies.
+
+    In the history dimension, the first index is the most recent and the last index is the oldest.
 
     Note:
         If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty, then this quantity is None.