双臂头部遥操作控制

scripts/environments/teleoperation/teleop_xr_agent.py

@@ -83,7 +83,7 @@ from xr_utils.geometry import R_HEADSET_TO_WORLD
 _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",
+    "Head":       "/World/envs/env_0/Robot/robot_head/ZED_X/base_link/ZED_X/CameraLeft",
     "Chest":      "/World/envs/env_0/Robot/robot_trunk/cam_chest",
 }
 
@@ -339,6 +339,43 @@ class XrTeleopController:
         return action
 
 
+# =====================================================================
+# Head Tracking Utility
+# =====================================================================
+
+def get_head_joint_targets(
+    xr_client, neutral_rot: R | None
+) -> tuple[np.ndarray, R | None]:
+    """Extract head joint targets [yaw, pitch] from HMD pose.
+
+    At first call (neutral_rot is None) the current headset orientation
+    is recorded as the neutral reference and zeros are returned.
+    Subsequent calls return yaw/pitch relative to that neutral pose.
+
+    Returns:
+        (joint_targets, neutral_rot) where joint_targets is shape (2,)
+        float32 [head_yaw_Joint_rad, head_pitch_Joint_rad].
+    """
+    try:
+        raw_pose = xr_client.get_pose("headset")
+        if not is_valid_quaternion(raw_pose[3:]):
+            return np.zeros(2, dtype=np.float32), neutral_rot
+        _, head_world_quat_xyzw = transform_xr_pose(raw_pose[:3], raw_pose[3:])
+        head_rot = R.from_quat(head_world_quat_xyzw)
+        if neutral_rot is None:
+            # First call — record neutral and hold zero
+            return np.zeros(2, dtype=np.float32), head_rot
+        # Relative rotation from neutral heading
+        rel_rot = head_rot * neutral_rot.inv()
+        # ZYX Euler: [0]=yaw (Z), [1]=pitch (Y)
+        euler_zyx = rel_rot.as_euler("ZYX")
+        yaw   = float(np.clip(euler_zyx[0], -1.57, 1.57))   # ±90°
+        pitch = float(np.clip(euler_zyx[1], -0.52, 0.52))   # ±30°
+        return np.array([yaw, pitch], dtype=np.float32), neutral_rot
+    except Exception:
+        return np.zeros(2, dtype=np.float32), neutral_rot
+
+
 # =====================================================================
 # Main Execution Loop
 # =====================================================================
@@ -519,6 +556,7 @@ def main() -> None:
     clear_ik_target_state()
     last_root_left = None
     last_root_right = None
+    neutral_head_rot = None  # calibrated on first headset sample after reset
 
     while simulation_app.is_running():
         try:
@@ -534,11 +572,18 @@ def main() -> None:
                     sim_frame = 0
                     last_root_left = None
                     last_root_right = None
+                    neutral_head_rot = None  # re-calibrate on next headset sample
                     continue
 
                 policy_obs = obs["policy"]
                 wheel_np, v_fwd, omega_base = get_chassis_commands()
 
+                # Head tracking: HMD yaw/pitch relative to neutral pose
+                head_cmd_np, neutral_head_rot = get_head_joint_targets(
+                    teleop_interface.xr_client, neutral_head_rot
+                )
+                head_cmd = torch.tensor(head_cmd_np, dtype=torch.float32)
+
                 if is_dual_arm:
                     eef_pos_left = policy_obs["eef_pos_left"][0].cpu().numpy()
                     eef_quat_left = policy_obs["eef_quat_left"][0].cpu().numpy()
@@ -554,10 +599,11 @@ def main() -> None:
                     if teleop_right_ref.grip_active or last_root_right is None:
                         last_root_right = convert_action_world_to_root(right_action)[:7].clone()
 
-                    # Action layout: left_arm(7) | wheel(4) | left_gripper(1) | right_arm(7) | right_gripper(1)
+                    # Action layout: left_arm(7) | wheel(4) | left_gripper(1) | right_arm(7) | right_gripper(1) | head(2)
                     action_np = torch.cat([
                         last_root_left, wheel_np, left_action[7:8],
                         last_root_right, right_action[7:8],
+                        head_cmd,
                     ])
                 else:
                     eef_pos = policy_obs["eef_pos"][0].cpu().numpy()