略
一些检查失败了
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
一些检查失败了
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
这个提交包含在:
1
WSRobotClient
子模块
1
WSRobotClient
子模块
子模块 WSRobotClient 已添加到 34f5276a1f
44
follower.json
普通文件
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
普通文件
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
普通文件
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
普通文件
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
普通文件
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:
|
||||
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)
|
||||
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.")
|
||||
|
||||
if blocking:
|
||||
subprocess.run(cmd, check=True)
|
||||
else:
|
||||
subprocess.Popen(cmd, creationflags=subprocess.CREATE_NO_WINDOW if system == "Windows" else 0)
|
||||
# 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, 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):
|
||||
|
||||
在新工单中引用
屏蔽一个用户