略
一些检查失败了
Quality / Style (push) Failing after 4s
Quality / Typos (push) Failing after 3s
Tests / Pytest (push) Failing after 4s
Tests / Pytest (minimal install) (push) Failing after 4s
Tests / End-to-end (push) Failing after 3s
Secret Leaks / trufflehog (push) Failing after 3s
Builds / CPU (push) Has been cancelled
Builds / GPU (push) Has been cancelled
Builds / GPU Dev (push) Has been cancelled
Nightly / CPU (push) Has been cancelled
Nightly / GPU (push) Has been cancelled

这个提交包含在:
2026-05-14 23:49:53 +08:00
父节点 846b968eea
当前提交 46cb98dd81
共有 18 个文件被更改,包括 1076 次插入 和 53 次删除

1
WSRobotClient 子模块

子模块 WSRobotClient 已添加到 34f5276a1f

44
follower.json 普通文件
查看文件

@ -0,0 +1,44 @@
{
"shoulder_pan": {
"id": 1,
"drive_mode": 0,
"homing_offset": -936,
"range_min": 1060,
"range_max": 3013
},
"shoulder_lift": {
"id": 2,
"drive_mode": 0,
"homing_offset": 1096,
"range_min": 909,
"range_max": 3194
},
"elbow_flex": {
"id": 3,
"drive_mode": 0,
"homing_offset": 1131,
"range_min": 1855,
"range_max": 4063
},
"wrist_flex": {
"id": 4,
"drive_mode": 0,
"homing_offset": -2015,
"range_min": 914,
"range_max": 3270
},
"wrist_roll": {
"id": 5,
"drive_mode": 0,
"homing_offset": 1005,
"range_min": 777,
"range_max": 3588
},
"gripper": {
"id": 6,
"drive_mode": 0,
"homing_offset": 1271,
"range_min": 1739,
"range_max": 3197
}
}

44
follower2.json 普通文件
查看文件

@ -0,0 +1,44 @@
{
"shoulder_pan": {
"id": 1,
"drive_mode": 0,
"homing_offset": -967,
"range_min": 923,
"range_max": 3446
},
"shoulder_lift": {
"id": 2,
"drive_mode": 0,
"homing_offset": 1124,
"range_min": 788,
"range_max": 3165
},
"elbow_flex": {
"id": 3,
"drive_mode": 0,
"homing_offset": 1041,
"range_min": 0,
"range_max": 4095
},
"wrist_flex": {
"id": 4,
"drive_mode": 0,
"homing_offset": -1831,
"range_min": 729,
"range_max": 3069
},
"wrist_roll": {
"id": 5,
"drive_mode": 0,
"homing_offset": 1092,
"range_min": 1078,
"range_max": 3179
},
"gripper": {
"id": 6,
"drive_mode": 0,
"homing_offset": 982,
"range_min": 2014,
"range_max": 3565
}
}

44
leader.json 普通文件
查看文件

@ -0,0 +1,44 @@
{
"shoulder_pan": {
"id": 1,
"drive_mode": 0,
"homing_offset": -869,
"range_min": 1152,
"range_max": 2804
},
"shoulder_lift": {
"id": 2,
"drive_mode": 0,
"homing_offset": -2024,
"range_min": 950,
"range_max": 3144
},
"elbow_flex": {
"id": 3,
"drive_mode": 0,
"homing_offset": -2024,
"range_min": 1861,
"range_max": 4083
},
"wrist_flex": {
"id": 4,
"drive_mode": 0,
"homing_offset": -1036,
"range_min": 889,
"range_max": 3248
},
"wrist_roll": {
"id": 5,
"drive_mode": 0,
"homing_offset": 1846,
"range_min": 168,
"range_max": 3358
},
"gripper": {
"id": 6,
"drive_mode": 0,
"homing_offset": 332,
"range_min": 1662,
"range_max": 2934
}
}

44
leader2.json 普通文件
查看文件

@ -0,0 +1,44 @@
{
"shoulder_pan": {
"id": 1,
"drive_mode": 0,
"homing_offset": -841,
"range_min": 1177,
"range_max": 2976
},
"shoulder_lift": {
"id": 2,
"drive_mode": 0,
"homing_offset": 2002,
"range_min": 872,
"range_max": 3340
},
"elbow_flex": {
"id": 3,
"drive_mode": 0,
"homing_offset": -2014,
"range_min": 1852,
"range_max": 4089
},
"wrist_flex": {
"id": 4,
"drive_mode": 0,
"homing_offset": -999,
"range_min": 845,
"range_max": 3221
},
"wrist_roll": {
"id": 5,
"drive_mode": 0,
"homing_offset": 1829,
"range_min": 743,
"range_max": 3581
},
"gripper": {
"id": 6,
"drive_mode": 0,
"homing_offset": -21,
"range_min": 2041,
"range_max": 3261
}
}

44
so_leader.json 普通文件
查看文件

@ -0,0 +1,44 @@
{
"shoulder_pan": {
"id": 1,
"drive_mode": 0,
"homing_offset": -912,
"range_min": 700,
"range_max": 3399
},
"shoulder_lift": {
"id": 2,
"drive_mode": 0,
"homing_offset": -2043,
"range_min": 810,
"range_max": 3287
},
"elbow_flex": {
"id": 3,
"drive_mode": 0,
"homing_offset": -1993,
"range_min": 1833,
"range_max": 4066
},
"wrist_flex": {
"id": 4,
"drive_mode": 0,
"homing_offset": -1066,
"range_min": 908,
"range_max": 3290
},
"wrist_roll": {
"id": 5,
"drive_mode": 0,
"homing_offset": 1812,
"range_min": 1046,
"range_max": 3109
},
"gripper": {
"id": 6,
"drive_mode": 0,
"homing_offset": 322,
"range_min": 1678,
"range_max": 2951
}
}

查看文件

@ -223,11 +223,11 @@ class FeetechMotorsBus(MotorsBus):
for motor in self.motors: for motor in self.motors:
# By default, Feetech motors have a 500µs delay response time (corresponding to a value of 250 on # By default, Feetech motors have a 500µs delay response time (corresponding to a value of 250 on
# the 'Return_Delay_Time' address). We ensure this is reduced to the minimum of 2µs (value of 0). # the 'Return_Delay_Time' address). We ensure this is reduced to the minimum of 2µs (value of 0).
self.write("Return_Delay_Time", motor, 0) self.write("Return_Delay_Time", motor, 0, num_retry=2)
# Set 'Maximum_Acceleration' to 254 to speedup acceleration and deceleration of the motors. # Set 'Maximum_Acceleration' to 254 to speedup acceleration and deceleration of the motors.
# Note: this address is not in the official STS3215 Memory Table # Note: this address is not in the official STS3215 Memory Table
self.write("Maximum_Acceleration", motor, 254) self.write("Maximum_Acceleration", motor, 254, num_retry=2)
self.write("Acceleration", motor, 254) self.write("Acceleration", motor, 254, num_retry=2)
@property @property
def is_calibrated(self) -> bool: def is_calibrated(self) -> bool:
@ -273,9 +273,9 @@ class FeetechMotorsBus(MotorsBus):
def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None: def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None:
for motor, calibration in calibration_dict.items(): for motor, calibration in calibration_dict.items():
if self.protocol_version == 0: if self.protocol_version == 0:
self.write("Homing_Offset", motor, calibration.homing_offset) self.write("Homing_Offset", motor, calibration.homing_offset, num_retry=2)
self.write("Min_Position_Limit", motor, calibration.range_min) self.write("Min_Position_Limit", motor, calibration.range_min, num_retry=2)
self.write("Max_Position_Limit", motor, calibration.range_max) self.write("Max_Position_Limit", motor, calibration.range_max, num_retry=2)
self.calibration = calibration_dict self.calibration = calibration_dict

查看文件

@ -329,7 +329,7 @@ class MotorsBus(abc.ABC):
else: else:
raise TypeError(f"'{motor}' should be int, str.") raise TypeError(f"'{motor}' should be int, str.")
def _get_motor_model(self, motor: NameOrID) -> int: def _get_motor_model(self, motor: NameOrID) -> str:
if isinstance(motor, str): if isinstance(motor, str):
return self.motors[motor].model return self.motors[motor].model
elif isinstance(motor, int): elif isinstance(motor, int):
@ -682,9 +682,12 @@ class MotorsBus(abc.ABC):
for motor in motors: for motor in motors:
model = self._get_motor_model(motor) model = self._get_motor_model(motor)
max_res = self.model_resolution_table[model] - 1 max_res = self.model_resolution_table[model] - 1
self.write("Homing_Offset", motor, 0, normalize=False) motor_name = motor if isinstance(motor, str) else self._id_to_name(motor)
self.write("Min_Position_Limit", motor, 0, normalize=False) # Calibration routines are often run on a live hardware bus that can occasionally drop packets.
self.write("Max_Position_Limit", motor, max_res, normalize=False) # Use a small retry budget here to avoid forcing a full restart on transient RX timeouts.
self.write("Homing_Offset", motor_name, 0, normalize=False, num_retry=2)
self.write("Min_Position_Limit", motor_name, 0, normalize=False, num_retry=2)
self.write("Max_Position_Limit", motor_name, max_res, normalize=False, num_retry=2)
self.calibration = {} self.calibration = {}
@ -707,11 +710,22 @@ class MotorsBus(abc.ABC):
elif not isinstance(motors, list): elif not isinstance(motors, list):
raise TypeError(motors) raise TypeError(motors)
self.reset_calibration(motors) motor_names: list[str] = [m if isinstance(m, str) else self._id_to_name(m) for m in motors]
actual_positions = self.sync_read("Present_Position", motors, normalize=False)
self.reset_calibration(motor_names)
try:
actual_positions = self.sync_read(
"Present_Position", motor_names, normalize=False, num_retry=2
)
except ConnectionError:
# Fallback for controllers/adapters that are flaky with group reads.
actual_positions = {
motor: self.read("Present_Position", motor, normalize=False, num_retry=2)
for motor in motor_names
}
homing_offsets = self._get_half_turn_homings(actual_positions) homing_offsets = self._get_half_turn_homings(actual_positions)
for motor, offset in homing_offsets.items(): for motor, offset in homing_offsets.items():
self.write("Homing_Offset", motor, offset) self.write("Homing_Offset", motor, offset, num_retry=2)
return homing_offsets return homing_offsets
@ -743,20 +757,39 @@ class MotorsBus(abc.ABC):
elif not isinstance(motors, list): elif not isinstance(motors, list):
raise TypeError(motors) raise TypeError(motors)
start_positions = self.sync_read("Present_Position", motors, normalize=False) motor_names: list[str] = [m if isinstance(m, str) else self._id_to_name(m) for m in motors]
use_sync_read = True
def read_positions() -> dict[str, Value]:
nonlocal use_sync_read
if use_sync_read:
try:
return self.sync_read(
"Present_Position", motor_names, normalize=False, num_retry=2
)
except ConnectionError:
use_sync_read = False
return {
motor: self.read("Present_Position", motor, normalize=False, num_retry=2)
for motor in motor_names
}
start_positions = read_positions()
mins = start_positions.copy() mins = start_positions.copy()
maxes = start_positions.copy() maxes = start_positions.copy()
user_pressed_enter = False user_pressed_enter = False
while not user_pressed_enter: while not user_pressed_enter:
positions = self.sync_read("Present_Position", motors, normalize=False) positions = read_positions()
mins = {motor: min(positions[motor], min_) for motor, min_ in mins.items()} mins = {motor: min(positions[motor], min_) for motor, min_ in mins.items()}
maxes = {motor: max(positions[motor], max_) for motor, max_ in maxes.items()} maxes = {motor: max(positions[motor], max_) for motor, max_ in maxes.items()}
if display_values: if display_values:
print("\n-------------------------------------------") print("\n-------------------------------------------")
print(f"{'NAME':<15} | {'MIN':>6} | {'POS':>6} | {'MAX':>6}") print(f"{'NAME':<15} | {'MIN':>6} | {'POS':>6} | {'MAX':>6}")
for motor in motors: for motor in motor_names:
print(f"{motor:<15} | {mins[motor]:>6} | {positions[motor]:>6} | {maxes[motor]:>6}") print(f"{motor:<15} | {mins[motor]:>6} | {positions[motor]:>6} | {maxes[motor]:>6}")
if enter_pressed(): if enter_pressed():
@ -764,9 +797,9 @@ class MotorsBus(abc.ABC):
if display_values and not user_pressed_enter: if display_values and not user_pressed_enter:
# Move cursor up to overwrite the previous output # Move cursor up to overwrite the previous output
move_cursor_up(len(motors) + 3) move_cursor_up(len(motor_names) + 3)
same_min_max = [motor for motor in motors if mins[motor] == maxes[motor]] same_min_max = [motor for motor in motor_names if mins[motor] == maxes[motor]]
if same_min_max: if same_min_max:
raise ValueError(f"Some motors have the same min and max values:\n{pformat(same_min_max)}") raise ValueError(f"Some motors have the same min and max values:\n{pformat(same_min_max)}")

查看文件

@ -351,11 +351,19 @@ def record(cfg: RecordConfig) -> LeRobotDataset:
if teleop is not None: if teleop is not None:
teleop.disconnect() teleop.disconnect()
if not is_headless() and listener is not None: if listener is not None and hasattr(listener, "stop"):
listener.stop() listener.stop()
if cfg.dataset.push_to_hub: if cfg.dataset.push_to_hub:
dataset.push_to_hub(tags=cfg.dataset.tags, private=cfg.dataset.private) try:
dataset.push_to_hub(tags=cfg.dataset.tags, private=cfg.dataset.private)
except Exception as e:
logging.error(
"Push to Hugging Face Hub failed, but your dataset is saved locally at: "
f"{dataset.root}\n"
"To skip uploading, re-run with: --dataset.push_to_hub=false\n"
f"Error: {e}"
)
log_say("Exiting", cfg.play_sounds) log_say("Exiting", cfg.play_sounds)
return dataset return dataset

查看文件

@ -176,7 +176,33 @@ class KochFollower(Robot):
# Read arm position # Read arm position
start = time.perf_counter() start = time.perf_counter()
obs_dict = self.bus.sync_read("Present_Position") use_sync_read = getattr(self, "_use_sync_read_present_position", True)
if use_sync_read:
try:
obs_dict = self.bus.sync_read("Present_Position", num_retry=2)
except ConnectionError as e:
self._use_sync_read_present_position = False
if not getattr(self, "_warned_sync_read_fallback", False):
logger.warning(
"sync_read('Present_Position') failed; falling back to sequential reads. %s",
str(e),
)
self._warned_sync_read_fallback = True
clear_port = getattr(self.bus.port_handler, "clearPort", None)
if callable(clear_port):
try:
clear_port()
except Exception:
pass
obs_dict = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
else:
obs_dict = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()} obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()}
dt_ms = (time.perf_counter() - start) * 1e3 dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read state: {dt_ms:.1f}ms") logger.debug(f"{self} read state: {dt_ms:.1f}ms")

查看文件

@ -160,7 +160,33 @@ class SO100Follower(Robot):
# Read arm position # Read arm position
start = time.perf_counter() start = time.perf_counter()
obs_dict = self.bus.sync_read("Present_Position") use_sync_read = getattr(self, "_use_sync_read_present_position", True)
if use_sync_read:
try:
obs_dict = self.bus.sync_read("Present_Position", num_retry=2)
except ConnectionError as e:
self._use_sync_read_present_position = False
if not getattr(self, "_warned_sync_read_fallback", False):
logger.warning(
"sync_read('Present_Position') failed; falling back to sequential reads. %s",
str(e),
)
self._warned_sync_read_fallback = True
clear_port = getattr(self.bus.port_handler, "clearPort", None)
if callable(clear_port):
try:
clear_port()
except Exception:
pass
obs_dict = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
else:
obs_dict = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()} obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()}
dt_ms = (time.perf_counter() - start) * 1e3 dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read state: {dt_ms:.1f}ms") logger.debug(f"{self} read state: {dt_ms:.1f}ms")

查看文件

@ -181,7 +181,33 @@ class SO100FollowerEndEffector(SO100Follower):
# Read arm position # Read arm position
start = time.perf_counter() start = time.perf_counter()
obs_dict = self.bus.sync_read("Present_Position") use_sync_read = getattr(self, "_use_sync_read_present_position", True)
if use_sync_read:
try:
obs_dict = self.bus.sync_read("Present_Position", num_retry=2)
except ConnectionError as e:
self._use_sync_read_present_position = False
if not getattr(self, "_warned_sync_read_fallback", False):
logger.warning(
"sync_read('Present_Position') failed; falling back to sequential reads. %s",
str(e),
)
self._warned_sync_read_fallback = True
clear_port = getattr(self.bus.port_handler, "clearPort", None)
if callable(clear_port):
try:
clear_port()
except Exception:
pass
obs_dict = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
else:
obs_dict = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()} obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()}
dt_ms = (time.perf_counter() - start) * 1e3 dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read state: {dt_ms:.1f}ms") logger.debug(f"{self} read state: {dt_ms:.1f}ms")

查看文件

@ -156,7 +156,35 @@ class SO101Follower(Robot):
# Read arm position # Read arm position
start = time.perf_counter() start = time.perf_counter()
obs_dict = self.bus.sync_read("Present_Position") use_sync_read = getattr(self, "_use_sync_read_present_position", True)
if use_sync_read:
try:
obs_dict = self.bus.sync_read("Present_Position", num_retry=2)
except ConnectionError as e:
self._use_sync_read_present_position = False
# GroupSyncRead can be flaky on some USB↔UART adapters/controllers.
# Disable it after the first failure to avoid repeated exceptions.
if not getattr(self, "_warned_sync_read_fallback", False):
logger.warning(
"sync_read('Present_Position') failed; falling back to sequential reads. %s",
str(e),
)
self._warned_sync_read_fallback = True
clear_port = getattr(self.bus.port_handler, "clearPort", None)
if callable(clear_port):
try:
clear_port()
except Exception:
pass
obs_dict = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
else:
obs_dict = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()} obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()}
dt_ms = (time.perf_counter() - start) * 1e3 dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read state: {dt_ms:.1f}ms") logger.debug(f"{self} read state: {dt_ms:.1f}ms")

查看文件

@ -0,0 +1,427 @@
#!/usr/bin/env python
"""WebSocket server for controlling a real SO-101 follower arm.
This module implements the same minimal WebSocket protocol used by
`WSRobotClient/mock-server` so you can reuse that frontend with a real arm.
Protocol
--------
- On connect, server sends an init payload (InitResponse):
`{ "id": "<robot_id>", "joints": [ ... ] }`
- Client sends joint commands (MoveRequest):
`{ "id": "<joint_id>", "angel": <number> }`
- Server responds for each command (MoveAck):
`{ "type": "move_ack", "joint": "<joint_id>", "angle": <number>, "status": "success"|"error" }`
- Heartbeat:
Client: `{ "type": "ping", "timestamp": <ms> }`
Server: `{ "type": "pong", "timestamp": <ms> }`
By default, the server uses *degrees* for the 5 arm joints, and *0..100* for
the gripper, matching the WSRobotClient UI.
Requirements
------------
- LeRobot with Feetech support: `pip install -e ".[feetech]"`
- Web server deps: `pip install fastapi uvicorn[standard] websockets`
Example
-------
```bash
python -m lerobot.so101_follower_ws_server \
--robot.type=so101_follower \
--robot.port=/dev/ttyACM0 \
--robot.id=my_so101 \
--host=0.0.0.0 \
--port=8000 \
--units=degrees
# Then connect from the WSRobotClient frontend using:
# ws://localhost:8000/ws/six_axis
# (The server accepts any /ws/<robot_type> path, e.g. /ws/scara or /ws/so101_follower)
```
"""
import asyncio
import json
import logging
from dataclasses import asdict, dataclass
from typing import Any, Literal
import draccus
try:
from lerobot.robots.so101_follower import SO101Follower, SO101FollowerConfig
except ImportError as exc: # pragma: no cover
raise ImportError(
"SO101Follower WS server requires Feetech support. "
"Install with: pip install -e '.[feetech]'"
) from exc
from lerobot.robots import RobotConfig
from lerobot.utils.utils import init_logging
logger = logging.getLogger(__name__)
Units = Literal["degrees", "normalized", "raw"]
_SO101_JOINTS: list[dict[str, Any]] = [
{"id": "shoulder_pan", "type": "flex_w", "next": "shoulder_lift"},
{"id": "shoulder_lift", "type": "flex_v", "next": "elbow_flex"},
{"id": "elbow_flex", "type": "flex_v", "next": "wrist_flex"},
{"id": "wrist_flex", "type": "flex_v", "next": "wrist_roll"},
{"id": "wrist_roll", "type": "flex_w", "next": "gripper"},
{"id": "gripper", "type": "gripper", "next": None},
]
@dataclass
class SO101FollowerWsServerConfig:
robot: RobotConfig
host: str = "0.0.0.0"
port: int = 8000
# Units expected in incoming `MoveRequest.angel`.
# - degrees: 5 joints in degrees, gripper in 0..100
# - normalized: 5 joints in -100..100, gripper in 0..100
# - raw: raw motor units (encoder ticks)
units: Units = "degrees"
# Only allow a single active client to avoid serial-port conflicts.
single_client: bool = True
# Use the current measured joint positions as the defaults in init payload.
init_default_from_present: bool = True
# Disconnect robot (and optionally disable torque) when the WS client disconnects.
disconnect_on_client_close: bool = True
# Log every move request (can be noisy when dragging sliders)
log_moves: bool = True
def __post_init__(self) -> None:
if self.robot.id is None:
raise ValueError("Please provide a robot id via --robot.id=<name> (needed for calibration lookup).")
if not isinstance(self.robot, SO101FollowerConfig):
raise ValueError(
"This server only supports SO-101 follower. "
f"Please use --robot.type=so101_follower (got: {self.robot.type})."
)
def _require_fastapi() -> Any:
try:
from fastapi import FastAPI, WebSocket, WebSocketDisconnect # noqa: WPS433
from fastapi.middleware.cors import CORSMiddleware # noqa: WPS433
except ImportError as exc: # pragma: no cover
raise ImportError(
"Missing optional dependencies for the WS server. "
"Install them with: pip install fastapi uvicorn[standard] websockets"
) from exc
return FastAPI, WebSocket, WebSocketDisconnect, CORSMiddleware
def _get_raw_range(robot: SO101Follower, motor: str) -> tuple[float, float]:
bus = robot.bus
if bus.calibration and motor in bus.calibration:
cal = bus.calibration[motor]
return float(cal.range_min), float(cal.range_max)
model = bus.motors[motor].model
max_res = bus.model_resolution_table[model] - 1
return 0.0, float(max_res)
def _get_degrees_range(robot: SO101Follower, motor: str) -> tuple[float, float]:
bus = robot.bus
if not bus.calibration or motor not in bus.calibration:
return -180.0, 180.0
cal = bus.calibration[motor]
model = bus.motors[motor].model
max_res = bus.model_resolution_table[model] - 1
span = float(cal.range_max - cal.range_min)
if max_res <= 0 or span <= 0:
return -180.0, 180.0
max_abs_deg = span * 180.0 / float(max_res)
max_abs_deg = max(0.0, max_abs_deg)
if max_abs_deg == 0.0:
return -180.0, 180.0
return -max_abs_deg, max_abs_deg
def _get_joint_range(robot: SO101Follower, motor: str, units: Units) -> tuple[float, float]:
if units == "raw":
return _get_raw_range(robot, motor)
if motor == "gripper":
return 0.0, 100.0
if units == "normalized":
return -100.0, 100.0
return _get_degrees_range(robot, motor)
def _clamp(value: float, low: float, high: float) -> float:
return min(max(value, low), high)
async def _to_thread(func, /, *args, **kwargs):
return await asyncio.to_thread(func, *args, **kwargs)
async def _read_present_positions(robot: SO101Follower, units: Units) -> dict[str, float] | None:
try:
if units == "raw":
present = await _to_thread(robot.bus.sync_read, "Present_Position", normalize=False)
return {k: float(v) for k, v in present.items()}
present = await _to_thread(robot.bus.sync_read, "Present_Position", normalize=True)
return {k: float(v) for k, v in present.items()}
except Exception as exc:
logger.warning(f"Failed to read Present_Position for init defaults: {exc}")
return None
def _build_init_response(
robot: SO101Follower,
units: Units,
present_positions: dict[str, float] | None,
) -> dict[str, Any]:
joints: list[dict[str, Any]] = []
for spec in _SO101_JOINTS:
motor = str(spec["id"])
low, high = _get_joint_range(robot, motor, units)
if present_positions and motor in present_positions:
default_val = _clamp(float(present_positions[motor]), low, high)
else:
if units == "raw":
default_val = (low + high) / 2.0
else:
default_val = 50.0 if motor == "gripper" else 0.0
joints.append(
{
"id": motor,
"range": [low, high],
"default": default_val,
"type": spec["type"],
"next": spec["next"],
}
)
robot_id = robot.id if robot.id is not None else robot.name
return {"id": str(robot_id), "joints": joints}
def _apply_units_to_robot_config(robot_cfg: SO101FollowerConfig, units: Units) -> SO101FollowerConfig:
# Normalize UI expects degrees by default.
if units == "degrees":
if not robot_cfg.use_degrees:
logger.info("Overriding --robot.use_degrees=false because units=degrees")
robot_cfg.use_degrees = True
elif units == "normalized":
robot_cfg.use_degrees = False
return robot_cfg
def _has_required_calibration(robot: SO101Follower, units: Units) -> bool:
if units == "raw":
return True
# Any normalized mode needs calibration for (un)normalization.
return bool(robot.bus.calibration)
def _build_app(cfg: SO101FollowerWsServerConfig):
FastAPI, WebSocket, WebSocketDisconnect, CORSMiddleware = _require_fastapi()
app = FastAPI(title="SO101 Follower WS Server")
app.add_middleware(
CORSMiddleware,
allow_origins=["*"],
allow_credentials=True,
allow_methods=["*"],
allow_headers=["*"],
)
client_lock = asyncio.Lock()
@app.get("/health")
async def health() -> dict[str, str]:
return {"status": "ok"}
@app.websocket("/ws/{robot_type}")
async def websocket_endpoint(websocket: WebSocket, robot_type: str = "six_axis"): # type: ignore[valid-type]
await websocket.accept()
if cfg.single_client and client_lock.locked():
await websocket.close(code=1013, reason="Robot is busy (single_client=true)")
return
lock_cm = client_lock if cfg.single_client else _NullAsyncLock()
async with lock_cm:
robot_cfg = cfg.robot
assert isinstance(robot_cfg, SO101FollowerConfig)
robot_cfg = _apply_units_to_robot_config(robot_cfg, cfg.units)
robot = SO101Follower(robot_cfg)
try:
await _to_thread(robot.connect, False)
if not _has_required_calibration(robot, cfg.units):
await websocket.send_json(
{
"type": "error",
"message": (
"No calibration found for this robot id. "
"Run: python -m lerobot.calibrate --robot.type=so101_follower "
"--robot.port=<PORT> --robot.id=<ID> (or start server with --units=raw)."
),
}
)
await websocket.close(code=1011, reason="Missing calibration")
return
present_positions = None
if cfg.init_default_from_present:
present_positions = await _read_present_positions(robot, cfg.units)
await websocket.send_json(_build_init_response(robot, cfg.units, present_positions))
if cfg.log_moves:
logger.info(
f"WS connected: robot_type='{robot_type}', units='{cfg.units}', robot_id='{robot.id}'"
)
while True:
message = await websocket.receive_text()
data = json.loads(message)
if isinstance(data, dict) and data.get("type") == "ping":
await websocket.send_json(
{"type": "pong", "timestamp": int(data.get("timestamp") or 0)}
)
continue
if not isinstance(data, dict) or "id" not in data or "angel" not in data:
logger.info(f"Unknown message: {data}")
continue
joint_id = str(data["id"])
try:
target = float(data["angel"])
except Exception:
await websocket.send_json(
{
"type": "move_ack",
"joint": joint_id,
"angle": data.get("angel"),
"status": "error",
}
)
continue
if joint_id not in robot.bus.motors:
await websocket.send_json(
{
"type": "move_ack",
"joint": joint_id,
"angle": target,
"status": "error",
}
)
continue
low, high = _get_joint_range(robot, joint_id, cfg.units)
requested = target
target = _clamp(target, low, high)
if cfg.log_moves:
clamp_note = "" if target == requested else f" (clamped from {requested})"
logger.info(
f"MoveRequest joint='{joint_id}' target={target}{clamp_note} units='{cfg.units}'"
)
try:
if cfg.units == "raw":
await _to_thread(
robot.bus.sync_write,
"Goal_Position",
{joint_id: int(target)},
normalize=False,
)
else:
# degrees / normalized modes rely on bus normalization.
await _to_thread(
robot.bus.sync_write,
"Goal_Position",
{joint_id: target},
normalize=True,
)
await websocket.send_json(
{
"type": "move_ack",
"joint": joint_id,
"angle": target,
"status": "success",
}
)
except Exception as exc:
logger.exception(f"Failed to move {joint_id} to {target}: {exc}")
await websocket.send_json(
{
"type": "move_ack",
"joint": joint_id,
"angle": target,
"status": "error",
}
)
except WebSocketDisconnect:
logger.info("Client disconnected")
except json.JSONDecodeError as exc:
logger.warning(f"JSON parse error: {exc}")
except Exception as exc:
logger.exception(f"WS server error: {exc}")
finally:
if cfg.disconnect_on_client_close:
try:
await _to_thread(robot.disconnect)
except Exception:
pass
return app
class _NullAsyncLock:
async def __aenter__(self):
return self
async def __aexit__(self, exc_type, exc, tb):
return False
@draccus.wrap() # type: ignore[attr-defined]
def main(cfg: SO101FollowerWsServerConfig) -> None:
init_logging()
logger.info(f"Starting so101_follower WS server with config: {asdict(cfg)}")
try:
import uvicorn # noqa: WPS433
except ImportError as exc: # pragma: no cover
raise ImportError("Missing 'uvicorn'. Install with: pip install uvicorn[standard]") from exc
app = _build_app(cfg)
uvicorn.run(app, host=cfg.host, port=cfg.port)
if __name__ == "__main__":
main() # type: ignore[call-arg]

查看文件

@ -83,9 +83,9 @@ class SO100Leader(Teleoperator):
def calibrate(self) -> None: def calibrate(self) -> None:
logger.info(f"\nRunning calibration of {self}") logger.info(f"\nRunning calibration of {self}")
self.bus.disable_torque() self.bus.disable_torque(num_retry=5)
for motor in self.bus.motors: for motor in self.bus.motors:
self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value) self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value, num_retry=5)
input(f"Move {self} to the middle of its range of motion and press ENTER....") input(f"Move {self} to the middle of its range of motion and press ENTER....")
homing_offsets = self.bus.set_half_turn_homings() homing_offsets = self.bus.set_half_turn_homings()
@ -115,10 +115,10 @@ class SO100Leader(Teleoperator):
print(f"Calibration saved to {self.calibration_fpath}") print(f"Calibration saved to {self.calibration_fpath}")
def configure(self) -> None: def configure(self) -> None:
self.bus.disable_torque() self.bus.disable_torque(num_retry=5)
self.bus.configure_motors() self.bus.configure_motors()
for motor in self.bus.motors: for motor in self.bus.motors:
self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value) self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value, num_retry=5)
def setup_motors(self) -> None: def setup_motors(self) -> None:
for motor in reversed(self.bus.motors): for motor in reversed(self.bus.motors):
@ -128,7 +128,33 @@ class SO100Leader(Teleoperator):
def get_action(self) -> dict[str, float]: def get_action(self) -> dict[str, float]:
start = time.perf_counter() start = time.perf_counter()
action = self.bus.sync_read("Present_Position") use_sync_read = getattr(self, "_use_sync_read_present_position", True)
if use_sync_read:
try:
action = self.bus.sync_read("Present_Position", num_retry=2)
except ConnectionError as e:
self._use_sync_read_present_position = False
if not getattr(self, "_warned_sync_read_fallback", False):
logger.warning(
"sync_read('Present_Position') failed; falling back to sequential reads. %s",
str(e),
)
self._warned_sync_read_fallback = True
clear_port = getattr(self.bus.port_handler, "clearPort", None)
if callable(clear_port):
try:
clear_port()
except Exception:
pass
action = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
else:
action = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
action = {f"{motor}.pos": val for motor, val in action.items()} action = {f"{motor}.pos": val for motor, val in action.items()}
dt_ms = (time.perf_counter() - start) * 1e3 dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read action: {dt_ms:.1f}ms") logger.debug(f"{self} read action: {dt_ms:.1f}ms")

查看文件

@ -84,9 +84,9 @@ class SO101Leader(Teleoperator):
def calibrate(self) -> None: def calibrate(self) -> None:
logger.info(f"\nRunning calibration of {self}") logger.info(f"\nRunning calibration of {self}")
self.bus.disable_torque() self.bus.disable_torque(num_retry=5)
for motor in self.bus.motors: for motor in self.bus.motors:
self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value) self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value, num_retry=5)
input(f"Move {self} to the middle of its range of motion and press ENTER....") input(f"Move {self} to the middle of its range of motion and press ENTER....")
homing_offsets = self.bus.set_half_turn_homings() homing_offsets = self.bus.set_half_turn_homings()
@ -112,10 +112,10 @@ class SO101Leader(Teleoperator):
print(f"Calibration saved to {self.calibration_fpath}") print(f"Calibration saved to {self.calibration_fpath}")
def configure(self) -> None: def configure(self) -> None:
self.bus.disable_torque() self.bus.disable_torque(num_retry=5)
self.bus.configure_motors() self.bus.configure_motors()
for motor in self.bus.motors: for motor in self.bus.motors:
self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value) self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value, num_retry=5)
def setup_motors(self) -> None: def setup_motors(self) -> None:
for motor in reversed(self.bus.motors): for motor in reversed(self.bus.motors):
@ -125,7 +125,35 @@ class SO101Leader(Teleoperator):
def get_action(self) -> dict[str, float]: def get_action(self) -> dict[str, float]:
start = time.perf_counter() start = time.perf_counter()
action = self.bus.sync_read("Present_Position") use_sync_read = getattr(self, "_use_sync_read_present_position", True)
if use_sync_read:
try:
action = self.bus.sync_read("Present_Position", num_retry=2)
except ConnectionError as e:
# GroupSyncRead can be flaky on some USB↔UART adapters/controllers.
# Disable it after the first failure to avoid repeated exceptions.
self._use_sync_read_present_position = False
if not getattr(self, "_warned_sync_read_fallback", False):
logger.warning(
"sync_read('Present_Position') failed; falling back to sequential reads. %s",
str(e),
)
self._warned_sync_read_fallback = True
clear_port = getattr(self.bus.port_handler, "clearPort", None)
if callable(clear_port):
try:
clear_port()
except Exception:
pass
action = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
else:
action = {
motor: self.bus.read("Present_Position", motor, num_retry=2)
for motor in self.bus.motors
}
action = {f"{motor}.pos": val for motor, val in action.items()} action = {f"{motor}.pos": val for motor, val in action.items()}
dt_ms = (time.perf_counter() - start) * 1e3 dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read action: {dt_ms:.1f}ms") logger.debug(f"{self} read action: {dt_ms:.1f}ms")

查看文件

@ -18,6 +18,10 @@
import logging import logging
import os
import sys
import threading
import time
import traceback import traceback
from contextlib import nullcontext from contextlib import nullcontext
from copy import copy from copy import copy
@ -144,35 +148,183 @@ def init_keyboard_listener():
events["rerecord_episode"] = False events["rerecord_episode"] = False
events["stop_recording"] = False events["stop_recording"] = False
if is_headless(): def _handle_control_key(key_name: str):
logging.warning( if key_name == "right":
"Headless environment detected. On-screen cameras display and keyboard inputs will not be available." print("Right arrow key pressed. Exiting loop...")
) events["exit_early"] = True
listener = None elif key_name == "left":
print("Left arrow key pressed. Exiting loop and rerecord the last episode...")
events["rerecord_episode"] = True
events["exit_early"] = True
elif key_name == "esc":
print("Escape key pressed. Stopping data recording...")
events["stop_recording"] = True
events["exit_early"] = True
# Prefer terminal-based key reading when we have a real TTY.
# This avoids echoing escape sequences like '^[[C' to the terminal.
if hasattr(sys.stdin, "isatty") and sys.stdin.isatty():
class _TerminalKeyboardListener:
def __init__(self):
self._stop_event = threading.Event()
self._thread = threading.Thread(target=self._run, daemon=True)
self._fd = None
self._old_term_attrs = None
def start(self):
self._thread.start()
def stop(self):
self._stop_event.set()
if self._thread.is_alive():
self._thread.join(timeout=1.0)
self._restore_terminal()
def _set_terminal_rawish(self):
if os.name == "nt":
return
try:
import atexit
import termios
self._fd = sys.stdin.fileno()
self._old_term_attrs = termios.tcgetattr(self._fd)
new_attrs = termios.tcgetattr(self._fd)
# Disable canonical mode and echo, but keep signals (Ctrl+C).
new_attrs[3] = new_attrs[3] & ~(termios.ICANON | termios.ECHO)
termios.tcsetattr(self._fd, termios.TCSADRAIN, new_attrs)
atexit.register(self._restore_terminal)
except Exception:
self._fd = None
self._old_term_attrs = None
def _restore_terminal(self):
if os.name == "nt":
return
if self._fd is None or self._old_term_attrs is None:
return
try:
import termios
termios.tcsetattr(self._fd, termios.TCSADRAIN, self._old_term_attrs)
except Exception:
pass
def _run(self):
self._set_terminal_rawish()
if os.name == "nt":
try:
import msvcrt
while not self._stop_event.is_set():
if not msvcrt.kbhit():
time.sleep(0.05)
continue
ch = msvcrt.getwch()
if ch == "\x1b":
_handle_control_key("esc")
continue
# Arrow keys come as a two-char sequence on Windows.
if ch in ("\x00", "\xe0") and msvcrt.kbhit():
code = msvcrt.getwch()
if code == "M":
_handle_control_key("right")
elif code == "K":
_handle_control_key("left")
finally:
self._restore_terminal()
return
if self._fd is None:
return
import select
buf = b""
esc_pending_since = None
while not self._stop_event.is_set():
rlist, _, _ = select.select([self._fd], [], [], 0.1)
if not rlist:
# If ESC was pressed alone, treat it as ESC after a short timeout.
if buf == b"\x1b" and esc_pending_since is not None:
if (time.perf_counter() - esc_pending_since) > 0.05:
_handle_control_key("esc")
buf = b""
esc_pending_since = None
continue
try:
data = os.read(self._fd, 32)
except Exception:
continue
if not data:
continue
buf += data
# Parse known escape sequences: ESC, ESC [ C, ESC [ D
while buf:
if buf[0:1] != b"\x1b":
buf = buf[1:]
esc_pending_since = None
continue
if len(buf) == 1:
if esc_pending_since is None:
esc_pending_since = time.perf_counter()
break
if buf[1:2] != b"[":
_handle_control_key("esc")
buf = buf[1:]
esc_pending_since = None
continue
if len(buf) < 3:
break
code = buf[2:3]
if code == b"C":
_handle_control_key("right")
buf = buf[3:]
esc_pending_since = None
elif code == b"D":
_handle_control_key("left")
buf = buf[3:]
esc_pending_since = None
else:
# Unknown ESC sequence; drop the ESC and keep scanning.
buf = buf[1:]
esc_pending_since = None
self._restore_terminal()
listener = _TerminalKeyboardListener()
listener.start()
return listener, events return listener, events
# Only import pynput if not in a headless environment # Fallback to pynput global key listener (e.g. when stdin is not a TTY).
if is_headless():
logging.warning(
"Headless environment detected. On-screen cameras display may not be available, and stdin is not a TTY."
)
return None, events
from pynput import keyboard from pynput import keyboard
def on_press(key): def on_press(key):
try: try:
if key == keyboard.Key.right: if key == keyboard.Key.right:
print("Right arrow key pressed. Exiting loop...") _handle_control_key("right")
events["exit_early"] = True
elif key == keyboard.Key.left: elif key == keyboard.Key.left:
print("Left arrow key pressed. Exiting loop and rerecord the last episode...") _handle_control_key("left")
events["rerecord_episode"] = True
events["exit_early"] = True
elif key == keyboard.Key.esc: elif key == keyboard.Key.esc:
print("Escape key pressed. Stopping data recording...") _handle_control_key("esc")
events["stop_recording"] = True
events["exit_early"] = True
except Exception as e: except Exception as e:
print(f"Error handling key press: {e}") print(f"Error handling key press: {e}")
listener = keyboard.Listener(on_press=on_press) listener = keyboard.Listener(on_press=on_press)
listener.start() listener.start()
return listener, events return listener, events

查看文件

@ -184,7 +184,15 @@ def capture_timestamp_utc():
return datetime.now(timezone.utc) return datetime.now(timezone.utc)
_TTS_DISABLED = False
def say(text: str, blocking: bool = False): def say(text: str, blocking: bool = False):
global _TTS_DISABLED
if _TTS_DISABLED:
return
system = platform.system() system = platform.system()
if system == "Darwin": if system == "Darwin":
@ -206,10 +214,24 @@ def say(text: str, blocking: bool = False):
else: else:
raise RuntimeError("Unsupported operating system for text-to-speech.") raise RuntimeError("Unsupported operating system for text-to-speech.")
if blocking: # Text-to-speech is a convenience feature; it should never crash the main process.
subprocess.run(cmd, check=True) # On some Linux setups, `spd-say` can fail if Speech Dispatcher isn't available.
else: try:
subprocess.Popen(cmd, creationflags=subprocess.CREATE_NO_WINDOW if system == "Windows" else 0) if blocking:
subprocess.run(cmd, check=True, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
else:
subprocess.Popen(
cmd,
stdout=subprocess.DEVNULL,
stderr=subprocess.DEVNULL,
creationflags=subprocess.CREATE_NO_WINDOW if system == "Windows" else 0,
)
except (FileNotFoundError, subprocess.CalledProcessError) as e:
_TTS_DISABLED = True
logging.warning(f"Text-to-speech disabled ({cmd[0]}): {e}")
except Exception as e:
_TTS_DISABLED = True
logging.warning(f"Text-to-speech disabled: {e}")
def log_say(text: str, play_sounds: bool = True, blocking: bool = False): def log_say(text: str, play_sounds: bool = True, blocking: bool = False):