From 97884f3db507d4537b4fceb3284f47e95566e805 Mon Sep 17 00:00:00 2001 From: luochun <56000204@qq.com> Date: Mon, 28 Sep 2026 21:59:04 +0800 Subject: [PATCH 1/2] =?UTF-8?q?feat(joint-follow):=20S1=20=E2=80=94=20host?= =?UTF-8?q?-side=20entry=20for=20CMD=5FJOINT=5FFOLLOW=20(0x08)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Arm.joint_follow(q, dq, kp, kd) sends 16N bytes with no tau; the firmware computes the feedforward itself. Registered in COMMAND_COVERAGE so the cross-repo contract test keeps both sides honest, and the two new error codes (payload short, non finite values) plus the three return codes the accept function shares with move_mit_all are in ERR_TEXT. The contract tests caught all four omissions on their own -- the coverage list, the exercised manifest, the symbolic site manifest and its pinned count -- which is exactly what they are for. --- src/litearm/_protocol.py | 7 +++++++ src/litearm/arm.py | 17 +++++++++++++++++ src/litearm/errors.py | 5 +++++ tests/test_error_codes.py | 6 +++++- tests/test_full_coverage.py | 4 +++- 5 files changed, 37 insertions(+), 2 deletions(-) diff --git a/src/litearm/_protocol.py b/src/litearm/_protocol.py index 59de324..e36f2d7 100644 --- a/src/litearm/_protocol.py +++ b/src/litearm/_protocol.py @@ -30,6 +30,12 @@ #: ⚠ 代价: 被最慢的轴拖住, 整体比 0x01 慢 —— 故是**可选模式**, 不是替换。 #: ⚠ 固件状态帧仍报 mode=MOVE_J, **无法**从状态区分同步/异步。 CMD_MOVE_J_SYNC = 0x07 +#: [JOINT_FOLLOW] 从臂跟随 —— **伺服环落在固件里**，上位机只喂目标与增益。 +#: 载荷 = q_target[N] f32 + dq_ref[N] f32 + K[N] f32 + B[N] f32 = 16N 字节。 +#: ⚠ 与 `CMD_MOVE_MIT_ALL` 的关键差别: **帧里没有 tau** —— 前馈由固件算 +#: `τ_ff = clamp(G(q) + wall, ±tau_max)`，省掉上位机的 `get_gravity` 往返。 +#: ⚠ 固件状态帧仍报 `mode=MOVE_MIT_ALL`(模式字段是 3 bit 已满, 不新增模式号)。 +CMD_JOINT_FOLLOW = 0x08 CMD_ENABLE = 0x10 CMD_DISABLE = 0x11 CMD_EMERGENCY_STOP = 0x12 @@ -280,6 +286,7 @@ def parse_boot_banner(text: str) -> Optional[str]: 0x04: "Arm.send_mit", 0x05: "Arm.send_mit_all", 0x06: "Arm.zero_g", + 0x08: "Arm.joint_follow", 0x10: "Arm.enable", 0x11: "Arm.disable", 0x12: "Arm.emergency_stop", diff --git a/src/litearm/arm.py b/src/litearm/arm.py index e2fad2d..ec459c7 100644 --- a/src/litearm/arm.py +++ b/src/litearm/arm.py @@ -2193,6 +2193,23 @@ def send_mit_all(self, q, dq, kp, kd, tau) -> None: payload = P.pack_f32s(list(q) + list(dq) + list(kp) + list(kd) + list(tau)) self._cmd(P.CMD_MOVE_MIT_ALL, payload, "send_mit_all") + def joint_follow(self, q, dq, kp, kd) -> None: + """[JOINT_FOLLOW] 从臂跟随：**伺服环在固件里**，这里只喂目标与增益。 + + ⚠ 与 :meth:`send_mit_all` 的差别是**没有 `tau`** —— 前馈（重力 `G(q)` 与限位墙） + 由固件每拍自己算，照 litearm-server 的 `joint_follow.compute_tau_ff(q, dq)`。 + 好处是省掉一次 `get_gravity` 往返；代价是这条命令**依赖固件实现了本命令** + （旧固件会回 `ERR{0x08, 0x00}` = 无此命令，不会静默）。 + + ⚠ 本命令需**周期重发**（fail-soft 看门狗 0.1 s），与 `send_mit_all` 同。 + """ + self._reject_if_in_dfu() + for arr, label in ((q, "q"), (dq, "dq"), (kp, "kp"), (kd, "kd")): + if len(arr) != self.n: + raise InvalidCommandError(f"joint_follow {label} 需 N 个") + payload = P.pack_f32s(list(q) + list(dq) + list(kp) + list(kd)) + self._cmd(P.CMD_JOINT_FOLLOW, payload, "joint_follow") + # ---------- FF/动力学 调参 (固件 0x26-0x28/0x31) ---------- def set_ff_mask(self, mask: int) -> None: """写 `ff_mask` (固件 0x27)。 diff --git a/src/litearm/errors.py b/src/litearm/errors.py index f8782d4..02a6541 100644 --- a/src/litearm/errors.py +++ b/src/litearm/errors.py @@ -261,6 +261,11 @@ class ArmIsInDfuError(LiteArmError): (0x05, 0x03): "未使能, 或 EMERGENCY 锁存 (ctrl_accept_move_mit_all)", (0x05, 0x04): "零重力(拖动示教)进行中", (0x05, 0x06): "掉线刚性持位锁存 (drop_hold) 中", + (0x08, 0x01): "载荷长度不足 (需 16×N 字节)", + (0x08, 0x02): "q/dq/kp/kd 含非有限值", + (0x08, 0x03): "未使能, 或 EMERGENCY 锁存 (ctrl_accept_joint_follow)", + (0x08, 0x04): "零重力(拖动示教)进行中", + (0x08, 0x06): "掉线刚性持位锁存 (drop_hold) 中", # ---- 0x06 ZERO_G（进入/退出拖动示教; 重发即保活）---- (0x06, 0x01): "载荷长度不足 (需 1 字节: on)", diff --git a/tests/test_error_codes.py b/tests/test_error_codes.py index dd73262..8d3734d 100644 --- a/tests/test_error_codes.py +++ b/tests/test_error_codes.py @@ -387,6 +387,10 @@ def test_doc_warns_that_inference_is_one_sided(): (_USB_CMD_C, "0x05", "rma"): (frozenset({(0x05, 0x03), (0x05, 0x04), (0x05, 0x06)}), "ctrl_accept_move_mit_all 的返回码"), + (_USB_CMD_C, "0x08", "rjf"): + (frozenset({(0x08, 0x03), (0x08, 0x04), (0x08, 0x06)}), + "ctrl_accept_joint_follow 的返回码 (与 0x05 同表: 0x03 未使能/急停锁存 / " + "0x04 零重力中 / 0x06 掉线刚性持位锁存)"), (_USB_CMD_C, "0x10", "e"): (frozenset({(0x10, 0x03), (0x10, 0x06), (0x10, 0x07), (0x10, 0x08)}), "ctrl_enable 的返回码 (control_api.h: 0x03 可重试 / 0x06 锁存须 reset / " @@ -401,7 +405,7 @@ def test_doc_warns_that_inference_is_one_sided(): } #: 清单自身的**条数** —— 断言它, 免得清单被"顺手"改小而不自知。 -SYMBOLIC_ERR_SITES_COUNT = 13 +SYMBOLIC_ERR_SITES_COUNT = 14 #: `ERR_TEXT` 里**不在本固件树产出范围内**、但刻意保留的条目 (spec R5): #: 出处是 `origin/feat/hyy-model-import` (1.5.3 `cdb744a` 不是 master 祖先), diff --git a/tests/test_full_coverage.py b/tests/test_full_coverage.py index 1f7a22f..02be121 100644 --- a/tests/test_full_coverage.py +++ b/tests/test_full_coverage.py @@ -39,6 +39,7 @@ def test_every_implemented_command_is_exercised_through_its_entry(offline_arm): arm.move_js([0.0] * 7, dq=[0.0] * 7, tau_ff=[0.0] * 7) arm.send_mit(0, 0.0, 0.0, 50.0, 2.0, 0.0) # 0x04 arm.send_mit_all([0.0] * 7, [0.0] * 7, [50.0] * 7, [2.0] * 7, [0.0] * 7) # 0x05 + arm.joint_follow([0.0] * 7, [0.0] * 7, [50.0] * 7, [2.0] * 7) # 0x08 arm.set_motion_mode(0) # 0x20 arm.set_speed(50) # 0x21 arm.ik((0.30, 0.0, 0.35, 0.0, 0.0, 0.0)) # 0x42 @@ -172,7 +173,8 @@ def test_public_api_surface_is_all_exercised(offline_arm): "connect", "close", "get_state", "get_status_now", "get_tcp", "ik", "home", "enable", "disable", "emergency_stop", "reset", "clear_faults", "set_motion_mode", "park", "set_speed", "zero_g", "zero_g_start", "zero_g_stop", - "movej", "movej_sync", "move_p", "move_js", "send_mit", "send_mit_all", "reconnect", + "movej", "movej_sync", "move_p", "move_js", "send_mit", "send_mit_all", + "joint_follow", "reconnect", # 收尾的两个名字 (同一个操作): 判据在 tests/test_teardown.py "disconnect", "set_ff_mask", "ff_preset", "set_ff_vec", "set_ff_scalar", "get_ff_vec", From 8bc02c37364dc58ff2e33f3d317809d1a6b3e919 Mon Sep 17 00:00:00 2001 From: luochun <56000204@qq.com> Date: Mon, 28 Sep 2026 22:54:44 +0800 Subject: [PATCH 2/2] =?UTF-8?q?test(0x08):=20=E9=92=89=E4=BD=8F=20joint=5F?= =?UTF-8?q?follow=20=E7=9A=84=E8=BD=BD=E8=8D=B7=E5=B8=83=E5=B1=80=EF=BC=8C?= =?UTF-8?q?=E4=B8=A4=E7=AB=AF=E4=B8=80=E8=B5=B7=E6=A0=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 审查缺口：0x08 的载荷布局（q_target | dq_ref | K | B 各 N 个 f32 = 16N 字节） 此前**没有任何判据**。test_full_coverage 只断言"这条命令发出去过"，不看载荷 内容 ⇒ 只改一端（SDK 打包或固件解析）不会被任何用例发现。 新判据两头都钉： ① 从固件 usb_cmd.h 的声明里正则提取分段顺序（顺序/长度改了先红） ② 用互不相同的哨兵值发出**真实帧**，逐段核对解出的值 —— 不复述 arm.py 的 表达式：期望值与实现同源的话判据会随实现一起错（本仓栽过：ACK/ERR 常量 写错而测试自洽地全绿）。 判别力已双向反验： · 改固件声明顺序（用临时目录副本，不动真文件）⇒ 红 · 改 SDK 打包顺序（临时改 arm.py 后还原）⇒ 红 654 passed / 2 skipped（新增 1 条）。 --- tests/test_protocol_sync.py | 43 +++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/tests/test_protocol_sync.py b/tests/test_protocol_sync.py index 9bcfcf9..65f652c 100644 --- a/tests/test_protocol_sync.py +++ b/tests/test_protocol_sync.py @@ -32,6 +32,7 @@ import os import re +import struct import pytest @@ -296,3 +297,45 @@ def test_firmware_version_not_below_sdk_minimum(): assert p[:3] >= arm_mod.MIN_FW, ( f"固件自报 {v} 低于 SDK 要求的 " f"{'.'.join(map(str, arm_mod.MIN_FW))}") + + +# ------------------------------------------------- 7. 0x08 的载荷布局（跨仓） +def test_joint_follow_payload_layout_matches_firmware(hdr, offline_arm): + """⛔ `0x08` 的载荷：固件**声明的分段顺序**必须等于 SDK **实际发出的字节**。 + + 契约两端是 SDK `arm.joint_follow()` 的打包与固件 `usb_cmd.c` 的 + `case CMD_JOINT_FOLLOW` 解析。**只改一边**是这里最容易出的错，而别的用例 + 都发现不了它：`test_full_coverage` 只断言"这条命令发出去过"，不看载荷内容。 + + ⚠ 判据取**真实发出的字节**逐段核对，而**不是**复述 `arm.py` 里的那个 + `pack_f32s(list(q)+list(dq)+list(kp)+list(kd))` —— 期望值与实现同源的话， + 判据会随实现一起错（本仓栽过：ACK/ERR 常量写错而测试自洽地全绿）。 + """ + # ① 固件头文件里声明的分段顺序 + 总长（顺序或长度改了这里先红） + m = re.search( + r"payload\s*=\s*q_target\[N\]\s*f32\s*\+\s*dq_ref\[N\]\s*f32\s*\+" + r"\s*K\[N\]\s*f32\s*\+\s*B\[N\]\s*f32\s*=\s*16N\s*字节", hdr) + assert m, ( + "固件 usb_cmd.h 里 CMD_JOINT_FOLLOW 的载荷声明变了或没了 —— " + "两端（SDK 打包 / 固件解析）必须一起改；本条判据就是防「只改一边」") + + # ② SDK 实发字节：四段用互不相同的哨兵值，好钉住每一段的位置 + n = 7 + q = [0.1 * (i + 1) for i in range(n)] # 0.1 … 0.7 + dq = [10.0 + i for i in range(n)] # 10 … 16 + kp = [100.0 + i for i in range(n)] # 100 … 106 + kd = [1000.0 + i for i in range(n)] # 1000 … 1006 + offline_arm.connect() + offline_arm.joint_follow(q, dq, kp, kd) + + payloads = [p for c, p in offline_arm._tr.tx_log if c == P.CMD_JOINT_FOLLOW] + assert payloads, "0x08 一帧都没发出去" + pl = payloads[-1] + + assert len(pl) == 16 * n, f"载荷应为 16N={16 * n} 字节，实为 {len(pl)}" + got = list(struct.unpack("<" + "f" * (4 * n), pl)) # 小端，与 pack_f32s 同 + seg = ("q_target", "dq_ref", "K", "B") + want = (q, dq, kp, kd) + for i in range(4): + assert got[i * n:(i + 1) * n] == pytest.approx(want[i], rel=1e-5), ( + f"第 {i + 1} 段不是 {seg[i]}（固件按 {seg} 的顺序解析）")