略
一些检查失败了
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:
# 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).
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.
# Note: this address is not in the official STS3215 Memory Table
self.write("Maximum_Acceleration", motor, 254)
self.write("Acceleration", motor, 254)
self.write("Maximum_Acceleration", motor, 254, num_retry=2)
self.write("Acceleration", motor, 254, num_retry=2)
@property
def is_calibrated(self) -> bool:
@ -273,9 +273,9 @@ class FeetechMotorsBus(MotorsBus):
def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None:
for motor, calibration in calibration_dict.items():
if self.protocol_version == 0:
self.write("Homing_Offset", motor, calibration.homing_offset)
self.write("Min_Position_Limit", motor, calibration.range_min)
self.write("Max_Position_Limit", motor, calibration.range_max)
self.write("Homing_Offset", motor, calibration.homing_offset, num_retry=2)
self.write("Min_Position_Limit", motor, calibration.range_min, num_retry=2)
self.write("Max_Position_Limit", motor, calibration.range_max, num_retry=2)
self.calibration = calibration_dict

查看文件

@ -329,7 +329,7 @@ class MotorsBus(abc.ABC):
else:
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):
return self.motors[motor].model
elif isinstance(motor, int):
@ -682,9 +682,12 @@ class MotorsBus(abc.ABC):
for motor in motors:
model = self._get_motor_model(motor)
max_res = self.model_resolution_table[model] - 1
self.write("Homing_Offset", motor, 0, normalize=False)
self.write("Min_Position_Limit", motor, 0, normalize=False)
self.write("Max_Position_Limit", motor, max_res, normalize=False)
motor_name = motor if isinstance(motor, str) else self._id_to_name(motor)
# Calibration routines are often run on a live hardware bus that can occasionally drop packets.
# 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 = {}
@ -707,11 +710,22 @@ class MotorsBus(abc.ABC):
elif not isinstance(motors, list):
raise TypeError(motors)
self.reset_calibration(motors)
actual_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]
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)
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
@ -743,20 +757,39 @@ class MotorsBus(abc.ABC):
elif not isinstance(motors, list):
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()
maxes = start_positions.copy()
user_pressed_enter = False
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()}
maxes = {motor: max(positions[motor], max_) for motor, max_ in maxes.items()}
if display_values:
print("\n-------------------------------------------")
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}")
if enter_pressed():
@ -764,9 +797,9 @@ class MotorsBus(abc.ABC):
if display_values and not user_pressed_enter:
# 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:
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:
teleop.disconnect()
if not is_headless() and listener is not None:
if listener is not None and hasattr(listener, "stop"):
listener.stop()
if cfg.dataset.push_to_hub:
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)
return dataset

查看文件

@ -176,7 +176,33 @@ class KochFollower(Robot):
# Read arm position
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()}
dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read state: {dt_ms:.1f}ms")

查看文件

@ -160,7 +160,33 @@ class SO100Follower(Robot):
# Read arm position
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()}
dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read state: {dt_ms:.1f}ms")

查看文件

@ -181,7 +181,33 @@ class SO100FollowerEndEffector(SO100Follower):
# Read arm position
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()}
dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read state: {dt_ms:.1f}ms")

查看文件

@ -156,7 +156,35 @@ class SO101Follower(Robot):
# Read arm position
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()}
dt_ms = (time.perf_counter() - start) * 1e3
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:
logger.info(f"\nRunning calibration of {self}")
self.bus.disable_torque()
self.bus.disable_torque(num_retry=5)
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....")
homing_offsets = self.bus.set_half_turn_homings()
@ -115,10 +115,10 @@ class SO100Leader(Teleoperator):
print(f"Calibration saved to {self.calibration_fpath}")
def configure(self) -> None:
self.bus.disable_torque()
self.bus.disable_torque(num_retry=5)
self.bus.configure_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:
for motor in reversed(self.bus.motors):
@ -128,7 +128,33 @@ class SO100Leader(Teleoperator):
def get_action(self) -> dict[str, float]:
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()}
dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read action: {dt_ms:.1f}ms")

查看文件

@ -84,9 +84,9 @@ class SO101Leader(Teleoperator):
def calibrate(self) -> None:
logger.info(f"\nRunning calibration of {self}")
self.bus.disable_torque()
self.bus.disable_torque(num_retry=5)
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....")
homing_offsets = self.bus.set_half_turn_homings()
@ -112,10 +112,10 @@ class SO101Leader(Teleoperator):
print(f"Calibration saved to {self.calibration_fpath}")
def configure(self) -> None:
self.bus.disable_torque()
self.bus.disable_torque(num_retry=5)
self.bus.configure_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:
for motor in reversed(self.bus.motors):
@ -125,7 +125,35 @@ class SO101Leader(Teleoperator):
def get_action(self) -> dict[str, float]:
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()}
dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read action: {dt_ms:.1f}ms")

查看文件

@ -18,6 +18,10 @@
import logging
import os
import sys
import threading
import time
import traceback
from contextlib import nullcontext
from copy import copy
@ -144,35 +148,183 @@ def init_keyboard_listener():
events["rerecord_episode"] = False
events["stop_recording"] = False
if is_headless():
logging.warning(
"Headless environment detected. On-screen cameras display and keyboard inputs will not be available."
)
listener = None
def _handle_control_key(key_name: str):
if key_name == "right":
print("Right arrow key pressed. Exiting loop...")
events["exit_early"] = True
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
# 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
def on_press(key):
try:
if key == keyboard.Key.right:
print("Right arrow key pressed. Exiting loop...")
events["exit_early"] = True
_handle_control_key("right")
elif key == keyboard.Key.left:
print("Left arrow key pressed. Exiting loop and rerecord the last episode...")
events["rerecord_episode"] = True
events["exit_early"] = True
_handle_control_key("left")
elif key == keyboard.Key.esc:
print("Escape key pressed. Stopping data recording...")
events["stop_recording"] = True
events["exit_early"] = True
_handle_control_key("esc")
except Exception as e:
print(f"Error handling key press: {e}")
listener = keyboard.Listener(on_press=on_press)
listener.start()
return listener, events

查看文件

@ -184,7 +184,15 @@ def capture_timestamp_utc():
return datetime.now(timezone.utc)
_TTS_DISABLED = False
def say(text: str, blocking: bool = False):
global _TTS_DISABLED
if _TTS_DISABLED:
return
system = platform.system()
if system == "Darwin":
@ -206,10 +214,24 @@ def say(text: str, blocking: bool = False):
else:
raise RuntimeError("Unsupported operating system for text-to-speech.")
# Text-to-speech is a convenience feature; it should never crash the main process.
# On some Linux setups, `spd-say` can fail if Speech Dispatcher isn't available.
try:
if blocking:
subprocess.run(cmd, check=True)
subprocess.run(cmd, check=True, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
else:
subprocess.Popen(cmd, creationflags=subprocess.CREATE_NO_WINDOW if system == "Windows" else 0)
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):