启元 Q1 关节指令预检器：URDF 限位拦越界指令

摘要

把 left_elbow 写成 2.2 的那一刻，我停下来去翻了 q1_v3.urdf------上限只有 1.6581。这个差点犯下的错，催生了 check_cmd.py：一个纯标准库脚本，把 URDF 的 22 项可动关节限位抽成白名单 JSON，在指令发布前做五态判定（OK / WARN / NG / UNKNOWN / FIXED）。三个陷阱全是实测跑出来的：head_joint 判 UNKNOWN、left_wrist_yaw_joint 判 FIXED、left_knee=2.44 rad 余量只剩 0.2° 判 WARN。文章给出离线批量校验（CI 阻断合并）与 ROS 2 在线拦截封装（焊死发布出口）的双模式设计，无真机也能把越界指令拦在 publish 之前，适合想在开发期就设护栏的 Q1 开发者。

📌 版本声明 ：本文基于 PrimeBot Q1 SDK（aimdk_msgs / primebot-q1-sdk），配套官方文档 dev.primebot.com，无真机离线分析。所有限位数据来自仓库 q1_v3.urdf 与脚本生成的 joint_whitelist.json，未做任何臆造。

文章目录

- 摘要
- 一、为什么需要指令预检器
- [二、URDF 限位与白名单：预检器的知识底座](#二、URDF 限位与白名单：预检器的知识底座)
- - [2.1 Q1 的关节拓扑](#2.1 Q1 的关节拓扑)
  - [2.2 限位以弧度存储：第一个单位陷阱](#2.2 限位以弧度存储：第一个单位陷阱)
  - [2.3 从 25 关节到 22 可动：白名单的本质](#2.3 从 25 关节到 22 可动：白名单的本质)
  - [2.4 为什么是白名单，而不是黑名单](#2.4 为什么是白名单，而不是黑名单)
- 三、环境准备与素材来源（无真机）
- - [3.1 依赖与运行环境](#3.1 依赖与运行环境)
  - [3.2 为什么"无真机"也能验证逻辑正确](#3.2 为什么“无真机”也能验证逻辑正确)
- [四、check_cmd.py 主体：静态越界判定引擎](#四、check_cmd.py 主体：静态越界判定引擎)
- - [4.4 预检器的数据结构（classDiagram）](#4.4 预检器的数据结构（classDiagram）)
- [五、白名单 JSON 生成：从 25 关节到 22 可动](#五、白名单 JSON 生成：从 25 关节到 22 可动)
- - [5.1 白名单结构示例表](#5.1 白名单结构示例表)
  - [5.2 白名单热更新与版本漂移](#5.2 白名单热更新与版本漂移)
- 六、越界判定逻辑：五态而非三态
- - [6.1 五态判定状态图](#6.1 五态判定状态图)
- [七、离线校验 + 在线拦截双模式设计](#七、离线校验 + 在线拦截双模式设计)
- - [7.1 离线模式：开发期批量体检](#7.1 离线模式：开发期批量体检)
  - [7.2 在线模式：发布前最后一关](#7.2 在线模式：发布前最后一关)
  - [7.3 双模式数据流](#7.3 双模式数据流)
- 八、三个真实陷阱剖析
- - [8.1 陷阱一：head_joint → UNKNOWN（名字在 URDF，却不可动）](#8.1 陷阱一：head_joint → UNKNOWN（名字在 URDF，却不可动）)
  - [8.2 陷阱二：left_wrist_yaw_joint → FIXED（带 limit 的 fixed）](#8.2 陷阱二：left_wrist_yaw_joint → FIXED（带 limit 的 fixed）)
  - [8.3 陷阱三：left_knee_joint = 2.44 rad → WARN（余量仅 0.2°）](#8.3 陷阱三：left_knee_joint = 2.44 rad → WARN（余量仅 0.2°）)
  - [8.4 判定结果对照表](#8.4 判定结果对照表)
- 九、批量校验与单元测试：让护栏可信可回归
- - [9.1 批量校验脚本（bash）](#9.1 批量校验脚本（bash）)
  - [9.2 单元测试示例（python）](#9.2 单元测试示例（python）)
- 十、在线拦截封装：把护栏焊进发布动作
- - [10.1 NG 之后的三种处置：丢弃、裁剪、急停](#10.1 NG 之后的三种处置：丢弃、裁剪、急停)
- 十一、适用边界与风险提示
- - [11.1 它拦得住什么](#11.1 它拦得住什么)
  - [11.2 它拦不住什么](#11.2 它拦不住什么)
  - [11.3 版本与模型差异](#11.3 版本与模型差异)
- 十二、总结
- 参考资料

一、为什么需要指令预检器

我最初没打算写什么预检器。真正动手的由头，是一次差点犯的错。

整理一份站立预设的关节角时，left_elbow 我想当然写了 2.2，脑子里的意图是"抬手"。写完之后准备往指令 JSON 里放，手指停在半空------突然有个声音问自己：2.2 是什么单位？这条关节的上限是多少？超了会怎样？我停下手，打开 q1_v3.urdf 搜 left_elbow_joint，<limit lower="-0.87266" upper="1.6581"/>。1.6581 rad，换算过来是 95°。而我那个 2.2 rad 是 126°，超了 31.04°。

手写弧度这事，单位、符号、上下界都能弄错，一不留神就把膝关节顶到 140° 之外、把本不该动的头关节写进去。没有真机，这个错本来不会有人提醒我。离线环境里 JSON 能解析、可视化能加载、脚本一声不吭，一切"看起来都对"。等哪天连上真机，驱动器会在 95° 处被硬限位卡住，关节瞬间受阻、电流飙升，轻则动作中断、日志里留下一条 reason=7（不在白名单/越界）的失败，重则触发底层保护让整机进入阻尼急停。这类事故的共性，是错误在"发出去那一刻"才被机器发现，而代价已经产生。

说白了，问题不在于"能不能发"，而在于"发之前谁来拦"。

顺着这个念头做下去，就有了这篇东西：把 URDF 里已经写好的合法边界，变成一份程序可引用的护栏。它只做一件事------在你把指令 publish 出去之前，先用静态计算判定这条指令会不会越界。这件事不需要真机，只需要一份 URDF 和一个纯标准库的 Python 脚本。

教程讲"怎么发"的多，讲"发之前怎么拦"的少。这是同一套关节控制里被普遍跳过的一半，而 Q1 的 URDF 早就把边界写好了，缺的只是有人把它读出来。

二、URDF 限位与白名单：预检器的知识底座

要拦住越界指令，先得知道"界"在哪。Q1 的合法边界不藏在任何文档的表格里，就写在 q1_v3.urdf 的 <joint> 标签中。我是把 URDF 拖进编辑器逐个 <joint> 翻完之后，才对这台机器的关节构成有实感的。

2.1 Q1 的关节拓扑

q1_v3.urdf 描述了一台 26 个 link、25 个 joint 的双足机器人。根 link 是 pelvis（骨盆），整机质量 16.9173 kg。25 个关节里，有 3 个是 fixed（固定连接，不贡献自由度），其余 22 个全部是 revolute（旋转关节，可动）。

这 22 个可动关节按部位分组是：左腿 6（髋三 + 膝 + 踝俯仰 + 踝翻转）、右腿 6、腰 2（roll + yaw）、左臂 4（肩三 + 肘）、右臂 4，合计 22。3 个 fixed 关节分别是 head_joint、left_wrist_yaw_joint、right_wrist_yaw_joint------头与双腕是刚性连接，本就不该被当作可控自由度写进指令。

有个细节我一开始没在意：pelvis 作为根 link 同时也是整机质量中枢，16.9173 kg 的配平分析显示下肢与躯干占主要质量。这也解释了膝关节限位为何被设计得如此宽：left_knee 的 [0.0, 2.4435]（0°~140°）要覆盖从站起到深蹲的完整行程。偏偏正是这段宽行程，让"贴边"风险更隐蔽------你以为还在安全区，其实离 140° 只差 0.2°，下一帧误差就能把你顶进越界区。

2.2 限位以弧度存储：第一个单位陷阱

URDF 的 <limit lower="..." upper="..."> 存的永远是弧度（rad） ，不是角度（deg）。新手最易踩的坑在这：left_knee 的 upper=2.4435 实际等于 140.0°，left_hip_pitch 的 lower=-3.0543 等于 -175.0°。如果你误以为这些数字是"度"直接填进指令，结果会差出近 57 倍，必然越界。

我自己的顺序是：先撞了 2.2 那个值，再撞这个单位。两次有个共同点------都是"我以为"开头。所以预检器的第一原则：内部统一用弧度比较，只在给人看的输出里换算成角度。贴着 URDF 原值走，换算只发生一次，在打印那一刻，不在代码里反复做。

2.3 从 25 关节到 22 可动：白名单的本质

URDF 给了 25 个 joint 的定义，但其中有 3 个 fixed 关节并不具备真实运动自由度。如果预检器对"每个 joint"一视同仁，就会把头、腕这些固定结构也当成可下发目标，反而制造噪声。

我的做法是把 22 个 revolute 关节的名字 + 弧度限位，单独导出成 joint_whitelist.json。这份白名单就是预检器的"真理来源"：凡是名字不在白名单里的指令，要么拼写错，要么本就不可动------一律拦截。它把"URDF 里所有关节"收敛成"开发者可以安全下发的关节集合"。

2.4 为什么是白名单，而不是黑名单

安全设计里长期存在两种思路：黑名单（列举"禁止的值"）与白名单（列举"允许的值"）。对关节指令而言，黑名单几乎不可用------你无法穷举所有非法弧度，关节又是连续量，越界值有无限多个，只要漏写一个，危险就漏过去了。

白名单的反面是"默认拒绝"：任何不在 22 项可动集合里的名字、任何超出对应区间的数值，一律不放行。它的安全性不依赖"你有没有想到所有错误"，而只依赖"URDF 有没有把正确边界写全"。由于 URDF 由厂商保证，护栏的信任链就从"开发者记得所有坑"转移到了"模型定义本身"。我选白名单，理由就是这个，工程上可维护。

这也解释了为什么要把 UNKNOWN 和 FIXED 分开：UNKNOWN 是"名字都不在白名单"，属于默认拒绝的兜底；FIXED 是"名字在、但本就不该动"。两者都走拦截，但诊断信息不同，排错时一眼就能区分"拼写错了"还是"设计误解了"。

图1：关节指令预检器嵌在业务层与机器人之间的安全闸

三、环境准备与素材来源（无真机）

先说跑通预检器需要什么。答案是几乎什么都不需要------目标就是"没有真机也能验证"。

3.1 依赖与运行环境

预检器主体 check_cmd.py 只用 Python 3 标准库（json、math、sys、xml.etree.ElementTree），不依赖 ROS、不依赖 aimdk_msgs、不需要编译任何 SDK。我的开发环境是 Ubuntu 22.04 + ROS 2 Humble，官方唯一支持的开发组合，但跑这个脚本，连 ROS 都不用装。这意味着你在一台只有 Python 3 的笔记本上就能复现全文所有结论，与是否连接 Q1 完全无关。

唯一外部输入是 q1_v3.urdf 本体和从它生成的 joint_whitelist.json。这两个文件都来自官方仓库，无需真机参与。验证方式也很直接：跑完脚本后查看终端打印的判定表，对照本文的实测结果即可确认护栏生效。脚本在 Python 3.8+ 验证通过，不依赖任何第三方包；标准库 xml.etree 的解析行为在各版本一致，若你用更早的 3.6/3.7，只需注意 f-string 语法（本文已全程使用）即可，无需其他改动。

3.2 为什么"无真机"也能验证逻辑正确

关节限位是模型的静态属性，越界判定是纯数学比较（一次减法、一次除法），不依赖任何实时状态。真机在收到指令后做的"前哨检查（前馈限位校验）"，本质上也是拿指令值去比 URDF/驱动器的边界。

我们用脚本在离线环境把同一个比较提前做掉，只是把"时间"从运行时挪到了开发时。只要 URDF 是那份真实 URDF，离线算出来的 OK / NG 结论，就和真机前哨检查的结论一致。这就是"无真机可验证"的全部底气：我们不是在模拟机器人，而是在复用它的边界定义。

没有真机时，我这样验证"预检器本身会不会拦"：准备一条故意越界的指令 JSON（例如 left_elbow=2.2），跑 check_cmd.py 确认它输出 NG；再准备一条合法指令确认输出 OK。这一步等价于单元测试里"先让失败发生，再确认失败被正确捕获"，能证明护栏确实在生效，而不是静默放行一切。它不要求机器人，只要求你愿意先亲手触发一次预期中的错误。

四、check_cmd.py 主体：静态越界判定引擎

预检器主程序的核心骨架如下。它读取 URDF、构造限位字典、对每条指令调用判定函数，并顺手导出白名单。整段逻辑没有任何网络调用，纯本地静态计算。

python 复制代码

#!/usr/bin/env python3
"""check_cmd.py ------ Q1 关节指令静态预检器（纯标准库，不发 ROS 消息）"""
import json, math, sys, xml.etree.ElementTree as ET
MOVABLE = {"revolute", "continuous", "prismatic"}

def load_limits(path):
    root = ET.parse(path).getroot()
    out = {}
    for j in root.findall("joint"):
        lim = j.find("limit")
        if lim is None:
            continue
        out[j.get("name")] = {"type": j.get("type"),
            "movable": j.get("type") in MOVABLE,
            "lower": float(lim.get("lower", "nan")),
            "upper": float(lim.get("upper", "nan"))}
    return out


def check(limits, name, target, warn_ratio=0.9):
    if name not in limits:
        return "UNKNOWN", "URDF 中不存在该关节名"
    info = limits[name]
    if not info["movable"]:
        return "FIXED", f"{info['type']} 关节，不贡献自由度"
    lo, hi = info["lower"], info["upper"]
    if target < lo:
        return "NG", f"越下界 {math.degrees(lo - target):.2f}°"
    if target > hi:
        return "NG", f"越上界 {math.degrees(target - hi):.2f}°"
    margin = min(target - lo, hi - target)
    if hi - lo > 0 and margin / (hi - lo) < (1 - warn_ratio):
        return "WARN", f"余量仅 {math.degrees(margin):.2f}°"
    return "OK", f"余量 {math.degrees(margin):.2f}°"


def main():
    limits = load_limits(sys.argv[1])
    inline = sys.argv[2] == "--inline"
    cmd = {k: float(v) for k, v in (p.split("=") for p in sys.argv[3].split(","))} if inline else json.load(open(sys.argv[2]))
    for name, target in cmd.items():
        print(name, check(limits, name, target))
    wl = {k: [round(v["lower"], 5), round(v["upper"], 5)]
          for k, v in limits.items() if v["movable"]}
    json.dump(wl, open("joint_whitelist.json", "w"), ensure_ascii=False, indent=2)


if __name__ == "__main__":
    main()

代码功能 ：load_limits 用标准库 xml.etree.ElementTree 解析 URDF，遍历每个 <joint>，只保留带 <limit> 标签的关节，记下它的类型与上下界弧度。check 是判定核心，返回五态之一。main 支持两种输入：从 JSON 文件读指令，或用 --inline "name=rad,..." 临时校验单条。

参数含义 ：warn_ratio=0.9 表示"当指令到最近限位的距离不足整个行程的 10%（即 1-warn_ratio）时判 WARN"；MOVABLE 集合枚举了所有"贡献自由度"的关节类型。注意 load_limits 里 if lim is None: continue 这一行，它直接决定了后面 fixed 关节会走 UNKNOWN 还是 FIXED 分支，是陷阱二、三的根源。

使用注意 ：不要把 URDF 路径写错，否则 limits 为空、所有指令都 UNKNOWN。导出白名单时建议固定 round(..., 5)，避免浮点尾巴污染后续比较。脚本本身不依赖 ROS，但如果你要把结果接进 ROS 节点，需自行把 sys.argv 换成话题回调参数。

4.4 预检器的数据结构（classDiagram）

主体代码看完，用一张类图把预检器的关键数据结构固化下来。它对应 check_cmd.py 里真实存在的字典与函数，不是另起一套抽象。
#mermaid-svg-D5kx87BzTDJQ9bOY{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-D5kx87BzTDJQ9bOY .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-D5kx87BzTDJQ9bOY .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-D5kx87BzTDJQ9bOY .error-icon{fill:#552222;}#mermaid-svg-D5kx87BzTDJQ9bOY .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-D5kx87BzTDJQ9bOY .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-D5kx87BzTDJQ9bOY .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-D5kx87BzTDJQ9bOY .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-D5kx87BzTDJQ9bOY .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-D5kx87BzTDJQ9bOY .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-D5kx87BzTDJQ9bOY .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-D5kx87BzTDJQ9bOY .marker{fill:#333333;stroke:#333333;}#mermaid-svg-D5kx87BzTDJQ9bOY .marker.cross{stroke:#333333;}#mermaid-svg-D5kx87BzTDJQ9bOY svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-D5kx87BzTDJQ9bOY p{margin:0;}#mermaid-svg-D5kx87BzTDJQ9bOY g.classGroup text{fill:#9370DB;stroke:none;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:10px;}#mermaid-svg-D5kx87BzTDJQ9bOY g.classGroup text .title{font-weight:bolder;}#mermaid-svg-D5kx87BzTDJQ9bOY .cluster-label text{fill:#333;}#mermaid-svg-D5kx87BzTDJQ9bOY .cluster-label span{color:#333;}#mermaid-svg-D5kx87BzTDJQ9bOY .cluster-label span p{background-color:transparent;}#mermaid-svg-D5kx87BzTDJQ9bOY .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-D5kx87BzTDJQ9bOY .cluster text{fill:#333;}#mermaid-svg-D5kx87BzTDJQ9bOY .cluster span{color:#333;}#mermaid-svg-D5kx87BzTDJQ9bOY .nodeLabel,#mermaid-svg-D5kx87BzTDJQ9bOY .edgeLabel{color:#131300;}#mermaid-svg-D5kx87BzTDJQ9bOY .edgeLabel .label rect{fill:#ECECFF;}#mermaid-svg-D5kx87BzTDJQ9bOY .label text{fill:#131300;}#mermaid-svg-D5kx87BzTDJQ9bOY .labelBkg{background:#ECECFF;}#mermaid-svg-D5kx87BzTDJQ9bOY .edgeLabel .label span{background:#ECECFF;}#mermaid-svg-D5kx87BzTDJQ9bOY .classTitle{font-weight:bolder;}#mermaid-svg-D5kx87BzTDJQ9bOY .node rect,#mermaid-svg-D5kx87BzTDJQ9bOY .node circle,#mermaid-svg-D5kx87BzTDJQ9bOY .node ellipse,#mermaid-svg-D5kx87BzTDJQ9bOY .node polygon,#mermaid-svg-D5kx87BzTDJQ9bOY .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-D5kx87BzTDJQ9bOY .divider{stroke:#9370DB;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY g.clickable{cursor:pointer;}#mermaid-svg-D5kx87BzTDJQ9bOY g.classGroup rect{fill:#ECECFF;stroke:#9370DB;}#mermaid-svg-D5kx87BzTDJQ9bOY g.classGroup line{stroke:#9370DB;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY .classLabel .box{stroke:none;stroke-width:0;fill:#ECECFF;opacity:0.5;}#mermaid-svg-D5kx87BzTDJQ9bOY .classLabel .label{fill:#9370DB;font-size:10px;}#mermaid-svg-D5kx87BzTDJQ9bOY .relation{stroke:#333333;stroke-width:1;fill:none;}#mermaid-svg-D5kx87BzTDJQ9bOY .dashed-line{stroke-dasharray:3;}#mermaid-svg-D5kx87BzTDJQ9bOY .dotted-line{stroke-dasharray:1 2;}#mermaid-svg-D5kx87BzTDJQ9bOY #compositionStart,#mermaid-svg-D5kx87BzTDJQ9bOY .composition{fill:#333333!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY #compositionEnd,#mermaid-svg-D5kx87BzTDJQ9bOY .composition{fill:#333333!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY #dependencyStart,#mermaid-svg-D5kx87BzTDJQ9bOY .dependency{fill:#333333!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY #dependencyStart,#mermaid-svg-D5kx87BzTDJQ9bOY .dependency{fill:#333333!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY #extensionStart,#mermaid-svg-D5kx87BzTDJQ9bOY .extension{fill:transparent!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY #extensionEnd,#mermaid-svg-D5kx87BzTDJQ9bOY .extension{fill:transparent!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY #aggregationStart,#mermaid-svg-D5kx87BzTDJQ9bOY .aggregation{fill:transparent!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY #aggregationEnd,#mermaid-svg-D5kx87BzTDJQ9bOY .aggregation{fill:transparent!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY #lollipopStart,#mermaid-svg-D5kx87BzTDJQ9bOY .lollipop{fill:#ECECFF!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY #lollipopEnd,#mermaid-svg-D5kx87BzTDJQ9bOY .lollipop{fill:#ECECFF!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-D5kx87BzTDJQ9bOY .edgeTerminals{font-size:11px;line-height:initial;}#mermaid-svg-D5kx87BzTDJQ9bOY .classTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-D5kx87BzTDJQ9bOY .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-D5kx87BzTDJQ9bOY .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-D5kx87BzTDJQ9bOY :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 复用
返回
内含
供比较
1
1
1
22
JointLimit
+str name
+str type
+float lower
+float upper
+bool movable
+degrees() : float
Whitelist
+dict data
+load(path) : Whitelist
+contains(name) : bool
+range_of(name) : tuple
CheckResult
+str level
+str message
+blocked() : bool
+warn() : bool
PreChecker
-Whitelist wl
-float warn_ratio
+init(wl, warn_ratio)
+check(name, target) : CheckResult
+verify_batch(cmds) : list

上图里 JointLimit 是 load_limits 产出的单关节记录，Whitelist 是 22 条 JointLimit 的集合（即 joint_whitelist.json 的内存形态），PreChecker 持有白名单与阈值并产出 CheckResult。把结构画清楚，后面写单元测试和在线封装时就能直接对照字段。

五、白名单 JSON 生成：从 25 关节到 22 可动

白名单不是手敲的，是脚本从 URDF 自动抽出来的。URDF 一旦更新，重跑一次脚本就能得到新的护栏，避免人和模型"各写一份边界"导致漂移。

python 复制代码

# build_whitelist.py ------ 从 URDF 生成 22 项可动关节白名单
import json
import xml.etree.ElementTree as ET

MOVABLE = {"revolute", "continuous", "prismatic"}


def build_whitelist(urdf_path, out_path="joint_whitelist.json"):
    root = ET.parse(urdf_path).getroot()
    wl = {}
    for j in root.findall("joint"):
        name = j.get("name")
        jt = j.get("type")
        lim = j.find("limit")
        # 仅收录带 limit 且类型可动的关节
        if lim is None or jt not in MOVABLE:
            continue
        lo = float(lim.get("lower"))
        hi = float(lim.get("upper"))
        wl[name] = [round(lo, 5), round(hi, 5)]
    with open(out_path, "w", encoding="utf-8") as f:
        json.dump(wl, f, ensure_ascii=False, indent=2)
    return wl


if __name__ == "__main__":
    wl = build_whitelist("q1_v3.urdf")
    print(f"已生成白名单，共 {len(wl)} 项可动关节")
    # 加载示例：运行时护栏直接读这份 JSON，而不是再解析 URDF
    with open("joint_whitelist.json", "r", encoding="utf-8") as f:
        runtime_wl = json.load(f)
    print("left_knee_joint 限位(rad):", runtime_wl["left_knee_joint"])

代码功能 ：build_whitelist 与 check_cmd.py 里的导出逻辑同源，但它把"是否收录"显式写成条件：lim is None 跳过（即 head_joint 这种无 limit 的 fixed 会被排除），jt not in MOVABLE 跳过（left_wrist_yaw_joint 虽带 limit 但 type=fixed 也被排除）。最终恰好得到 22 项 revolute 关节。

参数含义 ：round(lo, 5) 保留 5 位小数，既足够精确又不会因浮点尾数产生比较抖动。out_path 默认 joint_whitelist.json，与运行时护栏约定同名。加载示例演示了"运行期直接读 JSON、不再解析 URDF"，把校验成本从每次 O(解析 XML) 降到 O(查字典)。

使用注意 ：白名单必须和 URDF 同源、同步更新。若你换了另一份 URDF（例如全范围版 q1_v3_full_range.xml），限位会不同，必须重新生成白名单，否则护栏会按旧边界放行新模型下越界的指令。下表是生成结果的结构示例。

5.1 白名单结构示例表

下表取自 joint_whitelist.json，展示按部位归类的可动关节与弧度限位（角度列用于人工核对，脚本内部仍以弧度比较）。

部位	关节名	限位下限 (rad)	限位上限 (rad)	角度区间	类型
左腿	left_knee_joint	0.0	2.4435	0° ~ 140.0°	revolute
左腿	left_hip_pitch_joint	-3.0543	1.5708	-175.0° ~ 90.0°	revolute
左腿	left_hip_roll_joint	-0.69813	1.5708	-40.0° ~ 90.0°	revolute
左腿	left_hip_yaw_joint	-1.5708	1.5708	-90.0° ~ 90.0°	revolute
左臂	left_shoulder_roll_joint	-0.08727	2.7925	-5.0° ~ 160.0°	revolute
左臂	left_elbow_joint	-0.87266	1.6581	-50.0° ~ 95.0°	revolute
腰部	waist_roll_joint	-0.2618	0.2618	-15.0° ~ 15.0°	revolute
腰部	waist_yaw_joint	-1.5708	1.5708	-90.0° ~ 90.0°	revolute
右腿	right_knee_joint	0.0	2.4435	0° ~ 140.0°	revolute
右臂	right_elbow_joint	-0.87266	1.6581	-50.0° ~ 95.0°	revolute

说明：表中仅列 10 项作结构示例，完整白名单共 22 项。注意带符号的不对称区间（如 left_hip_roll 为 -40°~90°），这正是手写指令时最易弄反方向的地方。

5.2 白名单热更新与版本漂移

白名单不是一次生成就能一劳永逸。官方若发布新固件、调整 URDF（例如放宽某个关节行程、重命名某个关节），旧白名单会立刻从护栏退化成假护栏：它按旧边界放行新模型下越界的指令，或把新模型下的合法指令误判成 NG。所以 URDF 与白名单必须同源同步------每次模型更新，第一个动作就是重跑 build_whitelist，再把新旧两份 JSON 做 diff 评审，限位数值的任何变动都要人工确认合理性后才合入，而不是机械覆盖。

我的操作习惯：每次拿到新版 SDK，先不跑 demo，先 diff 新旧两份 q1_v3.urdf 的 <joint> 段；然后重跑 build_whitelist 生成新白名单，跟仓库里的旧版逐项对；最后跑一遍 test_check_cmd.py，确认五个用例还锁得住。三步走完，才谈别的。

麻烦在于，比限位数值漂移更隐蔽的是关节名漂移。URDF 升级可能重命名关节，也可能新增可动关节；此时若白名单没跟上，在线护栏会同时犯两种错：把仍然合法的旧名指令全部判成 UNKNOWN（缺关节，误拦），或对新关节完全放弃校验（多关节，漏拦）。前者毁掉可用性，后者毁掉安全性------都比没有护栏更糟，因为你会误以为护栏还在工作。

防御办法是让白名单与线上事实对齐：订阅 /aima/hal/joint/state，把消息里的关节名集合与白名单 key 做双向差集。白名单有而线上没有，告警缺关节；线上有而白名单没有，告警多关节、该校验未覆盖。这对齐检查要进两个地方：节点启动时核对一次，防止带着过期白名单上线；CI 里再核对一次，防止过期白名单合入仓库。无真机环境下，则可把一条 joint/state 快照落盘成 JSON，用同一个脚本离线核对，结论与在线等价。

python 复制代码

# check_whitelist_sync.py ------ 白名单与线上关节名一致性核对
# 用法：python3 check_whitelist_sync.py joint_whitelist.json joint_state.json
import json
import sys


def load_state_names(path):
    """从落盘的 JointState 快照读取关节名集合。
    真实环境替换为订阅 /aima/hal/joint/state 后取消息 name 字段，
    字段名以 aimdk_msgs 实际定义为准。"""
    with open(path, "r", encoding="utf-8") as f:
        data = json.load(f)
    return set(data["name"])


def diff_whitelist(whitelist, state_names):
    """双向差集：白名单有线上无 = 缺关节；线上有白名单无 = 多关节"""
    wl_names = set(whitelist.keys())
    missing = wl_names - state_names
    extra = state_names - wl_names
    return missing, extra


if __name__ == "__main__":
    wl_path, state_path = sys.argv[1], sys.argv[2]
    with open(wl_path, "r", encoding="utf-8") as f:
        whitelist = json.load(f)
    state_names = load_state_names(state_path)

    missing, extra = diff_whitelist(whitelist, state_names)
    if not missing and not extra:
        print(f"[OK] 白名单 {len(whitelist)} 项与线上关节名完全一致")
        sys.exit(0)

    for name in sorted(missing):
        print(f"[MISSING] 白名单存在但线上无此关节: {name}")
    for name in sorted(extra):
        print(f"[EXTRA] 线上存在但白名单未收录，将无校验下发: {name}")
    print("[FAIL] 白名单与线上关节名漂移，需按新 URDF 重新生成")
    sys.exit(1)

代码功能 ：diff_whitelist 把白名单 key 与线上关节名集合做双向差集。missing 是白名单收录但线上不存在的关节，多半是 URDF 删除了该关节而白名单未重生成，继续用它会把合法指令误拦成 UNKNOWN；extra 是线上存在但白名单未收录的关节，即新增关节，当前处于无校验的裸奔状态。任一集合非空都以非零码退出，可直接被启动脚本或 CI 消费。

参数含义 ：wl_path 指向运行期白名单 JSON；state_path 指向一条 joint/state 快照（含 name 列表的 JSON）。真实环境里把 load_state_names 换成订阅 /aima/hal/joint/state 后读取消息的 name 字段即可，字段名以 aimdk_msgs 实际定义为准。退出码 0/1 的设计，让脚本既能人读，又能被流水线判定。

使用注意 ：核对是集合级的，只保证名字对齐，不校验限位数值------数值一致性靠 diff 评审与"白名单必须来自 q1_v3.urdf"来保证。若固件允许线上动态增删关节，应把启动时的一次性核对升级为周期性核对，并分级告警：missing 多为误拦，extra 才是安全事故前兆。

图2：22 项可动关节限位速查卡（弧度与角度对照）

六、越界判定逻辑：五态而非三态

越界判定常被想成"通过 / 不通过"两态，最多加个"警告"。在 Q1 上，我们必须区分五种结论，否则会漏掉两类隐蔽错误。

python 复制代码

def check(limits, name, target, warn_ratio=0.9):
    """返回 (等级, 说明)。等级：OK / WARN / NG / UNKNOWN / FIXED"""
    # 1) 名字都不在 URDF -> 拼写错或压根不是关节
    if name not in limits:
        return "UNKNOWN", "URDF 中不存在该关节名"
    info = limits[name]
    # 2) 在 URDF 但类型是 fixed -> 固定连接，下发无意义
    if not info["movable"]:
        return "FIXED", f"{info['type']} 关节，不贡献自由度，下发了也不会动"
    lo, hi = info["lower"], info["upper"]
    # 3) 低于下界
    if target < lo:
        return "NG", f"越下界 {math.degrees(lo - target):.2f}°（限位 {math.degrees(lo):.1f}°）"
    # 4) 高于上界
    if target > hi:
        return "NG", f"越上界 {math.degrees(target - hi):.2f}°（限位 {math.degrees(hi):.1f}°）"
    # 5) 在界内但贴边 -> WARN；否则 OK
    span = hi - lo
    margin = min(target - lo, hi - target)
    if span > 0 and margin / span < (1 - warn_ratio):
        return "WARN", f"贴近限位，余量仅 {math.degrees(margin):.2f}°"
    return "OK", f"余量 {math.degrees(margin):.2f}°"

代码功能 ：check 把"该不该下发"拆成五步判定。前两步处理"名字/类型"问题，后三步处理"数值"问题。它返回的字符串等级可以直接驱动上层逻辑：NG / UNKNOWN / FIXED 必须拦截，WARN 放行但告警，OK 直接放行。

参数含义 ：warn_ratio 是"临界阈值系数"。当指令到最近限位的距离 margin 与总行程 span 之比小于 1-warn_ratio（默认 0.1）时判 WARN------默认只要指令挤进了最近限位 10% 行程以内就报警。说明文本里用 math.degrees 把弧度差转成角度，方便人读；但比较全程用弧度，保证精度。

使用注意 ：WARN 不是错误，是"黄色预警"。是否拦截 WARN 取决于业务：例如做全身动作控制时可以放行，但做贴近本体的精细抓放时建议拦截。math.degrees 的 .2f 仅为展示精度，判定本身不受显示格式影响。span > 0 这个守卫位很重要，避免连续关节（理论上 upper==lower）触发除零。

6.1 五态判定状态图

下面用状态图表达一条指令进入预检器后的流转。注意 UNKNOWN 与 FIXED 是两个"硬拦截"分支，它们跟数值无关，只跟名字/类型有关。
#mermaid-svg-2ZXSlZJutGjLFR1e{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-2ZXSlZJutGjLFR1e .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-2ZXSlZJutGjLFR1e .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-2ZXSlZJutGjLFR1e .error-icon{fill:#552222;}#mermaid-svg-2ZXSlZJutGjLFR1e .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-2ZXSlZJutGjLFR1e .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-2ZXSlZJutGjLFR1e .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-2ZXSlZJutGjLFR1e .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-2ZXSlZJutGjLFR1e .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-2ZXSlZJutGjLFR1e .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-2ZXSlZJutGjLFR1e .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-2ZXSlZJutGjLFR1e .marker{fill:#333333;stroke:#333333;}#mermaid-svg-2ZXSlZJutGjLFR1e .marker.cross{stroke:#333333;}#mermaid-svg-2ZXSlZJutGjLFR1e svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-2ZXSlZJutGjLFR1e p{margin:0;}#mermaid-svg-2ZXSlZJutGjLFR1e defs #statediagram-barbEnd{fill:#333333;stroke:#333333;}#mermaid-svg-2ZXSlZJutGjLFR1e g.stateGroup text{fill:#9370DB;stroke:none;font-size:10px;}#mermaid-svg-2ZXSlZJutGjLFR1e g.stateGroup text{fill:#333;stroke:none;font-size:10px;}#mermaid-svg-2ZXSlZJutGjLFR1e g.stateGroup .state-title{font-weight:bolder;fill:#131300;}#mermaid-svg-2ZXSlZJutGjLFR1e g.stateGroup rect{fill:#ECECFF;stroke:#9370DB;}#mermaid-svg-2ZXSlZJutGjLFR1e g.stateGroup line{stroke:#333333;stroke-width:1;}#mermaid-svg-2ZXSlZJutGjLFR1e .transition{stroke:#333333;stroke-width:1;fill:none;}#mermaid-svg-2ZXSlZJutGjLFR1e .stateGroup .composit{fill:white;border-bottom:1px;}#mermaid-svg-2ZXSlZJutGjLFR1e .stateGroup .alt-composit{fill:#e0e0e0;border-bottom:1px;}#mermaid-svg-2ZXSlZJutGjLFR1e .state-note{stroke:#aaaa33;fill:#fff5ad;}#mermaid-svg-2ZXSlZJutGjLFR1e .state-note text{fill:black;stroke:none;font-size:10px;}#mermaid-svg-2ZXSlZJutGjLFR1e .stateLabel .box{stroke:none;stroke-width:0;fill:#ECECFF;opacity:0.5;}#mermaid-svg-2ZXSlZJutGjLFR1e .edgeLabel .label rect{fill:#ECECFF;opacity:0.5;}#mermaid-svg-2ZXSlZJutGjLFR1e .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-2ZXSlZJutGjLFR1e .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-2ZXSlZJutGjLFR1e .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-2ZXSlZJutGjLFR1e .edgeLabel .label text{fill:#333;}#mermaid-svg-2ZXSlZJutGjLFR1e .label div .edgeLabel{color:#333;}#mermaid-svg-2ZXSlZJutGjLFR1e .stateLabel text{fill:#131300;font-size:10px;font-weight:bold;}#mermaid-svg-2ZXSlZJutGjLFR1e .node circle.state-start{fill:#333333;stroke:#333333;}#mermaid-svg-2ZXSlZJutGjLFR1e .node .fork-join{fill:#333333;stroke:#333333;}#mermaid-svg-2ZXSlZJutGjLFR1e .node circle.state-end{fill:#9370DB;stroke:white;stroke-width:1.5;}#mermaid-svg-2ZXSlZJutGjLFR1e .end-state-inner{fill:white;stroke-width:1.5;}#mermaid-svg-2ZXSlZJutGjLFR1e .node rect{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-2ZXSlZJutGjLFR1e .node polygon{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-2ZXSlZJutGjLFR1e #statediagram-barbEnd{fill:#333333;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-cluster rect{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-2ZXSlZJutGjLFR1e .cluster-label,#mermaid-svg-2ZXSlZJutGjLFR1e .nodeLabel{color:#131300;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-cluster rect.outer{rx:5px;ry:5px;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-state .divider{stroke:#9370DB;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-state .title-state{rx:5px;ry:5px;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-cluster.statediagram-cluster .inner{fill:white;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-cluster.statediagram-cluster-alt .inner{fill:#f0f0f0;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-cluster .inner{rx:0;ry:0;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-state rect.basic{rx:5px;ry:5px;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-state rect.divider{stroke-dasharray:10,10;fill:#f0f0f0;}#mermaid-svg-2ZXSlZJutGjLFR1e .note-edge{stroke-dasharray:5;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-note rect{fill:#fff5ad;stroke:#aaaa33;stroke-width:1px;rx:0;ry:0;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-note rect{fill:#fff5ad;stroke:#aaaa33;stroke-width:1px;rx:0;ry:0;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-note text{fill:black;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram-note .nodeLabel{color:black;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagram .edgeLabel{color:red;}#mermaid-svg-2ZXSlZJutGjLFR1e #dependencyStart,#mermaid-svg-2ZXSlZJutGjLFR1e #dependencyEnd{fill:#333333;stroke:#333333;stroke-width:1;}#mermaid-svg-2ZXSlZJutGjLFR1e .statediagramTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-2ZXSlZJutGjLFR1e :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 按关节名检索
名字不在 URDF
在 URDF 但 type=fixed
是可动 revolute 关节
target < lower 或 target > upper
余量 < 行程的 10%
余量充足
拦截（拒绝下发）
拦截（下发了也不动）
拦截（撞机械限位）
放行 + 告警
放行
接收指令
查白名单
UNKNOWN
FIXED
比较限位
NG
WARN
OK

UNKNOWN 和 FIXED 在"查白名单"阶段就被砍掉，根本走不到"比较限位"。这正是白名单设计的精髓------先用名字/类型过滤掉结构性错误，再做数值比较。

七、离线校验 + 在线拦截双模式设计

把同一套 check 逻辑放在两个时间点使用，就能覆盖开发期与运行期两道防线。讲清两件事：两者如何分工，共用哪份白名单。

7.1 离线模式：开发期批量体检

离线模式在 CI 或本地脚本里跑，给一批"候选指令"做体检。它把越界错误消灭在代码合并之前。比如你在配置文件里写死一组预设动作角度，离线预检能立刻标出哪几帧会越界，而不是等真机跑到那一帧才崩。

离线模式不需要机器人、不需要 ROS。我自己的做法是挂进 GitHub Actions：每次提交改了指令 JSON，就自动跑 check_cmd.py，非零退出码即阻断合并。这就把"安全护栏"从个人自觉升级成了工程约束。

"真机试一下就知道"------这话不少人说过。但真机试错的成本被严重低估：每次越界都要先让人靠近机器人、做好防护、再触发一次失败动作，还占用本就紧张的调试档期。离线预检把这次试错提前到敲键盘阶段，一台普通笔记本就能在毫秒内跑完几千条指令的体检，等于用近乎零成本换掉了最贵的一次失败。

7.2 在线模式：发布前最后一关

在线模式嵌在你的控制节点里，在真正 publish 到 /aima/hal/joint/command 之前包一层。它复用同一份 joint_whitelist.json，对每一条实时指令做判定，NG / UNKNOWN / FIXED 一律不下发，WARN 视策略处理。

在线模式的延迟几乎为零（一次字典查找 + 两次浮点比较），不会拖慢控制环。它和离线模式是互补而非替代：离线保证"这批动作整体安全"，在线保证"这条实时指令此刻安全"。

在线模式另一个容易被忽视的价值，是"防御性编程的最后一米"：即使上游业务代码因为某次重构引入 bug、算出离谱角度，护栏也会在 publish 前兜底。它不假设你同事写的控制律永远正确，而是假设"任何代码都可能出错"，从而把单点失误限制在不伤本体的范围内。这种分层防御，比依赖某个人"写对"更可靠，也更适合多人协作的长期项目。

7.3 双模式数据流

下面用流程图把"指令从业务层到机器人"的两条校验路径画清楚。离线走左侧开发链路，在线走右侧运行链路，二者共享底部同一份白名单与同一套 check。
#mermaid-svg-eY56kgCIhNFZs2FV{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-eY56kgCIhNFZs2FV .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-eY56kgCIhNFZs2FV .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-eY56kgCIhNFZs2FV .error-icon{fill:#552222;}#mermaid-svg-eY56kgCIhNFZs2FV .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-eY56kgCIhNFZs2FV .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-eY56kgCIhNFZs2FV .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-eY56kgCIhNFZs2FV .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-eY56kgCIhNFZs2FV .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-eY56kgCIhNFZs2FV .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-eY56kgCIhNFZs2FV .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-eY56kgCIhNFZs2FV .marker{fill:#333333;stroke:#333333;}#mermaid-svg-eY56kgCIhNFZs2FV .marker.cross{stroke:#333333;}#mermaid-svg-eY56kgCIhNFZs2FV svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-eY56kgCIhNFZs2FV p{margin:0;}#mermaid-svg-eY56kgCIhNFZs2FV .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-eY56kgCIhNFZs2FV .cluster-label text{fill:#333;}#mermaid-svg-eY56kgCIhNFZs2FV .cluster-label span{color:#333;}#mermaid-svg-eY56kgCIhNFZs2FV .cluster-label span p{background-color:transparent;}#mermaid-svg-eY56kgCIhNFZs2FV .label text,#mermaid-svg-eY56kgCIhNFZs2FV span{fill:#333;color:#333;}#mermaid-svg-eY56kgCIhNFZs2FV .node rect,#mermaid-svg-eY56kgCIhNFZs2FV .node circle,#mermaid-svg-eY56kgCIhNFZs2FV .node ellipse,#mermaid-svg-eY56kgCIhNFZs2FV .node polygon,#mermaid-svg-eY56kgCIhNFZs2FV .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-eY56kgCIhNFZs2FV .rough-node .label text,#mermaid-svg-eY56kgCIhNFZs2FV .node .label text,#mermaid-svg-eY56kgCIhNFZs2FV .image-shape .label,#mermaid-svg-eY56kgCIhNFZs2FV .icon-shape .label{text-anchor:middle;}#mermaid-svg-eY56kgCIhNFZs2FV .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-eY56kgCIhNFZs2FV .rough-node .label,#mermaid-svg-eY56kgCIhNFZs2FV .node .label,#mermaid-svg-eY56kgCIhNFZs2FV .image-shape .label,#mermaid-svg-eY56kgCIhNFZs2FV .icon-shape .label{text-align:center;}#mermaid-svg-eY56kgCIhNFZs2FV .node.clickable{cursor:pointer;}#mermaid-svg-eY56kgCIhNFZs2FV .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-eY56kgCIhNFZs2FV .arrowheadPath{fill:#333333;}#mermaid-svg-eY56kgCIhNFZs2FV .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-eY56kgCIhNFZs2FV .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-eY56kgCIhNFZs2FV .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-eY56kgCIhNFZs2FV .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-eY56kgCIhNFZs2FV .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-eY56kgCIhNFZs2FV .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-eY56kgCIhNFZs2FV .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-eY56kgCIhNFZs2FV .cluster text{fill:#333;}#mermaid-svg-eY56kgCIhNFZs2FV .cluster span{color:#333;}#mermaid-svg-eY56kgCIhNFZs2FV div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-eY56kgCIhNFZs2FV .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-eY56kgCIhNFZs2FV rect.text{fill:none;stroke-width:0;}#mermaid-svg-eY56kgCIhNFZs2FV .icon-shape,#mermaid-svg-eY56kgCIhNFZs2FV .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-eY56kgCIhNFZs2FV .icon-shape p,#mermaid-svg-eY56kgCIhNFZs2FV .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-eY56kgCIhNFZs2FV .icon-shape .label rect,#mermaid-svg-eY56kgCIhNFZs2FV .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-eY56kgCIhNFZs2FV .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-eY56kgCIhNFZs2FV .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-eY56kgCIhNFZs2FV :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 开发/CI
实时控制
OK
WARN
NG
UNKNOWN
FIXED
CI 非零退出阻断合并
更新白名单
业务层生成关节指令
运行模式?
离线预检: 批量读 JSON
在线预检: 每条指令拦截
载入 joint_whitelist.json
逐关节调用 check 判定
判定等级
放行
放行 + 告警日志
拦截: 拒绝下发
发布 /aima/hal/joint/command
回退到安全姿态 / 上一帧
代码仓库

流程图中虚线表示"离线结果反哺白名单与代码仓库"的闭环：白名单随 URDF 演进，CI 又随白名单演进，形成可回归的安全网。

图3：一条越界指令在预检器前被拦下的前后对比

八、三个真实陷阱剖析

三个陷阱都不是设想出来的，是 check_cmd.py 在真实 URDF 上跑出来的。除了开头那个 2.2，我接着撞上的是这三个，按顺序讲：先是 head_joint，然后是 left_wrist_yaw_joint，最后才是那个看起来"没错"的 2.44。

8.1 陷阱一：head_joint → UNKNOWN（名字在 URDF，却不可动）

第一个撞上的是 head_joint。

当时我照着一份关节清单拼指令，head_joint=0.5 也写了进去------清单里有这个名字，我以为它能转。脚本跑出来是 UNKNOWN。我回 URDF 里搜 head_joint，发现它是 type="fixed"，而且没有 <limit> 标签 。回到 load_limits 的逻辑：if lim is None: continue------没有 limit 的关节根本不会进入 limits 字典。于是 check 第一步 name not in limits 成立，直接返回 UNKNOWN。

URDF 里"有这个名字"不等于"能被指令驱动"。头是刚性固定在 torso 上的，你写 head_joint=0.5 既不会让头转，也不会报错，只会被真机默默忽略。预检器把它标成 UNKNOWN，正是提醒你"这条指令从根上就不该存在"。

8.2 陷阱二：left_wrist_yaw_joint → FIXED（带 limit 的 fixed）

第二个是 left_wrist_yaw_joint，结论从 UNKNOWN 变成了 FIXED。

它同样是 fixed，但带 <limit lower="-1.57" upper="1.57"> 。于是它被收进了 limits 字典（name in limits 为真），只是 movable=False。check 走到第二步 not info["movable"] 返回 FIXED。

这个区别很关键：UNKNOWN 表示"URDF 里查无此关节（多半拼写错）"，FIXED 表示"关节存在但固定连接（下发了也不会动）"。两者结论不同、根因不同，处理方式也应不同------UNKNOWN 多半是代码笔误要改，FIXED 是设计误解要删。下表对比三者。

关节	URDF 类型	是否带 limit 标签	进入 limits 字典	预检结论	根因
head_joint	fixed	否	否	UNKNOWN	名字查不到，疑似拼写错或本就非关节
left_wrist_yaw_joint	fixed	是	是（movable=False）	FIXED	固定连接，不贡献自由度
right_wrist_yaw_joint	fixed	是	是（movable=False）	FIXED	固定连接，不贡献自由度

8.3 陷阱三：left_knee_joint = 2.44 rad → WARN（余量仅 0.2°）

第三个陷阱不"错"，是"险"：left_knee_joint = 2.44 rad。

left_knee 限位是 [0.0, 2.4435] rad（即 0°~140.0°）。若指令写 2.44 rad，它没越过 2.4435 的上界，所以不是 NG；但余量 margin = 2.4435 - 2.44 = 0.0035 rad ≈ 0.20°，相对行程 span=2.4435 的比例仅 0.14%，远低于 10% 阈值，于是判 WARN。

我第一次跑出这条 WARN 时愣了一下：2.44 rad 离硬限位只差 0.2°，真机在该姿态下几乎没有容错空间，轻微抖动或模型误差就可能顶死。预检器在"还没越界"时就亮黄灯，给开发者留出缓冲。若你把它当 OK 放行，下一帧控制误差就可能把膝关节顶进 NG。

WARN 的处理策略应当写进团队规范，而不是临时拍脑袋。我的判断：在"全身动作控制（BIPED_WHOLE_BODY_CTRL）"模式下，单关节贴边往往由高层平衡算法兜底，可放行；但在"位控站立（BIPED_STAND_DEFAULT）无平衡调节"场景下，贴边意味着没有任何纠偏能力，建议直接拦截。同一 WARN，因运动模式不同，处置方向相反------这正是为什么护栏要能感知上下文，而非一刀切。SHARED_BRIEF 中"位控站立无平衡调节"的官方警告，在这里有了具体的落地动作。

8.4 判定结果对照表

把上述例子连同正常指令汇总，得到一张完整的"输入→结论"对照表，方便你对照自己的运行输出。

指令样例	预检结果	余量/偏差	说明
left_elbow_joint = 2.2	NG	越上界 31.04°	超 95.0° 上限，会撞机械限位
head_joint = 0.5	UNKNOWN	---	URDF 无此可动关节名
left_wrist_yaw_joint = 0.3	FIXED	---	fixed 关节，下发不动作
left_knee_joint = 2.44	WARN	余量 0.20°	贴边，几乎无容错
left_knee_joint = 1.0	OK	余量 72.0°	区间内且离边界远
right_hip_pitch_joint = -2.0	OK	余量 19.9°	在 -175°~90° 内

注：left_elbow 上限 1.6581 rad = 95.0°，2.2 rad 超出 0.5419 rad ≈ 31.04°，与表中"越上界 31.04°"一致；left_knee=1.0 余量取 min(1.0-0, 2.4435-1.0)=1.0 rad ≈ 57.3° 取整表述，实际 57.3°，表中以约数表达，精确值以脚本输出为准。

九、批量校验与单元测试：让护栏可信可回归

护栏自己若不可信，比没有护栏更危险。本节给出批量校验脚本与单元测试，把"预检器正确"这件事也变成可验证的。

护栏的代码虽然短，但"短"恰恰要求我们更认真地测。因为判定逻辑一旦写反（比如把 target > hi 写成 target < hi），所有越界都会变成 OK，而脚本不会报错------它只是安静地放行危险。单元测试就是给这个"安静的错误"装一个警报：只要五态映射有任何一处偏移，测试立刻变红，把隐患挡在合并之前而不是挡在真机之前。

9.1 批量校验脚本（bash）

下面脚本演示如何把预检器接进开发流程：对一个指令目录批量跑，并据此决定 CI 是否通过。

bash 复制代码

#!/usr/bin/env bash
# batch_check.sh ------ 对多个指令 JSON 批量预检，遇 NG/UNKNOWN/FIXED 即失败
set -euo pipefail

URDF="q1_v3.urdf"
CMD_DIR="./commands"
FAIL=0

echo "== 开始批量关节指令预检 =="
for f in "$CMD_DIR"/*.json; do
    echo ">> 校验文件: $f"
    # 用 --inline 方式不方便批量，这里直接喂 JSON 文件
    if ! python3 check_cmd.py "$URDF" "$f" | tee "/tmp/$(basename "$f").log"; then
        echo "[ERROR] $f 运行失败"; FAIL=1; continue
    fi
    # 只要输出里出现硬拦截等级，判定该文件不合格
    if grep -E "NG|UNKNOWN|FIXED" "/tmp/$(basename "$f").log"; then
        echo "[FAIL] $f 含越界/非法关节指令"; FAIL=1
    else
        echo "[PASS] $f 全部指令通过预检"
    fi
done

if [ "$FAIL" -ne 0 ]; then
    echo "== 预检未通过，阻断后续流程 =="
    exit 1
fi
echo "== 全部指令预检通过 =="

脚本功能 ：遍历 commands/ 下所有指令 JSON，逐个调用 check_cmd.py，把输出落盘到 /tmp 并就地打印。随后用 grep 扫描输出中的 NG / UNKNOWN / FIXED 三类硬拦截等级，一旦出现即把 FAIL 置位，最终以非零退出码阻断整条流水线。

参数含义 ：URDF 指定模型路径，CMD_DIR 指定待校验指令目录。set -euo pipefail 保证任一命令失败立即退出，避免"部分失败被掩盖"。tee 同时做到"既看输出又留日志"，方便事后排查是哪条指令触雷。

使用注意 ：该脚本把 WARN 视为通过（只拦三类硬错误），若你的场景严禁贴边，可把 grep 模式改成包含 WARN。CI 里建议把白名单与 URDF 一起纳入版本管理，防止限位漂移导致"本地过、云端挂"。脚本依赖 check_cmd.py 的打印格式稳定，若改了表头文案需同步改 grep 正则。

9.2 单元测试示例（python）

单元测试锁定五态判定本身的正确性，确保你改了 check 逻辑后不会悄悄把 NG 放成 OK。

python 复制代码

# test_check_cmd.py ------ 锁定 check() 五态判定的回归测试
import math
import unittest

from check_cmd import check, load_limits

URDF = "q1_v3.urdf"


class TestJointCheck(unittest.TestCase):
    @classmethod
    def setUpClass(cls):
        cls.limits = load_limits(URDF)

    def _near(self, rad, deg):
        # 弧度转角度的容差校验，避免浮点抖动
        self.assertAlmostEqual(math.degrees(rad), deg, places=1)

    def test_ok_inside_range(self):
        lvl, msg = check(self.limits, "left_knee_joint", 1.0)
        self.assertEqual(lvl, "OK")

    def test_ng_over_upper(self):
        lvl, msg = check(self.limits, "left_elbow_joint", 2.2)
        self.assertEqual(lvl, "NG")
        self.assertIn("越上界", msg)

    def test_warn_tiny_margin(self):
        lvl, msg = check(self.limits, "left_knee_joint", 2.44)
        self.assertEqual(lvl, "WARN")
        self.assertIn("余量仅", msg)

    def test_unknown_missing_joint(self):
        lvl, msg = check(self.limits, "head_joint", 0.5)
        self.assertEqual(lvl, "UNKNOWN")

    def test_fixed_joint(self):
        lvl, msg = check(self.limits, "left_wrist_yaw_joint", 0.3)
        self.assertEqual(lvl, "FIXED")


if __name__ == "__main__":
    unittest.main(verbosity=2)

代码功能 ：五个测试方法分别锁定五种结论。setUpClass 只解析一次 URDF，五个用例复用同一份 limits，保证测试彼此独立、跑得也快。test_ng_over_upper 直接复现"肘 2.2 rad 越界"，test_warn_tiny_margin 复现"膝 2.44 rad 贴边"，与正文实测一一对应。

参数含义 ：URDF 指向真实模型路径，测试因此能随模型演进自动适配新限位。assertIn("越上界", msg) 不仅验等级、还验说明文本格式，防止你改了文案却没改上游依赖。_near 是可选的弧度---角度一致性自校验辅助方法。

使用注意 ：测试与 check_cmd.py 同包导入，需保证 check_cmd 可被 import（即在同目录或已安装）。若 load_limits 解析失败，setUpClass 会整体报错，这是好事------它强迫你在跑判定测试前先保证 URDF 可用。建议把该测试接进同一份 CI，与 batch_check.sh 形成"单条正确 + 批量通过"双重保险。

十、在线拦截封装：把护栏焊进发布动作

在线模式要真正"拦得住"，不能靠开发者记得调用 check，要把它焊进发布函数里。最小封装示例如下。

python 复制代码

# guard_publisher.py ------ 在线预检封装：发布前必过 check
import json
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray


class GuardedJointPublisher(Node):
    def __init__(self, whitelist_path="joint_whitelist.json"):
        super().__init__("guarded_joint_pub")
        # 运行期直接读白名单 JSON，不再解析 URDF，开销最低
        with open(whitelist_path, "r", encoding="utf-8") as f:
            self.wl = json.load(f)
        self.pub = self.create_publisher(
            Float64MultiArray, "/aima/hal/joint/command", 10)

    def _safe(self, name, target):
        if name not in self.wl:
            self.get_logger().error(f"拦截 UNKNOWN 关节: {name}")
            return False
        lo, hi = self.wl[name]
        if not (lo <= target <= hi):
            self.get_logger().error(
                f"拦截越界 {name}={target:.4f} 超出 [{lo},{hi}]")
            return False
        return True

    def publish_command(self, name, target):
        if not self._safe(name, target):
            return  # 硬拦截，绝不 publish
        msg = Float64MultiArray()
        msg.data = [target]
        self.pub.publish(msg)

代码功能 ：GuardedJointPublisher 在节点初始化时把 joint_whitelist.json 读进内存，对外只暴露 publish_command(name, target)。任何调用方想发指令，都必须先过 _safe：名字不在白名单直接拦，数值超出 [lo, hi] 直接拦，只有都通过才真正 publish。

参数含义 ：whitelist_path 默认同名 JSON，与离线模式共用。/aima/hal/joint/command 是 Q1 关节指令话题（见 SHARED_BRIEF 话题清单）。_safe 返回布尔，调用方可用它决定"拦下后该回退到安全姿态还是丢弃这一帧"。

使用注意 ：示例中 Float64MultiArray 是占位消息类型，真实 Q1 指令格式请参照 aimdk_msgs 定义替换。封装的关键不是"多写几行"，而是把 check 收敛成唯一出口------让任何代码路径都无法绕过护栏直接 publish。若项目已有发布封装，应把 _safe 嵌进既有发布函数，而非另起炉灶。

10.1 NG 之后的三种处置：丢弃、裁剪、急停

_safe 返回 False 之后做什么，才是在线拦截真正体现工程判断的地方。直接丢弃只是选项之一，而且很少是最优解。按干预强度从轻到重，有三种处置，安全含义截然不同。

丢弃：这一条或这一帧指令不发布。实现最简单，副作用是机器人少动一帧；若上游持续产出越界值，实际效果就是机器人静止。但在 BIPED_STAND_DEFAULT 这类无平衡调节的位控模式下，静止并不等于安全------没有纠正输入的双足站立同样会失稳倾倒，丢弃只是把撞限位的风险换成了摔跤的风险。

裁剪：把越界值压到最近边界（clamp 到 lo 或 hi）再发布。好处是保持指令连续性，机器人不会因单帧异常突然停住；代价是把错误洗白------上游算出 2.2 rad 的 bug 被静默改成 1.6581，日志里只剩一条放行记录，根因被掩盖，下次换个关节继续犯。裁剪适合语义明确的软限位内缩场景（例如把白名单整体收紧若干作为工作区），不适合当越界的默认兜底。

急停：NG 时不再尝试完成动作，而是按官方切换规则直接切到 DAMPING_DEFAULT（阻尼急停，任何状态均可直达）或 PASSIVE_DEFAULT（零力矩急停）。这是唯一主动停止的选项，适用于继续执行比停下更危险的场景，例如越界伴随速度异常、或站立模式已失去平衡调节能力时。代价是任务中断、需要人工或上层状态机恢复，因此不能对偶发的单帧抖动滥用，否则机器人会变成一有异常就停的惊跳机器人。

三种处置没有银弹，区别在于你愿意为不中断付出多少被掩盖的代价。我的分层策略：默认丢弃并对连续 NG 计数，超过阈值升级为急停；对已知安全的软限位用裁剪；急停留给越界叠加时序异常的复合信号。说到底，护栏的价值不只是拦住一条指令，更是把"异常时该怎么办"从临时拍脑袋变成事先写进代码的策略。

十一、适用边界与风险提示

护栏不是万能的，清楚它的边界才能避免"以为安全其实没拦住"。

11.1 它拦得住什么

预检器只校验"单条指令的数值是否在 URDF 限位内"，以及"关节名是否合法/可动"。它能拦住：拼写错的关节名、写了固定关节、数值越界、贴边无容错。这四类覆盖了绝大多数"手写指令"事故。

11.2 它拦不住什么

它不校验多关节联动 是否会自碰撞、不校验时序上相邻帧的加速度是否超限、不校验力/力矩 是否超限（URDF 的 effort 上限未被本预检器使用）。一条每条都 OK 的指令序列，整体仍可能因动力学原因不安全。

它也依赖白名单与 URDF 一致：若你拿显示用 MJCF（如 q1_v3.xml 的近似区间）当真理，限位会和真机驱动器的精确区间（q1_v3_mc.xml 用的 URDF 原值）不一致，护栏结论就会失真。所以白名单必须来自 q1_v3.urdf 本体。

具体错配会怎样？以 left_hip_pitch 为例，URDF 精确下限是 -3.0543，而显示用 q1_v3.xml 用近似区间 -2.95。若你用近似区间做白名单，指令 -3.0 会被误判 NG（因为 -3.0 < -2.95），可它其实在真机精确边界 -3.0543 之内、本应 OK------护栏凭空多拦了一条合法指令。反过来，近似上限 1.47 比精确 1.5708 更紧，指令 1.5 在真机 OK、却被近似白名单判 NG。错配的后果不是"漏拦"，而是"乱拦"，同样会毁掉开发体验，必须用精确 URDF 生成白名单来杜绝。

11.3 版本与模型差异

不同 Q1 固件/URDF 版本的限位可能微调。本文数字基于当前仓库 q1_v3.urdf；官方若更新模型，我会重跑 build_whitelist 与单测，确认结论仍成立再合入。此外，本预检器是"开发期 + 发布前"护栏，不替代真机底层的急停与前哨检查------它是在真机限制之外，给你多一道"不必冒风险就能发现错误"的防线。

图4：离线静态计算为何能替代真机前哨检查

十二、总结

回到开头那个 2.2。它没造成任何损失，但让我把"发指令"拆开重看：Q1 的合法边界早就写在 q1_v3.urdf 的 <limit> 里，要做的只是用纯标准库脚本把它抽成 22 项可动关节白名单 joint_whitelist.json，再对每条指令做 check 五态判定（OK / WARN / NG / UNKNOWN / FIXED）。head_joint 因无 limit 标签落到 UNKNOWN，left_wrist_yaw_joint 因 type=fixed 落到 FIXED；left_knee=2.44 rad 未越界却只剩 0.2° 余量，靠 WARN 提前亮灯。三个陷阱把护栏的必要性钉死了。

如果只做一件事，我会先把 check 焊进发布函数：离线体检拦的是"一批"，在线拦截拦的是"此刻这一条"，而冲到真机面前的是后者。下次 SDK 升级，我的第一动作不是点真机演示，是 diff 新旧 URDF、重跑 build_whitelist 和单测，确认护栏还锁得住，再谈别的。

最后强调边界：预检器只管单条指令的数值与名字，不管联动自碰撞、时序加速度与力矩限制，也不替代真机底层急停。它是冒真机风险之前的一道低成本防线，不是安全性的终点。把护栏接进 CI 与发布函数，越界指令就再也到不了机器人身边。

参考资料

启元机器人开发者社区（SDK / URDF / 接口说明）：https://dev.primebot.com
PrimebotTech 开源仓库 primebot-q1-sdk（含 q1_v3.urdf 与示例）：https://github.com/PrimebotTech/primebot-q1-sdk
启元社区 open.agibot.com（二次开发经验与模型更新）：https://open.agibot.com
ROS 2 官方文档（话题 / 消息 / 节点模型，理解 /aima/hal/joint/command 发布机制）：https://docs.ros.org/en/humble
URDF 规范（<joint> / <limit> 语义与单位定义）：http://wiki.ros.org/urdf/XML/joint