Extend ContactSensorData by force_matrix_w_history attribute

CONTRIBUTORS.md

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

source/extensions/omni.isaac.lab/config/extension.toml

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

source/extensions/omni.isaac.lab/docs/CHANGELOG.rst

@@ -1,6 +1,15 @@
 Changelog
 ---------
 
+0.30.7 (2025-01-28)
+~~~~~~~~~~~~~~~~~~~
+
+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.30.6 (2025-01-17)
 ~~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/contact_sensor/contact_sensor.py

@@ -142,11 +142,10 @@ def reset(self, env_ids: Sequence[int] | None = None):
         # 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
@@ -305,11 +304,18 @@ def _initialize_impl(self):
             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."""
@@ -324,7 +330,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
         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
@@ -335,6 +341,9 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
             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/extensions/omni.isaac.lab/omni/isaac/lab/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.