Replicates fk invalidation on other assets

source/isaaclab_newton/config/extension.toml

@@ -1,7 +1,7 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.5.20"
+version = "0.5.21"
 
 # Description
 title = "Newton simulation interfaces for IsaacLab core package"

source/isaaclab_newton/docs/CHANGELOG.rst

@@ -1,6 +1,20 @@
 Changelog
 ---------
 
+0.5.21 (2026-04-23)
+~~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed stale :attr:`~isaaclab_newton.assets.RigidObjectData.body_link_pose_w` and
+  :attr:`~isaaclab_newton.assets.RigidObjectCollectionData.body_link_pose_w` after calls to
+  ``write_root_*_pose_to_sim_*`` / ``write_body_*_pose_to_sim_*``. The pose writers now
+  invalidate forward kinematics via :meth:`~isaaclab_newton.physics.SimulationManager.invalidate_fk`
+  and an internal ``_fk_timestamp`` so the next pose read triggers a forward-kinematics update,
+  matching the behavior already implemented in :class:`~isaaclab_newton.assets.Articulation`.
+
+
 0.5.20 (2026-04-22)
 ~~~~~~~~~~~~~~~~~~~
 

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

source/isaaclab_newton/test/assets/test_rigid_object.py

@@ -1259,3 +1259,49 @@ def test_warmup_attach_stage_not_called_for_cpu():
             f"This indicates the CPU MBP broadphase double-initialization regression is present: "
             f"attach_stage() + force_load_physics_from_usd() must not be combined for CPU."
         )
+
+
+@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
+@pytest.mark.parametrize("writer", ["link_index", "link_mask", "com_index", "com_mask"])
+@pytest.mark.isaacsim_ci
+def test_body_link_pose_w_fresh_after_root_pose_write(device, writer):
+    """Regression: ``body_link_pose_w`` must reflect a freshly written root pose without an intervening sim step.
+
+    After ``write_root_{link,com}_pose_to_sim_{index,mask}``, the cached ``_sim_bind_body_link_pose_w``
+    (Newton ``body_q``) is stale until forward kinematics is re-evaluated. The getter must call
+    :meth:`SimulationManager.forward` so the returned tensor matches the written pose. Without the fix,
+    the getter returns the pre-write value.
+    """
+    num_cubes = 2
+    with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim:
+        sim._app_control_on_stop_handle = None
+        cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.5, device=device)
+
+        sim.reset()
+        assert cube_object.is_initialized
+
+        # Step once so that _sim_timestamp > 0 and caches are primed.
+        sim.step()
+        cube_object.update(sim.cfg.dt)
+
+        # Prime the body_link_pose_w cache with the current pose.
+        _ = wp.to_torch(cube_object.data.body_link_pose_w).clone()
+
+        # Build a target pose clearly distinct from the current one.
+        target_pose = wp.to_torch(cube_object.data.root_link_pose_w).clone()
+        target_pose[..., 0] += 10.0
+        target_pose[..., 1] += 5.0
+        target_pose[..., 2] += 2.0
+
+        if writer == "link_index":
+            cube_object.write_root_link_pose_to_sim_index(root_pose=target_pose)
+        elif writer == "link_mask":
+            cube_object.write_root_link_pose_to_sim_mask(root_pose=target_pose)
+        elif writer == "com_index":
+            cube_object.write_root_com_pose_to_sim_index(root_pose=target_pose)
+        elif writer == "com_mask":
+            cube_object.write_root_com_pose_to_sim_mask(root_pose=target_pose)
+
+        # Read without stepping: getter must trigger forward kinematics and return the fresh pose.
+        body_link = wp.to_torch(cube_object.data.body_link_pose_w).view(num_cubes, 7)
+        torch.testing.assert_close(body_link[..., :3], target_pose[..., :3], rtol=1e-4, atol=1e-4)

source/isaaclab_newton/test/assets/test_rigid_object_collection.py

@@ -906,3 +906,50 @@ def test_write_object_state_functions_data_consistency(num_envs, num_cubes, devi
             torch.testing.assert_close(body_com_vel_w, com_vel_w)
             torch.testing.assert_close(body_link_pose_w, link_pose_w)
             torch.testing.assert_close(body_com_vel_w[..., 3:], link_vel_w[..., 3:])
+
+
+@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
+@pytest.mark.parametrize("writer", ["link_index", "link_mask", "com_index", "com_mask"])
+@pytest.mark.isaacsim_ci
+def test_body_pose_write_marks_fk_dirty(device, writer):
+    """Regression: ``write_body_{link,com}_pose_to_sim_{index,mask}`` must mark FK dirty.
+
+    For a collection, ``_sim_bind_body_link_pose_w`` is bound directly to the simulator's root-transforms
+    buffer, so the property read is not what becomes stale — the simulator's internal ``body_q`` used by
+    collision detection is. The write methods must therefore call :meth:`SimulationManager.invalidate_fk`
+    and invalidate ``_fk_timestamp`` so downstream consumers re-run forward kinematics before the next step.
+    Without the fix, ``_fk_dirty`` remains ``False`` after an explicit pose write.
+    """
+    num_envs = 2
+    num_cubes = 2
+    with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim:
+        sim._app_control_on_stop_handle = None
+        cube_object, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.5, device=device)
+
+        sim.reset()
+        assert cube_object.is_initialized
+
+        sim.step()
+        cube_object.update(sim.cfg.dt)
+
+        # Clear the dirty flag so we can observe that the write sets it.
+        SimulationManager.forward()
+        assert not SimulationManager._fk_dirty
+        assert cube_object.data._fk_timestamp >= 0.0
+
+        target_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone()
+        target_pose[..., 0] += 10.0
+        target_pose[..., 1] += 5.0
+        target_pose[..., 2] += 2.0
+
+        if writer == "link_index":
+            cube_object.write_body_link_pose_to_sim_index(body_poses=target_pose)
+        elif writer == "link_mask":
+            cube_object.write_body_link_pose_to_sim_mask(body_poses=target_pose)
+        elif writer == "com_index":
+            cube_object.write_body_com_pose_to_sim_index(body_poses=target_pose)
+        elif writer == "com_mask":
+            cube_object.write_body_com_pose_to_sim_mask(body_poses=target_pose)
+
+        assert SimulationManager._fk_dirty, "pose write must call SimulationManager.invalidate_fk()"
+        assert cube_object.data._fk_timestamp < 0.0, "pose write must reset data._fk_timestamp to -1.0"