From 293a8d9a77f917d2c5e98c796c5a39857013e0d3 Mon Sep 17 00:00:00 2001 From: Caroline Pascal Date: Sun, 5 Jul 2026 20:56:26 +0200 Subject: [PATCH] =?UTF-8?q?feat(examples):=20add=20Isaac=20Teleop=20?= =?UTF-8?q?=E2=86=92=20SO-101=20teleoperation=20and=20dataset=20recording?= =?UTF-8?q?=20example=20(#3927)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add Isaac Teleop SO-101 leader-arm teleoperator Add the NVIDIA Isaac Teleop teleoperator scaffolding and its first device: SO101LeaderArm, a back-drivable SO-101 leader arm on Isaac Teleop's generic joint-space device path. It reads the leader's joints from the so101_leader plugin via JointStateSource and emits follower-ready {joint}.pos (rad2deg arm, gripper -> RANGE_0_100) for direct 1:1 joint drive. - IsaacTeleopTeleoperator base + IsaacTeleopConfig (shared session/CloudXR config) - SO101LeaderArm / SO101LeaderArmConfig and leader_joints_to_robot_action - examples/isaac_teleop_to_so101/teleoperate_leader.py example - pure-numpy conversion tests - isaac-teleop optional extra + NVIDIA PyPI index in pyproject * Add Isaac Teleop XR controller teleoperator for SO-101 Add end-to-end XR (VR) controller teleoperation of an SO-101 follower arm via the NVIDIA Isaac Teleop stack, layered on the Isaac Teleop scaffolding. Teleoperator (src/lerobot/teleoperators/isaac_teleop/): - XRController / XRControllerConfig: connect to the CloudXR runtime, auto-launch the Isaac Teleop session, and expose get_action() emitting the raw base-frame grip pose, squeeze, and trigger. - MapXRControllerActionToRobotAction: stateless per-frame mapper from the XR action to the IK input contract (absolute ee.x/y/z, ee.gripper_pos, wrist_roll). - OverwriteWristRollFromAngle: post-IK step writing the operator wrist-roll [rad] onto wrist_roll.pos [deg], recovering the under-determined roll DOF. Example (examples/isaac_teleop_to_so101/): - teleoperate.py: thin absolute-pose IK pipeline with an in-loop clutch (engage latch + 1:1 delta rebase of position and orientation), EEBoundsAndSafety, and InverseKinematicsEEToJoints; slews to a recorded home on startup. - record_reset_pose.py / download_assets.py / webxr.env / .gitignore. Also: - Extend robot_kinematic_processor.py with EEBoundsAndSafety and InverseKinematicsEEToJoints. - Add XRControllerConfig + base_T_anchor to the Isaac Teleop config. - Add docs/source/isaac_teleop.mdx and the _toctree entry. - Add unit tests for the CloudXR launcher and the XR controller processor. * Unify Isaac Teleop SO-101 scripts behind a mandatory device selector Merge teleoperate.py (XR controller: clutch + soft-orientation IK) and teleoperate_leader.py (SO-101 leader arm: 1:1 joint mirror) into a single teleoperate.py driven by a `lerobot-teleoperate`-style draccus CLI: a follower `--robot.*` and an input `--teleop.*`, where `--teleop.type` (xr_controller | so101_leader) selects the Isaac device. Uses a "dispatch, don't merge" shape: per-device setup_xr/setup_leader build a Device bundle (compute / startup / cleanup / command); a shared slew() takes a per-step target callable (XR a fixed reset pose, leader a live re-read so the 1:1 handoff stays continuous); one device-branchless outer loop runs both, with compute() -> None meaning "hold at the measured pose" (XR disengaged or leader stale). The entrypoint is @parser.wrap()'d over a TeleoperateConfig dataclass and dispatches on the parsed config type; device knobs ride on --teleop.* (the leader serial port is --teleop.port, forwarded to the plugin) and loop/launch knobs are top-level (--launch_plugin= collapses the old --launch-plugin/--plugin-bin pair; --reset_to_origin/--align/--dry_run). To let the Isaac devices claim the natural --teleop.type names without colliding with the serial so101_leader of lerobot-teleoperate, give IsaacTeleopConfig its own draccus choice registry (own _choice_registry, decoupled from the global TeleoperatorConfig one) and register XRControllerConfig as "xr_controller" and SO101LeaderArmConfig as "so101_leader" there; the example types its teleop field as IsaacTeleopConfig so the choices resolve against that scoped registry. These devices drive the bespoke clutch/IK/align loop and are not routed through make_teleoperator_from_config, so dropping them from the global registry is inert. YAGNI sweep of the commit train: delete the orphaned OverwriteWristRollFromAngle (wrist_roll_processor.py) plus its export and tests -- no producer emits wrist_roll; the live XR path uses orientation-weight IK on the 5-DOF arm by design. Kept the load-bearing knobs (orientation_weight, raise_on_jump, base_T_anchor) and the optional reset-pose recorder. Updated isaac_teleop.mdx for the unified entrypoint and excised the stale roll-retargeter prose. Net LOC down (two scripts 714 lines -> one), in-loop device branches reduced to zero. Planned and reviewed via a 6-persona multi-agent panel (3-round planning convergence + 2-round review). Verification (isaacteleop/placo not installable here, so the device classes cannot connect, but their config dataclasses and the script import fine via deferred imports): the teleoperators test suite passes (45 passed, 2 skipped), draccus parsing of both target command lines yields the right config subclass with scoped --teleop.type, --help renders the scoped choices, the serial so101_leader stays in the global registry, and ruff check/format are green. Signed-off-by: Jiwen Cai * Add Isaac Teleop SO-101 dataset recording script record.py records a LeRobot dataset while driving the SO-101 follower with either Isaac Teleop device (--teleop.type=xr_controller | so101_leader), mirroring teleoperate.py's device dispatch. * Extract shared Isaac Teleop SO-101 example infra into common.py teleoperate.py and record.py both built the per-device pipeline and ran the same read -> compute -> hold-when-idle -> sleep loop, with record.py importing internals from teleoperate.py. Move the shared device/loop infrastructure (Device, slew, Clutch, setup_xr/setup_leader + leader helpers, reset infra and constants) into a new common.py, and add build_device() + hold_action() to collapse the connect/dispatch/startup and idle-hold glue duplicated in both entry points. The setup functions now type their config against a LoopConfig Protocol, so common.py is decoupled from either CLI; both import from it. Also rename record_reset_pose.py -> override_reset_pose.py so it is not confused with record.py, and update the doc references. * Add stdin keyboard backend so recording shortcuts work over SSH/headless lerobot's init_keyboard_listener() uses pynput, which hooks GLOBAL key events from the display server. Over SSH, under Wayland, or on a headless box with only a TTY, keystrokes go to the terminal's stdin instead, so the listener never fires and the Right/Left/Esc recording shortcuts silently do nothing. Add a stdin (termios) keyboard backend to the example's common.py and an init_keyboard_listener() that prefers it whenever stdin is an interactive TTY (works over SSH / Wayland / headless-with-tty), falling back to lerobot's pynput/headless listener for GUI launches with no controlling terminal. Selectable via LEROBOT_KEYBOARD_BACKEND={auto,stdin,pynput,none}. The backend keeps ISIG so Ctrl-C still works and always restores the terminal (on stop() and via atexit). record.py now sources init_keyboard_listener from common; the Right/Left/Esc -> flag mapping and the (listener, events) contract are unchanged. Also convert record.py's loop_kwargs to a dict literal (ruff C408). * Wait for the XR headset to connect before driving the arm On the xr_controller path the example connected CloudXR and immediately ran the reset slew + control loop, even if no headset was connected — the arm moved before the operator was in VR, and get_action() just returned zeros so the clutch never engaged. Add an is_tracking property to XRController (set from the controller stream's optional group, mirroring SO101LeaderArm) and a _wait_for_xr_controller() helper in common.py that prints connection instructions (CloudXR web client URL + this workstation's candidate IPv4s, with loopback/link-local and virtual/bridge/USB-gadget interfaces filtered out) and polls until the controllers stream (indefinite, 15s reminder, Ctrl-C to abort). setup_xr.startup() now connects, waits for the headset, THEN runs the reset slew and seeds the clutch — so the arm only moves once the operator is connected and watching. Mirrors the leader path's _wait_for_leader; both record.py and teleoperate.py inherit it via the shared setup_xr. * Address review feedback on the Isaac Teleop -> SO-101 example Review-response and CI fixes for the Isaac Teleop -> SO-101 example. - Move the XR Clutch into src/lerobot/teleoperators/isaac_teleop/clutch.py (pure numpy + Rotation, no isaacteleop import), export it, and add tests/teleoperators/test_clutch.py. - Drop the vendored stdin keyboard listener; record.py uses a small terminal- first wrapper over upstream's TerminalKeyListener (works over SSH even with a local X display), falling back to upstream init_keyboard_listener otherwise. - record.py: pass rgb_encoder/depth_encoder to LeRobotDataset create()/resume() (upstream removed camera_encoder), fixing the AttributeError at record time. - build_device: derive motor names from robot.action_features instead of robot.bus (supports non-bus robots), and disconnect the follower if any step after connect() fails so a failed setup never leaks the connection. - Read leader joints by the group's declared names (_joints_group_to_rad) instead of positionally, so a layout mismatch can't silently mirror the wrong DOF onto the follower; add tests including a reversed-layout group. - base.py: hoist `from pathlib import Path` to module scope; only the isaacteleop CloudXRLauncher import stays lazy (optional dep). - Trim the common.py module docstring and point to docs/source/isaac_teleop.mdx. - default.env: correct the NV_DEVICE_PROFILE comment (auto-webrtc is the default; this file overrides to Quest3, which works for both Quest 3 and Pico 4). - download_assets.py: correct the RAW_BASE comment (tracks main, not pinned) and add `# nosec B310` next to the existing `# noqa: S310` for the bandit hook. - uv.lock: add the isaac-teleop extra's deps so `uv sync --locked` matches pyproject; regenerated with uv 0.8.0 to keep lockfile revision 2 (CI's uv). - isaac_teleop.mdx: prettier formatting. * fix(.gitignore): removing .gitignore and using lerobot cache folder instead to store local user files * chore(docstrings): reducing docstrings in default.env * feat(URDF): cleaning up and simplifying the URDF download procedure * feat(robot guard): adding a guard in case an unsupported robot type is provided (so-arms only) * fix(imports): enforcing a python module structure to simplify imports * feat(safe read): extending the motor bus safe read rationale to reset pose setting * chore(trim): trimming lenghty comments and docstrings * fix(deps): use isaacteleop [retargeters-lite] extra to unblock aarch64 (DGX Spark) (#3933) * fix(deps): drop isaacteleop [retargeters] extra to unblock aarch64 The [retargeters] extra pulls dex-retargeting (pins numpy<2.0, conflicting with lerobot's numpy>=2.0) and nlopt>=2.8 (no aarch64 wheels), making lerobot[isaac-teleop] unresolvable on ARM (DGX Spark, Jetson Thor, GH200) and over-constrained on numpy everywhere else. The LeRobot teleoperators only import isaacteleop.retargeting_engine, isaacteleop.cloudxr and isaacteleop.teleop_session_manager, all shipped in the base wheel (requires only numpy>=1.23), so the extra is unused. Verified on DGX Spark (aarch64, Python 3.12): resolves and installs with isaacteleop 1.3.131 + numpy 2.2.6; all imported symbols load. * fix(deps): use isaacteleop [retargeters-lite] extra for aarch64 support Pin to isaacteleop ~=1.3.131 (the release that added ARM64/aarch64 support) and swap the full [retargeters] extra for the new [retargeters-lite] one (scipy-only). The full extra drags in dex-retargeting (pins numpy<2, conflicting with lerobot's numpy>=2.0) and nlopt>=2.8 (no aarch64 wheels), making lerobot[isaac-teleop] unresolvable on ARM hosts (DGX Spark, Jetson Thor, GH200) and over-constrained on numpy everywhere else. The LeRobot teleoperators only import isaacteleop.retargeting_engine, isaacteleop.cloudxr and isaacteleop.teleop_session_manager — all covered by the base wheel + retargeters-lite. Verified on DGX Spark (aarch64, Python 3.12/3.13): resolves and installs with isaacteleop 1.3.131 + numpy 2.2.6 + scipy 1.18. * feat(deps): re-add full [retargeters] extra gated to x86_64 Keep the dex-retargeting/nlopt-based retargeters available on x86_64 (where their wheels exist) via an environment marker, while ARM hosts (DGX Spark, Jetson Thor, GH200) resolve with base + [retargeters-lite] only. Verified: uv lock resolves on both platforms; on aarch64 the compile excludes nlopt/dex-retargeting, on x86_64 they are included. --------- Co-authored-by: Johnny Nunez <22727137+johnnynunez@users.noreply.github.com> * chore(docstrings): trimming latest docstrings * chore(teleop): move isaac-teleop to examples + update docs + add readme with installation notes * chore(deps): restore uv.lock * fix(example: isaac teleop parsing config * fix(examples): isaac atomic-gripper controller * feat(Examples): isaac-teleop holdlatch * chore(examples): some other minor improvements for isaac-teleop * chore(examples): top-level imports isaac-teleop * chore(Examples): address ai review isaac-teleop --------- Signed-off-by: Jiwen Cai Co-authored-by: Jiwen Cai Co-authored-by: Johnny Co-authored-by: Johnny Nunez <22727137+johnnynunez@users.noreply.github.com> Co-authored-by: Steven Palma Co-authored-by: Steven Palma --- docs/source/_toctree.yml | 2 + docs/source/isaac_teleop.mdx | 397 +++++++++++ examples/isaac_teleop_to_so101/README.md | 131 ++++ examples/isaac_teleop_to_so101/__init__.py | 17 + examples/isaac_teleop_to_so101/common.py | 650 ++++++++++++++++++ examples/isaac_teleop_to_so101/default.env | 21 + .../isaac_teleop/__init__.py | 40 ++ .../isaac_teleop/base.py | 282 ++++++++ .../isaac_teleop/clutch.py | 102 +++ .../isaac_teleop/config_isaac_teleop.py | 135 ++++ .../isaac_teleop/teleop_so101_leader_arm.py | 186 +++++ .../isaac_teleop/teleop_xr_controller.py | 204 ++++++ .../isaac_teleop/xr_controller_processor.py | 87 +++ .../override_reset_pose.py | 73 ++ examples/isaac_teleop_to_so101/record.py | 321 +++++++++ examples/isaac_teleop_to_so101/teleoperate.py | 117 ++++ .../so_follower/robot_kinematic_processor.py | 32 +- 17 files changed, 2795 insertions(+), 2 deletions(-) create mode 100644 docs/source/isaac_teleop.mdx create mode 100644 examples/isaac_teleop_to_so101/README.md create mode 100644 examples/isaac_teleop_to_so101/__init__.py create mode 100644 examples/isaac_teleop_to_so101/common.py create mode 100644 examples/isaac_teleop_to_so101/default.env create mode 100644 examples/isaac_teleop_to_so101/isaac_teleop/__init__.py create mode 100644 examples/isaac_teleop_to_so101/isaac_teleop/base.py create mode 100644 examples/isaac_teleop_to_so101/isaac_teleop/clutch.py create mode 100644 examples/isaac_teleop_to_so101/isaac_teleop/config_isaac_teleop.py create mode 100644 examples/isaac_teleop_to_so101/isaac_teleop/teleop_so101_leader_arm.py create mode 100644 examples/isaac_teleop_to_so101/isaac_teleop/teleop_xr_controller.py create mode 100644 examples/isaac_teleop_to_so101/isaac_teleop/xr_controller_processor.py create mode 100644 examples/isaac_teleop_to_so101/override_reset_pose.py create mode 100644 examples/isaac_teleop_to_so101/record.py create mode 100644 examples/isaac_teleop_to_so101/teleoperate.py diff --git a/docs/source/_toctree.yml b/docs/source/_toctree.yml index 92a9b22b2..7f7a34e6a 100644 --- a/docs/source/_toctree.yml +++ b/docs/source/_toctree.yml @@ -169,6 +169,8 @@ - sections: - local: phone_teleop title: Phone + - local: isaac_teleop + title: Isaac Teleop title: "Teleoperators" - sections: - local: cameras diff --git a/docs/source/isaac_teleop.mdx b/docs/source/isaac_teleop.mdx new file mode 100644 index 000000000..6236a5ebc --- /dev/null +++ b/docs/source/isaac_teleop.mdx @@ -0,0 +1,397 @@ +# Isaac Teleop + +Control your robot with NVIDIA [Isaac Teleop](https://github.com/NVIDIA/IsaacTeleop), a +multi-modal teleoperation framework. Isaac Teleop drives a single `TeleopSession` from a range +of input devices — XR (VR) controllers, hand tracking, full-body tracking, Manus gloves, foot +pedals, and more. + +In LeRobot, Isaac Teleop ships as a self-contained example under +[`examples/isaac_teleop_to_so101/`](https://github.com/huggingface/lerobot/tree/main/examples/isaac_teleop_to_so101). +Each Isaac Teleop input device is its own `Teleoperator` subclass in the example's +`isaac_teleop` package, sharing one session lifecycle (see `IsaacTeleopTeleoperator`). The +devices available today are the **XR controller** (`XRController`) and a back-drivable +**SO-101 leader arm** (`SO101LeaderArm`); Manus gloves and hand/full-body tracking are the +natural next devices. This guide focuses on the XR controller; the SO-101 leader is summarized +under [Run the example](#step-3-run-the-example). + +**In this guide you'll learn:** + +- How an Isaac Teleop device drives a robot end‑effector (EE) target +- How the _clutch_ (squeeze/grip on the XR controller) engages teleoperation without jerking the arm +- How to run the SO‑101 teleoperation example and tune motion / gripper / IK + +## Installation + +The example lives in the LeRobot repository (it is not part of the `lerobot` pip package), so +clone the repo and install from source. The canonical, always-up-to-date install and usage +reference is the example's +[`README.md`](https://github.com/huggingface/lerobot/tree/main/examples/isaac_teleop_to_so101/README.md); +in short: + +```bash +git clone https://github.com/huggingface/lerobot.git +cd lerobot +uv pip install -e ".[feetech,kinematics,dataset]" "huggingface_hub>=1.5" +uv pip install "isaacteleop[cloudxr,retargeters-lite]~=1.3.131" "scipy>=1.14" +``` + +`isaacteleop` is published on public PyPI (Linux only). The `cloudxr` extra brings the CloudXR +runtime bindings; `retargeters-lite` is the scipy-based retargeter path that resolves on both +x86_64 and ARM (on aarch64 — e.g. a DGX Spark — the full `retargeters` extra does not resolve +because of its `dex-retargeting`/`nlopt` pins, which is why it is not the default here). On +x86_64 you can additionally install the full retargeter stack: + +```bash +uv pip install "isaacteleop[retargeters]~=1.3.131" +``` + +### Set up CloudXR and connect a headset + +Isaac Teleop streams the headset to your machine over **NVIDIA CloudXR**, which provides the +OpenXR runtime the session connects to. By default LeTeleop **auto-launches the CloudXR runtime +for you** when you call `teleop_device.connect()` — you no longer have to run `python -m +isaacteleop.cloudxr` and `source cloudxr.env` in a separate shell. All you need is a supported +headset connected and the CloudXR firewall ports open. Follow the Isaac Teleop +[Quick Start](https://nvidia.github.io/IsaacTeleop/main/getting_started/quick_start.html) for the +headset-pairing and firewall details. + +**First run (EULA).** The very first launch must accept the NVIDIA CloudXR EULA. The auto-launch +prompts for it **on stdin**, so on a headless machine it will hang waiting for input. Bootstrap +the EULA once, interactively, with: + +```bash +python -m isaacteleop.cloudxr --accept-eula # one-time: accept the CloudXR EULA +``` + +After that, `connect()` launches the runtime non-interactively. The launch **blocks for ~30s** +while the runtime comes up. + +**Configuration.** Two fields on `IsaacTeleopConfig` (shared by every device) control this: + +- `auto_launch_cloudxr` (default `True`) — whether `connect()` starts the runtime. Set `False` + when CloudXR is already running externally. +- `cloudxr_env_file` (default `None`) — an optional CloudXR device-profile `.env` selecting the + headset transport (e.g. an Apple Vision Pro profile). This is launcher **input**; it is not the + `~/.cloudxr/run/cloudxr.env` **output** file the old manual flow told you to `source`. `None` + keeps the default auto-WebRTC profile — though the SO-101 example overrides it to the + `default.env` shipped next to `teleoperate.py` unless you pass `--teleop.cloudxr_env_file`. + +**Opting out.** To skip the auto-launch (CloudXR already running), either set +`auto_launch_cloudxr=False` or export: + +```bash +export LEROBOT_CLOUDXR_SKIP_AUTOLAUNCH=1 +``` + +The **env var takes precedence over the config field**: if `LEROBOT_CLOUDXR_SKIP_AUTOLAUNCH=1` is +set, the auto-launch is skipped even when `auto_launch_cloudxr=True`. This variable is +**independent** of Isaac Lab's `ISAACLAB_CXR_SKIP_AUTOLAUNCH` — setting one does not affect the +other. + +**One teleoperator per process.** The CloudXR runtime configures the environment process-wide (a +singleton), so run a single Isaac Teleop teleoperator per process. + +**Shutting down.** Always call `teleop_device.disconnect()` on exit — including on Ctrl-C. Wrap +your teleoperation loop in `try/finally` and call `disconnect()` in the `finally`. This tears down +the OpenXR session **before** the CloudXR runtime, which is the required order; the launcher's +`atexit` hook only reaps the runtime and does not run the session's `__exit__`, so without an +explicit `disconnect()` an interrupted run shuts down in the wrong order. + +```python +teleop_device.connect() +try: + while True: + action = teleop_device.get_action() + # ... drive the robot ... +finally: + teleop_device.disconnect() +``` + +See [System Requirements](https://nvidia.github.io/IsaacTeleop/main/references/requirements.html) +for supported OS / GPU / CloudXR versions and headsets. + +## How it works + +The XR controller is one Isaac Teleop **input** device. `XRController` is a deliberately thin +reader: it exposes the **raw** controller grip pose — already statically rebased into the robot +base frame — plus the squeeze and trigger analog values. It has **no** retargeters and **no** +clutch logic of its own. The clutch (engage latch + delta rebasing onto the EE) and the gripper +mapping live downstream in the example loop, which then feeds LeRobot's existing closed‑loop +Cartesian IK pipeline — the same one the phone teleoperator uses. The device‑specific pieces are +`XRController`, the loop's `Clutch`, and `MapXRControllerActionToRobotAction`; everything downstream +(`EEBoundsAndSafety`, `InverseKinematicsEEToJoints`) is shared, and a future device (e.g. Manus +gloves) would swap in its own `teleop_.py` + processor while reusing the rest. + +`XRController._build_pipeline` wires Isaac Teleop's `ControllersSource` — statically rebased into +the robot base frame by the native `ControllerTransform` (`base_T_anchor`) — and exposes the +transformed controller stream verbatim. `get_action()` reads the grip pose, squeeze, and trigger +straight off it; the session is always stepped `RUNNING` (there is no clutch retargeter to gate). + +The `Clutch` class (in `examples/isaac_teleop_to_so101/isaac_teleop/clutch.py`, driven by the +loop in `common.py`) mirrors Isaac Teleop's `SO101ClutchRetargeter`, but lives in-loop so the +device can stay a thin reader: + +- It latches its engage origin on the squeeze **engage edge** (the frame the squeeze first crosses + `clutch_threshold`) and rebases both position and orientation around it, so engaging does not + teleport the arm. `Clutch.rebase` returns the absolute base-frame target as a `(pos, quat)` + pair, which the loop concatenates into the 7D `ee_pose` fed to the processor. +- The analog trigger becomes a gripper `closedness` in `[0, 1]` (0 = open, 1 = closed), + proportional to the trigger pull, which `MapXRControllerActionToRobotAction` maps to a jaw target. + +See the Isaac Teleop +[Retargeting interface](https://nvidia.github.io/IsaacTeleop/main/references/retargeting/index.html) +and [architecture overview](https://nvidia.github.io/IsaacTeleop/main/overview/architecture.html) +for how source nodes and retargeters compose. + +```text + VR controller (OpenXR) + │ + ▼ + XRController.get_action() ── raw base-frame grip_pos / grip_quat + squeeze + trigger + │ (TeleopSession always stepped RUNNING; clutch lives downstream) + ▼ + Clutch.rebase(grip_pos, grip_quat) ── engage-relative delta applied to the EE home (pos + orient) + │ ee_pose (7) / closedness → absolute ee_pose; closedness = trigger + ▼ + MapXRControllerActionToRobotAction ── absolute ee.x/y/z; ee.w* = orientation rotvec target; + │ ee.x/y/z / ee.w* / ee.gripper_pos ee.gripper_pos = (1 - closedness) * 100 + ▼ + EEBoundsAndSafety ── workspace clip + per-frame step clamp (clamp+warn) + │ + ▼ + InverseKinematicsEEToJoints ── closed-loop Placo IK; position + soft-orientation + │ (orientation_weight=0.01) (passes ee.gripper_pos → gripper.pos) + ▼ + SO-101 follower joint targets +``` + +### The clutch: owned by the example loop + +Unlike the phone pipeline (which splits the clutch across `MapPhoneActionToRobotAction` and +`EEReferenceAndDelta`), the XR clutch lives entirely in the example loop's `Clutch` class. It emits +an **absolute** EE pose, so there is no `EEReferenceAndDelta` stage and no delta accumulation in the +processor — `MapXRControllerActionToRobotAction` is a pure, stateless per‑frame mapping. + +The clutch latches its engage origin on the squeeze **engage edge** (the moment the squeeze crosses +`clutch_threshold`) and drives the EE from the motion _relative_ to that origin, so the arm does not +teleport on engage. On **every** engage — startup and mid‑task re‑clutch alike — the home +_position_ is latched from forward kinematics on the arm's **measured joints**, so the home equals +where the arm physically is even if it moved while disengaged, and the engage is jump‑free. The +home _orientation_ keeps the last commanded rotation: the 5‑DOF arm tracks orientation only +softly, so latching the measured wrist orientation would inject its tracking offset into the +command on every re‑clutch. + +## Controls + +- **Squeeze / grip** — the **clutch** (deadman). Hold it past `clutch_threshold` to engage + teleoperation; release to pause. Each engage re‑captures the origin, so you can reposition + your hand while paused and re‑engage without the arm jumping (index/clutch style). +- **Trigger** — the **gripper**, controlled **analog**. The jaw tracks the trigger + proportionally — a half‑pressed trigger leaves the jaw half‑closed — via a closedness in + `[0, 1]` (0 = open, 1 = closed) that maps to an absolute gripper joint target. +- **Controller orientation** — the **wrist**. The clutch rebases the controller orientation + (engage‑relative, base‑frame) into a soft IK orientation target the wrist tracks alongside + position. On the 5‑DOF SO‑101 the wrist follows the hand only partially by design — see + `orientation_weight` below. + +## Get started + +### Step 1: Create the teleoperator + +```python +# Run from the repo root so the `examples` package is importable. +from examples.isaac_teleop_to_so101.isaac_teleop import XRController, XRControllerConfig + +teleop_config = XRControllerConfig( + hand_side="right", # "left" or "right" controller + clutch_threshold=0.5, # squeeze value above which the clutch engages +) +teleop_device = XRController(teleop_config) +``` + +`XRController.get_action()` returns the **raw** base‑frame controller pose, not a clutch‑rebased +target: `grip_pos` (3,) `[x, y, z]` [m] and `grip_quat` (4,) `[qx, qy, qz, qw]` in the robot base +frame, plus scalar `squeeze` and `trigger` analog values in `[0, 1]`. The example loop's `Clutch` +turns these into the absolute `ee_pose`, and the squeeze is thresholded by the loop against +`clutch_threshold` to engage. + +### Step 2: Connect + +Calling `teleop_device.connect()` first auto-launches the CloudXR runtime (unless you opted out — +see [Set up CloudXR and connect a headset](#set-up-cloudxr-and-connect-a-headset); this blocks for +~30s and on the first run prompts for the EULA on stdin), then starts the Isaac Teleop +[`TeleopSession`](https://nvidia.github.io/IsaacTeleop/main/getting_started/teleop_session.html) +(opens the OpenXR session and discovers the controllers). XR controllers are self‑calibrating, so +there is no manual calibration step — the clutch handles re‑centering each time you engage. Pair +`connect()` with a `try/finally` that calls `disconnect()` so the session tears down before the +runtime on exit/Ctrl-C. + +### Step 3: Run the example + +The example assumes you configured your robot (SO‑101 follower) and set the correct serial port. + +The **robot URDF and its meshes are fetched automatically** on first run: the XR device downloads +the SO-101 URDF from the +[`lerobot/robot-urdfs` Hugging Face bucket](https://huggingface.co/buckets/lerobot/robot-urdfs/tree/so101) +into the LeRobot cache (`HF_LEROBOT_HOME/robot-urdfs/so101/`) and reuses it after, so there is no +separate download step : + +```bash +python -m examples.isaac_teleop_to_so101.teleoperate --robot.type=so101_follower --robot.port=/dev/ttyACM0 \ + --robot.id=so101_follower_arm --teleop.type=xr_controller +``` + +The CLI is `lerobot-teleoperate`-style (draccus): `--robot.*` configures the SO-101 follower and +`--teleop.type` selects the Isaac input device (`xr_controller` | `so101_leader`), with +`--teleop.*` its device knobs. `--teleop.type=xr_controller` runs the XR-controller path described +above. The startup safety contract: by default it slews all joints to a default reset pose over +`--reset_duration` seconds (`--reset_to_origin=false` keeps the arm where it is), then seeds the +clutch home from the arm's measured pose so the first engage is jump-free; the follower is +commanded only while the clutch is engaged. + +**Customizing the reset pose.** The reset pose ships as a built-in default (a comfortable mid-range +pose) and works out of the box — you do **not** need to record anything. To tailor it to your setup, +back-drive the arm to the pose you want and run +`python -m examples.isaac_teleop_to_so101.override_reset_pose --id `; it writes the +current joints to a per-arm file in the LeRobot cache +(`HF_LEROBOT_HOME/reset_poses//.json`, keyed like calibration), which then takes +priority over the built-in default on the next run. Because it lives in the user-local cache (not +the repo), your override stays on your machine, and both `teleoperate` and `record` honor it +when launched with the same `--robot.id`. + +The other device, `--teleop.type=so101_leader`, mirrors the follower 1:1 from a back-drivable +SO-101 _leader arm_ whose joints are streamed by Isaac Teleop's native `so101_leader` plugin (no +clutch, no IK — the leader and follower share the SO-101 kinematics). + +The `so101_leader_plugin` binary is a C++ plugin that is **not** part of the `isaacteleop` pip +package — you build it from the Isaac Teleop source tree. Follow +[Build Isaac Teleop from source](https://nvidia.github.io/IsaacTeleop/main/getting_started/build_from_source/index.html) +(in short, from your Isaac Teleop checkout: `cmake -B build && cmake --build build --parallel && +cmake --install build`); the build installs the plugins under `/install/plugins/`, so +the binary lands at `install/plugins/so101_leader/so101_leader_plugin` — the `--launch_plugin` path +below. See the plugin's own `README.md` (next to the binary) for its serial/calibration details. + +Point `--teleop.port` at the physical leader's serial port and `--launch_plugin` at that plugin +binary to have the script spawn it after CloudXR is up: + +```bash +python -m examples.isaac_teleop_to_so101.teleoperate --robot.type=so101_follower --robot.port=/dev/ttyACM0 \ + --robot.id=so101_follower_arm --teleop.type=so101_leader \ + --teleop.port=/dev/ttyACM1 --teleop.id=so101_leader_arm \ + --launch_plugin=/code/Teleop/install/plugins/so101_leader/so101_leader_plugin +``` + +(Note `so101_leader` here is the _Isaac_ leader, resolved against the Isaac Teleop device +registry, distinct from `lerobot-teleoperate`'s serial `so101_leader`.) When a `--teleop.port` is +set, the plugin's tick→radian calibration is inferred from `--teleop.id` and passed to the plugin +as its third positional arg — the LeRobot-format JSON at +`HF_LEROBOT_CALIBRATION/teleoperators/so_leader/.json`, the same file the serial SO-101 leader +uses (`lerobot-calibrate --teleop.type=so101_leader --teleop.id=`). If it is missing the script +warns and the plugin uses built-in defaults. Run `python -m examples.isaac_teleop_to_so101.teleoperate --help` for all flags. Its +startup safety contract: by default the follower is +slewed to the leader's first reading over `--align_duration` seconds (`--align=false` to skip) so +the arm does not snap when the mirror begins, and while the leader stream is stale the follower is +held at its measured pose. + +The URDF fetch uses `huggingface_hub` (already a LeRobot dependency) against the public +`lerobot/robot-urdfs` bucket, so it needs no login. It is cached under +`HF_LEROBOT_HOME/robot-urdfs/so101/`; delete that folder to force a re‑download. + +Then, in your headset: squeeze and hold the grip to engage, move the controller to drive the +arm, twist/tilt it to orient the wrist, and press the trigger to close the gripper +(proportionally — release to open). + +To record a dataset (not just teleoperate), use `record.py` in the same folder. It dispatches on +`--teleop.type` (`xr_controller` | `so101_leader`) exactly like `teleoperate.py`, so either device +can drive the follower, and it saves the commanded joints to a LeRobot dataset (`lerobot-record`-style +`--dataset.*` flags). See its module docstring for the full CLI and the keyboard recording shortcuts. + +## Important pipeline steps and options + +The clutch already produces an absolute base‑frame pose, so the processor side is a thin +**absolute‑pose** path — there is no frame remap, no delta accumulation, and no +`EEReferenceAndDelta` stage. + +- `MapXRControllerActionToRobotAction` is a stateless per‑frame mapping from the device output to + the IK input contract. It writes the absolute base‑frame position, encodes the absolute + orientation as a rotvec target, and inverts the closedness into a motor gripper target: + + ```python + action["ee.x"], action["ee.y"], action["ee.z"] = ee_pose[:3] # absolute, base frame [m] + action["ee.wx"], action["ee.wy"], action["ee.wz"] = orient_rotvec # orientation target (rotvec) + action["ee.gripper_pos"] = (1 - closedness) * 100 # motor units; SO-101 calibrates 100 = open + ``` + + The gripper polarity (`100 = open, 0 = closed`) is a hardware‑calibration convention in the source — flip it there if the jaw opens when it should close. + +- `EEBoundsAndSafety` clamps the EE to a workspace and rate‑limits per‑frame jumps. The clutch's + no‑teleport keeps frames small, so `max_ee_step_m` mostly catches transient controller tracking + glitches. The z floor is `0.0` (the table plane) so a stray target cannot drive the EE below the + table; x/y stay at the loose `[-1, 1]` m box. Set `raise_on_jump=False` so an over‑limit frame is + **clamped and warned** instead of raising — a crash mid‑loop would leave the arm uncontrolled: + + ```python + EEBoundsAndSafety( + end_effector_bounds={"min": [-1.0, -1.0, 0.0], "max": [1.0, 1.0, 1.0]}, + max_ee_step_m=0.10, + raise_on_jump=False, + ) + ``` + +- `InverseKinematicsEEToJoints(initial_guess_current_joints=False, orientation_weight=0.01)` solves + closed‑loop Placo IK. SO‑101 is a 5‑DOF arm, so the IK is position‑dominant; the small + `orientation_weight` lets it softly track the orientation target carried in `ee.w*` so the wrist + follows the hand, while the under‑determined roll stays partial by design. There is **no** + `GripperVelocityToJoint`: the absolute `ee.gripper_pos` is passed straight to `gripper.pos`. + `initial_guess_current_joints=False` warm‑starts each solve from the **previous IK solution** + rather than re‑seeding from the measured joints, so the joint trajectory stays continuous + frame‑to‑frame. Tune `orientation_weight` on hardware — too high fights position tracking, too + low ignores the orientation command. + +The example also gates safety at the loop level: after the startup reset slew (on by default — +pass `--reset_to_origin=false` to keep the arm where it is), it commands the robot **only while +the clutch is engaged**, and re‑sends the measured joints while disengaged, so releasing the +clutch freezes the arm in place. + +See the [Processors for Robots and Teleoperators](./processors_robots_teleop) guide for more on +adapting the pipeline to other robots. + +## Troubleshooting + +- **`ModuleNotFoundError: isaacteleop`** — the `isaacteleop` package is not installed in the + active environment. Re-run the install command at the top of this guide: + `uv pip install "isaacteleop[cloudxr,retargeters-lite]~=1.3.131"`. +- **No controllers found** — make sure the CloudXR runtime is running, the firewall ports are + whitelisted, and the headset is connected (see + [Set up CloudXR and connect a headset](#set-up-cloudxr-and-connect-a-headset) and the Isaac + Teleop [Quick Start](https://nvidia.github.io/IsaacTeleop/main/getting_started/quick_start.html)). +- **CloudXR auto-launch failed** — `connect()` raises a `RuntimeError` if the runtime does not + come up within its startup timeout. Check the launcher logs under `~/.cloudxr/logs`. Common + causes: the EULA was never accepted (run `python -m isaacteleop.cloudxr --accept-eula` once, + interactively — the auto-launch prompts on stdin and hangs headless), or the runtime is already + running externally (set `LEROBOT_CLOUDXR_SKIP_AUTOLAUNCH=1` or `auto_launch_cloudxr=False` to + skip the auto-launch). +- **Arm does not move** — the clutch is a deadman: you must hold the squeeze/grip past + `clutch_threshold`. Lower the threshold if your controller's squeeze is reported softly. +- **Motion feels misaligned** — confirm the headset/play space orientation. The controller stream + is rebased into the robot base frame by the `base_T_anchor` transform on `XRControllerConfig` + (default: standard OpenXR → robot axis convention); adjust it if your anchor frame differs. + +## Learn more + +NVIDIA Isaac Teleop documentation ([docs home](https://nvidia.github.io/IsaacTeleop/), +[GitHub](https://github.com/NVIDIA/IsaacTeleop)): + +- [Quick Start](https://nvidia.github.io/IsaacTeleop/main/getting_started/quick_start.html) — + install, run the CloudXR server, connect a headset, run a teleop example. +- [TeleopSession](https://nvidia.github.io/IsaacTeleop/main/getting_started/teleop_session.html) — + the session API `XRController` wraps. +- [Retargeting interface](https://nvidia.github.io/IsaacTeleop/main/references/retargeting/index.html) + and [architecture overview](https://nvidia.github.io/IsaacTeleop/main/overview/architecture.html) — + how source nodes and retargeters compose into a pipeline. +- [Build from source](https://nvidia.github.io/IsaacTeleop/main/getting_started/build_from_source/index.html) — + build `isaacteleop` (and its C++ plugins, including the `so101_leader` plugin used above) from a + local checkout. +- [System Requirements](https://nvidia.github.io/IsaacTeleop/main/references/requirements.html) and + the [CloudXR SDK docs](https://docs.nvidia.com/cloudxr-sdk) — supported platforms, GPUs, + CloudXR/OpenXR runtime versions, and headsets. diff --git a/examples/isaac_teleop_to_so101/README.md b/examples/isaac_teleop_to_so101/README.md new file mode 100644 index 000000000..a2493af54 --- /dev/null +++ b/examples/isaac_teleop_to_so101/README.md @@ -0,0 +1,131 @@ +# Isaac Teleop → SO-101 + +Teleoperate an SO-101/SO-100 follower arm — and record LeRobot datasets — with NVIDIA +[Isaac Teleop](https://github.com/NVIDIA/IsaacTeleop). Two input devices ship today: + +- **XR (VR) controller** (`--teleop.type=xr_controller`) — the controller's grip pose drives the + end-effector through a squeeze-to-engage clutch and LeRobot's Cartesian IK pipeline; the analog + trigger drives the gripper. +- **SO-101 leader arm** (`--teleop.type=so101_leader`) — a back-drivable leader arm mirrored 1:1 + onto the follower via Isaac Teleop's native `so101_leader` plugin (no clutch, no IK). + +The full narrative guide (how the clutch works, CloudXR setup, headset pairing, tuning, and +troubleshooting) is in the [LeRobot docs](https://huggingface.co/docs/lerobot/isaac_teleop) +(source: `docs/source/isaac_teleop.mdx`). This README is the canonical install and usage +reference. + +## Requirements + +- Linux workstation (see NVIDIA's + [system requirements](https://nvidia.github.io/IsaacTeleop/main/references/requirements.html) + for supported OS/GPU/headset combinations; `isaacteleop` publishes Linux wheels only). +- An SO-101 (or SO-100) follower arm, calibrated with `lerobot-calibrate`. +- For the XR device: a CloudXR-capable headset (e.g. Quest 3, Pico 4, Apple Vision Pro) on the + same network. +- For the leader device: a second, back-drivable SO-101 leader arm and the `so101_leader` plugin + binary built from the Isaac Teleop source tree (see + [Build from source](https://nvidia.github.io/IsaacTeleop/main/getting_started/build_from_source/index.html)). + +## Installation + +This example lives in the LeRobot repository and is not part of the `lerobot` pip package, so +work from a source checkout. From the repo root: + +```bash +# LeRobot with the extras this example uses: +# feetech - SO-101 serial motor bus +# kinematics - Placo IK solver (XR controller path) +# dataset - dataset recording (record.py) +# huggingface_hub >= 1.5 is needed by the automatic URDF fetch (Buckets API). +uv pip install -e ".[feetech,kinematics,dataset]" "huggingface_hub>=1.5" + +# Isaac Teleop from public PyPI. `cloudxr` brings the CloudXR runtime bindings; +# `retargeters-lite` is the scipy-based retargeter path that resolves on both +# x86_64 and ARM (the full `retargeters` extra does not resolve on aarch64). +uv pip install "isaacteleop[cloudxr,retargeters-lite]~=1.3.131" "scipy>=1.14" + +# Optional, x86_64 only: the full retargeter stack. +uv pip install "isaacteleop[retargeters]~=1.3.131" +``` + +One-time CloudXR EULA (the auto-launch prompts on stdin and would hang on a headless machine): + +```bash +python -m isaacteleop.cloudxr --accept-eula +``` + +## Usage + +Run everything from the repo root with `python -m` so the `examples` package resolves. + +### Teleoperate — XR controller + +```bash +python -m examples.isaac_teleop_to_so101.teleoperate \ + --robot.type=so101_follower \ + --robot.port=/dev/ttyACM0 \ + --robot.id=so101_follower_arm \ + --teleop.type=xr_controller +``` + +On startup the script launches the CloudXR runtime (~30 s), prints the workstation IP to enter in +the headset's CloudXR web client, waits for the controllers to stream, slews the arm to a reset +pose (`--reset_to_origin=false` to skip), and then: **hold the squeeze/grip** to engage, move the +controller to drive the arm, pull the trigger to close the gripper. Releasing the squeeze freezes +the arm. The SO-101 URDF is fetched automatically from the `lerobot/robot-urdfs` Hugging Face +bucket into the LeRobot cache on first run. + +To customize the reset pose: back-drive the arm to the pose you want, then + +```bash +python -m examples.isaac_teleop_to_so101.override_reset_pose --port /dev/ttyACM0 --id so101_follower_arm +``` + +which writes it to `HF_LEROBOT_HOME/reset_poses//.json`; runs with the same +`--robot.id` use it automatically. + +### Teleoperate — SO-101 leader arm + +```bash +python -m examples.isaac_teleop_to_so101.teleoperate \ + --robot.type=so101_follower --robot.port=/dev/ttyACM0 --robot.id=so101_follower_arm \ + --teleop.type=so101_leader --teleop.port=/dev/ttyACM1 --teleop.id=so101_leader_arm \ + --launch_plugin=/path/to/IsaacTeleop/install/plugins/so101_leader/so101_leader_plugin +``` + +The follower is first slewed to the leader's pose over `--align_duration` seconds +(`--align=false` to skip), then mirrors it 1:1. The plugin reuses the serial leader's calibration +(`HF_LEROBOT_CALIBRATION/teleoperators/so_leader/.json`). + +### Record a dataset + +`record.py` takes the same `--robot.*`/`--teleop.*`/loop flags plus `lerobot-record`-style +`--dataset.*` flags: + +```bash +python -m examples.isaac_teleop_to_so101.record \ + --robot.type=so101_follower --robot.port=/dev/ttyACM0 --robot.id=so101_follower_arm \ + --teleop.type=xr_controller \ + --robot.cameras="{ front: {type: opencv, index_or_path: 0, width: 640, height: 480, fps: 30}}" \ + --dataset.repo_id=/ \ + --dataset.single_task="Pick up the cube" \ + --dataset.num_episodes=3 --dataset.episode_time_s=20 --dataset.reset_time_s=5 +``` + +Keyboard shortcuts (terminal-first, so they work over SSH): **Right/n** end episode early, +**Left/r** re-record, **Esc/q** stop after the current episode. + +Run either script with `--help` for all flags. + +## Layout + +``` +isaac_teleop/ device library: session lifecycle (base.py), XRController, + SO101LeaderArm, Clutch, configs, and the XR→IK processor step +common.py shared loop infra: device bundles, clutch/IK pipeline wiring, + reset/align slews, URDF fetch, keyboard listener +teleoperate.py teleoperation CLI (device selected via --teleop.type) +record.py dataset-recording CLI (same device selection + --dataset.*) +override_reset_pose.py save the current joints as the per-arm reset pose +default.env CloudXR device-profile overrides passed to the launcher +``` diff --git a/examples/isaac_teleop_to_so101/__init__.py b/examples/isaac_teleop_to_so101/__init__.py new file mode 100644 index 000000000..a05119e31 --- /dev/null +++ b/examples/isaac_teleop_to_so101/__init__.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Isaac Teleop -> SO-101 example package.""" diff --git a/examples/isaac_teleop_to_so101/common.py b/examples/isaac_teleop_to_so101/common.py new file mode 100644 index 000000000..80f56bae4 --- /dev/null +++ b/examples/isaac_teleop_to_so101/common.py @@ -0,0 +1,650 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Shared device + control-loop infrastructure for the Isaac Teleop -> SO-101 examples. + +Consumed by ``teleoperate.py`` and ``record.py``, which both build a per-device +:class:`Device` bundle and run the same loop: read -> (maybe command) -> hold-when-idle -> +sleep. A :class:`Device` bundles three closures: ``compute(obs) -> RobotAction | None`` +(``None`` = hold at the measured pose while idle), ``startup``, and ``cleanup``. The devices: + +* ``xr_controller`` — a thin :class:`XRController` whose raw grip pose an in-loop + :class:`Clutch` turns into an EE target for LeRobot's Cartesian IK pipeline. +* ``so101_leader`` — a back-drivable leader arm mirrored 1:1 into the follower. + +Requires the ``isaacteleop`` package and an OpenXR runtime (install instructions in this +folder's ``README.md``). User-facing guide: ``docs/source/isaac_teleop.mdx``. +""" + +import json +import logging +import socket +import subprocess +import sys +import time +from collections.abc import Callable +from contextlib import suppress +from dataclasses import dataclass +from importlib.resources import files +from pathlib import Path +from typing import Protocol + +import numpy as np + +from lerobot.model.kinematics import RobotKinematics +from lerobot.processor import ( + RobotProcessorPipeline, + robot_action_observation_to_transition, + transition_to_robot_action, +) +from lerobot.robots import RobotConfig, make_robot_from_config +from lerobot.robots.so_follower import SOFollowerConfig # noqa: F401 (registers so101_follower) +from lerobot.robots.so_follower.robot_kinematic_processor import ( + EEBoundsAndSafety, + InverseKinematicsEEToJoints, +) +from lerobot.types import RobotAction, RobotObservation +from lerobot.utils.constants import HF_LEROBOT_CALIBRATION, HF_LEROBOT_HOME, TELEOPERATORS +from lerobot.utils.robot_utils import precise_sleep + +from .isaac_teleop import ( + Clutch, + IsaacTeleopConfig, + MapXRControllerActionToRobotAction, + SO101LeaderArm, + SO101LeaderArmConfig, + XRController, +) + +# Fixed rate [Hz] for the teleoperate loop and the pre-loop slews / connect-wait poll sleeps. +FPS = 30 + +# CloudXR device-profile env file passed to the launcher (see default.env in this package). +CLOUDXR_ENV_FILE = str(files(__package__) / "default.env") + + +class LoopConfig(Protocol): + """Structural type for the loop/launch knobs ``build_device`` and the ``setup_*`` read. + + Both ``TeleoperateConfig`` and ``RecordConfig`` satisfy it, keeping ``common`` decoupled + from either entry point's concrete config. + """ + + teleop: IsaacTeleopConfig + robot: RobotConfig + launch_plugin: str | None + reset_to_origin: bool + reset_duration: float + align: bool + align_duration: float + + +# Per-device bundle consumed by the shared loop. ``compute`` returns None to mean +# "idle -> hold at the measured pose"; ``startup`` warms up; ``cleanup`` reaps/disconnects. +@dataclass(frozen=True) +class Device: + compute: Callable[[RobotObservation | None], RobotAction | None] + startup: Callable[[], None] + cleanup: Callable[[], None] + + +def hold_action(obs: RobotObservation, motor_names: list[str]) -> dict[str, float]: + """Re-send the measured joints — the explicit hold when a device is idle.""" + return {f"{name}.pos": float(obs[f"{name}.pos"]) for name in motor_names} + + +class HoldLatch: + """Resolve the per-frame action, holding one LATCHED pose while the device is idle. + + Re-sending the freshly measured joints on every idle frame would ratchet the arm + downward: under gravity the P-only servo settles below its goal by a steady-state + error, so each re-command of the measurement lowers the goal by that error again. + Latching the target once on the active->idle transition holds a fixed pose instead. + """ + + def __init__(self, motor_names: list[str]): + self._motor_names = motor_names + self._held: dict[str, float] | None = None + + def resolve(self, action: RobotAction | None, obs: RobotObservation) -> RobotAction: + """Pass through an active action (clearing the latch); latch + hold when idle.""" + if action is not None: + self._held = None + return action + if self._held is None: + self._held = hold_action(obs, self._motor_names) + return self._held + + +def slew( + robot, + motor_names: list[str], + target_fn: Callable[[], dict[str, float]], + duration_s: float, +) -> None: + """Linearly slew all joints from their current measured pose toward a target. + + ``target_fn`` is called EACH step, so the leader can pass a live re-read (landing on its + current pose at ``alpha == 1`` for a continuous handoff) while XR passes a constant. + """ + obs = robot.get_observation() + start = {name: float(obs[f"{name}.pos"]) for name in motor_names} + n_steps = max(1, int(duration_s * FPS)) + for step in range(1, n_steps + 1): + alpha = step / n_steps + target = target_fn() + action = {f"{name}.pos": start[name] + alpha * (target[name] - start[name]) for name in motor_names} + robot.send_action(action) + precise_sleep(1.0 / FPS) + + +# ============================================================================ +# XR controller device +# ============================================================================ + +# Per-frame EE rate limit [m]. With raise_on_jump=False, EEBoundsAndSafety clamps an +# over-limit step instead of raising, absorbing a tracking glitch as one slow frame. At +# FPS=30, 0.1 m/frame caps EE speed at ~3 m/s. (end_effector_bounds clips the absolute target.) +MAX_EE_STEP_M = 0.1 + +# Soft-orientation IK weight: small but nonzero so the wrist follows the hand while position +# dominates (the 5-DOF SO-101 cannot realize an arbitrary orientation). 0.0 = position-only. +IK_ORIENTATION_WEIGHT = 0.01 + + +def _ensure_so101_urdf() -> str: + """Return the cached SO-101 URDF path, fetching the ``so101`` folder (URDF + meshes) from + the public ``lerobot/robot-urdfs`` HF bucket into the LeRobot cache on first use.""" + dest_dir = HF_LEROBOT_HOME / "robot-urdfs" / "so101" + urdf_path = dest_dir / "so101_new_calib.urdf" + # Completeness marker written only after a FULL sync: the URDF file alone is not a + # completeness signal (an interrupted first sync can leave the meshes it references + # missing, which the URDF's mere existence would then hide forever). Re-syncing is + # idempotent and repairs a partial cache; delete the folder to force a re-download. + marker = dest_dir / ".sync_complete" + if not marker.exists(): + from huggingface_hub import sync_bucket + + sync_bucket("hf://buckets/lerobot/robot-urdfs/so101", str(dest_dir), quiet=True) + marker.touch() + return str(urdf_path) + + +# Default duration [s] for the startup reset-to-origin slew. +RESET_DURATION_S = 5.0 + +# Optional cached file written by override_reset_pose.py. When present it takes priority over RESET_ORIGIN_DEG. +RESET_POSE_FILE = str(HF_LEROBOT_HOME / "reset_poses" / "{robot_name}" / "{robot_id}.json") + +# Reset target in each motor's native units (arm joints in degrees, gripper RANGE_0_100, +# 100 = open). An empirically comfortable pose (elbow/wrist bent) avoiding the singularity of +# a fully-extended arm; assumes standard calibration. Override per-arm via override_reset_pose.py. +RESET_ORIGIN_DEG: dict[str, float] = { + "shoulder_pan": -4.0, + "shoulder_lift": -103.0, + "elbow_flex": 97.0, + "wrist_flex": 78.0, + "wrist_roll": -65.0, + "gripper": 0.0, +} + + +def _load_reset_target(reset_pose_file: Path, motor_names: list[str]) -> dict[str, float]: + """Return reset targets: the saved reset pose if present, else RESET_ORIGIN_DEG.""" + if reset_pose_file.exists(): + saved = json.loads(reset_pose_file.read_text()) + # Fill any missing motors from the fallback dict. + return {name: float(saved.get(name, RESET_ORIGIN_DEG.get(name, 0.0))) for name in motor_names} + return {name: RESET_ORIGIN_DEG.get(name, 0.0) for name in motor_names} + + +# CloudXR web client URL opened in the headset (Isaac Teleop quick start, step 5). +_CLOUDXR_WEB_CLIENT_URL = "https://nvidia.github.io/IsaacTeleop/client" +# WSS-proxy / self-signed-cert port the operator accepts in-browser before connecting. +_CLOUDXR_WSS_PORT = 48322 +# How often to re-print the connection hint while waiting for the headset [s]. +_XR_CONNECT_REMINDER_S = 15.0 +# Virtual / bridge / USB-gadget interfaces a headset can't reach over the network — skip +# by name prefix (``docker0``, compose ``br-*``, ``veth*``, libvirt ``virbr*``, and the +# Tegra USB device-mode bridge ``l4tbr0``). +_SKIP_IFACE_PREFIXES = ("docker", "br-", "veth", "virbr", "l4tbr") + + +def _primary_ipv4() -> str | None: + """The workstation's primary outbound IPv4, via the UDP-socket trick (``connect()`` on a + datagram socket selects the egress interface without sending packets).""" + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: + try: + s.connect(("8.8.8.8", 80)) + return s.getsockname()[0] + except OSError: + return None + + +def _candidate_ipv4s() -> list[tuple[str, str]]: + """Return ``[(interface, ipv4), ...]`` the headset might reach this workstation at. + + Lists each interface's IPv4 via ``psutil`` (dropping loopback, link-local, and the + virtual/bridge interfaces in ``_SKIP_IFACE_PREFIXES``), primary outbound first. Falls + back to just the primary IP when ``psutil`` is unavailable. + """ + primary = _primary_ipv4() + found: list[tuple[str, str]] = [] + try: + import psutil + + for iface, addrs in psutil.net_if_addrs().items(): + if iface.startswith(_SKIP_IFACE_PREFIXES): + continue + for addr in addrs: + if addr.family != socket.AF_INET: + continue + ip = addr.address + if ip.startswith("127.") or ip.startswith("169.254."): + continue + found.append((iface, ip)) + except Exception: + if primary: + found.append(("default", primary)) + found.sort(key=lambda t: t[1] != primary) # primary outbound interface first + return found + + +def _print_xr_connect_help() -> None: + """Print how to connect the headset to this workstation over CloudXR.""" + ips = _candidate_ipv4s() + print("\n" + "=" * 76) + print("Connect your XR headset to this workstation over NVIDIA CloudXR:") + print(f" 1. In the headset, open the CloudXR web client: {_CLOUDXR_WEB_CLIENT_URL}") + print(" 2. Enter this workstation's IP address:") + if ips: + for iface, ip in ips: + print(f" {ip:<15} ({iface})") + if len(ips) > 1: + print(" (use the address on the same network as your headset)") + else: + print(" ") + print(f" 3. Accept the self-signed cert at https://:{_CLOUDXR_WSS_PORT}/ , then Connect.") + print("=" * 76 + "\n") + + +def _wait_for_xr_controller(teleop_device: XRController) -> None: + """Block until the XR controller is tracked, polling ``get_action()`` and re-printing a + reminder every ``_XR_CONNECT_REMINDER_S``. User-paced; ``Ctrl-C`` aborts (no hard timeout). + """ + _print_xr_connect_help() + print("Waiting for the headset controllers to start streaming… (Ctrl-C to abort)") + last_reminder = time.time() + while True: + teleop_device.get_action() # steps the session; updates is_tracking + if teleop_device.is_tracking: + print("Headset connected — controllers are streaming.") + return + if time.time() - last_reminder >= _XR_CONNECT_REMINDER_S: + print("…still waiting for the headset to connect (Ctrl-C to abort).") + last_reminder = time.time() + time.sleep(1.0 / FPS) + + +def setup_xr(cfg: LoopConfig, robot, motor_names: list[str]) -> Device: + """Build the XR controller device bundle (clutch + soft-orientation IK pipeline).""" + kinematics_solver = RobotKinematics( + urdf_path=_ensure_so101_urdf(), + target_frame_name="gripper_frame_link", + joint_names=motor_names, + ) + + teleop_config = cfg.teleop # XRControllerConfig (selected via --teleop.type=xr_controller) + teleop_device = XRController(teleop_config) + + # The clutch (below) turns the raw grip pose into an absolute base-frame ee_pose; this + # pipeline maps it to joint targets: rename -> bounds/rate-limit -> IK. + xr_to_robot_joints_processor = RobotProcessorPipeline[tuple[RobotAction, RobotObservation], RobotAction]( + steps=[ + MapXRControllerActionToRobotAction(), + # raise_on_jump=False: an over-limit step (e.g. a tracking glitch) is clamped + + # warned instead of raised, since a crash mid-loop would leave the arm uncontrolled. + # z floor 0.0 keeps a stray target above the table; x/y stay at a loose [-1,1]m box. + EEBoundsAndSafety( + end_effector_bounds={"min": [-1.0, -1.0, 0.0], "max": [1.0, 1.0, 1.0]}, + max_ee_step_m=MAX_EE_STEP_M, + raise_on_jump=False, + ), + # initial_guess_current_joints=False: warm-start from the previous IK solution so + # the joint trajectory stays continuous frame-to-frame. + InverseKinematicsEEToJoints( + kinematics=kinematics_solver, + motor_names=motor_names, + initial_guess_current_joints=False, + orientation_weight=IK_ORIENTATION_WEIGHT, + ), + ], + to_transition=robot_action_observation_to_transition, + to_output=transition_to_robot_action, + ) + + # The clutch is built in startup() (after the optional reset slew, seeded from the + # post-slew MEASURED pose) and shared with compute() via nonlocal. + clutch: Clutch | None = None + prev_enabled = False + + def startup() -> None: + nonlocal clutch + # Connect and wait for the operator to don the headset BEFORE moving the arm, so the + # reset slew happens while they are watching in VR. + teleop_device.connect() + if not teleop_device.is_connected: + raise ValueError("Teleop is not connected!") + _wait_for_xr_controller(teleop_device) + + if cfg.reset_to_origin: + reset_pose_file = Path(RESET_POSE_FILE.format(robot_name=robot.name, robot_id=robot.id)) + target = _load_reset_target(reset_pose_file, motor_names) + source = str(reset_pose_file) if reset_pose_file.exists() else "hardcoded defaults" + print(f"Reset target source: {source}") + print(f"Resetting to origin over {cfg.reset_duration:.1f} s…") + slew(robot, motor_names, lambda: target, cfg.reset_duration) + print("Reset complete.") + + # Seed the clutch home from the arm's measured pose (FK of the current joints) so the + # first engage is jump-free, whether or not a reset slew ran. + obs0 = robot.get_observation() + q_measured_deg = np.array([float(obs0[f"{name}.pos"]) for name in motor_names], dtype=float) + home_base_T_ee = kinematics_solver.forward_kinematics(q_measured_deg) # noqa: N806 + clutch = Clutch(home_base_T_ee) + + print("Starting teleop loop. Squeeze and move the controller to teleoperate the robot...") + + def compute(robot_obs: RobotObservation | None) -> RobotAction | None: + nonlocal prev_enabled + if clutch is None: # set in startup(), which runs before compute() + raise RuntimeError("compute() called before startup(); the clutch is not initialized") + xr_action = teleop_device.get_action() + grip_pos = np.asarray(xr_action["grip_pos"], dtype=float) + grip_quat = np.asarray(xr_action["grip_quat"], dtype=float) + squeeze = float(xr_action["squeeze"]) + trigger = float(xr_action["trigger"]) + enabled = squeeze > teleop_config.clutch_threshold + + # On the engage edge, latch the clutch home at the arm's MEASURED EE pose (FK of + # the live joints) and the controller origin so the per-frame delta starts at zero. + # Latching the last commanded pose instead would snap the arm back to it at full + # servo speed if the arm moved while disengaged (gravity sag, external contact). + is_engage_frame = enabled and not prev_enabled + if is_engage_frame: + q_measured = np.array([float(robot_obs[f"{name}.pos"]) for name in motor_names], dtype=float) + measured_base_T_ee = kinematics_solver.forward_kinematics(q_measured) # noqa: N806 + clutch.engage(grip_pos, grip_quat, measured_base_T_ee=measured_base_T_ee) + # Re-anchor the pipeline state at the measured pose as well: EEBoundsAndSafety's + # rate limiter and the IK warm start otherwise still reference the stale + # pre-disengage command and would fight the fresh home for several frames. + xr_to_robot_joints_processor.reset() + prev_enabled = enabled + + # SAFETY GATE: command the robot ONLY while the clutch is engaged; otherwise return + # None so the loop holds the measured joints (releasing the clutch freezes the arm). + if not enabled: + return None + + # Rebase the raw grip pose onto the EE, then run the pipeline. closedness = trigger. + ee_pos, ee_quat = clutch.rebase(grip_pos, grip_quat) + ee_action = { + "ee_pose": np.concatenate([ee_pos, ee_quat]).astype(np.float32), + "closedness": trigger, + } + return xr_to_robot_joints_processor((ee_action, robot_obs)) + + return Device(compute=compute, startup=startup, cleanup=teleop_device.disconnect) + + +# ============================================================================ +# SO-101 leader arm device +# ============================================================================ + +# Default duration [s] for the startup alignment slew (follower current -> leader first pose). +ALIGN_DURATION_S = 3.0 + +# How long to wait for the leader plugin to start streaming before aligning / looping. +LEADER_WARMUP_TIMEOUT_S = 20.0 + +# The plugin converts the leader's servo ticks to radians, so it reuses the serial SO-101 +# leader's calibration, stored by lerobot-calibrate under SO101Leader.name == "so_leader". +SO_LEADER_CALIBRATION_NAME = "so_leader" + + +def _leader_calibration_path(cfg: LoopConfig) -> Path | None: + """Infer the calibration JSON the launched plugin should read, or None. + + Path convention: ``HF_LEROBOT_CALIBRATION / teleoperators / so_leader / {--teleop.id}.json`` + (or ``--teleop.calibration_dir`` if set). Returns None (plugin falls back to defaults) when + it does not exist, warning if an id was given, or when no ``--teleop.id`` is set. + """ + if not cfg.teleop.id: + return None + calib_dir = cfg.teleop.calibration_dir or ( + HF_LEROBOT_CALIBRATION / TELEOPERATORS / SO_LEADER_CALIBRATION_NAME + ) + calib_path = Path(calib_dir) / f"{cfg.teleop.id}.json" + if calib_path.is_file(): + return calib_path + print( + f"WARNING: no leader calibration at {calib_path}; the plugin will use built-in defaults. " + f"Calibrate with the serial leader (`lerobot-calibrate --teleop.type=so101_leader " + f"--teleop.id={cfg.teleop.id}`) or the plugin's `calibrate` subcommand." + ) + return None + + +def _wait_for_leader(teleop: SO101LeaderArm, timeout_s: float) -> dict[str, float]: + """Poll the leader until it streams a live frame; return that frame's ``{joint}.pos``. + + Raises ``SystemExit`` if no live frame arrives within ``timeout_s`` (plugin not pushing, + wrong ``--teleop.collection_id``, or CloudXR not up). + """ + print(f"Waiting up to {timeout_s:.0f}s for the so101_leader plugin to stream…") + deadline = time.time() + timeout_s + while time.time() < deadline: + action = teleop.get_action() + if teleop.is_tracking: + print("Leader is streaming.") + return action + time.sleep(1.0 / FPS) + raise SystemExit( + f"FAILED: leader did not stream within {timeout_s:.0f}s. Is the so101_leader plugin " + "running and pushing (check --teleop.collection_id)? Is CloudXR up?" + ) + + +def _maybe_launch_plugin(cfg: LoopConfig) -> subprocess.Popen | None: + """Spawn the so101_leader plugin if ``--launch_plugin `` was given (after connect()).""" + if cfg.launch_plugin is None: + return None + if not Path(cfg.launch_plugin).exists(): + raise SystemExit( + f"plugin binary not found: {cfg.launch_plugin} (build it in the IsaacTeleop repo first)" + ) + leader_port = cfg.teleop.port # SO101LeaderArmConfig.port, forwarded to the plugin + backend = f"leader on {leader_port}" if leader_port else "synthetic trajectory" + print(f"launching plugin: {cfg.launch_plugin} ({backend})") + # Positional args: [device_path] [collection_id] [calibration_file]. Empty device_path -> + # synthetic backend. Calibration (only real hardware needs it) is appended when a port is set. + argv = [cfg.launch_plugin, leader_port, cfg.teleop.collection_id] + if leader_port: + calib_path = _leader_calibration_path(cfg) + if calib_path is not None: + argv.append(str(calib_path)) + print(f" leader calibration: {calib_path}") + # Spawned after connect() so it inherits the CloudXR runtime env (XR_RUNTIME_JSON, ...). + proc = subprocess.Popen(argv) + time.sleep(1.5) # let it create its OpenXR session and start pushing + return proc + + +def setup_leader(cfg: LoopConfig, robot, motor_names: list[str]) -> Device: + """Build the SO-101 leader arm device bundle (1:1 joint mirror).""" + teleop_config = cfg.teleop # SO101LeaderArmConfig (selected via --teleop.type=so101_leader) + teleop = SO101LeaderArm(teleop_config) + + plugin_proc: subprocess.Popen | None = None + + def startup() -> None: + nonlocal plugin_proc + # connect() auto-launches CloudXR (unless opted out); spawn the plugin AFTER so it + # inherits the runtime env. The plugin is reaped in cleanup(). + teleop.connect() + plugin_proc = _maybe_launch_plugin(cfg) + + if not teleop.is_connected: + raise ValueError("Teleop is not connected!") + + # Block until the leader streams a live frame (clear error if it never does). + _wait_for_leader(teleop, LEADER_WARMUP_TIMEOUT_S) + + if cfg.align: + print(f"Aligning follower to leader over {cfg.align_duration:.1f}s…") + + # Re-read the live leader pose once per step so alpha=1 lands on its current pose + # from a single coherent frame. + def _leader_target() -> dict[str, float]: + leader_now = teleop.get_action() + return {name: float(leader_now[f"{name}.pos"]) for name in motor_names} + + slew(robot, motor_names, _leader_target, cfg.align_duration) + print("Alignment complete.") + + print( + "Starting joint-mirror loop. Back-drive the leader to teleoperate the follower… (Ctrl-C to stop)" + ) + + def compute(robot_obs: RobotObservation | None) -> RobotAction | None: + leader_action = teleop.get_action() + # Hold the follower at its measured pose when the leader drops out (stale stream) + # rather than commanding a possibly-old target. + if not teleop.is_tracking: + return None + return leader_action + + def cleanup() -> None: + # A plugin-reaping failure must not skip the session disconnect (and vice versa + # the disconnect runs after the plugin stops pushing on it). + try: + if plugin_proc is not None: + plugin_proc.terminate() + try: + plugin_proc.wait(timeout=5) + except subprocess.TimeoutExpired: + plugin_proc.kill() + finally: + teleop.disconnect() + + return Device(compute=compute, startup=startup, cleanup=cleanup) + + +# ============================================================================ +# Shared setup +# ============================================================================ + + +def build_device(cfg: LoopConfig) -> tuple: + """Connect the follower, build the selected Isaac device, and run its pre-loop startup. + + Connects the follower FIRST (so the startup slew / clutch-home seed can read live joints), + dispatches on ``--teleop.type``, then runs ``device.startup()`` before returning. On any + failure after ``connect()`` the follower is disconnected so the connection never leaks. + + Returns ``(robot, device, motor_names)``. + """ + # Default the CloudXR input profile to this example's default.env unless the user overrode + # it via --teleop.cloudxr_env_file. + if cfg.teleop.cloudxr_env_file is None: + cfg.teleop.cloudxr_env_file = CLOUDXR_ENV_FILE + + # SO-101/SO-100 only (both share the SO-101 URDF), reject other followers. + supported_robots = {"so101_follower", "so100_follower"} + if cfg.robot.type not in supported_robots: + raise ValueError( + f"This example only supports SO-101/SO-100 followers ({sorted(supported_robots)}), " + f"but got --robot.type={cfg.robot.type}." + ) + + # The degree-based pipeline relies on --robot.use_degrees (default True). + robot = make_robot_from_config(cfg.robot) + # Connect FIRST so the startup slew and clutch-home seed can read live joints. + robot.connect() + # Everything after connect() can fail; this runs outside the callers' try/finally, so + # disconnect the follower on any failure to avoid leaking the connection. + device: Device | None = None + try: + # Joint names in action order, read from {name}.pos action features (robot-agnostic). + motor_names = [key.removesuffix(".pos") for key in robot.action_features if key.endswith(".pos")] + + if isinstance(cfg.teleop, SO101LeaderArmConfig): + device = setup_leader(cfg, robot, motor_names) + else: + device = setup_xr(cfg, robot, motor_names) + + device.startup() + except BaseException: + # Reap a partially-started device, then always disconnect the follower. + if device is not None: + with suppress(Exception): + device.cleanup() + robot.disconnect() + raise + + return robot, device, motor_names + + +# ============================================================================ +# Keyboard control +# ============================================================================ + + +def init_keyboard_listener(): + """Recording shortcuts, terminal-first so they work over SSH. + + Whenever stdin is a TTY we use the stdlib :class:`TerminalKeyListener` directly rather + than upstream's pynput-first :func:`init_keyboard_listener`, whose global listener would + capture the workstation console instead of this (often SSH) terminal. With no TTY we defer + to upstream (pynput on a GUI, else headless no-op). + """ + if not (sys.stdin is not None and sys.stdin.isatty()): + from lerobot.utils.keyboard_input import init_keyboard_listener as _upstream + + return _upstream() + + from lerobot.utils.keyboard_input import TerminalKeyListener, apply_recording_control + + events = {"exit_early": False, "rerecord_episode": False, "stop_recording": False} + + # n/r/q are the arrow/Esc equivalents that survive escape-sequence splitting over laggy + # SSH/VNC links. Case-insensitive so Shift+letter still works. + def on_key(name: str) -> None: + key = name.lower() + if key in ("right", "n"): + apply_recording_control("right", events) + elif key in ("left", "r"): + apply_recording_control("left", events) + elif key in ("esc", "q"): + apply_recording_control("esc", events) + + listener = TerminalKeyListener(on_key) + listener.start() + logging.info( + "Keyboard control via terminal — keep this terminal focused: " + "Right/n = end episode early, Left/r = re-record, Esc/q = stop." + ) + return listener, events diff --git a/examples/isaac_teleop_to_so101/default.env b/examples/isaac_teleop_to_so101/default.env new file mode 100644 index 000000000..6f815b66e --- /dev/null +++ b/examples/isaac_teleop_to_so101/default.env @@ -0,0 +1,21 @@ +# CloudXR device-profile overrides for the Isaac Teleop XR -> SO-101 example. +# +# Passed to isaacteleop's CloudXRLauncher as `env_config` (via +# XRControllerConfig.cloudxr_env_file). Format: KEY=value, one per line; `#` +# comments and blank lines ignored; $VARS / ~ expanded. See +# isaacteleop/cloudxr/env_config.py::_load_env_file. +# +# Runtime-resolved keys (XR_RUNTIME_JSON, XRT_NO_STDIN, NV_CXR_RUNTIME_DIR, +# NV_CXR_OUTPUT_DIR) are reserved and ignored if set here. + +# Transport profile the runtime advertises (CloudXR default: auto-webrtc). +# "Quest3" also covers the Pico 4. Other values: auto-native, AppleVisionPro. +NV_DEVICE_PROFILE=Quest3 + +# Input device discovery channels (both default to true; pinned for clarity). +NV_CXR_ENABLE_PUSH_DEVICES=true +NV_CXR_ENABLE_TENSOR_DATA=true + +# Runtime logs to ~/.cloudxr/logs — helps debug connection issues +# (e.g. "Failed to get OpenXR system: -35"). +NV_CXR_FILE_LOGGING=true diff --git a/examples/isaac_teleop_to_so101/isaac_teleop/__init__.py b/examples/isaac_teleop_to_so101/isaac_teleop/__init__.py new file mode 100644 index 000000000..17d9919b3 --- /dev/null +++ b/examples/isaac_teleop_to_so101/isaac_teleop/__init__.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""NVIDIA Isaac Teleop teleoperators for LeRobot. + +Each input device is an :class:`IsaacTeleopTeleoperator` subclass: :class:`XRController` +(XR/VR controller) and :class:`SO101LeaderArm` (back-drivable SO-101 leader arm) ship today. +""" + +from .base import IsaacTeleopTeleoperator +from .clutch import Clutch +from .config_isaac_teleop import IsaacTeleopConfig, SO101LeaderArmConfig, XRControllerConfig +from .teleop_so101_leader_arm import SO101LeaderArm, leader_joints_to_robot_action +from .teleop_xr_controller import XRController +from .xr_controller_processor import MapXRControllerActionToRobotAction + +__all__ = [ + "Clutch", + "IsaacTeleopConfig", + "IsaacTeleopTeleoperator", + "MapXRControllerActionToRobotAction", + "SO101LeaderArm", + "SO101LeaderArmConfig", + "XRController", + "XRControllerConfig", + "leader_joints_to_robot_action", +] diff --git a/examples/isaac_teleop_to_so101/isaac_teleop/base.py b/examples/isaac_teleop_to_so101/isaac_teleop/base.py new file mode 100644 index 000000000..3a4e5c585 --- /dev/null +++ b/examples/isaac_teleop_to_so101/isaac_teleop/base.py @@ -0,0 +1,282 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Shared base for NVIDIA Isaac Teleop-backed LeRobot teleoperators. + +Isaac Teleop is a multi-modal framework: a single ``TeleopSession`` can be driven by +XR controllers, hand tracking, Manus gloves, etc. Each modality is a +:class:`Teleoperator` subclass in its own ``teleop_.py``. + +:class:`IsaacTeleopTeleoperator` owns what those devices share — the session +lifecycle, the per-step staleness/worker-health guard, and the no-op calibration +tracking devices need. A concrete device implements :meth:`_build_pipeline` (its +retargeting graph) and :meth:`get_action` (usually via :meth:`_step`). + +``isaacteleop`` is an optional NVIDIA dependency (install instructions in the example's +``README.md``); its imports are guarded behind an availability check at module top, so this +module imports without it and constructing a device fails fast with install instructions. +""" + +from __future__ import annotations + +import abc +import logging +import os +from collections.abc import Mapping +from pathlib import Path +from typing import TYPE_CHECKING, Any + +from lerobot.teleoperators.teleoperator import Teleoperator +from lerobot.utils.import_utils import is_package_available + +from .config_isaac_teleop import IsaacTeleopConfig + +_isaacteleop_available = is_package_available("isaacteleop") + +if TYPE_CHECKING or _isaacteleop_available: + from isaacteleop.cloudxr import CloudXRLauncher + from isaacteleop.retargeting_engine.interface import ( + ExecutionEvents, + ExecutionState, + GraphExecutable, + RetargeterIO, + ) + from isaacteleop.teleop_session_manager import TeleopSession, TeleopSessionConfig +else: + CloudXRLauncher = None + ExecutionEvents = None + ExecutionState = None + GraphExecutable = None + RetargeterIO = None + TeleopSession = None + TeleopSessionConfig = None + +logger = logging.getLogger(__name__) + +# Gripper closedness [0, 1] -> SO-101 follower motor units [0, 100] (RANGE_0_100, 100 = OPEN). +# Shared by the XR processor and leader device, which invert via ``pos = (1 - c) * SCALE``. +_GRIPPER_MOTOR_SCALE = 100.0 + + +def _require_isaacteleop() -> None: + """Fail fast with install pointers when the optional ``isaacteleop`` package is missing.""" + if not _isaacteleop_available: + raise ImportError( + "The 'isaacteleop' package is required for Isaac Teleop devices but is not " + "installed. See examples/isaac_teleop_to_so101/README.md for install instructions." + ) + + +class IsaacTeleopTeleoperator(Teleoperator): + """Abstract base for teleoperators backed by an Isaac Teleop ``TeleopSession``. + + Owns the session lifecycle and the per-step health guard; subclasses supply + :meth:`_build_pipeline` and :meth:`get_action`. + """ + + config_class = IsaacTeleopConfig + + def __init__(self, config: IsaacTeleopConfig): + _require_isaacteleop() + super().__init__(config) + self.config: IsaacTeleopConfig = config + self._session: TeleopSession | None = None + self._cloudxr_launcher: CloudXRLauncher | None = None + + # ------------------------------------------------------------------ + # Pipeline construction (device override point) + # ------------------------------------------------------------------ + + @abc.abstractmethod + def _build_pipeline(self) -> GraphExecutable: + """Build this device's retargeting pipeline (the ``GraphExecutable`` for + ``TeleopSessionConfig.pipeline``). Called once in :meth:`connect`; its output + keys must match what :meth:`get_action` unpacks. + """ + raise NotImplementedError + + # ------------------------------------------------------------------ + # Lifecycle (shared) + # ------------------------------------------------------------------ + + @property + def is_connected(self) -> bool: + return self._session is not None + + @property + def is_calibrated(self) -> bool: + return True # Tracking devices are self-calibrating. + + def calibrate(self) -> None: + pass + + def configure(self) -> None: + pass + + def connect(self, calibrate: bool = True) -> None: + """Auto-launch the CloudXR runtime (unless opted out) and open the session. + + The CloudXR launch blocks ~30s and, on the first run, prompts on stdin for the + EULA (accept once via ``python -m isaacteleop.cloudxr --accept-eula``). Opt out + when CloudXR runs externally via ``config.auto_launch_cloudxr=False`` or + ``LEROBOT_CLOUDXR_SKIP_AUTOLAUNCH=1`` (env var wins). + """ + if self._session is not None: + raise RuntimeError("Already connected. Call disconnect() first.") + + self._ensure_cloudxr_runtime() + + try: + pipeline = self._build_pipeline() + session_config = TeleopSessionConfig(app_name=self.config.app_name, pipeline=pipeline) + self._session = TeleopSession(session_config) + self._session.__enter__() + except Exception: + self._session = None + try: + self._stop_cloudxr_runtime() + except Exception: + logger.exception("Failed to stop CloudXR runtime during connect() rollback") + raise + logger.info("Isaac Teleop session started: %s", self.config.app_name) + + def disconnect(self) -> None: + try: + if self._session is not None: + # Null the handle BEFORE __exit__: even a failed session teardown must not + # wedge the device as is_connected (blocking every later connect/disconnect). + session = self._session + self._session = None + session.__exit__(None, None, None) + logger.info("Isaac Teleop session ended") + finally: + # Reap the CloudXR runtime even if session teardown raised, and even if no + # session was ever established (e.g. the launcher came up but session creation + # failed before this point); a no-op when we never launched CloudXR (opt-out / + # externally-owned runtime), so we never stop a runtime we don't own. + self._stop_cloudxr_runtime() + + # ------------------------------------------------------------------ + # CloudXR runtime (shared) + # ------------------------------------------------------------------ + + def _ensure_cloudxr_runtime(self) -> None: + """Auto-launch the CloudXR runtime once, unless opted out. + + Idempotent (no-op once the launcher is up). ``LEROBOT_CLOUDXR_SKIP_AUTOLAUNCH`` + is checked first and wins over ``config.auto_launch_cloudxr``. Constructing + :class:`CloudXRLauncher` mutates the process env (``XR_RUNTIME_JSON`` etc.) and + blocks until the runtime is ready or raises :class:`RuntimeError`. + """ + if self._cloudxr_launcher is not None: + return + + if os.environ.get("LEROBOT_CLOUDXR_SKIP_AUTOLAUNCH", "").strip() == "1": + logger.info( + "LEROBOT_CLOUDXR_SKIP_AUTOLAUNCH=1 set; skipping CloudXR auto-launch " + "(assuming CloudXR is already running externally)" + ) + return + + if not self.config.auto_launch_cloudxr: + logger.info( + "config.auto_launch_cloudxr is False; skipping CloudXR auto-launch " + "(assuming CloudXR is already running externally)" + ) + return + + logger.info("Launching CloudXR runtime (first run may prompt for EULA and take ~30s)...") + + self._cloudxr_launcher = CloudXRLauncher( + install_dir=str(Path.home() / ".cloudxr"), + env_config=self.config.cloudxr_env_file, + accept_eula=False, + ) + + def _stop_cloudxr_runtime(self) -> None: + """Stop the auto-launched CloudXR runtime, if any. + + Clean stop nulls the handle. On :class:`RuntimeError` the handle is RETAINED so + the launcher's ``atexit`` hook owns the retry — a later :meth:`connect` then + treats the retained runtime as still up and will not relaunch. + """ + if self._cloudxr_launcher is None: + return + try: + self._cloudxr_launcher.stop() + except RuntimeError: + logger.warning("CloudXR runtime could not be terminated; handle retained for atexit cleanup") + else: + self._cloudxr_launcher = None + logger.info("CloudXR runtime stopped") + + def send_feedback(self, feedback: dict[str, Any]) -> None: + pass # Haptic feedback not yet implemented. + + # ------------------------------------------------------------------ + # Stepping (shared) + # ------------------------------------------------------------------ + + def _running_events(self) -> ExecutionEvents: + """Constant ``RUNNING`` ``ExecutionEvents`` for a device with no clutch lifecycle. + + Keeps the stream flowing; ``reset`` stays ``False``. A clutched device that needs + a real lifecycle should build its own ``ExecutionEvents`` instead. + """ + return ExecutionEvents(execution_state=ExecutionState.RUNNING, reset=False) + + def _step( + self, + *, + execution_events: ExecutionEvents | None = None, + external_inputs: Mapping[str, Any] | None = None, + ) -> RetargeterIO: + """Step the session once and return the raw pipeline outputs. + + Applies the shared guard: re-raises a retargeting-worker exception and warns on a + stale frame. Subclasses call this from :meth:`get_action`. + + Args: + execution_events: The ``ExecutionEvents`` driving the session this frame. + Devices with a lifecycle (clutch) MUST pass this every frame — when + ``None``, ``TeleopSession.step`` auto-fires ``RUNNING`` (the clutch would + latch immediately and never stop). + external_inputs: Per-step inputs (e.g. a static ``base_T_anchor``) in the + ``{leaf_node_name: {output_port_name: TensorGroup}}`` shape ``step`` expects. + + Raises: + RuntimeError: If not connected, or if the retargeting worker raised. + """ + if self._session is None: + raise RuntimeError("Not connected. Call connect() first.") + + result = self._session.step( + execution_events=execution_events, + external_inputs=external_inputs, + ) + + info = self._session.last_step_info + if info is not None: + if info.worker_exception is not None: + raise RuntimeError( + "Isaac Teleop retargeting worker raised an exception" + ) from info.worker_exception + if info.frame_deadline_miss: + logger.warning( + "Isaac Teleop frame deadline miss (returned_age_frames=%s)", + info.returned_age_frames, + ) + return result diff --git a/examples/isaac_teleop_to_so101/isaac_teleop/clutch.py b/examples/isaac_teleop_to_so101/isaac_teleop/clutch.py new file mode 100644 index 000000000..af0ae6a41 --- /dev/null +++ b/examples/isaac_teleop_to_so101/isaac_teleop/clutch.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Engage-relative clutch for the XR -> SO-101 teleop loop. + +Turns the raw controller grip pose into an absolute base-frame EE target, so the XR +device can stay a thin raw-pose reader. Pure numpy + the local ``Rotation`` helper (no +``isaacteleop``), so it is unit-testable without the XR runtime. +""" + +from __future__ import annotations + +import numpy as np + +from lerobot.utils.rotation import Rotation + + +class Clutch: + """Engage-relative clutch for both position AND orientation. + + Latch an origin on engage, then track the base-frame delta from it, applied + independently to position and orientation. State: + + - ``_last_commanded_pos`` / ``_last_commanded_rot``: last commanded EE pose; held + while disengaged so the arm freezes where it was left. + - ``_home_pos`` / ``_home_rot``: latched on engage — the EE pose the delta applies to. + The position comes from the arm's MEASURED pose when the caller provides it (so an + arm that moved while disengaged is not snapped back to a stale command); the + orientation always comes from the last commanded rotation (see NOTE below). + - ``_origin_pos`` / ``_origin_rot``: latched on engage — the controller pose the delta + is measured against. + + Each engaged frame :meth:`rebase` returns:: + + pos = home_pos + (grip_pos - origin_pos) # 1:1 controller -> EE translation + rot = (R_ctrl @ R_origin ^ -1) @ R_home # base-frame delta, left-composed + + On the engage edge the output is exactly the home pose (no teleport). The orientation + delta is left-composed (base frame), so hand rotation about base Z maps to EE rotation + about base Z. A re-clutch latches a fresh home/origin. + + NOTE: ``_home_rot`` is the last *commanded* orientation even when the measured pose is + supplied: the 5-DOF SO-101 tracks orientation only softly, so its measured wrist + orientation persistently differs from the command, and latching the measurement would + inject that offset into the commanded signal on every re-clutch. Position has no such + tracking gap, and there latching the measurement is what prevents the snap-back. + """ + + def __init__(self, home_base_T_ee: np.ndarray): # noqa: N803 + # Seed the held pose from the arm's measured startup EE pose so the first + # engage latches home there (no jump on the first squeeze). + home = np.asarray(home_base_T_ee, dtype=float) + self._last_commanded_pos = home[:3, 3].copy() + self._last_commanded_rot = Rotation.from_matrix(home[:3, :3]) + self._home_pos = self._last_commanded_pos.copy() + self._home_rot = self._last_commanded_rot + self._origin_pos = np.zeros(3, dtype=float) + self._origin_rot = Rotation.from_quat(np.array([0.0, 0.0, 0.0, 1.0])) + + def engage( + self, + grip_pos: np.ndarray, + grip_quat: np.ndarray, + measured_base_T_ee: np.ndarray | None = None, # noqa: N803 + ) -> None: + """Latch the engage home (where the arm is now) and controller origin. + + Pass ``measured_base_T_ee`` (FK of the measured joints) so the home POSITION is + where the arm physically is — if the arm moved while disengaged (gravity sag, + external contact), latching the stale last-commanded position would make the + first engaged frame command a full-speed jump back to it. The home ORIENTATION + always stays the last commanded one (see the class NOTE). + """ + if measured_base_T_ee is not None: + self._home_pos = np.asarray(measured_base_T_ee, dtype=float)[:3, 3].copy() + else: + self._home_pos = self._last_commanded_pos.copy() + self._home_rot = self._last_commanded_rot + self._origin_pos = np.asarray(grip_pos, dtype=float).copy() + self._origin_rot = Rotation.from_quat(np.asarray(grip_quat, dtype=float)) + + def rebase(self, grip_pos: np.ndarray, grip_quat: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + """Return the absolute base-frame EE target ``(pos [m], quat [xyzw])`` for this frame.""" + pos = self._home_pos + (np.asarray(grip_pos, dtype=float) - self._origin_pos) + rot_ctrl = Rotation.from_quat(np.asarray(grip_quat, dtype=float)) + rot = (rot_ctrl * self._origin_rot.inv()) * self._home_rot + self._last_commanded_pos = pos.copy() + self._last_commanded_rot = rot + return pos, rot.as_quat() diff --git a/examples/isaac_teleop_to_so101/isaac_teleop/config_isaac_teleop.py b/examples/isaac_teleop_to_so101/isaac_teleop/config_isaac_teleop.py new file mode 100644 index 000000000..d9f8f993f --- /dev/null +++ b/examples/isaac_teleop_to_so101/isaac_teleop/config_isaac_teleop.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Configuration dataclasses for NVIDIA Isaac Teleop-backed teleoperators. + +:class:`IsaacTeleopConfig` holds the shared fields; each device adds its own subclass +(e.g. :class:`XRControllerConfig`, :class:`SO101LeaderArmConfig`). +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import ClassVar + +from lerobot.teleoperators.config import TeleoperatorConfig + + +@dataclass(kw_only=True) +class IsaacTeleopConfig(TeleoperatorConfig): + """Shared config for all Isaac Teleop-backed teleoperators. + + Uses its own draccus ``_choice_registry`` (decoupled from the global + :class:`TeleoperatorConfig` one) so ``--teleop.type`` on a field typed + ``IsaacTeleopConfig`` resolves against ONLY the Isaac devices — letting them claim + short names (``xr_controller``, ``so101_leader``) without colliding with the global + registry. These devices are selected by the example scripts, not routed through + ``make_teleoperator_from_config``. + """ + + _choice_registry: ClassVar[dict] = {} + + app_name: str = "LeTeleop" + """Application name for the OpenXR / Isaac Teleop session.""" + + auto_launch_cloudxr: bool = True + """Auto-launch the CloudXR runtime on :meth:`connect`. Set ``False`` (or export + ``LEROBOT_CLOUDXR_SKIP_AUTOLAUNCH=1``, which wins) when CloudXR runs externally. + """ + + cloudxr_env_file: str | None = None + """Optional CloudXR device-profile ``.env`` (an INPUT profile selecting the headset + transport) passed to ``CloudXRLauncher``. ``None`` keeps the default auto-WebRTC profile. + """ + + +# Static rebase from the OpenXR controller anchor frame (X=Right, Y=Up, Z=Backward) into the +# robot base frame (X=Forward, Y=Left, Z=Up). A proper rotation (det=+1): controller motion +# forward -> robot +X, right -> robot -Y (i.e. rightward), up -> robot +Z. +_DEFAULT_BASE_T_ANCHOR: list[list[float]] = [ + [0.0, 0.0, -1.0, 0.0], + [-1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1.0], +] + + +@IsaacTeleopConfig.register_subclass("xr_controller") +@dataclass(kw_only=True) +class XRControllerConfig(IsaacTeleopConfig): + """Config for Isaac Teleop XR (VR) controller teleoperation. + + Exposes the raw base-frame grip pose, squeeze, and trigger via ``ControllersSource``. + No retargeters: the clutch and gripper mapping live in the owning loop. + """ + + hand_side: str = "right" + """Which controller hand to use: ``"left"`` or ``"right"``. A plain ``str`` (validated in + ``__post_init__``) because draccus cannot decode ``Literal``-typed fields from the CLI.""" + + clutch_threshold: float = 0.5 + """Squeeze value above which the owning loop's clutch engages (held-to-enable). The + device reports only the raw squeeze; the threshold is applied by the loop.""" + + base_T_anchor: list[list[float]] = field( # noqa: N815 (frameA_T_frameB transform-matrix convention) + # Fresh copy per instance: returning the module-level list itself would alias one + # mutable matrix across every config. + default_factory=lambda: [row.copy() for row in _DEFAULT_BASE_T_ANCHOR] + ) + """Static 4x4 [row-major] transform rebasing the OpenXR controller anchor frame into + the robot base frame. Defaults to OpenXR (X=Right, Y=Up, Z=Backward) -> robot + (X=Forward, Y=Left, Z=Up). Plain nested lists so the config stays serializable. + """ + + def __post_init__(self): + if self.hand_side not in ("left", "right"): + raise ValueError(f"hand_side must be 'left' or 'right', got {self.hand_side!r}") + + +# Provisional gripper open/close endpoints [rad], normalizing the streamed gripper angle +# into the follower's RANGE_0_100 jaw target. Derived from the so101_leader plugin README's +# example calibration (home_ticks=2048, range 2000..3000; angle = (ticks-home)*2*pi/4096). +_DEFAULT_GRIPPER_OPEN_RAD = -0.074 +_DEFAULT_GRIPPER_CLOSE_RAD = 1.460 + + +@IsaacTeleopConfig.register_subclass("so101_leader") +@dataclass(kw_only=True) +class SO101LeaderArmConfig(IsaacTeleopConfig): + """Config for an Isaac Teleop SO-101 *leader arm* (generic joint-space device). + + Mirrors the leader's joint angles 1:1 onto a follower SO-101. The leader state is + streamed in radians by the native ``so101_leader`` plugin and read via a + ``JointStateSource``; the device converts arm joints to degrees and the gripper to the + follower's RANGE_0_100 jaw target (no IK/clutch/retargeter on the LeRobot side). + """ + + port: str = "" + """Serial port of the physical LEADER arm (e.g. ``/dev/ttyACM1``), forwarded to the + plugin (which reads the servos) when the example launches it. Empty -> the plugin runs + its synthetic trajectory.""" + + collection_id: str = "so101_leader" + """Tensor collection id the leader plugin pushes on; must match the running + ``so101_leader`` plugin (its second positional arg, default ``"so101_leader"``).""" + + gripper_open_rad: float = _DEFAULT_GRIPPER_OPEN_RAD + """Leader gripper angle [rad] at fully OPEN -> follower jaw 100. Provisional default; + set from the plugin's ``calibrate`` subcommand. See ``_DEFAULT_GRIPPER_OPEN_RAD``.""" + + gripper_close_rad: float = _DEFAULT_GRIPPER_CLOSE_RAD + """Leader gripper angle [rad] at fully CLOSED -> follower jaw 0. Provisional default; + set from the plugin's ``calibrate`` subcommand. See ``_DEFAULT_GRIPPER_CLOSE_RAD``.""" diff --git a/examples/isaac_teleop_to_so101/isaac_teleop/teleop_so101_leader_arm.py b/examples/isaac_teleop_to_so101/isaac_teleop/teleop_so101_leader_arm.py new file mode 100644 index 000000000..f23c4e9b7 --- /dev/null +++ b/examples/isaac_teleop_to_so101/isaac_teleop/teleop_so101_leader_arm.py @@ -0,0 +1,186 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""SO-101 leader-arm device for NVIDIA Isaac Teleop, exposed to LeRobot. + +The leader is a back-drivable SO-101 whose six joint angles are streamed (in radians) by +the native ``so101_leader`` plugin; this device reads them via a ``JointStateSource`` and +converts them into follower-ready ``{joint}.pos``. Same kinematics as the follower, so it +needs no retargeting — a 1:1 joint mirror, direct joint drive. + +Units (converted in the device so the output is always follower-valid): + +* arm joints: ``rad2deg`` — correct only if the leader's calibrated zero and the follower's + homing map to the same physical zero (the standard same-hardware assumption). +* gripper: normalized from ``[gripper_open_rad, gripper_close_rad]`` to RANGE_0_100. + +``isaacteleop`` imports are guarded behind the availability flag so this module — and the +pure :func:`leader_joints_to_robot_action` converter — import without it (construction +fails fast via the base class). +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np + +from lerobot.types import RobotAction + +from .base import _GRIPPER_MOTOR_SCALE, IsaacTeleopTeleoperator, _isaacteleop_available +from .config_isaac_teleop import SO101LeaderArmConfig + +if TYPE_CHECKING or _isaacteleop_available: + from isaacteleop.retargeting_engine.deviceio_source_nodes import JointStateSource + from isaacteleop.retargeting_engine.interface import OutputCombiner +else: + JointStateSource = None + OutputCombiner = None + +# Canonical SO-101 DOF names and order — matches the plugin stream and the follower's motor +# order. Passed to the ``JointStateSource`` as its output layout; the source maps by name and +# :func:`_joints_group_to_rad` reads back by name, so a layout mismatch can't mislabel a DOF. +SO101_LEADER_JOINTS = [ + "shoulder_pan", + "shoulder_lift", + "elbow_flex", + "wrist_flex", + "wrist_roll", + "gripper", +] + + +def leader_joints_to_robot_action( + joints_rad: dict[str, float], + *, + gripper_joint: str, + gripper_open_rad: float, + gripper_close_rad: float, +) -> RobotAction: + """Convert streamed leader joint angles [rad] to follower-ready ``{joint}.pos``. + + Pure (no ``isaacteleop``, no I/O). Iteration follows ``joints_rad`` insertion order, so + pass it in :data:`SO101_LEADER_JOINTS` order for a stable layout. Arm joints are + converted ``rad2deg``; ``gripper_joint`` is normalized from + ``[gripper_open_rad, gripper_close_rad]`` to RANGE_0_100 (clipped). + """ + action: RobotAction = {} + span = gripper_close_rad - gripper_open_rad + for name, rad in joints_rad.items(): + if name == gripper_joint: + # Closedness c=0 at open, c=1 at closed; invert to the follower's 100=open jaw. + closedness = 0.0 if span == 0.0 else (rad - gripper_open_rad) / span + closedness = min(1.0, max(0.0, closedness)) + action[f"{name}.pos"] = (1.0 - closedness) * _GRIPPER_MOTOR_SCALE + else: + action[f"{name}.pos"] = float(np.rad2deg(rad)) + return action + + +def _joints_group_to_rad(joints) -> dict[str, float]: + """Read a ``JointStateSource`` output group into ``{joint_name: angle [rad]}``. + + Pure (duck-typed on the group). The group is positional but each slot carries its joint + name in ``group.group_type.types``; we key off those names (not a positional index) so a + layout mismatch surfaces as a wrong/missing key here rather than a mislabeled DOF. + """ + names = [t.name for t in joints.group_type.types] + return {name: float(joints[i]) for i, name in enumerate(names)} + + +class SO101LeaderArm(IsaacTeleopTeleoperator): + """SO-101 leader-arm teleoperator (joint-space), direct joint mirror to the follower. + + Reads the six joint angles off a single ``JointStateSource`` each frame; no retargeter, + no clutch. When the leader is not streaming, :meth:`get_action` returns the held-last + joints and :attr:`is_tracking` is ``False`` so the owning loop can hold the follower. + """ + + config_class = SO101LeaderArmConfig + name = "isaac_teleop_so101_leader" + + def __init__(self, config: SO101LeaderArmConfig): + super().__init__(config) + self.config: SO101LeaderArmConfig = config + # Held-last joint angles [rad], seeded at zero (URDF/home pose) so the first frames + # before the plugin starts pushing read as the home pose, not garbage. + self._last_joints_rad: dict[str, float] = dict.fromkeys(SO101_LEADER_JOINTS, 0.0) + # Whether the most recent get_action() read live leader data (vs held-last). + self._is_tracking = False + + # ------------------------------------------------------------------ + # Pipeline construction + # ------------------------------------------------------------------ + + def _build_pipeline(self) -> OutputCombiner: + """Build the joint-mirror pipeline: a single ``JointStateSource`` leaf that converts + the raw stream into a name-keyed joint group. No retargeter (shared kinematics).""" + source = JointStateSource( + name="so101_leader", + collection_id=self.config.collection_id, + joint_names=SO101_LEADER_JOINTS, + ) + return OutputCombiner({"joints": source.output(JointStateSource.JOINTS)}) + + # ------------------------------------------------------------------ + # Action features + # ------------------------------------------------------------------ + + @property + def action_features(self) -> dict[str, type]: + # Matches the serial SOLeader's action features so this is a drop-in joint-space + # leader: one float `{joint}.pos` per DOF, sendable straight to an SO-101 follower. + return {f"{name}.pos": float for name in SO101_LEADER_JOINTS} + + @property + def feedback_features(self) -> dict[str, type]: + return {} + + @property + def is_tracking(self) -> bool: + """Whether the last :meth:`get_action` read live leader data (vs held-last).""" + return self._is_tracking + + # ------------------------------------------------------------------ + # Action extraction + # ------------------------------------------------------------------ + + def get_action(self) -> RobotAction: + """Step the session and return the leader joints as follower-ready ``{joint}.pos``. + + When the leader is streaming, the live angles are cached and converted; otherwise the + held-last angles are reused and :attr:`is_tracking` is set ``False``. + """ + result = self._step(execution_events=self._running_events()) + + joints = result["joints"] + # The JointStateSource output is Optional: absent (is_none) when the device is + # inactive. Treat that as "not tracking" and reuse the held-last angles. + self._is_tracking = not getattr(joints, "is_none", False) + if self._is_tracking: + try: + self._last_joints_rad = _joints_group_to_rad(joints) + except (AttributeError, IndexError, KeyError, TypeError, ValueError): + # A partially-populated / malformed group on an odd frame: keep held-last, but + # report it as not-tracking so the loop holds the follower rather than trusting it. + self._is_tracking = False + + return leader_joints_to_robot_action( + self._last_joints_rad, + gripper_joint="gripper", + gripper_open_rad=self.config.gripper_open_rad, + gripper_close_rad=self.config.gripper_close_rad, + ) diff --git a/examples/isaac_teleop_to_so101/isaac_teleop/teleop_xr_controller.py b/examples/isaac_teleop_to_so101/isaac_teleop/teleop_xr_controller.py new file mode 100644 index 000000000..25949876b --- /dev/null +++ b/examples/isaac_teleop_to_so101/isaac_teleop/teleop_xr_controller.py @@ -0,0 +1,204 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""XR (VR) controller device for NVIDIA Isaac Teleop, exposed to LeRobot. + +A deliberately thin reader: exposes the raw controller grip pose off +``ControllersSource`` (statically rebased into the robot base frame by +``ControllerTransform``), plus squeeze and trigger. No retargeters and no clutch — +the clutch rebasing and gripper mapping live downstream in the owning loop, so this +device is stateless across frames. + +``isaacteleop`` imports are guarded behind the availability flag so this module imports +without it (construction fails fast via the base class). +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +import numpy as np + +from lerobot.types import RobotAction + +from .base import IsaacTeleopTeleoperator, _isaacteleop_available +from .config_isaac_teleop import XRControllerConfig + +if TYPE_CHECKING or _isaacteleop_available: + from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource + from isaacteleop.retargeting_engine.interface import OutputCombiner, TensorGroup, ValueInput + from isaacteleop.retargeting_engine.tensor_types import TransformMatrix + from isaacteleop.retargeting_engine.tensor_types.indices import ControllerInputIndex +else: + ControllersSource = None + OutputCombiner = None + TensorGroup = None + ValueInput = None + TransformMatrix = None + ControllerInputIndex = None + +# Source-node name for the static base_T_anchor rebase input fed via +# ``TeleopSession.step(external_inputs=...)`` each frame. +_BASE_T_ANCHOR_INPUT = "base_T_anchor" + + +class XRController(IsaacTeleopTeleoperator): + """Raw XR controller grip-pose teleoperator (base-frame), no retargeters. + + Reads the raw grip pose + squeeze + trigger off a ``ControllersSource`` rebased into + the robot base frame. :meth:`get_action` returns the absolute base-frame grip pose + untouched; the owning loop owns the clutch and gripper mapping. + """ + + config_class = XRControllerConfig + name = "isaac_teleop_controller" + + def __init__(self, config: XRControllerConfig): + super().__init__(config) + self.config: XRControllerConfig = config + + # Constant base_T_anchor input, built once in connect() (a TensorGroup is heavy and + # isaacteleop-backed) and reused every step. + self._external_inputs: dict[str, Any] | None = None + # Whether the last get_action() read a tracked controller; the owning loop polls this + # to wait for the operator to connect before driving the arm. + self._is_tracking = False + + # ------------------------------------------------------------------ + # Pipeline construction + # ------------------------------------------------------------------ + + def _build_pipeline(self) -> OutputCombiner: + """Build the raw-grip-pose pipeline: a ``ControllersSource`` rebased into the base + frame by ``ControllerTransform``, exposed verbatim as ``"controller"``. No retargeters. + """ + side = self.config.hand_side + controller_key = f"controller_{side}" + + controllers = ControllersSource(name="controllers") + # Static base_T_anchor rebase fed via external_inputs each step. + xform = ValueInput(_BASE_T_ANCHOR_INPUT, TransformMatrix()) + transformed = controllers.transformed(xform.output("value")) + ctrl = transformed.output(controller_key) + + return OutputCombiner({"controller": ctrl}) + + def _build_external_inputs(self) -> dict[str, Any]: + """Materialize the constant ``base_T_anchor`` external input (once, in connect).""" + tg = TensorGroup(TransformMatrix()) + tg[0] = np.asarray(self.config.base_T_anchor, dtype=np.float32) + return {_BASE_T_ANCHOR_INPUT: {"value": tg}} + + def connect(self, calibrate: bool = True) -> None: + super().connect(calibrate=calibrate) + try: + self._external_inputs = self._build_external_inputs() + except Exception: + # Roll the session/runtime back so a failed connect() leaves no half-state + # (a live session behind a raised connect would leak the CloudXR runtime). + self.disconnect() + raise + + # ------------------------------------------------------------------ + # Action features + # ------------------------------------------------------------------ + + @property + def action_features(self) -> dict: + return { + "grip_pos": { + "dtype": "float32", + "shape": (3,), + "names": {"x": 0, "y": 1, "z": 2}, + }, + "grip_quat": { + "dtype": "float32", + "shape": (4,), + "names": {"qx": 0, "qy": 1, "qz": 2, "qw": 3}, + }, + # ``get_action`` returns scalars for these two, so the advertised + # shape is () (0-d) to stay consistent with the returned values. + "squeeze": { + "dtype": "float32", + "shape": (), + "names": None, + }, + "trigger": { + "dtype": "float32", + "shape": (), + "names": None, + }, + } + + @property + def feedback_features(self) -> dict: + return {} + + @property + def is_tracking(self) -> bool: + """Whether the last :meth:`get_action` read a tracked controller. ``False`` until the + headset is connected over CloudXR and its controllers are live; the owning loop polls + it to wait for the operator before commanding the arm.""" + return self._is_tracking + + # ------------------------------------------------------------------ + # Action extraction + # ------------------------------------------------------------------ + + def get_action(self) -> RobotAction: + """Step the session and return the raw base-frame grip pose. + + Reads the grip pose + squeeze + trigger off the transformed controller stream (with + the constant ``base_T_anchor`` rebase). When the controller is not tracked, returns + identity pose and squeeze/trigger = 0.0 so the owning loop freezes the arm. + + Returns: + ``{"grip_pos": (3,) [m], "grip_quat": (4,) [qx,qy,qz,qw], "squeeze": float, + "trigger": float}`` — pose in the robot base frame; squeeze/trigger in ``[0, 1]``. + """ + result = self._step(execution_events=self._running_events(), external_inputs=self._external_inputs) + + # Optional controller group is None until the headset is connected and its controllers + # are live; expose that as is_tracking so the loop can wait before driving the arm. + controller = result["controller"] + grip_pos = np.zeros(3, dtype=np.float32) + grip_quat = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) + squeeze = 0.0 + trigger = 0.0 + self._is_tracking = not getattr(controller, "is_none", False) + if self._is_tracking: + # Read ALL four fields into locals before committing any of them: a failure on a + # partially-populated frame must not mix live values with the safe defaults (a + # live squeeze paired with a defaulted trigger=0.0 would keep the clutch engaged + # while commanding the gripper fully open, dropping whatever is grasped). On + # failure the defaults stand untouched and the frame reports not-tracked. + try: + pos = np.asarray(controller[ControllerInputIndex.GRIP_POSITION], dtype=np.float32) + quat = np.asarray(controller[ControllerInputIndex.GRIP_ORIENTATION], dtype=np.float32) + squeeze_val = float(controller[ControllerInputIndex.SQUEEZE_VALUE]) + trigger_val = float(controller[ControllerInputIndex.TRIGGER_VALUE]) + except (IndexError, KeyError, TypeError, ValueError): + self._is_tracking = False + else: + grip_pos, grip_quat = pos, quat + squeeze, trigger = squeeze_val, trigger_val + + return { + "grip_pos": grip_pos, + "grip_quat": grip_quat, + "squeeze": squeeze, + "trigger": trigger, + } diff --git a/examples/isaac_teleop_to_so101/isaac_teleop/xr_controller_processor.py b/examples/isaac_teleop_to_so101/isaac_teleop/xr_controller_processor.py new file mode 100644 index 000000000..b0eaa8280 --- /dev/null +++ b/examples/isaac_teleop_to_so101/isaac_teleop/xr_controller_processor.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Processor step that maps XR controller actions to robot EE targets. + +Analogous to ``MapPhoneActionToRobotAction``, this bridges the clutch-rebased EE pose to +the IK pipeline's input contract (``EEBoundsAndSafety`` -> ``InverseKinematicsEEToJoints``). +Pure (no ``isaacteleop``), so it is unit-testable without the XR runtime. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +from lerobot.configs.types import FeatureType, PipelineFeatureType, PolicyFeature +from lerobot.processor import ProcessorStepRegistry, RobotActionProcessorStep +from lerobot.types import RobotAction +from lerobot.utils.rotation import Rotation + +from .base import _GRIPPER_MOTOR_SCALE + + +@ProcessorStepRegistry.register("map_xr_controller_action_to_robot_action") +@dataclass +class MapXRControllerActionToRobotAction(RobotActionProcessorStep): + """Maps an absolute base-frame EE pose + gripper closedness to the IK input contract. + + Pure, stateless rename (the owning loop's clutch already produced the absolute base-frame + target). Each frame it writes: + + - ``ee.x/y/z`` = ``ee_pose[:3]`` (position [m]); + - ``ee.wx/wy/wz`` = rotvec of ``ee_pose[3:7]`` (orientation; the IK tracks it softly at a + small ``orientation_weight`` on the 5-DOF SO-101); + - ``ee.gripper_pos`` = ``(1 - closedness) * _GRIPPER_MOTOR_SCALE`` (jaw target [0, 100], + RANGE_0_100 where 100 = open, so closedness is inverted). + + Input keys: ``ee_pose`` ``(7,)`` ``[x,y,z,qx,qy,qz,qw]``, ``closedness`` float in [0, 1]. + """ + + def action(self, action: RobotAction) -> RobotAction: + ee_pose = action.pop("ee_pose") + closedness = float(action.pop("closedness")) + + action["ee.x"] = float(ee_pose[0]) + action["ee.y"] = float(ee_pose[1]) + action["ee.z"] = float(ee_pose[2]) + # Orientation target as a rotvec (quat [qx,qy,qz,qw] -> axis-angle); the IK + # consumes ee.w* as a rotvec and tracks it with orientation_weight. + rotvec = Rotation.from_quat(ee_pose[3:7]).as_rotvec() + action["ee.wx"] = float(rotvec[0]) + action["ee.wy"] = float(rotvec[1]) + action["ee.wz"] = float(rotvec[2]) + # Inverted: closedness c=1 (closed) -> 0, c=0 (open) -> 100 (SO-101 calibration). + action["ee.gripper_pos"] = (1.0 - closedness) * _GRIPPER_MOTOR_SCALE + return action + + def transform_features( + self, features: dict[PipelineFeatureType, dict[str, PolicyFeature]] + ) -> dict[PipelineFeatureType, dict[str, PolicyFeature]]: + for feat in ["ee_pose", "closedness"]: + features[PipelineFeatureType.ACTION].pop(feat, None) + + for feat in [ + "ee.x", + "ee.y", + "ee.z", + "ee.wx", + "ee.wy", + "ee.wz", + "ee.gripper_pos", + ]: + features[PipelineFeatureType.ACTION][feat] = PolicyFeature(type=FeatureType.ACTION, shape=(1,)) + + return features diff --git a/examples/isaac_teleop_to_so101/override_reset_pose.py b/examples/isaac_teleop_to_so101/override_reset_pose.py new file mode 100644 index 000000000..580f9f6f1 --- /dev/null +++ b/examples/isaac_teleop_to_so101/override_reset_pose.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Save the current SO-101 joint positions as the reset-origin pose (override). + +Move the arm to the desired reset pose by hand (torque off), then run this script to write +those joints to a per-arm file in the LeRobot cache. ``teleoperate.py`` / ``record.py`` load +it on startup (matched by ``--robot.id``) as the reset target instead of the defaults. + +Usage:: + + # 1. Move arm to desired reset pose by hand + python -m examples.isaac_teleop_to_so101.override_reset_pose [--port /dev/ttyACM0] [--id so101_follower_arm] + + # 2. Launch teleop with the SAME --robot.id — it will now reset to this pose on startup + python -m examples.isaac_teleop_to_so101.teleoperate --robot.type=so101_follower --robot.port=/dev/ttyACM0 --robot.id=so101_follower_arm --teleop.type=xr_controller +""" + +import argparse +import json +from pathlib import Path + +from lerobot.robots.so_follower import SO100Follower, SO100FollowerConfig + +from .common import RESET_POSE_FILE + + +def parse_args(): + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("--port", type=str, default="/dev/ttyACM0") + parser.add_argument("--id", type=str, default="so101_follower_arm") + return parser.parse_args() + + +def main(): + args = parse_args() + robot = SO100Follower(SO100FollowerConfig(port=args.port, id=args.id, use_degrees=True)) + robot.connect() + # Always disconnect the follower so a failure never leaks the serial connection. + try: + obs = robot.get_observation() + motor_names = list(robot.bus.motors.keys()) + pose = {name: float(obs[f"{name}.pos"]) for name in motor_names} + finally: + robot.disconnect() + + print("Current joint positions:") + for name, val in pose.items(): + print(f" {name:20s}: {val:.2f}") + + reset_pose_file = Path(RESET_POSE_FILE.format(robot_name=robot.name, robot_id=robot.id)) + reset_pose_file.parent.mkdir(parents=True, exist_ok=True) + reset_pose_file.write_text(json.dumps(pose, indent=2)) + print(f"\nSaved to {reset_pose_file}") + + +if __name__ == "__main__": + main() diff --git a/examples/isaac_teleop_to_so101/record.py b/examples/isaac_teleop_to_so101/record.py new file mode 100644 index 000000000..9ae769446 --- /dev/null +++ b/examples/isaac_teleop_to_so101/record.py @@ -0,0 +1,321 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Record a LeRobot dataset via NVIDIA Isaac Teleop -> SO-101. + +Runs ``teleoperate.py``'s control loop while also saving each frame to a LeRobot dataset. +``--teleop.type`` selects the device (``xr_controller`` | ``so101_leader``) as in +``teleoperate.py``. + +Usage:: + + # XR (VR) controller: clutch + soft-orientation IK + python -m examples.isaac_teleop_to_so101.record \\ + --robot.type=so101_follower \\ + --robot.port=/dev/ttyACM0 \\ + --robot.id=so101_follower_arm \\ + --teleop.type=xr_controller \\ + --robot.cameras="{ front: {type: opencv, index_or_path: 0, width: 640, height: 480, fps: 30}}" \\ + --dataset.repo_id=/ \\ + --dataset.single_task="Pick up vial from rack on the left side" \\ + --dataset.num_episodes=3 \\ + --dataset.episode_time_s=20 \\ + --dataset.reset_time_s=5 + + # SO-101 leader arm: 1:1 joint mirror (real leader on /dev/ttyACM1) + python -m examples.isaac_teleop_to_so101.record \\ + --robot.type=so101_follower --robot.port=/dev/ttyACM0 --robot.id=so101_follower_arm \\ + --teleop.type=so101_leader --teleop.port=/dev/ttyACM1 --teleop.id=so101_leader_arm \\ + --launch_plugin=/path/to/IsaacTeleop/install/plugins/so101_leader/so101_leader_plugin \\ + --dataset.repo_id=/ --dataset.single_task="Pick up the cube" \\ + --dataset.num_episodes=3 --dataset.episode_time_s=20 --dataset.reset_time_s=5 + +The loop/launch knobs mirror ``teleoperate.py`` (tagged ``[xr]`` / ``[leader]`` below). + +Keyboard shortcuts: Right/n = end episode early and save, Left/r = discard + re-record, +Esc/q = stop after the current episode. All frames are recorded (including hold frames). +""" + +import logging +import time +from dataclasses import asdict, dataclass +from pprint import pformat + +from lerobot.cameras import CameraConfig # noqa: F401 +from lerobot.cameras.opencv import OpenCVCameraConfig # noqa: F401 +from lerobot.common.control_utils import sanity_check_dataset_robot_compatibility +from lerobot.configs import parser +from lerobot.configs.dataset import DatasetRecordConfig +from lerobot.datasets import ( + LeRobotDataset, + VideoEncodingManager, + aggregate_pipeline_dataset_features, + create_initial_features, + safe_stop_image_writer, +) +from lerobot.processor import make_default_processors +from lerobot.robots import RobotConfig +from lerobot.robots.so_follower import SOFollowerConfig # noqa: F401 (registers so101_follower) +from lerobot.utils.constants import ACTION, OBS_STR +from lerobot.utils.feature_utils import build_dataset_frame, combine_feature_dicts +from lerobot.utils.robot_utils import precise_sleep +from lerobot.utils.utils import init_logging + +from .common import ( + ALIGN_DURATION_S, + RESET_DURATION_S, + Device, + HoldLatch, + build_device, + init_keyboard_listener, +) +from .isaac_teleop import IsaacTeleopConfig + + +@dataclass +class RecordConfig: + """CLI config for Isaac Teleop -> SO-101 dataset recording. + + ``--robot.*`` / ``--teleop.*`` / ``--dataset.*`` configure the follower, device, and + recording; the loop/launch knobs below carry the same ``[xr]`` / ``[leader]`` tags as + ``teleoperate.py``. Use ``--flag=false`` for booleans (draccus style). + """ + + robot: RobotConfig + # --teleop.type=xr_controller|so101_leader, resolved against IsaacTeleopConfig's registry. + teleop: IsaacTeleopConfig + dataset: DatasetRecordConfig + + # [leader] Path to the so101_leader plugin binary to spawn after CloudXR is up (it then + # inherits the runtime env). None (default) -> assume the plugin already runs externally. + launch_plugin: str | None = None + + # [xr] Slew all joints to the reset pose before the first episode (--reset_to_origin=false to + # keep the arm where it is). After the slew the clutch seeds its home from the measured pose. + reset_to_origin: bool = True + # [xr] Duration [s] of the reset-to-origin slew (passed through to setup_xr). + reset_duration: float = RESET_DURATION_S + + # [leader] Slew the follower to the leader's first pose before mirroring (--align=false to + # begin the 1:1 mirror immediately; the follower may snap). + align: bool = True + # [leader] Duration [s] of the startup alignment slew. + align_duration: float = ALIGN_DURATION_S + + # Resume recording on an existing (previously interrupted) dataset. + resume: bool = False + + +@safe_stop_image_writer +def _record_loop( + robot, + device: Device, + motor_names: list[str], + events: dict, + fps: int, + dataset: LeRobotDataset | None = None, + control_time_s: float = 0.0, + single_task: str | None = None, +) -> None: + """Run one episode (or reset phase) of the control loop. + + When ``dataset`` is None the loop still controls the robot (so the operator + can reposition the arm during the reset window) but does not record frames. + """ + control_interval = 1.0 / fps + timestamp = 0.0 + start_t = time.perf_counter() + record_frames = dataset is not None + hold = HoldLatch(motor_names) + + while timestamp < control_time_s: + loop_start = time.perf_counter() + + if events["exit_early"]: + events["exit_early"] = False + break + + obs = robot.get_observation() + + if record_frames: + observation_frame = build_dataset_frame(dataset.features, obs, prefix=OBS_STR) + + # Device idle (XR clutch disengaged, or leader stream stale) -> hold the pose + # latched on the active->idle edge. + action = hold.resolve(device.compute(obs), obs) + + robot.send_action(action) + + if record_frames: + action_frame = build_dataset_frame(dataset.features, action, prefix=ACTION) + dataset.add_frame({**observation_frame, **action_frame, "task": single_task}) + + dt_s = time.perf_counter() - loop_start + precise_sleep(max(control_interval - dt_s, 0.0)) + timestamp = time.perf_counter() - start_t + + +@parser.wrap() +def record(cfg: RecordConfig) -> LeRobotDataset: + init_logging() + logging.info(pformat(asdict(cfg))) + + # Connect the follower, build the selected Isaac device, and run its pre-loop startup + # (reset slew / leader align) — shared with teleoperate.py. + robot, device, motor_names = build_device(cfg) + + # Build dataset feature spec. The IK pipeline lives inside device.compute(), so the + # action features are exactly robot.action_features (joint positions in degrees). + teleop_proc, _, obs_proc = make_default_processors() + dataset_features = combine_feature_dicts( + aggregate_pipeline_dataset_features( + pipeline=teleop_proc, + initial_features=create_initial_features(action=robot.action_features), + use_videos=cfg.dataset.video, + ), + aggregate_pipeline_dataset_features( + pipeline=obs_proc, + initial_features=create_initial_features(observation=robot.observation_features), + use_videos=cfg.dataset.video, + ), + ) + + num_cameras = len(robot.cameras) if hasattr(robot, "cameras") else 0 + image_writer_threads = cfg.dataset.num_image_writer_threads_per_camera * num_cameras + + dataset: LeRobotDataset | None = None + listener = None + try: + if cfg.resume: + dataset = LeRobotDataset.resume( + cfg.dataset.repo_id, + root=cfg.dataset.root, + batch_encoding_size=cfg.dataset.video_encoding_batch_size, + rgb_encoder=cfg.dataset.rgb_encoder, + depth_encoder=cfg.dataset.depth_encoder, + encoder_threads=cfg.dataset.encoder_threads, + streaming_encoding=cfg.dataset.streaming_encoding, + encoder_queue_maxsize=cfg.dataset.encoder_queue_maxsize, + image_writer_processes=cfg.dataset.num_image_writer_processes if num_cameras > 0 else 0, + image_writer_threads=image_writer_threads if num_cameras > 0 else 0, + ) + sanity_check_dataset_robot_compatibility(dataset, robot, cfg.dataset.fps, dataset_features) + else: + cfg.dataset.stamp_repo_id() + dataset = LeRobotDataset.create( + cfg.dataset.repo_id, + cfg.dataset.fps, + root=cfg.dataset.root, + robot_type=robot.name, + features=dataset_features, + use_videos=cfg.dataset.video, + image_writer_processes=cfg.dataset.num_image_writer_processes, + image_writer_threads=image_writer_threads, + batch_encoding_size=cfg.dataset.video_encoding_batch_size, + rgb_encoder=cfg.dataset.rgb_encoder, + depth_encoder=cfg.dataset.depth_encoder, + encoder_threads=cfg.dataset.encoder_threads, + streaming_encoding=cfg.dataset.streaming_encoding, + encoder_queue_maxsize=cfg.dataset.encoder_queue_maxsize, + ) + + listener, events = init_keyboard_listener() + + loop_kwargs = { + "robot": robot, + "device": device, + "motor_names": motor_names, + "events": events, + "fps": cfg.dataset.fps, + "single_task": cfg.dataset.single_task, + } + + with VideoEncodingManager(dataset): + recorded_episodes = 0 + while recorded_episodes < cfg.dataset.num_episodes and not events["stop_recording"]: + logging.info(f"Recording episode {dataset.num_episodes}") + _record_loop( + **loop_kwargs, + dataset=dataset, + control_time_s=cfg.dataset.episode_time_s, + ) + + # Reset window: give the operator time to reposition the scene. + # Skipped for the last episode (or if stop_recording was set). + if not events["stop_recording"] and ( + recorded_episodes < cfg.dataset.num_episodes - 1 or events["rerecord_episode"] + ): + logging.info("Reset the environment") + _record_loop( + **loop_kwargs, + dataset=None, + control_time_s=cfg.dataset.reset_time_s, + ) + + if events["rerecord_episode"]: + logging.info("Re-record episode") + events["rerecord_episode"] = False + events["exit_early"] = False + dataset.clear_episode_buffer() + continue + + dataset.save_episode() + recorded_episodes += 1 + + finally: + logging.info("Stop recording") + + # Hardware teardown FIRST, each step guarded: the arm must be freed promptly (not + # after a potentially long finalize/encode), a cleanup failure must not skip the + # follower disconnect (which is what disables torque), and neither must prevent + # the dataset from being finalized below. + try: + device.cleanup() + except Exception: + logging.exception("Device cleanup failed") + try: + if robot.is_connected: + robot.disconnect() + except Exception: + logging.exception("Robot disconnect failed") + + # Restore the terminal before the (potentially long) finalize/encode. + if listener is not None: + try: + listener.stop() + except Exception: + logging.exception("Keyboard listener stop failed") + + if dataset is not None: + dataset.finalize() + + if cfg.dataset.push_to_hub: + if dataset is not None and dataset.num_episodes > 0: + dataset.push_to_hub(tags=cfg.dataset.tags, private=cfg.dataset.private) + else: + logging.warning("No episodes saved — skipping push to hub") + + logging.info("Exiting") + + return dataset + + +def main(): + record() + + +if __name__ == "__main__": + main() diff --git a/examples/isaac_teleop_to_so101/teleoperate.py b/examples/isaac_teleop_to_so101/teleoperate.py new file mode 100644 index 000000000..5e382f530 --- /dev/null +++ b/examples/isaac_teleop_to_so101/teleoperate.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python + +# Copyright 2026 NVIDIA Corporation and The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Teleoperate an SO-101 follower arm via NVIDIA Isaac Teleop. + +``lerobot-teleoperate``-style CLI (draccus): ``--teleop.type`` selects the Isaac device +(``xr_controller`` | ``so101_leader``), ``--robot.*`` the follower:: + + # XR (VR) controller: clutch + soft-orientation IK + python -m examples.isaac_teleop_to_so101.teleoperate --robot.type=so101_follower \ + --robot.port=/dev/ttyACM0 --robot.id=so101_follower_arm --teleop.type=xr_controller + + # SO-101 leader arm: 1:1 joint mirror (real leader on /dev/ttyACM1) + python -m examples.isaac_teleop_to_so101.teleoperate --robot.type=so101_follower \ + --robot.port=/dev/ttyACM0 --robot.id=so101_follower_arm --teleop.type=so101_leader \ + --teleop.port=/dev/ttyACM1 --teleop.id=so101_leader_arm \ + --launch_plugin=/code/Teleop/install/plugins/so101_leader/so101_leader_plugin + +``--teleop.type`` resolves against the Isaac device registry (see :class:`IsaacTeleopConfig`), +distinct from the serial ``so101_leader``. The pipelines, clutch/IK/align internals, and +reset-pose behavior live in ``common.py``. Requires the ``isaacteleop`` package and an OpenXR +runtime (install instructions in this folder's ``README.md``). +""" + +import time +from dataclasses import dataclass + +from lerobot.configs import parser +from lerobot.robots import RobotConfig +from lerobot.robots.so_follower import SOFollowerConfig # noqa: F401 (registers so101_follower) +from lerobot.utils.robot_utils import precise_sleep + +from .common import ( + ALIGN_DURATION_S, + FPS, + RESET_DURATION_S, + HoldLatch, + build_device, +) +from .isaac_teleop import IsaacTeleopConfig + + +@dataclass +class TeleoperateConfig: + """``lerobot-teleoperate``-style CLI for the Isaac Teleop -> SO-101 example. + + The fields below are the loop/launch knobs (not part of either device's config); the + ``[xr]`` / ``[leader]`` tags mark which device a knob applies to. Use ``--flag=false`` + for booleans (draccus style). + """ + + # Isaac Teleop input device + its knobs (--teleop.type=xr_controller|so101_leader, + # then --teleop.=...). Resolved against IsaacTeleopConfig's own choice registry. + teleop: IsaacTeleopConfig + # SO-101 FOLLOWER arm (--robot.type=so101_follower --robot.port=/dev/ttyACM0 --robot.id=...). + robot: RobotConfig + + # [leader] Path to the so101_leader plugin binary to spawn AFTER CloudXR is up (it then + # inherits the runtime env). None (default) -> assume the plugin already runs externally. + # The leader's serial port is --teleop.port (forwarded to the plugin; empty -> synthetic). + launch_plugin: str | None = None + + # [xr] Slew all joints to a default reset pose before the loop (--reset_to_origin=false to + # keep the arm where it is). After the slew the clutch seeds its home from the measured pose. + reset_to_origin: bool = True + # [xr] Duration [s] of the reset-to-origin slew. + reset_duration: float = RESET_DURATION_S + + # [leader] Slew the follower to the leader's first pose before mirroring (--align=false to + # begin the 1:1 mirror immediately; the follower may snap). + align: bool = True + # [leader] Duration [s] of the startup alignment slew. + align_duration: float = ALIGN_DURATION_S + + +@parser.wrap() +def teleoperate(cfg: TeleoperateConfig): + robot, device, motor_names = build_device(cfg) + hold = HoldLatch(motor_names) + try: + while True: + t0 = time.perf_counter() + obs = robot.get_observation() + # Idle (compute() -> None) holds the pose latched on the active->idle edge. + action = hold.resolve(device.compute(obs), obs) + robot.send_action(action) + precise_sleep(max(1.0 / FPS - (time.perf_counter() - t0), 0.0)) + except KeyboardInterrupt: + pass + finally: + # A failing device cleanup must not skip the follower disconnect (which is what + # disables torque on the arm). + try: + device.cleanup() + finally: + robot.disconnect() + + +def main(): + teleoperate() + + +if __name__ == "__main__": + main() diff --git a/src/lerobot/robots/so_follower/robot_kinematic_processor.py b/src/lerobot/robots/so_follower/robot_kinematic_processor.py index a95343b2d..ac2ec1f8b 100644 --- a/src/lerobot/robots/so_follower/robot_kinematic_processor.py +++ b/src/lerobot/robots/so_follower/robot_kinematic_processor.py @@ -14,6 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging from dataclasses import dataclass, field from typing import Any @@ -33,6 +34,8 @@ from lerobot.processor import ( ) from lerobot.utils.rotation import Rotation +logger = logging.getLogger(__name__) + @ProcessorStepRegistry.register("ee_reference_and_delta") @dataclass @@ -194,11 +197,17 @@ class EEBoundsAndSafety(RobotActionProcessorStep): Attributes: end_effector_bounds: A dictionary with "min" and "max" keys for position clipping. max_ee_step_m: The maximum allowed change in position (in meters) between steps. + raise_on_jump: When ``True`` (default) an over-limit per-frame step raises + ``ValueError`` (aborting the control loop). When ``False`` the step is + rate-limited to ``max_ee_step_m`` and a warning is logged instead — the + safer choice for live teleoperation, where a transient tracking glitch + should not crash the loop and leave the robot uncontrolled. _last_pos: Internal state storing the last commanded position. """ end_effector_bounds: dict max_ee_step_m: float = 0.05 + raise_on_jump: bool = True _last_pos: np.ndarray | None = field(default=None, init=False, repr=False) def action(self, action: RobotAction) -> RobotAction: @@ -226,8 +235,19 @@ class EEBoundsAndSafety(RobotActionProcessorStep): dpos = pos - self._last_pos n = float(np.linalg.norm(dpos)) if n > self.max_ee_step_m and n > 0: + # Clamp the step to the per-frame limit (rate-limit). The clamped + # value is computed either way; raise_on_jump only decides whether + # an over-limit step aborts the loop or is rate-limited + warned. pos = self._last_pos + dpos * (self.max_ee_step_m / n) - raise ValueError(f"EE jump {n:.3f}m > {self.max_ee_step_m}m") + if self.raise_on_jump: + raise ValueError(f"EE jump {n:.3f}m > {self.max_ee_step_m}m") + logger.warning( + "EE jump %.3fm > %.3fm; rate-limited to the per-frame step " + "(likely a transient tracking glitch; if it recurs every frame " + "the commanded target is systematically out of workspace).", + n, + self.max_ee_step_m, + ) self._last_pos = pos @@ -264,12 +284,18 @@ class InverseKinematicsEEToJoints(RobotActionProcessorStep): q_curr: Internal state storing the last joint positions, used as an initial guess for the IK solver. initial_guess_current_joints: If True, use the robot's current joint state as the IK guess. If False, use the solution from the previous step. + orientation_weight: Weight for the orientation constraint passed to + ``RobotKinematics.inverse_kinematics``. Defaults to ``0.01`` (matching the solver + default, so existing callers are unchanged). Set to ``0.0`` for position-only IK on + under-actuated arms; a small nonzero weight gives soft-orientation IK on the 5-DOF + SO-101, where the wrist tracks orientation only partially (position dominates). """ kinematics: RobotKinematics motor_names: list[str] q_curr: np.ndarray | None = field(default=None, init=False, repr=False) initial_guess_current_joints: bool = True + orientation_weight: float = 0.01 def action(self, action: RobotAction) -> RobotAction: x = action.pop("ee.x") @@ -308,7 +334,9 @@ class InverseKinematicsEEToJoints(RobotActionProcessorStep): t_des[:3, 3] = [x, y, z] # Compute inverse kinematics - q_target = self.kinematics.inverse_kinematics(self.q_curr, t_des) + q_target = self.kinematics.inverse_kinematics( + self.q_curr, t_des, orientation_weight=self.orientation_weight + ) self.q_curr = q_target # TODO: This is sentitive to order of motor_names = q_target mapping