feat(piper-teleop): optimize lightweight teleoperation

- add send-only follower support and skip unnecessary observation fetches

- add bi-piper process isolation and fresh feedback handling for manual control

- split piper and piperx gravity compensation defaults and expand teleop tests
这个提交包含在:
Elvin
2026-03-11 10:03:51 +08:00
父节点 dfd78e12ae
当前提交 fa20ecaf8d
共有 10 个文件被更改,包括 634 次插入 和 32 次删除

查看文件

@ -118,6 +118,10 @@ class BiPiperFollower(Robot):
self.left_arm.connect(calibrate)
self.right_arm.connect(calibrate)
def set_teleop_send_only_mode(self, enabled: bool) -> None:
self.left_arm.set_teleop_send_only_mode(enabled)
self.right_arm.set_teleop_send_only_mode(enabled)
@property
def is_calibrated(self) -> bool:
return self.left_arm.is_calibrated and self.right_arm.is_calibrated

查看文件

@ -54,6 +54,7 @@ class PiperFollower(Robot):
self.config = config
self._is_connected = False
self._last_mode_refresh_t = 0.0
self._teleop_send_only_mode = False
interface_cls, _ = get_piper_sdk()
self.arm = interface_cls(
@ -70,24 +71,36 @@ class PiperFollower(Robot):
cam: (self.config.cameras[cam].height, self.config.cameras[cam].width, 3) for cam in self.cameras
}
@property
def _motors_ft(self) -> dict[str, type]:
return dict.fromkeys(PIPER_ACTION_KEYS, float)
@cached_property
def observation_features(self) -> dict[str, type | tuple]:
return {**dict.fromkeys(PIPER_ACTION_KEYS, float), **self._cameras_ft}
return {**self._motors_ft, **self._cameras_ft}
@cached_property
def action_features(self) -> dict[str, type]:
return dict.fromkeys(PIPER_ACTION_KEYS, float)
return self._motors_ft
@property
def is_connected(self) -> bool:
return self._is_connected and all(cam.is_connected for cam in self.cameras.values())
return self._is_connected and (
self._teleop_send_only_mode or all(cam.is_connected for cam in self.cameras.values())
)
def set_teleop_send_only_mode(self, enabled: bool) -> None:
if self._is_connected:
raise RuntimeError("teleop send-only mode must be configured before connecting the robot.")
self._teleop_send_only_mode = enabled
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None:
self.arm.ConnectPort()
self.arm.ConnectPort(start_thread=not self._teleop_send_only_mode)
if self.config.startup_sleep_s > 0:
time.sleep(self.config.startup_sleep_s)
guard_piper_ctrl_mode_on_connect(arm=self.arm, interface_name=self.config.port)
if not self._teleop_send_only_mode:
guard_piper_ctrl_mode_on_connect(arm=self.arm, interface_name=self.config.port)
self._is_connected = True
connected_cameras = []
@ -103,12 +116,16 @@ class PiperFollower(Robot):
# Enable behavior should be controlled by enable_on_connect, independent from calibrate flag.
# This keeps connect(calibrate=False) commandable for callers that only want to skip interactive calibration.
should_enable = self.config.enable_on_connect
if should_enable and not self._wait_enable(self.config.enable_timeout_s):
logger.warning("Piper follower did not report enabled state before timeout.")
if should_enable:
if self._teleop_send_only_mode:
self.arm.EnablePiper()
elif not self._wait_enable(self.config.enable_timeout_s):
logger.warning("Piper follower did not report enabled state before timeout.")
for cam in self.cameras.values():
cam.connect()
connected_cameras.append(cam)
if not self._teleop_send_only_mode:
for cam in self.cameras.values():
cam.connect()
connected_cameras.append(cam)
except Exception:
self.arm.DisconnectPort()
for cam in connected_cameras:
@ -239,6 +256,10 @@ class PiperFollower(Robot):
@check_if_not_connected
def get_observation(self) -> RobotObservation:
if self._teleop_send_only_mode:
raise RuntimeError(
f"{self} was connected in teleop send-only mode, so follower observations are unavailable."
)
obs: dict[str, float] = self._read_raw_observation()
for cam_key, cam in self.cameras.items():
@ -295,7 +316,8 @@ class PiperFollower(Robot):
finally:
self.arm.DisconnectPort()
for cam in self.cameras.values():
cam.disconnect()
if cam.is_connected:
cam.disconnect()
self._is_connected = False
logger.info("%s disconnected.", self)

查看文件

@ -178,6 +178,10 @@ class Robot(abc.ABC):
"""
pass
def set_teleop_send_only_mode(self, enabled: bool) -> None:
"""Optionally let robots skip observation-side work during pure teleoperation send loops."""
del enabled
@abc.abstractmethod
def get_observation(self) -> RobotObservation:
"""

查看文件

@ -52,6 +52,7 @@ lerobot-teleoperate \
"""
import logging
import sys
import time
from dataclasses import asdict, dataclass
from pprint import pformat
@ -62,6 +63,7 @@ from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig # no
from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401
from lerobot.configs import parser
from lerobot.processor import (
IdentityProcessorStep,
RobotAction,
RobotObservation,
RobotProcessorPipeline,
@ -109,6 +111,9 @@ from lerobot.utils.utils import init_logging, move_cursor_up
from lerobot.utils.visualization_utils import init_rerun, log_rerun_data
LOOP_STATUS_INTERVAL_S = 0.5
@dataclass
class TeleoperateConfig:
# TODO: pepijn, steven: if more robots require multiple teleoperators (like lekiwi) its good to make this possibele in teleop.py and record.py with List[Teleoperator]
@ -127,6 +132,33 @@ class TeleoperateConfig:
display_compressed_images: bool = False
def _processor_pipeline_needs_observation(
processor: RobotProcessorPipeline[tuple[RobotAction, RobotObservation], RobotAction],
) -> bool:
return not all(isinstance(step, IdentityProcessorStep) for step in processor.steps)
def _teleop_needs_robot_observation(
display_data: bool,
teleop_action_processor: RobotProcessorPipeline[tuple[RobotAction, RobotObservation], RobotAction],
robot_action_processor: RobotProcessorPipeline[tuple[RobotAction, RobotObservation], RobotAction],
) -> bool:
return (
display_data
or _processor_pipeline_needs_observation(teleop_action_processor)
or _processor_pipeline_needs_observation(robot_action_processor)
)
def _configure_robot_for_lightweight_teleop(
robot: Robot,
should_fetch_obs: bool,
) -> None:
if should_fetch_obs:
return
robot.set_teleop_send_only_mode(True)
def teleop_loop(
teleop: Teleoperator,
robot: Robot,
@ -157,6 +189,10 @@ def teleop_loop(
display_len = max(len(key) for key in robot.action_features)
start = time.perf_counter()
last_loop_status_t = 0.0
should_fetch_obs = _teleop_needs_robot_observation(
display_data, teleop_action_processor, robot_action_processor
)
while True:
loop_start = time.perf_counter()
@ -165,7 +201,7 @@ def teleop_loop(
# Not really needed for now other than for visualization
# teleop_action_processor can take None as an observation
# given that it is the identity processor as default
obs = robot.get_observation()
obs = robot.get_observation() if should_fetch_obs else {}
# Get teleop action
raw_action = teleop.get_action()
@ -191,16 +227,20 @@ def teleop_loop(
print("\n" + "-" * (display_len + 10))
print(f"{'NAME':<{display_len}} | {'NORM':>7}")
# Display the final robot action that was sent
for motor, value in robot_action_to_send.items():
print(f"{motor:<{display_len}} | {value:>7.2f}")
move_cursor_up(len(robot_action_to_send) + 3)
if sys.stdout.isatty():
move_cursor_up(len(robot_action_to_send) + 3)
dt_s = time.perf_counter() - loop_start
precise_sleep(max(1 / fps - dt_s, 0.0))
loop_s = time.perf_counter() - loop_start
print(f"Teleop loop time: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
move_cursor_up(1)
now = time.monotonic()
if now - last_loop_status_t >= LOOP_STATUS_INTERVAL_S:
print(f"Teleop loop time: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
if sys.stdout.isatty():
move_cursor_up(1)
last_loop_status_t = now
if duration is not None and time.perf_counter() - start >= duration:
return
@ -222,6 +262,10 @@ def teleoperate(cfg: TeleoperateConfig):
teleop = make_teleoperator_from_config(cfg.teleop)
robot = make_robot_from_config(cfg.robot)
teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
should_fetch_obs = _teleop_needs_robot_observation(
cfg.display_data, teleop_action_processor, robot_action_processor
)
_configure_robot_for_lightweight_teleop(robot, should_fetch_obs)
teleop.connect()
robot.connect()

查看文件

@ -15,6 +15,8 @@
# limitations under the License.
import logging
import multiprocessing as mp
import traceback
from functools import cached_property
from typing import Any
@ -25,6 +27,7 @@ from lerobot.teleoperators.piper_leader import (
PiperXLeader,
PiperXLeaderConfig,
)
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from ..teleoperator import Teleoperator
@ -33,6 +36,150 @@ from .config_bi_piper_leader import BiPiperLeaderConfig, BiPiperXLeaderConfig
logger = logging.getLogger(__name__)
def _bi_piper_leader_worker(conn, arm_cls, arm_config) -> None:
arm = arm_cls(arm_config)
try:
while True:
request = conn.recv()
command = request["command"]
if command == "__close__":
break
try:
if command == "__get_is_calibrated__":
result = arm.is_calibrated
else:
args = request.get("args", ())
kwargs = request.get("kwargs", {})
result = getattr(arm, command)(*args, **kwargs)
conn.send({"ok": True, "result": result})
except Exception as exc: # noqa: BLE001
conn.send(
{
"ok": False,
"error": repr(exc),
"traceback": traceback.format_exc(),
}
)
finally:
try:
if arm.is_connected:
arm.disconnect()
except Exception: # noqa: BLE001
pass
conn.close()
class _PiperLeaderProcessProxy:
def __init__(self, arm_cls, arm_config):
self._arm_cls = arm_cls
self._arm_config = arm_config
self._ctx = mp.get_context("spawn")
self._parent_conn = None
self._process = None
self._is_connected = False
@cached_property
def action_features(self) -> dict[str, type]:
return dict.fromkeys(PIPER_ACTION_KEYS, float)
@cached_property
def feedback_features(self) -> dict[str, type]:
return dict.fromkeys(PIPER_ACTION_KEYS, float)
@property
def is_connected(self) -> bool:
return self._is_connected
@property
def is_calibrated(self) -> bool:
if self._process is None:
return False
return bool(self._call("__get_is_calibrated__"))
def _ensure_process(self) -> None:
if self._process is not None and self._process.is_alive():
return
parent_conn, child_conn = self._ctx.Pipe()
process = self._ctx.Process(
target=_bi_piper_leader_worker,
args=(child_conn, self._arm_cls, self._arm_config),
daemon=True,
)
process.start()
child_conn.close()
self._parent_conn = parent_conn
self._process = process
def _call(self, command: str, *args, **kwargs):
self._ensure_process()
assert self._parent_conn is not None
self._parent_conn.send({"command": command, "args": args, "kwargs": kwargs})
response = self._parent_conn.recv()
if response["ok"]:
return response.get("result")
raise RuntimeError(
f"bi_piper leader worker command '{command}' failed: {response['error']}\n"
f"{response['traceback']}"
)
def connect(self, calibrate: bool = True) -> None:
try:
self._call("connect", calibrate)
self._is_connected = True
except Exception:
self.disconnect()
raise
def calibrate(self) -> None:
self._call("calibrate")
def configure(self) -> None:
self._call("configure")
def setup_motors(self) -> None:
self._call("setup_motors")
def set_manual_control(self, enabled: bool) -> None:
self._call("set_manual_control", enabled)
def get_action(self) -> RobotAction:
return self._call("get_action")
def send_feedback(self, feedback: dict[str, Any]) -> None:
self._call("send_feedback", feedback)
def disconnect(self) -> None:
if self._process is None:
self._is_connected = False
return
if self._parent_conn is not None:
try:
if self._is_connected:
self._call("disconnect")
except Exception:
pass
try:
self._parent_conn.send({"command": "__close__"})
except Exception:
pass
try:
self._parent_conn.close()
except Exception:
pass
if self._process.is_alive():
self._process.join(timeout=2.0)
if self._process.is_alive():
self._process.terminate()
self._process.join(timeout=1.0)
self._parent_conn = None
self._process = None
self._is_connected = False
class BiPiperLeader(Teleoperator):
"""Bimanual PiPER/PiPER-X leader arms."""
@ -74,6 +221,7 @@ class BiPiperLeader(Teleoperator):
def __init__(self, config: BiPiperLeaderConfig | BiPiperXLeaderConfig):
super().__init__(config)
self.config = config
self._use_process_isolation = config.process_isolation
if config.type == "bi_piperx_leader":
arm_config_cls = PiperXLeaderConfig
@ -85,8 +233,12 @@ class BiPiperLeader(Teleoperator):
left_arm_config = self._build_arm_config(arm_config_cls, config.left_arm_config, "left")
right_arm_config = self._build_arm_config(arm_config_cls, config.right_arm_config, "right")
self.left_arm = arm_cls(left_arm_config)
self.right_arm = arm_cls(right_arm_config)
if self._use_process_isolation:
self.left_arm = _PiperLeaderProcessProxy(arm_cls, left_arm_config)
self.right_arm = _PiperLeaderProcessProxy(arm_cls, right_arm_config)
else:
self.left_arm = arm_cls(left_arm_config)
self.right_arm = arm_cls(right_arm_config)
@cached_property
def action_features(self) -> dict[str, type]:

查看文件

@ -16,7 +16,7 @@
from dataclasses import dataclass
from lerobot.teleoperators.piper_leader import PiperLeaderConfigBase
from lerobot.teleoperators.piper_leader import PiperLeaderConfigBase, PiperXLeaderConfigBase
from ..config import TeleoperatorConfig
@ -28,6 +28,7 @@ class BiPiperLeaderConfig(TeleoperatorConfig):
left_arm_config: PiperLeaderConfigBase
right_arm_config: PiperLeaderConfigBase
process_isolation: bool = True
@TeleoperatorConfig.register_subclass("bi_piperx_leader")
@ -35,5 +36,6 @@ class BiPiperLeaderConfig(TeleoperatorConfig):
class BiPiperXLeaderConfig(TeleoperatorConfig):
"""Configuration class for bimanual PiPER-X leader teleoperators."""
left_arm_config: PiperLeaderConfigBase
right_arm_config: PiperLeaderConfigBase
left_arm_config: PiperXLeaderConfigBase
right_arm_config: PiperXLeaderConfigBase
process_isolation: bool = True

查看文件

@ -14,5 +14,10 @@
# See the License for the specific language governing permissions and
# limitations under the License.
from .config_piper_leader import PiperLeaderConfig, PiperLeaderConfigBase, PiperXLeaderConfig
from .config_piper_leader import (
PiperLeaderConfig,
PiperLeaderConfigBase,
PiperXLeaderConfig,
PiperXLeaderConfigBase,
)
from .piper_leader import PiperLeader, PiperXLeader

查看文件

@ -18,6 +18,9 @@ from dataclasses import dataclass
from ..config import TeleoperatorConfig
DEFAULT_PIPER_GRAVITY_COMP_TX_RATIO = (0.2, 0.2, 0.2, 0.2, 0.2, 0.2)
DEFAULT_PIPERX_GRAVITY_COMP_TX_RATIO = (1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
@dataclass
class PiperLeaderConfigBase:
@ -52,7 +55,7 @@ class PiperLeaderConfigBase:
# Gravity compensation settings (used when manual_control=true)
gravity_comp_control_hz: float = 200.0
gravity_comp_tx_ratio: tuple[float, float, float, float, float, float] = (1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
gravity_comp_tx_ratio: tuple[float, float, float, float, float, float] = DEFAULT_PIPER_GRAVITY_COMP_TX_RATIO
gravity_comp_torque_limit: float = 8.0
gravity_comp_mit_kp: float = 0.0
gravity_comp_mit_kd: float = 0.0
@ -102,8 +105,13 @@ class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase):
_validate_piper_leader_config(self)
@dataclass
class PiperXLeaderConfigBase(PiperLeaderConfigBase):
gravity_comp_tx_ratio: tuple[float, float, float, float, float, float] = DEFAULT_PIPERX_GRAVITY_COMP_TX_RATIO
@TeleoperatorConfig.register_subclass("piperx_leader")
@dataclass
class PiperXLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase):
class PiperXLeaderConfig(TeleoperatorConfig, PiperXLeaderConfigBase):
def __post_init__(self):
_validate_piper_leader_config(self)

查看文件

@ -46,6 +46,8 @@ PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)}
DEFAULT_PIPER_GRAVITY_URDF = "assets/piper_description/urdf/piper_no_gripper_description.urdf"
DEFAULT_PIPERX_GRAVITY_URDF = "assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf"
GIT_LFS_POINTER_PREFIX = b"version https://git-lfs.github.com/spec/v1"
MANUAL_CONTROL_FRESH_FEEDBACK_TIMEOUT_S = 0.03
MANUAL_CONTROL_FRESH_FEEDBACK_POLL_S = 0.001
def _ensure_not_lfs_pointer(path_obj: Any, relpath: str) -> None:
@ -76,6 +78,8 @@ class PiperLeader(Teleoperator):
self._manual_control_enabled: bool | None = None
self._last_mode_refresh_t = 0.0
self._gravity_comp_loop: PiperGravityCompensationLoop | None = None
self._last_feedback_joint_timestamp = 0.0
self._last_feedback_gripper_timestamp = 0.0
interface_cls, _ = get_piper_sdk()
self.arm = interface_cls(
@ -107,6 +111,8 @@ class PiperLeader(Teleoperator):
self._is_connected = True
# Recompute control mode on every fresh connection.
self._manual_control_enabled = None
self._last_feedback_joint_timestamp = 0.0
self._last_feedback_gripper_timestamp = 0.0
try:
self.configure()
if not self.is_calibrated and calibrate and self.config.require_calibration:
@ -292,23 +298,63 @@ class PiperLeader(Teleoperator):
return None
return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
def _wait_for_fresh_feedback_if_needed(self) -> None:
if self._manual_control_enabled is not True:
return
needs_fresh_gripper = self.config.sync_gripper and self.config.fallback_to_feedback
deadline = time.monotonic() + MANUAL_CONTROL_FRESH_FEEDBACK_TIMEOUT_S
while time.monotonic() < deadline:
joint_ts = float(getattr(self.arm.GetArmJointMsgs(), "time_stamp", 0.0) or 0.0)
has_fresh_joint = joint_ts > self._last_feedback_joint_timestamp
if not has_fresh_joint:
time.sleep(MANUAL_CONTROL_FRESH_FEEDBACK_POLL_S)
continue
if not needs_fresh_gripper:
return
gripper_ts = float(getattr(self.arm.GetArmGripperMsgs(), "time_stamp", 0.0) or 0.0)
has_fresh_gripper = gripper_ts > self._last_feedback_gripper_timestamp
if has_fresh_gripper:
return
time.sleep(MANUAL_CONTROL_FRESH_FEEDBACK_POLL_S)
def _read_raw_action(self) -> RobotAction:
used_feedback_for_joints = False
action: dict[str, float] | None = None
if self.config.prefer_ctrl_messages:
# In manual-control teleop, the operator physically backdrives the leader arm while a
# background gravity-compensation thread emits MIT commands. In that mode, `GetArmJointCtrl`
# reflects transmitted control targets rather than the operator's live pose, which can lag
# or jump under dual-arm load. Prefer direct joint feedback so teleop follows the real arm.
prefer_feedback = self._manual_control_enabled is True
if prefer_feedback:
self._wait_for_fresh_feedback_if_needed()
action = self._read_joint_from_feedback()
used_feedback_for_joints = action is not None
self._last_feedback_joint_timestamp = float(
getattr(self.arm.GetArmJointMsgs(), "time_stamp", 0.0) or 0.0
)
if action is None and self.config.prefer_ctrl_messages:
action = self._read_joint_from_ctrl()
if action is None and self.config.fallback_to_feedback:
if action is None and self.config.fallback_to_feedback and not used_feedback_for_joints:
action = self._read_joint_from_feedback()
used_feedback_for_joints = action is not None
if action is None:
action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES}
use_ctrl_for_gripper = self.config.prefer_ctrl_messages and not used_feedback_for_joints
use_ctrl_for_gripper = (
self.config.prefer_ctrl_messages and not prefer_feedback and not used_feedback_for_joints
)
gripper_pos = self._read_gripper_from_ctrl() if use_ctrl_for_gripper else None
if gripper_pos is None and self.config.fallback_to_feedback:
gripper_pos = self._read_gripper_from_feedback()
self._last_feedback_gripper_timestamp = float(
getattr(self.arm.GetArmGripperMsgs(), "time_stamp", 0.0) or 0.0
)
action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos
return action