litearm-python 1.2.0__py3-none-any.whl

litearm/log.py

@@ -0,0 +1,233 @@
+"""300Hz 控制拍采集 —— 固件 `CMD_LOG_CTRL(0x2D)` / `CMD_LOG_READ(0x2E)` 的封装。
+
+固件侧 (`hal/log_capture.h`): 跑激励轨迹时把每控制拍 (tick / 执行参考 q_ref /
+执行速度 dq / 电机实测力矩 tau) 追加进 RAM 缓冲, `LOG_MAX_SAMPLES=2400` (≈8s@300Hz)
+记满自停; 上位机用 `LOG_READ` 按字节游标分块读回 —— 这条"绕开连续 USB 流、按游标
+续读"的设计正是为规避 CDC 间歇掉帧而做的, 所以本模块的重试/续读语义是**必须**的,
+不是可选优化。
+
+样本布局 (固件 `log_sample_t`): `u32 tick + q_ref[N] + dq[N] + tau[N]`, 即 `4 + 12N` 字节。
+"""
+from __future__ import annotations
+
+import struct
+import time
+from dataclasses import dataclass
+from typing import TYPE_CHECKING, Iterator, List, Optional
+
+from litearm import _protocol as P
+from litearm.errors import (
+    InvalidCommandError,
+    MotionTimeoutError,
+    TransportError,
+)
+
+if TYPE_CHECKING:                       # 避免与 arm 循环导入
+    from litearm.arm import Arm
+
+__all__ = ["LogSample", "LogReader", "ArmLog", "sample_size"]
+
+#: 固件 `LOG_MAX_SAMPLES` (hal/log_capture.h) —— 超过会回 ERR{0x2D,0x02}
+LOG_MAX_SAMPLES = 2400
+#: 固件单块读回上限 `LOG_READ_CHUNK` (usb_cmd.c) —— 9B 头 + n ≤ 245
+LOG_READ_CHUNK = 245
+#: 单块 RSP_LOG_DATA 的头长度: u32 total + u32 next + u8 n
+_LOG_HDR = 9
+#: 固件控制循环频率 (`litearm.h` 的 `LITEARM_CTRL_HZ`) —— 采集是**逐拍**记录的,
+#: 故 n 拍需要 n/HZ 秒才录满。用于推算默认等待预算。
+CTRL_HZ = 300
+#: 轮询已记录量的间隔
+_POLL_S = 0.05
+
+
+def sample_size(n_joints: int) -> int:
+    """单拍字节数 = `sizeof(u32) + 3 * N * sizeof(f32)`。"""
+    return 4 + 12 * int(n_joints)
+
+
+@dataclass(frozen=True)
+class LogSample:
+    """一拍采样 (固件 `log_sample_t`)。`q_ref`/`dq`/`tau` 为 N 元组。"""
+
+    tick: int
+    q_ref: tuple
+    dq: tuple
+    tau: tuple
+
+
+def parse_samples(blob: bytes, n_joints: int) -> List[LogSample]:
+    """把原始字节流解析成样本列表; 不是样本整数倍则报错。"""
+    sz = sample_size(n_joints)
+    if sz <= 0:
+        raise InvalidCommandError(f"关节数非法: {n_joints}")
+    if len(blob) % sz:
+        raise TransportError(
+            f"采集字节数 {len(blob)} 不是单拍 {sz}B 的整数倍 —— 数据截断/错位, "
+            f"拒绝解析出半截样本")
+    out = []
+    for off in range(0, len(blob), sz):
+        tick = struct.unpack_from("<I", blob, off)[0]
+        q_ref = tuple(P.unpack_f32s(blob, off + 4, n_joints))
+        dq = tuple(P.unpack_f32s(blob, off + 4 + 4 * n_joints, n_joints))
+        tau = tuple(P.unpack_f32s(blob, off + 4 + 8 * n_joints, n_joints))
+        out.append(LogSample(tick, q_ref, dq, tau))
+    return out
+
+
+class LogReader:
+    """按固件游标 (`next_byte`) 分块读回采集缓冲。
+
+    `next_byte == 0` 表示读完; 掉帧 (应答整块丢失) 时按当前游标重试, 不会丢一段数据。
+    """
+
+    def __init__(self, arm: "Arm", timeout: float = 1.0, retries: int = 3):
+        self._arm = arm
+        self.timeout = float(timeout)
+        self.retries = int(retries)
+        #: 最近一次读回的固件总字节数 (读完后可查)
+        self.total_bytes = 0
+        self._next = 0
+
+    def _read_chunk(self, offset: int) -> bytes:
+        last: Optional[Exception] = None
+        for _ in range(self.retries + 1):
+            try:
+                self._arm._write_query(P.CMD_LOG_READ, struct.pack("<I", offset))
+                _, p = self._arm._require().expect(
+                    P.RSP_LOG_DATA, self.timeout, f"log_read@{offset}",
+                    echo_cmd=P.CMD_LOG_READ)
+            except MotionTimeoutError as e:         # 掉帧: 同一游标重试
+                last = e
+                continue
+            if len(p) < _LOG_HDR:
+                raise TransportError(f"RSP_LOG_DATA 帧短 ({len(p)}B)")
+            total, nxt, n = struct.unpack_from("<IIB", p, 0)
+            if n > LOG_READ_CHUNK or len(p) < _LOG_HDR + n:
+                raise TransportError(f"RSP_LOG_DATA 载荷异常: n={n}, 实到 {len(p) - _LOG_HDR}B")
+            self.total_bytes = int(total)
+            self._next = int(nxt)
+            return bytes(p[_LOG_HDR:_LOG_HDR + n])
+        raise last if last is not None else TransportError("log_read 失败")
+
+    def total(self) -> int:
+        """查询固件当前**已记录**的字节数 (一次 `LOG_READ` 往返, 不保存数据)。
+
+        采集是 300Hz 逐拍进行的, `start(n)` 返回时缓冲里还没有数据 —— 等待录满
+        必须靠轮询本方法, 不能靠 sleep 猜时间。
+        """
+        self._read_chunk(0)
+        return self.total_bytes
+
+    def wait_for(self, n_ticks: int, timeout: Optional[float] = None,
+                 poll: float = _POLL_S) -> int:
+        """等固件记满 `n_ticks` 拍; 返回实际总字节数。
+
+        `timeout=None` 时按 `n_ticks / CTRL_HZ * 1.5 + 3s` 推算 (300Hz 逐拍记录)。
+        超时抛 `MotionTimeoutError` 并报出实际录到多少拍 —— 绝不返回半截数据冒充成功。
+        """
+        n = int(n_ticks)
+        sz = sample_size(self._arm.n)
+        want = n * sz
+        if want <= 0:
+            return 0
+        if timeout is None:
+            timeout = n / float(CTRL_HZ) * 1.5 + 3.0
+        end = time.monotonic() + float(timeout)
+        while True:
+            total = self.total()
+            if total >= want:
+                return total
+            if time.monotonic() >= end:
+                raise MotionTimeoutError(
+                    f"采集未在 {float(timeout):.1f}s 内录满 {n} 拍 "
+                    f"(只录到 {total // sz} 拍 / {total}B) —— 可调大 record_timeout")
+            time.sleep(poll)
+
+    def iter_chunks(self) -> Iterator[bytes]:
+        """逐块产出原始字节; 读到 `next_byte == 0` 结束。
+
+        ⚠ 必须自检**游标是否前进**: 固件游标若因异常/干扰回了一个不大于当前 offset
+        的非零值, 天真的实现会在同一位置无限次重读 (死循环)。这里直接报错。
+        """
+        off = 0
+        while True:
+            chunk = self._read_chunk(off)
+            if chunk:
+                yield chunk
+            if self._next == 0:
+                return
+            if self._next <= off:
+                raise TransportError(
+                    f"LOG_READ 游标未前进 (offset={off} -> next_byte={self._next}), "
+                    f"拒绝在同一游标上重复读回 (死循环保护)")
+            off = self._next
+
+    def read_all(self) -> bytes:
+        return b"".join(self.iter_chunks())
+
+    def samples(self) -> List[LogSample]:
+        """读回并解析为样本列表 (便捷; 大缓冲建议先 `dump()` 落盘)。"""
+        return parse_samples(self.read_all(), self._arm.n)
+
+
+class ArmLog:
+    """`arm.log` —— 采集启停 + 读回入口。"""
+
+    def __init__(self, arm: "Arm"):
+        self._arm = arm
+        #: 最近一次 `start(n)` 的 n —— `dump()` 靠它知道该等录满多少拍
+        self._last_target = 0
+
+    def start(self, n_ticks: int) -> None:
+        """开始记录 `n_ticks` 拍后自停 (n=0 等效 `stop()`)。
+
+        ⚠ 固件是**逐拍**记录的: 本调用返回时缓冲里还没有数据 (300Hz, n 拍要
+        n/300 秒)。要拿完整数据须用 `capture()` (自动等) 或 `dump()` (默认等)。
+
+        固件侧 `n > LOG_MAX_SAMPLES(2400)` 回 `ERR{0x2D,0x02}`。
+        """
+        n = int(n_ticks)
+        if n < 0:
+            raise InvalidCommandError(f"n_ticks 需 >=0 (给的是 {n_ticks})")
+        self._arm._cmd(P.CMD_LOG_CTRL, struct.pack("<I", n), "log_start")
+        self._last_target = n
+
+    def stop(self) -> None:
+        """停止/清空记录 (停止后仍可用 `reader()` 读回已记录部分)。"""
+        self._arm._cmd(P.CMD_LOG_CTRL, struct.pack("<I", 0), "log_stop")
+        self._last_target = 0
+
+    def reader(self, timeout: float = 1.0, retries: int = 3) -> LogReader:
+        return LogReader(self._arm, timeout=timeout, retries=retries)
+
+    def capture(self, n_ticks: int, timeout: float = 1.0, retries: int = 3,
+                record_timeout: Optional[float] = None) -> List[LogSample]:
+        """便捷: 采 `n_ticks` 拍 -> **等录满** -> 读回 -> 解析成样本列表。
+
+        ⚠ 固件是 300Hz **逐拍**记录的 —— `start(n)` 返回时缓冲里还没有数据,
+        `n_ticks=600` 要约 2 秒才录满, 故本方法会轮询等待 (`record_timeout=None`
+        时按 `n/HZ*1.5+3s` 推算)。等不满则抛错, 不返回半截数据。
+
+        ⚠ 读回是 ~`n_ticks*(4+12N)/245` 次往返 (7J 满量程 2400 拍 ≈ 863 次),
+        大缓冲建议用 `dump()` 落盘后离线解析。`timeout` 是**单块**超时。
+        """
+        self.start(n_ticks)
+        reader = self.reader(timeout=timeout, retries=retries)
+        reader.wait_for(n_ticks, timeout=record_timeout)
+        return reader.samples()
+
+    def dump(self, path: str, wait: bool = True, timeout: float = 1.0,
+             retries: int = 3, record_timeout: Optional[float] = None) -> int:
+        """读回原始字节流落盘 (解析交给调用方 / `parse_samples`)。返回字节数。
+
+        `wait=True`(默认) 会先等最近一次 `start(n)` 的 n 拍录满 —— 否则 `start()`
+        之后立刻 dump 会落盘一个空/半截文件。要读"此刻已录到多少"就传
+        `wait=False`。
+        """
+        reader = self.reader(timeout=timeout, retries=retries)
+        if wait and self._last_target > 0:
+            reader.wait_for(self._last_target, timeout=record_timeout)
+        blob = reader.read_all()
+        with open(path, "wb") as f:
+            f.write(blob)
+        return len(blob)

litearm/model.py

@@ -0,0 +1,253 @@
+"""动力学模型在线导入 —— 固件 `0x30/0x32/0x33/0x34/0x35/0x37/0x38/0x39` 的面向对象封装。
+
+## 为什么需要它
+
+stm32 固件里, 连杆动力学 (m / com / I) 与电机转子惯量 jm 原先都是**编译期常量**
+(`dyn_model_data.h` / `dyn_jm_default.h`)。产线要求「每台臂出厂前上 HYY 控制器辨识
+一次动力学 → 把参数写进固件」, 逐台不同 ⇒ 必须能在线写入, 否则每台都得重编译烧录。
+
+固件的模型分三层 (见 `litearm-stm32/User/litearm/dynamics/dyn.h`):
+
+    bank     模型唯一权威, **永不含 payload**; 来源 = 编译期常量 或 导入/flash
+    staging  0x30 / 0x33 写这里, 不参与递推
+    生效层   bank + payload 复合, RNEA 读这一层
+
+`0x32` commit 把 staging **整组原子**拷进 bank ⇒ 不存在"半截模型生效"。
+
+## 正确用法
+
+```python
+arm.model.set_body(1, [m, cmx, cmy, cmz, ixx, ixy, ixz, iyy, iyz, izz])   # J1
+...                                                                        # J2..J7
+arm.model.set_jm([...])                                                    # 7 个
+arm.model.commit(0x2FE)     # body1..7 + jm 的位图; 掩码必须与实际写过的项逐位相等
+```
+
+`expected_mask` 的位: `bit i (0..8) = body i`, `bit 9 = jm`。固件用它关掉两类误用:
+「写一半就 commit」与「跨会话拼接」。本设计的工具侧固定写 `body1..7 + jm` ⇒ `0x2FE`
+(见 `MODEL_MASK_WRITTEN`)。
+
+## 门控
+
+`commit` 与 `revert` **要求失能** (固件判据 `enabled || enable_pending`), 否则回
+`ERR{cmd,0x04}`。写 staging (0x30/0x33) 不要求失能 —— 不生效即无风险。
+
+固件侧载荷与错误码见 `litearm-stm32/User/litearm/hal/usb_cmd.c` 的对应分支。
+"""
+from __future__ import annotations
+
+import struct
+from dataclasses import dataclass
+from typing import TYPE_CHECKING, List, Sequence
+
+from litearm import _protocol as P
+from litearm.errors import (InvalidCommandError, TransportError,
+                                    UnsupportedByFirmwareError)
+
+if TYPE_CHECKING:                       # 避免与 arm 循环导入
+    from litearm.arm import Arm, Msg
+
+__all__ = ["ModelStatus", "ModelParams", "MODEL_NBODY", "MODEL_BODY_PARAMS",
+           "MODEL_BIT_BODY", "MODEL_BIT_JM", "MODEL_MASK_WRITTEN"]
+
+#: 模型刚体数 —— **与固件 `dyn_model_data.h` 的 `DYN_MODEL_NBODY` 同值**。
+#: ⚠ 与 `arm.n` (关节数, 台架 1J 时为 1) **无关**: 模型恒 9 刚体 (0=base, 1..7=关节,
+#: 8=ee 固定体)。照抄 `JointParams._check_idx` 用 `arm.n` 做上界会让台架完全无法验证模型。
+MODEL_NBODY = 9
+
+#: 单 body 的字段数: m, cmx, cmy, cmz, ixx, ixy, ixz, iyy, iyz, izz
+MODEL_BODY_PARAMS = 10
+
+#: jm 长度 (关节数)
+MODEL_JM_N = 7
+
+
+def MODEL_BIT_BODY(i: int) -> int:
+    """body i 的 `expected_mask` 位 (i = 0..8)。"""
+    return 1 << int(i)
+
+
+MODEL_BIT_JM = 1 << 9                       #: jm 的 expected_mask 位
+
+#: 工具/产线实际写入的集合: body1..7 + jm (body0=base 与 body8=ee 不写) = 0x2FE。
+#: 固件侧的 `T_DIRTY_ALL & ~(1 | 1 << 8)` 必须与之逐位相等。
+MODEL_MASK_WRITTEN = sum(MODEL_BIT_BODY(i) for i in range(1, 8)) | MODEL_BIT_JM
+assert MODEL_MASK_WRITTEN == 0x2FE, "MODEL_MASK_WRITTEN 常量漂移"
+
+
+@dataclass(frozen=True)
+class ModelStatus:
+    """固件 `RSP_MODEL_STATUS(0x56)` 的 5B 应答。"""
+
+    override: int        #: 0 = 编译期常量; 1 = 导入/flash 模型 (**不区分二者**)
+    staged_mask: int     #: bit0..8 = body0..8, bit9 = jm (0..0x3FF)
+    dirty: int           #: 1 = RAM 有未固化改动 (定义 = RAM != flash)
+
+    @classmethod
+    def decode(cls, payload: bytes) -> "ModelStatus":
+        # payload 含首字节 RSP id (与 RSP_FF_VEC/RSP_FF_SCALAR 同口径)
+        if len(payload) != 5:
+            raise TransportError(f"RSP_MODEL_STATUS 帧长 {len(payload)}B, 期望 5B")
+        if payload[0] != P.RSP_MODEL_STATUS:
+            raise TransportError(f"RSP_MODEL_STATUS 帧 id 0x{payload[0]:02X}, 期望 0x{P.RSP_MODEL_STATUS:02X}")
+        mask = struct.unpack_from("<H", payload, 2)[0]      # u16 **LE**
+        return cls(int(payload[1]), int(mask), int(payload[4]))
+
+
+def _check_body_vals(vals: Sequence[float]) -> List[float]:
+    if len(vals) != MODEL_BODY_PARAMS:
+        raise InvalidCommandError(
+            f"body 参数须为 {MODEL_BODY_PARAMS} 个 (m, cmx, cmy, cmz, ixx, ixy, ixz, iyy, iyz, izz), "
+            f"给的是 {len(vals)} 个")
+    return [float(v) for v in vals]
+
+
+class ModelParams:
+    """`arm.model` —— 动力学模型的读/写/提交/回退入口。
+
+    无状态: 每次调用直发一帧 (与 `arm.params` 同风格), 不做任何本地缓存
+    —— 固件才是唯一权威, 缓存会与"另一端可能改了"打架。
+    """
+
+    def __init__(self, arm: "Arm"):
+        self._arm = arm
+
+    # ---- 内部 ----
+    def _check_body_idx(self, idx: int) -> int:
+        idx = int(idx)
+        if not 0 <= idx < MODEL_NBODY:
+            raise InvalidCommandError(f"刚体索引越界: {idx} (有效 0..{MODEL_NBODY - 1})")
+        return idx
+
+    # ---- 0x34 (探测 + 读回共用) ----
+    def probe(self) -> bool:
+        """固件是否支持模型在线导入。
+
+        用 `0x34` 探测: 旧固件**没有**这条命令的 case ⇒ 落 `default` ⇒ `ERR{0x34,0x00}`
+        ⇒ 这里捕获 `UnsupportedByFirmwareError` 并回 `False`。
+
+        ⚠ **不能用 `0x30`/`0x33` 探测** —— 旧固件对它们有**显式 case** 回 `ERR{cmd,0x02}`
+        (与"数值非法"同码), 无法区分「无此命令」与「参数非法」。
+        """
+        try:
+            self.get_body(0)
+            return True
+        except UnsupportedByFirmwareError:
+            return False
+
+    def get_body(self, idx: int, timeout: float = 1.0) -> "Msg[List[float]]":
+        """读**生效 bank** 的第 `idx` 个刚体 (0..8) 的 10 个参数。
+
+        读的是 bank (无 payload 复合), 故与 `set_body` 写入值可直接逐项比对。
+        返回 :class:`Msg` 信封 (帧 id `RSP_MODEL_PARAM`); 单发请求/应答式, 见 `Msg`。
+        """
+        idx = self._check_body_idx(idx)
+        arm = self._arm
+        arm._write_query(P.CMD_GET_MODEL_PARAM, bytes([idx]))
+        _, p = arm._require().expect(P.RSP_MODEL_PARAM, timeout, f"model.get_body({idx})",
+                                     echo_cmd=P.CMD_GET_MODEL_PARAM)
+        if len(p) != 2 + 4 * MODEL_BODY_PARAMS:
+            raise TransportError(f"RSP_MODEL_PARAM 帧长 {len(p)}B, 期望 {2 + 4 * MODEL_BODY_PARAMS}B")
+        if p[0] != P.RSP_MODEL_PARAM:
+            raise TransportError(f"RSP_MODEL_PARAM 帧 id 0x{p[0]:02X}, 期望 0x{P.RSP_MODEL_PARAM:02X}")
+        if p[1] != idx:
+            raise TransportError(f"RSP_MODEL_PARAM 回显 body_idx={p[1]}, 期望 {idx}")
+        return arm._msg(list(struct.unpack_from("<" + "f" * MODEL_BODY_PARAMS, p, 2)),
+                        P.RSP_MODEL_PARAM)
+
+    # ---- 0x30 ----
+    def set_body(self, idx: int, vals: Sequence[float]) -> None:
+        """写 staging 的第 `idx` 个刚体。**不生效**, 须 `commit()`。
+
+        固件拒绝 (回 `ERR{0x30,0x02}`) 的情形: 索引越界 / 任一分量非有限或 |v|>1e6 /
+        `idx == 8` (ee 固定体, payload 复合位) 而质量非 0。
+        """
+        idx = self._check_body_idx(idx)
+        payload = bytes([idx]) + struct.pack("<" + "f" * MODEL_BODY_PARAMS,
+                                             *_check_body_vals(vals))
+        self._arm._cmd(P.CMD_SET_MODEL_PARAM, payload, f"model.set_body({idx})")
+
+    # ---- 0x33 ----
+    def get_jm(self, timeout: float = 1.0) -> "Msg[List[float]]":
+        """读**生效 bank** 的 jm (7 个关节转子惯量)。
+
+        返回 :class:`Msg` 信封 (帧 id `RSP_MODEL_JM`); 单发请求/应答式, 见 `Msg`。
+        """
+        arm = self._arm
+        arm._write_query(P.CMD_GET_MODEL_JM, b"")
+        _, p = arm._require().expect(P.RSP_MODEL_JM, timeout, "model.get_jm",
+                                     echo_cmd=P.CMD_GET_MODEL_JM)
+        if len(p) != 1 + 4 * MODEL_JM_N:
+            raise TransportError(f"RSP_MODEL_JM 帧长 {len(p)}B, 期望 {1 + 4 * MODEL_JM_N}B")
+        if p[0] != P.RSP_MODEL_JM:
+            raise TransportError(f"RSP_MODEL_JM 帧 id 0x{p[0]:02X}, 期望 0x{P.RSP_MODEL_JM:02X}")
+        return arm._msg(list(struct.unpack_from("<" + "f" * MODEL_JM_N, p, 1)),
+                        P.RSP_MODEL_JM)
+
+    def set_jm(self, vals: Sequence[float]) -> None:
+        """写 staging 的 jm[7]。**不生效**, 须 `commit()`。"""
+        if len(vals) != MODEL_JM_N:
+            raise InvalidCommandError(f"jm 须为 {MODEL_JM_N} 个, 给的是 {len(vals)} 个")
+        payload = struct.pack("<" + "f" * MODEL_JM_N, *[float(v) for v in vals])
+        self._arm._cmd(P.CMD_SET_MODEL_JM, payload, "model.set_jm")
+
+    # ---- 0x32 ----
+    def commit(self, expected_mask: int) -> None:
+        """staging 整组原子生效。
+
+        `expected_mask` 必须与固件侧"本次会话实际写过的项"逐位相等, 否则回
+        `ERR{0x32,0x07}` (掩码不符 —— 与"数值非法"分开的码, 便于产线定位)。
+        零模型哨兵不过回 `ERR{0x32,0x02}`; 已武装回 `ERR{0x32,0x04}`。
+
+        ⚠ u16 **LE** —— `_protocol` 没有 u16 pack helper, 必须显式 `struct.pack("<H", ...)`;
+        写成 `bytes([mask])` 会截断成 1 字节 (`0x2FE & 0xFF = 0xFE`) 而固件 `len < 2` 直接拒。
+        """
+        self._arm._cmd(P.CMD_MODEL_COMMIT, struct.pack("<H", int(expected_mask)),
+                       "model.commit", timeout=2.5)
+
+    # ---- 0x37 ----
+    def revert(self) -> None:
+        """**只回退模型**: bank 回编译期常量 (含 jm) + 清 staging。须失能。
+
+        ⚠ 三条后果 (工具/产线必须知道):
+        1. **不动 flash** ⇒ flash 里的导入模型仍在, **重新上电会复活**。
+        2. 此后任何一次 `save_params()` 会把 flash 里的导入模型**一并抹掉**
+           (整扇区擦除, 旧记录不可恢复)。
+        3. 回退后 `status().dirty == 1` (RAM != flash), 但**不要**据此提示"补固化"。
+        """
+        self._arm._cmd(P.CMD_REVERT_MODEL, b"", "model.revert", timeout=2.5)
+
+    # ---- 0x38 ----
+    def status(self, timeout: float = 1.0) -> "Msg[ModelStatus]":
+        """读模型状态 (override / staged_mask / dirty)。
+
+        返回 :class:`Msg` 信封 (帧 id `RSP_MODEL_STATUS`); 单发请求/应答式, 见 `Msg`。
+        """
+        arm = self._arm
+        arm._write_query(P.CMD_GET_MODEL_STATUS, b"")
+        _, p = arm._require().expect(P.RSP_MODEL_STATUS, timeout, "model.status",
+                                     echo_cmd=P.CMD_GET_MODEL_STATUS)
+        return arm._msg(ModelStatus.decode(p), P.RSP_MODEL_STATUS)
+
+    # ---- 0x39 ----
+    def get_gravity(self, q: Sequence[float], timeout: float = 1.0) -> "Msg[List[float]]":
+        """给定关节角算重力项 `G(q)` (纯读, 无门控, 不改任何状态)。
+
+        产线的「静态重力核查」用它: 到位静止后比对实测 τ 与 `G(q)` —— 这是唯一能抓住
+        「错台 yaml / 重力符号错 / 模型没生效」的判据 (读回比对只能证明字节落位)。
+
+        返回 :class:`Msg` 信封 (帧 id `RSP_GRAVITY`); 单发请求/应答式, 见 `Msg`。
+        """
+        q = [float(v) for v in q]
+        if len(q) != MODEL_JM_N:
+            raise InvalidCommandError(f"q 须为 {MODEL_JM_N} 个, 给的是 {len(q)} 个")
+        arm = self._arm
+        arm._write_query(P.CMD_GET_GRAVITY, struct.pack("<" + "f" * MODEL_JM_N, *q))
+        _, p = arm._require().expect(P.RSP_GRAVITY, timeout, "model.get_gravity",
+                                     echo_cmd=P.CMD_GET_GRAVITY)
+        if len(p) != 1 + 4 * MODEL_JM_N:
+            raise TransportError(f"RSP_GRAVITY 帧长 {len(p)}B, 期望 {1 + 4 * MODEL_JM_N}B")
+        if p[0] != P.RSP_GRAVITY:
+            raise TransportError(f"RSP_GRAVITY 帧 id 0x{p[0]:02X}, 期望 0x{P.RSP_GRAVITY:02X}")
+        return arm._msg(list(struct.unpack_from("<" + "f" * MODEL_JM_N, p, 1)),
+                        P.RSP_GRAVITY)

litearm/params.py

@@ -0,0 +1,120 @@
+"""关节级运行时参数 —— 固件 `CMD_SET_JOINT_PARAM(0x22)` / `SET_JOINT_LIMITS(0x23)` /
+`GET_JOINT_PARAM(0x24)` / `PARAM_RESET(0x36)` 的面向对象封装。
+
+与 FF/动力学调参族 (留在 `Arm` 上) 的分工: 那一族是**前馈/控制律**参数
+(0x26/0x27/0x28/0x31 + 读回 0x2B/0x2C); 本模块是**关节级** MIT 刚度/阻尼/力矩钳幅与
+软限位。两者都是 RAM 生效, 持久化统一走 `Arm.save_params()` (0x25)。
+
+固件侧载荷与错误码见 `litearm-stm32/User/litearm/hal/usb_cmd.c` 的
+`CMD_SET_JOINT_PARAM`/`CMD_SET_JOINT_LIMITS`/`CMD_GET_JOINT_PARAM`/`CMD_PARAM_RESET` 分支。
+"""
+from __future__ import annotations
+
+import struct
+from dataclasses import dataclass
+from typing import TYPE_CHECKING
+
+from litearm import _protocol as P
+from litearm.errors import InvalidCommandError, TransportError
+
+if TYPE_CHECKING:                       # 避免与 arm 循环导入
+    from litearm.arm import Arm, Msg
+
+__all__ = ["JointParam", "JointParams"]
+
+
+@dataclass(frozen=True)
+class JointParam:
+    """固件 `RSP_JOINT_PARAM(0x49)` 的 21B 应答: idx + kp,kd,tau_max,q_min,q_max。"""
+
+    idx: int
+    kp: float
+    kd: float
+    tau_max: float
+    q_min: float
+    q_max: float
+
+    @classmethod
+    def decode(cls, payload: bytes) -> "JointParam":
+        if len(payload) < 21:
+            raise TransportError(f"RSP_JOINT_PARAM 帧短 ({len(payload)}B, 期望 21B)")
+        idx = payload[0]
+        kp, kd, tm, lo, hi = struct.unpack_from("<fffff", payload, 1)
+        return cls(idx, kp, kd, tm, lo, hi)
+
+
+class JointParams:
+    """`arm.params` —— 关节级参数的读写入口 (RAM 生效)。"""
+
+    def __init__(self, arm: "Arm"):
+        self._arm = arm
+
+    # ---- 内部 ----
+    def _check_idx(self, idx: int) -> int:
+        idx = int(idx)
+        n = self._arm.n
+        if not 0 <= idx < max(n, 1):
+            raise InvalidCommandError(f"关节索引越界: {idx} (有效 0..{n - 1})")
+        return idx
+
+    # ---- 0x22 ----
+    def set_joint_param(self, idx: int, kp: float, kd: float, tau_max: float) -> None:
+        """改写单关节 MIT 刚度/阻尼/力矩钳幅 (RAM; 须 `save_params()` 才持久化)。
+
+        固件侧还有一道值合法性校验 (`params_set_joint_ctrl`), 非法时回 `ERR{0x22,0x02}`。
+        """
+        idx = self._check_idx(idx)
+        payload = bytes([idx]) + struct.pack("<fff", float(kp), float(kd), float(tau_max))
+        self._arm._cmd(P.CMD_SET_JOINT_PARAM, payload, f"set_joint_param(J{idx + 1})")
+
+    # ---- 0x23 ----
+    def set_joint_limits(self, idx: int, q_min: float, q_max: float) -> None:
+        """改写单关节软限位 (RAM)。固件要求 `q_min < q_max`。
+
+        ⚠⚠ **写入是"照编译期默认值取交集", 而且静默** —— 固件只收窄: 放宽请求
+        (**包括写回当前值**) 会被**接受、回 ACK、且值不变**, 不报错。
+        所以本方法**是幂等的**, 但**不能拿它当写口自检** —— 想确认写没写进去只能
+        :meth:`get_joint_param` 回读。
+        `ERR{0x23,0x02}` 的触发条件是 NaN / 交集为空 (`q_min >= q_max`) / idx 越界,
+        **不含放宽** (固件原文: "固件只许收窄, 照编译期默认值取交集")。
+        ⚠ **收窄在 RAM 里不可逆**: 交集只会越取越小, 不重启回不去。
+        """
+        idx = self._check_idx(idx)
+        if not float(q_min) < float(q_max):
+            raise InvalidCommandError(f"软限位需 q_min < q_max (给的是 {q_min}, {q_max})")
+        payload = bytes([idx]) + struct.pack("<ff", float(q_min), float(q_max))
+        self._arm._cmd(P.CMD_SET_JOINT_LIMITS, payload, f"set_joint_limits(J{idx + 1})")
+
+    # ---- 0x24 ----
+    def get_joint_param(self, idx: int, timeout: float = 1.0) -> "Msg[JointParam]":
+        """读回单关节参数 (与 `set_joint_param`/`set_joint_limits` 构成写→读回闭环)。
+
+        返回 :class:`Msg` 信封 (帧 id `RSP_JOINT_PARAM`)。单发请求/应答式 ⇒ 第一次调用
+        `hz == 0.0`, 第二次起等于本调用方自己的轮询频率 (见 `Msg`)。
+        """
+        idx = self._check_idx(idx)
+        arm = self._arm
+        arm._write_query(P.CMD_GET_JOINT_PARAM, bytes([idx]))      # 查询类: 不受守卫限制
+        _, p = arm._require().expect(P.RSP_JOINT_PARAM, timeout,
+                                     f"get_joint_param(J{idx + 1})",
+                                     echo_cmd=P.CMD_GET_JOINT_PARAM)
+        return arm._msg(JointParam.decode(p), P.RSP_JOINT_PARAM)
+
+    def all_joint_params(self) -> list:
+        """逐关节读回全部参数 (N 次往返)。
+
+        ⚠ 返回 `list[JointParam]` —— **不是** `list[Msg]`: 它是 N 次往返的**聚合**
+        (N 帧、N 个到达时刻), 一个 `hz`/`timestamp` 描述不了它; 要单帧的信封请调
+        `get_joint_param(i)`。
+        """
+        return [self.get_joint_param(i).value for i in range(self._arm.n)]
+
+    # ---- 0x36 ----
+    def reset_factory(self) -> None:
+        """恢复出厂默认 + 失效 flash (固件 0x36)。
+
+        ⚠ 固件**要求失能态** (擦写窗口 CPU 停顿, 电机不能在无监督下保持使能),
+        已武装时回 `ERR{0x36,0x04}`; 本方法不代劳 `disable()`, 以免替调用方做安全决策。
+        ⚠ 失败时固件保持 RAM 与 flash 一致 (先失效 flash 再重载 RAM), 回出的 ERR 是真话。
+        """
+        self._arm._cmd(P.CMD_PARAM_RESET, b"", "reset_factory", timeout=2.5)