diff --git a/source/isaaclab_newton/config/extension.toml b/source/isaaclab_newton/config/extension.toml index 8a95afe963a0..7d5691efdeb4 100644 --- a/source/isaaclab_newton/config/extension.toml +++ b/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" diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index 3e6cdd8cd115..27aec83fc7a1 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/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) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py index fb2d29091203..329d2d056755 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py +++ b/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, diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py index 2af92df4592c..c54e3825adbc 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py +++ b/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 diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py index 52216290d329..141eb11ab1c5 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py +++ b/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, diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py index 3fbc1801322f..4dc23ed85886 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py +++ b/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 diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index 138b23b9fb4b..70a1125ca48d 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/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) diff --git a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py index 4c5599e35887..8a13fb1d269e 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py +++ b/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"