镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
@ -118,6 +118,10 @@ class BiPiperFollower(Robot):
|
|||||||
self.left_arm.connect(calibrate)
|
self.left_arm.connect(calibrate)
|
||||||
self.right_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
|
@property
|
||||||
def is_calibrated(self) -> bool:
|
def is_calibrated(self) -> bool:
|
||||||
return self.left_arm.is_calibrated and self.right_arm.is_calibrated
|
return self.left_arm.is_calibrated and self.right_arm.is_calibrated
|
||||||
|
|||||||
@ -54,6 +54,7 @@ class PiperFollower(Robot):
|
|||||||
self.config = config
|
self.config = config
|
||||||
self._is_connected = False
|
self._is_connected = False
|
||||||
self._last_mode_refresh_t = 0.0
|
self._last_mode_refresh_t = 0.0
|
||||||
|
self._teleop_send_only_mode = False
|
||||||
|
|
||||||
interface_cls, _ = get_piper_sdk()
|
interface_cls, _ = get_piper_sdk()
|
||||||
self.arm = interface_cls(
|
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
|
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
|
@cached_property
|
||||||
def observation_features(self) -> dict[str, type | tuple]:
|
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
|
@cached_property
|
||||||
def action_features(self) -> dict[str, type]:
|
def action_features(self) -> dict[str, type]:
|
||||||
return dict.fromkeys(PIPER_ACTION_KEYS, float)
|
return self._motors_ft
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def is_connected(self) -> bool:
|
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
|
@check_if_already_connected
|
||||||
def connect(self, calibrate: bool = True) -> None:
|
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:
|
if self.config.startup_sleep_s > 0:
|
||||||
time.sleep(self.config.startup_sleep_s)
|
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
|
self._is_connected = True
|
||||||
connected_cameras = []
|
connected_cameras = []
|
||||||
@ -103,12 +116,16 @@ class PiperFollower(Robot):
|
|||||||
# Enable behavior should be controlled by enable_on_connect, independent from calibrate flag.
|
# 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.
|
# This keeps connect(calibrate=False) commandable for callers that only want to skip interactive calibration.
|
||||||
should_enable = self.config.enable_on_connect
|
should_enable = self.config.enable_on_connect
|
||||||
if should_enable and not self._wait_enable(self.config.enable_timeout_s):
|
if should_enable:
|
||||||
logger.warning("Piper follower did not report enabled state before timeout.")
|
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():
|
if not self._teleop_send_only_mode:
|
||||||
cam.connect()
|
for cam in self.cameras.values():
|
||||||
connected_cameras.append(cam)
|
cam.connect()
|
||||||
|
connected_cameras.append(cam)
|
||||||
except Exception:
|
except Exception:
|
||||||
self.arm.DisconnectPort()
|
self.arm.DisconnectPort()
|
||||||
for cam in connected_cameras:
|
for cam in connected_cameras:
|
||||||
@ -239,6 +256,10 @@ class PiperFollower(Robot):
|
|||||||
|
|
||||||
@check_if_not_connected
|
@check_if_not_connected
|
||||||
def get_observation(self) -> RobotObservation:
|
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()
|
obs: dict[str, float] = self._read_raw_observation()
|
||||||
|
|
||||||
for cam_key, cam in self.cameras.items():
|
for cam_key, cam in self.cameras.items():
|
||||||
@ -295,7 +316,8 @@ class PiperFollower(Robot):
|
|||||||
finally:
|
finally:
|
||||||
self.arm.DisconnectPort()
|
self.arm.DisconnectPort()
|
||||||
for cam in self.cameras.values():
|
for cam in self.cameras.values():
|
||||||
cam.disconnect()
|
if cam.is_connected:
|
||||||
|
cam.disconnect()
|
||||||
self._is_connected = False
|
self._is_connected = False
|
||||||
logger.info("%s disconnected.", self)
|
logger.info("%s disconnected.", self)
|
||||||
|
|
||||||
|
|||||||
@ -178,6 +178,10 @@ class Robot(abc.ABC):
|
|||||||
"""
|
"""
|
||||||
pass
|
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
|
@abc.abstractmethod
|
||||||
def get_observation(self) -> RobotObservation:
|
def get_observation(self) -> RobotObservation:
|
||||||
"""
|
"""
|
||||||
|
|||||||
@ -52,6 +52,7 @@ lerobot-teleoperate \
|
|||||||
"""
|
"""
|
||||||
|
|
||||||
import logging
|
import logging
|
||||||
|
import sys
|
||||||
import time
|
import time
|
||||||
from dataclasses import asdict, dataclass
|
from dataclasses import asdict, dataclass
|
||||||
from pprint import pformat
|
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.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401
|
||||||
from lerobot.configs import parser
|
from lerobot.configs import parser
|
||||||
from lerobot.processor import (
|
from lerobot.processor import (
|
||||||
|
IdentityProcessorStep,
|
||||||
RobotAction,
|
RobotAction,
|
||||||
RobotObservation,
|
RobotObservation,
|
||||||
RobotProcessorPipeline,
|
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
|
from lerobot.utils.visualization_utils import init_rerun, log_rerun_data
|
||||||
|
|
||||||
|
|
||||||
|
LOOP_STATUS_INTERVAL_S = 0.5
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class TeleoperateConfig:
|
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]
|
# 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
|
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(
|
def teleop_loop(
|
||||||
teleop: Teleoperator,
|
teleop: Teleoperator,
|
||||||
robot: Robot,
|
robot: Robot,
|
||||||
@ -157,6 +189,10 @@ def teleop_loop(
|
|||||||
|
|
||||||
display_len = max(len(key) for key in robot.action_features)
|
display_len = max(len(key) for key in robot.action_features)
|
||||||
start = time.perf_counter()
|
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:
|
while True:
|
||||||
loop_start = time.perf_counter()
|
loop_start = time.perf_counter()
|
||||||
@ -165,7 +201,7 @@ def teleop_loop(
|
|||||||
# Not really needed for now other than for visualization
|
# Not really needed for now other than for visualization
|
||||||
# teleop_action_processor can take None as an observation
|
# teleop_action_processor can take None as an observation
|
||||||
# given that it is the identity processor as default
|
# 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
|
# Get teleop action
|
||||||
raw_action = teleop.get_action()
|
raw_action = teleop.get_action()
|
||||||
@ -191,16 +227,20 @@ def teleop_loop(
|
|||||||
|
|
||||||
print("\n" + "-" * (display_len + 10))
|
print("\n" + "-" * (display_len + 10))
|
||||||
print(f"{'NAME':<{display_len}} | {'NORM':>7}")
|
print(f"{'NAME':<{display_len}} | {'NORM':>7}")
|
||||||
# Display the final robot action that was sent
|
|
||||||
for motor, value in robot_action_to_send.items():
|
for motor, value in robot_action_to_send.items():
|
||||||
print(f"{motor:<{display_len}} | {value:>7.2f}")
|
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
|
dt_s = time.perf_counter() - loop_start
|
||||||
precise_sleep(max(1 / fps - dt_s, 0.0))
|
precise_sleep(max(1 / fps - dt_s, 0.0))
|
||||||
loop_s = time.perf_counter() - loop_start
|
loop_s = time.perf_counter() - loop_start
|
||||||
print(f"Teleop loop time: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
|
now = time.monotonic()
|
||||||
move_cursor_up(1)
|
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:
|
if duration is not None and time.perf_counter() - start >= duration:
|
||||||
return
|
return
|
||||||
@ -222,6 +262,10 @@ def teleoperate(cfg: TeleoperateConfig):
|
|||||||
teleop = make_teleoperator_from_config(cfg.teleop)
|
teleop = make_teleoperator_from_config(cfg.teleop)
|
||||||
robot = make_robot_from_config(cfg.robot)
|
robot = make_robot_from_config(cfg.robot)
|
||||||
teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
|
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()
|
teleop.connect()
|
||||||
robot.connect()
|
robot.connect()
|
||||||
|
|||||||
@ -15,6 +15,8 @@
|
|||||||
# limitations under the License.
|
# limitations under the License.
|
||||||
|
|
||||||
import logging
|
import logging
|
||||||
|
import multiprocessing as mp
|
||||||
|
import traceback
|
||||||
from functools import cached_property
|
from functools import cached_property
|
||||||
from typing import Any
|
from typing import Any
|
||||||
|
|
||||||
@ -25,6 +27,7 @@ from lerobot.teleoperators.piper_leader import (
|
|||||||
PiperXLeader,
|
PiperXLeader,
|
||||||
PiperXLeaderConfig,
|
PiperXLeaderConfig,
|
||||||
)
|
)
|
||||||
|
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
|
||||||
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
||||||
|
|
||||||
from ..teleoperator import Teleoperator
|
from ..teleoperator import Teleoperator
|
||||||
@ -33,6 +36,150 @@ from .config_bi_piper_leader import BiPiperLeaderConfig, BiPiperXLeaderConfig
|
|||||||
logger = logging.getLogger(__name__)
|
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):
|
class BiPiperLeader(Teleoperator):
|
||||||
"""Bimanual PiPER/PiPER-X leader arms."""
|
"""Bimanual PiPER/PiPER-X leader arms."""
|
||||||
|
|
||||||
@ -74,6 +221,7 @@ class BiPiperLeader(Teleoperator):
|
|||||||
def __init__(self, config: BiPiperLeaderConfig | BiPiperXLeaderConfig):
|
def __init__(self, config: BiPiperLeaderConfig | BiPiperXLeaderConfig):
|
||||||
super().__init__(config)
|
super().__init__(config)
|
||||||
self.config = config
|
self.config = config
|
||||||
|
self._use_process_isolation = config.process_isolation
|
||||||
|
|
||||||
if config.type == "bi_piperx_leader":
|
if config.type == "bi_piperx_leader":
|
||||||
arm_config_cls = PiperXLeaderConfig
|
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")
|
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")
|
right_arm_config = self._build_arm_config(arm_config_cls, config.right_arm_config, "right")
|
||||||
|
|
||||||
self.left_arm = arm_cls(left_arm_config)
|
if self._use_process_isolation:
|
||||||
self.right_arm = arm_cls(right_arm_config)
|
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
|
@cached_property
|
||||||
def action_features(self) -> dict[str, type]:
|
def action_features(self) -> dict[str, type]:
|
||||||
|
|||||||
@ -16,7 +16,7 @@
|
|||||||
|
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
|
|
||||||
from lerobot.teleoperators.piper_leader import PiperLeaderConfigBase
|
from lerobot.teleoperators.piper_leader import PiperLeaderConfigBase, PiperXLeaderConfigBase
|
||||||
|
|
||||||
from ..config import TeleoperatorConfig
|
from ..config import TeleoperatorConfig
|
||||||
|
|
||||||
@ -28,6 +28,7 @@ class BiPiperLeaderConfig(TeleoperatorConfig):
|
|||||||
|
|
||||||
left_arm_config: PiperLeaderConfigBase
|
left_arm_config: PiperLeaderConfigBase
|
||||||
right_arm_config: PiperLeaderConfigBase
|
right_arm_config: PiperLeaderConfigBase
|
||||||
|
process_isolation: bool = True
|
||||||
|
|
||||||
|
|
||||||
@TeleoperatorConfig.register_subclass("bi_piperx_leader")
|
@TeleoperatorConfig.register_subclass("bi_piperx_leader")
|
||||||
@ -35,5 +36,6 @@ class BiPiperLeaderConfig(TeleoperatorConfig):
|
|||||||
class BiPiperXLeaderConfig(TeleoperatorConfig):
|
class BiPiperXLeaderConfig(TeleoperatorConfig):
|
||||||
"""Configuration class for bimanual PiPER-X leader teleoperators."""
|
"""Configuration class for bimanual PiPER-X leader teleoperators."""
|
||||||
|
|
||||||
left_arm_config: PiperLeaderConfigBase
|
left_arm_config: PiperXLeaderConfigBase
|
||||||
right_arm_config: PiperLeaderConfigBase
|
right_arm_config: PiperXLeaderConfigBase
|
||||||
|
process_isolation: bool = True
|
||||||
|
|||||||
@ -14,5 +14,10 @@
|
|||||||
# See the License for the specific language governing permissions and
|
# See the License for the specific language governing permissions and
|
||||||
# limitations under the License.
|
# 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
|
from .piper_leader import PiperLeader, PiperXLeader
|
||||||
|
|||||||
@ -18,6 +18,9 @@ from dataclasses import dataclass
|
|||||||
|
|
||||||
from ..config import TeleoperatorConfig
|
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
|
@dataclass
|
||||||
class PiperLeaderConfigBase:
|
class PiperLeaderConfigBase:
|
||||||
@ -52,7 +55,7 @@ class PiperLeaderConfigBase:
|
|||||||
|
|
||||||
# Gravity compensation settings (used when manual_control=true)
|
# Gravity compensation settings (used when manual_control=true)
|
||||||
gravity_comp_control_hz: float = 200.0
|
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_torque_limit: float = 8.0
|
||||||
gravity_comp_mit_kp: float = 0.0
|
gravity_comp_mit_kp: float = 0.0
|
||||||
gravity_comp_mit_kd: float = 0.0
|
gravity_comp_mit_kd: float = 0.0
|
||||||
@ -102,8 +105,13 @@ class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase):
|
|||||||
_validate_piper_leader_config(self)
|
_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")
|
@TeleoperatorConfig.register_subclass("piperx_leader")
|
||||||
@dataclass
|
@dataclass
|
||||||
class PiperXLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase):
|
class PiperXLeaderConfig(TeleoperatorConfig, PiperXLeaderConfigBase):
|
||||||
def __post_init__(self):
|
def __post_init__(self):
|
||||||
_validate_piper_leader_config(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_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"
|
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"
|
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:
|
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._manual_control_enabled: bool | None = None
|
||||||
self._last_mode_refresh_t = 0.0
|
self._last_mode_refresh_t = 0.0
|
||||||
self._gravity_comp_loop: PiperGravityCompensationLoop | None = None
|
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()
|
interface_cls, _ = get_piper_sdk()
|
||||||
self.arm = interface_cls(
|
self.arm = interface_cls(
|
||||||
@ -107,6 +111,8 @@ class PiperLeader(Teleoperator):
|
|||||||
self._is_connected = True
|
self._is_connected = True
|
||||||
# Recompute control mode on every fresh connection.
|
# Recompute control mode on every fresh connection.
|
||||||
self._manual_control_enabled = None
|
self._manual_control_enabled = None
|
||||||
|
self._last_feedback_joint_timestamp = 0.0
|
||||||
|
self._last_feedback_gripper_timestamp = 0.0
|
||||||
try:
|
try:
|
||||||
self.configure()
|
self.configure()
|
||||||
if not self.is_calibrated and calibrate and self.config.require_calibration:
|
if not self.is_calibrated and calibrate and self.config.require_calibration:
|
||||||
@ -292,23 +298,63 @@ class PiperLeader(Teleoperator):
|
|||||||
return None
|
return None
|
||||||
return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
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:
|
def _read_raw_action(self) -> RobotAction:
|
||||||
used_feedback_for_joints = False
|
used_feedback_for_joints = False
|
||||||
action: dict[str, float] | None = None
|
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()
|
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()
|
action = self._read_joint_from_feedback()
|
||||||
used_feedback_for_joints = action is not None
|
used_feedback_for_joints = action is not None
|
||||||
|
|
||||||
if action is None:
|
if action is None:
|
||||||
action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES}
|
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
|
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:
|
if gripper_pos is None and self.config.fallback_to_feedback:
|
||||||
gripper_pos = self._read_gripper_from_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
|
action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos
|
||||||
return action
|
return action
|
||||||
|
|
||||||
|
|||||||
@ -1,14 +1,17 @@
|
|||||||
# ruff: noqa: N802
|
# ruff: noqa: N802
|
||||||
import sys
|
import sys
|
||||||
|
import threading
|
||||||
import time
|
import time
|
||||||
from types import SimpleNamespace
|
from types import SimpleNamespace
|
||||||
|
|
||||||
import pytest
|
import pytest
|
||||||
|
|
||||||
import lerobot.robots.piper_follower.piper_follower as piper_follower_module
|
import lerobot.robots.piper_follower.piper_follower as piper_follower_module
|
||||||
|
import lerobot.teleoperators.bi_piper_leader.bi_piper_leader as bi_piper_leader_module
|
||||||
import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module
|
import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module
|
||||||
import lerobot.utils.piper_sdk as piper_sdk_utils
|
import lerobot.utils.piper_sdk as piper_sdk_utils
|
||||||
from lerobot.motors import MotorCalibration
|
from lerobot.motors import MotorCalibration
|
||||||
|
from lerobot.processor import make_default_processors
|
||||||
from lerobot.robots.bi_piper_follower import (
|
from lerobot.robots.bi_piper_follower import (
|
||||||
BiPiperFollower,
|
BiPiperFollower,
|
||||||
BiPiperFollowerConfig,
|
BiPiperFollowerConfig,
|
||||||
@ -35,7 +38,9 @@ from lerobot.teleoperators.piper_leader import (
|
|||||||
PiperLeaderConfigBase,
|
PiperLeaderConfigBase,
|
||||||
PiperXLeader,
|
PiperXLeader,
|
||||||
PiperXLeaderConfig,
|
PiperXLeaderConfig,
|
||||||
|
PiperXLeaderConfigBase,
|
||||||
)
|
)
|
||||||
|
from lerobot.scripts.lerobot_teleoperate import teleop_loop
|
||||||
from lerobot.teleoperators.utils import make_teleoperator_from_config
|
from lerobot.teleoperators.utils import make_teleoperator_from_config
|
||||||
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
|
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
|
||||||
|
|
||||||
@ -65,6 +70,7 @@ class FakePiperInterface:
|
|||||||
self.enable_calls = 0
|
self.enable_calls = 0
|
||||||
self.disable_calls = 0
|
self.disable_calls = 0
|
||||||
self.is_enabled = False
|
self.is_enabled = False
|
||||||
|
self.connect_calls = []
|
||||||
|
|
||||||
self._joint_ctrl = SimpleNamespace(
|
self._joint_ctrl = SimpleNamespace(
|
||||||
time_stamp=1.0,
|
time_stamp=1.0,
|
||||||
@ -116,7 +122,10 @@ class FakePiperInterface:
|
|||||||
arm_status=SimpleNamespace(ctrl_mode=0x01),
|
arm_status=SimpleNamespace(ctrl_mode=0x01),
|
||||||
)
|
)
|
||||||
|
|
||||||
def ConnectPort(self):
|
def ConnectPort(self, can_init=False, piper_init=True, start_thread=True):
|
||||||
|
self.connect_calls.append(
|
||||||
|
{"can_init": can_init, "piper_init": piper_init, "start_thread": start_thread}
|
||||||
|
)
|
||||||
self.connected = True
|
self.connected = True
|
||||||
|
|
||||||
def DisconnectPort(self, thread_timeout=0.1):
|
def DisconnectPort(self, thread_timeout=0.1):
|
||||||
@ -349,13 +358,124 @@ def test_piper_require_calibration_false_allows_uncalibrated_control(monkeypatch
|
|||||||
try:
|
try:
|
||||||
action = teleop.get_action()
|
action = teleop.get_action()
|
||||||
sent = robot.send_action(action)
|
sent = robot.send_action(action)
|
||||||
assert sent["joint_1.pos"] == 10.0
|
assert sent["joint_1.pos"] == 11.0
|
||||||
assert sent["gripper.pos"] == 42.0
|
assert sent["gripper.pos"] == 43.0
|
||||||
finally:
|
finally:
|
||||||
teleop.disconnect()
|
teleop.disconnect()
|
||||||
robot.disconnect()
|
robot.disconnect()
|
||||||
|
|
||||||
|
|
||||||
|
def test_piper_gravity_comp_defaults_are_model_specific():
|
||||||
|
assert PiperLeaderConfig(port="can1").gravity_comp_tx_ratio == (0.2, 0.2, 0.2, 0.2, 0.2, 0.2)
|
||||||
|
assert PiperXLeaderConfig(port="can1").gravity_comp_tx_ratio == (1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_piper_leader_manual_control_prefers_feedback_state(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
|
teleop = PiperLeader(
|
||||||
|
PiperLeaderConfig(
|
||||||
|
port="can1",
|
||||||
|
manual_control=True,
|
||||||
|
require_calibration=False,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
teleop.connect(calibrate=False)
|
||||||
|
try:
|
||||||
|
action = teleop.get_action()
|
||||||
|
assert action["joint_1.pos"] == 11.0
|
||||||
|
assert action["joint_2.pos"] == 21.0
|
||||||
|
assert action["gripper.pos"] == 43.0
|
||||||
|
finally:
|
||||||
|
teleop.disconnect()
|
||||||
|
|
||||||
|
|
||||||
|
def test_piper_leader_manual_control_waits_for_fresh_feedback(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
|
teleop = PiperLeader(
|
||||||
|
PiperLeaderConfig(
|
||||||
|
port="can1",
|
||||||
|
manual_control=True,
|
||||||
|
require_calibration=False,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
teleop.connect(calibrate=False)
|
||||||
|
try:
|
||||||
|
initial = teleop.get_action()
|
||||||
|
assert initial["joint_1.pos"] == 11.0
|
||||||
|
|
||||||
|
def publish_fresh_feedback():
|
||||||
|
time.sleep(0.01)
|
||||||
|
teleop.arm._joint_state.time_stamp = 2.0
|
||||||
|
teleop.arm._joint_state.joint_state.joint_1 = 12000
|
||||||
|
teleop.arm._joint_state.joint_state.joint_2 = 22000
|
||||||
|
teleop.arm._joint_state.joint_state.joint_3 = 32000
|
||||||
|
teleop.arm._joint_state.joint_state.joint_4 = 42000
|
||||||
|
teleop.arm._joint_state.joint_state.joint_5 = 52000
|
||||||
|
teleop.arm._joint_state.joint_state.joint_6 = 62000
|
||||||
|
teleop.arm._gripper_state.time_stamp = 2.0
|
||||||
|
teleop.arm._gripper_state.gripper_state.grippers_angle = 44000
|
||||||
|
|
||||||
|
publisher = threading.Thread(target=publish_fresh_feedback, daemon=True)
|
||||||
|
publisher.start()
|
||||||
|
|
||||||
|
t0 = time.monotonic()
|
||||||
|
action = teleop.get_action()
|
||||||
|
publisher.join(timeout=0.1)
|
||||||
|
|
||||||
|
assert time.monotonic() - t0 >= 0.008
|
||||||
|
assert action["joint_1.pos"] == 12.0
|
||||||
|
assert action["joint_2.pos"] == 22.0
|
||||||
|
assert action["gripper.pos"] == 44.0
|
||||||
|
finally:
|
||||||
|
teleop.disconnect()
|
||||||
|
|
||||||
|
|
||||||
|
def test_piper_leader_manual_control_does_not_unblock_on_gripper_feedback_only(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
|
teleop = PiperLeader(
|
||||||
|
PiperLeaderConfig(
|
||||||
|
port="can1",
|
||||||
|
manual_control=True,
|
||||||
|
require_calibration=False,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
teleop.connect(calibrate=False)
|
||||||
|
try:
|
||||||
|
initial = teleop.get_action()
|
||||||
|
assert initial["joint_1.pos"] == 11.0
|
||||||
|
|
||||||
|
def publish_feedback():
|
||||||
|
time.sleep(0.005)
|
||||||
|
teleop.arm._gripper_state.time_stamp = 2.0
|
||||||
|
teleop.arm._gripper_state.gripper_state.grippers_angle = 45000
|
||||||
|
time.sleep(0.01)
|
||||||
|
teleop.arm._joint_state.time_stamp = 2.0
|
||||||
|
teleop.arm._joint_state.joint_state.joint_1 = 13000
|
||||||
|
teleop.arm._joint_state.joint_state.joint_2 = 23000
|
||||||
|
teleop.arm._joint_state.joint_state.joint_3 = 33000
|
||||||
|
teleop.arm._joint_state.joint_state.joint_4 = 43000
|
||||||
|
teleop.arm._joint_state.joint_state.joint_5 = 53000
|
||||||
|
teleop.arm._joint_state.joint_state.joint_6 = 63000
|
||||||
|
|
||||||
|
publisher = threading.Thread(target=publish_feedback, daemon=True)
|
||||||
|
publisher.start()
|
||||||
|
|
||||||
|
t0 = time.monotonic()
|
||||||
|
action = teleop.get_action()
|
||||||
|
publisher.join(timeout=0.1)
|
||||||
|
|
||||||
|
assert time.monotonic() - t0 >= 0.013
|
||||||
|
assert action["joint_1.pos"] == 13.0
|
||||||
|
finally:
|
||||||
|
teleop.disconnect()
|
||||||
|
|
||||||
|
|
||||||
def test_piper_follower_connect_calibrates_then_reenables(monkeypatch, tmp_path):
|
def test_piper_follower_connect_calibrates_then_reenables(monkeypatch, tmp_path):
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
@ -509,6 +629,7 @@ def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path):
|
|||||||
BiPiperLeaderConfig(
|
BiPiperLeaderConfig(
|
||||||
left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
|
left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
|
||||||
right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True),
|
right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True),
|
||||||
|
process_isolation=False,
|
||||||
),
|
),
|
||||||
BiPiperFollowerConfig(
|
BiPiperFollowerConfig(
|
||||||
left_arm_config=PiperFollowerConfigBase(port="can0", sync_gripper=True),
|
left_arm_config=PiperFollowerConfigBase(port="can0", sync_gripper=True),
|
||||||
@ -523,8 +644,9 @@ def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path):
|
|||||||
),
|
),
|
||||||
(
|
(
|
||||||
BiPiperXLeaderConfig(
|
BiPiperXLeaderConfig(
|
||||||
left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
|
left_arm_config=PiperXLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
|
||||||
right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True),
|
right_arm_config=PiperXLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True),
|
||||||
|
process_isolation=False,
|
||||||
),
|
),
|
||||||
BiPiperXFollowerConfig(
|
BiPiperXFollowerConfig(
|
||||||
left_arm_config=PiperFollowerConfigBase(port="can0", sync_gripper=True),
|
left_arm_config=PiperFollowerConfigBase(port="can0", sync_gripper=True),
|
||||||
@ -599,3 +721,196 @@ def test_bimanual_piper_leader_follower_roundtrip(
|
|||||||
finally:
|
finally:
|
||||||
teleop.disconnect()
|
teleop.disconnect()
|
||||||
robot.disconnect()
|
robot.disconnect()
|
||||||
|
|
||||||
|
|
||||||
|
def test_bimanual_piper_follower_action_features_are_available_without_connect(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
|
robot = make_robot_from_config(
|
||||||
|
BiPiperFollowerConfig(
|
||||||
|
left_arm_config=PiperFollowerConfigBase(port="can3", sync_gripper=True),
|
||||||
|
right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
action_features = robot.action_features
|
||||||
|
|
||||||
|
assert action_features["left_joint_1.pos"] is float
|
||||||
|
assert action_features["left_gripper.pos"] is float
|
||||||
|
assert action_features["right_joint_1.pos"] is float
|
||||||
|
assert action_features["right_gripper.pos"] is float
|
||||||
|
|
||||||
|
|
||||||
|
def test_bimanual_piper_teleop_loop_smoke(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
|
teleop = make_teleoperator_from_config(
|
||||||
|
BiPiperLeaderConfig(
|
||||||
|
left_arm_config=PiperLeaderConfigBase(port="can0", manual_control=False, sync_gripper=True),
|
||||||
|
right_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
|
||||||
|
process_isolation=False,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
robot = make_robot_from_config(
|
||||||
|
BiPiperFollowerConfig(
|
||||||
|
left_arm_config=PiperFollowerConfigBase(port="can3", sync_gripper=True),
|
||||||
|
right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
teleop.left_arm.calibration = make_identity_calibration()
|
||||||
|
teleop.right_arm.calibration = make_identity_calibration()
|
||||||
|
robot.left_arm.calibration = make_identity_calibration()
|
||||||
|
robot.right_arm.calibration = make_identity_calibration()
|
||||||
|
|
||||||
|
teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
|
||||||
|
robot.get_observation = lambda: (_ for _ in ()).throw(
|
||||||
|
AssertionError("teleop_loop should not fetch observation for identity processors without display")
|
||||||
|
)
|
||||||
|
|
||||||
|
teleop.connect(calibrate=False)
|
||||||
|
robot.connect(calibrate=False)
|
||||||
|
try:
|
||||||
|
teleop_loop(
|
||||||
|
teleop=teleop,
|
||||||
|
robot=robot,
|
||||||
|
fps=60,
|
||||||
|
teleop_action_processor=teleop_action_processor,
|
||||||
|
robot_action_processor=robot_action_processor,
|
||||||
|
robot_observation_processor=robot_observation_processor,
|
||||||
|
display_data=False,
|
||||||
|
duration=0.0,
|
||||||
|
)
|
||||||
|
finally:
|
||||||
|
teleop.disconnect()
|
||||||
|
robot.disconnect()
|
||||||
|
|
||||||
|
assert robot.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||||
|
assert robot.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||||
|
assert robot.left_arm.arm.last_gripper[0] == 42000
|
||||||
|
assert robot.right_arm.arm.last_gripper[0] == 42000
|
||||||
|
|
||||||
|
|
||||||
|
def test_bimanual_piper_leader_uses_process_proxy_by_default(monkeypatch):
|
||||||
|
class DummyProxy:
|
||||||
|
def __init__(self, arm_cls, arm_config):
|
||||||
|
self.arm_cls = arm_cls
|
||||||
|
self.arm_config = arm_config
|
||||||
|
self.is_connected = False
|
||||||
|
self.is_calibrated = True
|
||||||
|
|
||||||
|
action_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
|
||||||
|
feedback_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
|
||||||
|
|
||||||
|
def connect(self, calibrate=True):
|
||||||
|
del calibrate
|
||||||
|
self.is_connected = True
|
||||||
|
|
||||||
|
def calibrate(self):
|
||||||
|
pass
|
||||||
|
|
||||||
|
def configure(self):
|
||||||
|
pass
|
||||||
|
|
||||||
|
def setup_motors(self):
|
||||||
|
pass
|
||||||
|
|
||||||
|
def set_manual_control(self, enabled: bool):
|
||||||
|
del enabled
|
||||||
|
|
||||||
|
def get_action(self):
|
||||||
|
return dict.fromkeys(PIPER_ACTION_KEYS, 0.0)
|
||||||
|
|
||||||
|
def send_feedback(self, feedback):
|
||||||
|
del feedback
|
||||||
|
|
||||||
|
def disconnect(self):
|
||||||
|
self.is_connected = False
|
||||||
|
|
||||||
|
monkeypatch.setattr(bi_piper_leader_module, "_PiperLeaderProcessProxy", DummyProxy)
|
||||||
|
|
||||||
|
teleop = make_teleoperator_from_config(
|
||||||
|
BiPiperLeaderConfig(
|
||||||
|
left_arm_config=PiperLeaderConfigBase(port="can0", manual_control=False, sync_gripper=True),
|
||||||
|
right_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
assert isinstance(teleop.left_arm, DummyProxy)
|
||||||
|
assert isinstance(teleop.right_arm, DummyProxy)
|
||||||
|
|
||||||
|
|
||||||
|
def test_piper_follower_send_only_mode_skips_sdk_reader_threads(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
|
robot = make_robot_from_config(
|
||||||
|
PiperFollowerConfig(port="can3", sync_gripper=True, require_calibration=False)
|
||||||
|
)
|
||||||
|
robot.calibration = make_identity_calibration()
|
||||||
|
robot.set_teleop_send_only_mode(True)
|
||||||
|
|
||||||
|
robot.connect(calibrate=False)
|
||||||
|
try:
|
||||||
|
assert robot.arm.connect_calls[-1]["start_thread"] is False
|
||||||
|
with pytest.raises(RuntimeError, match="send-only mode"):
|
||||||
|
robot.get_observation()
|
||||||
|
finally:
|
||||||
|
robot.disconnect()
|
||||||
|
|
||||||
|
|
||||||
|
def test_piper_follower_send_only_mode_skips_camera_connects_and_stays_connected(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
|
class FakeCamera:
|
||||||
|
def __init__(self):
|
||||||
|
self.is_connected = False
|
||||||
|
self.connect_calls = 0
|
||||||
|
self.disconnect_calls = 0
|
||||||
|
|
||||||
|
def connect(self):
|
||||||
|
self.connect_calls += 1
|
||||||
|
self.is_connected = True
|
||||||
|
|
||||||
|
def disconnect(self):
|
||||||
|
self.disconnect_calls += 1
|
||||||
|
self.is_connected = False
|
||||||
|
|
||||||
|
camera = FakeCamera()
|
||||||
|
monkeypatch.setattr(piper_follower_module, "make_cameras_from_configs", lambda _: {"front": camera})
|
||||||
|
|
||||||
|
robot = make_robot_from_config(
|
||||||
|
PiperFollowerConfig(port="can3", sync_gripper=True, require_calibration=False)
|
||||||
|
)
|
||||||
|
robot.calibration = make_identity_calibration()
|
||||||
|
robot.set_teleop_send_only_mode(True)
|
||||||
|
|
||||||
|
robot.connect(calibrate=False)
|
||||||
|
try:
|
||||||
|
assert robot.is_connected
|
||||||
|
assert camera.connect_calls == 0
|
||||||
|
assert not camera.is_connected
|
||||||
|
finally:
|
||||||
|
robot.disconnect()
|
||||||
|
|
||||||
|
assert camera.disconnect_calls == 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_bimanual_piper_send_only_mode_propagates_to_both_followers(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
|
robot = make_robot_from_config(
|
||||||
|
BiPiperFollowerConfig(
|
||||||
|
left_arm_config=PiperFollowerConfigBase(port="can3", sync_gripper=True, require_calibration=False),
|
||||||
|
right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True, require_calibration=False),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
robot.left_arm.calibration = make_identity_calibration()
|
||||||
|
robot.right_arm.calibration = make_identity_calibration()
|
||||||
|
robot.set_teleop_send_only_mode(True)
|
||||||
|
|
||||||
|
robot.connect(calibrate=False)
|
||||||
|
try:
|
||||||
|
assert robot.left_arm.arm.connect_calls[-1]["start_thread"] is False
|
||||||
|
assert robot.right_arm.arm.connect_calls[-1]["start_thread"] is False
|
||||||
|
finally:
|
||||||
|
robot.disconnect()
|
||||||
|
|||||||
在新工单中引用
屏蔽一个用户