feat(joint-follow): host-side entry for CMD_JOINT_FOLLOW (0x08)

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",

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)。

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)",

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 祖先),

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",

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} 的顺序解析）")