From 36b4273582a3334876384af49caaece97914b5d7 Mon Sep 17 00:00:00 2001 From: yt lee Date: Fri, 6 Mar 2026 15:05:45 +0800 Subject: [PATCH] VR --- .../teleoperation/teleop_xr_agent.py | 55 +++++++++---------- 1 file changed, 26 insertions(+), 29 deletions(-) diff --git a/scripts/environments/teleoperation/teleop_xr_agent.py b/scripts/environments/teleoperation/teleop_xr_agent.py index 2071de0..fb44ed3 100644 --- a/scripts/environments/teleoperation/teleop_xr_agent.py +++ b/scripts/environments/teleoperation/teleop_xr_agent.py @@ -188,40 +188,37 @@ class XrTeleopController: self.grip_active = True return self.get_zero_action(trigger, current_eef_pos, current_eef_quat) - xr_delta_pos = raw_pose[:3] - self.prev_xr_pos - pos_norm = np.linalg.norm(xr_delta_pos) - - # PICO -> Isaac World mapping: - # XR +X (Right) -> World -Y - # XR +Y (Up) -> World +Z - # XR +Z (Back) -> World -X - raw_to_world_delta_pos = np.array([-xr_delta_pos[2], -xr_delta_pos[0], xr_delta_pos[1]]) - delta_pos = raw_to_world_delta_pos * self.pos_sensitivity + # Since OpenXR headset zero is not guaranteed to match robot zero, we compute the + # raw transformation in World Frame, but apply it relatively to the stored target. - # Rotation Delta - xr_delta_rot = quat_diff_as_rotvec_xyzw(self.prev_xr_quat, raw_pose[3:]) - rot_norm = np.linalg.norm(xr_delta_rot) - - raw_to_world_delta_rot = np.array([-xr_delta_rot[2], -xr_delta_rot[0], xr_delta_rot[1]]) - delta_rot = raw_to_world_delta_rot * self.rot_sensitivity + # 1. First, find current XR pose projected into World frame + xr_world_pos, xr_world_quat_xyzw = transform_xr_pose(raw_pose[:3], raw_pose[3:]) + prev_xr_world_pos, prev_xr_world_quat_xyzw = transform_xr_pose(self.prev_xr_pos, self.prev_xr_quat) - # Gripper + # 2. Extract Delta POS in World frame + world_delta_pos = (xr_world_pos - prev_xr_world_pos) * self.pos_sensitivity + pos_norm = np.linalg.norm((raw_pose[:3] - self.prev_xr_pos)) + + # 3. Extract Delta ROT in World frame + world_delta_rot = quat_diff_as_rotvec_xyzw(prev_xr_world_quat_xyzw, xr_world_quat_xyzw) * self.rot_sensitivity + rot_norm = np.linalg.norm(quat_diff_as_rotvec_xyzw(self.prev_xr_quat, raw_pose[3:])) + + # 4. Gripper gripper_action = 1.0 if trigger > 0.5 else -1.0 if self.is_absolute: - # Apply delta integrally to target pose if self.target_eef_pos is None: - # Failsafe if we don't have eef_pos injected yet self.target_eef_pos = np.zeros(3) self.target_eef_quat = np.array([1.0, 0.0, 0.0, 0.0]) - self.target_eef_pos += delta_pos + # Position update (simple translation in world frame) + self.target_eef_pos += world_delta_pos - # Multiply rotation. Target quat is wxyz. Delta rot is rotvec in world frame. - # scipy Rotation uses xyzw ! So convert wxyz -> xyzw, multiply, then -> wxyz + # Rotation update: apply delta_R (in world frame) to target_R (in world frame) + # R_new = R_delta @ R_target target_r = R.from_quat([self.target_eef_quat[1], self.target_eef_quat[2], self.target_eef_quat[3], self.target_eef_quat[0]]) - delta_r = R.from_rotvec(delta_rot) - new_r = delta_r * target_r # Apply delta rotation in global space + delta_r = R.from_rotvec(world_delta_rot) + new_r = delta_r * target_r new_rot_xyzw = new_r.as_quat() self.target_eef_quat = np.array([new_rot_xyzw[3], new_rot_xyzw[0], new_rot_xyzw[1], new_rot_xyzw[2]]) @@ -237,15 +234,15 @@ class XrTeleopController: # Relative clamping limits (used by relative mode to avoid divergence) max_pos_delta = 0.04 if pos_norm > max_pos_delta: - delta_pos = (raw_to_world_delta_pos * max_pos_delta / pos_norm) * self.pos_sensitivity + world_delta_pos = (world_delta_pos * max_pos_delta / pos_norm) max_rot_delta = 0.02 if rot_norm > max_rot_delta: - delta_rot = (raw_to_world_delta_rot * max_rot_delta / rot_norm) * self.rot_sensitivity + world_delta_rot = (world_delta_rot * max_rot_delta / rot_norm) action = torch.tensor([ - delta_pos[0], delta_pos[1], delta_pos[2], - delta_rot[0], delta_rot[1], delta_rot[2], + world_delta_pos[0], world_delta_pos[1], world_delta_pos[2], + world_delta_rot[0], world_delta_rot[1], world_delta_rot[2], gripper_action], dtype=torch.float32) self.prev_xr_pos = raw_pose[:3].copy() @@ -259,8 +256,8 @@ class XrTeleopController: print(f"| Task Mode: {'ABSOLUTE' if self.is_absolute else 'RELATIVE'}") print(f"| Raw VR Pos (OpenXR): {np.array(raw_pose[:3])}") print(f"| Raw VR Quat (xyzw): {np.array(raw_pose[3:])}") - print(f"| XR Delta Pos (raw): {xr_delta_pos} (norm={pos_norm:.4f})") - print(f"| XR Delta Rot (raw): {xr_delta_rot} (norm={rot_norm:.4f})") + print(f"| XR Delta Pos (world): {world_delta_pos} (norm={pos_norm:.4f})") + print(f"| XR Delta Rot (world): {world_delta_rot} (norm={rot_norm:.4f})") if self.is_absolute: print(f"| Targ Pos (dx,dy,dz): {action[:3].numpy()}") print(f"| Targ Quat (wxyz): {action[3:7].numpy()}")