镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
Revert "fix(piper): robust ctrl_mode parsing for enum/string values"
This reverts commit 2190f5c237.
这个提交包含在:
@ -16,10 +16,7 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
from functools import lru_cache
|
||||
from typing import Any
|
||||
|
||||
@ -35,13 +32,6 @@ PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES)
|
||||
PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",)
|
||||
PIPER_CTRL_MODE_TEACH = 0x02
|
||||
PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT = 0x06
|
||||
_CTRL_MODE_HEX_RE = re.compile(r"0x([0-9a-fA-F]+)")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PiperCtrlMode:
|
||||
code: int | None
|
||||
text: str
|
||||
|
||||
|
||||
def milli_to_unit(value: float | int) -> float:
|
||||
@ -89,54 +79,27 @@ def wait_enable_piper(arm: Any, timeout_s: float, retry_interval_s: float = 0.2)
|
||||
return False
|
||||
|
||||
|
||||
def parse_piper_ctrl_mode(value: Any) -> PiperCtrlMode:
|
||||
if value is None:
|
||||
return PiperCtrlMode(code=None, text="None")
|
||||
|
||||
def _piper_mode_to_int(value: Any) -> int | None:
|
||||
if isinstance(value, bool):
|
||||
code = int(value)
|
||||
return PiperCtrlMode(code=code, text=f"0x{code:02X}")
|
||||
|
||||
return int(value)
|
||||
if isinstance(value, int):
|
||||
return PiperCtrlMode(code=value, text=f"0x{value:02X}")
|
||||
|
||||
if isinstance(value, Enum):
|
||||
enum_value = value.value
|
||||
if isinstance(enum_value, int):
|
||||
return PiperCtrlMode(code=enum_value, text=str(value))
|
||||
if isinstance(enum_value, str):
|
||||
m = _CTRL_MODE_HEX_RE.search(enum_value)
|
||||
if m:
|
||||
return PiperCtrlMode(code=int(m.group(1), 16), text=str(value))
|
||||
if enum_value.isdigit():
|
||||
return PiperCtrlMode(code=int(enum_value), text=str(value))
|
||||
return PiperCtrlMode(code=None, text=str(value))
|
||||
|
||||
raw_value = getattr(value, "value", None)
|
||||
if isinstance(raw_value, int):
|
||||
return PiperCtrlMode(code=raw_value, text=str(value))
|
||||
|
||||
if isinstance(value, str):
|
||||
m = _CTRL_MODE_HEX_RE.search(value)
|
||||
if m:
|
||||
return PiperCtrlMode(code=int(m.group(1), 16), text=value)
|
||||
if value.isdigit():
|
||||
code = int(value)
|
||||
return PiperCtrlMode(code=code, text=f"0x{code:02X}")
|
||||
return PiperCtrlMode(code=None, text=value)
|
||||
|
||||
return PiperCtrlMode(code=None, text=repr(value))
|
||||
return value
|
||||
if hasattr(value, "value") and isinstance(value.value, int):
|
||||
return int(value.value)
|
||||
if hasattr(value, "__int__"):
|
||||
return int(value)
|
||||
return None
|
||||
|
||||
|
||||
def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> PiperCtrlMode | None:
|
||||
def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> int | None:
|
||||
deadline = time.monotonic() + max(0.0, timeout_s)
|
||||
while time.monotonic() < deadline:
|
||||
status_msg = arm.GetArmStatus()
|
||||
if getattr(status_msg, "time_stamp", 0.0) > 0.0:
|
||||
arm_status = getattr(status_msg, "arm_status", None)
|
||||
if arm_status is not None:
|
||||
mode = parse_piper_ctrl_mode(getattr(arm_status, "ctrl_mode", None))
|
||||
if mode.code is not None:
|
||||
mode = _piper_mode_to_int(getattr(arm_status, "ctrl_mode", None))
|
||||
if mode is not None:
|
||||
return mode
|
||||
time.sleep(max(0.005, poll_s))
|
||||
return None
|
||||
@ -156,18 +119,11 @@ def guard_piper_ctrl_mode_on_connect(
|
||||
f"[{interface_name}] could not read arm ctrl_mode within {timeout_s:.2f}s. "
|
||||
"Check CAN wiring/power and rerun."
|
||||
)
|
||||
|
||||
if mode.code is None:
|
||||
raise RuntimeError(
|
||||
f"[{interface_name}] got unreadable arm ctrl_mode value: {mode.text}. "
|
||||
"Check SDK version/firmware and rerun."
|
||||
)
|
||||
|
||||
if mode.code in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}:
|
||||
if mode in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}:
|
||||
arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
||||
if settle_s > 0:
|
||||
time.sleep(settle_s)
|
||||
raise RuntimeError(
|
||||
f"[{interface_name}] arm is in master/teaching role (ctrl_mode={mode.text}). "
|
||||
f"[{interface_name}] arm is in master/teaching role (ctrl_mode=0x{mode:02X}). "
|
||||
"Follower role command (0xFC) has been sent. Power-cycle this arm, then retry."
|
||||
)
|
||||
|
||||
在新工单中引用
屏蔽一个用户