Oculus teleop

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/"]

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)

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.
 """
 

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 <data_dir>/raw/<task>/"""
@@ -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":

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()

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"
+        )