Fixes outdated sensor data after reset

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

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

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

@@ -1,6 +1,15 @@
 Changelog
 ---------
 
+0.27.15 (2024-11-17)
+~~~~~~~~~~~~~~~~~~~~
+
+Changed
+^^^^^^^
+
+* Added call to update articulation kinematics after reset to ensure states are updated for non-rendering sensors. Previously, some changes in reset such as modifying joint states would not be reflected in the rigid body states immediately after reset.
+
+
 0.27.14 (2024-10-23)
 ~~~~~~~~~~~~~~~~~~~~
 
@@ -153,6 +162,7 @@ Added
   the class :class:`omni.isaac.lab.command_generators.NullCommandGenerator`.
 * Moved the ``meshes`` attribute in the :class:`omni.isaac.lab.sensors.RayCaster` class from class variable to instance variable.
   This prevents the meshes to overwrite each other.
+>>>>>>> 9d6594b4b360d5c48e460d5886440affd2810563
 
 
 0.26.0 (2024-10-16)

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/direct_rl_env.py

@@ -273,6 +273,10 @@ def reset(self, seed: int | None = None, options: dict[str, Any] | None = None)
         indices = torch.arange(self.num_envs, dtype=torch.int64, device=self.device)
         self._reset_idx(indices)
 
+        # update articulation kinematics
+        self.scene.write_data_to_sim()
+        self.sim.forward()
+
         # if sensors are added to the scene, make sure we render to reflect changes in reset
         if self.sim.has_rtx_sensors() and self.cfg.rerender_on_reset:
             self.sim.render()
@@ -346,6 +350,9 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn:
         reset_env_ids = self.reset_buf.nonzero(as_tuple=False).squeeze(-1)
         if len(reset_env_ids) > 0:
             self._reset_idx(reset_env_ids)
+            # update articulation kinematics
+            self.scene.write_data_to_sim()
+            self.sim.forward()
             # if sensors are added to the scene, make sure we render to reflect changes in reset
             if self.sim.has_rtx_sensors() and self.cfg.rerender_on_reset:
                 self.sim.render()

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/manager_based_env.py

@@ -253,6 +253,9 @@ def reset(self, seed: int | None = None, options: dict[str, Any] | None = None)
         indices = torch.arange(self.num_envs, dtype=torch.int64, device=self.device)
         self._reset_idx(indices)
 
+        # update articulation kinematics
+        self.scene.write_data_to_sim()
+        self.sim.forward()
         # if sensors are added to the scene, make sure we render to reflect changes in reset
         if self.sim.has_rtx_sensors() and self.cfg.rerender_on_reset:
             self.sim.render()

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/manager_based_rl_env.py

@@ -194,6 +194,9 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn:
         reset_env_ids = self.reset_buf.nonzero(as_tuple=False).squeeze(-1)
         if len(reset_env_ids) > 0:
             self._reset_idx(reset_env_ids)
+            # update articulation kinematics
+            self.scene.write_data_to_sim()
+            self.sim.forward()
             # if sensors are added to the scene, make sure we render to reflect changes in reset
             if self.sim.has_rtx_sensors() and self.cfg.rerender_on_reset:
                 self.sim.render()

source/extensions/omni.isaac.lab/omni/isaac/lab/sim/simulation_context.py

@@ -416,6 +416,14 @@ def get_setting(self, name: str) -> Any:
         """
         return self._settings.get(name)
 
+    def forward(self) -> None:
+        """Updates articulation kinematics and fabric for rendering."""
+        if self._fabric_iface is not None:
+            if self.physics_sim_view is not None and self.is_playing():
+                # Update the articulations' link's poses before rendering
+                self.physics_sim_view.update_articulations_kinematic()
+            self._update_fabric(0.0, 0.0)
+
     """
     Operations - Override (standalone)
     """
@@ -486,11 +494,7 @@ def render(self, mode: RenderMode | None = None):
                 self.set_setting("/app/player/playSimulations", True)
         else:
             # manually flush the fabric data to update Hydra textures
-            if self._fabric_iface is not None:
-                if self.physics_sim_view is not None and self.is_playing():
-                    # Update the articulations' link's poses before rendering
-                    self.physics_sim_view.update_articulations_kinematic()
-                self._update_fabric(0.0, 0.0)
+            self.forward()
             # render the simulation
             # note: we don't call super().render() anymore because they do above operation inside
             #  and we don't want to do it twice. We may remove it once we drop support for Isaac Sim 2022.2.

source/extensions/omni.isaac.lab/test/sensors/test_frame_transformer.py

@@ -595,9 +595,114 @@ def test_sensor_print(self):
 
         # Play the simulator
         self.sim.reset()
+
         # print info
         print(scene.sensors["frame_transformer"])
 
+    def test_frame_transformer_all_bodies_after_reset(self):
+        """Test transformation of all bodies w.r.t. base source frame after a reset is performed.
+
+        In this test, the source frame is the robot base.
+
+        The target_frames are all bodies in the robot, implemented using .* pattern.
+        """
+        # Spawn things into stage
+        scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False)
+        scene_cfg.frame_transformer = FrameTransformerCfg(
+            prim_path="{ENV_REGEX_NS}/Robot/base",
+            target_frames=[
+                FrameTransformerCfg.FrameCfg(
+                    prim_path="{ENV_REGEX_NS}/Robot/.*",
+                ),
+            ],
+        )
+        scene = InteractiveScene(scene_cfg)
+
+        # Play the simulator
+        self.sim.reset()
+
+        target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names
+        articulation_body_names = scene.articulations["robot"].data.body_names
+
+        reordering_indices = [target_frame_names.index(name) for name in articulation_body_names]
+
+        # default joint targets
+        default_actions = scene.articulations["robot"].data.default_joint_pos.clone()
+        # Define simulation stepping
+        sim_dt = self.sim.get_physics_dt()
+        # Simulate physics
+        for count in range(100):
+            # # reset
+            if count % 25 == 0:
+                # -- ground-truth before reset
+                pre_reset_root_pose_w = scene.articulations["robot"].data.root_state_w[:, :7].clone()
+                pre_reset_bodies_pos_w_gt = scene.articulations["robot"].data.body_pos_w.clone()
+                pre_reset_bodies_quat_w_gt = scene.articulations["robot"].data.body_quat_w.clone()
+
+                # reset root state
+                root_state = scene.articulations["robot"].data.default_root_state.clone()
+                root_state[:, :3] += scene.env_origins
+                root_state[:, 0] += 10
+                joint_pos = scene.articulations["robot"].data.default_joint_pos
+                joint_vel = scene.articulations["robot"].data.default_joint_vel
+
+                # -- set root state
+                # -- robot
+                scene.articulations["robot"].write_root_state_to_sim(root_state)
+                scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel)
+                # reset buffers
+                scene.reset()
+
+                # step kinematics
+                scene.write_data_to_sim()
+                self.sim.forward()
+
+                # -- frame transformer
+                source_pos_w_tf = scene.sensors["frame_transformer"].data.source_pos_w
+                source_quat_w_tf = scene.sensors["frame_transformer"].data.source_quat_w
+                bodies_pos_w_tf = scene.sensors["frame_transformer"].data.target_pos_w
+                bodies_quat_w_tf = scene.sensors["frame_transformer"].data.target_quat_w
+
+                # check absolute frame transforms in world frame
+                # -- ground-truth
+                root_pose_w = scene.articulations["robot"].data.root_state_w[:, :7]
+                bodies_pos_w_gt = scene.articulations["robot"].data.body_pos_w
+                bodies_quat_w_gt = scene.articulations["robot"].data.body_quat_w
+
+                # check if they are same
+                torch.testing.assert_close(root_pose_w[:, :3], source_pos_w_tf)
+                torch.testing.assert_close(root_pose_w[:, 3:], source_quat_w_tf)
+                torch.testing.assert_close(bodies_pos_w_gt, bodies_pos_w_tf[:, reordering_indices])
+                torch.testing.assert_close(bodies_quat_w_gt, bodies_quat_w_tf[:, reordering_indices])
+
+                # check if transforms were reset
+                self.assertFalse(torch.allclose(pre_reset_root_pose_w[:, :3], source_pos_w_tf))
+                self.assertFalse(torch.allclose(pre_reset_root_pose_w[:, 3:], source_quat_w_tf))
+                self.assertFalse(torch.allclose(pre_reset_bodies_pos_w_gt, bodies_pos_w_tf[:, reordering_indices]))
+                self.assertFalse(torch.allclose(pre_reset_bodies_quat_w_gt, bodies_quat_w_tf[:, reordering_indices]))
+
+                bodies_pos_source_tf = scene.sensors["frame_transformer"].data.target_pos_source
+                bodies_quat_source_tf = scene.sensors["frame_transformer"].data.target_quat_source
+
+                # Go through each body and check if relative transforms are same
+                for index in range(len(articulation_body_names)):
+                    body_pos_b, body_quat_b = math_utils.subtract_frame_transforms(
+                        root_pose_w[:, :3], root_pose_w[:, 3:], bodies_pos_w_tf[:, index], bodies_quat_w_tf[:, index]
+                    )
+
+                    torch.testing.assert_close(bodies_pos_source_tf[:, index], body_pos_b)
+                    torch.testing.assert_close(bodies_quat_source_tf[:, index], body_quat_b)
+
+            # set joint targets
+            robot_actions = default_actions + 0.5 * torch.randn_like(default_actions)
+            scene.articulations["robot"].set_joint_position_target(robot_actions)
+            # write data to sim
+            scene.write_data_to_sim()
+            # perform step
+            self.sim.step()
+            # read data from sim
+            scene.update(sim_dt)
+
 
 if __name__ == "__main__":
     run_tests()