[Newton] Refresh FK before ray-cast sensor reads

source/isaaclab_newton/changelog.d/fix-raycaster-fk-sensitive-read.rst

@@ -0,0 +1,4 @@
+Fixed
+^^^^^
+
+* Fixed Newton ray-caster updates reading stale carrier poses after joint or root state writes.

source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py

@@ -2658,10 +2658,10 @@ def get_state_0(cls) -> State:
 
     @classmethod
     def get_state(cls, scene_data_provider: SceneDataProvider | None = None) -> State:
-        """Get the current Newton state for visualization.
+        """Get the current Newton state with derived transforms refreshed.
 
-        Use this method from visualizers/renderers/video recorders that need a
-        backend-agnostic Newton ``State``. When the sim backend is PhysX this
+        Use this method from sensors, visualizers, renderers, and video recorders that need
+        a backend-agnostic Newton ``State``. When the sim backend is PhysX this
         refreshes the shadow ``_state_0.body_q`` from the live PhysX scene via
         :meth:`update_visualization_state` before returning, so callers never
         observe stale transforms. Under the Newton sim backend, pending
@@ -2708,12 +2708,13 @@ def _unregister_sensor_task(cls, name: str) -> None:
 
     @classmethod
     def _update_sensor_tasks(cls, *names: str) -> None:
-        """Refit the shape and particle BVHs and run the requested scene-query tasks."""
+        """Refresh derived state, refit the BVHs, and run the requested scene-query tasks."""
         for name in names:
             if name not in cls._sensor_tasks:
                 raise KeyError(f"Newton sensor task '{name}' is not registered.")
 
-        state = cls.get_state_0()
+        # Resolve pending FK before entering the graph-capturable sensor pipeline.
+        state = cls.get_state()
         if state is not cls._sensor_state:
             cls._sensor_state = state
             cls._sensor_state_dirty = True

source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py

@@ -551,8 +551,6 @@ def update_camera(
     def render(self, render_data: RenderData):
         """Render and write to output buffers. See :meth:`~isaaclab.renderers.base_renderer.BaseRenderer.render`."""
 
-        # Refresh the shadow state under PhysX before the manager refits the BVH.
-        NewtonManager.get_state()
         if render_data.sensor_task_name is None:
             render_data.sensor_task_name = f"newton_warp_render:{id(render_data)}"
             NewtonManager._register_sensor_task(render_data.sensor_task_name, lambda: self._launch_render(render_data))

source/isaaclab_newton/isaaclab_newton/sensors/ray_caster/newton_raycast_sensor.py

@@ -166,7 +166,8 @@ def _update_ray_infos(self: Any, env_mask: wp.array) -> None:
         )
 
     def get_world_poses(self: Any, indices=None) -> tuple[ProxyArray, ProxyArray]:
-        """Return world poses for legacy camera helpers."""
+        """Return current world poses after resolving pending FK."""
+        NewtonManager.get_state()
         self._update_newton_site_transforms(
             self._sensor_site_indices, self._newton_pose_w, self._newton_pos_w.warp, self._newton_quat_w.warp
         )
@@ -192,7 +193,7 @@ def _update_newton_site_transforms(
         pos_buf: wp.array,
         quat_buf: wp.array,
     ) -> None:
-        """Update site transforms using the manager-bound model and state."""
+        """Update site transforms from manager state already refreshed by the caller."""
         model = NewtonManager.get_model()
         state = NewtonManager.get_state_0()
         wp.launch(

source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py

@@ -293,7 +293,7 @@ def test_sensor_task_builds_and_refits_bvhs_before_rendering(monkeypatch):
     """Shape and particle BVHs are built and refit before a render task runs."""
 
     state = object()
-    status = {"shape_refit": False, "particle_refit": False, "rendered": False}
+    status = {"state_refreshed": False, "shape_refit": False, "particle_refit": False, "rendered": False}
 
     class FakeModel:
         shape_count = 1
@@ -320,14 +320,20 @@ def bvh_refit_particles(self, current_state):
     model = FakeModel()
 
     def render():
+        assert status["state_refreshed"]
         assert model.bvh_shapes is not None
         assert model.bvh_particles is not None
         assert status["shape_refit"]
         assert status["particle_refit"]
         status["rendered"] = True
 
+    def get_state(cls):
+        status["state_refreshed"] = True
+        return state
+
     monkeypatch.setattr(NewtonManager, "get_model", classmethod(lambda cls: model))
     monkeypatch.setattr(NewtonManager, "get_state_0", classmethod(lambda cls: state))
+    monkeypatch.setattr(NewtonManager, "get_state", classmethod(get_state))
     monkeypatch.setattr(NewtonManager, "_model", model, raising=False)
     monkeypatch.setattr(NewtonManager, "_sensor_tasks", {}, raising=False)
     monkeypatch.setattr(NewtonManager, "_sensor_state", None, raising=False)

source/isaaclab_newton/test/sensors/test_newton_raycast_sensor.py

@@ -186,6 +186,42 @@ def test_bvh_refit_tracks_moving_geometry(sim):
     torch.testing.assert_close(distances, torch.full_like(distances, RAY_START_HEIGHT - 1.0), atol=1e-3, rtol=0)
 
 
+def test_sensor_read_refreshes_fk_after_carrier_pose_write(sim):
+    """The first sensor read after a pose write uses the refreshed carrier pose."""
+    scene = InteractiveScene(RaycastTestSceneCfg(num_envs=1))
+    sim.reset()
+    sensor = _step_and_read(sim, scene)
+    initial_distances = sensor.data.ray_distances.torch.clone()
+
+    sensor_body: RigidObject = scene["sensor_body"]
+    target_pose = sensor_body.data.root_link_pose_w.torch.clone()
+    target_pose[:, 2] += 1.0
+    sensor_body.write_root_pose_to_sim_index(root_pose=target_pose)
+    sensor.reset()
+
+    # Read the sensor first: no simulation step or FK-sensitive asset getter may hide stale body_q.
+    distances = sensor.data.ray_distances.torch
+    torch.testing.assert_close(distances, initial_distances + 1.0, atol=1e-3, rtol=0)
+
+
+def test_world_pose_getter_refreshes_fk_after_carrier_pose_write(sim):
+    """The ray-caster pose getter resolves pending FK before reading ``body_q``."""
+    scene = InteractiveScene(RaycastTestSceneCfg(num_envs=1))
+    sim.reset()
+    sensor = _step_and_read(sim, scene)
+    initial_positions = sensor.get_world_poses()[0].torch.clone()
+
+    sensor_body: RigidObject = scene["sensor_body"]
+    target_pose = sensor_body.data.root_link_pose_w.torch.clone()
+    target_pose[:, 0] += 1.0
+    sensor_body.write_root_pose_to_sim_index(root_pose=target_pose)
+
+    positions = sensor.get_world_poses()[0].torch
+    expected_positions = initial_positions.clone()
+    expected_positions[:, 0] += 1.0
+    torch.testing.assert_close(positions, expected_positions, atol=1e-3, rtol=0)
+
+
 @configclass
 class RaycastCameraSceneCfg(RaycastTestSceneCfg):
     """Adds a downward-looking Newton tiled camera next to the ray-cast sensor."""