镜像自地址
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
|
from __future__ import annotations
|
||||||
|
|
||||||
import re
|
|
||||||
import time
|
import time
|
||||||
from dataclasses import dataclass
|
|
||||||
from enum import Enum
|
|
||||||
from functools import lru_cache
|
from functools import lru_cache
|
||||||
from typing import Any
|
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_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",)
|
||||||
PIPER_CTRL_MODE_TEACH = 0x02
|
PIPER_CTRL_MODE_TEACH = 0x02
|
||||||
PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT = 0x06
|
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:
|
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
|
return False
|
||||||
|
|
||||||
|
|
||||||
def parse_piper_ctrl_mode(value: Any) -> PiperCtrlMode:
|
def _piper_mode_to_int(value: Any) -> int | None:
|
||||||
if value is None:
|
|
||||||
return PiperCtrlMode(code=None, text="None")
|
|
||||||
|
|
||||||
if isinstance(value, bool):
|
if isinstance(value, bool):
|
||||||
code = int(value)
|
return int(value)
|
||||||
return PiperCtrlMode(code=code, text=f"0x{code:02X}")
|
|
||||||
|
|
||||||
if isinstance(value, int):
|
if isinstance(value, int):
|
||||||
return PiperCtrlMode(code=value, text=f"0x{value:02X}")
|
return value
|
||||||
|
if hasattr(value, "value") and isinstance(value.value, int):
|
||||||
if isinstance(value, Enum):
|
return int(value.value)
|
||||||
enum_value = value.value
|
if hasattr(value, "__int__"):
|
||||||
if isinstance(enum_value, int):
|
return int(value)
|
||||||
return PiperCtrlMode(code=enum_value, text=str(value))
|
return None
|
||||||
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))
|
|
||||||
|
|
||||||
|
|
||||||
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)
|
deadline = time.monotonic() + max(0.0, timeout_s)
|
||||||
while time.monotonic() < deadline:
|
while time.monotonic() < deadline:
|
||||||
status_msg = arm.GetArmStatus()
|
status_msg = arm.GetArmStatus()
|
||||||
if getattr(status_msg, "time_stamp", 0.0) > 0.0:
|
if getattr(status_msg, "time_stamp", 0.0) > 0.0:
|
||||||
arm_status = getattr(status_msg, "arm_status", None)
|
arm_status = getattr(status_msg, "arm_status", None)
|
||||||
if arm_status is not None:
|
if arm_status is not None:
|
||||||
mode = parse_piper_ctrl_mode(getattr(arm_status, "ctrl_mode", None))
|
mode = _piper_mode_to_int(getattr(arm_status, "ctrl_mode", None))
|
||||||
if mode.code is not None:
|
if mode is not None:
|
||||||
return mode
|
return mode
|
||||||
time.sleep(max(0.005, poll_s))
|
time.sleep(max(0.005, poll_s))
|
||||||
return None
|
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. "
|
f"[{interface_name}] could not read arm ctrl_mode within {timeout_s:.2f}s. "
|
||||||
"Check CAN wiring/power and rerun."
|
"Check CAN wiring/power and rerun."
|
||||||
)
|
)
|
||||||
|
if mode in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}:
|
||||||
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}:
|
|
||||||
arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
||||||
if settle_s > 0:
|
if settle_s > 0:
|
||||||
time.sleep(settle_s)
|
time.sleep(settle_s)
|
||||||
raise RuntimeError(
|
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."
|
"Follower role command (0xFC) has been sent. Power-cycle this arm, then retry."
|
||||||
)
|
)
|
||||||
|
|||||||
在新工单中引用
屏蔽一个用户