fix: judge arrival from the frame-to-frame position delta

src/litearm/_protocol.py

@@ -222,6 +222,15 @@
 #: ⚠ 跨仓常量 —— 由 tests/test_protocol_sync.py 直接解析固件 joint_cfg.h 断言其一致。
 BENCH_MODEL_AXIS = 5
 
+#: 固件**状态帧频率** (Hz)。`RSP_STATUS` 是被动连续流, 固定 100 Hz (10 ms) ——
+#: 见 `arm._Ack._on_status` 的说明与状态帧布局文档。实测 `get_state().hz` = 99.99。
+#:
+#: ⚠ 用途: 把**速度阈**换算成**每帧位移阈**。`_arrive` 与 `cart._wait_settled` 的
+#: 静止判据现在都读"相邻两帧的位置差"而不是上报的 `dq` (理由见 `Arm._arrive`),
+#: 换算靠本常量。
+STATUS_HZ = 100.0
+
+
 MODE_ZERO_G = 7
 MODE_NAMES = {0: "INIT", 1: "MOVE_J", 2: "MOVE_P", 3: "MOVE_JS",
               4: "MOVE_MIT", 5: "MIT_ALL", 6: "EMERGENCY", 7: "ZERO_G"}

src/litearm/arm.py

@@ -1890,19 +1890,40 @@ def _arrive(self, target: Sequence[float],
         ⚠ **理论上**多了一个"两拍之间的瞬时故障"的口子（今天逐帧判、新机制采样）。
         固件的故障主要是**锁存**的（`reset` 清不掉的那一类）⇒ 实际不构成风险；
         **但"哪些故障可能瞬时"没有核过**（见 `docs/reports/READ_DESIGN.md` §七）。
+
+        ⚠⚠ **[2026-10-10] 静止判据不再读上报的 `dq`, 改读"相邻两帧的位置差"。**
+        实测 (yd 机整臂, 固件 `Litearm1.10.0-7J`): 臂静止 **800 帧**、位置漂移
+        **0.00000 rad**, 而 **J7 上报的 `dq` 仍在 0.007~0.256 rad/s 之间摆动**
+        (量化步长恒为 0.01465), **66% 的帧超过 `dq_tol=0.1`** ⇒ 连续
+        `arrive_frames` 拍永远凑不齐 ⇒ **臂明明停在目标上 (偏差 0.0026 rad,
+        远在 `q_tol=0.03` 内) 却报 `未到位, 超时 15.0s`**。
+        同一段采样里**位置数据是干净的**, 所以改用它 —— 它直接回答"有没有动",
+        与电机内部的速度估计无关。两次复现的位形见 `tests/` 与审计记录。
+
+        ⚠ 需要**相邻两帧**才能判静止: 第一帧只登记 `prev`, 不参与判定 (故实际比
+        旧实现多用一帧, 约 10 ms)。
         """
         a = self._require()
         tgt = [float(v) for v in target]
         budget = self.move_timeout if timeout is None else float(timeout)
+        #: 静止阈的**位移**形式 = 速度阈 × 状态帧周期。状态帧 100 Hz
+        #: (`_protocol.STATUS_HZ`), 故 `dq_tol=0.10 rad/s` ⇒ `0.001 rad/帧`。
+        #: 仍由 `dq_tol` 驱动, 严格程度与旧写法 (直接比 `dq`) 等价。
+        dstep = self.dq_tol / P.STATUS_HZ
 
-        def done(st: ST.RobotState) -> bool:
+        def done(st: ST.RobotState, prev: Optional[Sequence[float]]) -> bool:
+            """到位 **且** 两帧之间没有可观测的位移。"""
             if len(st.joints) != len(tgt):
                 return False
             qs = st.q
-            return (all(abs(qs[i] - tgt[i]) < self.q_tol for i in range(len(tgt)))
-                    and all(abs(d) < self.dq_tol for d in st.dq))
+            if not all(abs(qs[i] - tgt[i]) < self.q_tol for i in range(len(tgt))):
+                return False
+            if prev is None or len(prev) != len(qs):
+                return False
+            return all(abs(qs[i] - prev[i]) < dstep for i in range(len(tgt)))
 
         n_ok = 0                              # 连续到位拍数（防到位瞬间误判/抖动）
+        prev_q: Optional[Sequence[float]] = None
         seq0 = a.status_seq
         end = time.monotonic() + budget
         with a._cond:
@@ -1914,7 +1935,8 @@ def done(st: ST.RobotState) -> bool:
                     seq0 = a.status_seq
                     if st.faulted:
                         raise MotorFaultError(f"未到位即故障: FAULT {st.fault_detail}")
-                    n_ok = n_ok + 1 if done(st) else 0
+                    n_ok = n_ok + 1 if done(st, prev_q) else 0
+                    prev_q = st.q
                     if n_ok >= self.arrive_frames:
                         return st
                 left = end - time.monotonic()

src/litearm/cart.py

@@ -1016,14 +1016,18 @@ def _wait_settled(arm, label: str, seq0: Optional[int]):
 
     ⚠ **判据是新写的, 不能复用 `_Ack.pump` / `Arm._arrive`**: 两者的 `done` 判据都要
     **目标关节向量**, 而笛卡尔路径的目标 `q` 在 PC 侧**不存在** (规划在固件里, PC 不发
-    也不做 IK) ⇒ 这里的 `q_tol` 语义随之从"与**目标**的差"改成"与**上一帧**的差"
-    (数值沿用 `0.03`)。取帧走 `Arm._read_status` (它内部就是 `_read_one`), 上限沿用
+    也不做 IK) ⇒ 这里的"静止"只能比**相邻两帧** (位置差 < `dq_tol / STATUS_HZ`,
+    见下条), 没有目标可比。取帧走 `Arm._read_status` (它内部就是 `_read_one`), 上限沿用
     `Arm.move_timeout` (不新造旋钮)。
 
-    ⚠ **双判据缺一不可** (与 `Arm._arrive` 的 `done()` 同款, 两处都必须是这两条):
-    相邻两帧的 q 逐轴差 < `q_tol` 连续 `arrive_frames` 帧 **且** `dq` 的 max-norm <
-    `dq_tol`。只抄 `q_tol` 一半会宽松得多 —— 静态保持下 `dq` 的抖动足以把"还在爬行"
-    判成"停稳"。
+    ⚠ **[2026-10-10] 静止判据只有一条**: 相邻两帧的 q 逐轴差 < `dq_tol / STATUS_HZ`,
+    连续 `arrive_frames` 帧 (与 `Arm._arrive` 的 `done()` 同款, **两处必须同步改**)。
+    旧写法是"`dq` 的 max-norm < `dq_tol` **且** 帧间差 < `q_tol`", 其中 **`dq` 那条
+    是绑定项** (0.1 rad/s 在 100 Hz 下蕴含帧间差 < 0.001 rad, 远小于 0.03), 故换成
+    等价位移后**严格程度不变**, 唯一变化是不再相信上报的 `dq`。
+    理由: 实测 (yd 机整臂, `Litearm1.10.0-7J`) 臂静止 800 帧、位置漂移 0.00000 rad,
+    而 **J7 上报的 `dq` 仍摆到 0.256 rad/s, 66% 的帧超 `dq_tol`** —— 叠着它会让
+    "停稳"永远不成立 (臂停在目标上却报 `未到位, 超时`)。
 
     ⚠⚠ **新鲜度闸** (`seq0` + `(st.seq - seq0) & 0xFFFF ∈ (0, 32768)`, u16 回绕安全):
     只信"命令发出之后生成"的状态帧。**陈旧帧直接丢弃: 既不判到位, 也不判故障** ——
@@ -1080,6 +1084,10 @@ def _wait_settled(arm, label: str, seq0: Optional[int]):
     """
     budget = arm.move_timeout
     end = time.monotonic() + budget
+    #: 静止阈的**位移**形式 = 速度阈 × 状态帧周期 (见 `_protocol.STATUS_HZ`,
+    #: 固定 100 Hz)。与 `Arm._arrive` 同款 —— 旧写法里 `dq < dq_tol` 本就蕴含
+    #: 每帧位移 < 这个值, 故换过来严格程度不变。
+    dstep = arm.dq_tol / P.STATUS_HZ
     started_busy = False
     prev_q: Optional[List[float]] = None
     #: 判到位的那几帧的 q (`arrive_frames` 帧为窗, 静止判据一破就清空重来) ——
@@ -1113,12 +1121,13 @@ def _wait_settled(arm, label: str, seq0: Optional[int]):
         if st.cart_busy:
             started_busy = True
         q = list(st.q)
-        # ⚠ 无前一帧时**不能**短路掉 `dq` 那半边 (起点那帧一样要过 `dq_tol`): 让一个
-        # `dq` 超容差的帧当上静止窗的起点, 就等于窗里少判了一帧。
-        still = (all(abs(d) < arm.dq_tol for d in st.dq)
-                 and (prev_q is None or (len(prev_q) == len(q)
-                      and all(abs(q[i] - prev_q[i]) < arm.q_tol
-                              for i in range(len(q))))))
+        # ⚠ [2026-10-10] 静止判据**只剩相邻两帧的位置差**, 不再叠加 `dq` ——
+        # 与 `Arm._arrive` 的 `done()` 同款, 两处必须同步改 (理由见那里的长注释:
+        # 实测 J7 静止时上报的 `dq` 会摆到 0.256 rad/s、66% 的帧超 `dq_tol`,
+        # 叠着它"停稳"永远不成立 ⇒ 臂停在目标上却报超时)。
+        # 起点那帧没有"上一帧"可差, 但仍算一帧 (与 `_arrive` 的 `n_ok` 同款)。
+        still = (prev_q is None or (len(prev_q) == len(q)
+                 and all(abs(q[i] - prev_q[i]) < dstep for i in range(len(q)))))
         # 窗 = 本段**连续静止**的帧 (起点那帧没有"上一帧"可差, 但它不破坏连续性: 与
         # `_arrive` 的 `n_ok` 同款 —— 第一帧就算数); 一动就清空重来。
         window = (window + [q])[-arm.arrive_frames:] if still else []

tests/test_cart_wait.py

@@ -234,24 +234,34 @@ def test_the_degrade_branch_settles_and_is_not_a_failure(cart_arm):
 
 
 # ---------------------------------------------------------------------------
-# 2. `q` 静止判据是**双判据** —— 只抄 `q_tol` 一半会宽松得多
+# 2. `q` 静止判据 —— [2026-10-10] 从"`dq` + 帧间差"两条改为**只看帧间差**
 # ---------------------------------------------------------------------------
 
-def test_bit10_falling_to_zero_is_not_enough_without_a_static_q(cart_arm):
-    """⚠ `bit10` 落 0 那一刻臂可能**还在爬** —— 主判据必须**同时**要求 `q` 静止。
+def test_a_reported_dq_over_tolerance_does_not_block_settling(cart_arm):
+    """⚠ [2026-10-10] **判据改向**: 上报的 `dq` 不再参与静止判定, 只信相邻两帧的位置差。
 
-    这里让 `q` 一直不动而 `dq` 一直超容差 (`0.5 > dq_tol=0.10`): 桩的 `bit10` 帧序照旧
-    `1→0`, 但 `dq` 那条判据永远不成立 ⇒ 到位等待只能耗满 `move_timeout`。
-    只抄 `q_tol` 一半的实现会在第 8 帧报"到位"。
+    旧写法要求 `dq` 的 max-norm < `dq_tol` **且** 帧间差 < `q_tol`。但实测
+    (yd 机整臂, 固件 `Litearm1.10.0-7J`): 臂静止 **800 帧**、位置漂移
+    **0.00000 rad**, 而 **J7 上报的 `dq` 摆到 0.256 rad/s, 66% 的帧超 `dq_tol`**
+    ⇒ 旧判据下"停稳"永远不成立: **臂明明停在目标上却报 `未到位, 超时`**。
+    位置数据在同一段采样里是干净的, 分辨率也够 (0.383 mrad/帧量化 ≈ 0.038 rad/s,
+    比 `dq_tol=0.1 rad/s` 更灵敏), 故改用它。
+
+    本用例正钉这一点: `q` 恒不动而 `dq` 一直 0.5 (远超 `dq_tol=0.10`) ⇒ **仍须判到位**。
+
+    ⚠ 与旧用例的关系: 原名 `test_bit10_falling_to_zero_is_not_enough_without_a_static_q`
+    断言的恰是"高 `dq` 必须挡住到位" —— 那正是**被实测推翻的前提**, 故按新契约
+    **反向重写**。"还在爬"那一支仍由 `test_a_q_that_keeps_moving_never_settles` 覆盖。
     """
     arm = cart_arm(cls=_ScriptedStatus, move_timeout=0.4, dq_value=0.5)
-    with pytest.raises(MotionTimeoutError):
-        arm.move_l(TCP0, speed=0.3)
+    plan = arm.move_l(TCP0, speed=0.3)
+    assert plan.settled is True, (
+        "q 静止即视为停稳 —— 上报的 dq 在这台硬件上不可信, 已不参与判定")
 
 
 def test_a_q_that_keeps_moving_never_settles(cart_arm):
-    """相邻两帧 Δq 超 `q_tol` 时同样不算静止 (另一半判据 —— `dq` 为 0 也没用)。"""
-    # 每帧比上一帧多 0.5 rad: Δq 恒 > q_tol (0.03), 永不停止
+    """相邻两帧 Δq 超位移阈时不算静止 —— 现在是**唯一**的静止判据。"""
+    # 每帧比上一帧多 0.5 rad: Δq 恒远超阈, 永不停止
     arm = cart_arm(cls=_ScriptedStatus, move_timeout=0.4, no_busy=True,
                    q_script=lambda i: [i * 0.5] * 7)
     with pytest.raises(MotionTimeoutError):
@@ -262,20 +272,23 @@ def test_settle_err_rad_is_the_jitter_of_the_settling_frames(cart_arm):
     """⚠ `settle_err_rad` 是**新语义**: "结束时各轴 `q` 与 `q_final` 的差" (最后几帧的
     抖动幅度), **不是**"与终点指令的偏差" (固件原生路径下那个量物理不可得)。
 
-    脚本 `0 → 0 → 0.02`: 第 3 帧时静止窗 (`arrive_frames=3`) 第一次填满, 于是收尾帧的
-    `q_final = 0.02`, 而窗内与它的最大差 = `|0 - 0.02| = 0.02` (逐轴). 若实现成
-    "与指令的偏差" 或恒 0, 这条会红。
+    脚本 `0 / 0.0004` 交替: 静止窗 (`arrive_frames=3`) 填满时收尾帧的 `q_final`
+    是二者之一, 而窗内与它的最大差 = `0.0004` (逐轴). 若实现成"与指令的偏差"或恒 0,
+    这条会红。
     """
-    # ⚠ 脚本改成**交替**（0 / 0.02 交替）而不是"0,0,0.02"：读线程一开，脚本按**时间**推进，
-    # "`_wait_settled` 从第几帧开始采样"不再可控。交替的话**任意**连续 3 帧都含 0 与 0.02
-    # ⇒ 抖动恒为 0.02，判据与起采样点无关。（|Δq|=0.02 < q_tol 0.03 ⇒ 仍算"静止"。）
+    # ⚠ [2026-10-10] 抖动幅度**必须落在位移阈之内**, 否则按新判据不算静止 (窗填不满 ⇒
+    #   超时)。阈值上限 ≈ `dq_tol / STATUS_HZ` = 0.001 rad/帧, 故旧文里的 0.02 已不适用
+    #   —— 0.02 rad/帧 = 2 rad/s, 那本来就该判"还在动"。
+    # ⚠ 脚本用**交替**（0 / 0.0004 交替）而不是"0,0,0.0004"：读线程一开，脚本按**时间**
+    # 推进，"`_wait_settled` 从第几帧开始采样"不再可控。交替的话**任意**连续 3 帧都含两个
+    # 值 ⇒ 抖动恒为 0.0004，判据与起采样点无关。
     arm = cart_arm(cls=_ScriptedStatus, no_busy=True,
-                   q_script=lambda i: [0.0 if i % 2 == 0 else 0.02] * 7)
+                   q_script=lambda i: [0.0 if i % 2 == 0 else 0.0004] * 7)
     plan = arm.move_l(TCP0, speed=0.3)
     assert plan.settled is True
-    assert plan.q_final == pytest.approx([0.02] * 7)
-    assert plan.settle_err_rad == pytest.approx(0.02, abs=1e-6), (
-        "抖动幅度应为窗内 q 与 q_final 的最大差 (0.02)")
+    assert plan.q_final == pytest.approx([0.0004] * 7)
+    assert plan.settle_err_rad == pytest.approx(0.0004, abs=1e-9), (
+        "抖动幅度应为窗内 q 与 q_final 的最大差 (0.0004)")
 
 
 # ---------------------------------------------------------------------------