Replicates fk invalidation on other assets

source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py

@@ -333,6 +333,8 @@ def write_root_link_pose_to_sim_index(
             self.data._root_link_state_w.timestamp = -1.0
         if self.data._root_state_w is not None:
             self.data._root_state_w.timestamp = -1.0
+        self.data._fk_timestamp = -1.0  # Forces a kinematic update to get the latest body link poses.
+        SimulationManager.invalidate_fk()
 
     def write_root_link_pose_to_sim_mask(
         self,
@@ -379,6 +381,8 @@ def write_root_link_pose_to_sim_mask(
             self.data._root_link_state_w.timestamp = -1.0
         if self.data._root_state_w is not None:
             self.data._root_state_w.timestamp = -1.0
+        self.data._fk_timestamp = -1.0  # Forces a kinematic update to get the latest body link poses.
+        SimulationManager.invalidate_fk()
 
     def write_root_com_pose_to_sim_index(
         self,
@@ -433,6 +437,8 @@ def write_root_com_pose_to_sim_index(
             self.data._root_link_state_w.timestamp = -1.0
         if self.data._root_state_w is not None:
             self.data._root_state_w.timestamp = -1.0
+        self.data._fk_timestamp = -1.0  # Forces a kinematic update to get the latest body link poses.
+        SimulationManager.invalidate_fk()
 
     def write_root_com_pose_to_sim_mask(
         self,
@@ -484,6 +490,8 @@ def write_root_com_pose_to_sim_mask(
             self.data._root_link_state_w.timestamp = -1.0
         if self.data._root_state_w is not None:
             self.data._root_state_w.timestamp = -1.0
+        self.data._fk_timestamp = -1.0  # Forces a kinematic update to get the latest body link poses.
+        SimulationManager.invalidate_fk()
 
     def write_root_com_velocity_to_sim_index(
         self,

source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py

@@ -67,6 +67,7 @@ def __init__(self, root_view: ArticulationView, device: str):
         # Set initial time stamp
         self._sim_timestamp = 0.0
         self._is_primed = False
+        self._fk_timestamp = 0.0
 
         # Convert to direction vector
         gravity = wp.to_torch(SimulationManager.get_model().gravity)[0]
@@ -112,6 +113,9 @@ def update(self, dt: float) -> None:
         """
         # update the simulation timestamp
         self._sim_timestamp += dt
+        # FK is current after a sim step — keep fk_timestamp in sync unless it was explicitly invalidated
+        if self._fk_timestamp >= 0.0:
+            self._fk_timestamp = self._sim_timestamp
         # Trigger an update of the body com acceleration buffer at a higher frequency
         # since we do finite differencing.
         self.body_com_acc_w
@@ -280,6 +284,9 @@ def body_link_pose_w(self) -> wp.array:
         This quantity is the pose of the actor frame of the rigid body relative to the world.
         The orientation is provided in (x, y, z, w) format.
         """
+        if self._fk_timestamp < self._sim_timestamp:
+            SimulationManager.forward()
+            self._fk_timestamp = self._sim_timestamp
         return self._sim_bind_body_link_pose_w
 
     @property

source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py

@@ -429,6 +429,8 @@ def write_body_link_pose_to_sim_index(
         self.data._body_com_state_w.timestamp = -1.0
         self.data._body_link_state_w.timestamp = -1.0
         self.data._body_state_w.timestamp = -1.0
+        self.data._fk_timestamp = -1.0  # Forces a kinematic update to get the latest body link poses.
+        SimulationManager.invalidate_fk()
 
     def write_body_link_pose_to_sim_mask(
         self,
@@ -520,6 +522,8 @@ def write_body_com_pose_to_sim_index(
         self.data._body_link_state_w.timestamp = -1.0
         self.data._body_state_w.timestamp = -1.0
         self.data._body_com_state_w.timestamp = -1.0
+        self.data._fk_timestamp = -1.0  # Forces a kinematic update to get the latest body link poses.
+        SimulationManager.invalidate_fk()
 
     def write_body_com_pose_to_sim_mask(
         self,

source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py

@@ -68,6 +68,7 @@ def __init__(self, root_view: ArticulationView, num_bodies: int, device: str):
         # Set initial time stamp
         self._sim_timestamp = 0.0
         self._is_primed = False
+        self._fk_timestamp = 0.0
 
         # Convert gravity to direction vector
         gravity = wp.to_torch(SimulationManager.get_model().gravity)[0]
@@ -114,6 +115,9 @@ def update(self, dt: float) -> None:
         """
         # update the simulation timestamp
         self._sim_timestamp += dt
+        # FK is current after a sim step — keep fk_timestamp in sync unless it was explicitly invalidated
+        if self._fk_timestamp >= 0.0:
+            self._fk_timestamp = self._sim_timestamp
         # Trigger an update of the body com acceleration buffer at a higher frequency
         # since we do finite differencing.
         self.body_com_acc_w
@@ -190,6 +194,9 @@ def body_link_pose_w(self) -> wp.array:
         This quantity is the pose of the actor frame of the rigid body relative to the world.
         The orientation is provided in (x, y, z, w) format.
         """
+        if self._fk_timestamp < self._sim_timestamp:
+            SimulationManager.forward()
+            self._fk_timestamp = self._sim_timestamp
         return self._sim_bind_body_link_pose_w
 
     @property