diff --git a/src/litegrip/can/motor.py b/src/litegrip/can/motor.py index 5089565..fe6d022 100644 --- a/src/litegrip/can/motor.py +++ b/src/litegrip/can/motor.py @@ -7,6 +7,7 @@ from __future__ import annotations +import time from dataclasses import dataclass, field from enum import IntEnum @@ -133,6 +134,28 @@ def is_fault(self) -> bool: def last_update(self) -> float: return self._last_update + @property + def has_data(self) -> bool: + """True once at least one status frame has been decoded. + + A disabled DM motor does not stream status frames on its own, so + before the first enable (or after a disable, once the RX buffer + drains) there is nothing to read and every value here is still the + constructor default — position 0.0 with temperatures 0/0. + """ + return self.rx_count > 0 + + @property + def data_age_s(self) -> float: + """Seconds since the last decoded status frame. + + Returns ``inf`` when no frame has ever arrived, so comparisons like + ``data_age_s > threshold`` work without a separate has-data check. + """ + if self.rx_count == 0: + return float("inf") + return max(0.0, time.monotonic() - self._last_update) + # ── control mode management ───────────────────────────────────────── def set_mode(self, mode: ControlMode) -> None: diff --git a/src/litegrip/constants.py b/src/litegrip/constants.py index 641c91d..4e54796 100644 --- a/src/litegrip/constants.py +++ b/src/litegrip/constants.py @@ -72,19 +72,25 @@ class ErrorCode: """Damiao motor error codes (extracted from status frame data[0] >> 4).""" DISABLED: Final = 0 ENABLED: Final = 1 + OV_FAULT: Final = 0x8 UV_FAULT: Final = 0x9 OC_FAULT: Final = 0xA MOS_OT: Final = 0xB COIL_OT: Final = 0xC + COMM_LOSS: Final = 0xD + OVERLOAD: Final = 0xE ERROR_DESCRIPTIONS = { 0x0: "已失能", 0x1: "已使能", + 0x8: "过压故障 (OV)", 0x9: "欠压故障 (UV)", 0xA: "过流故障 (OC)", 0xB: "MOS 过温故障", 0xC: "线圈过温故障", + 0xD: "通讯丢失 (CAN 超时)", + 0xE: "过载故障", } diff --git a/src/litegrip/gripper.py b/src/litegrip/gripper.py index a52e516..b158bc1 100644 --- a/src/litegrip/gripper.py +++ b/src/litegrip/gripper.py @@ -54,6 +54,7 @@ _os.path.join(_os.path.expanduser("~"), ".litegrip", "litegrip_calibration.json"), ) from .models import ( + STALE_AFTER_S, GripperState, GripperConfig, GripperInfo, @@ -1294,6 +1295,12 @@ def _find_limit(direction: str) -> float: def get_state(self, wait: bool = True) -> GripperState: """Return the current gripper state. + A disabled motor does not stream status frames on its own, so when no + fresh frame arrives the returned snapshot is the last decoded one — or + constructor defaults (position 0.0, temperatures 0/0) if none ever + arrived. Check :attr:`GripperState.is_stale` / ``data_age_s`` before + trusting the readings, or send a refresh frame first. + Args: wait: If True (default), waits up to 50 ms for a fresh status frame. If False, returns immediately with the last cached @@ -1311,6 +1318,16 @@ def get_state(self, wait: bool = True) -> GripperState: else: self._can.poll(timeout_s=0.0) + motor = self._can.motor + data_age_s = motor.data_age_s if motor is not None else float("inf") + if wait and data_age_s > STALE_AFTER_S: + log.warning( + "get_state(): 未收到新状态帧（%s）——返回的是缓存/默认值，" + "不是当前测量。失能状态的电机不主动发状态帧。", + "从未收到" if data_age_s == float("inf") + else f"最近一帧 {data_age_s:.2f}s 前", + ) + position_rad = self._can.get_position() velocity_rad_s = self._can.get_velocity() torque_nm = self._can.get_torque() @@ -1330,10 +1347,28 @@ def get_state(self, wait: bool = True) -> GripperState: temperature_coil=t_coil, error_code=error_code, timestamp=time.time(), + data_age_s=data_age_s, position_mm=position_mm, force_n=force_n, ) + def refresh_status(self, timeout_s: float = 0.5) -> bool: + """Request a status frame from the motor, even while disabled. + + A disabled motor does not stream status frames, so :meth:`get_state` + keeps returning cached values (or zeros, before the first enable). + This sends the 0xCC refresh command — which the motor answers + regardless of enable state — and waits for the reply. Useful for + reading the position before enabling. No motion, no output change. + + Returns: + True if a fresh status frame arrived. + """ + self._check_connected() + if self._can is None: + return False + return self._can.refresh_status(timeout_s=timeout_s) + def get_position(self) -> float: """Current position in mm.""" return self.get_state().position_mm diff --git a/src/litegrip/models.py b/src/litegrip/models.py index d151397..0eee19e 100644 --- a/src/litegrip/models.py +++ b/src/litegrip/models.py @@ -7,6 +7,11 @@ import time from typing import Optional +# A status frame older than this is treated as no longer representing the +# present. DM motors emit status at ~10 Hz while enabled, so 0.5 s is ~5 +# missed frames; the motor's own CAN-timeout fault trips at ~0.9 s. +STALE_AFTER_S = 0.5 + class GripperMode(IntEnum): """Gripper control mode.""" @@ -41,10 +46,26 @@ class GripperState: error_code: int = 0 timestamp: float = field(default_factory=time.time) + # Age of the status frame these values came from; inf = never received. + # A disabled motor does not stream status frames, so a state read before + # the first enable (or after a disable) holds constructor defaults, not + # measurements — check this before trusting position/force/temperature. + data_age_s: float = float("inf") + # Convenience — computed from raw values with unit conversion position_mm: float = 0.0 force_n: float = 0.0 + @property + def has_data(self) -> bool: + """True if at least one status frame has been decoded.""" + return self.data_age_s != float("inf") + + @property + def is_stale(self) -> bool: + """True if the snapshot is not backed by a recent status frame.""" + return self.data_age_s > STALE_AFTER_S + @property def is_enabled(self) -> bool: """True if the motor is enabled (error_code == 1).""" diff --git a/src/litegrip/protocols/can_bus.py b/src/litegrip/protocols/can_bus.py index 5c36a60..6f77bb2 100644 --- a/src/litegrip/protocols/can_bus.py +++ b/src/litegrip/protocols/can_bus.py @@ -138,13 +138,110 @@ def register_gripper( except Exception as e: raise CommError(f"注册夹爪电机失败: {e}") + # ═══════════════════════════════════════════════════════════════════ + # Enable window + # ═══════════════════════════════════════════════════════════════════ + # + # In MIT mode the motor continuously executes the last target frame it + # received: τ = kp·(q_target − q) + kd·(dq_target − dq) + tau_ff, with + # every one of those five values coming from that frame. The 0xFC enable + # command carries no target of its own and does not clear the target + # registers — it only re-engages the control loop. So the instant enable + # takes effect, a motor left over from a previous session would resume + # driving toward *that* session's target (e.g. you Ctrl+C'd mid-close() + # and the register still holds q=closed, kp=100). + # + # These two helpers close that window: stream zero-gain frames over it, + # then leave the motor holding wherever it actually is. + + def _hold_at_current(self, kp: float, kd: float, + duration_s: float = 0.05) -> bool: + """Stream hold-position frames at the motor's current position. + + Call this only after a fresh status frame has been decoded — the + MotorState default position is 0.0, and holding at 0.0 with a real + gain would drive the gripper to a bogus target. + + Leaving "hold where you are" as the last target also makes the *next* + enable inherently safe, since that is what the motor resumes from. + + Returns: + True if the hold frames were streamed. + """ + if self._controller is None or self._motor is None: + return False + if self._motor.rx_count == 0: + log.warning("Refusing to hold position: no status frame decoded yet") + return False + return self.control_mit_stream( + q_target=self._motor.position, kp=kp, kd=kd, + duration_s=duration_s) + + def _enable_and_hold(self, kp: float, kd: float, + timeout_s: float = DefaultParams.INIT_TIMEOUT_S + ) -> Optional[int]: + """Enable the motor and leave it holding its current position. + + Sequence: 0xFC enable → streamed zero-gain frames → wait for a fresh + status frame → hold at the freshly-read position with *kp*/*kd*. + + Returns: + The error code from the first fresh status frame, or None if no + feedback arrived within *timeout_s*. + """ + if self._controller is None or self._motor is None: + return None + + ctrl = self._controller + m = self._motor + prev_rx = m.rx_count + + ctrl.enable(m) + + # Zero-gain cover. Streamed rather than a single frame: everything + # else in this SDK streams for the same reason, and here one lost + # frame means the motor keeps running the stale target indefinitely + # rather than for ~10 ms. + self.control_mit_stream(q_target=0.0, kp=0.0, kd=0.0, + duration_s=0.05, interval_s=0.005) + + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + # Keep feeding the motor while we wait. It is ENABLED from the + # 0xFC above, and an enabled motor that hears nothing for ~900 ms + # latches the 0xD comm-loss fault (measured on hardware) — which + # is exactly the fault this wait is supposed to detect, so silence + # here manufactures the failure it is looking for. The 0.05 s + # burst before the loop is not long enough to cover a 2 s wait. + ctrl.control_mit(m, kp=0, kd=0, q=0, dq=0, tau=0) + ctrl.poll(timeout_s=0.01) + if m.rx_count > prev_rx: + break + time.sleep(0.005) + else: + return None + + # rx_count only advances on a decoded status frame, so m.position is a + # real reading from here on. + self._hold_at_current(kp, kd) + return m.error + # ═══════════════════════════════════════════════════════════════════ # Initialization / enable / disable # ═══════════════════════════════════════════════════════════════════ - def initialize(self) -> bool: + def initialize(self, kp: Optional[float] = None, + kd: Optional[float] = None) -> bool: """Full initialization: disable → switch to MIT → enable → verify feedback. + Once enabled, the motor is left holding its current position (see + :meth:`_enable_and_hold`) rather than outputting zero torque, so it + cannot jump toward whatever target the previous session left behind. + + Args: + kp: Stiffness used for the post-enable hold (default DEFAULT_KP). + kd: Damping used for the post-enable hold (default DEFAULT_KD). + Returns: True if the motor reports ``err == 1`` (enabled). ``err == 0`` means the enable command did not take effect — the attempt is @@ -159,6 +256,8 @@ def initialize(self) -> bool: ctrl = self._controller m = self._motor + kp = kp if kp is not None else GripperParams.DEFAULT_KP + kd = kd if kd is not None else GripperParams.DEFAULT_KD last_err = -1 for attempt in range(GripperParams.FAULT_CLEAR_RETRIES): @@ -172,37 +271,28 @@ def initialize(self) -> bool: log.warning("switchControlMode verification failed; proceeding anyway") time.sleep(0.05) - # 3. Enable - ctrl.enable(m) - - # 4. Send zero-torque MIT frame and poll for feedback - prev_rx = m.rx_count - ctrl.control_mit(m, kp=0, kd=0, q=0, dq=0, tau=0) - - deadline = time.monotonic() + DefaultParams.INIT_TIMEOUT_S - while time.monotonic() < deadline: - ctrl.poll(timeout_s=0.01) - if m.rx_count > prev_rx: - err = m.error - if err == 1: - self._initialized = True - return True - # err == 0 → the enable frame was lost; retry. - # anything else → a real fault; retry after clearing. - last_err = err - break - time.sleep(0.005) - else: + # 3+4. Enable, cover the window, verify feedback, then hold + # at the freshly-read position. + err = self._enable_and_hold(kp, kd) + + if err is None: last_err = -2 # timeout, no feedback + elif err == 1: + self._initialized = True + return True + else: + # err == 0 → the enable frame was lost; retry. + # anything else → a real fault; retry after clearing. + last_err = err except Exception: last_err = -3 - # Retry: clear fault before next attempt + # Retry: clear the fault before the next attempt. No bare + # enable() here — the next iteration disables immediately, and an + # uncovered enable is exactly the window this method avoids. if attempt < GripperParams.FAULT_CLEAR_RETRIES - 1: ctrl.clear_fault(m) - time.sleep(0.005) - ctrl.enable(m) time.sleep(0.01) # All retries exhausted → raise @@ -222,15 +312,21 @@ def initialize(self) -> bool: else: raise HardwareError("初始化失败：所有重试耗尽") - def enable(self) -> bool: - """Send enable command only (no full init). Prefer initialize().""" + def enable(self, kp: Optional[float] = None, + kd: Optional[float] = None) -> bool: + """Send enable command only (no full init). Prefer initialize(). + + The motor is left holding its current position once enabled — see + :meth:`_enable_and_hold`. + """ if self._controller is None or self._motor is None: return False + kp = kp if kp is not None else GripperParams.DEFAULT_KP + kd = kd if kd is not None else GripperParams.DEFAULT_KD try: - self._controller.enable(self._motor) - self._controller.control_mit(self._motor, kp=0, kd=0, q=0) - self._controller.poll(timeout_s=0.05) - return self._motor.error in (0, 1) + # None (no feedback) fails the membership test, so a silent motor + # is no longer reported as a successful enable. + return self._enable_and_hold(kp, kd) in (0, 1) except Exception: return False @@ -253,33 +349,32 @@ def _clear_fault(self) -> bool: """[Deprecated] Use :meth:`clear_fault` instead.""" return self.clear_fault() - def clear_fault(self) -> bool: + def clear_fault(self, kp: Optional[float] = None, + kd: Optional[float] = None) -> bool: """Clear latched fault and re-enable the motor. Tries two strategies: 1. Direct clear (0xFB) + enable (0xFC) — works for UV/OC/OT faults where the motor is already effectively disabled. 2. Full disable → clear → enable cycle. + + Both re-enable through :meth:`_enable_and_hold`, so the motor ends up + holding its current position instead of resuming a stale target, and a + motor that never answers counts as a failure rather than a success. """ if self._controller is None or self._motor is None: return False ctrl = self._controller m = self._motor + kp = kp if kp is not None else GripperParams.DEFAULT_KP + kd = kd if kd is not None else GripperParams.DEFAULT_KD for _ in range(GripperParams.FAULT_CLEAR_RETRIES): # Strategy 1: Direct clear + enable (proven for UV_FAULT) ctrl.clear_fault(m) time.sleep(0.005) - ctrl.enable(m) - time.sleep(0.01) - - # Verify - ctrl.control_mit(m, kp=0, kd=0, q=0) - for _ in range(20): - ctrl.poll(timeout_s=0.01) - time.sleep(0.003) - if m.error in (0, 1): + if self._enable_and_hold(kp, kd) in (0, 1): return True # Strategy 2: Full disable → clear → enable @@ -287,14 +382,7 @@ def clear_fault(self) -> bool: time.sleep(0.01) ctrl.clear_fault(m) time.sleep(0.01) - ctrl.enable(m) - time.sleep(0.01) - - ctrl.control_mit(m, kp=0, kd=0, q=0) - for _ in range(20): - ctrl.poll(timeout_s=0.01) - time.sleep(0.003) - if m.error in (0, 1): + if self._enable_and_hold(kp, kd) in (0, 1): return True return False @@ -371,6 +459,31 @@ def update_state(self, timeout_s: float = 0.05) -> bool: return False return self._controller.poll_until(self._motor, timeout_s=timeout_s) + def refresh_status(self, timeout_s: float = 0.5) -> bool: + """Request a status frame and wait for it (0xCC refresh command). + + A disabled motor does not stream status frames on its own, so + :meth:`update_state` finds nothing and callers read stale/default + values. The refresh command is answered regardless of enable state, + which makes the position readable before the first enable. Sends no + motion command and changes no motor output. + + Returns: + True if a fresh status frame arrived within *timeout_s*. + """ + if self._controller is None or self._motor is None: + return False + m = self._motor + prev_rx = m.rx_count + self._controller.refresh_status(m) + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + self._controller.poll(timeout_s=0.01) + if m.rx_count > prev_rx: + return True + time.sleep(0.005) + return False + def get_position(self) -> float: """Current position in rad.""" if self._motor is None: diff --git a/tests/fake_can.py b/tests/fake_can.py index a726771..4e05462 100644 --- a/tests/fake_can.py +++ b/tests/fake_can.py @@ -76,6 +76,10 @@ def __init__(self, pos: float = 0.0, block_rad: Optional[float] = None, self._block_dir = 0.0 if block_rad is not None: self._block_dir = 1.0 if block_rad > pos else -1.0 + # 状态帧的年龄。假电机一直在线上，所以取 0（= 新鲜）；要模拟 + # 「失能电机不发状态帧」的用例，把这个值调大即可（见 + # ``GripperState.is_stale`` / ``STALE_AFTER_S``）。 + self.data_age_s = 0.0 def reported_pos(self) -> float: """上报位置（可量化 —— 模拟闭合侧约 0.0103 rad 的粘滑死区）。""" diff --git a/tests/test_enable.py b/tests/test_enable.py index 500cb59..a078673 100644 --- a/tests/test_enable.py +++ b/tests/test_enable.py @@ -15,12 +15,18 @@ class FakeMotorState: - """``MotorState`` 的最小替身（initialize 只碰这三个字段）。""" + """``MotorState`` 的最小替身。 + + ``initialize`` 使能后会把电机保持在**实测位置**上（见 + ``can_bus._enable_and_hold``），所以除了判定的三个字段，还要给出 + ``position`` —— 真 ``MotorState`` 一直都有。 + """ def __init__(self): self.rx_count = 0 self.error = 0 self.mst_id = 0x18 + self.position = 0.0 class FakeController: @@ -45,6 +51,10 @@ def disable(self, motor): def enable(self, motor): self.calls.append("enable") + # 错误码按 **enable 次数** 推进，而不是按 poll 次数：脚本描述的正是 + # 「第几次使能有没有生效」。_enable_and_hold 使能后会先流一小段零增益 + # 帧、期间 poll 多次，按 poll 推进会让脚本在那段覆盖里就被吃光。 + self.motor.error = self._next_err() def clear_fault(self, motor): self.calls.append("clear_fault") @@ -57,8 +67,8 @@ def control_mit(self, motor, kp, kd, q, dq=0.0, tau=0.0): self.calls.append("control_mit") def poll(self, timeout_s=0.0): + # 一次 poll 解出一帧状态；错误码由 enable 决定（见上）。 self.motor.rx_count += 1 - self.motor.error = self._next_err() return self.motor def close(self): diff --git a/tests/test_stale_state.py b/tests/test_stale_state.py new file mode 100644 index 0000000..2dc3aa2 --- /dev/null +++ b/tests/test_stale_state.py @@ -0,0 +1,132 @@ +"""陈旧状态与刷新帧：失能电机不主动发状态帧，读到的可能是缓存/默认值。 + +``MotorState`` / ``GripperState`` 现在带 ``data_age_s`` 与 ``is_stale``， +``refresh_status()`` 给出「主动要一帧」的路径 —— 失能状态下也能读到位置， +不必为了读位置先使能（使能本身可能让电机朝上一会话遗留的目标运动）。 +""" + +from __future__ import annotations + +import time +import unittest + +import _sdkpath # noqa: F401 +from litegrip.can.motor import MotorParams, MotorState +from litegrip.constants import describe_error +from litegrip.models import STALE_AFTER_S, GripperState +from litegrip.protocols.can_bus import LiteGripCAN + + +def _motor_with_frame(age_s: float = 0.0) -> MotorState: + """一个「刚收到一帧（或 age_s 秒前收到过一帧）」的 MotorState。""" + m = MotorState(MotorParams()) + m.update_from_status(position=0.5, velocity=0.0, torque=0.0, + error=1, t_mos=30, t_coil=31, + timestamp=time.monotonic() - age_s) + return m + + +class TestMotorStateFreshness(unittest.TestCase): + + def test_never_received_has_infinite_age_and_no_data(self): + m = MotorState(MotorParams()) + self.assertFalse(m.has_data) + self.assertEqual(m.data_age_s, float("inf")) + + def test_a_status_frame_makes_it_fresh_and_have_data(self): + m = _motor_with_frame() + self.assertTrue(m.has_data) + self.assertLess(m.data_age_s, STALE_AFTER_S) + + def test_age_is_measured_from_the_frame(self): + # 有数据，但已经旧了 —— 这两件事必须能分开判断：失能电机的典型 + # 状态就是「读到过，但此刻不新鲜」。 + m = _motor_with_frame(age_s=5.0) + self.assertTrue(m.has_data) + self.assertGreater(m.data_age_s, 4.0) + + +class TestGripperStateFreshness(unittest.TestCase): + + def test_default_state_has_no_data_and_is_stale(self): + s = GripperState() + self.assertFalse(s.has_data) + self.assertTrue(s.is_stale) + + def test_fresh_state_is_not_stale(self): + s = GripperState(data_age_s=0.0) + self.assertTrue(s.has_data) + self.assertFalse(s.is_stale) + + def test_threshold_is_exclusive(self): + self.assertFalse(GripperState(data_age_s=STALE_AFTER_S).is_stale) + self.assertTrue(GripperState(data_age_s=STALE_AFTER_S + 0.001).is_stale) + + +class TestCommLossIsNamed(unittest.TestCase): + """0xD 是 DM4310 在 CAN 静默约 900 ms 后 latch 的状态。 + + 未使能、或刚失能的那段时间就会看到它，所以它不能报成「未知错误」—— + 那会把一个正常状态当成硬件故障来排查。 + """ + + def test_comm_loss_has_a_description(self): + self.assertIn("通讯", describe_error(0xD)) + + def test_overvoltage_and_overload_have_descriptions(self): + self.assertIn("过压", describe_error(0x8)) + self.assertIn("过载", describe_error(0xE)) + + def test_unknown_codes_still_say_unknown(self): + self.assertIn("未知", describe_error(0x7F)) + + +class FakeRefreshController: + """只实现 ``refresh_status`` / ``poll`` 的最小控制器。""" + + def __init__(self, motor: MotorState, answer: bool): + self.motor = motor + self.answer = answer + self.refresh_calls = 0 + + def refresh_status(self, motor: MotorState) -> None: + self.refresh_calls += 1 + + def poll(self, timeout_s: float = 0.0): + if self.answer: + self.motor.update_from_status( + position=0.25, velocity=0.0, torque=0.0, + error=0, t_mos=30, t_coil=31, + timestamp=time.monotonic()) + return self.motor + + +def make_can(answer: bool = True): + can = LiteGripCAN(channel="vcan0") + motor = MotorState(MotorParams()) + can._motor = motor + can._controller = FakeRefreshController(motor, answer) + can._connected = True + return can, motor + + +class TestRefreshStatus(unittest.TestCase): + + def test_reads_a_position_while_disabled(self): + can, motor = make_can() + self.assertFalse(motor.has_data) # 还没使能，一帧都没有 + self.assertTrue(can.refresh_status(timeout_s=0.2)) + self.assertTrue(motor.has_data) + + def test_reports_failure_when_the_motor_is_silent(self): + can, _ = make_can(answer=False) + self.assertFalse(can.refresh_status(timeout_s=0.1)) + + def test_sends_the_refresh_command_and_no_motion(self): + can, _ = make_can() + can.refresh_status(timeout_s=0.2) + self.assertEqual(can._controller.refresh_calls, 1) + + +if __name__ == "__main__": + unittest.main()