Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@ source/leisaac/leisaac/devices/lerobot/.cache/so101_leader.json
source/leisaac/leisaac/devices/lerobot/.cache/left_so101_leader.json
source/leisaac/leisaac/devices/lerobot/.cache/right_so101_leader.json
source/leisaac/leisaac/devices/lerobot/.cache/lekiwi_leader.json
scripts/environments/teleoperation/.cache/

# scene assets
assets/scenes/*
Expand Down
95 changes: 95 additions & 0 deletions docs/docs/docs/getting_started/teleoperation.md
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@ python scripts/environments/teleoperation/teleop_se3_agent.py \

- `--port`: Specify the port of teleoperation device, e.g., `/dev/ttyACM0`. Only used when teleop_device is `so101leader` and `lekiwi-leader`.

- `--remote_endpoint`: ZMQ endpoint for remote so101leader (e.g., `tcp://192.168.1.10:5556`). When set, connects to a `so101_joint_state_server.py` running on the machine with the leader arm. See [Remote Teleoperation](#remote-teleoperation) below.

- `--left_arm_port`: Specify the port of left arm, e.g., `/dev/ttyACM0`. Only used when teleop_device is `bi-so101leader`.

- `--right_arm_port`: Specify the port of right arm, e.g., `/dev/ttyACM1`. Only used when teleop_device is `bi-so101leader`.
Expand Down Expand Up @@ -59,6 +61,99 @@ python scripts/environments/teleoperation/teleop_se3_agent.py \
We support multiple devices for teleoperation. See [here](/resources/available_devices) for more devices and usage instructions.
::::

## Remote Teleoperation

When the leader arm is connected to a different machine than the one running Isaac Sim
(e.g., Isaac Sim on a cloud GPU instance, leader arm on your laptop), you can use
**remote teleoperation** via ZMQ.

### How it works

The leader arm machine runs a lightweight publisher that reads motor positions and
streams them over the network. The Isaac Sim machine subscribes and uses the joint
states for teleoperation — no USB forwarding needed.

```
Laptop (leader arm) Cloud GPU (Isaac Sim)
┌──────────────────────────┐ ZMQ PUB/SUB ┌──────────────────────┐
│ so101_joint_state_server │──────────────►│ SO101LeaderRemote │
│ reads motors │ tcp:5556 │ teleop_se3_agent.py │
│ at 50 Hz │ │ --remote_endpoint │
└──────────────────────────┘ └──────────────────────┘
```

### Prerequisites

- Network connectivity between the two machines (direct or via SSH tunnel)
- `pyzmq` installed on the Isaac Sim machine: `pip install "source/leisaac[remote]"` or `pip install pyzmq`

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

  1. I think it would be better to reuse the existing calibration logic in leisaac, so that there is no need to introduce an additional lerobot environment on the local machine.
  2. Perhaps we can avoid introducing uv for now and keep it consistent with the project’s current environment management approach.
  3. It would be better to add a detailed local environment installation guide here; I think you may also need to run pip install -e source/leisaac[remote] on the local machine?

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These suggestions and questions are related. Let me explain why I used uv in the first place first. I used uv because it supports PEP-508 which let me embed dependencies of the script. And you could run it with uv run xxx.py and all dependencies will be installed in uv's venv. I do this because leisaac has a heavy set of dependencies. And since the local machine does not need any other features of leisaac except forwarding the motor states to the remote. It would be convenient for user to run the script with uv run without pulling a lot of dependencies.

So back to your comments:

  1. The dependency of leisaac is avoided on purpose. But I could change it to depend on leisaac if you thought it would better fit the project.

  2. uv is only introduced for users to run the script easily. You could use any other tool that supports PEP-508. Or if you already have all dependencies installed, you could just run it directly without uv. So uv is not required, it's just my suggested way to run the script.

  3. No, you don't need to do any pip install in the local machine. uv run takes care of all dependencies embedded in the script. But I could improve the doc to make it more clear.

Let me know what you think for 1., I will take your decision.

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I agree that installing a clean environment on the local machine (without Isaac Sim + Lab + torch) is better for joint_state_server.

The pyproject.toml in leisaac does not enforce the installation of Isaac Lab/Sim/torch. I think we can depend on leisaac (to reuse the calibration logic), and at the same time use try(ImportError)-except when importing related packages to skip them and print corresponding warning messages, or perform the imports within the functions where these dependencies are actually used.

Ideally, users should only need to run pip install leisaac[remote] without installing other complex dependencies to run so101_joint_state_server.py.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure, I will try this path. I was assuming Isaac Sim/torch is a dependency of leisaac without confirmation. Thanks for the correction.

### Local Machine Setup

On the machine where the leader arm is connected, install leisaac with remote support:

```bash
pip install "source/leisaac[remote]"
```

::::info
On the remote machine (the Isaac Sim machine), you need to install the full simulation stack, including PyTorch, Isaac Sim, and IsaacLab. On your local machine, you can skip these heavyweight dependencies—just run the command above; local installation of PyTorch/Isaac Sim/IsaacLab is not required.
::::

### Usage

**Terminal 1 — Local machine (leader arm):**

```bash
python scripts/environments/teleoperation/so101_joint_state_server.py \
--port /dev/ttyACM0 --id leader_arm --rate 50
```

If no calibration file exists, the script will run an interactive calibration process automatically. To force recalibration, add `--recalibrate`.

**Terminal 2 — Remote machine (Isaac Sim):**

```bash
python scripts/environments/teleoperation/teleop_se3_agent.py \
--task=LeIsaac-SO101-PickOrange-v0 \
--teleop_device=so101leader \
--remote_endpoint=tcp://<local-machine-ip>:5556 \
--num_envs=1 --device=cuda --enable_cameras
```

### SSH Reverse Port Forwarding

If the cloud instance cannot reach your laptop directly (e.g., behind NAT or firewall),
use SSH reverse port forwarding to expose the publisher's port on the remote machine:

```bash
# On your laptop — forward local port 5556 to the remote machine's localhost:5556
ssh -R 5556:localhost:5556 ubuntu@<cloud-instance-ip>
```

Then on the remote machine, connect to `localhost` instead of your laptop's IP:

```bash
python scripts/environments/teleoperation/teleop_se3_agent.py \
--task=LeIsaac-SO101-PickOrange-v0 \
--teleop_device=so101leader \
--remote_endpoint=tcp://localhost:5556 \
--num_envs=1 --device=cuda --enable_cameras
```

### Parameters

- `--remote_endpoint`: ZMQ endpoint to subscribe to (e.g., `tcp://192.168.1.10:5556`
or `tcp://localhost:5556` with SSH tunnel). When set, uses `SO101LeaderRemote` instead
of the local `SO101Leader`.

- `--id` (publisher): Calibration ID (default: `leader_arm`). Calibration is stored in
`scripts/environments/teleoperation/.cache/{id}.json`.

- `--rate` (publisher): Motor read rate in Hz (default: 50). 30–50 Hz is sufficient
for LeIsaac teleoperation.

- `--recalibrate` (publisher): Force recalibration even if a calibration file exists.

## Operating Instructions

If the calibration file does not exist at the specified cache path, or if you launch with `--recalibrate`, you will be prompted to calibrate the SO101Leader. Please refer to the [documentation](https://huggingface.co/docs/lerobot/so101#calibration-video) for calibration steps.
Expand Down
169 changes: 169 additions & 0 deletions scripts/environments/teleoperation/so101_joint_state_server.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,169 @@
# /// script

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can this script include automatic detection and calibration logic, similar to the teleoperation in leisaac?

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, this might be a better UX instead of relying on another lerobot package.

# requires-python = ">=3.10"
# dependencies = ["leisaac[remote]"]
# ///
"""Publish normalized SO101 leader arm joint states over ZMQ PUB.

Run this on the machine where the leader arm is physically connected.
A remote LeIsaac instance subscribes to receive joint states for teleoperation.

Usage:
python so101_joint_state_server.py --port /dev/ttyACM0 --id leader_arm --rate 50
python so101_joint_state_server.py --port /dev/ttyACM0 --id leader_arm --recalibrate
"""

import argparse
import json
import os
import struct
import time

import zmq

try:
from leisaac.devices.lerobot.common.motors import (
FeetechMotorsBus,
Motor,
MotorCalibration,
MotorNormMode,
)
except ImportError as e:
raise ImportError("leisaac is required. Install with: pip install leisaac[remote]") from e

MOTORS = {
"shoulder_pan": Motor(1, "sts3215", MotorNormMode.RANGE_M100_100),
"shoulder_lift": Motor(2, "sts3215", MotorNormMode.RANGE_M100_100),
"elbow_flex": Motor(3, "sts3215", MotorNormMode.RANGE_M100_100),
"wrist_flex": Motor(4, "sts3215", MotorNormMode.RANGE_M100_100),
"wrist_roll": Motor(5, "sts3215", MotorNormMode.RANGE_M100_100),
"gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100),
}
MOTOR_NAMES = list(MOTORS.keys())
CACHE_DIR = os.path.join(os.path.dirname(__file__), ".cache")


def get_calibration_path(arm_id: str) -> str:
return os.path.join(CACHE_DIR, f"{arm_id}.json")


def load_calibration(path: str) -> dict[str, MotorCalibration]:
with open(path) as f:
data = json.load(f)
return {
name: MotorCalibration(
id=int(d["id"]),
drive_mode=int(d["drive_mode"]),
homing_offset=int(d["homing_offset"]),
range_min=int(d["range_min"]),
range_max=int(d["range_max"]),
)
for name, d in data.items()
}


def save_calibration(path: str, calibration: dict[str, MotorCalibration]):
os.makedirs(os.path.dirname(path), exist_ok=True)
data = {
k: {
"id": v.id,
"drive_mode": v.drive_mode,
"homing_offset": v.homing_offset,
"range_min": v.range_min,
"range_max": v.range_max,
}
for k, v in calibration.items()
}
with open(path, "w") as f:
json.dump(data, f, indent=4)


def calibrate(port: str, arm_id: str) -> dict[str, MotorCalibration]:
"""Run interactive calibration using leisaac's calibration logic."""
print(f"\nRunning calibration for {arm_id} on {port}")

bus = FeetechMotorsBus(port=port, motors=MOTORS)
bus.connect()
bus.disable_torque()

input("Move the leader arm to the MIDDLE of its range of motion and press ENTER...")
homing_offsets = bus.set_half_turn_homings()

print("Move all joints through their FULL range of motion.")
print("Press ENTER when done...")
range_mins, range_maxes = bus.record_ranges_of_motion()

calibration = {
name: MotorCalibration(
id=MOTORS[name].id,
drive_mode=0,
homing_offset=homing_offsets[name],
range_min=range_mins[name],
range_max=range_maxes[name],
)
for name in MOTOR_NAMES
}

path = get_calibration_path(arm_id)
save_calibration(path, calibration)
print(f"Calibration saved to {path}")

bus.disconnect()
return calibration


def main():
parser = argparse.ArgumentParser(description="SO101 leader arm joint state publisher")
parser.add_argument("--port", default="/dev/ttyACM0")
parser.add_argument("--id", default="leader_arm", help="Calibration ID")
parser.add_argument("--bind", default="tcp://0.0.0.0:5556")
parser.add_argument("--rate", type=int, default=50, help="Publish rate in Hz")
parser.add_argument("--recalibrate", action="store_true", help="Force recalibration")
args = parser.parse_args()

calib_path = get_calibration_path(args.id)

if args.recalibrate or not os.path.exists(calib_path):
calibration = calibrate(args.port, args.id)
else:
calibration = load_calibration(calib_path)
print(f"Loaded calibration from {calib_path}")

bus = FeetechMotorsBus(port=args.port, motors=MOTORS, calibration=calibration)
bus.connect()
bus.disable_torque()

ctx = zmq.Context()
pub = ctx.socket(zmq.PUB)
pub.setsockopt(zmq.CONFLATE, 1)
pub.bind(args.bind)
print(f"Publishing on {args.bind} at {args.rate} Hz")
time.sleep(0.5)

interval = 1.0 / args.rate
count = 0
next_t = time.monotonic()

try:
while True:
positions = bus.sync_read("Present_Position")
values = [positions[name] for name in MOTOR_NAMES]
pub.send(struct.pack("<6f", *values), zmq.NOBLOCK)

count += 1
if count % (args.rate * 10) == 0:
print(f"{count} msgs sent")

next_t += interval
sleep_t = next_t - time.monotonic()
if sleep_t > 0:
time.sleep(sleep_t)
except KeyboardInterrupt:
print(f"\nDone: {count} msgs")
finally:
bus.disconnect()
pub.close()
ctx.term()


if __name__ == "__main__":
main()
18 changes: 16 additions & 2 deletions scripts/environments/teleoperation/teleop_se3_agent.py
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,15 @@
parser.add_argument(
"--port", type=str, default="/dev/ttyACM0", help="Port for the teleop device:so101leader, default is /dev/ttyACM0"
)
parser.add_argument(
"--remote_endpoint",
type=str,
default=None,
help=(
"ZMQ endpoint for remote so101leader (e.g. tcp://192.168.1.10:5556). Uses so101_joint_state_server.py on the"
" remote machine."
),
)
parser.add_argument(
"--left_arm_port",
type=str,
Expand Down Expand Up @@ -246,9 +255,14 @@ def main(): # noqa: C901

teleop_interface = SO101Gamepad(env, sensitivity=args_cli.sensitivity)
elif args_cli.teleop_device == "so101leader":
from leisaac.devices import SO101Leader
if args_cli.remote_endpoint:
from leisaac.devices import SO101LeaderRemote

teleop_interface = SO101LeaderRemote(env, endpoint=args_cli.remote_endpoint)
else:
from leisaac.devices import SO101Leader

teleop_interface = SO101Leader(env, port=args_cli.port, recalibrate=args_cli.recalibrate)
teleop_interface = SO101Leader(env, port=args_cli.port, recalibrate=args_cli.recalibrate)
elif args_cli.teleop_device == "bi-so101leader":
from leisaac.devices import BiSO101Leader

Expand Down
8 changes: 6 additions & 2 deletions source/leisaac/leisaac/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -8,5 +8,9 @@
"""

# Register Gym environments.
from .tasks import *
from .utils import monkey_patch
try:
from .tasks import *
from .utils import monkey_patch
except ImportError as e:
print(f"[leisaac] ERROR: Failed to import: {e!r}")
print("[leisaac] If you are using remote teleoperation, you can ignore the above error.")
49 changes: 44 additions & 5 deletions source/leisaac/leisaac/devices/__init__.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,44 @@
from .device_base import DeviceBase
from .gamepad import SO101Gamepad
from .keyboard import SO101Keyboard
from .lekiwi import LeKiwiGamepad, LeKiwiKeyboard, LeKiwiLeader
from .lerobot import BiSO101Leader, SO101Leader
from importlib import import_module
from typing import TYPE_CHECKING

if TYPE_CHECKING:
from .device_base import DeviceBase
from .gamepad import SO101Gamepad
from .keyboard import SO101Keyboard
from .lekiwi import LeKiwiGamepad, LeKiwiKeyboard, LeKiwiLeader
from .lerobot import BiSO101Leader, SO101Leader, SO101LeaderRemote

__all__ = [
"DeviceBase",
"SO101Gamepad",
"SO101Keyboard",
"LeKiwiGamepad",
"LeKiwiKeyboard",
"LeKiwiLeader",
"BiSO101Leader",
"SO101Leader",
"SO101LeaderRemote",
]

_LAZY_IMPORTS = {
"DeviceBase": (".device_base", "DeviceBase"),
"SO101Gamepad": (".gamepad", "SO101Gamepad"),
"SO101Keyboard": (".keyboard", "SO101Keyboard"),
"LeKiwiGamepad": (".lekiwi", "LeKiwiGamepad"),
"LeKiwiKeyboard": (".lekiwi", "LeKiwiKeyboard"),
"LeKiwiLeader": (".lekiwi", "LeKiwiLeader"),
"BiSO101Leader": (".lerobot", "BiSO101Leader"),
"SO101Leader": (".lerobot", "SO101Leader"),
"SO101LeaderRemote": (".lerobot", "SO101LeaderRemote"),
}


def __getattr__(name):
if name not in _LAZY_IMPORTS:
raise AttributeError(f"module {__name__!r} has no attribute {name!r}")

module_name, attr_name = _LAZY_IMPORTS[name]
module = import_module(module_name, __name__)
value = getattr(module, attr_name)
globals()[name] = value
return value
Loading
Loading