镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
720 行
24 KiB
Python
720 行
24 KiB
Python
# ruff: noqa: N802
|
|
from types import SimpleNamespace
|
|
|
|
import pytest
|
|
|
|
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.utils.piper_sdk as piper_sdk_utils
|
|
from lerobot.processor import make_default_processors
|
|
from lerobot.robots.bi_piper_follower import (
|
|
BiPiperFollower,
|
|
BiPiperFollowerConfig,
|
|
BiPiperXFollower,
|
|
BiPiperXFollowerConfig,
|
|
)
|
|
from lerobot.robots.piper_follower import (
|
|
PiperFollower,
|
|
PiperFollowerConfig,
|
|
PiperFollowerConfigBase,
|
|
PiperXFollower,
|
|
PiperXFollowerConfig,
|
|
)
|
|
from lerobot.robots.utils import make_robot_from_config
|
|
from lerobot.scripts.lerobot_teleoperate import teleop_loop
|
|
from lerobot.teleoperators.bi_piper_leader import (
|
|
BiPiperLeader,
|
|
BiPiperLeaderConfig,
|
|
BiPiperXLeader,
|
|
BiPiperXLeaderConfig,
|
|
)
|
|
from lerobot.teleoperators.piper_leader import (
|
|
PiperLeader,
|
|
PiperLeaderConfig,
|
|
PiperLeaderConfigBase,
|
|
PiperXLeader,
|
|
PiperXLeaderConfig,
|
|
PiperXLeaderConfigBase,
|
|
)
|
|
from lerobot.teleoperators.utils import make_teleoperator_from_config
|
|
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
|
|
|
|
|
|
class FakeLogLevel:
|
|
DEBUG = "DEBUG"
|
|
INFO = "INFO"
|
|
WARNING = "WARNING"
|
|
ERROR = "ERROR"
|
|
CRITICAL = "CRITICAL"
|
|
SILENT = "SILENT"
|
|
|
|
|
|
class FakePiperInterface:
|
|
def __init__(self, can_name, judge_flag=False, can_auto_init=True, logger_level=None):
|
|
self.can_name = can_name
|
|
self.judge_flag = judge_flag
|
|
self.can_auto_init = can_auto_init
|
|
self.logger_level = logger_level
|
|
self.connected = False
|
|
self.mode_commands = []
|
|
self.role_commands = []
|
|
self.last_joint = None
|
|
self.last_gripper = None
|
|
self.gripper_calls = []
|
|
self.joint_mit_calls = []
|
|
self.enable_calls = 0
|
|
self.disable_calls = 0
|
|
self.is_enabled = False
|
|
self.connect_calls = []
|
|
self.command_log = []
|
|
|
|
self._joint_ctrl = SimpleNamespace(
|
|
time_stamp=1.0,
|
|
Hz=120.0,
|
|
joint_ctrl=SimpleNamespace(
|
|
joint_1=10000,
|
|
joint_2=20000,
|
|
joint_3=30000,
|
|
joint_4=40000,
|
|
joint_5=50000,
|
|
joint_6=60000,
|
|
),
|
|
)
|
|
self._joint_state = SimpleNamespace(
|
|
time_stamp=1.0,
|
|
Hz=120.0,
|
|
joint_state=SimpleNamespace(
|
|
joint_1=11000,
|
|
joint_2=21000,
|
|
joint_3=31000,
|
|
joint_4=41000,
|
|
joint_5=51000,
|
|
joint_6=61000,
|
|
),
|
|
)
|
|
self._gripper_ctrl = SimpleNamespace(
|
|
time_stamp=1.0,
|
|
Hz=120.0,
|
|
gripper_ctrl=SimpleNamespace(grippers_angle=42000, grippers_effort=1500, status_code=0x01),
|
|
)
|
|
self._gripper_state = SimpleNamespace(
|
|
time_stamp=1.0,
|
|
Hz=120.0,
|
|
gripper_state=SimpleNamespace(grippers_angle=43000, grippers_effort=1400, status_code=0x01),
|
|
)
|
|
self._high_spd = SimpleNamespace(
|
|
time_stamp=1.0,
|
|
Hz=120.0,
|
|
motor_1=SimpleNamespace(motor_speed=0),
|
|
motor_2=SimpleNamespace(motor_speed=0),
|
|
motor_3=SimpleNamespace(motor_speed=0),
|
|
motor_4=SimpleNamespace(motor_speed=0),
|
|
motor_5=SimpleNamespace(motor_speed=0),
|
|
motor_6=SimpleNamespace(motor_speed=0),
|
|
)
|
|
self._arm_status = SimpleNamespace(
|
|
time_stamp=1.0,
|
|
Hz=120.0,
|
|
arm_status=SimpleNamespace(ctrl_mode=0x01),
|
|
)
|
|
|
|
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
|
|
|
|
def DisconnectPort(self, thread_timeout=0.1):
|
|
del thread_timeout
|
|
self.connected = False
|
|
|
|
def MotionCtrl_2(self, *args):
|
|
self.mode_commands.append(args)
|
|
self.command_log.append(("motion_mode", args))
|
|
|
|
def MasterSlaveConfig(self, *args):
|
|
self.role_commands.append(args)
|
|
self.command_log.append(("role", args))
|
|
if args and args[0] == 0xFC:
|
|
self._arm_status.arm_status.ctrl_mode = 0x01
|
|
elif args and args[0] == 0xFA:
|
|
self._arm_status.arm_status.ctrl_mode = 0x06
|
|
|
|
def EnablePiper(self):
|
|
self.enable_calls += 1
|
|
self.command_log.append(("enable", ()))
|
|
self.is_enabled = True
|
|
return True
|
|
|
|
def DisableArm(self, motor_num):
|
|
del motor_num
|
|
self.disable_calls += 1
|
|
self.is_enabled = False
|
|
|
|
def JointCtrl(self, *args):
|
|
self.last_joint = args
|
|
self.command_log.append(("joint", args))
|
|
|
|
def JointMitCtrl(self, *args):
|
|
self.joint_mit_calls.append(args)
|
|
|
|
def GripperCtrl(self, *args):
|
|
self.last_gripper = args
|
|
self.gripper_calls.append(args)
|
|
self.command_log.append(("gripper", args))
|
|
|
|
def GetArmJointCtrl(self):
|
|
return self._joint_ctrl
|
|
|
|
def GetArmJointMsgs(self):
|
|
return self._joint_state
|
|
|
|
def GetArmGripperCtrl(self):
|
|
return self._gripper_ctrl
|
|
|
|
def GetArmGripperMsgs(self):
|
|
return self._gripper_state
|
|
|
|
def GetArmHighSpdInfoMsgs(self):
|
|
return self._high_spd
|
|
|
|
def GetArmStatus(self):
|
|
return self._arm_status
|
|
|
|
|
|
def patch_fake_sdk(monkeypatch):
|
|
def fake_loader():
|
|
return (FakePiperInterface, FakeLogLevel)
|
|
|
|
monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", fake_loader)
|
|
monkeypatch.setattr(piper_follower_module, "get_piper_sdk", fake_loader)
|
|
monkeypatch.setattr(piper_leader_module, "get_piper_sdk", fake_loader)
|
|
monkeypatch.setattr(piper_follower_module, "resolve_piper_can_interface", lambda serial: serial)
|
|
monkeypatch.setattr(piper_leader_module, "resolve_piper_can_interface", lambda serial: serial)
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
("teleop_cfg", "robot_cfg", "teleop_cls", "robot_cls"),
|
|
[
|
|
(
|
|
PiperLeaderConfig(port="can1", manual_control=True, sync_gripper=True),
|
|
PiperFollowerConfig(port="can0", sync_gripper=True),
|
|
PiperLeader,
|
|
PiperFollower,
|
|
),
|
|
(
|
|
PiperXLeaderConfig(port="can1", manual_control=True, sync_gripper=True),
|
|
PiperXFollowerConfig(port="can0", sync_gripper=True),
|
|
PiperXLeader,
|
|
PiperXFollower,
|
|
),
|
|
],
|
|
)
|
|
def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_cfg, teleop_cls, robot_cls):
|
|
patch_fake_sdk(monkeypatch)
|
|
|
|
teleop = make_teleoperator_from_config(teleop_cfg)
|
|
robot = make_robot_from_config(robot_cfg)
|
|
|
|
assert isinstance(teleop, teleop_cls)
|
|
assert isinstance(robot, robot_cls)
|
|
|
|
teleop.connect()
|
|
robot.connect()
|
|
try:
|
|
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
|
|
assert robot.arm.role_commands == [(0xFC, 0x00, 0x00, 0x00)]
|
|
action = teleop.get_action()
|
|
sent = robot.send_action(action)
|
|
obs = robot.get_observation()
|
|
|
|
assert robot.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
|
assert robot.arm.last_gripper == (
|
|
43000,
|
|
robot_cfg.gripper_effort_default,
|
|
robot_cfg.gripper_status_code,
|
|
0x00,
|
|
)
|
|
assert sent["joint_1.pos"] == 11.0
|
|
assert sent["gripper.pos"] == 43.0
|
|
assert obs["joint_1.pos"] == 11.0
|
|
assert obs["gripper.pos"] == 43.0
|
|
|
|
teleop.send_feedback(action)
|
|
assert teleop.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00)
|
|
assert teleop.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
|
assert teleop.arm.last_gripper == (
|
|
43000,
|
|
teleop_cfg.gripper_effort_default,
|
|
teleop_cfg.gripper_status_code,
|
|
0x00,
|
|
)
|
|
finally:
|
|
teleop.disconnect()
|
|
robot.disconnect()
|
|
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
|
|
|
|
|
|
def test_piper_does_not_initialize_calibration_state(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
|
|
leader = PiperLeader(PiperLeaderConfig(port="can1"))
|
|
follower = PiperFollower(PiperFollowerConfig(port="can0"))
|
|
bi_leader = BiPiperLeader(
|
|
BiPiperLeaderConfig(
|
|
left_arm_config=PiperLeaderConfigBase(port="can1"),
|
|
right_arm_config=PiperLeaderConfigBase(port="can3"),
|
|
process_isolation=False,
|
|
)
|
|
)
|
|
bi_follower = BiPiperFollower(
|
|
BiPiperFollowerConfig(
|
|
left_arm_config=PiperFollowerConfigBase(port="can0"),
|
|
right_arm_config=PiperFollowerConfigBase(port="can2"),
|
|
)
|
|
)
|
|
|
|
devices = [
|
|
leader,
|
|
follower,
|
|
bi_leader,
|
|
bi_leader.left_arm,
|
|
bi_leader.right_arm,
|
|
bi_follower,
|
|
bi_follower.left_arm,
|
|
bi_follower.right_arm,
|
|
]
|
|
assert all(not hasattr(device, "calibration") for device in devices)
|
|
assert all(not hasattr(device, "calibration_fpath") for device in devices)
|
|
|
|
|
|
def test_piper_leader_reconnect_reapplies_mode(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
|
|
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
|
|
teleop.connect()
|
|
teleop.disconnect()
|
|
teleop.connect()
|
|
try:
|
|
assert teleop.arm.enable_calls == 2
|
|
finally:
|
|
teleop.disconnect()
|
|
|
|
|
|
def test_piper_follower_connect_rolls_back_connected_cameras(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
|
|
class FakeCamera:
|
|
def __init__(self, should_fail: bool):
|
|
self.should_fail = should_fail
|
|
self.is_connected = False
|
|
self.disconnect_calls = 0
|
|
|
|
def connect(self):
|
|
if self.should_fail:
|
|
raise RuntimeError("camera connect failed")
|
|
self.is_connected = True
|
|
|
|
def disconnect(self):
|
|
self.disconnect_calls += 1
|
|
self.is_connected = False
|
|
|
|
def async_read(self):
|
|
return None
|
|
|
|
cam_ok = FakeCamera(should_fail=False)
|
|
cam_fail = FakeCamera(should_fail=True)
|
|
monkeypatch.setattr(
|
|
piper_follower_module,
|
|
"make_cameras_from_configs",
|
|
lambda _: {"cam_ok": cam_ok, "cam_fail": cam_fail},
|
|
)
|
|
|
|
robot = PiperFollower(PiperFollowerConfig(port="can0"))
|
|
with pytest.raises(RuntimeError, match="camera connect failed"):
|
|
robot.connect()
|
|
|
|
assert cam_ok.disconnect_calls == 1
|
|
assert not cam_ok.is_connected
|
|
assert not robot.is_connected
|
|
|
|
|
|
def test_piper_leader_default_uses_hardware_leader_role(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
|
|
|
teleop.connect()
|
|
try:
|
|
assert teleop.arm.role_commands == [
|
|
(0xFC, 0x00, 0x00, 0x00),
|
|
(0xFA, 0x00, 0x00, 0x00),
|
|
]
|
|
assert teleop.arm.enable_calls == 0
|
|
assert teleop.arm.mode_commands == []
|
|
assert teleop.arm.gripper_calls == []
|
|
assert teleop.arm.joint_mit_calls == []
|
|
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_switches_between_manual_and_policy_control(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
|
teleop.connect()
|
|
try:
|
|
teleop.arm.command_log.clear()
|
|
teleop.set_manual_control(False)
|
|
assert teleop.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00)
|
|
assert teleop.arm.mode_commands[-1][:3] == (0x01, 0x01, teleop.config.command_speed_ratio)
|
|
assert teleop.arm.enable_calls == 1
|
|
assert [name for name, _ in teleop.arm.command_log[:3]] == ["role", "motion_mode", "enable"]
|
|
|
|
role_count = len(teleop.arm.role_commands)
|
|
action = {
|
|
"joint_1.pos": 1.0,
|
|
"joint_2.pos": 2.0,
|
|
"joint_3.pos": 3.0,
|
|
"joint_4.pos": 4.0,
|
|
"joint_5.pos": 5.0,
|
|
"joint_6.pos": 6.0,
|
|
"gripper.pos": 7.0,
|
|
}
|
|
teleop.send_feedback(action)
|
|
assert len(teleop.arm.role_commands) == role_count
|
|
assert teleop.arm.last_joint == (1000, 2000, 3000, 4000, 5000, 6000)
|
|
assert teleop.arm.last_gripper[0] == 7000
|
|
|
|
teleop.set_manual_control(True)
|
|
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
|
|
assert teleop.arm.joint_mit_calls == []
|
|
finally:
|
|
teleop.disconnect()
|
|
|
|
|
|
def test_piper_leader_policy_mode_reads_feedback(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
|
|
|
|
teleop.connect()
|
|
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_uses_feedback_seed_without_manual_frames(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
|
teleop.arm._joint_ctrl.time_stamp = 0.0
|
|
teleop.arm._gripper_ctrl.time_stamp = 0.0
|
|
teleop.connect()
|
|
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_failed_policy_switch_leaves_mode_unknown(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
teleop = PiperLeader(PiperLeaderConfig(port="can1", enable_timeout_s=0.0))
|
|
teleop.connect()
|
|
try:
|
|
with pytest.raises(RuntimeError, match="did not enable"):
|
|
teleop.set_manual_control(False)
|
|
assert teleop._manual_control_enabled is None
|
|
assert teleop.arm.role_commands[-1][0] == 0xFC
|
|
finally:
|
|
teleop.disconnect()
|
|
|
|
|
|
def test_piper_leader_disconnect_cleans_up_if_role_restore_fails(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
|
|
teleop.connect()
|
|
|
|
def fail_role_restore(*args):
|
|
raise RuntimeError("leader role restore failed")
|
|
|
|
monkeypatch.setattr(teleop.arm, "MasterSlaveConfig", fail_role_restore)
|
|
|
|
with pytest.raises(RuntimeError, match="leader role restore failed"):
|
|
teleop.disconnect()
|
|
assert teleop.arm.disable_calls == 0
|
|
assert not teleop.arm.connected
|
|
|
|
|
|
def test_piper_follower_connect_enables_arm(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
robot = PiperFollower(PiperFollowerConfig(port="can0", enable_on_connect=True))
|
|
|
|
robot.connect()
|
|
try:
|
|
assert robot.arm.enable_calls == 1
|
|
assert robot.arm.is_enabled
|
|
assert robot.arm.disable_calls == 0
|
|
finally:
|
|
robot.disconnect()
|
|
|
|
|
|
def test_piper_follower_connect_sets_follower_role_without_restart(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
device = PiperFollower(PiperFollowerConfig(port="can0"))
|
|
device.arm._arm_status.arm_status.ctrl_mode = 0x06
|
|
|
|
device.connect()
|
|
try:
|
|
assert device.arm.role_commands == [(0xFC, 0x00, 0x00, 0x00)]
|
|
assert [name for name, _ in device.arm.command_log[:3]] == ["role", "motion_mode", "enable"]
|
|
finally:
|
|
device.disconnect()
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
(
|
|
"teleop_cfg",
|
|
"robot_cfg",
|
|
"bi_teleop_cls",
|
|
"bi_robot_cls",
|
|
"left_teleop_cls",
|
|
"right_teleop_cls",
|
|
"left_robot_cls",
|
|
"right_robot_cls",
|
|
),
|
|
[
|
|
(
|
|
BiPiperLeaderConfig(
|
|
left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=True, sync_gripper=True),
|
|
right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=True, sync_gripper=True),
|
|
process_isolation=False,
|
|
),
|
|
BiPiperFollowerConfig(
|
|
left_arm_config=PiperFollowerConfigBase(port="can0", sync_gripper=True),
|
|
right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True),
|
|
),
|
|
BiPiperLeader,
|
|
BiPiperFollower,
|
|
PiperLeader,
|
|
PiperLeader,
|
|
PiperFollower,
|
|
PiperFollower,
|
|
),
|
|
(
|
|
BiPiperXLeaderConfig(
|
|
left_arm_config=PiperXLeaderConfigBase(port="can1", manual_control=True, sync_gripper=True),
|
|
right_arm_config=PiperXLeaderConfigBase(port="can3", manual_control=True, sync_gripper=True),
|
|
process_isolation=False,
|
|
),
|
|
BiPiperXFollowerConfig(
|
|
left_arm_config=PiperFollowerConfigBase(port="can0", sync_gripper=True),
|
|
right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True),
|
|
),
|
|
BiPiperXLeader,
|
|
BiPiperXFollower,
|
|
PiperXLeader,
|
|
PiperXLeader,
|
|
PiperXFollower,
|
|
PiperXFollower,
|
|
),
|
|
],
|
|
)
|
|
def test_bimanual_piper_leader_follower_roundtrip(
|
|
monkeypatch,
|
|
teleop_cfg,
|
|
robot_cfg,
|
|
bi_teleop_cls,
|
|
bi_robot_cls,
|
|
left_teleop_cls,
|
|
right_teleop_cls,
|
|
left_robot_cls,
|
|
right_robot_cls,
|
|
):
|
|
patch_fake_sdk(monkeypatch)
|
|
|
|
teleop = make_teleoperator_from_config(teleop_cfg)
|
|
robot = make_robot_from_config(robot_cfg)
|
|
|
|
assert isinstance(teleop, bi_teleop_cls)
|
|
assert isinstance(robot, bi_robot_cls)
|
|
assert isinstance(teleop.left_arm, left_teleop_cls)
|
|
assert isinstance(teleop.right_arm, right_teleop_cls)
|
|
assert isinstance(robot.left_arm, left_robot_cls)
|
|
assert isinstance(robot.right_arm, right_robot_cls)
|
|
|
|
teleop.connect()
|
|
robot.connect()
|
|
try:
|
|
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFA
|
|
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFA
|
|
action = teleop.get_action()
|
|
assert "left_joint_1.pos" in action
|
|
assert "right_joint_1.pos" in action
|
|
|
|
sent = robot.send_action(action)
|
|
obs = robot.get_observation()
|
|
|
|
assert robot.left_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
|
assert robot.right_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
|
assert robot.left_arm.arm.last_gripper[0] == 43000
|
|
assert robot.right_arm.arm.last_gripper[0] == 43000
|
|
|
|
assert sent["left_joint_1.pos"] == 11.0
|
|
assert sent["right_joint_1.pos"] == 11.0
|
|
assert sent["left_gripper.pos"] == 43.0
|
|
assert sent["right_gripper.pos"] == 43.0
|
|
assert obs["left_joint_1.pos"] == 11.0
|
|
assert obs["right_joint_1.pos"] == 11.0
|
|
assert obs["left_gripper.pos"] == 43.0
|
|
assert obs["right_gripper.pos"] == 43.0
|
|
|
|
teleop.send_feedback(action)
|
|
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFC
|
|
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFC
|
|
assert teleop.left_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
|
assert teleop.right_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
|
assert teleop.left_arm.arm.last_gripper[0] == 43000
|
|
assert teleop.right_arm.arm.last_gripper[0] == 43000
|
|
finally:
|
|
teleop.disconnect()
|
|
robot.disconnect()
|
|
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFA
|
|
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFA
|
|
|
|
|
|
def test_bimanual_piper_get_action_uses_feedback_seed_for_silent_side(monkeypatch):
|
|
patch_fake_sdk(monkeypatch)
|
|
|
|
teleop = make_teleoperator_from_config(
|
|
BiPiperLeaderConfig(
|
|
left_arm_config=PiperLeaderConfigBase(port="can2"),
|
|
right_arm_config=PiperLeaderConfigBase(port="can3"),
|
|
process_isolation=False,
|
|
)
|
|
)
|
|
|
|
teleop.connect()
|
|
try:
|
|
teleop.right_arm.arm._joint_ctrl.time_stamp = 0.0
|
|
action = teleop.get_action()
|
|
assert "left_joint_1.pos" in action
|
|
assert "right_joint_1.pos" in action
|
|
finally:
|
|
teleop.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=True, sync_gripper=True),
|
|
right_arm_config=PiperLeaderConfigBase(port="can1", manual_control=True, 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_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()
|
|
robot.connect()
|
|
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 == (11000, 21000, 31000, 41000, 51000, 61000)
|
|
assert robot.right_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
|
assert robot.left_arm.arm.last_gripper[0] == 43000
|
|
assert robot.right_arm.arm.last_gripper[0] == 43000
|
|
|
|
|
|
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
|
|
|
|
action_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
|
|
feedback_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
|
|
|
|
def connect(self):
|
|
self.is_connected = True
|
|
|
|
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)
|