mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-07 01:51:47 +00:00
Compare commits
4 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 30da8e687a | |||
| 93257e3468 | |||
| b895ed0fe4 | |||
| 293a8d9a77 |
@@ -169,6 +169,8 @@
|
||||
- sections:
|
||||
- local: phone_teleop
|
||||
title: Phone
|
||||
- local: isaac_teleop
|
||||
title: Isaac Teleop
|
||||
title: "Teleoperators"
|
||||
- sections:
|
||||
- local: cameras
|
||||
|
||||
@@ -117,14 +117,6 @@ middle_dip | 1484 | 1500 | 1547
|
||||
|
||||
Once calibration is complete, the system will save the calibration to `/Users/your_username/.cache/huggingface/lerobot/calibration/teleoperators/homunculus_glove/red.json`
|
||||
|
||||
#### Visualizing Teleoperator Glove
|
||||
|
||||
After calibration, you can visualize the glove movements in real-time. Open the visualizer by navigating to the visualizer directory and opening the HTML file in your browser:
|
||||
|
||||
```bash
|
||||
open examples/hopejr/visualizer/index.html
|
||||
```
|
||||
|
||||
### 1.3 Calibrate Robot Arm
|
||||
|
||||
```bash
|
||||
|
||||
@@ -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_<device>.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 <robot.id>`; it writes the
|
||||
current joints to a per-arm file in the LeRobot cache
|
||||
(`HF_LEROBOT_HOME/reset_poses/<robot.name>/<robot.id>.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 `<IsaacTeleop>/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/<id>.json`, the same file the serial SO-101 leader
|
||||
uses (`lerobot-calibrate --teleop.type=so101_leader --teleop.id=<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.
|
||||
@@ -6,12 +6,11 @@ Encoding frames into an MP4 is a full FFmpeg pipeline: choice of encoder, pixel
|
||||
|
||||
You can set these parameters from the CLI with `--dataset.rgb_encoder.<field>` (e.g. with `lerobot-record` or `lerobot-rollout`). The same block applies to every camera video stream in that run.
|
||||
|
||||
<Tip>
|
||||
Video storage must be on for `rgb_encoder` to have any effect —
|
||||
`use_videos=True` in Python APIs, or `--dataset.video=true` on the CLI (the
|
||||
recording default). With video off, inputs stay as images and `rgb_encoder` is
|
||||
ignored.
|
||||
</Tip>
|
||||
> [!TIP]
|
||||
> Video storage must be on for `rgb_encoder` to have any effect —
|
||||
> `use_videos=True` in Python APIs, or `--dataset.video=true` on the CLI (the
|
||||
> recording default). With video off, inputs stay as images and `rgb_encoder` is
|
||||
> ignored.
|
||||
|
||||
For details on **when** frames are written vs. encoded (streaming vs. post-episode), queues, and other top-level `--dataset.*` switches, see [Streaming Video Encoding](./streaming_video_encoding). For an encoding-parameter comparison and experiments, see the [video-benchmark Space](https://huggingface.co/spaces/lerobot/video-benchmark).
|
||||
|
||||
@@ -43,12 +42,10 @@ lerobot-record \
|
||||
|
||||
## Tuning parameters
|
||||
|
||||
<Tip warning={true}>
|
||||
The defaults are tuned to balance **compression ratio**, **visual quality**, and **decoding/seek speed** for typical robotics datasets. Changing them can affect both recording (CPU load, frame drops) and training (decoding throughput, image quality).
|
||||
|
||||
Only override these parameters if you have a specific reason to, and measure the impact on your pipeline before relying on the new settings.
|
||||
|
||||
</Tip>
|
||||
> [!WARNING]
|
||||
> The defaults are tuned to balance **compression ratio**, **visual quality**, and **decoding/seek speed** for typical robotics datasets. Changing them can affect both recording (CPU load, frame drops) and training (decoding throughput, image quality).
|
||||
>
|
||||
> Only override these parameters if you have a specific reason to, and measure the impact on your pipeline before relying on the new settings.
|
||||
|
||||
All flags below are prefixed with `--dataset.rgb_encoder.` on the CLI.
|
||||
|
||||
@@ -69,25 +66,92 @@ All flags below are prefixed with `--dataset.rgb_encoder.` on the CLI.
|
||||
|
||||
Depth maps (Intel RealSense, Reachy 2) are stored as their **own video streams** alongside the RGB streams. Raw depth (`uint16` millimetres or `float32` metres) can't survive an 8-bit codec, so LeRobot **quantizes** each map to a 12-bit code (`[0, 4095]`) — logarithmically by default, to match the `1/depth` error profile of depth sensors — then packs it into a high-bit-depth pixel format (`gray12le`) and encodes it with a 12-bit codec.
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
A["Raw depth (uint16 mm / float32 m)"] --> B["Clip to depth_min, depth_max"]
|
||||
B --> C["Quantize to 12-bit code 0–4095 (log or linear)"]
|
||||
C --> D["Pack into gray12le"]
|
||||
D --> E["Encode video (hevc Main 12)"]
|
||||
E --> F[("MP4 + metadata: depth_min/max, shift, use_log")]
|
||||
F -. "load time (depth_output_unit)" .-> G["Dequantize to mm or m"]
|
||||
|
||||
classDef input fill:#e3f2fd,stroke:#1565c0,color:#0d47a1;
|
||||
classDef encode fill:#ede7f6,stroke:#5e35b1,color:#311b92;
|
||||
classDef store fill:#fff8e1,stroke:#f9a825,color:#e65100;
|
||||
classDef load fill:#e8f5e9,stroke:#2e7d32,color:#1b5e20;
|
||||
|
||||
class A input;
|
||||
class B,C,D,E encode;
|
||||
class F store;
|
||||
class G load;
|
||||
```
|
||||
<div style="margin:28px 0;padding:14px 0;">
|
||||
<div style="margin:0 auto;display:flex;flex-wrap:wrap;justify-content:center;align-items:stretch;gap:6px;font-family:'Source Sans 3',ui-sans-serif,system-ui,sans-serif;font-size:14px;font-weight:600;color:#1B1B1D;">
|
||||
<span style="display:flex;flex-direction:column;justify-content:center;align-items:center;text-align:center;gap:2px;background:#DBEAFE;color:#1D4ED8;border-radius:9px;padding:8px 12px;">
|
||||
<span>Raw depth</span>
|
||||
<span style="font-size:11px;font-weight:400;color:#3B6FD4;white-space:nowrap;">
|
||||
uint16 mm
|
||||
<br />
|
||||
float32 m
|
||||
</span>
|
||||
</span>
|
||||
<span style="display:flex;align-items:center;font-size:16px;color:#C3CBD9;">
|
||||
→
|
||||
</span>
|
||||
<div style="border:2px dashed #C4B5FD;border-radius:13px;padding:18px 12px 12px;position:relative;display:flex;align-items:stretch;gap:6px;">
|
||||
<span style="position:absolute;top:-10px;left:12px;background:#fff;padding:0 6px;font-size:11px;font-weight:700;color:#7E22CE;text-transform:uppercase;letter-spacing:0.5px;white-space:nowrap;">
|
||||
Record time
|
||||
</span>
|
||||
<span style="display:flex;flex-direction:column;justify-content:center;align-items:center;text-align:center;gap:2px;background:#F3E8FF;color:#7E22CE;border-radius:9px;padding:8px 12px;">
|
||||
<span>Clip</span>
|
||||
<span style="font-size:11px;font-weight:400;color:#9061C2;white-space:nowrap;">
|
||||
to [depth_min,
|
||||
<br />
|
||||
depth_max]
|
||||
</span>
|
||||
</span>
|
||||
<span style="display:flex;align-items:center;font-size:16px;color:#C3CBD9;">
|
||||
→
|
||||
</span>
|
||||
<span style="display:flex;flex-direction:column;justify-content:center;align-items:center;text-align:center;gap:2px;background:#F3E8FF;color:#7E22CE;border-radius:9px;padding:8px 12px;">
|
||||
<span>Quantize</span>
|
||||
<span style="font-size:11px;font-weight:400;color:#9061C2;white-space:nowrap;">
|
||||
12-bit codes 0–4095
|
||||
<br />
|
||||
log (default) or linear
|
||||
</span>
|
||||
</span>
|
||||
<span style="display:flex;align-items:center;font-size:16px;color:#C3CBD9;">
|
||||
→
|
||||
</span>
|
||||
<span style="display:flex;flex-direction:column;justify-content:center;align-items:center;text-align:center;gap:2px;background:#F3E8FF;color:#7E22CE;border-radius:9px;padding:8px 12px;">
|
||||
<span>Pack</span>
|
||||
<span style="font-size:11px;font-weight:400;color:#9061C2;white-space:nowrap;">
|
||||
into gray12le
|
||||
<br />
|
||||
plane
|
||||
</span>
|
||||
</span>
|
||||
<span style="display:flex;align-items:center;font-size:16px;color:#C3CBD9;">
|
||||
→
|
||||
</span>
|
||||
<span style="display:flex;flex-direction:column;justify-content:center;align-items:center;text-align:center;gap:2px;background:#F3E8FF;color:#7E22CE;border-radius:9px;padding:8px 12px;">
|
||||
<span>Encode</span>
|
||||
<span style="font-size:11px;font-weight:400;color:#9061C2;white-space:nowrap;">
|
||||
HEVC
|
||||
<br />
|
||||
Main 12
|
||||
</span>
|
||||
</span>
|
||||
</div>
|
||||
<span style="display:flex;align-items:center;font-size:16px;color:#C3CBD9;">
|
||||
→
|
||||
</span>
|
||||
<span style="display:flex;flex-direction:column;justify-content:center;align-items:center;text-align:center;gap:2px;background:#FEF3C7;color:#B45309;border-radius:9px;padding:8px 12px;">
|
||||
<span>MP4</span>
|
||||
<span style="font-size:11px;font-weight:400;color:#C77D18;white-space:nowrap;">
|
||||
stored
|
||||
<br />
|
||||
stream
|
||||
</span>
|
||||
</span>
|
||||
<span style="display:flex;align-items:center;font-size:16px;color:#34A06B;">
|
||||
→
|
||||
</span>
|
||||
<div style="border:2px dashed #6EE7B7;border-radius:13px;padding:18px 12px 12px;position:relative;display:flex;align-items:center;gap:6px;">
|
||||
<span style="position:absolute;top:-10px;left:12px;background:#fff;padding:0 6px;font-size:11px;font-weight:700;color:#047857;text-transform:uppercase;letter-spacing:0.5px;white-space:nowrap;">
|
||||
Load time
|
||||
</span>
|
||||
<span style="display:flex;flex-direction:column;justify-content:center;align-items:center;text-align:center;gap:2px;background:#D1FAE5;color:#047857;border-radius:9px;padding:8px 12px;">
|
||||
<span>Dequantize</span>
|
||||
<span style="font-size:11px;font-weight:400;color:#059669;white-space:nowrap;">
|
||||
to mm / m
|
||||
</span>
|
||||
</span>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
Configure the depth pipeline through a parallel **`depth_encoder`** block (`DepthEncoderConfig`). It shares every `RGBEncoderConfig` field (`vcodec`, `pix_fmt`, `crf`, …) and adds four quantizer knobs, set via `--dataset.depth_encoder.<field>`:
|
||||
|
||||
@@ -168,15 +232,16 @@ After the first episode of a video stream is encoded, the encoder configuration
|
||||
|
||||
Two sources contribute to the `info` block:
|
||||
|
||||
- **Stream-derived** (read back from the encoded MP4 with PyAV): `video.height`, `video.width`, `video.codec`, `video.pix_fmt`, `video.fps`, `video.channels`, `is_depth_map`, plus `audio.*` if an audio stream is present.
|
||||
- **Encoder-derived** (taken from `RGBEncoderConfig` or `DepthEncoderConfig`): `video.g`, `video.crf`, `video.preset`, `video.fast_decode`, `video.video_backend`, `video.extra_options`.
|
||||
| Source | Where it comes from | Fields |
|
||||
| ------------------- | ----------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------- |
|
||||
| **Stream-derived** | Read back from the encoded MP4 with PyAV. | `video.height`, `video.width`, `video.codec`, `video.pix_fmt`, `video.fps`, `video.channels`, `is_depth_map`, `audio.*` |
|
||||
| **Encoder-derived** | Taken from `RGBEncoderConfig` / `DepthEncoderConfig`. | `video.g`, `video.crf`, `video.preset`, `video.fast_decode`, `video.video_backend`, `video.extra_options` |
|
||||
|
||||
<Tip>
|
||||
This block is populated **once**, from the **first** episode. It assumes every
|
||||
episode in the dataset was encoded with the same `rgb_encoder`. Changing
|
||||
encoder settings partway through a recording is not supported — the
|
||||
`info.json` will only reflect the parameters used for the first episode.
|
||||
</Tip>
|
||||
> [!IMPORTANT]
|
||||
> This block is populated **once**, from the **first** episode. It assumes every
|
||||
> episode in the dataset was encoded with the same `rgb_encoder`. Changing
|
||||
> encoder settings partway through a recording is not supported — the
|
||||
> `info.json` will only reflect the parameters used for the first episode.
|
||||
|
||||
---
|
||||
|
||||
@@ -184,5 +249,7 @@ Two sources contribute to the `info` block:
|
||||
|
||||
When aggregating datasets with `merge_datasets`, video files are concatenated as-is (no re-encoding), and encoder fields in `info.json` are merged per-key:
|
||||
|
||||
- **Stream-derived fields must match** across sources: `video.codec`, `video.pix_fmt`, `video.height`, `video.width`, `video.fps`. Otherwise FFmpeg's concat demuxer fails.
|
||||
- **Encoder-tuning fields are merged loosely**: `video.g`, `video.crf`, `video.preset`, `video.fast_decode`, `video.extra_options`. If every source agrees, the value is kept; if not, it's set to `null` (or `{}` for `video.extra_options`) and a warning is logged.
|
||||
| Merge rule | Fields | Behaviour |
|
||||
| ------------------ | ---------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------- |
|
||||
| **Must match** | `video.codec`, `video.pix_fmt`, `video.height`, `video.width`, `video.fps` | Stream-derived fields must match across sources, otherwise FFmpeg's concat demuxer fails. |
|
||||
| **Merged loosely** | `video.g`, `video.crf`, `video.preset`, `video.fast_decode`, `video.extra_options` | Encoder-tuning fields. If every source agrees, the value is kept; if not, it's set to `null` (or `{}` for `video.extra_options`) and a warning is logged. |
|
||||
|
||||
@@ -1,182 +0,0 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||
<title>3D Hand Joint Visualizer</title>
|
||||
<style>
|
||||
body {
|
||||
font-family: Arial, sans-serif;
|
||||
margin: 0;
|
||||
padding: 0;
|
||||
overflow: hidden;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
height: 100vh;
|
||||
}
|
||||
|
||||
.controls {
|
||||
padding: 15px;
|
||||
background-color: #f5f5f5;
|
||||
z-index: 100;
|
||||
}
|
||||
|
||||
.status {
|
||||
padding: 10px;
|
||||
border-radius: 5px;
|
||||
margin: 10px 0;
|
||||
}
|
||||
|
||||
.connected {
|
||||
background-color: #d4edda;
|
||||
color: #155724;
|
||||
}
|
||||
|
||||
.disconnected {
|
||||
background-color: #f8d7da;
|
||||
color: #721c24;
|
||||
}
|
||||
|
||||
button {
|
||||
padding: 8px 16px;
|
||||
background-color: #4CAF50;
|
||||
color: white;
|
||||
border: none;
|
||||
border-radius: 4px;
|
||||
cursor: pointer;
|
||||
margin-right: 10px;
|
||||
}
|
||||
|
||||
button:hover {
|
||||
background-color: #45a049;
|
||||
}
|
||||
|
||||
button:disabled {
|
||||
background-color: #cccccc;
|
||||
cursor: not-allowed;
|
||||
}
|
||||
|
||||
.container {
|
||||
display: flex;
|
||||
flex: 1;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
#canvas-container {
|
||||
flex: 3;
|
||||
position: relative;
|
||||
}
|
||||
|
||||
#sidebar {
|
||||
flex: 1;
|
||||
padding: 15px;
|
||||
background-color: #f8f9fa;
|
||||
overflow-y: auto;
|
||||
max-width: 300px;
|
||||
border-left: 1px solid #ddd;
|
||||
}
|
||||
|
||||
.joint-info {
|
||||
margin-bottom: 10px;
|
||||
padding: 8px;
|
||||
border: 1px solid #ddd;
|
||||
border-radius: 4px;
|
||||
}
|
||||
|
||||
.joint-name {
|
||||
font-weight: bold;
|
||||
}
|
||||
|
||||
.joint-value {
|
||||
font-family: monospace;
|
||||
}
|
||||
|
||||
.bar-container {
|
||||
width: 100%;
|
||||
background-color: #e0e0e0;
|
||||
height: 10px;
|
||||
border-radius: 5px;
|
||||
overflow: hidden;
|
||||
margin-top: 5px;
|
||||
}
|
||||
|
||||
.bar {
|
||||
height: 100%;
|
||||
background-color: #4CAF50;
|
||||
width: 0%;
|
||||
transition: width 0.2s ease-in-out;
|
||||
}
|
||||
|
||||
.log-container {
|
||||
margin-top: 20px;
|
||||
border: 1px solid #ddd;
|
||||
border-radius: 5px;
|
||||
padding: 10px;
|
||||
height: 150px;
|
||||
overflow-y: auto;
|
||||
font-family: monospace;
|
||||
background-color: #f8f9fa;
|
||||
}
|
||||
|
||||
.view-controls {
|
||||
position: absolute;
|
||||
bottom: 10px;
|
||||
left: 10px;
|
||||
z-index: 10;
|
||||
}
|
||||
|
||||
.view-button {
|
||||
background-color: rgba(0, 0, 0, 0.5);
|
||||
color: white;
|
||||
border: none;
|
||||
padding: 5px 10px;
|
||||
margin-right: 5px;
|
||||
border-radius: 3px;
|
||||
cursor: pointer;
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<div class="controls">
|
||||
<button id="connectButton">Connect to Device</button>
|
||||
<button id="disconnectButton" disabled>Disconnect</button>
|
||||
<select id="baudRate">
|
||||
<option value="9600">9600</option>
|
||||
<option value="19200">19200</option>
|
||||
<option value="38400">38400</option>
|
||||
<option value="57600">57600</option>
|
||||
<option value="115200" selected>115200</option>
|
||||
</select>
|
||||
<span id="statusIndicator" class="status disconnected">Status: Disconnected</span>
|
||||
</div>
|
||||
|
||||
<div class="container">
|
||||
<div id="canvas-container">
|
||||
<!-- 3D canvas will be inserted here -->
|
||||
<div class="view-controls">
|
||||
<button class="view-button" id="frontView">Front</button>
|
||||
<button class="view-button" id="sideView">Side</button>
|
||||
<button class="view-button" id="topView">Top</button>
|
||||
<button class="view-button" id="resetView">Reset</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="sidebar">
|
||||
<h3>Joint Values</h3>
|
||||
<div id="jointsContainer">
|
||||
<!-- Joint info will be added here -->
|
||||
</div>
|
||||
|
||||
<div class="log-container" id="logContainer">
|
||||
<!-- Log messages will be added here -->
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Import Three.js -->
|
||||
<script src="https://cdn.jsdelivr.net/npm/three@0.132.2/build/three.min.js"></script>
|
||||
<script src="https://cdn.jsdelivr.net/npm/three@0.132.2/examples/js/controls/OrbitControls.js"></script>
|
||||
|
||||
<script src="script.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -1,669 +0,0 @@
|
||||
// === Hand Visualizer with Pre-Connect Sliders + Per-Joint Angle Limits ===
|
||||
// Assumes your HTML already has elements with the following IDs:
|
||||
// connectButton, disconnectButton, baudRate, statusIndicator, jointsContainer, logContainer,
|
||||
// canvas-container, frontView, sideView, topView, resetView
|
||||
// Requires Three.js + OrbitControls loaded on the page.
|
||||
|
||||
// -------------------- Config --------------------
|
||||
const MAX_JOINTS = 16;
|
||||
const RAW_MIN = 0, RAW_MAX = 4096;
|
||||
const RAW_CENTER = (RAW_MIN + RAW_MAX) / 2;
|
||||
const DEG = Math.PI / 180;
|
||||
const UI_DEG_MIN = -90, UI_DEG_MAX = 90; // UI sliders for angle limits
|
||||
|
||||
// -------------------- State --------------------
|
||||
let port;
|
||||
let reader;
|
||||
let keepReading = false;
|
||||
let isConnected = false;
|
||||
const decoder = new TextDecoder();
|
||||
let inputBuffer = '';
|
||||
|
||||
let jointValues = new Array(MAX_JOINTS).fill(RAW_CENTER);
|
||||
|
||||
// Auto-calibration: track observed min/max per joint
|
||||
let observedMin = new Array(MAX_JOINTS).fill(Infinity);
|
||||
let observedMax = new Array(MAX_JOINTS).fill(-Infinity);
|
||||
let calibrationEnabled = true;
|
||||
|
||||
// Three.js
|
||||
let scene, camera, renderer, controls;
|
||||
let hand = { palm: null, fingers: [] };
|
||||
|
||||
// DOM
|
||||
const connectButton = document.getElementById('connectButton');
|
||||
const disconnectButton = document.getElementById('disconnectButton');
|
||||
const baudRateSelect = document.getElementById('baudRate');
|
||||
const statusIndicator = document.getElementById('statusIndicator');
|
||||
const jointsContainer = document.getElementById('jointsContainer');
|
||||
const logContainer = document.getElementById('logContainer');
|
||||
const canvasContainer = document.getElementById('canvas-container');
|
||||
const frontViewBtn = document.getElementById('frontView');
|
||||
const sideViewBtn = document.getElementById('sideView');
|
||||
const topViewBtn = document.getElementById('topView');
|
||||
const resetViewBtn = document.getElementById('resetView');
|
||||
|
||||
// Helpers
|
||||
const clamp = (x, a, b) => Math.max(a, Math.min(b, x));
|
||||
const invLerp = (a, b, x) => clamp((x - a) / (b - a), 0, 1);
|
||||
|
||||
// -------------------- Joint Map with per-joint angle limits --------------------
|
||||
const fingerJointMap = [
|
||||
// Thumb (4)
|
||||
{ finger:0, joint:0, type:'CMC_ABDUCTION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:0, joint:1, type:'CMC_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:0, joint:2, type:'MCP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true }, // +45° only
|
||||
{ finger:0, joint:3, type:'IP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true }, // +45° only
|
||||
|
||||
// Index (3)
|
||||
{ finger:1, joint:0, type:'MCP_ABDUCTION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:1, joint:1, type:'MCP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:false },
|
||||
{ finger:1, joint:2, type:'PIP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true }, // +45° only
|
||||
|
||||
// Middle (3)
|
||||
{ finger:2, joint:0, type:'MCP_ABDUCTION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:2, joint:1, type:'MCP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:2, joint:2, type:'PIP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true }, // +45° only
|
||||
|
||||
// Ring (3)
|
||||
{ finger:3, joint:0, type:'MCP_ABDUCTION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:3, joint:1, type:'MCP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:false },
|
||||
{ finger:3, joint:2, type:'PIP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:false }, // +45° only
|
||||
|
||||
// Pinky (3)
|
||||
{ finger:4, joint:0, type:'MCP_ABDUCTION', min:RAW_MIN, max:RAW_MAX, inverted:false },
|
||||
{ finger:4, joint:1, type:'MCP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:false },
|
||||
{ finger:4, joint:2, type:'PIP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:false } // +45° only
|
||||
];
|
||||
|
||||
// Assign angle limits (radians) per joint (default ±45°, exceptions: +45° only)
|
||||
for (const j of fingerJointMap) {
|
||||
const isThumb = j.finger === 0;
|
||||
const isPIP = j.type === 'PIP_FLEXION';
|
||||
let minA = -45 * DEG, maxA = +45 * DEG;
|
||||
if ((isThumb && (j.type === 'MCP_FLEXION' || j.type === 'IP_FLEXION')) || (!isThumb && isPIP)) {
|
||||
minA = 0;
|
||||
maxA = +45 * DEG;
|
||||
}
|
||||
j.angleMin = minA;
|
||||
j.angleMax = maxA;
|
||||
}
|
||||
|
||||
// -------------------- UI: Joint Panel --------------------
|
||||
const uiRefs = []; // per joint: { valueLabel, bar, barWrap, slider, invertChk, minDeg, maxDeg }
|
||||
|
||||
function initializeJointElements() {
|
||||
jointsContainer.innerHTML = '';
|
||||
uiRefs.length = 0;
|
||||
|
||||
for (let i = 0; i < MAX_JOINTS; i++) {
|
||||
const wrap = document.createElement('div');
|
||||
wrap.className = 'joint-info';
|
||||
|
||||
const fingerIndex = i < 4 ? 0 : Math.floor((i - 4) / 3) + 1;
|
||||
const jointInfo = fingerJointMap[i];
|
||||
const jointType = jointInfo?.type || 'Unknown';
|
||||
const fingerName = ['Thumb', 'Index', 'Middle', 'Ring', 'Pinky'][fingerIndex];
|
||||
|
||||
// Header
|
||||
const nameEl = document.createElement('div');
|
||||
nameEl.className = 'joint-name';
|
||||
nameEl.textContent = `${fingerName} – ${jointType}`;
|
||||
|
||||
// Value + bar
|
||||
const valueEl = document.createElement('div');
|
||||
valueEl.className = 'joint-value';
|
||||
valueEl.textContent = `Value: ${jointValues[i]}`;
|
||||
|
||||
const barWrap = document.createElement('div');
|
||||
barWrap.className = 'bar-container';
|
||||
const barEl = document.createElement('div');
|
||||
barEl.className = 'bar';
|
||||
barWrap.appendChild(barEl);
|
||||
|
||||
// Slider for pre-connect manual control
|
||||
const slider = document.createElement('input');
|
||||
slider.type = 'range';
|
||||
slider.min = String(RAW_MIN);
|
||||
slider.max = String(RAW_MAX);
|
||||
slider.value = String(jointValues[i]);
|
||||
slider.step = '1';
|
||||
slider.className = 'joint-slider';
|
||||
|
||||
slider.addEventListener('input', () => {
|
||||
if (isConnected) return; // ignore while connected
|
||||
let v = parseInt(slider.value, 10);
|
||||
if (jointInfo?.inverted) v = (jointInfo.min + jointInfo.max) - v;
|
||||
jointValues[i] = clamp(jointInfo ? v : 0, RAW_MIN, RAW_MAX);
|
||||
updateJointDisplay(i, jointValues[i]);
|
||||
updateHandModel();
|
||||
});
|
||||
|
||||
// Invert checkbox
|
||||
const invertLbl = document.createElement('label');
|
||||
invertLbl.className = 'invert-toggle';
|
||||
const invertChk = document.createElement('input');
|
||||
invertChk.type = 'checkbox';
|
||||
invertChk.checked = !!jointInfo?.inverted;
|
||||
invertChk.addEventListener('change', () => {
|
||||
if (jointInfo) jointInfo.inverted = invertChk.checked;
|
||||
addLogMessage(`${fingerName} ${jointType} inversion ${invertChk.checked ? 'enabled' : 'disabled'}`);
|
||||
});
|
||||
invertLbl.appendChild(invertChk);
|
||||
invertLbl.appendChild(document.createTextNode('Invert Values'));
|
||||
|
||||
// Angle limits (deg) controls
|
||||
const limitsRow = document.createElement('div');
|
||||
limitsRow.className = 'limits-row';
|
||||
|
||||
const minDeg = document.createElement('input');
|
||||
minDeg.type = 'number';
|
||||
minDeg.min = String(UI_DEG_MIN);
|
||||
minDeg.max = String(UI_DEG_MAX);
|
||||
minDeg.step = '1';
|
||||
minDeg.value = String(Math.round((jointInfo.angleMin || 0) / DEG));
|
||||
minDeg.className = 'limit-num';
|
||||
|
||||
const maxDeg = document.createElement('input');
|
||||
maxDeg.type = 'number';
|
||||
maxDeg.min = String(UI_DEG_MIN);
|
||||
maxDeg.max = String(UI_DEG_MAX);
|
||||
maxDeg.step = '1';
|
||||
maxDeg.value = String(Math.round((jointInfo.angleMax || 0) / DEG));
|
||||
maxDeg.className = 'limit-num';
|
||||
|
||||
const minLbl = document.createElement('span'); minLbl.textContent = 'min°';
|
||||
const maxLbl = document.createElement('span'); maxLbl.textContent = 'max°';
|
||||
minLbl.className = 'limit-label'; maxLbl.className = 'limit-label';
|
||||
|
||||
function syncLimits() {
|
||||
let mn = parseFloat(minDeg.value);
|
||||
let mx = parseFloat(maxDeg.value);
|
||||
if (isNaN(mn)) mn = -45;
|
||||
if (isNaN(mx)) mx = +45;
|
||||
if (mn > mx) [mn, mx] = [mx, mn];
|
||||
jointInfo.angleMin = clamp(mn, UI_DEG_MIN, UI_DEG_MAX) * DEG;
|
||||
jointInfo.angleMax = clamp(mx, UI_DEG_MIN, UI_DEG_MAX) * DEG;
|
||||
minDeg.value = String(Math.round(jointInfo.angleMin / DEG));
|
||||
maxDeg.value = String(Math.round(jointInfo.angleMax / DEG));
|
||||
updateHandModel();
|
||||
}
|
||||
minDeg.addEventListener('change', syncLimits);
|
||||
maxDeg.addEventListener('change', syncLimits);
|
||||
|
||||
limitsRow.appendChild(minLbl);
|
||||
limitsRow.appendChild(minDeg);
|
||||
limitsRow.appendChild(maxLbl);
|
||||
limitsRow.appendChild(maxDeg);
|
||||
|
||||
// Calibration controls
|
||||
const calibRow = document.createElement('div');
|
||||
calibRow.className = 'calib-row';
|
||||
|
||||
const resetCalibBtn = document.createElement('button');
|
||||
resetCalibBtn.textContent = 'Reset Calib';
|
||||
resetCalibBtn.className = 'calib-btn';
|
||||
resetCalibBtn.addEventListener('click', () => {
|
||||
observedMin[i] = Infinity;
|
||||
observedMax[i] = -Infinity;
|
||||
addLogMessage(`Reset calibration for ${fingerName} ${jointType}`);
|
||||
});
|
||||
|
||||
const calibStatus = document.createElement('span');
|
||||
calibStatus.className = 'calib-status';
|
||||
calibStatus.textContent = `Range: --`;
|
||||
|
||||
calibRow.appendChild(resetCalibBtn);
|
||||
calibRow.appendChild(calibStatus);
|
||||
|
||||
// Compose
|
||||
wrap.appendChild(nameEl);
|
||||
wrap.appendChild(valueEl);
|
||||
wrap.appendChild(barWrap);
|
||||
wrap.appendChild(slider);
|
||||
wrap.appendChild(invertLbl);
|
||||
wrap.appendChild(limitsRow);
|
||||
wrap.appendChild(calibRow);
|
||||
|
||||
jointsContainer.appendChild(wrap);
|
||||
|
||||
uiRefs[i] = { valueLabel: valueEl, bar: barEl, barWrap, slider, invertChk, minDeg, maxDeg, nameEl, calibStatus };
|
||||
}
|
||||
|
||||
setConnectedUI(false); // initial state: sliders active
|
||||
}
|
||||
|
||||
// Toggle UI between pre-connect SLIDERS vs post-connect BARS
|
||||
function setConnectedUI(connected) {
|
||||
isConnected = connected;
|
||||
for (let i = 0; i < uiRefs.length; i++) {
|
||||
const ui = uiRefs[i];
|
||||
if (!ui) continue;
|
||||
// Show bars when connected; sliders disabled/hidden
|
||||
ui.barWrap.style.display = connected ? '' : 'none';
|
||||
ui.slider.disabled = connected;
|
||||
ui.slider.style.display = connected ? 'none' : '';
|
||||
}
|
||||
|
||||
// Reset calibration when connecting
|
||||
if (connected) {
|
||||
observedMin.fill(Infinity);
|
||||
observedMax.fill(-Infinity);
|
||||
addLogMessage('Calibration reset - move joints through full range for best results');
|
||||
}
|
||||
}
|
||||
|
||||
// Update joint display (value text + bar color/width + slider position if needed)
|
||||
function updateJointDisplay(jointIndex, value) {
|
||||
const ui = uiRefs[jointIndex];
|
||||
const info = fingerJointMap[jointIndex];
|
||||
if (!ui || !info) return;
|
||||
|
||||
ui.valueLabel.textContent = `Value: ${value}`;
|
||||
|
||||
// bar
|
||||
const min = info.min, max = info.max;
|
||||
const pct = clamp((value - min) / (max - min), 0, 1) * 100;
|
||||
ui.bar.style.width = `${pct}%`;
|
||||
const hue = Math.floor(pct * 1.2); // 0..120
|
||||
ui.bar.style.backgroundColor = `hsl(${hue}, 80%, 50%)`;
|
||||
|
||||
// slider (only meaningful when not connected; keep in sync anyway)
|
||||
const rawForSlider = info.inverted ? (info.min + info.max) - value : value;
|
||||
if (!isConnected) ui.slider.value = String(clamp(Math.round(rawForSlider), RAW_MIN, RAW_MAX));
|
||||
}
|
||||
|
||||
// -------------------- Serial I/O --------------------
|
||||
async function readSerialData() {
|
||||
while (port?.readable && keepReading) {
|
||||
reader = port.readable.getReader();
|
||||
try {
|
||||
while (true) {
|
||||
const { value, done } = await reader.read();
|
||||
if (done) break;
|
||||
if (value) processData(decoder.decode(value));
|
||||
}
|
||||
} catch (err) {
|
||||
console.error('Error reading:', err);
|
||||
addLogMessage(`Error: ${err.message}`);
|
||||
break;
|
||||
} finally {
|
||||
reader.releaseLock();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
function processData(chunk) {
|
||||
inputBuffer += chunk;
|
||||
let idx;
|
||||
while ((idx = inputBuffer.indexOf('\n')) !== -1) {
|
||||
const line = inputBuffer.slice(0, idx).trim();
|
||||
inputBuffer = inputBuffer.slice(idx + 1);
|
||||
|
||||
const vals = line.split(/\s+/).map(v => parseInt(v, 10));
|
||||
if (vals.length === MAX_JOINTS && vals.every(v => Number.isFinite(v))) {
|
||||
for (let i = 0; i < MAX_JOINTS; i++) {
|
||||
const info = fingerJointMap[i];
|
||||
if (!info) continue;
|
||||
|
||||
let rawValue = vals[i];
|
||||
|
||||
// Update calibration tracking
|
||||
if (calibrationEnabled) {
|
||||
observedMin[i] = Math.min(observedMin[i], rawValue);
|
||||
observedMax[i] = Math.max(observedMax[i], rawValue);
|
||||
|
||||
// Update calibration display
|
||||
const ui = uiRefs[i];
|
||||
if (ui && ui.calibStatus) {
|
||||
if (observedMin[i] !== Infinity && observedMax[i] !== -Infinity) {
|
||||
ui.calibStatus.textContent = `Range: ${observedMin[i]}-${observedMax[i]}`;
|
||||
}
|
||||
}
|
||||
|
||||
// Remap observed range to target range
|
||||
if (observedMin[i] !== Infinity && observedMax[i] !== -Infinity && observedMax[i] > observedMin[i]) {
|
||||
const observedRange = observedMax[i] - observedMin[i];
|
||||
const targetRange = info.max - info.min;
|
||||
const normalizedValue = (rawValue - observedMin[i]) / observedRange;
|
||||
rawValue = info.min + (normalizedValue * targetRange);
|
||||
}
|
||||
}
|
||||
|
||||
let v = clamp(rawValue, info.min, info.max);
|
||||
if (info.inverted) v = (info.min + info.max) - v;
|
||||
jointValues[i] = v;
|
||||
updateJointDisplay(i, v);
|
||||
}
|
||||
updateHandModel();
|
||||
} else {
|
||||
addLogMessage(`Received: ${line}`);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
async function connectToDevice() {
|
||||
try {
|
||||
port = await navigator.serial.requestPort();
|
||||
const baudRate = parseInt(baudRateSelect.value, 10) || 115200;
|
||||
await port.open({ baudRate });
|
||||
|
||||
keepReading = true;
|
||||
setConnectedUI(true);
|
||||
|
||||
statusIndicator.textContent = 'Status: Connected';
|
||||
statusIndicator.className = 'status connected';
|
||||
connectButton.disabled = true;
|
||||
disconnectButton.disabled = false;
|
||||
baudRateSelect.disabled = true;
|
||||
|
||||
addLogMessage(`Connected at ${baudRate} baud`);
|
||||
readSerialData();
|
||||
} catch (e) {
|
||||
console.error('Connect error:', e);
|
||||
addLogMessage(`Connection error: ${e.message}`);
|
||||
}
|
||||
}
|
||||
|
||||
async function disconnectFromDevice() {
|
||||
try {
|
||||
keepReading = false;
|
||||
if (reader) {
|
||||
try { reader.cancel(); } catch {}
|
||||
}
|
||||
if (port) {
|
||||
await port.close();
|
||||
port = null;
|
||||
}
|
||||
} catch (e) {
|
||||
console.error('Disconnect error:', e);
|
||||
addLogMessage(`Disconnection error: ${e.message}`);
|
||||
} finally {
|
||||
setConnectedUI(false);
|
||||
statusIndicator.textContent = 'Status: Disconnected';
|
||||
statusIndicator.className = 'status disconnected';
|
||||
connectButton.disabled = false;
|
||||
disconnectButton.disabled = true;
|
||||
baudRateSelect.disabled = false;
|
||||
addLogMessage('Disconnected');
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------- Three.js Scene --------------------
|
||||
function initThreeJS() {
|
||||
scene = new THREE.Scene();
|
||||
scene.background = new THREE.Color(0xf0f0f0);
|
||||
|
||||
camera = new THREE.PerspectiveCamera(
|
||||
75,
|
||||
canvasContainer.clientWidth / canvasContainer.clientHeight,
|
||||
0.1, 1000
|
||||
);
|
||||
camera.position.set(0, 15, 15);
|
||||
camera.lookAt(0, 0, 0);
|
||||
|
||||
renderer = new THREE.WebGLRenderer({ antialias: true });
|
||||
renderer.setSize(canvasContainer.clientWidth, canvasContainer.clientHeight);
|
||||
renderer.setPixelRatio(window.devicePixelRatio);
|
||||
canvasContainer.appendChild(renderer.domElement);
|
||||
|
||||
controls = new THREE.OrbitControls(camera, renderer.domElement);
|
||||
controls.enableDamping = true;
|
||||
controls.dampingFactor = 0.25;
|
||||
|
||||
const ambientLight = new THREE.AmbientLight(0x404040);
|
||||
scene.add(ambientLight);
|
||||
const dir1 = new THREE.DirectionalLight(0xffffff, 0.5);
|
||||
dir1.position.set(1, 1, 1);
|
||||
scene.add(dir1);
|
||||
const dir2 = new THREE.DirectionalLight(0xffffff, 0.3);
|
||||
dir2.position.set(-1, 1, -1);
|
||||
scene.add(dir2);
|
||||
|
||||
const gridHelper = new THREE.GridHelper(20, 20);
|
||||
scene.add(gridHelper);
|
||||
|
||||
createHandModel();
|
||||
window.addEventListener('resize', onWindowResize);
|
||||
animate();
|
||||
}
|
||||
|
||||
function createHandModel() {
|
||||
const palmMaterial = new THREE.MeshPhongMaterial({ color: 0xf5c396 });
|
||||
const fingerMaterial = new THREE.MeshPhongMaterial({ color: 0xf5c396 });
|
||||
const jointMaterial = new THREE.MeshPhongMaterial({ color: 0xe3a977 });
|
||||
|
||||
const palmGeometry = new THREE.BoxGeometry(7, 1, 8);
|
||||
hand.palm = new THREE.Mesh(palmGeometry, palmMaterial);
|
||||
hand.palm.position.set(0, 0, 0);
|
||||
hand.palm.rotation.x = Math.PI / 2; // hand vertical, palm facing forward
|
||||
scene.add(hand.palm);
|
||||
|
||||
const fingerWidth = 1, fingerHeight = 0.8;
|
||||
const fingerSegmentLengths = [3, 2, 1.5];
|
||||
const thumbSegmentLengths = [2, 2, 1.5];
|
||||
|
||||
const fingerBasePositions = [
|
||||
[ 3, 0, -2], // Thumb
|
||||
[ 1.5,-0.5,-4], // Index
|
||||
[ 0, -0.5,-4], // Middle
|
||||
[-1.5,-0.5,-4], // Ring
|
||||
[-3, -0.5,-4], // Pinky
|
||||
];
|
||||
const fingerBaseRot = [
|
||||
{ x:0, y:-Math.PI/3, z: Math.PI/3 }, // Thumb
|
||||
{ x:0, y:-Math.PI/48, z: 0 },
|
||||
{ x:0, y: Math.PI/48, z: 0 },
|
||||
{ x:0, y: Math.PI/32, z: 0 },
|
||||
{ x:0, y: Math.PI/24, z: 0 }
|
||||
];
|
||||
|
||||
for (let fIdx = 0; fIdx < 5; fIdx++) {
|
||||
const finger = { name:['Thumb','Index','Middle','Ring','Pinky'][fIdx], segments:[], joints:[] };
|
||||
const isThumb = fIdx === 0;
|
||||
const segLens = isThumb ? thumbSegmentLengths : fingerSegmentLengths;
|
||||
|
||||
finger.group = new THREE.Group();
|
||||
finger.group.position.set(...fingerBasePositions[fIdx]);
|
||||
finger.group.rotation.x = fingerBaseRot[fIdx].x;
|
||||
finger.group.rotation.y = fingerBaseRot[fIdx].y;
|
||||
finger.group.rotation.z = fingerBaseRot[fIdx].z;
|
||||
finger.group.userData.baseRot = {
|
||||
x:finger.group.rotation.x,
|
||||
y:finger.group.rotation.y,
|
||||
z:finger.group.rotation.z
|
||||
};
|
||||
hand.palm.add(finger.group);
|
||||
|
||||
let parent = finger.group;
|
||||
for (let s = 0; s < segLens.length; s++) {
|
||||
const segGroup = new THREE.Group();
|
||||
|
||||
const jGeom = new THREE.SphereGeometry(fingerWidth * 0.6, 8, 8);
|
||||
const joint = new THREE.Mesh(jGeom, jointMaterial);
|
||||
segGroup.add(joint);
|
||||
|
||||
const segGeom = new THREE.BoxGeometry(fingerWidth, fingerHeight, segLens[s]);
|
||||
const seg = new THREE.Mesh(segGeom, fingerMaterial);
|
||||
seg.position.z = -segLens[s] / 2;
|
||||
segGroup.add(seg);
|
||||
|
||||
parent.add(segGroup);
|
||||
|
||||
finger.segments.push(segGroup);
|
||||
finger.joints.push(joint);
|
||||
|
||||
if (s < segLens.length - 1) {
|
||||
const connector = new THREE.Group();
|
||||
connector.position.z = -segLens[s];
|
||||
segGroup.add(connector);
|
||||
parent = connector;
|
||||
}
|
||||
}
|
||||
|
||||
hand.fingers.push(finger);
|
||||
}
|
||||
|
||||
addFingerLabels();
|
||||
addHandLabel();
|
||||
}
|
||||
|
||||
function addFingerLabels() {
|
||||
const names = ['Thumb','Index','Middle','Ring','Pinky'];
|
||||
for (let i = 0; i < hand.fingers.length; i++) {
|
||||
const finger = hand.fingers[i];
|
||||
const canvas = document.createElement('canvas');
|
||||
const ctx = canvas.getContext('2d');
|
||||
canvas.width = 128; canvas.height = 32;
|
||||
ctx.fillStyle = '#ffffff'; ctx.fillRect(0,0,canvas.width,canvas.height);
|
||||
ctx.font = 'bold 16px Arial';
|
||||
ctx.fillStyle = '#000000';
|
||||
ctx.textAlign = 'center'; ctx.textBaseline = 'middle';
|
||||
ctx.fillText(names[i], canvas.width/2, canvas.height/2);
|
||||
|
||||
const texture = new THREE.CanvasTexture(canvas);
|
||||
const geom = new THREE.PlaneGeometry(2, 0.5);
|
||||
const mat = new THREE.MeshBasicMaterial({ map:texture, transparent:true, side:THREE.DoubleSide });
|
||||
const label = new THREE.Mesh(geom, mat);
|
||||
label.position.set(0, -1.5, -2);
|
||||
label.rotation.x = Math.PI / 2;
|
||||
finger.group.add(label);
|
||||
}
|
||||
}
|
||||
|
||||
function addHandLabel() {
|
||||
const canvas = document.createElement('canvas');
|
||||
const ctx = canvas.getContext('2d');
|
||||
canvas.width = 256; canvas.height = 64;
|
||||
ctx.fillStyle = '#ffffff'; ctx.fillRect(0,0,canvas.width,canvas.height);
|
||||
ctx.font = 'bold 24px Arial';
|
||||
ctx.fillStyle = '#000000';
|
||||
ctx.textAlign = 'center'; ctx.textBaseline = 'middle';
|
||||
ctx.fillText('RIGHT HAND (VERTICAL)', canvas.width/2, canvas.height/2);
|
||||
|
||||
const texture = new THREE.CanvasTexture(canvas);
|
||||
const geom = new THREE.PlaneGeometry(7, 1.75);
|
||||
const mat = new THREE.MeshBasicMaterial({ map:texture, transparent:true, side:THREE.DoubleSide });
|
||||
const label = new THREE.Mesh(geom, mat);
|
||||
label.position.set(0, -2, 0);
|
||||
label.rotation.x = Math.PI / 2;
|
||||
scene.add(label);
|
||||
}
|
||||
|
||||
function updateHandModel() {
|
||||
for (let i = 0; i < MAX_JOINTS; i++) {
|
||||
const info = fingerJointMap[i];
|
||||
if (!info) continue;
|
||||
const { finger, joint, type, min, max, angleMin, angleMax } = info;
|
||||
const raw = jointValues[i];
|
||||
const f = hand.fingers[finger];
|
||||
if (!f) continue;
|
||||
|
||||
const center = (min + max) / 2;
|
||||
let angle = 0;
|
||||
|
||||
if (type.includes('ABDUCTION')) {
|
||||
// symmetric around neutral
|
||||
const k = clamp((raw - center) / ((max - min) / 2), -1, 1);
|
||||
angle = angleMin + (k + 1) * 0.5 * (angleMax - angleMin);
|
||||
|
||||
const base = f.group.userData.baseRot || {x:0,y:0,z:0};
|
||||
if (finger === 0 && joint === 0) {
|
||||
// Thumb: abduction about Z (toward/away from palm)
|
||||
f.group.rotation.z = base.z + angle;
|
||||
} else {
|
||||
// Other fingers: side-to-side about Y
|
||||
f.group.rotation.y = base.y + angle;
|
||||
}
|
||||
} else if (type.includes('FLEXION')) {
|
||||
const isThumb = finger === 0;
|
||||
const isMCP = type === 'MCP_FLEXION';
|
||||
const isPIP = type === 'PIP_FLEXION';
|
||||
const positiveOnly = (isThumb && (type === 'MCP_FLEXION' || type === 'IP_FLEXION')) || (!isThumb && isPIP);
|
||||
|
||||
if (positiveOnly) {
|
||||
const t = raw <= center ? 0 : invLerp(center, max, raw); // 0..1
|
||||
angle = angleMin + t * (angleMax - angleMin); // 0..+limit
|
||||
} else {
|
||||
const k = clamp((raw - center) / ((max - min) / 2), -1, 1);
|
||||
angle = angleMin + (k + 1) * 0.5 * (angleMax - angleMin);
|
||||
}
|
||||
|
||||
if (isMCP) {
|
||||
// MCP flexion applies to the finger base group (same as abduction)
|
||||
const base = f.group.userData.baseRot || {x:0,y:0,z:0};
|
||||
f.group.rotation.x = base.x + angle;
|
||||
} else if (f.segments[joint]) {
|
||||
// PIP/DIP/IP flexion applies to individual segments
|
||||
f.segments[joint].rotation.x = angle;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------- Render Loop --------------------
|
||||
function onWindowResize() {
|
||||
camera.aspect = canvasContainer.clientWidth / canvasContainer.clientHeight;
|
||||
camera.updateProjectionMatrix();
|
||||
renderer.setSize(canvasContainer.clientWidth, canvasContainer.clientHeight);
|
||||
}
|
||||
|
||||
function animate() {
|
||||
requestAnimationFrame(animate);
|
||||
controls.update();
|
||||
renderer.render(scene, camera);
|
||||
}
|
||||
|
||||
// -------------------- Misc UI --------------------
|
||||
function addLogMessage(msg) {
|
||||
const el = document.createElement('div');
|
||||
el.textContent = msg;
|
||||
logContainer.appendChild(el);
|
||||
logContainer.scrollTop = logContainer.scrollHeight;
|
||||
while (logContainer.children.length > 100) {
|
||||
logContainer.removeChild(logContainer.firstChild);
|
||||
}
|
||||
}
|
||||
|
||||
// Camera view controls
|
||||
frontViewBtn?.addEventListener('click', () => { camera.position.set(0, 0, 20); camera.lookAt(0,0,0); controls.update(); });
|
||||
sideViewBtn?.addEventListener('click', () => { camera.position.set(20, 0, 0); camera.lookAt(0,0,0); controls.update(); });
|
||||
topViewBtn?.addEventListener('click', () => { camera.position.set(0, 20, 0); camera.lookAt(0,0,0); controls.update(); });
|
||||
resetViewBtn?.addEventListener('click', () => { camera.position.set(10,10,10); camera.lookAt(0,0,0); controls.update(); });
|
||||
|
||||
// Serial connect buttons
|
||||
connectButton?.addEventListener('click', connectToDevice);
|
||||
disconnectButton?.addEventListener('click', disconnectFromDevice);
|
||||
|
||||
// Web Serial support check
|
||||
if (!navigator.serial) {
|
||||
statusIndicator.textContent = 'Status: Web Serial API not supported in this browser';
|
||||
connectButton.disabled = true;
|
||||
addLogMessage('ERROR: Web Serial API is not supported in this browser. Try Chrome or Edge.');
|
||||
}
|
||||
|
||||
// -------------------- Boot --------------------
|
||||
initThreeJS();
|
||||
initializeJointElements();
|
||||
|
||||
// -------------------- Styles (inline) --------------------
|
||||
const styleElement = document.createElement('style');
|
||||
styleElement.textContent = `
|
||||
.joint-info { border-bottom: 1px solid #eee; padding: 8px 0; }
|
||||
.joint-name { font-weight: 600; margin-bottom: 4px; }
|
||||
.joint-value { font-size: 12px; color: #333; margin-bottom: 4px; }
|
||||
.bar-container { width: 100%; height: 8px; background: #ddd; border-radius: 4px; overflow: hidden; }
|
||||
.bar { height: 100%; width: 0%; background: #4caf50; }
|
||||
.joint-slider { width: 100%; margin: 6px 0; }
|
||||
.invert-toggle { display: inline-flex; align-items: center; gap: 6px; margin-top: 4px; font-size: 12px; color: #555; }
|
||||
.limits-row { display: flex; align-items: center; gap: 6px; margin-top: 6px; flex-wrap: wrap; }
|
||||
.limit-label { font-size: 11px; color: #666; }
|
||||
.limit-num { width: 60px; }
|
||||
.calib-row { display: flex; align-items: center; gap: 8px; margin-top: 4px; }
|
||||
.calib-btn { padding: 2px 6px; font-size: 11px; background: #f44336; color: white; border: none; border-radius: 3px; cursor: pointer; }
|
||||
.calib-btn:hover { background: #d32f2f; }
|
||||
.calib-status { font-size: 11px; color: #666; }
|
||||
.status.connected { color: #0a0; }
|
||||
.status.disconnected { color: #a00; }
|
||||
`;
|
||||
document.head.appendChild(styleElement);
|
||||
@@ -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/<robot.name>/<robot.id>.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/<teleop.id>.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=<hf_user>/<dataset_name> \
|
||||
--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
|
||||
```
|
||||
@@ -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."""
|
||||
@@ -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(" <could not determine — check `hostname -I` / `ip addr`>")
|
||||
print(f" 3. Accept the self-signed cert at https://<that-ip>:{_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 <path>`` 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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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=<hf_user>/<dataset_name> \\
|
||||
--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=<hf_user>/<dataset_name> --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()
|
||||
@@ -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.<field>=...). 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()
|
||||
+1
-1
@@ -25,7 +25,7 @@ discord = "https://discord.gg/s3KuuzsPFb"
|
||||
|
||||
[project]
|
||||
name = "lerobot"
|
||||
version = "0.5.2"
|
||||
version = "0.6.0"
|
||||
description = "🤗 LeRobot: State-of-the-art Machine Learning for Real-World Robotics in Pytorch"
|
||||
dynamic = ["readme"]
|
||||
license = { text = "Apache-2.0" }
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
version = 1
|
||||
revision = 2
|
||||
revision = 3
|
||||
requires-python = ">=3.12"
|
||||
resolution-markers = [
|
||||
"(python_full_version >= '3.15' and platform_machine == 'AMD64' and sys_platform == 'linux') or (python_full_version >= '3.15' and platform_machine == 'x86_64' and sys_platform == 'linux')",
|
||||
@@ -2127,16 +2127,16 @@ wheels = [
|
||||
|
||||
[[package]]
|
||||
name = "hydra-core"
|
||||
version = "1.3.3"
|
||||
version = "1.3.4"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
dependencies = [
|
||||
{ name = "antlr4-python3-runtime", marker = "sys_platform == 'linux'" },
|
||||
{ name = "omegaconf", marker = "sys_platform == 'linux'" },
|
||||
{ name = "packaging", marker = "sys_platform == 'linux'" },
|
||||
]
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/0d/0b/7c0d941311aadc6479ec01767edba9c8a07db1452685de3567ed3058d0c9/hydra_core-1.3.3.tar.gz", hash = "sha256:b7477ee21f08b62f71bf0126d44695c048dc7e9c0cc79e2d593b707cb1e44048", size = 3262532, upload-time = "2026-06-11T05:54:26.835Z" }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/10/dd/220f0e91743136725352497e98540772a01fc7c3ab96ff16c3c74424e984/hydra_core-1.3.4.tar.gz", hash = "sha256:ad0f7b05a0242255a8984d5a4ed2f6847f7b783ed727368a2c0155ec52d6c34c", size = 3263348, upload-time = "2026-07-04T16:25:38.891Z" }
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/e5/57/4e39f85347f77144d2ad12e87d5df8fb8f17023f9bd9e8c6e903a128382c/hydra_core-1.3.3-py3-none-any.whl", hash = "sha256:cf349fc393f486f250e5825592c3d0a50c0af3effd726cf8dd5b637a7cb464e3", size = 154706, upload-time = "2026-06-11T05:54:24.917Z" },
|
||||
{ url = "https://files.pythonhosted.org/packages/ed/cd/a568610bafe991fdd3f628fb606316b3b2be52ded019284e895d9beb3a1e/hydra_core-1.3.4-py3-none-any.whl", hash = "sha256:e58683692904a09f1fdfffa1a9b86bfd94e215b59f1ee17e7cd7d92738090d33", size = 155478, upload-time = "2026-07-04T16:25:37.291Z" },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
@@ -2823,7 +2823,7 @@ wheels = [
|
||||
|
||||
[[package]]
|
||||
name = "lerobot"
|
||||
version = "0.5.2"
|
||||
version = "0.6.0"
|
||||
source = { editable = "." }
|
||||
dependencies = [
|
||||
{ name = "cmake" },
|
||||
@@ -6934,15 +6934,15 @@ wheels = [
|
||||
|
||||
[[package]]
|
||||
name = "uvicorn"
|
||||
version = "0.49.0"
|
||||
version = "0.50.0"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
dependencies = [
|
||||
{ name = "click" },
|
||||
{ name = "h11" },
|
||||
]
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/c4/1f/fa18009dea8469069cca78a4e877a008ab78f08b064bfc9ab891579077ff/uvicorn-0.49.0.tar.gz", hash = "sha256:ebf4271aa580d9de97f93192d4595176df6e91f9aae919ca73e4fc07df1e66a3", size = 91284, upload-time = "2026-06-03T22:01:30.448Z" }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/2e/41/06cce5dbb9f77591512957710ac709e60b12e6216a2f2d0d607fd49706e8/uvicorn-0.50.0.tar.gz", hash = "sha256:0c92e1bc2259cb7faa4fcef774a5966588f2e88542744550b66799fba10b76f1", size = 93257, upload-time = "2026-07-04T05:03:26.33Z" }
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/88/fa/e1388bbcf24ef3274f45c0c1c7b501fd14971037c1b6ee23610553307497/uvicorn-0.49.0-py3-none-any.whl", hash = "sha256:ba3d14c3ee7e41c6c654c46c9eb489d33213cdd30aa1696eab1374337c13f68f", size = 71376, upload-time = "2026-06-03T22:01:29.037Z" },
|
||||
{ url = "https://files.pythonhosted.org/packages/a0/3a/eb70620ca2bf8213603d5c731460687c49fee38b0072f0b4a637781f0a53/uvicorn-0.50.0-py3-none-any.whl", hash = "sha256:05f0eb19edf38208f79f43df8a63081b48df31b0cd1e5997be957a4dc97d1b19", size = 72716, upload-time = "2026-07-04T05:03:24.848Z" },
|
||||
]
|
||||
|
||||
[package.optional-dependencies]
|
||||
|
||||
@@ -1,182 +0,0 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||
<title>3D Hand Joint Visualizer</title>
|
||||
<style>
|
||||
body {
|
||||
font-family: Arial, sans-serif;
|
||||
margin: 0;
|
||||
padding: 0;
|
||||
overflow: hidden;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
height: 100vh;
|
||||
}
|
||||
|
||||
.controls {
|
||||
padding: 15px;
|
||||
background-color: #f5f5f5;
|
||||
z-index: 100;
|
||||
}
|
||||
|
||||
.status {
|
||||
padding: 10px;
|
||||
border-radius: 5px;
|
||||
margin: 10px 0;
|
||||
}
|
||||
|
||||
.connected {
|
||||
background-color: #d4edda;
|
||||
color: #155724;
|
||||
}
|
||||
|
||||
.disconnected {
|
||||
background-color: #f8d7da;
|
||||
color: #721c24;
|
||||
}
|
||||
|
||||
button {
|
||||
padding: 8px 16px;
|
||||
background-color: #4CAF50;
|
||||
color: white;
|
||||
border: none;
|
||||
border-radius: 4px;
|
||||
cursor: pointer;
|
||||
margin-right: 10px;
|
||||
}
|
||||
|
||||
button:hover {
|
||||
background-color: #45a049;
|
||||
}
|
||||
|
||||
button:disabled {
|
||||
background-color: #cccccc;
|
||||
cursor: not-allowed;
|
||||
}
|
||||
|
||||
.container {
|
||||
display: flex;
|
||||
flex: 1;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
#canvas-container {
|
||||
flex: 3;
|
||||
position: relative;
|
||||
}
|
||||
|
||||
#sidebar {
|
||||
flex: 1;
|
||||
padding: 15px;
|
||||
background-color: #f8f9fa;
|
||||
overflow-y: auto;
|
||||
max-width: 300px;
|
||||
border-left: 1px solid #ddd;
|
||||
}
|
||||
|
||||
.joint-info {
|
||||
margin-bottom: 10px;
|
||||
padding: 8px;
|
||||
border: 1px solid #ddd;
|
||||
border-radius: 4px;
|
||||
}
|
||||
|
||||
.joint-name {
|
||||
font-weight: bold;
|
||||
}
|
||||
|
||||
.joint-value {
|
||||
font-family: monospace;
|
||||
}
|
||||
|
||||
.bar-container {
|
||||
width: 100%;
|
||||
background-color: #e0e0e0;
|
||||
height: 10px;
|
||||
border-radius: 5px;
|
||||
overflow: hidden;
|
||||
margin-top: 5px;
|
||||
}
|
||||
|
||||
.bar {
|
||||
height: 100%;
|
||||
background-color: #4CAF50;
|
||||
width: 0%;
|
||||
transition: width 0.2s ease-in-out;
|
||||
}
|
||||
|
||||
.log-container {
|
||||
margin-top: 20px;
|
||||
border: 1px solid #ddd;
|
||||
border-radius: 5px;
|
||||
padding: 10px;
|
||||
height: 150px;
|
||||
overflow-y: auto;
|
||||
font-family: monospace;
|
||||
background-color: #f8f9fa;
|
||||
}
|
||||
|
||||
.view-controls {
|
||||
position: absolute;
|
||||
bottom: 10px;
|
||||
left: 10px;
|
||||
z-index: 10;
|
||||
}
|
||||
|
||||
.view-button {
|
||||
background-color: rgba(0, 0, 0, 0.5);
|
||||
color: white;
|
||||
border: none;
|
||||
padding: 5px 10px;
|
||||
margin-right: 5px;
|
||||
border-radius: 3px;
|
||||
cursor: pointer;
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<div class="controls">
|
||||
<button id="connectButton">Connect to Device</button>
|
||||
<button id="disconnectButton" disabled>Disconnect</button>
|
||||
<select id="baudRate">
|
||||
<option value="9600">9600</option>
|
||||
<option value="19200">19200</option>
|
||||
<option value="38400">38400</option>
|
||||
<option value="57600">57600</option>
|
||||
<option value="115200" selected>115200</option>
|
||||
</select>
|
||||
<span id="statusIndicator" class="status disconnected">Status: Disconnected</span>
|
||||
</div>
|
||||
|
||||
<div class="container">
|
||||
<div id="canvas-container">
|
||||
<!-- 3D canvas will be inserted here -->
|
||||
<div class="view-controls">
|
||||
<button class="view-button" id="frontView">Front</button>
|
||||
<button class="view-button" id="sideView">Side</button>
|
||||
<button class="view-button" id="topView">Top</button>
|
||||
<button class="view-button" id="resetView">Reset</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="sidebar">
|
||||
<h3>Joint Values</h3>
|
||||
<div id="jointsContainer">
|
||||
<!-- Joint info will be added here -->
|
||||
</div>
|
||||
|
||||
<div class="log-container" id="logContainer">
|
||||
<!-- Log messages will be added here -->
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Import Three.js -->
|
||||
<script src="https://cdn.jsdelivr.net/npm/three@0.132.2/build/three.min.js"></script>
|
||||
<script src="https://cdn.jsdelivr.net/npm/three@0.132.2/examples/js/controls/OrbitControls.js"></script>
|
||||
|
||||
<script src="script.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -1,669 +0,0 @@
|
||||
// === Hand Visualizer with Pre-Connect Sliders + Per-Joint Angle Limits ===
|
||||
// Assumes your HTML already has elements with the following IDs:
|
||||
// connectButton, disconnectButton, baudRate, statusIndicator, jointsContainer, logContainer,
|
||||
// canvas-container, frontView, sideView, topView, resetView
|
||||
// Requires Three.js + OrbitControls loaded on the page.
|
||||
|
||||
// -------------------- Config --------------------
|
||||
const MAX_JOINTS = 16;
|
||||
const RAW_MIN = 0, RAW_MAX = 4096;
|
||||
const RAW_CENTER = (RAW_MIN + RAW_MAX) / 2;
|
||||
const DEG = Math.PI / 180;
|
||||
const UI_DEG_MIN = -90, UI_DEG_MAX = 90; // UI sliders for angle limits
|
||||
|
||||
// -------------------- State --------------------
|
||||
let port;
|
||||
let reader;
|
||||
let keepReading = false;
|
||||
let isConnected = false;
|
||||
const decoder = new TextDecoder();
|
||||
let inputBuffer = '';
|
||||
|
||||
let jointValues = new Array(MAX_JOINTS).fill(RAW_CENTER);
|
||||
|
||||
// Auto-calibration: track observed min/max per joint
|
||||
let observedMin = new Array(MAX_JOINTS).fill(Infinity);
|
||||
let observedMax = new Array(MAX_JOINTS).fill(-Infinity);
|
||||
let calibrationEnabled = true;
|
||||
|
||||
// Three.js
|
||||
let scene, camera, renderer, controls;
|
||||
let hand = { palm: null, fingers: [] };
|
||||
|
||||
// DOM
|
||||
const connectButton = document.getElementById('connectButton');
|
||||
const disconnectButton = document.getElementById('disconnectButton');
|
||||
const baudRateSelect = document.getElementById('baudRate');
|
||||
const statusIndicator = document.getElementById('statusIndicator');
|
||||
const jointsContainer = document.getElementById('jointsContainer');
|
||||
const logContainer = document.getElementById('logContainer');
|
||||
const canvasContainer = document.getElementById('canvas-container');
|
||||
const frontViewBtn = document.getElementById('frontView');
|
||||
const sideViewBtn = document.getElementById('sideView');
|
||||
const topViewBtn = document.getElementById('topView');
|
||||
const resetViewBtn = document.getElementById('resetView');
|
||||
|
||||
// Helpers
|
||||
const clamp = (x, a, b) => Math.max(a, Math.min(b, x));
|
||||
const invLerp = (a, b, x) => clamp((x - a) / (b - a), 0, 1);
|
||||
|
||||
// -------------------- Joint Map with per-joint angle limits --------------------
|
||||
const fingerJointMap = [
|
||||
// Thumb (4)
|
||||
{ finger:0, joint:0, type:'CMC_ABDUCTION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:0, joint:1, type:'CMC_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:0, joint:2, type:'MCP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true }, // +45° only
|
||||
{ finger:0, joint:3, type:'IP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true }, // +45° only
|
||||
|
||||
// Index (3)
|
||||
{ finger:1, joint:0, type:'MCP_ABDUCTION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:1, joint:1, type:'MCP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:false },
|
||||
{ finger:1, joint:2, type:'PIP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true }, // +45° only
|
||||
|
||||
// Middle (3)
|
||||
{ finger:2, joint:0, type:'MCP_ABDUCTION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:2, joint:1, type:'MCP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:2, joint:2, type:'PIP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:true }, // +45° only
|
||||
|
||||
// Ring (3)
|
||||
{ finger:3, joint:0, type:'MCP_ABDUCTION', min:RAW_MIN, max:RAW_MAX, inverted:true },
|
||||
{ finger:3, joint:1, type:'MCP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:false },
|
||||
{ finger:3, joint:2, type:'PIP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:false }, // +45° only
|
||||
|
||||
// Pinky (3)
|
||||
{ finger:4, joint:0, type:'MCP_ABDUCTION', min:RAW_MIN, max:RAW_MAX, inverted:false },
|
||||
{ finger:4, joint:1, type:'MCP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:false },
|
||||
{ finger:4, joint:2, type:'PIP_FLEXION', min:RAW_MIN, max:RAW_MAX, inverted:false } // +45° only
|
||||
];
|
||||
|
||||
// Assign angle limits (radians) per joint (default ±45°, exceptions: +45° only)
|
||||
for (const j of fingerJointMap) {
|
||||
const isThumb = j.finger === 0;
|
||||
const isPIP = j.type === 'PIP_FLEXION';
|
||||
let minA = -45 * DEG, maxA = +45 * DEG;
|
||||
if ((isThumb && (j.type === 'MCP_FLEXION' || j.type === 'IP_FLEXION')) || (!isThumb && isPIP)) {
|
||||
minA = 0;
|
||||
maxA = +45 * DEG;
|
||||
}
|
||||
j.angleMin = minA;
|
||||
j.angleMax = maxA;
|
||||
}
|
||||
|
||||
// -------------------- UI: Joint Panel --------------------
|
||||
const uiRefs = []; // per joint: { valueLabel, bar, barWrap, slider, invertChk, minDeg, maxDeg }
|
||||
|
||||
function initializeJointElements() {
|
||||
jointsContainer.innerHTML = '';
|
||||
uiRefs.length = 0;
|
||||
|
||||
for (let i = 0; i < MAX_JOINTS; i++) {
|
||||
const wrap = document.createElement('div');
|
||||
wrap.className = 'joint-info';
|
||||
|
||||
const fingerIndex = i < 4 ? 0 : Math.floor((i - 4) / 3) + 1;
|
||||
const jointInfo = fingerJointMap[i];
|
||||
const jointType = jointInfo?.type || 'Unknown';
|
||||
const fingerName = ['Thumb', 'Index', 'Middle', 'Ring', 'Pinky'][fingerIndex];
|
||||
|
||||
// Header
|
||||
const nameEl = document.createElement('div');
|
||||
nameEl.className = 'joint-name';
|
||||
nameEl.textContent = `${fingerName} – ${jointType}`;
|
||||
|
||||
// Value + bar
|
||||
const valueEl = document.createElement('div');
|
||||
valueEl.className = 'joint-value';
|
||||
valueEl.textContent = `Value: ${jointValues[i]}`;
|
||||
|
||||
const barWrap = document.createElement('div');
|
||||
barWrap.className = 'bar-container';
|
||||
const barEl = document.createElement('div');
|
||||
barEl.className = 'bar';
|
||||
barWrap.appendChild(barEl);
|
||||
|
||||
// Slider for pre-connect manual control
|
||||
const slider = document.createElement('input');
|
||||
slider.type = 'range';
|
||||
slider.min = String(RAW_MIN);
|
||||
slider.max = String(RAW_MAX);
|
||||
slider.value = String(jointValues[i]);
|
||||
slider.step = '1';
|
||||
slider.className = 'joint-slider';
|
||||
|
||||
slider.addEventListener('input', () => {
|
||||
if (isConnected) return; // ignore while connected
|
||||
let v = parseInt(slider.value, 10);
|
||||
if (jointInfo?.inverted) v = (jointInfo.min + jointInfo.max) - v;
|
||||
jointValues[i] = clamp(jointInfo ? v : 0, RAW_MIN, RAW_MAX);
|
||||
updateJointDisplay(i, jointValues[i]);
|
||||
updateHandModel();
|
||||
});
|
||||
|
||||
// Invert checkbox
|
||||
const invertLbl = document.createElement('label');
|
||||
invertLbl.className = 'invert-toggle';
|
||||
const invertChk = document.createElement('input');
|
||||
invertChk.type = 'checkbox';
|
||||
invertChk.checked = !!jointInfo?.inverted;
|
||||
invertChk.addEventListener('change', () => {
|
||||
if (jointInfo) jointInfo.inverted = invertChk.checked;
|
||||
addLogMessage(`${fingerName} ${jointType} inversion ${invertChk.checked ? 'enabled' : 'disabled'}`);
|
||||
});
|
||||
invertLbl.appendChild(invertChk);
|
||||
invertLbl.appendChild(document.createTextNode('Invert Values'));
|
||||
|
||||
// Angle limits (deg) controls
|
||||
const limitsRow = document.createElement('div');
|
||||
limitsRow.className = 'limits-row';
|
||||
|
||||
const minDeg = document.createElement('input');
|
||||
minDeg.type = 'number';
|
||||
minDeg.min = String(UI_DEG_MIN);
|
||||
minDeg.max = String(UI_DEG_MAX);
|
||||
minDeg.step = '1';
|
||||
minDeg.value = String(Math.round((jointInfo.angleMin || 0) / DEG));
|
||||
minDeg.className = 'limit-num';
|
||||
|
||||
const maxDeg = document.createElement('input');
|
||||
maxDeg.type = 'number';
|
||||
maxDeg.min = String(UI_DEG_MIN);
|
||||
maxDeg.max = String(UI_DEG_MAX);
|
||||
maxDeg.step = '1';
|
||||
maxDeg.value = String(Math.round((jointInfo.angleMax || 0) / DEG));
|
||||
maxDeg.className = 'limit-num';
|
||||
|
||||
const minLbl = document.createElement('span'); minLbl.textContent = 'min°';
|
||||
const maxLbl = document.createElement('span'); maxLbl.textContent = 'max°';
|
||||
minLbl.className = 'limit-label'; maxLbl.className = 'limit-label';
|
||||
|
||||
function syncLimits() {
|
||||
let mn = parseFloat(minDeg.value);
|
||||
let mx = parseFloat(maxDeg.value);
|
||||
if (isNaN(mn)) mn = -45;
|
||||
if (isNaN(mx)) mx = +45;
|
||||
if (mn > mx) [mn, mx] = [mx, mn];
|
||||
jointInfo.angleMin = clamp(mn, UI_DEG_MIN, UI_DEG_MAX) * DEG;
|
||||
jointInfo.angleMax = clamp(mx, UI_DEG_MIN, UI_DEG_MAX) * DEG;
|
||||
minDeg.value = String(Math.round(jointInfo.angleMin / DEG));
|
||||
maxDeg.value = String(Math.round(jointInfo.angleMax / DEG));
|
||||
updateHandModel();
|
||||
}
|
||||
minDeg.addEventListener('change', syncLimits);
|
||||
maxDeg.addEventListener('change', syncLimits);
|
||||
|
||||
limitsRow.appendChild(minLbl);
|
||||
limitsRow.appendChild(minDeg);
|
||||
limitsRow.appendChild(maxLbl);
|
||||
limitsRow.appendChild(maxDeg);
|
||||
|
||||
// Calibration controls
|
||||
const calibRow = document.createElement('div');
|
||||
calibRow.className = 'calib-row';
|
||||
|
||||
const resetCalibBtn = document.createElement('button');
|
||||
resetCalibBtn.textContent = 'Reset Calib';
|
||||
resetCalibBtn.className = 'calib-btn';
|
||||
resetCalibBtn.addEventListener('click', () => {
|
||||
observedMin[i] = Infinity;
|
||||
observedMax[i] = -Infinity;
|
||||
addLogMessage(`Reset calibration for ${fingerName} ${jointType}`);
|
||||
});
|
||||
|
||||
const calibStatus = document.createElement('span');
|
||||
calibStatus.className = 'calib-status';
|
||||
calibStatus.textContent = `Range: --`;
|
||||
|
||||
calibRow.appendChild(resetCalibBtn);
|
||||
calibRow.appendChild(calibStatus);
|
||||
|
||||
// Compose
|
||||
wrap.appendChild(nameEl);
|
||||
wrap.appendChild(valueEl);
|
||||
wrap.appendChild(barWrap);
|
||||
wrap.appendChild(slider);
|
||||
wrap.appendChild(invertLbl);
|
||||
wrap.appendChild(limitsRow);
|
||||
wrap.appendChild(calibRow);
|
||||
|
||||
jointsContainer.appendChild(wrap);
|
||||
|
||||
uiRefs[i] = { valueLabel: valueEl, bar: barEl, barWrap, slider, invertChk, minDeg, maxDeg, nameEl, calibStatus };
|
||||
}
|
||||
|
||||
setConnectedUI(false); // initial state: sliders active
|
||||
}
|
||||
|
||||
// Toggle UI between pre-connect SLIDERS vs post-connect BARS
|
||||
function setConnectedUI(connected) {
|
||||
isConnected = connected;
|
||||
for (let i = 0; i < uiRefs.length; i++) {
|
||||
const ui = uiRefs[i];
|
||||
if (!ui) continue;
|
||||
// Show bars when connected; sliders disabled/hidden
|
||||
ui.barWrap.style.display = connected ? '' : 'none';
|
||||
ui.slider.disabled = connected;
|
||||
ui.slider.style.display = connected ? 'none' : '';
|
||||
}
|
||||
|
||||
// Reset calibration when connecting
|
||||
if (connected) {
|
||||
observedMin.fill(Infinity);
|
||||
observedMax.fill(-Infinity);
|
||||
addLogMessage('Calibration reset - move joints through full range for best results');
|
||||
}
|
||||
}
|
||||
|
||||
// Update joint display (value text + bar color/width + slider position if needed)
|
||||
function updateJointDisplay(jointIndex, value) {
|
||||
const ui = uiRefs[jointIndex];
|
||||
const info = fingerJointMap[jointIndex];
|
||||
if (!ui || !info) return;
|
||||
|
||||
ui.valueLabel.textContent = `Value: ${value}`;
|
||||
|
||||
// bar
|
||||
const min = info.min, max = info.max;
|
||||
const pct = clamp((value - min) / (max - min), 0, 1) * 100;
|
||||
ui.bar.style.width = `${pct}%`;
|
||||
const hue = Math.floor(pct * 1.2); // 0..120
|
||||
ui.bar.style.backgroundColor = `hsl(${hue}, 80%, 50%)`;
|
||||
|
||||
// slider (only meaningful when not connected; keep in sync anyway)
|
||||
const rawForSlider = info.inverted ? (info.min + info.max) - value : value;
|
||||
if (!isConnected) ui.slider.value = String(clamp(Math.round(rawForSlider), RAW_MIN, RAW_MAX));
|
||||
}
|
||||
|
||||
// -------------------- Serial I/O --------------------
|
||||
async function readSerialData() {
|
||||
while (port?.readable && keepReading) {
|
||||
reader = port.readable.getReader();
|
||||
try {
|
||||
while (true) {
|
||||
const { value, done } = await reader.read();
|
||||
if (done) break;
|
||||
if (value) processData(decoder.decode(value));
|
||||
}
|
||||
} catch (err) {
|
||||
console.error('Error reading:', err);
|
||||
addLogMessage(`Error: ${err.message}`);
|
||||
break;
|
||||
} finally {
|
||||
reader.releaseLock();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
function processData(chunk) {
|
||||
inputBuffer += chunk;
|
||||
let idx;
|
||||
while ((idx = inputBuffer.indexOf('\n')) !== -1) {
|
||||
const line = inputBuffer.slice(0, idx).trim();
|
||||
inputBuffer = inputBuffer.slice(idx + 1);
|
||||
|
||||
const vals = line.split(/\s+/).map(v => parseInt(v, 10));
|
||||
if (vals.length === MAX_JOINTS && vals.every(v => Number.isFinite(v))) {
|
||||
for (let i = 0; i < MAX_JOINTS; i++) {
|
||||
const info = fingerJointMap[i];
|
||||
if (!info) continue;
|
||||
|
||||
let rawValue = vals[i];
|
||||
|
||||
// Update calibration tracking
|
||||
if (calibrationEnabled) {
|
||||
observedMin[i] = Math.min(observedMin[i], rawValue);
|
||||
observedMax[i] = Math.max(observedMax[i], rawValue);
|
||||
|
||||
// Update calibration display
|
||||
const ui = uiRefs[i];
|
||||
if (ui && ui.calibStatus) {
|
||||
if (observedMin[i] !== Infinity && observedMax[i] !== -Infinity) {
|
||||
ui.calibStatus.textContent = `Range: ${observedMin[i]}-${observedMax[i]}`;
|
||||
}
|
||||
}
|
||||
|
||||
// Remap observed range to target range
|
||||
if (observedMin[i] !== Infinity && observedMax[i] !== -Infinity && observedMax[i] > observedMin[i]) {
|
||||
const observedRange = observedMax[i] - observedMin[i];
|
||||
const targetRange = info.max - info.min;
|
||||
const normalizedValue = (rawValue - observedMin[i]) / observedRange;
|
||||
rawValue = info.min + (normalizedValue * targetRange);
|
||||
}
|
||||
}
|
||||
|
||||
let v = clamp(rawValue, info.min, info.max);
|
||||
if (info.inverted) v = (info.min + info.max) - v;
|
||||
jointValues[i] = v;
|
||||
updateJointDisplay(i, v);
|
||||
}
|
||||
updateHandModel();
|
||||
} else {
|
||||
addLogMessage(`Received: ${line}`);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
async function connectToDevice() {
|
||||
try {
|
||||
port = await navigator.serial.requestPort();
|
||||
const baudRate = parseInt(baudRateSelect.value, 10) || 115200;
|
||||
await port.open({ baudRate });
|
||||
|
||||
keepReading = true;
|
||||
setConnectedUI(true);
|
||||
|
||||
statusIndicator.textContent = 'Status: Connected';
|
||||
statusIndicator.className = 'status connected';
|
||||
connectButton.disabled = true;
|
||||
disconnectButton.disabled = false;
|
||||
baudRateSelect.disabled = true;
|
||||
|
||||
addLogMessage(`Connected at ${baudRate} baud`);
|
||||
readSerialData();
|
||||
} catch (e) {
|
||||
console.error('Connect error:', e);
|
||||
addLogMessage(`Connection error: ${e.message}`);
|
||||
}
|
||||
}
|
||||
|
||||
async function disconnectFromDevice() {
|
||||
try {
|
||||
keepReading = false;
|
||||
if (reader) {
|
||||
try { reader.cancel(); } catch {}
|
||||
}
|
||||
if (port) {
|
||||
await port.close();
|
||||
port = null;
|
||||
}
|
||||
} catch (e) {
|
||||
console.error('Disconnect error:', e);
|
||||
addLogMessage(`Disconnection error: ${e.message}`);
|
||||
} finally {
|
||||
setConnectedUI(false);
|
||||
statusIndicator.textContent = 'Status: Disconnected';
|
||||
statusIndicator.className = 'status disconnected';
|
||||
connectButton.disabled = false;
|
||||
disconnectButton.disabled = true;
|
||||
baudRateSelect.disabled = false;
|
||||
addLogMessage('Disconnected');
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------- Three.js Scene --------------------
|
||||
function initThreeJS() {
|
||||
scene = new THREE.Scene();
|
||||
scene.background = new THREE.Color(0xf0f0f0);
|
||||
|
||||
camera = new THREE.PerspectiveCamera(
|
||||
75,
|
||||
canvasContainer.clientWidth / canvasContainer.clientHeight,
|
||||
0.1, 1000
|
||||
);
|
||||
camera.position.set(0, 15, 15);
|
||||
camera.lookAt(0, 0, 0);
|
||||
|
||||
renderer = new THREE.WebGLRenderer({ antialias: true });
|
||||
renderer.setSize(canvasContainer.clientWidth, canvasContainer.clientHeight);
|
||||
renderer.setPixelRatio(window.devicePixelRatio);
|
||||
canvasContainer.appendChild(renderer.domElement);
|
||||
|
||||
controls = new THREE.OrbitControls(camera, renderer.domElement);
|
||||
controls.enableDamping = true;
|
||||
controls.dampingFactor = 0.25;
|
||||
|
||||
const ambientLight = new THREE.AmbientLight(0x404040);
|
||||
scene.add(ambientLight);
|
||||
const dir1 = new THREE.DirectionalLight(0xffffff, 0.5);
|
||||
dir1.position.set(1, 1, 1);
|
||||
scene.add(dir1);
|
||||
const dir2 = new THREE.DirectionalLight(0xffffff, 0.3);
|
||||
dir2.position.set(-1, 1, -1);
|
||||
scene.add(dir2);
|
||||
|
||||
const gridHelper = new THREE.GridHelper(20, 20);
|
||||
scene.add(gridHelper);
|
||||
|
||||
createHandModel();
|
||||
window.addEventListener('resize', onWindowResize);
|
||||
animate();
|
||||
}
|
||||
|
||||
function createHandModel() {
|
||||
const palmMaterial = new THREE.MeshPhongMaterial({ color: 0xf5c396 });
|
||||
const fingerMaterial = new THREE.MeshPhongMaterial({ color: 0xf5c396 });
|
||||
const jointMaterial = new THREE.MeshPhongMaterial({ color: 0xe3a977 });
|
||||
|
||||
const palmGeometry = new THREE.BoxGeometry(7, 1, 8);
|
||||
hand.palm = new THREE.Mesh(palmGeometry, palmMaterial);
|
||||
hand.palm.position.set(0, 0, 0);
|
||||
hand.palm.rotation.x = Math.PI / 2; // hand vertical, palm facing forward
|
||||
scene.add(hand.palm);
|
||||
|
||||
const fingerWidth = 1, fingerHeight = 0.8;
|
||||
const fingerSegmentLengths = [3, 2, 1.5];
|
||||
const thumbSegmentLengths = [2, 2, 1.5];
|
||||
|
||||
const fingerBasePositions = [
|
||||
[ 3, 0, -2], // Thumb
|
||||
[ 1.5,-0.5,-4], // Index
|
||||
[ 0, -0.5,-4], // Middle
|
||||
[-1.5,-0.5,-4], // Ring
|
||||
[-3, -0.5,-4], // Pinky
|
||||
];
|
||||
const fingerBaseRot = [
|
||||
{ x:0, y:-Math.PI/3, z: Math.PI/3 }, // Thumb
|
||||
{ x:0, y:-Math.PI/48, z: 0 },
|
||||
{ x:0, y: Math.PI/48, z: 0 },
|
||||
{ x:0, y: Math.PI/32, z: 0 },
|
||||
{ x:0, y: Math.PI/24, z: 0 }
|
||||
];
|
||||
|
||||
for (let fIdx = 0; fIdx < 5; fIdx++) {
|
||||
const finger = { name:['Thumb','Index','Middle','Ring','Pinky'][fIdx], segments:[], joints:[] };
|
||||
const isThumb = fIdx === 0;
|
||||
const segLens = isThumb ? thumbSegmentLengths : fingerSegmentLengths;
|
||||
|
||||
finger.group = new THREE.Group();
|
||||
finger.group.position.set(...fingerBasePositions[fIdx]);
|
||||
finger.group.rotation.x = fingerBaseRot[fIdx].x;
|
||||
finger.group.rotation.y = fingerBaseRot[fIdx].y;
|
||||
finger.group.rotation.z = fingerBaseRot[fIdx].z;
|
||||
finger.group.userData.baseRot = {
|
||||
x:finger.group.rotation.x,
|
||||
y:finger.group.rotation.y,
|
||||
z:finger.group.rotation.z
|
||||
};
|
||||
hand.palm.add(finger.group);
|
||||
|
||||
let parent = finger.group;
|
||||
for (let s = 0; s < segLens.length; s++) {
|
||||
const segGroup = new THREE.Group();
|
||||
|
||||
const jGeom = new THREE.SphereGeometry(fingerWidth * 0.6, 8, 8);
|
||||
const joint = new THREE.Mesh(jGeom, jointMaterial);
|
||||
segGroup.add(joint);
|
||||
|
||||
const segGeom = new THREE.BoxGeometry(fingerWidth, fingerHeight, segLens[s]);
|
||||
const seg = new THREE.Mesh(segGeom, fingerMaterial);
|
||||
seg.position.z = -segLens[s] / 2;
|
||||
segGroup.add(seg);
|
||||
|
||||
parent.add(segGroup);
|
||||
|
||||
finger.segments.push(segGroup);
|
||||
finger.joints.push(joint);
|
||||
|
||||
if (s < segLens.length - 1) {
|
||||
const connector = new THREE.Group();
|
||||
connector.position.z = -segLens[s];
|
||||
segGroup.add(connector);
|
||||
parent = connector;
|
||||
}
|
||||
}
|
||||
|
||||
hand.fingers.push(finger);
|
||||
}
|
||||
|
||||
addFingerLabels();
|
||||
addHandLabel();
|
||||
}
|
||||
|
||||
function addFingerLabels() {
|
||||
const names = ['Thumb','Index','Middle','Ring','Pinky'];
|
||||
for (let i = 0; i < hand.fingers.length; i++) {
|
||||
const finger = hand.fingers[i];
|
||||
const canvas = document.createElement('canvas');
|
||||
const ctx = canvas.getContext('2d');
|
||||
canvas.width = 128; canvas.height = 32;
|
||||
ctx.fillStyle = '#ffffff'; ctx.fillRect(0,0,canvas.width,canvas.height);
|
||||
ctx.font = 'bold 16px Arial';
|
||||
ctx.fillStyle = '#000000';
|
||||
ctx.textAlign = 'center'; ctx.textBaseline = 'middle';
|
||||
ctx.fillText(names[i], canvas.width/2, canvas.height/2);
|
||||
|
||||
const texture = new THREE.CanvasTexture(canvas);
|
||||
const geom = new THREE.PlaneGeometry(2, 0.5);
|
||||
const mat = new THREE.MeshBasicMaterial({ map:texture, transparent:true, side:THREE.DoubleSide });
|
||||
const label = new THREE.Mesh(geom, mat);
|
||||
label.position.set(0, -1.5, -2);
|
||||
label.rotation.x = Math.PI / 2;
|
||||
finger.group.add(label);
|
||||
}
|
||||
}
|
||||
|
||||
function addHandLabel() {
|
||||
const canvas = document.createElement('canvas');
|
||||
const ctx = canvas.getContext('2d');
|
||||
canvas.width = 256; canvas.height = 64;
|
||||
ctx.fillStyle = '#ffffff'; ctx.fillRect(0,0,canvas.width,canvas.height);
|
||||
ctx.font = 'bold 24px Arial';
|
||||
ctx.fillStyle = '#000000';
|
||||
ctx.textAlign = 'center'; ctx.textBaseline = 'middle';
|
||||
ctx.fillText('RIGHT HAND (VERTICAL)', canvas.width/2, canvas.height/2);
|
||||
|
||||
const texture = new THREE.CanvasTexture(canvas);
|
||||
const geom = new THREE.PlaneGeometry(7, 1.75);
|
||||
const mat = new THREE.MeshBasicMaterial({ map:texture, transparent:true, side:THREE.DoubleSide });
|
||||
const label = new THREE.Mesh(geom, mat);
|
||||
label.position.set(0, -2, 0);
|
||||
label.rotation.x = Math.PI / 2;
|
||||
scene.add(label);
|
||||
}
|
||||
|
||||
function updateHandModel() {
|
||||
for (let i = 0; i < MAX_JOINTS; i++) {
|
||||
const info = fingerJointMap[i];
|
||||
if (!info) continue;
|
||||
const { finger, joint, type, min, max, angleMin, angleMax } = info;
|
||||
const raw = jointValues[i];
|
||||
const f = hand.fingers[finger];
|
||||
if (!f) continue;
|
||||
|
||||
const center = (min + max) / 2;
|
||||
let angle = 0;
|
||||
|
||||
if (type.includes('ABDUCTION')) {
|
||||
// symmetric around neutral
|
||||
const k = clamp((raw - center) / ((max - min) / 2), -1, 1);
|
||||
angle = angleMin + (k + 1) * 0.5 * (angleMax - angleMin);
|
||||
|
||||
const base = f.group.userData.baseRot || {x:0,y:0,z:0};
|
||||
if (finger === 0 && joint === 0) {
|
||||
// Thumb: abduction about Z (toward/away from palm)
|
||||
f.group.rotation.z = base.z + angle;
|
||||
} else {
|
||||
// Other fingers: side-to-side about Y
|
||||
f.group.rotation.y = base.y + angle;
|
||||
}
|
||||
} else if (type.includes('FLEXION')) {
|
||||
const isThumb = finger === 0;
|
||||
const isMCP = type === 'MCP_FLEXION';
|
||||
const isPIP = type === 'PIP_FLEXION';
|
||||
const positiveOnly = (isThumb && (type === 'MCP_FLEXION' || type === 'IP_FLEXION')) || (!isThumb && isPIP);
|
||||
|
||||
if (positiveOnly) {
|
||||
const t = raw <= center ? 0 : invLerp(center, max, raw); // 0..1
|
||||
angle = angleMin + t * (angleMax - angleMin); // 0..+limit
|
||||
} else {
|
||||
const k = clamp((raw - center) / ((max - min) / 2), -1, 1);
|
||||
angle = angleMin + (k + 1) * 0.5 * (angleMax - angleMin);
|
||||
}
|
||||
|
||||
if (isMCP) {
|
||||
// MCP flexion applies to the finger base group (same as abduction)
|
||||
const base = f.group.userData.baseRot || {x:0,y:0,z:0};
|
||||
f.group.rotation.x = base.x + angle;
|
||||
} else if (f.segments[joint]) {
|
||||
// PIP/DIP/IP flexion applies to individual segments
|
||||
f.segments[joint].rotation.x = angle;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------- Render Loop --------------------
|
||||
function onWindowResize() {
|
||||
camera.aspect = canvasContainer.clientWidth / canvasContainer.clientHeight;
|
||||
camera.updateProjectionMatrix();
|
||||
renderer.setSize(canvasContainer.clientWidth, canvasContainer.clientHeight);
|
||||
}
|
||||
|
||||
function animate() {
|
||||
requestAnimationFrame(animate);
|
||||
controls.update();
|
||||
renderer.render(scene, camera);
|
||||
}
|
||||
|
||||
// -------------------- Misc UI --------------------
|
||||
function addLogMessage(msg) {
|
||||
const el = document.createElement('div');
|
||||
el.textContent = msg;
|
||||
logContainer.appendChild(el);
|
||||
logContainer.scrollTop = logContainer.scrollHeight;
|
||||
while (logContainer.children.length > 100) {
|
||||
logContainer.removeChild(logContainer.firstChild);
|
||||
}
|
||||
}
|
||||
|
||||
// Camera view controls
|
||||
frontViewBtn?.addEventListener('click', () => { camera.position.set(0, 0, 20); camera.lookAt(0,0,0); controls.update(); });
|
||||
sideViewBtn?.addEventListener('click', () => { camera.position.set(20, 0, 0); camera.lookAt(0,0,0); controls.update(); });
|
||||
topViewBtn?.addEventListener('click', () => { camera.position.set(0, 20, 0); camera.lookAt(0,0,0); controls.update(); });
|
||||
resetViewBtn?.addEventListener('click', () => { camera.position.set(10,10,10); camera.lookAt(0,0,0); controls.update(); });
|
||||
|
||||
// Serial connect buttons
|
||||
connectButton?.addEventListener('click', connectToDevice);
|
||||
disconnectButton?.addEventListener('click', disconnectFromDevice);
|
||||
|
||||
// Web Serial support check
|
||||
if (!navigator.serial) {
|
||||
statusIndicator.textContent = 'Status: Web Serial API not supported in this browser';
|
||||
connectButton.disabled = true;
|
||||
addLogMessage('ERROR: Web Serial API is not supported in this browser. Try Chrome or Edge.');
|
||||
}
|
||||
|
||||
// -------------------- Boot --------------------
|
||||
initThreeJS();
|
||||
initializeJointElements();
|
||||
|
||||
// -------------------- Styles (inline) --------------------
|
||||
const styleElement = document.createElement('style');
|
||||
styleElement.textContent = `
|
||||
.joint-info { border-bottom: 1px solid #eee; padding: 8px 0; }
|
||||
.joint-name { font-weight: 600; margin-bottom: 4px; }
|
||||
.joint-value { font-size: 12px; color: #333; margin-bottom: 4px; }
|
||||
.bar-container { width: 100%; height: 8px; background: #ddd; border-radius: 4px; overflow: hidden; }
|
||||
.bar { height: 100%; width: 0%; background: #4caf50; }
|
||||
.joint-slider { width: 100%; margin: 6px 0; }
|
||||
.invert-toggle { display: inline-flex; align-items: center; gap: 6px; margin-top: 4px; font-size: 12px; color: #555; }
|
||||
.limits-row { display: flex; align-items: center; gap: 6px; margin-top: 6px; flex-wrap: wrap; }
|
||||
.limit-label { font-size: 11px; color: #666; }
|
||||
.limit-num { width: 60px; }
|
||||
.calib-row { display: flex; align-items: center; gap: 8px; margin-top: 4px; }
|
||||
.calib-btn { padding: 2px 6px; font-size: 11px; background: #f44336; color: white; border: none; border-radius: 3px; cursor: pointer; }
|
||||
.calib-btn:hover { background: #d32f2f; }
|
||||
.calib-status { font-size: 11px; color: #666; }
|
||||
.status.connected { color: #0a0; }
|
||||
.status.disconnected { color: #a00; }
|
||||
`;
|
||||
document.head.appendChild(styleElement);
|
||||
Reference in New Issue
Block a user