mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-25 10:46:01 +00:00
feat(examples): add Isaac Teleop → SO-101 teleoperation and dataset recording example (#3927)
* 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=<path> 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 <jiwenc@nvidia.com>
* 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 <jiwenc@nvidia.com>
Co-authored-by: Jiwen Cai <jiwenc@nvidia.com>
Co-authored-by: Johnny <johnnync13@gmail.com>
Co-authored-by: Johnny Nunez <22727137+johnnynunez@users.noreply.github.com>
Co-authored-by: Steven Palma <steven.palma@huggingface.co>
Co-authored-by: Steven Palma <imstevenpmwork@ieee.org>
This commit is contained in:
@@ -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",
|
||||
]
|
||||
@@ -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_<device>.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
|
||||
@@ -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()
|
||||
@@ -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``."""
|
||||
@@ -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,
|
||||
)
|
||||
@@ -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,
|
||||
}
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user