镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
feat(piper): add calibration-driven leader/follower mapping
- add calibration_scale config and validation for piper leader/follower - implement interactive calibration capture and calibrated offset conversion - require calibration before action/feedback paths and update teleop tests
这个提交包含在:
@ -46,6 +46,10 @@ class PiperFollowerConfigBase:
|
||||
enable_on_connect: bool = True
|
||||
enable_timeout_s: float = 3.0
|
||||
|
||||
# Calibration precision:
|
||||
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
|
||||
calibration_scale: int = 1000
|
||||
|
||||
# Gripper forwarding behavior
|
||||
sync_gripper: bool = True
|
||||
gripper_effort_default: int = 1000
|
||||
@ -69,6 +73,8 @@ class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase):
|
||||
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
||||
if self.enable_timeout_s < 0:
|
||||
raise ValueError("`enable_timeout_s` must be >= 0.")
|
||||
if self.calibration_scale <= 0:
|
||||
raise ValueError("`calibration_scale` must be > 0.")
|
||||
if self.startup_sleep_s < 0:
|
||||
raise ValueError("`startup_sleep_s` must be >= 0.")
|
||||
if not (0 <= self.gripper_effort_default <= 5000):
|
||||
|
||||
@ -20,6 +20,7 @@ from functools import cached_property
|
||||
from typing import Any
|
||||
|
||||
from lerobot.cameras.utils import make_cameras_from_configs
|
||||
from lerobot.motors import MotorCalibration
|
||||
from lerobot.processor import RobotAction, RobotObservation
|
||||
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
||||
from lerobot.utils.piper_sdk import (
|
||||
@ -30,11 +31,14 @@ from lerobot.utils.piper_sdk import (
|
||||
parse_piper_log_level,
|
||||
unit_to_milli,
|
||||
)
|
||||
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
||||
|
||||
from ..robot import Robot
|
||||
from .config_piper_follower import PiperFollowerConfig
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS)
|
||||
PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)}
|
||||
|
||||
|
||||
class PiperFollower(Robot):
|
||||
@ -78,7 +82,6 @@ class PiperFollower(Robot):
|
||||
|
||||
@check_if_already_connected
|
||||
def connect(self, calibrate: bool = True) -> None:
|
||||
del calibrate
|
||||
self.arm.ConnectPort()
|
||||
if self.config.startup_sleep_s > 0:
|
||||
time.sleep(self.config.startup_sleep_s)
|
||||
@ -90,8 +93,14 @@ class PiperFollower(Robot):
|
||||
time.sleep(0.05)
|
||||
|
||||
self.configure()
|
||||
if self.config.enable_on_connect and not self._wait_enable(self.config.enable_timeout_s):
|
||||
# lerobot-calibrate calls connect(calibrate=False) before running calibrate().
|
||||
# Keep the arm in drag mode for calibration by skipping auto-enable in this path.
|
||||
should_enable = self.config.enable_on_connect and calibrate
|
||||
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 not self.is_calibrated and calibrate:
|
||||
logger.info("No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", self.id)
|
||||
self.calibrate()
|
||||
|
||||
for cam in self.cameras.values():
|
||||
cam.connect()
|
||||
@ -104,10 +113,50 @@ class PiperFollower(Robot):
|
||||
|
||||
@property
|
||||
def is_calibrated(self) -> bool:
|
||||
if not all(key in self.calibration for key in PIPER_CALIB_KEYS):
|
||||
return False
|
||||
for key in PIPER_CALIB_KEYS:
|
||||
cal = self.calibration[key]
|
||||
if cal.range_max <= cal.range_min:
|
||||
return False
|
||||
return True
|
||||
|
||||
def calibrate(self) -> None:
|
||||
logger.info("Piper follower does not require lerobot joint-range calibration. Skipping.")
|
||||
if self.calibration and self.is_calibrated:
|
||||
user_input = input(
|
||||
f"Press ENTER to use existing calibration file for id '{self.id}', "
|
||||
"or type 'c' and press ENTER to run a new calibration: "
|
||||
)
|
||||
if user_input.strip().lower() != "c":
|
||||
return
|
||||
|
||||
logger.info("Running calibration for %s", self)
|
||||
# Calibration for Piper follower should be sampled in pure backdrivable mode.
|
||||
self.arm.DisableArm(7)
|
||||
time.sleep(0.1)
|
||||
input("Move piper-follower to your desired neutral/center pose, then press ENTER...")
|
||||
neutral = self._read_raw_observation()
|
||||
print("Move all piper-follower joints through full range. Press ENTER to stop recording...")
|
||||
range_mins, range_maxes = self._record_ranges_of_motion()
|
||||
|
||||
self.calibration = {}
|
||||
for key in PIPER_CALIB_KEYS:
|
||||
min_deg = range_mins[key]
|
||||
max_deg = range_maxes[key]
|
||||
if max_deg <= min_deg:
|
||||
raise ValueError(f"Invalid range for {key}: min={min_deg:.3f}, max={max_deg:.3f}")
|
||||
|
||||
neutral_deg = min(max_deg, max(min_deg, neutral[key]))
|
||||
self.calibration[key] = MotorCalibration(
|
||||
id=PIPER_CALIB_IDS[key],
|
||||
drive_mode=0,
|
||||
homing_offset=self._to_calibration_units(neutral_deg),
|
||||
range_min=self._to_calibration_units(min_deg),
|
||||
range_max=self._to_calibration_units(max_deg),
|
||||
)
|
||||
|
||||
self._save_calibration()
|
||||
print(f"Calibration saved to {self.calibration_fpath}")
|
||||
|
||||
def configure(self) -> None:
|
||||
self._send_motion_mode()
|
||||
@ -133,12 +182,11 @@ class PiperFollower(Robot):
|
||||
time.sleep(0.02)
|
||||
return False
|
||||
|
||||
@check_if_not_connected
|
||||
def get_observation(self) -> RobotObservation:
|
||||
def _read_raw_observation(self) -> RobotObservation:
|
||||
joint_msg = self.arm.GetArmJointMsgs()
|
||||
joint_state = getattr(joint_msg, "joint_state", None)
|
||||
|
||||
obs: dict[str, float] = {}
|
||||
obs: RobotObservation = {}
|
||||
for joint_name in PIPER_JOINT_NAMES:
|
||||
raw_value = getattr(joint_state, joint_name, 0)
|
||||
obs[f"{joint_name}.pos"] = milli_to_unit(raw_value)
|
||||
@ -146,6 +194,47 @@ class PiperFollower(Robot):
|
||||
gripper_msg = self.arm.GetArmGripperMsgs()
|
||||
gripper_state = getattr(gripper_msg, "gripper_state", None)
|
||||
obs["gripper.pos"] = abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
||||
return obs
|
||||
|
||||
def _to_calibration_units(self, angle_deg: float) -> int:
|
||||
return int(round(angle_deg * self.config.calibration_scale))
|
||||
|
||||
def _from_calibration_units(self, value: int) -> float:
|
||||
return float(value) / float(self.config.calibration_scale)
|
||||
|
||||
def _offset_to_target(self, key: str, offset_deg: float) -> float:
|
||||
cal = self.calibration[key]
|
||||
min_deg = self._from_calibration_units(cal.range_min)
|
||||
max_deg = self._from_calibration_units(cal.range_max)
|
||||
home_deg = self._from_calibration_units(cal.homing_offset)
|
||||
centered = -offset_deg if cal.drive_mode else offset_deg
|
||||
target = home_deg + centered
|
||||
return min(max_deg, max(min_deg, target))
|
||||
|
||||
def _record_ranges_of_motion(self) -> tuple[dict[str, float], dict[str, float]]:
|
||||
current = self._read_raw_observation()
|
||||
mins = current.copy()
|
||||
maxes = current.copy()
|
||||
|
||||
while True:
|
||||
current = self._read_raw_observation()
|
||||
mins = {key: min(mins[key], current[key]) for key in PIPER_CALIB_KEYS}
|
||||
maxes = {key: max(maxes[key], current[key]) for key in PIPER_CALIB_KEYS}
|
||||
|
||||
print("\n-----------------------------")
|
||||
print("JOINT | MIN | POS | MAX")
|
||||
for key in PIPER_CALIB_KEYS:
|
||||
print(f"{key:<11} | {mins[key]:>6.2f} | {current[key]:>6.2f} | {maxes[key]:>6.2f}")
|
||||
|
||||
if enter_pressed():
|
||||
break
|
||||
move_cursor_up(len(PIPER_CALIB_KEYS) + 3)
|
||||
|
||||
return mins, maxes
|
||||
|
||||
@check_if_not_connected
|
||||
def get_observation(self) -> RobotObservation:
|
||||
obs: dict[str, float] = self._read_raw_observation()
|
||||
|
||||
for cam_key, cam in self.cameras.items():
|
||||
obs[cam_key] = cam.async_read()
|
||||
@ -153,6 +242,11 @@ class PiperFollower(Robot):
|
||||
|
||||
@check_if_not_connected
|
||||
def send_action(self, action: RobotAction) -> RobotAction:
|
||||
if not self.is_calibrated:
|
||||
raise RuntimeError(
|
||||
f"{self} is not calibrated. Run `lerobot-calibrate --robot.type=piper_follower --robot.id={self.id}` first."
|
||||
)
|
||||
|
||||
self._refresh_motion_mode_if_needed()
|
||||
|
||||
sent_action: dict[str, float] = {}
|
||||
@ -160,14 +254,16 @@ class PiperFollower(Robot):
|
||||
joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES]
|
||||
has_all_joints = all(key in action for key in joint_keys)
|
||||
if has_all_joints:
|
||||
joint_commands = [unit_to_milli(action[key]) for key in joint_keys]
|
||||
joint_targets = [self._offset_to_target(key, action[key]) for key in joint_keys]
|
||||
joint_commands = [unit_to_milli(value) for value in joint_targets]
|
||||
self.arm.JointCtrl(*joint_commands)
|
||||
sent_action.update({key: milli_to_unit(raw) for key, raw in zip(joint_keys, joint_commands, strict=True)})
|
||||
elif any(key in action for key in joint_keys):
|
||||
logger.debug("Ignoring partial Piper joint action. Need all six joint keys to send command.")
|
||||
|
||||
if self.config.sync_gripper and "gripper.pos" in action:
|
||||
gripper_pos_raw = unit_to_milli(abs(action["gripper.pos"]))
|
||||
gripper_target = self._offset_to_target("gripper.pos", action["gripper.pos"])
|
||||
gripper_pos_raw = unit_to_milli(gripper_target)
|
||||
self.arm.GripperCtrl(
|
||||
gripper_pos_raw,
|
||||
self.config.gripper_effort_default,
|
||||
|
||||
@ -53,6 +53,10 @@ class PiperLeaderConfigBase:
|
||||
mode_refresh_interval_s: float = 1.0
|
||||
enable_timeout_s: float = 3.0
|
||||
|
||||
# Calibration precision:
|
||||
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
|
||||
calibration_scale: int = 1000
|
||||
|
||||
# Safety behavior on disconnect
|
||||
disable_on_disconnect: bool = False
|
||||
|
||||
@ -67,6 +71,8 @@ class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase):
|
||||
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
||||
if self.enable_timeout_s < 0:
|
||||
raise ValueError("`enable_timeout_s` must be >= 0.")
|
||||
if self.calibration_scale <= 0:
|
||||
raise ValueError("`calibration_scale` must be > 0.")
|
||||
if self.startup_sleep_s < 0:
|
||||
raise ValueError("`startup_sleep_s` must be >= 0.")
|
||||
if not (0 <= self.gripper_effort_default <= 5000):
|
||||
|
||||
@ -19,6 +19,7 @@ import time
|
||||
from functools import cached_property
|
||||
from typing import Any
|
||||
|
||||
from lerobot.motors import MotorCalibration
|
||||
from lerobot.processor import RobotAction
|
||||
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
||||
from lerobot.utils.piper_sdk import (
|
||||
@ -29,11 +30,14 @@ from lerobot.utils.piper_sdk import (
|
||||
parse_piper_log_level,
|
||||
unit_to_milli,
|
||||
)
|
||||
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
||||
|
||||
from ..teleoperator import Teleoperator
|
||||
from .config_piper_leader import PiperLeaderConfig
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS)
|
||||
PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)}
|
||||
|
||||
|
||||
class PiperLeader(Teleoperator):
|
||||
@ -71,7 +75,6 @@ class PiperLeader(Teleoperator):
|
||||
|
||||
@check_if_already_connected
|
||||
def connect(self, calibrate: bool = True) -> None:
|
||||
del calibrate
|
||||
self.arm.ConnectPort()
|
||||
if self.config.startup_sleep_s > 0:
|
||||
time.sleep(self.config.startup_sleep_s)
|
||||
@ -87,6 +90,9 @@ class PiperLeader(Teleoperator):
|
||||
self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
||||
time.sleep(0.05)
|
||||
self.configure()
|
||||
if not self.is_calibrated and calibrate:
|
||||
logger.info("No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.", self.id)
|
||||
self.calibrate()
|
||||
except Exception:
|
||||
self.arm.DisconnectPort()
|
||||
self._is_connected = False
|
||||
@ -96,10 +102,47 @@ class PiperLeader(Teleoperator):
|
||||
|
||||
@property
|
||||
def is_calibrated(self) -> bool:
|
||||
if not all(key in self.calibration for key in PIPER_CALIB_KEYS):
|
||||
return False
|
||||
for key in PIPER_CALIB_KEYS:
|
||||
cal = self.calibration[key]
|
||||
if cal.range_max <= cal.range_min:
|
||||
return False
|
||||
return True
|
||||
|
||||
def calibrate(self) -> None:
|
||||
logger.info("Piper leader does not require lerobot joint-range calibration. Skipping.")
|
||||
if self.calibration and self.is_calibrated:
|
||||
user_input = input(
|
||||
f"Press ENTER to use existing calibration file for id '{self.id}', "
|
||||
"or type 'c' and press ENTER to run a new calibration: "
|
||||
)
|
||||
if user_input.strip().lower() != "c":
|
||||
return
|
||||
|
||||
logger.info("Running calibration for %s", self)
|
||||
input("Move piper-leader to your desired neutral/center pose, then press ENTER...")
|
||||
neutral = self._read_raw_action()
|
||||
print("Move all piper-leader joints through full range. Press ENTER to stop recording...")
|
||||
range_mins, range_maxes = self._record_ranges_of_motion()
|
||||
|
||||
self.calibration = {}
|
||||
for key in PIPER_CALIB_KEYS:
|
||||
min_deg = range_mins[key]
|
||||
max_deg = range_maxes[key]
|
||||
if max_deg <= min_deg:
|
||||
raise ValueError(f"Invalid range for {key}: min={min_deg:.3f}, max={max_deg:.3f}")
|
||||
|
||||
neutral_deg = min(max_deg, max(min_deg, neutral[key]))
|
||||
self.calibration[key] = MotorCalibration(
|
||||
id=PIPER_CALIB_IDS[key],
|
||||
drive_mode=0,
|
||||
homing_offset=self._to_calibration_units(neutral_deg),
|
||||
range_min=self._to_calibration_units(min_deg),
|
||||
range_max=self._to_calibration_units(max_deg),
|
||||
)
|
||||
|
||||
self._save_calibration()
|
||||
print(f"Calibration saved to {self.calibration_fpath}")
|
||||
|
||||
def _send_command_mode(self) -> None:
|
||||
mit_mode = 0xAD if self.config.command_high_follow else 0x00
|
||||
@ -172,8 +215,7 @@ class PiperLeader(Teleoperator):
|
||||
return None
|
||||
return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
||||
|
||||
@check_if_not_connected
|
||||
def get_action(self) -> RobotAction:
|
||||
def _read_raw_action(self) -> RobotAction:
|
||||
action: dict[str, float] | None = None
|
||||
if self.config.prefer_ctrl_messages:
|
||||
action = self._read_joint_from_ctrl()
|
||||
@ -184,29 +226,92 @@ class PiperLeader(Teleoperator):
|
||||
if action is None:
|
||||
action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES}
|
||||
|
||||
if self.config.sync_gripper:
|
||||
gripper_pos = self._read_gripper_from_ctrl() if self.config.prefer_ctrl_messages else None
|
||||
if gripper_pos is None and self.config.fallback_to_feedback:
|
||||
gripper_pos = self._read_gripper_from_feedback()
|
||||
action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos
|
||||
else:
|
||||
action["gripper.pos"] = 0.0
|
||||
gripper_pos = self._read_gripper_from_ctrl() if self.config.prefer_ctrl_messages else None
|
||||
if gripper_pos is None and self.config.fallback_to_feedback:
|
||||
gripper_pos = self._read_gripper_from_feedback()
|
||||
action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos
|
||||
return action
|
||||
|
||||
def _to_calibration_units(self, angle_deg: float) -> int:
|
||||
return int(round(angle_deg * self.config.calibration_scale))
|
||||
|
||||
def _from_calibration_units(self, value: int) -> float:
|
||||
return float(value) / float(self.config.calibration_scale)
|
||||
|
||||
def _calibrated_to_offset(self, key: str, raw_deg: float) -> float:
|
||||
cal = self.calibration[key]
|
||||
min_deg = self._from_calibration_units(cal.range_min)
|
||||
max_deg = self._from_calibration_units(cal.range_max)
|
||||
home_deg = self._from_calibration_units(cal.homing_offset)
|
||||
bounded = min(max_deg, max(min_deg, raw_deg))
|
||||
centered = bounded - home_deg
|
||||
return -centered if cal.drive_mode else centered
|
||||
|
||||
def _offset_to_calibrated(self, key: str, offset_deg: float) -> float:
|
||||
cal = self.calibration[key]
|
||||
min_deg = self._from_calibration_units(cal.range_min)
|
||||
max_deg = self._from_calibration_units(cal.range_max)
|
||||
home_deg = self._from_calibration_units(cal.homing_offset)
|
||||
centered = -offset_deg if cal.drive_mode else offset_deg
|
||||
target = home_deg + centered
|
||||
return min(max_deg, max(min_deg, target))
|
||||
|
||||
def _record_ranges_of_motion(self) -> tuple[dict[str, float], dict[str, float]]:
|
||||
current = self._read_raw_action()
|
||||
mins = current.copy()
|
||||
maxes = current.copy()
|
||||
|
||||
while True:
|
||||
current = self._read_raw_action()
|
||||
mins = {key: min(mins[key], current[key]) for key in PIPER_CALIB_KEYS}
|
||||
maxes = {key: max(maxes[key], current[key]) for key in PIPER_CALIB_KEYS}
|
||||
|
||||
print("\n-----------------------------")
|
||||
print("JOINT | MIN | POS | MAX")
|
||||
for key in PIPER_CALIB_KEYS:
|
||||
print(f"{key:<11} | {mins[key]:>6.2f} | {current[key]:>6.2f} | {maxes[key]:>6.2f}")
|
||||
|
||||
if enter_pressed():
|
||||
break
|
||||
move_cursor_up(len(PIPER_CALIB_KEYS) + 3)
|
||||
|
||||
return mins, maxes
|
||||
|
||||
@check_if_not_connected
|
||||
def get_action(self) -> RobotAction:
|
||||
if not self.is_calibrated:
|
||||
raise RuntimeError(
|
||||
f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type=piper_leader --teleop.id={self.id}` first."
|
||||
)
|
||||
|
||||
raw_action = self._read_raw_action()
|
||||
action: RobotAction = {
|
||||
key: self._calibrated_to_offset(key, raw_action[key]) for key in PIPER_CALIB_KEYS
|
||||
}
|
||||
if not self.config.sync_gripper:
|
||||
action["gripper.pos"] = 0.0
|
||||
return action
|
||||
|
||||
@check_if_not_connected
|
||||
def send_feedback(self, feedback: dict[str, Any]) -> None:
|
||||
if not self.is_calibrated:
|
||||
raise RuntimeError(
|
||||
f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type=piper_leader --teleop.id={self.id}` first."
|
||||
)
|
||||
|
||||
self.set_manual_control(False)
|
||||
self._refresh_command_mode_if_needed()
|
||||
|
||||
joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES]
|
||||
has_all_joints = all(key in feedback for key in joint_keys)
|
||||
if has_all_joints:
|
||||
joint_commands = [unit_to_milli(feedback[key]) for key in joint_keys]
|
||||
joint_targets = [self._offset_to_calibrated(key, feedback[key]) for key in joint_keys]
|
||||
joint_commands = [unit_to_milli(value) for value in joint_targets]
|
||||
self.arm.JointCtrl(*joint_commands)
|
||||
|
||||
if self.config.sync_gripper and "gripper.pos" in feedback:
|
||||
gripper_pos_raw = unit_to_milli(abs(feedback["gripper.pos"]))
|
||||
gripper_target = self._offset_to_calibrated("gripper.pos", feedback["gripper.pos"])
|
||||
gripper_pos_raw = unit_to_milli(gripper_target)
|
||||
self.arm.GripperCtrl(
|
||||
gripper_pos_raw,
|
||||
self.config.gripper_effort_default,
|
||||
|
||||
@ -3,6 +3,7 @@ from types import SimpleNamespace
|
||||
import lerobot.utils.piper_sdk as piper_sdk_utils
|
||||
import lerobot.robots.piper_follower.piper_follower as piper_follower_module
|
||||
import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module
|
||||
from lerobot.motors import MotorCalibration
|
||||
from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig
|
||||
from lerobot.robots.utils import make_robot_from_config
|
||||
from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig
|
||||
@ -114,6 +115,19 @@ def patch_fake_sdk(monkeypatch):
|
||||
monkeypatch.setattr(piper_leader_module, "get_piper_sdk", fake_loader)
|
||||
|
||||
|
||||
def make_identity_calibration():
|
||||
return {
|
||||
key: MotorCalibration(
|
||||
id=idx,
|
||||
drive_mode=0,
|
||||
homing_offset=0,
|
||||
range_min=-200000,
|
||||
range_max=200000,
|
||||
)
|
||||
for idx, key in enumerate([f"joint_{i}.pos" for i in range(1, 7)] + ["gripper.pos"])
|
||||
}
|
||||
|
||||
|
||||
def test_piper_leader_follower_teleop_roundtrip(monkeypatch):
|
||||
patch_fake_sdk(monkeypatch)
|
||||
|
||||
@ -135,8 +149,11 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch):
|
||||
assert isinstance(teleop, PiperLeader)
|
||||
assert isinstance(robot, PiperFollower)
|
||||
|
||||
teleop.connect()
|
||||
robot.connect()
|
||||
teleop.calibration = make_identity_calibration()
|
||||
robot.calibration = make_identity_calibration()
|
||||
|
||||
teleop.connect(calibrate=False)
|
||||
robot.connect(calibrate=False)
|
||||
try:
|
||||
action = teleop.get_action()
|
||||
sent = robot.send_action(action)
|
||||
@ -167,14 +184,29 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch):
|
||||
robot.disconnect()
|
||||
|
||||
|
||||
def test_piper_calibration_is_noop(monkeypatch):
|
||||
def test_piper_requires_calibration(monkeypatch):
|
||||
patch_fake_sdk(monkeypatch)
|
||||
|
||||
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
||||
robot = PiperFollower(PiperFollowerConfig(port="can0"))
|
||||
|
||||
assert teleop.is_calibrated
|
||||
assert robot.is_calibrated
|
||||
assert not teleop.is_calibrated
|
||||
assert not robot.is_calibrated
|
||||
|
||||
teleop.calibrate()
|
||||
robot.calibrate()
|
||||
teleop.connect(calibrate=False)
|
||||
robot.connect(calibrate=False)
|
||||
try:
|
||||
try:
|
||||
teleop.get_action()
|
||||
assert False, "Expected teleop.get_action() to require calibration."
|
||||
except RuntimeError:
|
||||
pass
|
||||
|
||||
try:
|
||||
robot.send_action({"joint_1.pos": 0.0})
|
||||
assert False, "Expected robot.send_action() to require calibration."
|
||||
except RuntimeError:
|
||||
pass
|
||||
finally:
|
||||
teleop.disconnect()
|
||||
robot.disconnect()
|
||||
|
||||
在新工单中引用
屏蔽一个用户