diff --git a/packages/pyzed-5.3-cp311-cp311-linux_x86_64.whl b/packages/pyzed-5.3-cp311-cp311-linux_x86_64.whl new file mode 100644 index 0000000..fea441c Binary files /dev/null and b/packages/pyzed-5.3-cp311-cp311-linux_x86_64.whl differ diff --git a/pyproject.toml b/pyproject.toml index d92e15c..947cd1a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -32,6 +32,7 @@ dependencies = [ "onnxscript>=0.6.2", "chiral", "pyyaml>=6.0.3", + "oculus-reader", ] [project.optional-dependencies] @@ -54,9 +55,10 @@ rd = "raiden.cli:main" [tool.uv.sources] i2rt = { path = "third_party/i2rt" } -pyzed = { path = "packages/pyzed-5.2-cp311-cp311-linux_x86_64.whl" } +pyzed = { path = "packages/pyzed-5.3-cp311-cp311-linux_x86_64.whl" } pyroki = { git = "https://github.com/chungmin99/pyroki.git" } chiral = { git = "ssh://git@github.com/TRI-ML/chiral" } +oculus-reader = { git = "https://github.com/rail-berkeley/oculus_reader.git" } [tool.ruff] exclude = ["third_party/"] diff --git a/raiden/_xml_paths.py b/raiden/_xml_paths.py index 20ef931..a53b12f 100644 --- a/raiden/_xml_paths.py +++ b/raiden/_xml_paths.py @@ -2,14 +2,10 @@ import functools -from i2rt.robots.utils import ( - ARM_YAM_XML_PATH, - GRIPPER_LINEAR_4310_PATH, - combine_arm_and_gripper_xml, -) +from i2rt.robots.utils import ArmType, GripperType, combine_arm_and_gripper_xml @functools.lru_cache(maxsize=1) def get_yam_4310_linear_xml_path() -> str: """Return path to a combined YAM arm + linear-4310 gripper XML (written to /tmp/).""" - return combine_arm_and_gripper_xml(ARM_YAM_XML_PATH, GRIPPER_LINEAR_4310_PATH) + return combine_arm_and_gripper_xml(ArmType.YAM, GripperType.LINEAR_4310) diff --git a/raiden/cameras/zed.py b/raiden/cameras/zed.py index 8649a4d..1d065ea 100644 --- a/raiden/cameras/zed.py +++ b/raiden/cameras/zed.py @@ -3,7 +3,7 @@ Recording mode : opens by serial number, records to SVO2 (H264). Depth is NOT computed during recording (depth_mode=NONE) for lower CPU usage; the raw stereo pair is stored instead. -Playback mode : opens an SVO2 file, computes depth (NEURAL) for each frame. +Playback mode : opens an SVO2 file, computes depth (NEURAL_LIGHT) for each frame. Use ZedCamera.from_svo() to create a playback instance. """ diff --git a/raiden/cli.py b/raiden/cli.py index b091795..3b270f4 100644 --- a/raiden/cli.py +++ b/raiden/cli.py @@ -31,8 +31,8 @@ class TeleopCommand: """Start bimanual teleoperation with improved synchronization""" - control: Literal["leader", "spacemouse"] = "leader" - """Control mode: leader-follower arms or SpaceMouse EE velocity control""" + control: Literal["leader", "spacemouse", "oculus"] = "leader" + """Control mode: leader-follower arms, SpaceMouse EE velocity, or Oculus Touch""" arms: Literal["bimanual", "single"] = "bimanual" """Which arms to use: both (bimanual) or left arm only (single)""" @@ -55,13 +55,31 @@ class TeleopCommand: invert_rotation: bool = False """Negate all SpaceMouse rotation axes (spacemouse mode only)""" + oculus_right_controller: bool = True + """Use right Touch controller to drive the arm (oculus mode only)""" + + oculus_spatial_coeff: float = 1.0 + """Scale applied to VR position delta before feeding into IK (oculus mode only)""" + + oculus_pos_action_gain: float = 1.0 + """Position multiplier: 1.0 = 1:1 tracking, 2.0 = 2x sensitivity (oculus mode only)""" + + oculus_rot_action_gain: float = 1.0 + """Rotation multiplier applied to relative VR rotation (oculus mode only)""" + + oculus_rmat_reorder: List[int] = field(default_factory=lambda: [-2, -1, -3, 4]) + """Axis permutation/flip vector mapping VR frame to robot frame (oculus mode only)""" + + follower_home_pos: Optional[List[float]] = None + """7-DOF home position [j1..j6, gripper] the arm parks at on exit (all modes)""" + @dataclass class RecordCommand: """Record a demonstration with cameras and robot data""" - control: Literal["leader", "spacemouse"] = "leader" - """Control mode: leader-follower arms or SpaceMouse EE velocity control""" + control: Literal["leader", "spacemouse", "oculus"] = "leader" + """Control mode: leader-follower arms, SpaceMouse EE velocity, or Oculus Touch""" data_dir: str = "data" """Root data directory (default: ./data); episodes go to /raw//""" @@ -90,6 +108,24 @@ class RecordCommand: arms: Literal["bimanual", "single"] = "bimanual" """Which arms to use: both (bimanual) or left arm only (single)""" + oculus_right_controller: bool = True + """Use right Touch controller to drive the arm (oculus mode only)""" + + oculus_spatial_coeff: float = 1.0 + """Scale applied to VR position delta before feeding into IK (oculus mode only)""" + + oculus_pos_action_gain: float = 1.0 + """Position multiplier: 1.0 = 1:1 tracking, 2.0 = 2x sensitivity (oculus mode only)""" + + oculus_rot_action_gain: float = 1.0 + """Rotation multiplier applied to relative VR rotation (oculus mode only)""" + + oculus_rmat_reorder: List[int] = field(default_factory=lambda: [-2, -1, -3, 4]) + """Axis permutation/flip vector mapping VR frame to robot frame (oculus mode only)""" + + follower_home_pos: Optional[List[float]] = None + """7-DOF home position [j1..j6, gripper] the arm parks at on exit (all modes)""" + _CAN_BITRATE = 1000000 @@ -455,6 +491,12 @@ def main(): rot_scale=command.rot_scale, invert_rotation=command.invert_rotation, arms=command.arms, + oculus_right_controller=command.oculus_right_controller, + oculus_spatial_coeff=command.oculus_spatial_coeff, + oculus_pos_action_gain=command.oculus_pos_action_gain, + oculus_rot_action_gain=command.oculus_rot_action_gain, + oculus_rmat_reorder=command.oculus_rmat_reorder, + follower_home_pos=command.follower_home_pos, ) # teleop builds its own interface internally via build_interface() elif subcommand == "record": @@ -478,9 +520,15 @@ def main(): vel_scale=command.vel_scale, rot_scale=command.rot_scale, invert_rotation=command.invert_rotation, + oculus_right_controller=command.oculus_right_controller, + oculus_spatial_coeff=command.oculus_spatial_coeff, + oculus_pos_action_gain=command.oculus_pos_action_gain, + oculus_rot_action_gain=command.oculus_rot_action_gain, + oculus_rmat_reorder=command.oculus_rmat_reorder, ), arms=command.arms, data_dir=command.data_dir, + follower_home_pos=command.follower_home_pos, ) elif subcommand == "replay": diff --git a/raiden/control/__init__.py b/raiden/control/__init__.py index 5837f95..6614bea 100644 --- a/raiden/control/__init__.py +++ b/raiden/control/__init__.py @@ -1,8 +1,9 @@ from raiden.control.base import TeleopInterface +from raiden.control.oculus import OculusInterface from raiden.control.spacemouse import SpaceMouseInterface from raiden.control.yam import YAMInterface -__all__ = ["TeleopInterface", "YAMInterface", "SpaceMouseInterface", "build_interface"] +__all__ = ["TeleopInterface", "YAMInterface", "SpaceMouseInterface", "OculusInterface", "build_interface"] def build_interface( @@ -12,6 +13,11 @@ def build_interface( vel_scale: float = 0.07, rot_scale: float = 0.8, invert_rotation: bool = False, + oculus_right_controller: bool = True, + oculus_spatial_coeff: float = 1.0, + oculus_pos_action_gain: float = 1.0, + oculus_rot_action_gain: float = 1.0, + oculus_rmat_reorder: list | None = None, ) -> TeleopInterface: """Construct the right TeleopInterface from CLI-style arguments.""" if control == "spacemouse": @@ -22,4 +28,12 @@ def build_interface( rot_scale=rot_scale, invert_rotation=invert_rotation, ) + if control == "oculus": + return OculusInterface( + right_controller=oculus_right_controller, + spatial_coeff=oculus_spatial_coeff, + pos_action_gain=oculus_pos_action_gain, + rot_action_gain=oculus_rot_action_gain, + rmat_reorder=oculus_rmat_reorder, + ) return YAMInterface() diff --git a/raiden/control/oculus.py b/raiden/control/oculus.py new file mode 100644 index 0000000..784d35f --- /dev/null +++ b/raiden/control/oculus.py @@ -0,0 +1,208 @@ +"""Oculus Quest Touch controller absolute-pose teleoperation.""" + +import threading +import time + +from raiden.control.base import TeleopInterface +from raiden.robot.footpedal import ( + PEDAL_LEFT, + PEDAL_MIDDLE, + PEDAL_RIGHT, + try_open_footpedal, +) + + +class OculusInterface(TeleopInterface): + """EE absolute-pose control via Oculus Quest Touch controllers. + + Movement is enabled only while the grip trigger (RG) is held (deadman switch). + Press joystick (RJ) to zero the controller's current orientation as the + robot's forward direction before grabbing. + + A button: start episode / mark trigger + B button: mark failure + Right index trigger: gripper (0=open, 1=closed; inverted from trigger) + """ + + def __init__( + self, + right_controller: bool = True, + spatial_coeff: float = 1.0, + pos_action_gain: float = 1.0, + rot_action_gain: float = 1.0, + rmat_reorder: list | None = None, + ): + self._right_controller = right_controller + self._spatial_coeff = spatial_coeff + self._pos_action_gain = pos_action_gain + self._rot_action_gain = rot_action_gain + self._rmat_reorder = rmat_reorder if rmat_reorder is not None else [-2, -1, -3, 4] + + @property + def name(self) -> str: + return "oculus" + + # ------------------------------------------------------------------ + # Session-level lifecycle + # ------------------------------------------------------------------ + + def open(self) -> None: + from oculus_reader.reader import OculusReader + + self._btn_a = threading.Event() + self._btn_b = threading.Event() + self._pedal_trigger = threading.Event() + self._pedal_success = threading.Event() + self._pedal_failure = threading.Event() + self._btn_shutdown = threading.Event() + + self._oculus_reader = OculusReader() + time.sleep(0.5) # allow ADB connection to settle + + self._btn_poll_thread = threading.Thread( + target=self._button_poll_loop, name="oculus-btn-poll", daemon=True + ) + self._btn_poll_thread.start() + + self._footpedal = try_open_footpedal() + if self._footpedal is not None: + + def _cb(code: int) -> None: + if code == PEDAL_LEFT: + rc = getattr(self, "_recording_controller", None) + if rc is not None: + rc.soft_pause() + else: + self._pedal_trigger.set() + elif code == PEDAL_MIDDLE: + self._pedal_success.set() + elif code == PEDAL_RIGHT: + self._pedal_failure.set() + + self._footpedal.on_press(_cb) + self._footpedal.start() + print( + " ✓ FootPedal ready: left=trigger/pause middle=success right=failure" + ) + + print(" ✓ OculusReader ready (A=trigger B=failure RG=grip RJ=zero orientation)") + + def close(self) -> None: + if hasattr(self, "_btn_shutdown"): + self._btn_shutdown.set() + if getattr(self, "_footpedal", None) is not None: + self._footpedal.close() + self._footpedal = None + + def _button_poll_loop(self) -> None: + """20 Hz rising-edge detection for A/B buttons.""" + prev_a = False + prev_b = False + interval = 1.0 / 20.0 + while not self._btn_shutdown.is_set(): + t0 = time.monotonic() + try: + _, buttons = self._oculus_reader.get_transformations_and_buttons() + a = bool(buttons.get("A", False)) + b = bool(buttons.get("B", False)) + if a and not prev_a: + self._btn_a.set() + if b and not prev_b: + self._btn_b.set() + prev_a = a + prev_b = b + except Exception: + pass + elapsed = time.monotonic() - t0 + remaining = interval - elapsed + if remaining > 0: + time.sleep(remaining) + + # ------------------------------------------------------------------ + # Episode-level lifecycle + # ------------------------------------------------------------------ + + def setup(self, robot_controller) -> None: + robot_controller.warmup_spacemouse_ik() + + def start(self, robot_controller) -> None: + # Clear any button events that accumulated during setup/IK warmup + # to prevent a spurious A-press from immediately exiting the loop. + self._btn_a.clear() + self._btn_b.clear() + for ev_name in ("_pedal_trigger", "_pedal_success", "_pedal_failure"): + ev = getattr(self, ev_name, None) + if ev is not None: + ev.clear() + robot_controller.start_oculus_teleop( + oculus_reader=self._oculus_reader, + right_controller=self._right_controller, + spatial_coeff=self._spatial_coeff, + pos_action_gain=self._pos_action_gain, + rot_action_gain=self._rot_action_gain, + rmat_reorder=self._rmat_reorder, + ) + + def stop(self, robot_controller) -> None: + robot_controller.stop_oculus_teleop() + + # ------------------------------------------------------------------ + # Polling + # ------------------------------------------------------------------ + + def poll(self, robot_controller) -> bool: + if self._btn_a.is_set(): + self._btn_a.clear() + return True + trigger = getattr(self, "_pedal_trigger", None) + if trigger is not None and trigger.is_set(): + trigger.clear() + return True + return False + + def poll_success(self, robot_controller) -> bool: + if self._btn_a.is_set(): + self._btn_a.clear() + return True + ev = getattr(self, "_pedal_success", None) + if ev is not None and ev.is_set(): + ev.clear() + return True + return False + + def poll_failure(self, robot_controller) -> bool: + if self._btn_b.is_set(): + self._btn_b.clear() + return True + ev = getattr(self, "_pedal_failure", None) + if ev is not None and ev.is_set(): + ev.clear() + return True + return False + + @property + def waits_for_button_start(self) -> bool: + return True + + @property + def start_hint(self) -> str: + return "Press A button on the Oculus controller to START (then hold grip to move)." + + @property + def supports_verdict_button(self) -> bool: + return True + + @property + def banner(self) -> str: + return ( + "\n" + "=" * 60 + "\n" + " OCULUS TOUCH TELEOPERATION ACTIVE\n" + "=" * 60 + "\n\n" + " RJ (joystick click): zero controller orientation\n" + " RG (grip trigger): enable movement (deadman)\n" + " RT (index trigger): gripper (squeeze=close)\n" + " A button: start episode / mark success\n" + " B button: mark failure\n\n" + " Tip: point controller in desired forward direction,\n" + " click RJ, then grab RG to start moving.\n\n" + " Press Ctrl+C to stop\n\n" + "=" * 60 + "\n" + ) diff --git a/raiden/recorder.py b/raiden/recorder.py index 5d1cc25..ec15038 100644 --- a/raiden/recorder.py +++ b/raiden/recorder.py @@ -681,6 +681,7 @@ def run_recording( calibration_file: str = CALIBRATION_FILE, arms: str = "bimanual", data_dir: str = "data", + follower_home_pos: Optional[list] = None, ) -> None: """Run teleoperation with continuous demonstration recording. @@ -749,6 +750,11 @@ def _copy_calibration(dest_dir: Path) -> None: def emergency_stop(signum, frame): if _active_ctrl[0] is not None: _active_ctrl[0].emergency_stop() + else: + # No active controller (between episodes) — exit immediately. + print("\nCancelled.") + import os as _os + _os._exit(0) signal.signal(signal.SIGTERM, emergency_stop) signal.signal(signal.SIGINT, emergency_stop) @@ -764,6 +770,8 @@ def emergency_stop(signum, frame): use_right = arms == "bimanual" use_left = True + _home_pos_arr = np.array(follower_home_pos) if follower_home_pos is not None else None + try: while True: recorder = None @@ -774,6 +782,7 @@ def emergency_stop(signum, frame): use_left_leader=interface.uses_leaders and use_left, use_right_follower=use_right, use_left_follower=use_left, + follower_home_pos=_home_pos_arr, ) _active_ctrl[0] = robot_controller @@ -797,7 +806,11 @@ def emergency_stop(signum, frame): print("=" * 60) print(f"\n Data dir : {task_dir}") if interface.waits_for_button_start: - print("\n Press button on any leader arm or left pedal to START.") + start_hint = getattr(interface, "start_hint", None) + if start_hint: + print(f"\n {start_hint}") + else: + print("\n Press button on any leader arm or left pedal to START.") else: print("\n Press Enter or left pedal to START recording.") print(" Press 'q' to end session.\n") diff --git a/raiden/robot/controller.py b/raiden/robot/controller.py index 633b30a..b39f49e 100644 --- a/raiden/robot/controller.py +++ b/raiden/robot/controller.py @@ -58,6 +58,15 @@ def _patched_can_interface_init(self, channel="PCAN_USBBUS1", *args, **kwargs): "LEADER_HOME_POS", ] +# CAN interface name mapping — override here if your interfaces have different names +# (e.g. "can0" before udev rename rules are set up) +_CAN_NAMES = { + "follower_r": "can1", + "follower_l": "can0", + "leader_r": "can_leader_r", + "leader_l": "can_leader_l", +} + # Default home positions FOLLOWER_HOME_POS = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) # 6 joints + gripper LEADER_HOME_POS = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # 6 joints only @@ -313,6 +322,15 @@ def check_can_interface(interface: str) -> bool: return False +def _vec_to_reorder_mat(vec: list) -> np.ndarray: + """Axis permutation/flip matrix from signed-index vector (from droid VRPolicy).""" + X = np.zeros((len(vec), len(vec))) + for i in range(len(vec)): + ind = int(abs(vec[i])) - 1 + X[i, ind] = np.sign(vec[i]) + return X + + def spacemouse_to_target_pose( state, T_current: np.ndarray, @@ -366,6 +384,7 @@ def __init__( use_left_leader: bool = True, use_right_follower: bool = True, use_left_follower: bool = True, + follower_home_pos: Optional[np.ndarray] = None, ): """Initialize robot controller @@ -374,7 +393,13 @@ def __init__( use_left_leader: Initialize left leader arm use_right_follower: Initialize right follower arm use_left_follower: Initialize left follower arm + follower_home_pos: 7-DOF home position [j1..j6, gripper] the arm returns + to at the end of each episode. Defaults to all-zeros + gripper open. + Set this to a safe resting pose for brakeless arms. """ + self._follower_home_pos = ( + follower_home_pos if follower_home_pos is not None else FOLLOWER_HOME_POS.copy() + ) self.use_right_leader = use_right_leader self.use_left_leader = use_left_leader self.use_right_follower = use_right_follower @@ -424,13 +449,13 @@ def check_can_interfaces(self) -> bool: required_interfaces = [] if self.use_right_follower: - required_interfaces.append("can_follower_r") + required_interfaces.append(_CAN_NAMES["follower_r"]) if self.use_left_follower: - required_interfaces.append("can_follower_l") + required_interfaces.append(_CAN_NAMES["follower_l"]) if self.use_right_leader: - required_interfaces.append("can_leader_r") + required_interfaces.append(_CAN_NAMES["leader_r"]) if self.use_left_leader: - required_interfaces.append("can_leader_l") + required_interfaces.append(_CAN_NAMES["leader_l"]) missing_interfaces = [] for interface in required_interfaces: @@ -474,7 +499,7 @@ def _init(name: str, channel: str, gripper_type) -> None: threads.append( threading.Thread( target=_init, - args=("right follower", "can_follower_r", GripperType.LINEAR_4310), + args=("right follower", _CAN_NAMES["follower_r"], GripperType.LINEAR_4310), daemon=True, ) ) @@ -482,7 +507,7 @@ def _init(name: str, channel: str, gripper_type) -> None: threads.append( threading.Thread( target=_init, - args=("left follower", "can_follower_l", GripperType.LINEAR_4310), + args=("left follower", _CAN_NAMES["follower_l"], GripperType.LINEAR_4310), daemon=True, ) ) @@ -492,7 +517,7 @@ def _init(name: str, channel: str, gripper_type) -> None: target=_init, args=( "right leader", - "can_leader_r", + _CAN_NAMES["leader_r"], GripperType.YAM_TEACHING_HANDLE, ), daemon=True, @@ -504,7 +529,7 @@ def _init(name: str, channel: str, gripper_type) -> None: target=_init, args=( "left leader", - "can_leader_l", + _CAN_NAMES["leader_l"], GripperType.YAM_TEACHING_HANDLE, ), daemon=True, @@ -620,14 +645,14 @@ def move_to_home(robot, target_pos, name): threads.append( threading.Thread( target=move_to_home, - args=(self.follower_r, FOLLOWER_HOME_POS, "right_follower"), + args=(self.follower_r, self._follower_home_pos, "right_follower"), ) ) if self.follower_l: threads.append( threading.Thread( target=move_to_home, - args=(self.follower_l, FOLLOWER_HOME_POS, "left_follower"), + args=(self.follower_l, self._follower_home_pos, "left_follower"), ) ) if self.leader_r: @@ -653,10 +678,10 @@ def move_to_home(robot, target_pos, name): # Sequential movement if self.follower_r: print(" - Moving right follower...") - smooth_move_joints(self.follower_r, FOLLOWER_HOME_POS) + smooth_move_joints(self.follower_r, self._follower_home_pos) if self.follower_l: print(" - Moving left follower...") - smooth_move_joints(self.follower_l, FOLLOWER_HOME_POS) + smooth_move_joints(self.follower_l, self._follower_home_pos) if self.leader_r: print(" - Moving right leader...") smooth_move_joints(self.leader_r._robot, LEADER_HOME_POS) @@ -962,7 +987,7 @@ def start_teleoperation(self): print("✓ Teleoperation active (followers will follow leaders)") def stop_teleoperation(self): - """Stop all teleoperation control loops (leader-follower and SpaceMouse).""" + """Stop all teleoperation control loops (leader-follower, SpaceMouse, Oculus).""" if hasattr(self, "_teleop_shutdown"): self._teleop_shutdown.set() if hasattr(self, "_teleop_threads"): @@ -970,6 +995,7 @@ def stop_teleoperation(self): thread.join(timeout=1.0) self._teleop_threads.clear() self.stop_spacemouse_teleop() + self.stop_oculus_teleop() # ------------------------------------------------------------------------- # SpaceMouse Cartesian velocity teleop @@ -1008,6 +1034,8 @@ def warmup_spacemouse_ik(self, dt: float = 0.01) -> None: Stores results in ``self._prewarmed_ik`` keyed by side ("right"/"left"). """ + if getattr(self, "_prewarmed_ik", {}): + return # already warmed up by setup_for_teleop_recording self._prewarmed_ik: dict = {} errors: dict = {} lock = threading.Lock() @@ -1257,18 +1285,279 @@ def _ik_step(q: np.ndarray, T_target_tcp: np.ndarray) -> np.ndarray: print(f" SpaceMouse {side} loop error: {e}") time.sleep(dt) + # ------------------------------------------------------------------------- + # Oculus Quest Touch controller teleop + # ------------------------------------------------------------------------- + + def start_oculus_teleop( + self, + oculus_reader, + right_controller: bool = True, + spatial_coeff: float = 1.0, + pos_action_gain: float = 1.0, + rot_action_gain: float = 1.0, + rmat_reorder: Optional[list] = None, + dt: float = 0.02, + ) -> None: + """Start Oculus Quest Touch controller Cartesian teleop thread. + + Uses absolute-pose origin tracking: when the grip trigger is first pressed, + records the VR pose and robot EE pose as the origin pair. Each subsequent + frame tracks the controller pose relative to that origin. + + Args: + oculus_reader: OculusReader instance (already started). + right_controller: If True, use right Touch controller; else left. + spatial_coeff: Scale applied to VR translation (metres). + pos_action_gain: Position multiplier — 1.0 = 1:1 mapping, 2.0 = 2x amplification. + rot_action_gain: Rotation multiplier — 1.0 = 1:1 mapping. + rmat_reorder: Axis permutation/flip, e.g. [-2, -1, -3, 4]. + dt: Control loop period in seconds (default 50 Hz). + """ + if rmat_reorder is None: + rmat_reorder = [-2, -1, -3, 4] + + if self.follower_l is None and self.follower_r is None: + raise RuntimeError("start_oculus_teleop() requires at least one follower arm.") + + global_to_env_mat = _vec_to_reorder_mat(rmat_reorder) + + self._oculus_shutdown = threading.Event() + self._oculus_threads: list = [] + + # Single-arm: right controller → left arm. + # Bimanual: right controller → left arm, left controller → right arm. + arms = [] + if self.follower_l is not None: + arms.append(("left", self.follower_l, "r", "oculus-left")) + if self.follower_r is not None: + arms.append(("right", self.follower_r, "l", "oculus-right")) + + for side, follower, ctrl_id, thread_name in arms: + t = threading.Thread( + target=self._oculus_control_loop, + args=(side, follower, oculus_reader, ctrl_id, + spatial_coeff, pos_action_gain, rot_action_gain, + global_to_env_mat, dt), + name=thread_name, + daemon=True, + ) + t.start() + self._oculus_threads.append(t) + print(f"✓ Oculus teleop active ({len(arms)} arm(s))") + + def stop_oculus_teleop(self) -> None: + """Stop Oculus teleop thread.""" + if hasattr(self, "_oculus_shutdown"): + self._oculus_shutdown.set() + if hasattr(self, "_oculus_threads"): + for t in self._oculus_threads: + t.join(timeout=1.0) + self._oculus_threads.clear() + + def _oculus_control_loop( + self, + side: str, + follower, + oculus_reader, + controller_id: str, + spatial_coeff: float, + pos_action_gain: float, + rot_action_gain: float, + global_to_env_mat: np.ndarray, + dt: float, + ) -> None: + """Absolute-pose origin-tracking + J-PARSE IK loop for one arm. + + Coordinate frame pipeline (mirrors droid VRPolicy): + vr_mat = global_to_env_mat @ vr_to_global_mat @ raw_pose + + Grip trigger (RG/LG) is a deadman — robot only moves while grip is held. + On grip press: VR pose and robot EE pose are latched as the origin pair. + On release: robot holds last commanded position with full PD stiffness. + + Joystick button (RJ/LJ): re-enables one-shot orientation zeroing + (vr_to_global_mat = inv(raw_pose)) so the user can redefine "forward". + + Right/left index trigger controls the gripper: 0 = open, 1 = closed. + """ + prewarmed = getattr(self, "_prewarmed_ik", {}).get(side) + if prewarmed is not None: + pk_robot, link6_idx, step_jit = prewarmed + else: + print(f" [Oculus {side}] Setting up J-PARSE IK (JIT warmup)...", flush=True) + pk_robot, link6_idx, step_jit = _setup_pyroki(dt) + print(f" [Oculus {side}] IK ready.", flush=True) + + _home_cfg = np.zeros(6, dtype=np.float64) + + def _fk_tcp(q: np.ndarray) -> np.ndarray: + poses = pk_robot.forward_kinematics(jnp.asarray(q)) + T_link6 = np.array(jaxlie.SE3(poses[link6_idx]).as_matrix()) + return T_link6 @ _T_LINK6_TO_TCP + + def _ik_step(q: np.ndarray, T_target_tcp: np.ndarray) -> np.ndarray: + T_target_link6 = T_target_tcp @ _T_TCP_TO_LINK6 + target_pos = T_target_link6[:3, 3] + xyzw = Rotation.from_matrix(T_target_link6[:3, :3]).as_quat() + target_wxyz = np.array([xyzw[3], xyzw[0], xyzw[1], xyzw[2]]) + q_new, _ = step_jit( + robot=pk_robot, + cfg=q.astype(np.float64), + target_link_index=link6_idx, + target_position=target_pos, + target_wxyz=target_wxyz, + method="jparse", + dt=dt, + home_cfg=_home_cfg, + ) + return np.asarray(q_new) + + # Seed virtual arm state from real robot (MuJoCo→pyroki order). + q_full_init = follower.get_joint_pos() + q_arm = q_full_init[:6][::-1].copy() + gripper = q_full_init[6] + + hold_pos: Optional[np.ndarray] = None + was_paused = False + + grip_key = controller_id.upper() + "G" + joystick_key = controller_id.upper() + "J" + trig_key = "rightTrig" if controller_id == "r" else "leftTrig" + + vr_to_global_mat: np.ndarray = np.eye(4) + reset_orientation: bool = True # continuously update until first grip press + vr_origin_mat: Optional[np.ndarray] = None + raw_pose_origin: Optional[np.ndarray] = None + robot_origin_T: Optional[np.ndarray] = None + prev_grip: bool = False + + while not self._oculus_shutdown.is_set(): + try: + loop_start = time.monotonic() + + # --- soft pause (footpedal e-stop) --- + now_paused = time.monotonic() < self._pause_until + if now_paused: + if not was_paused: + hold_pos = follower.get_joint_pos().copy() + was_paused = True + follower.command_joint_pos(hold_pos) + time.sleep(dt) + continue + + if self._session_estop_event.is_set(): + break + + if was_paused: + q_full_resync = follower.get_joint_pos() + q_arm = q_full_resync[:6][::-1].copy() + gripper = q_full_resync[6] + # Force re-latch of origins on next grip press after pause. + vr_origin_mat = None + robot_origin_T = None + was_paused = False + + # --- read controller --- + poses, buttons = oculus_reader.get_transformations_and_buttons() + if not poses or controller_id not in poses: + time.sleep(dt) + continue + + grip = bool(buttons.get(grip_key, False)) + joystick_click = bool(buttons.get(joystick_key, False)) + trig_val = float(buttons.get(trig_key, [0.0])[0]) + raw_pose = np.asarray(poses[controller_id], dtype=np.float64) + + # --- orientation zeroing (mirrors droid _update_internal_state) --- + # Joystick press re-enables one-shot update of vr_to_global_mat. + if joystick_click: + reset_orientation = True + stop_updating = joystick_click or grip + if reset_orientation: + try: + vr_to_global_mat = np.linalg.inv(raw_pose) + except np.linalg.LinAlgError: + vr_to_global_mat = np.eye(4) + else: + if stop_updating: + reset_orientation = False + + vr_mat = global_to_env_mat @ vr_to_global_mat @ raw_pose + + # --- grip rising-edge: latch origins --- + if grip and not prev_grip: + vr_origin_mat = vr_mat.copy() + raw_pose_origin = raw_pose.copy() + robot_origin_T = _fk_tcp(q_arm).copy() + prev_grip = grip + + # --- deadman: hold position when grip not held --- + if not grip: + cmd = np.append(q_arm[::-1], gripper) + follower.command_joint_pos(cmd) + self._last_commanded_pos[side] = cmd.copy() + elapsed = time.monotonic() - loop_start + if dt - elapsed > 0: + time.sleep(dt - elapsed) + continue + + if vr_origin_mat is None or robot_origin_T is None: + time.sleep(dt) + continue + + # --- compute target EE pose --- + delta_pos = spatial_coeff * (vr_mat[:3, 3] - vr_origin_mat[:3, 3]) + target_pos = robot_origin_T[:3, 3] + pos_action_gain * delta_pos + + # Compute R_rel in VR world frame (bypasses the vr_to_global body-frame + # wrapping that causes yaw/roll coupling when the controller is tilted + # at zero time), then project to robot world frame via P-conjugation. + # _R_fix swaps the VR X (wrist-roll) and VR Y (tilt) contributions so + # they land on the correct robot axes without disturbing the position + # mapping (which uses global_to_env_mat directly). + P = global_to_env_mat[:3, :3] + _R_fix = np.array([[0., 1., 0.], [-1., 0., 0.], [0., 0., 1.]]) + P_rot = _R_fix @ P + R_delta_vr = raw_pose[:3, :3] @ raw_pose_origin[:3, :3].T + R_rel = P_rot @ R_delta_vr @ P_rot.T + angle_axis = Rotation.from_matrix(R_rel).as_rotvec() * rot_action_gain + target_rot = Rotation.from_rotvec(angle_axis).as_matrix() @ robot_origin_T[:3, :3] + + T_target = np.eye(4) + T_target[:3, :3] = target_rot + T_target[:3, 3] = target_pos + + # --- J-PARSE IK --- + q_arm = _ik_step(q_arm, T_target) + + # --- gripper: index trigger 0=open→robot 1.0, 1=closed→robot 0.0 --- + gripper = float(np.clip(1.0 - trig_val, 0.0, 1.0)) + + cmd = np.append(q_arm[::-1], gripper) # pyroki→MuJoCo + follower.command_joint_pos(cmd) + self._last_commanded_pos[side] = cmd.copy() + + elapsed = time.monotonic() - loop_start + if dt - elapsed > 0: + time.sleep(dt - elapsed) + + except Exception as e: + print(f" Oculus {side} loop error: {e}") + time.sleep(dt) + def signal_ready_with_grippers(self) -> None: """Close then re-open both follower grippers once to signal system ready.""" print(" - Signalling ready: closing and opening grippers...") - closed_pos = FOLLOWER_HOME_POS.copy() + closed_pos = self._follower_home_pos.copy() closed_pos[6] = 0.0 # closed def cycle(robot: Robot) -> None: for _ in range(2): smooth_move_joints(robot, closed_pos, time_interval_s=0.1, steps=10) smooth_move_joints( - robot, FOLLOWER_HOME_POS, time_interval_s=0.1, steps=10 + robot, self._follower_home_pos, time_interval_s=0.1, steps=10 ) threads = [] @@ -1336,6 +1625,38 @@ def setup_for_teleop_recording(self): print("Checking CAN interfaces...") self.check_can_interfaces() + # Pre-warm JAX/pyroki IK *before* starting the motor threads. + # JAX tracing holds the GIL for several seconds on first run; if it + # runs while motors are active the DM watchdog fires (loss communication). + if not getattr(self, "_prewarmed_ik", {}): + print("Pre-warming IK (JAX JIT, ~5 s on first run)...") + errors: dict = {} + lock = threading.Lock() + result: dict = {} + + def _warm(side: str) -> None: + try: + r = _setup_pyroki(0.01) + with lock: + result[side] = r + except Exception as exc: + with lock: + errors[side] = exc + + threads = [] + if self.use_left_follower or self.use_right_follower: + threads.append(threading.Thread(target=_warm, args=("left",), daemon=True)) + if self.use_right_follower: + threads.append(threading.Thread(target=_warm, args=("right",), daemon=True)) + for t in threads: + t.start() + for t in threads: + t.join() + if errors: + raise RuntimeError(f"IK pre-warm failed: {errors}") + self._prewarmed_ik = result + print("✓ IK ready") + # Initialize robots self.initialize_robots(gravity_comp_mode=False) diff --git a/raiden/robot/teleop.py b/raiden/robot/teleop.py index d07907c..de6b5b2 100644 --- a/raiden/robot/teleop.py +++ b/raiden/robot/teleop.py @@ -18,9 +18,17 @@ def run_bimanual_teleop( rot_scale: float = 3.0, invert_rotation: bool = False, arms: str = "bimanual", + oculus_right_controller: bool = True, + oculus_spatial_coeff: float = 1.0, + oculus_pos_action_gain: float = 1.0, + oculus_rot_action_gain: float = 1.0, + oculus_rmat_reorder: list | None = None, + follower_home_pos: list | None = None, ): """Run the bimanual teleoperation system""" + import numpy as np + use_right = arms == "bimanual" use_left = True @@ -31,6 +39,11 @@ def run_bimanual_teleop( vel_scale=vel_scale, rot_scale=rot_scale, invert_rotation=invert_rotation, + oculus_right_controller=oculus_right_controller, + oculus_spatial_coeff=oculus_spatial_coeff, + oculus_pos_action_gain=oculus_pos_action_gain, + oculus_rot_action_gain=oculus_rot_action_gain, + oculus_rmat_reorder=oculus_rmat_reorder, ) robot_controller = RobotController( @@ -38,6 +51,7 @@ def run_bimanual_teleop( use_left_leader=interface.uses_leaders and use_left, use_right_follower=use_right, use_left_follower=use_left, + follower_home_pos=np.array(follower_home_pos) if follower_home_pos is not None else None, ) interface.open() diff --git a/scripts/oculus_calib.py b/scripts/oculus_calib.py new file mode 100644 index 0000000..e03912b --- /dev/null +++ b/scripts/oculus_calib.py @@ -0,0 +1,200 @@ +#!/usr/bin/env python3 +"""Oculus controller axis calibration — no robot needed. + +Connects to an Oculus Quest via ADB and shows in real-time how the controller +axes map to the robot base frame under the current rmat_reorder. Use this to +find the right rmat_reorder before running a full teleop session. + +Usage: + uv run python scripts/oculus_calib.py + uv run python scripts/oculus_calib.py --rmat_reorder '-1,-2,3,4' + uv run python scripts/oculus_calib.py --controller r # right (default) + uv run python scripts/oculus_calib.py --controller l # left + +Controls: + RJ (joystick click) Zero the orientation (define current pose as origin) + RG (grip trigger) Hold to enable delta tracking from latch point + Ctrl-C Quit +""" + +import argparse +import os +import sys +import time + +import numpy as np + + +def _vec_to_reorder_mat(vec: list) -> np.ndarray: + X = np.zeros((len(vec), len(vec))) + for i in range(len(vec)): + ind = int(abs(vec[i])) - 1 + X[i, ind] = np.sign(vec[i]) + return X + + +def _axis_table(rmat_reorder: list) -> list[str]: + """Human-readable VR → robot axis mapping for the given rmat_reorder.""" + mat = _vec_to_reorder_mat(rmat_reorder) + vr_labels = ["+X (right)", "+Y (up)", "+Z (back)"] + robot_axes = ["X", "Y", "Z"] + lines = [] + for vr_i, vr_lbl in enumerate(vr_labels): + for rob_i, rob_ax in enumerate(robot_axes): + v = mat[rob_i, vr_i] + if abs(v) > 0.5: + sign = "+" if v > 0 else "-" + lines.append(f" VR {vr_lbl:<20} → Robot {sign}{rob_ax}") + return lines + + +def _render( + rmat_reorder: list, + controller_id: str, + zeroed: bool, + grip: bool, + raw_pos: np.ndarray | None, + robot_pos: np.ndarray | None, + origin_delta: np.ndarray | None, +) -> str: + width = 60 + lines = [ + "─" * width, + " Oculus Axis Calibration", + "─" * width, + f" rmat_reorder : {rmat_reorder}", + f" Controller : {'right' if controller_id == 'r' else 'left'}", + "", + " RJ = zero orientation RG = hold to track delta ^C = quit", + "", + f" Orient zeroed : {'YES' if zeroed else 'NO (press joystick to zero)'}", + f" Grip held : {'YES' if grip else 'NO'}", + "", + " VR → Robot axis map:", + ] + lines += _axis_table(rmat_reorder) + lines += [""] + + if raw_pos is not None: + lines.append( + f" VR raw pos (m) : x={raw_pos[0]:+.3f} y={raw_pos[1]:+.3f} z={raw_pos[2]:+.3f}" + ) + if robot_pos is not None: + lines.append( + f" Robot frame (m): x={robot_pos[0]:+.3f} y={robot_pos[1]:+.3f} z={robot_pos[2]:+.3f}" + ) + + if grip and origin_delta is not None: + lines += [ + "", + " Delta from grip latch (move controller to check axes):", + f" Δx={origin_delta[0]:+.3f} Δy={origin_delta[1]:+.3f} Δz={origin_delta[2]:+.3f}", + ] + elif grip: + lines += ["", " (grip just latched — move controller to see delta)"] + else: + lines += ["", " Hold RG (grip trigger) to see per-axis delta."] + + lines.append("─" * width) + return "\n".join(lines) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument( + "--rmat_reorder", + default="-2,-1,-3,4", + help="Signed-index axis map, comma-separated (default: -2,-1,-3,4)", + ) + parser.add_argument( + "--controller", + choices=["r", "l"], + default="r", + help="Which controller to read: r=right (default), l=left", + ) + args = parser.parse_args() + + rmat_reorder = [int(x) for x in args.rmat_reorder.split(",")] + controller_id = args.controller + + global_to_env_mat: np.ndarray = _vec_to_reorder_mat(rmat_reorder) + + print("Connecting to Oculus Quest via ADB ...") + from oculus_reader.reader import OculusReader + reader = OculusReader() + time.sleep(0.8) + + grip_key = controller_id.upper() + "G" + joystick_key = controller_id.upper() + "J" + + vr_to_global_mat: np.ndarray = np.eye(4) + zeroed = False + reset_orientation = True + + prev_grip = False + origin_mat: np.ndarray | None = None + + print("Connected. Put on the headset — the Teleop app should be visible.") + print("Press RJ (joystick) to zero orientation, then hold RG (grip) to track.\n") + + try: + while True: + t0 = time.monotonic() + + poses, buttons = reader.get_transformations_and_buttons() + if not poses or controller_id not in poses: + time.sleep(0.05) + continue + + raw_pose = np.asarray(poses[controller_id], dtype=np.float64) + grip = bool(buttons.get(grip_key, False)) + joystick = bool(buttons.get(joystick_key, False)) + + # Orientation zeroing (same logic as _oculus_control_loop) + stop_updating = joystick or grip + if reset_orientation: + try: + vr_to_global_mat = np.linalg.inv(raw_pose) + zeroed = True + except np.linalg.LinAlgError: + vr_to_global_mat = np.eye(4) + if stop_updating: + reset_orientation = False + if joystick and not stop_updating: + reset_orientation = True + + vr_mat = global_to_env_mat @ vr_to_global_mat @ raw_pose + raw_pos = raw_pose[:3, 3] + robot_pos = vr_mat[:3, 3] + + # Grip latch + if grip and not prev_grip: + origin_mat = vr_mat.copy() + if not grip: + origin_mat = None + prev_grip = grip + + origin_delta = None + if grip and origin_mat is not None: + origin_delta = robot_pos - origin_mat[:3, 3] + + frame = _render( + rmat_reorder, controller_id, zeroed, grip, + raw_pos, robot_pos, origin_delta, + ) + os.system("clear") + print(frame) + + elapsed = time.monotonic() - t0 + remaining = 0.1 - elapsed + if remaining > 0: + time.sleep(remaining) + + except KeyboardInterrupt: + print("\nDone.") + finally: + reader.stop() + + +if __name__ == "__main__": + main() diff --git a/third_party/i2rt b/third_party/i2rt index 7b6d501..4e462ec 160000 --- a/third_party/i2rt +++ b/third_party/i2rt @@ -1 +1 @@ -Subproject commit 7b6d5016f05ca63f9ef0185b7143e63f2c7a5708 +Subproject commit 4e462ecc435491a46f9bceb465237afb41d7b66f diff --git a/uv.lock b/uv.lock index daed73c..ce446d6 100644 --- a/uv.lock +++ b/uv.lock @@ -1360,6 +1360,15 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/a2/eb/86626c1bbc2edb86323022371c39aa48df6fd8b0a1647bc274577f72e90b/nvidia_nvtx_cu12-12.8.90-py3-none-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:5b17e2001cc0d751a5bc2c6ec6d26ad95913324a4adb86788c944f8ce9ba441f", size = 89954, upload-time = "2025-03-07T01:42:44.131Z" }, ] +[[package]] +name = "oculus-reader" +version = "1.0.0" +source = { git = "https://github.com/rail-berkeley/oculus_reader.git#01d11501b33844c359c87f4999d07e69c3214ade" } +dependencies = [ + { name = "numpy" }, + { name = "pure-python-adb" }, +] + [[package]] name = "omegaconf" version = "2.3.0" @@ -1592,6 +1601,12 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/8c/c7/7bb2e321574b10df20cbde462a94e2b71d05f9bbda251ef27d104668306a/psutil-7.2.2-cp37-abi3-win_arm64.whl", hash = "sha256:8c233660f575a5a89e6d4cb65d9f938126312bca76d8fe087b947b3a1aaac9ee", size = 134617, upload-time = "2026-01-28T18:15:36.514Z" }, ] +[[package]] +name = "pure-python-adb" +version = "0.3.0.dev0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/0a/b7/1c4d6b2cbe499b4180177abcf3ae2bb2d8b36acf695ae7d8e9eb99ba00ea/pure-python-adb-0.3.0.dev0.tar.gz", hash = "sha256:0ecc89d780160cfe03260ba26df2c471a05263b2cad0318363573ee8043fb94d", size = 25680, upload-time = "2020-08-05T18:41:27.157Z" } + [[package]] name = "pyarrow" version = "23.0.1" @@ -1849,14 +1864,14 @@ wheels = [ [[package]] name = "pyzed" -version = "5.2" -source = { path = "packages/pyzed-5.2-cp311-cp311-linux_x86_64.whl" } +version = "5.3" +source = { path = "packages/pyzed-5.3-cp311-cp311-linux_x86_64.whl" } dependencies = [ { name = "cython" }, { name = "numpy" }, ] wheels = [ - { filename = "pyzed-5.2-cp311-cp311-linux_x86_64.whl", hash = "sha256:c236065cc97f7235fb2e73586c0a60b700bb7361ed73252330f8f5a87addc71a" }, + { filename = "pyzed-5.3-cp311-cp311-linux_x86_64.whl", hash = "sha256:06a95ff4f1d147b5c3b0438261d2929fa4916fea40bab5d0d38f4d04e9ae7884" }, ] [package.metadata] @@ -1929,6 +1944,7 @@ dependencies = [ { name = "jaxlie" }, { name = "mink" }, { name = "numpy" }, + { name = "oculus-reader" }, { name = "onnxscript" }, { name = "opencv-contrib-python" }, { name = "pyrealsense2" }, @@ -2003,6 +2019,7 @@ requires-dist = [ { name = "mkdocs-material", marker = "extra == 'docs'", specifier = ">=9.0" }, { name = "mkdocstrings", extras = ["python"], marker = "extra == 'docs'", specifier = ">=0.24" }, { name = "numpy", specifier = ">=1.24" }, + { name = "oculus-reader", git = "https://github.com/rail-berkeley/oculus_reader.git" }, { name = "omegaconf", marker = "extra == 'ffs'" }, { name = "omegaconf", marker = "extra == 'ffs-trt-cu12'" }, { name = "omegaconf", marker = "extra == 'ffs-trt-cu13'" }, @@ -2018,7 +2035,7 @@ requires-dist = [ { name = "pysondb", specifier = ">=1.6.1" }, { name = "pyspacemouse", specifier = ">=1.0" }, { name = "pyyaml", specifier = ">=6.0.3" }, - { name = "pyzed", marker = "extra == 'zed'", path = "packages/pyzed-5.2-cp311-cp311-linux_x86_64.whl" }, + { name = "pyzed", marker = "extra == 'zed'", path = "packages/pyzed-5.3-cp311-cp311-linux_x86_64.whl" }, { name = "questionary", specifier = ">=2.0" }, { name = "rerun-sdk", specifier = ">=0.16" }, { name = "scipy", specifier = ">=1.11" },