Adds Teleop UI Feedback Visualization: Framework and Application

CONTRIBUTORS.md

@@ -87,6 +87,7 @@ Guidelines for modifications:
 * Kourosh Darvish
 * Kousheek Chakraborty
 * Lionel Gulich
+* Lotus Li
 * Louis Le Lay
 * Lorenz Wellhausen
 * Lukas Fröhlich

scripts/tools/record_demos.py

@@ -108,7 +108,8 @@
 
 import isaaclab_tasks  # noqa: F401
 from isaaclab_tasks.utils.parse_cfg import parse_env_cfg
-
+from isaaclab.ui.xr_widgets import XRVisualization
+from teleop_visualization_manager import TeleopVisualizationManager
 
 class RateLimiter:
     """Convenience class for enforcing rates in loops."""
@@ -469,16 +470,24 @@ def stop_recording_instance():
                 label_text = f"Recorded {current_recorded_demo_count} successful demonstrations."
                 print(label_text)
 
+            # Check if we've reached the desired number of demos
+            if args_cli.num_demos > 0 and env.recorder_manager.exported_successful_episode_count >= args_cli.num_demos:
+                label_text = f"All {current_recorded_demo_count} demonstrations recorded.\nExiting the app."
+                instruction_display.show_demo(label_text)
+                print(label_text)
+                target_time = time.time() + 0.8
+                while time.time() < target_time:
+                    if rate_limiter:
+                        rate_limiter.sleep(env)
+                    else:
+                        env.sim.render()
+                break
+
             # Handle reset if requested
             if should_reset_recording_instance:
                 success_step_count = handle_reset(env, success_step_count, instruction_display, label_text)
                 should_reset_recording_instance = False
 
-            # Check if we've reached the desired number of demos
-            if args_cli.num_demos > 0 and env.recorder_manager.exported_successful_episode_count >= args_cli.num_demos:
-                print(f"All {args_cli.num_demos} demonstrations recorded. Exiting the app.")
-                break
-
             # Check if simulation is stopped
             if env.sim.is_stopped():
                 break
@@ -512,6 +521,9 @@ def main() -> None:
     # Set up output directories
     output_dir, output_file_name = setup_output_directories()
 
+    # Assign the teleop visualization manager to the visualization system
+    XRVisualization.assign_manager(TeleopVisualizationManager)
+
     # Create and configure environment
     global env_cfg  # Make env_cfg available to setup_teleop_device
     env_cfg, success_term = create_environment_config(output_dir, output_file_name)

scripts/tools/test/scene_visualization_sample.py

@@ -0,0 +1,70 @@
+from isaaclab.ui.xr_widgets import XRVisualization, TriggerType, VisualizationManager, DataCollector,update_instruction
+from pxr import Gf
+
+def _sample_handle_ik_error(self, mgr: VisualizationManager, data_collector: DataCollector) -> None:
+    """Handle IK error events by displaying an error message widget.
+
+    Args:
+        data_collector: DataCollector instance (unused in this handler)
+    """
+
+    self.display_widget("IK Error Detected", "/ik_error", VisualizationManager.message_widget_preset() | {"text_color": self._error_text_color})
+
+def _sample_update_error_text_color(self, mgr: VisualizationManager, data_collector: DataCollector) -> None:
+    self._error_text_color = self._error_text_color + 0x100
+    if self._error_text_color >= 0xFFFFFFFF:
+        self._error_text_color = 0xFF0000FF
+
+def _sample_update_left_panel(self, mgr, data_collector) -> None:
+    """Update the left panel with current data and update counter.
+
+    Args:
+        data_collector: DataCollector instance containing current data
+    """
+    left_panel_id = getattr(self, '_left_panel_id', None)
+    if left_panel_id is not None:
+        content = f"{mgr._left_panel_updated_times}\n{data_collector.make_panel_content()}"
+        update_instruction(left_panel_id, content)
+        mgr._left_panel_updated_times += 1
+
+def _sample_update_right_panel(self, mgr, data_collector) -> None:
+    """Update the right panel with current data and update counter.
+
+    Args:
+        data_collector: DataCollector instance containing current data
+    """
+    right_panel_id = getattr(self, '_right_panel_id', None)
+    if right_panel_id is not None:
+        content = f"{mgr._right_panel_updated_times}\n{data_collector.make_panel_content()}"
+        update_instruction(right_panel_id, content)
+        mgr._right_panel_updated_times += 1
+
+def apply_sample_visualization():
+    # Error Message
+    XRVisualization.register_callback(TriggerType.TRIGGER_ON_EVENT, {"event_name": "ik_error"}, _sample_handle_ik_error)
+
+    # Display a panel on the left to display DataCollector data
+    # Refresh periodically
+    # Todo: use a better way to add '/' to pathname
+    XRVisualization.set_attrs({
+        "left_panel_id": "/left_panel",
+        "left_panel_translation": Gf.Vec3f(-2, 2.6, 2),
+        "left_panel_updated_times": 0,
+        "right_panel_updated_times": 0,
+    })
+    XRVisualization.register_callback(TriggerType.TRIGGER_ON_PERIOD, {"period": 1.0}, _sample_update_left_panel)
+
+    # Display a panel on the right to display DataCollector data
+    # Refresh when data changes
+    XRVisualization.set_attrs({
+        "right_panel_id": "/right_panel",
+        "right_panel_translation": Gf.Vec3f(1.5, 2, 2),
+    })
+    XRVisualization.register_callback(TriggerType.TRIGGER_ON_EVENT, {"event_name": "default_event_has_change"}, _sample_update_right_panel)
+
+    # Change error text color every second
+    XRVisualization.set_attrs({
+        "error_text_color": 0xFF0000FF,
+    })
+    XRVisualization.register_callback(TriggerType.TRIGGER_ON_UPDATE, {}, _sample_update_error_text_color)
+

source/isaaclab/isaaclab/devices/device_base.py

@@ -12,7 +12,7 @@
 from typing import Any
 
 from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg
-
+from isaaclab.ui.xr_widgets import XRVisualization
 
 @dataclass
 class DeviceCfg:
@@ -110,6 +110,8 @@ def advance(self) -> torch.Tensor:
         """
         raw_data = self._get_raw_data()
 
+        XRVisualization.push_data({"device_raw_data": raw_data})
+
         # If no retargeters, return raw data directly (not as a tuple)
         if not self._retargeters:
             return raw_data

source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py

@@ -12,12 +12,12 @@
 import isaaclab.utils.math as PoseUtils
 from isaaclab.devices import OpenXRDevice
 from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg
-from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg
 
 # This import exception is suppressed because gr1_t2_dex_retargeting_utils depends on pinocchio which is not available on windows
 with contextlib.suppress(Exception):
     from .gr1_t2_dex_retargeting_utils import GR1TR2DexRetargeting
 
+from isaaclab.ui.xr_widgets import XRVisualization
 
 @dataclass
 class GR1T2RetargeterCfg(RetargeterCfg):
@@ -48,24 +48,27 @@ def __init__(
             hand_joint_names: List of robot hand joint names
         """
 
+        XRVisualization.push_event("enable_teleop_visualization", cfg.enable_visualization)
+        XRVisualization.push_data({"sim_device": cfg.sim_device})
+        hand_torque_mapping = ["L_thumb_proximal_pitch_joint", "L_thumb_distal_joint", 
+                                "L_index_proximal_joint", "L_index_intermediate_joint", "L_index_intermediate_joint",
+                                "L_middle_proximal_joint", "L_middle_intermediate_joint", "L_middle_intermediate_joint",
+                                "L_ring_proximal_joint", "L_ring_intermediate_joint", "L_ring_intermediate_joint",
+                                "L_pinky_proximal_joint", "L_pinky_intermediate_joint", "L_pinky_intermediate_joint",
+                                "R_thumb_proximal_pitch_joint", "R_thumb_distal_joint", 
+                                "R_index_proximal_joint", "R_index_intermediate_joint", "R_index_intermediate_joint",
+                                "R_middle_proximal_joint", "R_middle_intermediate_joint", "R_middle_intermediate_joint",
+                                "R_ring_proximal_joint", "R_ring_intermediate_joint", "R_ring_intermediate_joint",
+                                "R_pinky_proximal_joint", "R_pinky_intermediate_joint", "R_pinky_intermediate_joint"]
+        XRVisualization.push_data({"hand_torque_mapping": hand_torque_mapping})
+
         self._hand_joint_names = cfg.hand_joint_names
         self._hands_controller = GR1TR2DexRetargeting(self._hand_joint_names)
 
         # Initialize visualization if enabled
         self._enable_visualization = cfg.enable_visualization
         self._num_open_xr_hand_joints = cfg.num_open_xr_hand_joints
         self._sim_device = cfg.sim_device
-        if self._enable_visualization:
-            marker_cfg = VisualizationMarkersCfg(
-                prim_path="/Visuals/markers",
-                markers={
-                    "joint": sim_utils.SphereCfg(
-                        radius=0.005,
-                        visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)),
-                    ),
-                },
-            )
-            self._markers = VisualizationMarkers(marker_cfg)
 
     def retarget(self, data: dict) -> torch.Tensor:
         """Convert hand joint poses to robot end-effector commands.
@@ -87,14 +90,6 @@ def retarget(self, data: dict) -> torch.Tensor:
         left_wrist = left_hand_poses.get("wrist")
         right_wrist = right_hand_poses.get("wrist")
 
-        if self._enable_visualization:
-            joints_position = np.zeros((self._num_open_xr_hand_joints, 3))
-
-            joints_position[::2] = np.array([pose[:3] for pose in left_hand_poses.values()])
-            joints_position[1::2] = np.array([pose[:3] for pose in right_hand_poses.values()])
-
-            self._markers.visualize(translations=torch.tensor(joints_position, device=self._sim_device))
-
         # Create array of zeros with length matching number of joint names
         left_hands_pos = self._hands_controller.compute_left(left_hand_poses)
         indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_left_joint_names()]

source/isaaclab/isaaclab/envs/manager_based_rl_env.py

@@ -17,7 +17,7 @@
 
 from isaaclab.managers import CommandManager, CurriculumManager, RewardManager, TerminationManager
 from isaaclab.ui.widgets import ManagerLiveVisualizer
-
+from isaaclab.ui.xr_widgets import XRVisualization
 from .common import VecEnvStepReturn
 from .manager_based_env import ManagerBasedEnv
 from .manager_based_rl_env_cfg import ManagerBasedRLEnvCfg
@@ -196,6 +196,14 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn:
             # update buffers at sim dt
             self.scene.update(dt=self.physics_dt)
 
+            # Todo: torque limit doesn't need updating every frame
+            # get joint torque limits
+            joints_torque_limit = self.scene["robot"].data.joint_effort_limits[0]
+            # get joint torque
+            joints_torque = self.scene["robot"].data.applied_torque[0]
+            joints_name = self.scene["robot"].data.joint_names
+            XRVisualization.push_data({"joints_torque": joints_torque, "joints_torque_limit": joints_torque_limit, "joints_name": joints_name})
+
         # post-step:
         # -- update env counters (used for curriculum generation)
         self.episode_length_buf += 1  # step in current episode (per env)

source/isaaclab/isaaclab/ui/xr_widgets/__init__.py

@@ -2,4 +2,5 @@
 # All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
-from .instruction_widget import SimpleTextWidget, show_instruction
+from .instruction_widget import SimpleTextWidget, show_instruction, hide_instruction, update_instruction
+from .scene_visualization import XRVisualization, TriggerType, DataCollector, VisualizationManager