From 7eaa79d2049c4c788784145246e6c349f690f5dc Mon Sep 17 00:00:00 2001 From: YOLO-DT <3199429837@qq.com> Date: Sat, 10 Oct 2026 16:54:11 +0800 Subject: [PATCH] fix: judge arrival from the frame-to-frame position delta _arrive and cart._wait_settled both required the reported dq to stay under dq_tol for arrive_frames consecutive frames. On real hardware that never happens at some poses: with the joints provably frozen -- 800 frames of bit-identical q, zero drift -- one joint's reported dq still swung between 0.007 and 0.256 rad/s, over dq_tol on 66% of frames. Arrival waits then ran the full move_timeout and raised MotionTimeoutError with the arm sitting on the target (0.0026 rad off, well inside q_tol). dq is not a firmware computation: control_loop.c:512 forwards the motor's own velocity report unfiltered, and a DM4310 reports it as a 12-bit field, so the 0.01465 rad/s quantum is the motor's resolution rather than a defect here. The position stream is clean, so both sites now compare consecutive frames instead. The threshold is dq_tol / STATUS_HZ -- the same limit expressed as a displacement per frame -- so the strictness is unchanged. Two tests pinned the old criterion. One asserted that a high dq must block arrival; that is the premise hardware contradicts, so it is rewritten to assert the opposite. The other used a 0.02 rad/frame jitter, which is 2 rad/s and should not count as settled; it now uses a jitter inside the new threshold. --- src/litearm/_protocol.py | 9 +++++++ src/litearm/arm.py | 30 ++++++++++++++++++++--- src/litearm/cart.py | 33 ++++++++++++++++--------- tests/test_cart_wait.py | 53 +++++++++++++++++++++++++--------------- 4 files changed, 89 insertions(+), 36 deletions(-) diff --git a/src/litearm/_protocol.py b/src/litearm/_protocol.py index e36f2d7..e20973e 100644 --- a/src/litearm/_protocol.py +++ b/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"} diff --git a/src/litearm/arm.py b/src/litearm/arm.py index ec459c7..6b7b55c 100644 --- a/src/litearm/arm.py +++ b/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() diff --git a/src/litearm/cart.py b/src/litearm/cart.py index 47fc7f7..f595dd1 100644 --- a/src/litearm/cart.py +++ b/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 [] diff --git a/tests/test_cart_wait.py b/tests/test_cart_wait.py index 0a01b84..5344215 100644 --- a/tests/test_cart_wait.py +++ b/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)") # ---------------------------------------------------------------------------