双臂遥操多view

scripts/environments/teleoperation/teleop_xr_agent.py

@@ -74,6 +74,54 @@ from xr_utils import XrClient, transform_xr_pose, quat_diff_as_rotvec_xyzw, is_v
 from xr_utils.geometry import R_HEADSET_TO_WORLD
 
 
+# =====================================================================
+# Robot Camera Viewport Utilities
+# =====================================================================
+
+# Resolved prim paths for env_0 — must match CameraCfg prim_path with
+# {ENV_REGEX_NS} → /World/envs/env_0
+_ROBOT_CAM_PRIMS: dict[str, str] = {
+    "Left Hand":  "/World/envs/env_0/Robot/rm_65_fb_left/Link_6/robot___left_8_02/cam_left_hand",
+    "Right Hand": "/World/envs/env_0/Robot/rm_65_b_right/Link6/robot___right_8_2/cam_right_hand",
+    "Head":       "/World/envs/env_0/Robot/robot_head/cam_head",
+    "Chest":      "/World/envs/env_0/Robot/robot_trunk/cam_chest",
+}
+
+# Stores created viewport window objects
+_robot_viewports: dict[str, object] = {}
+
+
+def create_robot_viewports() -> None:
+    """Create 4 viewport windows and bind each to a robot camera prim.
+
+    Must be called after env.reset() so all prims exist on the USD stage.
+    """
+    try:
+        import omni.kit.viewport.utility as vp_util
+    except ImportError:
+        logger.warning("[Viewport] omni.kit.viewport.utility not available — skipping viewport creation.")
+        return
+
+    for name, cam_path in _ROBOT_CAM_PRIMS.items():
+        vp_win = vp_util.create_viewport_window(
+            f"Robot {name} View", width=640, height=360
+        )
+        vp_win.viewport_api.camera_path = cam_path
+        _robot_viewports[name] = vp_win
+        print(f"[INFO] Viewport 'Robot {name} View' bound to: {cam_path}")
+
+
+def rebind_robot_viewports() -> None:
+    """Re-bind all robot viewports to their camera paths.
+
+    Call this after every env reset so viewports stay locked to the cameras.
+    """
+    for name, vp_win in _robot_viewports.items():
+        cam_path = _ROBOT_CAM_PRIMS[name]
+        vp_win.viewport_api.camera_path = cam_path
+
+
+
 # =====================================================================
 # Helpers
 # =====================================================================
@@ -446,6 +494,11 @@ def main() -> None:
     obs, _ = env.reset()
     clear_ik_target_state()
     teleop_interface.reset()
+
+    # Create 4 viewport windows bound to the robot cameras
+    create_robot_viewports()
+
+
     if is_dual_arm:
         teleop_right_ref.reset()
 
@@ -476,6 +529,7 @@ def main() -> None:
                     teleop_interface.reset()
                     if is_dual_arm:
                         teleop_right_ref.reset()
+                    rebind_robot_viewports()
                     should_reset = False
                     sim_frame = 0
                     last_root_left = None