P6 通止规装配：状态机与配置的纯逻辑层（含 92 项单测）
为什么先做纯逻辑 ---------------- 状态机、配置、限位计算、夹爪后端选择这四件事都有同一个性质：**算错了不会 抛异常**，只会让流程"走得下去但结果不对"（去错了孔、按毫米当米用、 贴着限位下发、调用了类型不匹配的服务）。所以它们必须在没有 ROS、没有真机 的情况下就能被钉住（规则 R5.1）。 新增 ---- - gauge_fsm.py：S00–S11 + E1/E2 的转移表（数据，不是 if/else 丛林）。 含 S05 稳定判据、S07 双通道切向突变、S08「切向且法向」停止判据、 棘轮四步顺序。E1 掉件有重放预算，超限明确 ABORT 并留下现场原因。 - gauge_config.py：site.yaml 的 gauge_assembly: 段（36 个键）解析 + 校验。 多头校验：未知键报错并给最相近候选名；类型不做隐式转换（"20" 不当 20）； 键名后缀必须体现单位（防"填毫米按米用"）；跨键矛盾（安全上限低于停止阈值） 直接拒绝；describe() 打印**实际生效**的值与派生结论。 - gauge_kin.py：URDF 限位解析（stdlib，不依赖 urdf_parser_py，XML 坏掉时 逐行兜底）、S01 预绕量的限位夹取、以及"确实绕工具 Z 转了多少度"的事后验证 （只看转了多少度不够：绕错轴转 90° 的读数一样）。 - gauge_gripper.py：按**服务类型**（不是名字）选夹爪后端。现场查证发现 /gripper/open 后面挂着两种类型：真机 dh_pgea 是 std_srvs/Trigger， 契约/仿真 是 arm_msgs/GripperExecute；manifest 声明的 arm_driver_dh_gripper 还不存在。两种都在线时报错，不撞运气。 - gauge_boot.py：S00 自检清单（纯内容层）。触觉/相机是必检（标成选检会让 力判据全部失效但流程照跑）；没有的设备不生成检查项（必然失败的项会训练人 忽略自检输出）；每项都写"缺了会怎样"。 验证 ---- python3 -m pytest src/arm_application/test/test_gauge_*.py -q → 92 passed python3 -m pyflakes src/arm_application/arm_application/gauge_*.py → 干净 写测试时被自己的检查抓到的两处 ------------------------------ 1. extreme_speed_deg_s 单位登记成 deg，被"键名必须体现单位"的检查报出来； 2. pick_backend 对"服务不在"和"类型不对"报同一句"节点没起来"—— 已拆成两种消息（前者给启动命令，后者给「谁占用了这个服务名」的查法）。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -0,0 +1,306 @@
|
|||||||
|
"""S00 开机自检：把"能不能跑"这件事变成一张**可打印的清单**（纯逻辑部分）。
|
||||||
|
|
||||||
|
为什么要有这一阶段
|
||||||
|
------------------
|
||||||
|
看板 S00 的要求是"各服务就绪后才开指令窗，任一不就绪即中止并报告"。它挡的是
|
||||||
|
这个仓库最常见的一类事故：**流程跑起来了，只是没接上硬件**。例如：
|
||||||
|
|
||||||
|
* 触觉没起 → 三阶段的力阈值全是 0 → 搜孔"一直没找到"（实际是没数据）；
|
||||||
|
* 相机没起 → 精拍拿不到图 → 报"精拍未拿到图像"（离真正原因差三层）；
|
||||||
|
* 夹爪没起 → S08 松爪/夹紧静默失败 → 棘轮循环空转，看起来像"拧不动"。
|
||||||
|
|
||||||
|
放纯逻辑里的原因
|
||||||
|
----------------
|
||||||
|
"要检查哪些东西"是**事实**，会随现场（两台臂、两种夹爪后端、D435 有没有标定）
|
||||||
|
变化；"怎么检查"是 ROS 机制。前者做成纯函数，就能被测试钉住：
|
||||||
|
|
||||||
|
* 少一项检查 → 测试红（比如"忘了查触觉"）；
|
||||||
|
* 检查项的顺序无关紧要，但**必检项**和**选检项**的划分必须明确 ——
|
||||||
|
把核心判据依赖的触觉标成选检，就会在没传感器时一路跑到力判据才失败。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import Dict, Iterable, List, Optional, Sequence, Tuple
|
||||||
|
|
||||||
|
#: 探针类型 —— 决定节点用哪种方式确认"就绪"。
|
||||||
|
#:
|
||||||
|
#: ``service`` 服务在线且类型匹配
|
||||||
|
#: ``action`` action server 在线
|
||||||
|
#: ``topic`` 话题在，且**真的收到过消息**（只看 topic list 会假阳性）
|
||||||
|
#: ``tactile`` 触觉状态话题在发，且 ``connected=true``
|
||||||
|
#: ``camera`` 图像话题在发（收到首帧）
|
||||||
|
#: ``detector`` 算法权重能加载（推理预热）
|
||||||
|
PROBES = ("service", "action", "topic", "tactile", "camera", "detector")
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class BootCheck:
|
||||||
|
"""一项自检。"""
|
||||||
|
|
||||||
|
key: str
|
||||||
|
"""稳定标识（日志/结果 JSON 里用它，改名要同步文档）。"""
|
||||||
|
|
||||||
|
label: str
|
||||||
|
"""人读的名字。"""
|
||||||
|
|
||||||
|
probe: str
|
||||||
|
"""见 :data:`PROBES`。"""
|
||||||
|
|
||||||
|
target: str
|
||||||
|
"""要探测的对象：服务名 / action 名 / 话题名 / 权重路径。"""
|
||||||
|
|
||||||
|
why: str
|
||||||
|
"""**缺了会怎样**。自检失败的提示必须写这个 —— 只报"未就绪"没人知道该不该急。"""
|
||||||
|
|
||||||
|
required: bool = True
|
||||||
|
"""必检 / 选检。选检失败只告警，不拦流程。"""
|
||||||
|
|
||||||
|
expect_type: Optional[str] = None
|
||||||
|
"""期望的消息/服务类型（有值时类型不符即算失败）。"""
|
||||||
|
|
||||||
|
|
||||||
|
#: 需要"绝对名字"的探针：相对名字会按节点命名空间解析，
|
||||||
|
#: 节点被放进 namespace 之后 ``home`` 会变成 ``/ns/home`` —— 自检就会去探一个
|
||||||
|
#: 不存在的名字，报"未就绪"。统一补前导斜杠，把这件事变成不可能发生。
|
||||||
|
_ABSOLUTE_PROBES = frozenset({"service", "action", "topic", "tactile", "camera"})
|
||||||
|
|
||||||
|
|
||||||
|
def _abs(name: str) -> str:
|
||||||
|
"""把 ROS 名字规范成绝对名（``home`` → ``/home``）。"""
|
||||||
|
n = (name or "").strip()
|
||||||
|
if not n:
|
||||||
|
return n
|
||||||
|
return n if n.startswith("/") else "/" + n
|
||||||
|
|
||||||
|
|
||||||
|
def _topic(prefix: str, suffix: str) -> str:
|
||||||
|
"""拼话题名，容忍 ``prefix`` 带不带前导斜杠。"""
|
||||||
|
p = "/" + prefix.strip("/") if prefix else ""
|
||||||
|
return f"{p}/{suffix.strip('/')}"
|
||||||
|
|
||||||
|
|
||||||
|
def build_checks(
|
||||||
|
*,
|
||||||
|
joint_action: str,
|
||||||
|
camera: str,
|
||||||
|
tactile_topic: str,
|
||||||
|
gripper_open: str,
|
||||||
|
gripper_close: str,
|
||||||
|
gripper_status: str,
|
||||||
|
gripper_status_type: str,
|
||||||
|
d435_camera: Optional[str] = None,
|
||||||
|
tool_name: str = "",
|
||||||
|
weights_path: str = "",
|
||||||
|
) -> List[BootCheck]:
|
||||||
|
"""按现场情况生成检查清单。
|
||||||
|
|
||||||
|
:param joint_action: JTC action 名（``rizon_arm_controller/follow_joint_trajectory``）。
|
||||||
|
:param camera: 腕部相机命名空间（``d405_left``）。
|
||||||
|
:param tactile_topic: 触觉状态话题（``/tactile/left/status``）。
|
||||||
|
:param gripper_status: 夹爪状态话题，随后端不同（``/gripper/status`` 或 ``/gripper/states``）。
|
||||||
|
:param d435_camera: 手外相机命名空间。给 None 表示这台臂没有手外相机 →
|
||||||
|
不生成 S03 相关的检查项（**不生成**比"生成了一个必然失败的项"更诚实：
|
||||||
|
后者会让每次启动都报一条无关的错，很快所有人都不看自检输出了）。
|
||||||
|
:param tool_name: ``site.yaml`` 的 ``robot.tool_name``。空 = 没指定工具系，
|
||||||
|
则 S01 的"工具系垂直向下锁定"无法确认 → 单独列为选检告警。
|
||||||
|
:param weights_path: 检测权重路径。空 = 不检查推理预热。
|
||||||
|
|
||||||
|
清单的顺序 = 打印顺序 = 排障顺序：先臂、再夹爪、再相机、再触觉、最后算法。
|
||||||
|
"""
|
||||||
|
checks: List[BootCheck] = [
|
||||||
|
BootCheck(
|
||||||
|
key="arm_jtc",
|
||||||
|
label="机械臂关节轨迹控制器",
|
||||||
|
probe="action",
|
||||||
|
target=joint_action,
|
||||||
|
why="没有它，S01/S06/S07/S10 的所有运动都发不出去",
|
||||||
|
expect_type="arm_msgs/action/FollowJointTrajectory",
|
||||||
|
),
|
||||||
|
BootCheck(
|
||||||
|
key="arm_home",
|
||||||
|
label="回零服务",
|
||||||
|
probe="service",
|
||||||
|
target="/home",
|
||||||
|
why="S01 回 home、S11 复位都靠它",
|
||||||
|
expect_type="std_srvs/srv/Trigger",
|
||||||
|
),
|
||||||
|
BootCheck(
|
||||||
|
key="arm_named_pose",
|
||||||
|
label="命名位姿服务",
|
||||||
|
probe="service",
|
||||||
|
target="/set_named_pose",
|
||||||
|
why="/home 内部转发给它；它不在时回零会超时",
|
||||||
|
expect_type="arm_msgs/srv/SetNamedPose",
|
||||||
|
),
|
||||||
|
BootCheck(
|
||||||
|
key="arm_switch_tool",
|
||||||
|
label="工具系切换服务",
|
||||||
|
probe="service",
|
||||||
|
target="/switch_tool",
|
||||||
|
why="tool1 必须全程垂直向下且位姿锁定，切不过去后面全是歪的",
|
||||||
|
expect_type="arm_msgs/srv/SwitchTool",
|
||||||
|
),
|
||||||
|
BootCheck(
|
||||||
|
key="arm_compliant",
|
||||||
|
label="柔顺下压 action",
|
||||||
|
probe="action",
|
||||||
|
target="/compliant_descent",
|
||||||
|
why="S07 的柔顺接触靠它；缺了只能用位置控制硬压",
|
||||||
|
expect_type="arm_msgs/action/CompliantDescent",
|
||||||
|
),
|
||||||
|
BootCheck(
|
||||||
|
key="gripper_open",
|
||||||
|
label="夹爪张开服务",
|
||||||
|
probe="service",
|
||||||
|
target=gripper_open,
|
||||||
|
why="S08 棘轮的『松爪』一步靠它；失败会让回转把规拧回去",
|
||||||
|
),
|
||||||
|
BootCheck(
|
||||||
|
key="gripper_close",
|
||||||
|
label="夹爪夹紧服务",
|
||||||
|
probe="service",
|
||||||
|
target=gripper_close,
|
||||||
|
why="S08 的『夹紧』一步靠它；失败会让慢转时规打滑（磨螺纹）",
|
||||||
|
),
|
||||||
|
BootCheck(
|
||||||
|
key="camera_color",
|
||||||
|
label="腕部相机彩图",
|
||||||
|
probe="camera",
|
||||||
|
target=_topic(camera, "color/image_raw"),
|
||||||
|
why="S05 精拍定位 k1/k2 靠它；缺了只能靠 S03 粗定位（精度不够）",
|
||||||
|
expect_type="sensor_msgs/msg/Image",
|
||||||
|
),
|
||||||
|
BootCheck(
|
||||||
|
key="camera_info",
|
||||||
|
label="腕部相机内参",
|
||||||
|
probe="topic",
|
||||||
|
target=_topic(camera, "color/camera_info"),
|
||||||
|
why="没有内参就无法把像素反投影成基座坐标（会退化成 60° 默认视场）",
|
||||||
|
expect_type="sensor_msgs/msg/CameraInfo",
|
||||||
|
),
|
||||||
|
BootCheck(
|
||||||
|
key="tactile_status",
|
||||||
|
label="触觉三向力",
|
||||||
|
probe="tactile",
|
||||||
|
target=tactile_topic,
|
||||||
|
why="S07 切向力突变停、S08 拧紧判据、E1 掉件都依赖它；"
|
||||||
|
"缺了这几个判据全部失效（流程能走完但结果不可信）",
|
||||||
|
expect_type="arm_msgs/msg/TactileStatus",
|
||||||
|
),
|
||||||
|
BootCheck(
|
||||||
|
key="gripper_status",
|
||||||
|
label="夹爪状态反馈",
|
||||||
|
probe="topic",
|
||||||
|
target=gripper_status,
|
||||||
|
why="E1 掉件检测靠它；缺了掉件只能靠人看",
|
||||||
|
expect_type=gripper_status_type,
|
||||||
|
),
|
||||||
|
]
|
||||||
|
|
||||||
|
if d435_camera:
|
||||||
|
checks.append(
|
||||||
|
BootCheck(
|
||||||
|
key="coarse_camera",
|
||||||
|
label="手外粗定位相机",
|
||||||
|
probe="camera",
|
||||||
|
target=_topic(d435_camera, "color/image_raw"),
|
||||||
|
why="S03 兜底粗定位用；缺了只能在 S04 跟踪上多试几次",
|
||||||
|
required=False,
|
||||||
|
expect_type="sensor_msgs/msg/Image",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
if tool_name:
|
||||||
|
checks.append(
|
||||||
|
BootCheck(
|
||||||
|
key="tool_frame",
|
||||||
|
label="工具坐标系已指定",
|
||||||
|
probe="service", # 具体内容由节点按 tool_name 复核（只查服务在不在）
|
||||||
|
target="/switch_tool",
|
||||||
|
why=f"要求 tool1（垂直向下、位姿锁定），site.yaml 里配的是 {tool_name!r}",
|
||||||
|
required=False,
|
||||||
|
expect_type="arm_msgs/srv/SwitchTool",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
if weights_path:
|
||||||
|
checks.append(
|
||||||
|
BootCheck(
|
||||||
|
key="detector",
|
||||||
|
label="检测算法预热",
|
||||||
|
probe="detector",
|
||||||
|
target=weights_path,
|
||||||
|
why="S04 跟踪、S03 粗定位都要它；预热失败会在第一次跟踪时才炸",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
# 网络端点统一成绝对名（见 _ABSOLUTE_PROBES 的说明）。
|
||||||
|
# detector 的 target 是文件路径，不参与规范化。
|
||||||
|
return [
|
||||||
|
c if c.probe not in _ABSOLUTE_PROBES
|
||||||
|
else BootCheck(
|
||||||
|
key=c.key, label=c.label, probe=c.probe, target=_abs(c.target),
|
||||||
|
why=c.why, required=c.required, expect_type=c.expect_type,
|
||||||
|
)
|
||||||
|
for c in checks
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def format_report(results: Sequence[Tuple[BootCheck, bool, str]]) -> Tuple[str, List[str]]:
|
||||||
|
"""把逐项结果排成一张表，返回 ``(表文本, 失败项描述)``。
|
||||||
|
|
||||||
|
``results`` 每项是 ``(检查项, 是否就绪, 细节)``。
|
||||||
|
|
||||||
|
两张输出分开是为了让调用方能按"必检失败"决定是否中止：表用来给人看，
|
||||||
|
失败项描述用来进日志/结果 JSON。
|
||||||
|
"""
|
||||||
|
lines: List[str] = ["S00 自检："]
|
||||||
|
failures: List[str] = []
|
||||||
|
for item, ok, detail in results:
|
||||||
|
mark = "OK " if ok else ("缺失" if item.required else "告警")
|
||||||
|
tag = "" if item.required else "（选检）"
|
||||||
|
lines.append(f" [{mark}] {item.label}{tag} {item.target}")
|
||||||
|
if detail:
|
||||||
|
lines.append(f" {detail}")
|
||||||
|
if not ok:
|
||||||
|
prefix = "必检项未就绪" if item.required else "选检项未就绪"
|
||||||
|
failures.append(f"{prefix}：{item.label}（{item.target}）—— {item.why}")
|
||||||
|
if failures:
|
||||||
|
lines.append(f" 共 {len(failures)} 项未就绪")
|
||||||
|
else:
|
||||||
|
lines.append(" 全部就绪，开指令窗")
|
||||||
|
return "\n".join(lines), failures
|
||||||
|
|
||||||
|
|
||||||
|
def blocking_failures(results: Iterable[Tuple[BootCheck, bool, str]]) -> List[str]:
|
||||||
|
"""只挑出**必检且未就绪**的项 —— 决定要不要中止的就是它们。
|
||||||
|
|
||||||
|
和 :func:`format_report` 的 ``failures`` 的区别：那里两项都收（含选检），
|
||||||
|
这里只收必检。合成一个函数的话，调用方一不留神就会拿选检失败去中止流程，
|
||||||
|
于是"D435 没接"变成"装配跑不起来"。
|
||||||
|
"""
|
||||||
|
return [
|
||||||
|
f"必检项未就绪：{item.label}（{item.target}）—— {item.why}"
|
||||||
|
for item, ok, _detail in results
|
||||||
|
if (not ok) and item.required
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def optional_warnings(results: Iterable[Tuple[BootCheck, bool, str]]) -> List[str]:
|
||||||
|
"""选检未就绪的告警（不拦流程，但必须打出来）。"""
|
||||||
|
return [
|
||||||
|
f"选检项未就绪：{item.label}（{item.target}）—— {item.why}"
|
||||||
|
for item, ok, _detail in results
|
||||||
|
if (not ok) and not item.required
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def summary_counts(results: Sequence[Tuple[BootCheck, bool, str]]) -> Dict[str, int]:
|
||||||
|
"""统计``必检/选检``各自通过多少，给启动日志一行摘要。"""
|
||||||
|
out = {"required_ok": 0, "required_total": 0, "optional_ok": 0, "optional_total": 0}
|
||||||
|
for item, ok, _detail in results:
|
||||||
|
slot = "required" if item.required else "optional"
|
||||||
|
out[f"{slot}_total"] += 1
|
||||||
|
if ok:
|
||||||
|
out[f"{slot}_ok"] += 1
|
||||||
|
return out
|
||||||
@@ -0,0 +1,468 @@
|
|||||||
|
"""通止规装配的配置：从 ``site.yaml`` 的 ``gauge_assembly:`` 段解析 + 校验。
|
||||||
|
|
||||||
|
为什么单独一个模块
|
||||||
|
------------------
|
||||||
|
这一段的键**全是会动的量**（力阈值、角度、悬高、超时），也是现场最常改的地方。
|
||||||
|
散在节点里读的话会有三个已知坑（都被这个仓库踩过）：
|
||||||
|
|
||||||
|
1. **名字与单位不一致** —— ``hover`` 填了毫米、代码按米用（规则 M3）；
|
||||||
|
2. **键没接线** —— YAML 里写着 ``screw_step_deg``，代码读的是 ``screw_steps``，
|
||||||
|
于是改了没效果，而报错也没有（``config_guard`` 模块的头注释就是这件事）；
|
||||||
|
3. **拼错键名** —— YAML 静默接受，行为退回默认值，现场"改了参数没反应"。
|
||||||
|
|
||||||
|
所以：所有键在一张表里声明（含单位），解析时**逐条校验**，多出来的键直接
|
||||||
|
报错并给出最相近的候选名，缺的键按 required/default 明确处理。
|
||||||
|
``describe()`` 把**派生**结果（比如"逆时针最多能转 147°"）打出来，让现场
|
||||||
|
看到的是"实际生效的值"，而不是"我填的值"。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import difflib
|
||||||
|
from dataclasses import dataclass, fields
|
||||||
|
from typing import Any, Dict, Iterable, List, Mapping, Optional, Tuple
|
||||||
|
|
||||||
|
#: 合法的单位后缀。键名必须以 ``_<单位>`` 结尾（``au`` = Orisys 任意单位增量）。
|
||||||
|
_UNIT_SUFFIX: Dict[str, str] = {
|
||||||
|
"s": "_s",
|
||||||
|
"deg": "_deg",
|
||||||
|
"mm": "_mm",
|
||||||
|
"m": "_m",
|
||||||
|
"px": "_px",
|
||||||
|
"pct": "_pct",
|
||||||
|
"au": "_delta",
|
||||||
|
}
|
||||||
|
|
||||||
|
#: 显式无量纲的键（布尔、计数、比例、枚举）。**必须在这里列出来** ——
|
||||||
|
#: 没有单位又不登记的键，会让"单位自检"变成一句空话（规则 M1）。
|
||||||
|
_UNITLESS = frozenset(
|
||||||
|
{
|
||||||
|
"require_preview",
|
||||||
|
"extreme_direction",
|
||||||
|
"grip_check",
|
||||||
|
"coarse_enabled",
|
||||||
|
"coarse_camera",
|
||||||
|
"coarse_conf",
|
||||||
|
"snap_min_frames",
|
||||||
|
"screw_max_cycles",
|
||||||
|
"unscrew_extra_cycles",
|
||||||
|
"holes_total",
|
||||||
|
"dry_run",
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
#: 每个键的**类型**（用于类型校验，不做隐式转换：``"20"`` 不当 20 用）。
|
||||||
|
_TYPES: Dict[str, type] = {
|
||||||
|
"boot_timeout_s": float,
|
||||||
|
"require_preview": bool,
|
||||||
|
"extreme_angle_deg": float,
|
||||||
|
"extreme_speed_deg_s": float,
|
||||||
|
"extreme_direction": int,
|
||||||
|
"extreme_margin_deg": float,
|
||||||
|
"grip_check": bool,
|
||||||
|
"grip_hold_s": float,
|
||||||
|
"coarse_enabled": bool,
|
||||||
|
"coarse_camera": str,
|
||||||
|
"coarse_hover_m": float,
|
||||||
|
"coarse_conf": float,
|
||||||
|
"coarse_timeout_s": float,
|
||||||
|
"track_lost_s": float,
|
||||||
|
"track_prewarm_s": float,
|
||||||
|
"snap_stable_s": float,
|
||||||
|
"snap_tol_px": float,
|
||||||
|
"snap_min_frames": int,
|
||||||
|
"hover_m": float,
|
||||||
|
"align_tol_mm": float,
|
||||||
|
"contact_speed_m_s": float,
|
||||||
|
"contact_step_mm": float,
|
||||||
|
"contact_max_depth_mm": float,
|
||||||
|
"contact_tangential_delta": float,
|
||||||
|
"contact_safe_delta": float,
|
||||||
|
"screw_step_deg": float,
|
||||||
|
"screw_speed_deg_s": float,
|
||||||
|
"screw_back_speed_deg_s": float,
|
||||||
|
"screw_max_cycles": int,
|
||||||
|
"screw_normal_delta": float,
|
||||||
|
"screw_shear_delta": float,
|
||||||
|
"screw_grip_force_pct": float,
|
||||||
|
"screw_grip_speed_pct": float,
|
||||||
|
"screw_release_wait_s": float,
|
||||||
|
"unscrew_extra_cycles": int,
|
||||||
|
"holes_total": int,
|
||||||
|
"dry_run": bool,
|
||||||
|
}
|
||||||
|
|
||||||
|
#: 每个键的单位。``""`` 表示无量纲（须同时登记在 :data:`_UNITLESS`）。
|
||||||
|
_UNITS: Dict[str, str] = {
|
||||||
|
"boot_timeout_s": "s",
|
||||||
|
"require_preview": "",
|
||||||
|
"extreme_angle_deg": "deg",
|
||||||
|
"extreme_speed_deg_s": "deg/s",
|
||||||
|
"extreme_direction": "",
|
||||||
|
"extreme_margin_deg": "deg",
|
||||||
|
"grip_check": "",
|
||||||
|
"grip_hold_s": "s",
|
||||||
|
"coarse_enabled": "",
|
||||||
|
"coarse_camera": "",
|
||||||
|
"coarse_hover_m": "m",
|
||||||
|
"coarse_conf": "",
|
||||||
|
"coarse_timeout_s": "s",
|
||||||
|
"track_lost_s": "s",
|
||||||
|
"track_prewarm_s": "s",
|
||||||
|
"snap_stable_s": "s",
|
||||||
|
"snap_tol_px": "px",
|
||||||
|
"snap_min_frames": "",
|
||||||
|
"hover_m": "m",
|
||||||
|
"align_tol_mm": "mm",
|
||||||
|
"contact_speed_m_s": "m/s",
|
||||||
|
"contact_step_mm": "mm",
|
||||||
|
"contact_max_depth_mm": "mm",
|
||||||
|
"contact_tangential_delta": "au",
|
||||||
|
"contact_safe_delta": "au",
|
||||||
|
"screw_step_deg": "deg",
|
||||||
|
"screw_speed_deg_s": "deg/s",
|
||||||
|
"screw_back_speed_deg_s": "deg/s",
|
||||||
|
"screw_max_cycles": "",
|
||||||
|
"screw_normal_delta": "au",
|
||||||
|
"screw_shear_delta": "au",
|
||||||
|
"screw_grip_force_pct": "pct",
|
||||||
|
"screw_grip_speed_pct": "pct",
|
||||||
|
"screw_release_wait_s": "s",
|
||||||
|
"unscrew_extra_cycles": "",
|
||||||
|
"holes_total": "",
|
||||||
|
"dry_run": "",
|
||||||
|
}
|
||||||
|
|
||||||
|
#: 键名后缀与单位必须一致（防"填毫米当米用"）。``contact_speed_m_s`` 这类
|
||||||
|
#: 复合单位只检查主单位段。
|
||||||
|
_SUFFIX_OVERRIDE: Dict[str, str] = {
|
||||||
|
# 复合单位：后缀取"最后一个单位段"，键名里写全
|
||||||
|
"contact_speed_m_s": "_m_s",
|
||||||
|
"extreme_speed_deg_s": "_deg_s",
|
||||||
|
"screw_speed_deg_s": "_deg_s",
|
||||||
|
"screw_back_speed_deg_s": "_deg_s",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
class GaugeConfigError(ValueError):
|
||||||
|
"""``gauge_assembly:`` 段配置错误。消息里必须给出**改哪里**。"""
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 默认值
|
||||||
|
# =============================================================================
|
||||||
|
#: 缺省值。**只有真的有现场依据的才写默认值**：力阈值这类必须现场标定的，
|
||||||
|
#: 默认值只是"能跑起来的起点"，必须在 ``describe()`` 里标出来让现场知道要改。
|
||||||
|
DEFAULTS: Dict[str, Any] = {
|
||||||
|
"boot_timeout_s": 30.0,
|
||||||
|
"require_preview": True,
|
||||||
|
# S01：逆时针预绕。符号靠 extreme_direction 定，不靠改代码里的负号。
|
||||||
|
"extreme_angle_deg": 150.0,
|
||||||
|
"extreme_speed_deg_s": 20.0,
|
||||||
|
"extreme_direction": 1,
|
||||||
|
"extreme_margin_deg": 3.0,
|
||||||
|
# S02（预留）
|
||||||
|
"grip_check": False,
|
||||||
|
"grip_hold_s": 0.5,
|
||||||
|
# S03
|
||||||
|
"coarse_enabled": True,
|
||||||
|
"coarse_camera": "d435",
|
||||||
|
"coarse_hover_m": 0.25,
|
||||||
|
"coarse_conf": 0.35,
|
||||||
|
"coarse_timeout_s": 20.0,
|
||||||
|
# S04
|
||||||
|
"track_lost_s": 3.0,
|
||||||
|
"track_prewarm_s": 2.0,
|
||||||
|
# S05
|
||||||
|
"snap_stable_s": 1.0,
|
||||||
|
"snap_tol_px": 2.0,
|
||||||
|
"snap_min_frames": 6,
|
||||||
|
# S06
|
||||||
|
"hover_m": 0.05,
|
||||||
|
"align_tol_mm": 1.0,
|
||||||
|
# S07
|
||||||
|
"contact_speed_m_s": 0.02,
|
||||||
|
"contact_step_mm": 0.5,
|
||||||
|
"contact_max_depth_mm": 25.0,
|
||||||
|
"contact_tangential_delta": 300.0,
|
||||||
|
"contact_safe_delta": 2000.0,
|
||||||
|
# S08
|
||||||
|
"screw_step_deg": 180.0,
|
||||||
|
"screw_speed_deg_s": 30.0,
|
||||||
|
"screw_back_speed_deg_s": 120.0,
|
||||||
|
"screw_max_cycles": 12,
|
||||||
|
"screw_normal_delta": 400.0,
|
||||||
|
"screw_shear_delta": 600.0,
|
||||||
|
"screw_grip_force_pct": 50.0,
|
||||||
|
"screw_grip_speed_pct": 50.0,
|
||||||
|
"screw_release_wait_s": 0.3,
|
||||||
|
# S09
|
||||||
|
"unscrew_extra_cycles": 1,
|
||||||
|
# 全局
|
||||||
|
"holes_total": 2,
|
||||||
|
"dry_run": False,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class GaugeConfig:
|
||||||
|
"""通止规装配的全部可调量（解析并校验之后）。"""
|
||||||
|
|
||||||
|
boot_timeout_s: float
|
||||||
|
require_preview: bool
|
||||||
|
extreme_angle_deg: float
|
||||||
|
extreme_speed_deg_s: float
|
||||||
|
extreme_direction: int
|
||||||
|
extreme_margin_deg: float
|
||||||
|
grip_check: bool
|
||||||
|
grip_hold_s: float
|
||||||
|
coarse_enabled: bool
|
||||||
|
coarse_camera: str
|
||||||
|
coarse_hover_m: float
|
||||||
|
coarse_conf: float
|
||||||
|
coarse_timeout_s: float
|
||||||
|
track_lost_s: float
|
||||||
|
track_prewarm_s: float
|
||||||
|
snap_stable_s: float
|
||||||
|
snap_tol_px: float
|
||||||
|
snap_min_frames: int
|
||||||
|
hover_m: float
|
||||||
|
align_tol_mm: float
|
||||||
|
contact_speed_m_s: float
|
||||||
|
contact_step_mm: float
|
||||||
|
contact_max_depth_mm: float
|
||||||
|
contact_tangential_delta: float
|
||||||
|
contact_safe_delta: float
|
||||||
|
screw_step_deg: float
|
||||||
|
screw_speed_deg_s: float
|
||||||
|
screw_back_speed_deg_s: float
|
||||||
|
screw_max_cycles: int
|
||||||
|
screw_normal_delta: float
|
||||||
|
screw_shear_delta: float
|
||||||
|
screw_grip_force_pct: float
|
||||||
|
screw_grip_speed_pct: float
|
||||||
|
screw_release_wait_s: float
|
||||||
|
unscrew_extra_cycles: int
|
||||||
|
holes_total: int
|
||||||
|
dry_run: bool
|
||||||
|
|
||||||
|
# -- 派生量（不占 YAML 键，免得两处各写一份，规则 M2）-------------------
|
||||||
|
@property
|
||||||
|
def screw_step_rad(self) -> float:
|
||||||
|
import math
|
||||||
|
|
||||||
|
return math.radians(self.screw_step_deg)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def total_screw_deg(self) -> float:
|
||||||
|
"""一根规要拧进去的总角度 = 单次转角 × 循环数。"""
|
||||||
|
return self.screw_step_deg * self.screw_max_cycles
|
||||||
|
|
||||||
|
@property
|
||||||
|
def unscrew_cycles(self) -> int:
|
||||||
|
"""拧出要多转几轮：多留 ``unscrew_extra_cycles`` 轮，确保螺纹完全脱开。"""
|
||||||
|
return self.screw_max_cycles + max(0, self.unscrew_extra_cycles)
|
||||||
|
|
||||||
|
def describe(self) -> List[str]:
|
||||||
|
"""启动时打印的行：**实际生效的**值与派生结论。
|
||||||
|
|
||||||
|
特别是 S01 的"能转多少度"和 S08 的"要拧多少轮"—— 这两个数是现场
|
||||||
|
判断"参数是否真的生效"最直接的依据，比打印"我读了 site.yaml"有用。
|
||||||
|
"""
|
||||||
|
return [
|
||||||
|
f"通止规装配：每孔孔数 {self.holes_total}，干跑={self.dry_run}",
|
||||||
|
f" S01 预绕 {self.extreme_angle_deg:.1f}°（方向={'逆时针' if self.extreme_direction > 0 else '顺时针'}，"
|
||||||
|
f"限位余量 {self.extreme_margin_deg:.1f}°，关节速度 {self.extreme_speed_deg_s:.1f}°/s）",
|
||||||
|
f" S03 粗定位={'开' if self.coarse_enabled else '关'} 相机={self.coarse_camera} "
|
||||||
|
f"悬高 {self.coarse_hover_m * 1000:.0f}mm 置信度≥{self.coarse_conf:.2f}",
|
||||||
|
f" S05 稳定 {self.snap_stable_s:.1f}s / 抖动≤{self.snap_tol_px:.1f}px / 至少 {self.snap_min_frames} 帧"
|
||||||
|
"（稳定判据用「到均值的最远距离」，不是相邻帧之差）",
|
||||||
|
f" S06 悬停 {self.hover_m * 1000:.0f}mm，对位容差 {self.align_tol_mm:.1f}mm",
|
||||||
|
f" S07 切向突变阈值 +{self.contact_tangential_delta:.0f}（两通道任一），"
|
||||||
|
f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm"
|
||||||
|
"（触觉读数是任意单位增量，必须现场标定）",
|
||||||
|
f" S08 棘轮 {self.screw_step_deg:.0f}°×{self.screw_max_cycles} 轮 = 累计 "
|
||||||
|
f"{self.total_screw_deg:.0f}°；停止判据「切向 >+{self.screw_shear_delta:.0f} 且 "
|
||||||
|
f"法向 >+{self.screw_normal_delta:.0f}」",
|
||||||
|
f" S09 拧出 {self.unscrew_cycles} 轮（比拧入多 {self.unscrew_extra_cycles} 轮）",
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def _resolve_one(key: str, raw: Mapping[str, Any]) -> Tuple[Any, bool]:
|
||||||
|
"""取一个键的值，返回 ``(值, 是否来自 site.yaml)``。"""
|
||||||
|
if key in raw:
|
||||||
|
value = raw[key]
|
||||||
|
elif key in DEFAULTS:
|
||||||
|
return DEFAULTS[key], False
|
||||||
|
else:
|
||||||
|
raise GaugeConfigError(
|
||||||
|
f"gauge_assembly.{key} 既没在 site.yaml 里给，也没有默认值。"
|
||||||
|
f"请在 config/site.yaml 的 gauge_assembly: 段补上"
|
||||||
|
f"（改完跑一次 bash scripts/show_site_config.sh 确认解析）"
|
||||||
|
)
|
||||||
|
|
||||||
|
want = _TYPES[key]
|
||||||
|
if want is bool and not isinstance(value, bool):
|
||||||
|
raise GaugeConfigError(_type_msg(key, value, "布尔 true/false"))
|
||||||
|
if want is int and (isinstance(value, bool) or not isinstance(value, int)):
|
||||||
|
raise GaugeConfigError(_type_msg(key, value, "整数"))
|
||||||
|
if want is float and (isinstance(value, bool) or not isinstance(value, (int, float))):
|
||||||
|
raise GaugeConfigError(_type_msg(key, value, "数字（可带小数）"))
|
||||||
|
if want is str and not isinstance(value, str):
|
||||||
|
raise GaugeConfigError(_type_msg(key, value, "字符串"))
|
||||||
|
return (float(value) if want is float else value), True
|
||||||
|
|
||||||
|
|
||||||
|
def _type_msg(key: str, value: Any, human: str) -> str:
|
||||||
|
unit = _UNITS[key]
|
||||||
|
hint = f"，单位 {unit}" if unit else ""
|
||||||
|
return (
|
||||||
|
f"gauge_assembly.{key} 的类型不对：期望 {human}{hint}，"
|
||||||
|
f"实际读到 {value!r}（{type(value).__name__}）。"
|
||||||
|
"本模块不做隐式转换 —— \"20\" 不会被当成 20 用，"
|
||||||
|
"因为那正是「填了字符串、跑起来是默认值」这类静默失效的入口"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def from_site(site: Mapping[str, Any], *, overrides: Optional[Mapping[str, Any]] = None) -> GaugeConfig:
|
||||||
|
"""解析并校验 ``site.yaml`` 的 ``gauge_assembly:`` 段。
|
||||||
|
|
||||||
|
:param site: ``site.yaml`` 解析后的整份字典。
|
||||||
|
:param overrides: CLI/launch 覆盖（只允许覆盖已知键，未知键报错）。
|
||||||
|
:raises GaugeConfigError: 缺键、类型错、未知键、单位与键名不符。
|
||||||
|
|
||||||
|
整段缺失**不报错**（全部走默认值），但会在 :meth:`GaugeConfig.describe`
|
||||||
|
之外由调用方打一条告警；理由是"默认值能不能用"取决于现场，
|
||||||
|
由节点决定要不要拒绝启动，配置模块只负责如实解析。
|
||||||
|
"""
|
||||||
|
raw_any = site.get("gauge_assembly") or {}
|
||||||
|
if not isinstance(raw_any, Mapping):
|
||||||
|
raise GaugeConfigError(
|
||||||
|
"site.yaml 的 gauge_assembly: 应该是一段映射（键值对），"
|
||||||
|
f"实际是 {type(raw_any).__name__}"
|
||||||
|
)
|
||||||
|
raw: Dict[str, Any] = dict(raw_any)
|
||||||
|
|
||||||
|
if overrides:
|
||||||
|
bad = [k for k in overrides if k not in _TYPES]
|
||||||
|
if bad:
|
||||||
|
raise GaugeConfigError(
|
||||||
|
f"未知的覆盖参数: {', '.join(sorted(bad))}。"
|
||||||
|
f"可在 gauge_assembly 段覆盖的键见 gauge_config._TYPES"
|
||||||
|
)
|
||||||
|
raw.update({k: v for k, v in overrides.items() if v is not None})
|
||||||
|
|
||||||
|
_reject_unknown(raw)
|
||||||
|
values = {key: _resolve_one(key, raw)[0] for key in _TYPES}
|
||||||
|
cfg = GaugeConfig(**values)
|
||||||
|
_sanity(cfg)
|
||||||
|
return cfg
|
||||||
|
|
||||||
|
|
||||||
|
def _reject_unknown(raw: Mapping[str, Any]) -> None:
|
||||||
|
"""多余的键直接报错，并给出最相近的候选名。
|
||||||
|
|
||||||
|
静默忽略多余键的代价是：现场写 ``screw_step_degs``，改参数没反应，
|
||||||
|
而日志里一切正常。报错里带候选名，是因为拼错比写错新键常见得多。
|
||||||
|
"""
|
||||||
|
unknown = sorted(set(raw) - set(_TYPES))
|
||||||
|
if not unknown:
|
||||||
|
return
|
||||||
|
lines: List[str] = []
|
||||||
|
for key in unknown:
|
||||||
|
close = difflib.get_close_matches(key, list(_TYPES), n=1, cutoff=0.6)
|
||||||
|
hint = f"，是不是想写 {close[0]}？" if close else ""
|
||||||
|
lines.append(f" - {key}{hint}")
|
||||||
|
raise GaugeConfigError(
|
||||||
|
"site.yaml 的 gauge_assembly: 段里有本框架不认识的键：\n"
|
||||||
|
+ "\n".join(lines)
|
||||||
|
+ "\n多余的键不会被忽略，因为「写了没生效」比「写错报错」难查得多。"
|
||||||
|
"确认要新增配置项时，先在 gauge_config._TYPES / _UNITS 里登记"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _sanity(cfg: GaugeConfig) -> None:
|
||||||
|
"""跨键的合理性检查：单键合法、组合起来没意义的，在这里挡住。"""
|
||||||
|
if cfg.holes_total < 1:
|
||||||
|
raise GaugeConfigError(f"gauge_assembly.holes_total 至少为 1，收到 {cfg.holes_total}")
|
||||||
|
if cfg.extreme_direction not in (-1, 1):
|
||||||
|
raise GaugeConfigError(
|
||||||
|
f"gauge_assembly.extreme_direction 只能是 +1（逆时针）或 -1（顺时针），"
|
||||||
|
f"收到 {cfg.extreme_direction}"
|
||||||
|
)
|
||||||
|
if cfg.screw_step_deg <= 0.0:
|
||||||
|
raise GaugeConfigError(
|
||||||
|
f"gauge_assembly.screw_step_deg 必须为正，收到 {cfg.screw_step_deg}"
|
||||||
|
)
|
||||||
|
if cfg.screw_max_cycles < 1:
|
||||||
|
raise GaugeConfigError(
|
||||||
|
f"gauge_assembly.screw_max_cycles 至少为 1，收到 {cfg.screw_max_cycles}"
|
||||||
|
)
|
||||||
|
if cfg.extreme_angle_deg <= 0.0:
|
||||||
|
raise GaugeConfigError(
|
||||||
|
f"gauge_assembly.extreme_angle_deg 必须为正（方向由 extreme_direction 决定，"
|
||||||
|
f"不要用负角度表方向），收到 {cfg.extreme_angle_deg}"
|
||||||
|
)
|
||||||
|
if cfg.contact_safe_delta <= cfg.contact_tangential_delta:
|
||||||
|
raise GaugeConfigError(
|
||||||
|
f"contact_safe_delta({cfg.contact_safe_delta}) 必须大于 "
|
||||||
|
f"contact_tangential_delta({cfg.contact_tangential_delta})："
|
||||||
|
"前者是「立刻抬臂」的安全上限，后者只是「停止下压」。"
|
||||||
|
"两者相等的话，接触一开始就会触发安全上限"
|
||||||
|
)
|
||||||
|
if cfg.snap_tol_px <= 0.0:
|
||||||
|
raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}")
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 自检：键名必须体现单位（防"填了毫米按米用"）
|
||||||
|
# =============================================================================
|
||||||
|
def unit_violations(keys: Optional[Iterable[str]] = None) -> List[str]:
|
||||||
|
"""返回"键名与单位不符"的清单（空 = 全部合规）。
|
||||||
|
|
||||||
|
规则：单位非空的键，**键名必须以对应的单位后缀结尾**（见 :data:`_UNIT_SUFFIX`）。
|
||||||
|
例外只有 :data:`_SUFFIX_OVERRIDE` 里显式登记的复合单位。
|
||||||
|
|
||||||
|
为什么值得一条专门的检查：``hover`` 填毫米、代码按米用，会给出一个
|
||||||
|
"看起来完全合理"的运动（悬停 5 变成悬停 5 米或 5 毫米），
|
||||||
|
而 :mod:`arm_interfaces.config_guard` 的头注释记录的正是这类事故。
|
||||||
|
"""
|
||||||
|
bad: List[str] = []
|
||||||
|
for key in keys if keys is not None else _UNITS:
|
||||||
|
unit = _UNITS[key]
|
||||||
|
if not unit:
|
||||||
|
if key not in _UNITLESS:
|
||||||
|
bad.append(f"{key}: 无量纲但未登记在 _UNITLESS")
|
||||||
|
continue
|
||||||
|
suffix = _SUFFIX_OVERRIDE.get(key)
|
||||||
|
if suffix is None:
|
||||||
|
base = unit.split("/")[0]
|
||||||
|
suffix = _UNIT_SUFFIX.get(base)
|
||||||
|
if suffix is None:
|
||||||
|
bad.append(f"{key}: 单位 {unit} 没有登记后缀（补 _UNIT_SUFFIX 或 _SUFFIX_OVERRIDE）")
|
||||||
|
continue
|
||||||
|
if not key.endswith(suffix):
|
||||||
|
bad.append(f"{key}: 单位是 {unit}，键名应以 {suffix} 结尾")
|
||||||
|
return bad
|
||||||
|
|
||||||
|
|
||||||
|
def missing_entries() -> List[str]:
|
||||||
|
"""三张表（类型/单位/默认值）是否互相覆盖完整。"""
|
||||||
|
problems: List[str] = []
|
||||||
|
for key in _TYPES:
|
||||||
|
if key not in _UNITS:
|
||||||
|
problems.append(f"{key}: 缺 _UNITS 登记")
|
||||||
|
if key not in DEFAULTS:
|
||||||
|
problems.append(f"{key}: 缺 DEFAULTS（required 的键也应写一个起点值并在文档里标注必标定）")
|
||||||
|
for key in _UNITS:
|
||||||
|
if key not in _TYPES:
|
||||||
|
problems.append(f"{key}: 在 _UNITS 里但不在 _TYPES 里")
|
||||||
|
for key in DEFAULTS:
|
||||||
|
if key not in _TYPES:
|
||||||
|
problems.append(f"{key}: 在 DEFAULTS 里但不在 _TYPES 里")
|
||||||
|
return problems
|
||||||
|
|
||||||
|
|
||||||
|
def dataclass_keys() -> List[str]:
|
||||||
|
""":class:`GaugeConfig` 的字段名 —— 与配置表比对，防"解析了但没存进对象"。"""
|
||||||
|
return [f.name for f in fields(GaugeConfig)]
|
||||||
@@ -0,0 +1,445 @@
|
|||||||
|
"""通止规装配的状态机 —— **纯逻辑**，不 import rclpy，可离线单测。
|
||||||
|
|
||||||
|
为什么单独一个文件
|
||||||
|
------------------
|
||||||
|
状态机的价值全在"顺序"和"边界"上：S07 的力判据有没有在 S08 之前生效、
|
||||||
|
E1 掉件之后到底回到哪一步、k2 是"回到 S05 记录的位姿"还是"重新拍照"。
|
||||||
|
这些错**不会在运行时暴露成异常**，只会表现成"流程走得下去但结果不对"，
|
||||||
|
和力轴向映射反了一样属于静默失效（规则 R5.1）。所以转移表写成**数据**，
|
||||||
|
由测试逐条钉住，而不是散在节点的一堆 if/else 里。
|
||||||
|
|
||||||
|
设计口径
|
||||||
|
--------
|
||||||
|
- 状态编号与看板《五、通止规定位装配：状态机设计》一致（S00–S11），
|
||||||
|
E1/E2 两条异常分支也在这里，**改名要同步改看板与文档**（规则 R6.1）。
|
||||||
|
- 转移表只回答"给这个事件、从当前状态去哪"；**做不做得了**由节点判断
|
||||||
|
（比如 S03 需要 D435 手眼，没标定就报 COARSE_FAIL 走 S04 兜底）。
|
||||||
|
状态机不掺硬件条件，否则没法离线测。
|
||||||
|
- 上下文里的两个字段（`hole_index` / `holes_total`）只影响 S09 的出口：
|
||||||
|
最后一个孔拧出后去 S11 复位，否则去 S10 换到下一个孔。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from enum import Enum
|
||||||
|
from typing import Dict, List, Optional, Tuple
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 状态与事件
|
||||||
|
# =============================================================================
|
||||||
|
class State(str, Enum):
|
||||||
|
"""S00–S11 全部状态 + 两个终态。"""
|
||||||
|
|
||||||
|
BOOT = "S00" # 各服务自检（伺服/相机/夹爪/触觉图传/推理预热）
|
||||||
|
HOME_EXTREME = "S01" # 回 home + 按 tool 系逆时针转到极限角
|
||||||
|
GRIP_CHECK = "S02" # 夹持确认（预留，当前跳过）
|
||||||
|
COARSE = "S03" # 手外 D435 粗定位
|
||||||
|
TRACK = "S04" # 腕部 D405 跟踪 k0
|
||||||
|
SNAP = "S05" # 像素稳定后拍照定位 k1/k2 + 记位姿
|
||||||
|
HOVER_K = "S06" # 到当前孔正悬停
|
||||||
|
CONTACT = "S07" # 柔顺接触（切向力突变停）
|
||||||
|
SCREW_IN = "S08" # 旋转装配循环
|
||||||
|
UNSCREW = "S09" # 反向拧出
|
||||||
|
NEXT_HOLE = "S10" # 换到下一个孔（回到 S05 记录的位姿）
|
||||||
|
RESET = "S11" # 复位退出
|
||||||
|
DONE = "DONE" # 正常结束
|
||||||
|
ABORT = "ABORT" # 中止（已按安全顺序退出到安全高度）
|
||||||
|
|
||||||
|
def __str__(self) -> str: # 日志里直接印 S07 而不是 State.CONTACT
|
||||||
|
return self.value
|
||||||
|
|
||||||
|
|
||||||
|
class Event(str, Enum):
|
||||||
|
"""驱动状态机的事件。命名规则：``动作_结果``。"""
|
||||||
|
|
||||||
|
BOOT_OK = "boot_ok"
|
||||||
|
BOOT_FAIL = "boot_fail"
|
||||||
|
|
||||||
|
POSE_OK = "pose_ok"
|
||||||
|
JOINT_LIMIT = "joint_limit" # S01 转不动 = 已经贴着关节极限
|
||||||
|
|
||||||
|
GRIP_PASS = "grip_pass"
|
||||||
|
GRIP_FAIL = "grip_fail"
|
||||||
|
GRIP_SKIPPED = "grip_skipped" # S02 预留：没装/没标定，明确跳过
|
||||||
|
|
||||||
|
COARSE_OK = "coarse_ok"
|
||||||
|
COARSE_FAIL = "coarse_fail"
|
||||||
|
|
||||||
|
TRACK_LOCKED = "track_locked"
|
||||||
|
TRACK_LOST = "track_lost" # 丢失超时 → 回 S03 兜底
|
||||||
|
|
||||||
|
SNAP_OK = "snap_ok"
|
||||||
|
SNAP_UNSTABLE = "snap_unstable" # 画面还在动，继续等
|
||||||
|
|
||||||
|
ALIGN_OK = "align_ok"
|
||||||
|
ALIGN_OUT = "align_out" # 对位超差 → 回 S05 重拍
|
||||||
|
|
||||||
|
CONTACT_OK = "contact_ok"
|
||||||
|
FORCE_OVER_LIMIT = "force_over_limit" # 力超安全上限 → 抬起
|
||||||
|
|
||||||
|
SCREW_DONE = "screw_done"
|
||||||
|
SCREW_STOP = "screw_stop" # 力判据满足：切向力>阈值 且 法向力突增
|
||||||
|
GAUGE_DROPPED = "gauge_dropped" # E1：夹持丢了
|
||||||
|
|
||||||
|
UNSCREW_DONE = "unscrew_done"
|
||||||
|
|
||||||
|
RELOADED = "reloaded" # E1：人工放好按 R 之后
|
||||||
|
TIMEOUT = "timeout" # E2：任意阶段超时
|
||||||
|
ABORT = "abort" # 用户 ^C / 外部中止
|
||||||
|
|
||||||
|
|
||||||
|
#: 终态：到这里状态机不再接受事件。
|
||||||
|
TERMINAL = frozenset({State.DONE, State.ABORT})
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 上下文：只放"影响去哪"的可变数据，不放硬件句柄
|
||||||
|
# =============================================================================
|
||||||
|
@dataclass
|
||||||
|
class GaugeContext:
|
||||||
|
"""状态机运行上下文。
|
||||||
|
|
||||||
|
:param holes_total: 一次任务要装的孔数（k1/k2 两个就是 2）。
|
||||||
|
:param hole_index: 当前孔序号，0 基。S10 会 +1。
|
||||||
|
:param relocations: E1 掉件后人工重放计数，用于日志与上限保护。
|
||||||
|
:param max_relocations: 允许的掉件重试次数，超了直接 ABORT，
|
||||||
|
避免"掉件 → 重放 → 又掉"无限循环。
|
||||||
|
"""
|
||||||
|
|
||||||
|
holes_total: int = 2
|
||||||
|
hole_index: int = 0
|
||||||
|
relocations: int = 0
|
||||||
|
max_relocations: int = 3
|
||||||
|
notes: List[str] = field(default_factory=list)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_last_hole(self) -> bool:
|
||||||
|
return self.hole_index >= self.holes_total - 1
|
||||||
|
|
||||||
|
def hole_name(self) -> str:
|
||||||
|
"""给日志用的人读孔名：k1 / k2。"""
|
||||||
|
return f"k{self.hole_index + 1}"
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 转移表
|
||||||
|
# =============================================================================
|
||||||
|
#: ``(状态, 事件) -> 下一状态``。**没有列出的组合就是非法转移**，
|
||||||
|
#: 会抛 :class:`FsmError` —— 宁可当场炸，也不要"悄悄停在原地"。
|
||||||
|
TRANSITIONS: Dict[Tuple[State, Event], State] = {
|
||||||
|
# --- S00 自检 ---
|
||||||
|
(State.BOOT, Event.BOOT_OK): State.HOME_EXTREME,
|
||||||
|
(State.BOOT, Event.BOOT_FAIL): State.ABORT,
|
||||||
|
# --- S01 home + 极限角 ---
|
||||||
|
(State.HOME_EXTREME, Event.POSE_OK): State.GRIP_CHECK,
|
||||||
|
# 转不动不是失败：已经贴着极限，就按当前角继续（提示即可）
|
||||||
|
(State.HOME_EXTREME, Event.JOINT_LIMIT): State.GRIP_CHECK,
|
||||||
|
# E1 掉件后半段：S01 流程走完 → 提示人工放置 → 按 R → 回记录位姿继续
|
||||||
|
(State.HOME_EXTREME, Event.RELOADED): State.NEXT_HOLE,
|
||||||
|
# --- S02 夹持确认（预留）---
|
||||||
|
(State.GRIP_CHECK, Event.GRIP_SKIPPED): State.TRACK,
|
||||||
|
(State.GRIP_CHECK, Event.GRIP_PASS): State.TRACK,
|
||||||
|
(State.GRIP_CHECK, Event.GRIP_FAIL): State.ABORT,
|
||||||
|
# --- S03 手外粗定位（S04 的兜底，不是必经）---
|
||||||
|
(State.COARSE, Event.COARSE_OK): State.SNAP,
|
||||||
|
(State.COARSE, Event.COARSE_FAIL): State.TRACK,
|
||||||
|
# --- S04 跟踪 ---
|
||||||
|
(State.TRACK, Event.TRACK_LOCKED): State.SNAP,
|
||||||
|
(State.TRACK, Event.TRACK_LOST): State.COARSE,
|
||||||
|
# --- S05 稳定拍照 ---
|
||||||
|
(State.SNAP, Event.SNAP_OK): State.HOVER_K,
|
||||||
|
(State.SNAP, Event.SNAP_UNSTABLE): State.SNAP, # 自环：继续等稳定
|
||||||
|
# --- S06 正悬停 ---
|
||||||
|
(State.HOVER_K, Event.ALIGN_OK): State.CONTACT,
|
||||||
|
(State.HOVER_K, Event.ALIGN_OUT): State.SNAP, # 对位超差 → 回 S05 重拍
|
||||||
|
# --- S07 柔顺接触 ---
|
||||||
|
(State.CONTACT, Event.CONTACT_OK): State.SCREW_IN,
|
||||||
|
(State.CONTACT, Event.FORCE_OVER_LIMIT): State.ABORT,
|
||||||
|
# --- S08 旋转装配循环 ---
|
||||||
|
(State.SCREW_IN, Event.SCREW_DONE): State.UNSCREW,
|
||||||
|
(State.SCREW_IN, Event.SCREW_STOP): State.UNSCREW,
|
||||||
|
(State.SCREW_IN, Event.GAUGE_DROPPED): State.HOME_EXTREME, # E1 前半段
|
||||||
|
# --- S09 反向拧出的出口与"是不是最后一个孔"有关，见 resolve_next() ---
|
||||||
|
# --- S10 换到下一个孔（= 回到 S05 记录的位姿），到了就重复 S06–S09 ---
|
||||||
|
(State.NEXT_HOLE, Event.POSE_OK): State.HOVER_K,
|
||||||
|
# --- S11 复位退出 ---
|
||||||
|
(State.RESET, Event.POSE_OK): State.DONE,
|
||||||
|
}
|
||||||
|
|
||||||
|
#: 兜底：任意非终态收到 ABORT / TIMEOUT 都去对应终态。
|
||||||
|
#: 单列出来是因为"每个状态各自列一遍"最容易漏，而漏的那个状态恰恰会在
|
||||||
|
#: 真机上表现为"按了急停但流程还在跑"。
|
||||||
|
GLOBAL: Dict[Event, State] = {
|
||||||
|
Event.ABORT: State.ABORT,
|
||||||
|
Event.TIMEOUT: State.ABORT,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def resolve_next(state: State, event: Event, ctx: GaugeContext) -> State:
|
||||||
|
"""算出下一状态。
|
||||||
|
|
||||||
|
绝大多数组合直接查 :data:`TRANSITIONS`；只有一处要结合上下文：
|
||||||
|
|
||||||
|
* ``S09 + UNSCREW_DONE``：最后一个孔 → S11 复位；否则 → S10 换到下一个孔。
|
||||||
|
|
||||||
|
其余"看起来需要上下文"的分支都用了**显式事件**，而不是让 resolve_next
|
||||||
|
去猜（比如 E1 重放用 ``RELOADED`` 而不是"看 relocations 是否大于 0"）。
|
||||||
|
猜出来的分支在测试里只能靠改内部计数复现，读代码的人也得跟着一起猜。
|
||||||
|
"""
|
||||||
|
# 全局兜底优先：谁都得能被中止
|
||||||
|
if event in GLOBAL:
|
||||||
|
return GLOBAL[event]
|
||||||
|
|
||||||
|
if state is State.UNSCREW and event is Event.UNSCREW_DONE:
|
||||||
|
return State.RESET if ctx.is_last_hole else State.NEXT_HOLE
|
||||||
|
|
||||||
|
try:
|
||||||
|
return TRANSITIONS[(state, event)]
|
||||||
|
except KeyError:
|
||||||
|
raise FsmError(state, event, ctx) from None
|
||||||
|
|
||||||
|
|
||||||
|
class FsmError(RuntimeError):
|
||||||
|
"""非法转移。附带当前状态与事件，方便直接定位是哪一步没接上。"""
|
||||||
|
|
||||||
|
def __init__(self, state: State, event: Event, ctx: Optional[GaugeContext] = None) -> None:
|
||||||
|
self.state = state
|
||||||
|
self.event = event
|
||||||
|
ctx = ctx or GaugeContext()
|
||||||
|
super().__init__(
|
||||||
|
f"通止规装配状态机：状态 {state}（{STATE_DOC.get(state, '?')}）"
|
||||||
|
f"不接受事件 {event.value}。当前孔 {ctx.hole_name()}，"
|
||||||
|
f"已重放 {ctx.relocations} 次。"
|
||||||
|
"若是新增流程，先在 gauge_fsm.TRANSITIONS 里登记这条转移。"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 状态机本体
|
||||||
|
# =============================================================================
|
||||||
|
@dataclass
|
||||||
|
class Step:
|
||||||
|
"""一次转移的记录，用于日志与事后复盘。"""
|
||||||
|
|
||||||
|
index: int
|
||||||
|
prev: State
|
||||||
|
event: Event
|
||||||
|
next: State
|
||||||
|
|
||||||
|
def __str__(self) -> str:
|
||||||
|
return f"#{self.index} {self.prev} --{self.event.value}--> {self.next}"
|
||||||
|
|
||||||
|
|
||||||
|
class GaugeFsm:
|
||||||
|
"""通止规装配状态机。
|
||||||
|
|
||||||
|
用法::
|
||||||
|
|
||||||
|
fsm = GaugeFsm(GaugeContext(holes_total=2))
|
||||||
|
fsm.handle(Event.BOOT_OK) # S00 -> S01
|
||||||
|
print(fsm.state) # S01
|
||||||
|
fsm.handle(Event.ABORT) # 任何状态都能中止
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self, ctx: Optional[GaugeContext] = None, start: State = State.BOOT) -> None:
|
||||||
|
self.ctx = ctx or GaugeContext()
|
||||||
|
self.state = start
|
||||||
|
self.history: List[Step] = []
|
||||||
|
|
||||||
|
# -- 查询 ---------------------------------------------------------------
|
||||||
|
@property
|
||||||
|
def is_terminal(self) -> bool:
|
||||||
|
return self.state in TERMINAL
|
||||||
|
|
||||||
|
@property
|
||||||
|
def ok(self) -> bool:
|
||||||
|
return self.state is State.DONE
|
||||||
|
|
||||||
|
def can(self, event: Event) -> bool:
|
||||||
|
"""这个事件在当前状态下合法吗（不改状态，给"要不要试"用）。"""
|
||||||
|
if event in GLOBAL:
|
||||||
|
return not self.is_terminal
|
||||||
|
try:
|
||||||
|
resolve_next(self.state, event, self.ctx)
|
||||||
|
except FsmError:
|
||||||
|
return False
|
||||||
|
return True
|
||||||
|
|
||||||
|
# -- 推进 ---------------------------------------------------------------
|
||||||
|
def handle(self, event: Event) -> State:
|
||||||
|
"""吃一个事件，返回新状态。非法转移抛 :class:`FsmError`。"""
|
||||||
|
if self.is_terminal:
|
||||||
|
raise FsmError(self.state, event, self.ctx)
|
||||||
|
prev = self.state
|
||||||
|
# 先算转移（非法转移在这里就抛，**不留副作用**）——
|
||||||
|
# 顺序反过来的话，一次手误的 GAUGE_DROPPED 会白白吃掉一次重放预算，
|
||||||
|
# 报错信息还说"状态不对"，而现场已经少了一次重试机会。
|
||||||
|
nxt = resolve_next(prev, event, self.ctx)
|
||||||
|
|
||||||
|
if event is Event.GAUGE_DROPPED:
|
||||||
|
self.ctx.relocations += 1
|
||||||
|
if self.ctx.relocations > self.ctx.max_relocations:
|
||||||
|
# **超过上限不抛异常**：掉件 4 次不是"程序非法状态"，是现场问题。
|
||||||
|
# 报错文案要指出现场（夹持/螺纹），而不是指着转移表说这条边不存在。
|
||||||
|
self.note(
|
||||||
|
f"通止规已掉 {self.ctx.relocations} 次（上限 "
|
||||||
|
f"{self.ctx.max_relocations}），不再自动重放，请检查夹持与螺纹"
|
||||||
|
)
|
||||||
|
nxt = State.ABORT
|
||||||
|
|
||||||
|
if nxt is State.NEXT_HOLE and prev is not State.NEXT_HOLE:
|
||||||
|
# 换孔动作：序号在**进入** S10 时推进；S10 完成就是新孔的 S06。
|
||||||
|
self.ctx.hole_index = min(self.ctx.hole_index + 1, max(0, self.ctx.holes_total - 1))
|
||||||
|
|
||||||
|
self.state = nxt
|
||||||
|
self.history.append(Step(len(self.history) + 1, prev, event, nxt))
|
||||||
|
return nxt
|
||||||
|
|
||||||
|
def note(self, text: str) -> None:
|
||||||
|
"""挂一条人工说明（比如"跳过 S03：D435 未标定"），随结果一起输出。"""
|
||||||
|
self.ctx.notes.append(text)
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 判据（纯函数，被节点与测试共用）
|
||||||
|
# =============================================================================
|
||||||
|
def pixel_stable(samples: List[Tuple[float, float]], tol_px: float, min_frames: int) -> bool:
|
||||||
|
"""S05：像素稳定判据。
|
||||||
|
|
||||||
|
:param samples: 最近若干帧的 ``(u, v)`` 像素坐标。
|
||||||
|
:param tol_px: 允许的抖动半径（像素）。
|
||||||
|
:param min_frames: 至少要看多少帧才敢说"稳定"。
|
||||||
|
:returns: 样本够多、且所有点到它们的均值的距离都 ≤ ``tol_px``。
|
||||||
|
|
||||||
|
为什么不用"两帧之差"：跟踪输出的抖动是**零均值**的，相邻两帧可能恰好
|
||||||
|
很近，于是"稳定"判据会在画面明明在晃的时候早早触发，接着拍出一张糊的
|
||||||
|
定位图 —— 表现为"孔位偶尔偏 1mm"，随机且难复现。用"到均值的最远距离"
|
||||||
|
才对得上"画面不动"这个物理事实。
|
||||||
|
"""
|
||||||
|
if len(samples) < max(2, int(min_frames)):
|
||||||
|
return False
|
||||||
|
us = [s[0] for s in samples]
|
||||||
|
vs = [s[1] for s in samples]
|
||||||
|
cu = sum(us) / len(us)
|
||||||
|
cv = sum(vs) / len(vs)
|
||||||
|
return all(math.hypot(u - cu, v - cv) <= tol_px for u, v in samples)
|
||||||
|
|
||||||
|
|
||||||
|
def tangential_jump(
|
||||||
|
shear_now: Tuple[float, float], shear_ref: Tuple[float, float], delta: float
|
||||||
|
) -> bool:
|
||||||
|
"""S07：切向力突变判据 —— **两个通道都看，任一超阈值即算**。
|
||||||
|
|
||||||
|
用户明确过：切向力到底走 X 还是 Y 通道未确认。只监控一个通道的话，
|
||||||
|
赌错了就是整段流程静默失效（下发正常、日志正常、就是停不住）。
|
||||||
|
两个都监控的代价只是可能更早停一点点，这个方向的错是安全的。
|
||||||
|
|
||||||
|
:param shear_now: 当前两路切向读数。
|
||||||
|
:param shear_ref: 静止基线的两路切向读数。
|
||||||
|
:param delta: 触发阈值（相对基线的增量）。
|
||||||
|
"""
|
||||||
|
return any(abs(n - r) >= delta for n, r in zip(shear_now, shear_ref))
|
||||||
|
|
||||||
|
|
||||||
|
def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool:
|
||||||
|
"""S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。
|
||||||
|
|
||||||
|
注意是"且"：只看切向力，规刚接触螺纹时会误停；只看法向力，规顶到孔底
|
||||||
|
时会误停。两者同时出现才说明"螺纹咬住了"，这是拧入到位的特征。
|
||||||
|
"""
|
||||||
|
return shear_max > shear_th and normal_delta > normal_th
|
||||||
|
|
||||||
|
|
||||||
|
#: S08 的四个原子步，顺序即执行顺序。
|
||||||
|
#: **故意不封装成一个"拧螺丝"复合动作**：中途掉件（E1）时，只有每一步都能
|
||||||
|
#: 单独调用、单独判断，才可能从任意一步精确恢复，而不是从头再来。
|
||||||
|
RATCHET_STEPS: Tuple[str, ...] = (
|
||||||
|
"rotate_slow", # 工具系顺时针慢转 step_deg（拧入，带着规一起转）
|
||||||
|
"release", # 松爪（规留在孔里）
|
||||||
|
"rotate_back", # 快回转 step_deg（空手回来）
|
||||||
|
"regrip", # 夹紧
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def ratchet_cycles(total_deg: float, step_deg: float) -> int:
|
||||||
|
"""要拧 ``total_deg`` 需要多少轮棘轮。向上取整，``step_deg<=0`` 报错。"""
|
||||||
|
if step_deg <= 0.0:
|
||||||
|
raise ValueError(f"单次转角必须为正，收到 {step_deg}")
|
||||||
|
if total_deg <= 0.0:
|
||||||
|
return 0
|
||||||
|
return int(math.ceil(total_deg / step_deg))
|
||||||
|
|
||||||
|
|
||||||
|
def is_joint_limit_error(text: str) -> bool:
|
||||||
|
"""S01：把驱动/规划报的字符串认成"撞到关节极限"。
|
||||||
|
|
||||||
|
认得出的理由：这一条在真机上不是异常而是**预期结果**（用户要求"转到不
|
||||||
|
触发关节极限的极限角"，那就一定会在某个角度贴住）。贴住时报 JOINT_LIMIT
|
||||||
|
继续流程；认不出来时按普通失败处理（更保守），但要把原文打印出来，
|
||||||
|
否则下次还得猜。
|
||||||
|
"""
|
||||||
|
low = (text or "").lower()
|
||||||
|
return any(
|
||||||
|
key in low
|
||||||
|
for key in (
|
||||||
|
"joint limit",
|
||||||
|
"joint_limit",
|
||||||
|
"超出关节",
|
||||||
|
"关节极限",
|
||||||
|
"限位",
|
||||||
|
"out of range",
|
||||||
|
"position limit",
|
||||||
|
"exceed",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 给人看的说明（文档 / 日志共用，避免两处各写一份，规则 M2）
|
||||||
|
# =============================================================================
|
||||||
|
STATE_DOC: Dict[State, str] = {
|
||||||
|
State.BOOT: "各服务自检（伺服/相机/夹爪/触觉图传/推理预热）",
|
||||||
|
State.HOME_EXTREME: "回 home + 按工具系逆时针转到极限角",
|
||||||
|
State.GRIP_CHECK: "夹持确认（预留，当前跳过）",
|
||||||
|
State.COARSE: "手外 D435 粗定位（S04 的兜底）",
|
||||||
|
State.TRACK: "腕部 D405 跟踪 k0",
|
||||||
|
State.SNAP: "像素稳定后拍照定位 k1/k2 并记位姿",
|
||||||
|
State.HOVER_K: "到当前孔正悬停",
|
||||||
|
State.CONTACT: "柔顺接触（切向力突变即停）",
|
||||||
|
State.SCREW_IN: "旋转装配循环（棘轮式拧入）",
|
||||||
|
State.UNSCREW: "反向拧出",
|
||||||
|
State.NEXT_HOLE: "换到下一个孔（回 S05 记录的位姿）",
|
||||||
|
State.RESET: "复位退出",
|
||||||
|
State.DONE: "正常结束",
|
||||||
|
State.ABORT: "中止（已退到安全高度）",
|
||||||
|
}
|
||||||
|
|
||||||
|
EVENT_DOC: Dict[Event, str] = {
|
||||||
|
Event.BOOT_OK: "全部服务就绪",
|
||||||
|
Event.BOOT_FAIL: "有服务不就绪",
|
||||||
|
Event.POSE_OK: "位姿就绪",
|
||||||
|
Event.JOINT_LIMIT: "已贴住关节极限",
|
||||||
|
Event.GRIP_PASS: "确认夹着通止规",
|
||||||
|
Event.GRIP_FAIL: "没夹住/判定不可信",
|
||||||
|
Event.GRIP_SKIPPED: "跳过夹持确认",
|
||||||
|
Event.COARSE_OK: "粗定位出坐标",
|
||||||
|
Event.COARSE_FAIL: "粗定位失败（回跟踪兜底）",
|
||||||
|
Event.TRACK_LOCKED: "跟踪锁定 k0",
|
||||||
|
Event.TRACK_LOST: "跟踪丢失超时",
|
||||||
|
Event.SNAP_OK: "定位到 k1/k2",
|
||||||
|
Event.SNAP_UNSTABLE: "画面还在动",
|
||||||
|
Event.ALIGN_OK: "对位在容差内",
|
||||||
|
Event.ALIGN_OUT: "对位超差",
|
||||||
|
Event.CONTACT_OK: "已接触工件面",
|
||||||
|
Event.FORCE_OVER_LIMIT: "力超安全上限",
|
||||||
|
Event.SCREW_DONE: "达到目标圈数",
|
||||||
|
Event.SCREW_STOP: "力判据满足（咬住螺纹）",
|
||||||
|
Event.GAUGE_DROPPED: "通止规掉了",
|
||||||
|
Event.UNSCREW_DONE: "已拧出",
|
||||||
|
Event.RELOADED: "人工放好并按 R",
|
||||||
|
Event.TIMEOUT: "阶段超时",
|
||||||
|
Event.ABORT: "中止",
|
||||||
|
}
|
||||||
@@ -0,0 +1,228 @@
|
|||||||
|
"""夹爪客户端：把"真机今天的接口"和"框架契约里的接口"收敛到一个出入口。
|
||||||
|
|
||||||
|
为什么要这一层（这不是多余抽象）
|
||||||
|
--------------------------------
|
||||||
|
现场查证（2026-09-24）发现**同一个服务名后面挂着两种类型**：
|
||||||
|
|
||||||
|
=============== ========================== ================================
|
||||||
|
服务 A：真机今天跑的 B：框架契约 / 仿真
|
||||||
|
=============== ========================== ================================
|
||||||
|
``/gripper/open`` ``std_srvs/Trigger`` ``arm_msgs/srv/GripperExecute``
|
||||||
|
``/gripper/close`` ``std_srvs/Trigger`` ``arm_msgs/srv/GripperExecute``
|
||||||
|
状态 ``/gripper/states`` ``/gripper/status``
|
||||||
|
（Float32MultiArray，5 元） （arm_msgs/GripperStatus）
|
||||||
|
=============== ========================== ================================
|
||||||
|
|
||||||
|
A 来自 ``arm_application/dh_pgea_node.py``（真机在跑的那个节点，grip_state 直接
|
||||||
|
带"咬住/掉落"）；B 来自 ``devices/grippers/dh_pgea/manifest.yaml`` 声明的
|
||||||
|
``arm_driver_dh_gripper`` —— **那个包还不存在**（``src/`` 下没有），目前只有
|
||||||
|
仿真适配器实现了它。
|
||||||
|
|
||||||
|
于是有两个错法，都得避免：
|
||||||
|
|
||||||
|
* 只按契约（B）写：真机上服务类型对不上，装配流程一步都走不了，
|
||||||
|
而且报错会像是"服务没起"（实际起了，只是类型不同）；
|
||||||
|
* 只按真机（A）写：仿真跑不了，而且把"临时接口"固化成事实（规则 M2）。
|
||||||
|
|
||||||
|
所以：**运行时看服务类型自己选**（:func:`pick_backend`），装配节点只依赖
|
||||||
|
:class:`GripperClient` 的 ``open/close/wait/is_gripped`` 四个方法。
|
||||||
|
等 ``arm_driver_dh_gripper`` 落地后，把 ``gauge_assembly.gripper_backend``
|
||||||
|
改回 ``auto`` 即可自动切过去，应用层不用改一行。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import Dict, List, Mapping, Optional, Sequence
|
||||||
|
|
||||||
|
#: 契约后端用到的服务名（``arm_msgs/srv/GripperExecute``）。
|
||||||
|
CONTRACT_TYPE = "arm_msgs/srv/GripperExecute"
|
||||||
|
#: 真机今天用的后端（``std_srvs/srv/Trigger``）。
|
||||||
|
LEGACY_TYPE = "std_srvs/srv/Trigger"
|
||||||
|
|
||||||
|
CONTRACT_OPEN = "/gripper/open"
|
||||||
|
CONTRACT_CLOSE = "/gripper/close"
|
||||||
|
CONTRACT_STATUS_TOPIC = "/gripper/status"
|
||||||
|
LEGACY_STATUS_TOPIC = "/gripper/states"
|
||||||
|
|
||||||
|
#: DH 状态字（与 ``arm_msgs/GripperStatus.msg`` 里的注释一致）。
|
||||||
|
GRIP_STATE_MOVING = 0
|
||||||
|
GRIP_STATE_REACHED = 1
|
||||||
|
GRIP_STATE_CAUGHT = 2
|
||||||
|
GRIP_STATE_DROPPED = 3
|
||||||
|
GRIP_STATE_NAME = {
|
||||||
|
GRIP_STATE_MOVING: "运动中",
|
||||||
|
GRIP_STATE_REACHED: "到位",
|
||||||
|
GRIP_STATE_CAUGHT: "咬住物体",
|
||||||
|
GRIP_STATE_DROPPED: "物体掉落",
|
||||||
|
}
|
||||||
|
|
||||||
|
BACKENDS = ("contract", "legacy")
|
||||||
|
|
||||||
|
|
||||||
|
class GripperInterfaceError(RuntimeError):
|
||||||
|
"""夹爪接口无法确定。消息里必须写清"现在是什么状态、怎么修"。"""
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class BackendChoice:
|
||||||
|
"""选择结果。"""
|
||||||
|
|
||||||
|
backend: str
|
||||||
|
"""``contract`` 或 ``legacy``。"""
|
||||||
|
|
||||||
|
reason: str
|
||||||
|
"""为什么这么选 —— 要能直接打进启动日志。"""
|
||||||
|
|
||||||
|
status_topic: str
|
||||||
|
"""该后端下状态从哪个话题读。"""
|
||||||
|
|
||||||
|
|
||||||
|
def pick_backend(
|
||||||
|
services: Mapping[str, Sequence[str]], prefer: str = "auto"
|
||||||
|
) -> BackendChoice:
|
||||||
|
"""按**实际服务类型**决定用哪种夹爪接口。
|
||||||
|
|
||||||
|
:param services: ``{服务名: [类型, ...]}``，直接来自
|
||||||
|
``Node.get_service_names_and_types()``。
|
||||||
|
:param prefer: ``auto`` / ``contract`` / ``legacy``。
|
||||||
|
非 auto 时若实际类型与之不符，**不静默降级**，而是报错 ——
|
||||||
|
现场明确指定了后端却跑在另一个上，是最难查的一类"改了没生效"。
|
||||||
|
:raises GripperInterfaceError: 认不出来、两种都有、或指定的后端对不上。
|
||||||
|
|
||||||
|
用类型而不是名字判断，是因为两种后端的**服务名完全一样**（都是
|
||||||
|
``/gripper/open``）：只看名字必然选不出来，而"随便选一个"的后果是
|
||||||
|
调用一个类型不匹配的服务，报出来的错与真实原因无关（规则 M1）。
|
||||||
|
"""
|
||||||
|
if prefer not in ("auto",) + BACKENDS:
|
||||||
|
raise GripperInterfaceError(
|
||||||
|
f"gripper_backend 只能是 auto / {' / '.join(BACKENDS)}，收到 {prefer!r}"
|
||||||
|
)
|
||||||
|
|
||||||
|
open_types = list(services.get(CONTRACT_OPEN, ()))
|
||||||
|
close_types = list(services.get(CONTRACT_CLOSE, ()))
|
||||||
|
has_contract = CONTRACT_TYPE in open_types and CONTRACT_TYPE in close_types
|
||||||
|
has_legacy = LEGACY_TYPE in open_types and LEGACY_TYPE in close_types
|
||||||
|
|
||||||
|
if prefer == "contract" and not has_contract:
|
||||||
|
raise GripperInterfaceError(
|
||||||
|
f"配置要求 contract 后端，但 {CONTRACT_OPEN} 的类型是 "
|
||||||
|
f"{open_types or '（服务不存在）'}，不是 {CONTRACT_TYPE}。\n"
|
||||||
|
" 怎么修：① 起仿真（它有契约服务）或等 arm_driver_dh_gripper 落地；"
|
||||||
|
"② 改用 gauge_assembly.gripper_backend: legacy（真机今天的接口）；"
|
||||||
|
"③ 改回 auto 让本函数自己选"
|
||||||
|
)
|
||||||
|
if prefer == "legacy" and not has_legacy:
|
||||||
|
raise GripperInterfaceError(
|
||||||
|
f"配置要求 legacy 后端，但 {CONTRACT_OPEN} 的类型是 "
|
||||||
|
f"{open_types or '（服务不存在）'}，不是 {LEGACY_TYPE}。\n"
|
||||||
|
" 怎么修：确认真机侧起的是 arm_application 的 dh_pgea 节点"
|
||||||
|
"（bash scripts/run_gripper.sh --arm 89）"
|
||||||
|
)
|
||||||
|
|
||||||
|
if has_contract and has_legacy:
|
||||||
|
# 同名服务两种类型同时在线：ROS 2 允许，但调用方只能撞运气。
|
||||||
|
raise GripperInterfaceError(
|
||||||
|
f"{CONTRACT_OPEN} 同时存在两种类型：{open_types}。"
|
||||||
|
"是仿真适配器和真机节点同时起着（或旧的没停干净）。\n"
|
||||||
|
" 怎么修：只留一个 —— 仿真用 scripts/stop_sim_gz.sh 停干净，"
|
||||||
|
"真机 pkill -f dh_pgea"
|
||||||
|
)
|
||||||
|
|
||||||
|
if prefer == "auto":
|
||||||
|
if has_contract:
|
||||||
|
return BackendChoice("contract", f"{CONTRACT_OPEN} 是 {CONTRACT_TYPE}", CONTRACT_STATUS_TOPIC)
|
||||||
|
if has_legacy:
|
||||||
|
return BackendChoice(
|
||||||
|
"legacy",
|
||||||
|
f"{CONTRACT_OPEN} 是 {LEGACY_TYPE}（真机 dh_pgea 节点；"
|
||||||
|
"契约驱动 arm_driver_dh_gripper 还没落地）",
|
||||||
|
LEGACY_STATUS_TOPIC,
|
||||||
|
)
|
||||||
|
raise GripperInterfaceError(_unrecognised_msg(services, open_types))
|
||||||
|
|
||||||
|
if prefer == "contract":
|
||||||
|
return BackendChoice("contract", "配置指定 contract", CONTRACT_STATUS_TOPIC)
|
||||||
|
return BackendChoice("legacy", "配置指定 legacy", LEGACY_STATUS_TOPIC)
|
||||||
|
|
||||||
|
|
||||||
|
def _unrecognised_msg(
|
||||||
|
services: Mapping[str, Sequence[str]], open_types: Sequence[str]
|
||||||
|
) -> str:
|
||||||
|
"""认不出来时，把"服务不在"和"服务在但类型不对"分开说。
|
||||||
|
|
||||||
|
两者都报成"服务没起"是这个仓库反复踩过的坑：现场明明起了夹爪节点，
|
||||||
|
却被提示"去起夹爪节点"，然后开始怀疑人生（规则 M1、R5.4）。
|
||||||
|
"""
|
||||||
|
missing = [n for n in (CONTRACT_OPEN, CONTRACT_CLOSE) if n not in services]
|
||||||
|
if missing:
|
||||||
|
return (
|
||||||
|
f"{' / '.join(missing)} 不在，夹爪节点没起来。\n"
|
||||||
|
" 怎么修：真机 bash scripts/run_gripper.sh --arm 89；"
|
||||||
|
"仿真 bash scripts/run_sim_gz.sh（仿真自带契约服务）"
|
||||||
|
)
|
||||||
|
return (
|
||||||
|
f"{CONTRACT_OPEN} 在，但类型是 {list(open_types)}，"
|
||||||
|
f"既不认识 {CONTRACT_TYPE}（契约）也不认识 {LEGACY_TYPE}（真机 dh_pgea）。\n"
|
||||||
|
" 常见原因：这个服务名被别的节点占用了。查是谁：\n"
|
||||||
|
" ros2 service list -t | grep gripper\n"
|
||||||
|
" 怎么修：停掉那个节点，或改 gauge_assembly.gripper_backend 明确指定后端"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 状态解读（纯函数）
|
||||||
|
# =============================================================================
|
||||||
|
def is_gripped_from_state(grip_state: int) -> Optional[bool]:
|
||||||
|
"""DH 状态字 → "爪里有没有东西"。
|
||||||
|
|
||||||
|
``2`` 咬住 / ``3`` 掉落 是明确的；``0`` 运动中、``1`` 到位（空爪）都要
|
||||||
|
小心：**"到位"不等于"夹住了"** —— 空爪闭合也会到位。所以只有 2/3 给
|
||||||
|
明确答案，其余返回 None（"不知道"），由调用方决定要不要等。
|
||||||
|
|
||||||
|
返回 None 而不是 False 很重要：把"不知道"当成"没夹住"会让 E1 掉件检测
|
||||||
|
在任何一次开合过程中误报。
|
||||||
|
"""
|
||||||
|
if grip_state == GRIP_STATE_CAUGHT:
|
||||||
|
return True
|
||||||
|
if grip_state == GRIP_STATE_DROPPED:
|
||||||
|
return False
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def is_dropped_from_state(grip_state: int, was_gripped: bool) -> bool:
|
||||||
|
"""E1 掉件判据：**曾经夹住过**，现在状态字变成"掉落"才是掉件。
|
||||||
|
|
||||||
|
为什么要 ``was_gripped``：装配前爪里本来就是空的，那时 ``grip_state``
|
||||||
|
可能是 3（掉落/无物），直接判成掉件会让流程一开始就报 E1。
|
||||||
|
"""
|
||||||
|
return was_gripped and grip_state == GRIP_STATE_DROPPED
|
||||||
|
|
||||||
|
|
||||||
|
def legacy_states_to_fields(data: Sequence[float]) -> Dict[str, float]:
|
||||||
|
"""``/gripper/states`` 的 5 元素数组 → 具名字段。
|
||||||
|
|
||||||
|
超出长度或为空时返回空字典（而不是抛异常）：这是**状态话题**，
|
||||||
|
收到畸形数据时应该让上层走"不知道"分支，而不是让整个节点崩掉。
|
||||||
|
"""
|
||||||
|
if not data or len(data) < 5:
|
||||||
|
return {}
|
||||||
|
return {
|
||||||
|
"initialized": float(data[0]),
|
||||||
|
"grip_state": float(data[1]),
|
||||||
|
"position": float(data[2]),
|
||||||
|
"target_position": float(data[3]),
|
||||||
|
"target_force": float(data[4]),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def describe_backend_choice(choice: BackendChoice) -> List[str]:
|
||||||
|
"""给启动日志用的多行说明（含"契约驱动还没落地"这类现场事实）。"""
|
||||||
|
lines = [f"夹爪后端 = {choice.backend}（{choice.reason}）"]
|
||||||
|
if choice.backend == "legacy":
|
||||||
|
lines.append(
|
||||||
|
" 注意：legacy 后端下 /gripper/open|close 是 std_srvs/Trigger，"
|
||||||
|
"开口是 1000‰ 的百分比语义，不是毫米；装配流程只用它开合，"
|
||||||
|
"不靠它量开口"
|
||||||
|
)
|
||||||
|
return lines
|
||||||
@@ -0,0 +1,244 @@
|
|||||||
|
"""通止规装配用的小几何/解析工具 —— **纯函数**，可离线单测。
|
||||||
|
|
||||||
|
放这里的都是"算错了不会抛异常、只会让动作做错"的东西（规则 M3、R5.1）：
|
||||||
|
|
||||||
|
* ``parse_joint_limits``：从 URDF 文本里抠关节限位，给 S01 的"转到极限角"
|
||||||
|
做软夹取。为什么不用 ``urdf_parser_py``：本机没装，多一个依赖就多一个
|
||||||
|
"在真机上 import 失败"的机会；限位信息在 URDF 里是极规整的一行
|
||||||
|
``<limit lower=... upper=...>``，用标准库解析足够，而且**可控**。
|
||||||
|
* ``prewind``：算"逆时针转到不触发关节极限的极限角"到底能转多少度。
|
||||||
|
* ``rotation_about_axis_deg``：事后验证那个动作**真的**是绕工具 Z 转的。
|
||||||
|
这条是 S01 的验收判据：不验证的话，"绕错轴"会表现成"转是转了，孔位
|
||||||
|
偏了"，而且要到几个阶段之后才看出来。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
import re
|
||||||
|
import xml.etree.ElementTree as ET
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import Dict, Iterable, Optional, Sequence, Tuple
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
#: 限位夹取时留的余量（度）。贴到限位上再下发，驱动器会因为"位置超出限位"
|
||||||
|
#: 直接拒绝整条轨迹 —— 那不是我们要的"极限角"，是报错。
|
||||||
|
DEFAULT_LIMIT_MARGIN_DEG = 3.0
|
||||||
|
|
||||||
|
_JOINT_RE = re.compile(r"<joint\s+name\s*=\s*\"([^\"]+)\"")
|
||||||
|
_ATTR_RE = re.compile(r"(lower|upper)\s*=\s*\"([-+0-9.eE]+)\"")
|
||||||
|
|
||||||
|
|
||||||
|
def _limits_by_lines(text: str) -> Dict[str, Tuple[float, float]]:
|
||||||
|
"""逐行兜底解析：记住最近一个 ``<joint name=...>``，紧跟的 ``<limit>`` 归它。
|
||||||
|
|
||||||
|
为什么要"记住"而不是"要求同一行"：正常格式化的 URDF 里关节名和 limit
|
||||||
|
**不在同一行**，只认可同一行的兜底等于永远不生效（一个不会触发的兜底，
|
||||||
|
和没有兜底是一样的）。属性顺序也按出现顺序读，不假设 lower 在 upper 前。
|
||||||
|
"""
|
||||||
|
out: Dict[str, Tuple[float, float]] = {}
|
||||||
|
current: Optional[str] = None
|
||||||
|
for line in text.splitlines():
|
||||||
|
m = _JOINT_RE.search(line)
|
||||||
|
if m:
|
||||||
|
current = m.group(1)
|
||||||
|
if current is None or "<limit" not in line:
|
||||||
|
continue
|
||||||
|
attrs = dict(_ATTR_RE.findall(line))
|
||||||
|
if "lower" in attrs and "upper" in attrs:
|
||||||
|
out[current] = (float(attrs["lower"]), float(attrs["upper"]))
|
||||||
|
current = None
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class PrewindResult:
|
||||||
|
"""S01 预绕量的计算结果。"""
|
||||||
|
|
||||||
|
target_rad: float
|
||||||
|
"""目标关节角 [rad]。"""
|
||||||
|
|
||||||
|
applied_deg: float
|
||||||
|
"""实际能转的角度（带符号，符合"逆时针为正"的约定）[deg]。"""
|
||||||
|
|
||||||
|
want_deg: float
|
||||||
|
"""希望转的角度 [deg]。"""
|
||||||
|
|
||||||
|
clamped: bool
|
||||||
|
"""是否因为关节限位被夹取过。被夹取时 ``applied_deg < want_deg``。"""
|
||||||
|
|
||||||
|
has_limits: bool
|
||||||
|
"""是否拿到了可信的关节限位。False 表示没有任何夹取依据。"""
|
||||||
|
|
||||||
|
|
||||||
|
def parse_joint_limits(urdf_xml: str) -> Dict[str, Tuple[float, float]]:
|
||||||
|
"""从 URDF 文本里解析 ``关节名 -> (lower, upper)``（弧度）。
|
||||||
|
|
||||||
|
关节名**原样保留**（含 ``Rizon4-063989_`` 前缀），调用方自己决定怎么匹配；
|
||||||
|
这里不做"去前缀"之类的猜测 —— 猜测会在换臂时静默失配，然后限位夹取
|
||||||
|
悄悄失效（所有关节都"查不到限位"，于是不夹取，于是贴着限位下发）。
|
||||||
|
|
||||||
|
解析失败的关节直接不进结果集：``continuous`` 关节本来就没有限位，
|
||||||
|
不该被当成"解析失败"。调用方用"查不到 = 不夹取"来处理。
|
||||||
|
"""
|
||||||
|
out: Dict[str, Tuple[float, float]] = {}
|
||||||
|
if not urdf_xml or "<robot" not in urdf_xml:
|
||||||
|
return out
|
||||||
|
try:
|
||||||
|
root = ET.fromstring(urdf_xml)
|
||||||
|
except ET.ParseError:
|
||||||
|
# URDF 里出现未展开的 xacro 或未转义的 ``&`` 是常见的（拿错话题就是
|
||||||
|
# 这种内容）。降级成逐行扫描仍能救回限位；救不回就返回空表（= 不夹取），
|
||||||
|
# 由调用方告警 —— **不会**返回半套数据。
|
||||||
|
return _limits_by_lines(urdf_xml)
|
||||||
|
|
||||||
|
for joint in root.findall(".//joint"):
|
||||||
|
name = joint.get("name")
|
||||||
|
limit = joint.find("limit")
|
||||||
|
if not name or limit is None:
|
||||||
|
continue
|
||||||
|
lo, up = limit.get("lower"), limit.get("upper")
|
||||||
|
if lo is None or up is None:
|
||||||
|
continue
|
||||||
|
try:
|
||||||
|
out[name] = (float(lo), float(up))
|
||||||
|
except ValueError:
|
||||||
|
continue
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def match_limits(
|
||||||
|
limits: Dict[str, Tuple[float, float]], joint_names: Sequence[str]
|
||||||
|
) -> Optional[Tuple[float, float]]:
|
||||||
|
"""按名字给一串关节名找限位，**要求全部找到**才返回。
|
||||||
|
|
||||||
|
只有一部分找到时返回 None 而不是"把找到的那部分用上"：半套限位会让
|
||||||
|
"能不能转到极限角"的结论变得不可解释（有些关节夹了、有些没夹），
|
||||||
|
与其给出一个说不清的边界，不如明确地说"这次没有限位依据"。
|
||||||
|
"""
|
||||||
|
found = [limits.get(n) for n in joint_names]
|
||||||
|
if any(v is None for v in found):
|
||||||
|
return None
|
||||||
|
return found[-1] # 最后一个 = 我们要绕的那根轴（调用方传的顺序有意义）
|
||||||
|
|
||||||
|
|
||||||
|
def prewind(
|
||||||
|
q_now_rad: float,
|
||||||
|
want_deg: float,
|
||||||
|
direction: int,
|
||||||
|
limits: Optional[Tuple[float, float]] = None,
|
||||||
|
margin_deg: float = DEFAULT_LIMIT_MARGIN_DEG,
|
||||||
|
) -> PrewindResult:
|
||||||
|
"""算 S01 的预绕目标角。
|
||||||
|
|
||||||
|
:param q_now_rad: 当前关节角 [rad]。
|
||||||
|
:param want_deg: 想转的角度（**正值**）[deg]。
|
||||||
|
:param direction: ``+1`` = 绕 +Z 逆时针（右手定则）；``-1`` = 顺时针。
|
||||||
|
符号到底对应现场的哪个方向，用真机确认一次后改 ``site.yaml`` 即可，
|
||||||
|
不要靠"改代码里的负号"—— 那种改动没人 review 得到（规则 M2）。
|
||||||
|
:param limits: ``(lower, upper)`` [rad]。给 None 时不做夹取。
|
||||||
|
:param margin_deg: 距离限位保留的余量 [deg]。
|
||||||
|
|
||||||
|
:returns: :class:`PrewindResult`。**无论有没有限位，都返回目标角**，
|
||||||
|
因为"贴到限位再继续转"在现场是预期行为（用户要求转到极限角），
|
||||||
|
驱动器真拒绝时会由 :func:`arm_application.gauge_fsm.is_joint_limit_error`
|
||||||
|
认成 JOINT_LIMIT 事件继续流程。
|
||||||
|
|
||||||
|
为什么要在这里夹取（而不是直接下发、等驱动器报错）：
|
||||||
|
驱动器拒绝一条轨迹时，整个 action 失败，连带把已经转过的那部分**也**
|
||||||
|
变成"没转过"的语义（角度实际到位了，但流程认为失败）。夹取到
|
||||||
|
"限位 − 余量"能拿到一个明确成功、且确实接近极限的姿态度。
|
||||||
|
"""
|
||||||
|
if want_deg <= 0.0:
|
||||||
|
raise ValueError(f"预绕角度必须为正，收到 {want_deg}")
|
||||||
|
if direction not in (-1, 1):
|
||||||
|
raise ValueError(f"方向只能是 +1 / -1，收到 {direction}")
|
||||||
|
|
||||||
|
signed = direction * abs(want_deg)
|
||||||
|
target = q_now_rad + math.radians(signed)
|
||||||
|
has_limits = limits is not None
|
||||||
|
clamped = False
|
||||||
|
if limits is not None:
|
||||||
|
lo = limits[0] + math.radians(margin_deg)
|
||||||
|
up = limits[1] - math.radians(margin_deg)
|
||||||
|
if lo > up: # 限位本身比余量还窄（不该发生，但别让它变成静默走偏）
|
||||||
|
lo = up = 0.5 * (limits[0] + limits[1])
|
||||||
|
if target > up:
|
||||||
|
target, clamped = up, True
|
||||||
|
elif target < lo:
|
||||||
|
target, clamped = lo, True
|
||||||
|
applied = math.degrees(target - q_now_rad)
|
||||||
|
return PrewindResult(
|
||||||
|
target_rad=target,
|
||||||
|
applied_deg=applied,
|
||||||
|
want_deg=abs(want_deg),
|
||||||
|
clamped=clamped,
|
||||||
|
has_limits=has_limits,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def rotation_about_axis_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float:
|
||||||
|
"""从两个旋转矩阵算出"绕 ``axis`` 转了多少度"（带符号）。
|
||||||
|
|
||||||
|
S01 的验收判据就靠它：绕工具 Z 转 Δ 之后，如果实际绕的是别的轴，
|
||||||
|
这个函数给出的角度会与 Δ 明显不符（或者 axis 分量不占优）。
|
||||||
|
做法是把相对旋转投影到 ``axis`` 上：
|
||||||
|
|
||||||
|
``θ = atan2( axis·(R_b·R_a⁻¹ 的反对称部分) , (trace-1)/2 )``
|
||||||
|
|
||||||
|
:param axis: **单位**轴（工具 Z 在世界系下的方向 = ``R[:, 2]``）。
|
||||||
|
"""
|
||||||
|
a = np.asarray(axis, dtype=float)
|
||||||
|
n = float(np.linalg.norm(a))
|
||||||
|
if n < 1e-9:
|
||||||
|
raise ValueError("轴向量长度为 0，无法判定旋转")
|
||||||
|
a = a / n
|
||||||
|
|
||||||
|
Rb = np.asarray(R_before, dtype=float)[:3, :3]
|
||||||
|
Ra = np.asarray(R_after, dtype=float)[:3, :3]
|
||||||
|
Rrel = Ra @ Rb.T
|
||||||
|
# 反对称部分：sin(θ)·[axis]_×
|
||||||
|
v = np.array([Rrel[2, 1] - Rrel[1, 2], Rrel[0, 2] - Rrel[2, 0], Rrel[1, 0] - Rrel[0, 1]])
|
||||||
|
sin_t = 0.5 * float(np.dot(v, a))
|
||||||
|
cos_t = 0.5 * (float(np.trace(Rrel)) - 1.0)
|
||||||
|
return math.degrees(math.atan2(sin_t, cos_t))
|
||||||
|
|
||||||
|
|
||||||
|
def axis_misalignment_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float:
|
||||||
|
"""相对旋转的转轴与 ``axis`` 的夹角 [deg]。
|
||||||
|
|
||||||
|
纯绕轴旋转时这个值≈0；转错轴时它会接近 90°。用作 S01 的判错依据：
|
||||||
|
只看"转了多少度"是不够的 —— 绕正确的轴转 90° 和绕错误的轴转 90°
|
||||||
|
给出的角度读数一样。
|
||||||
|
"""
|
||||||
|
Rb = np.asarray(R_before, dtype=float)[:3, :3]
|
||||||
|
Ra = np.asarray(R_after, dtype=float)[:3, :3]
|
||||||
|
Rrel = Ra @ Rb.T
|
||||||
|
# 旋转轴 = 反对称部分对应的向量（sinθ 因子不影响方向）
|
||||||
|
v = np.array([Rrel[2, 1] - Rrel[1, 2], Rrel[0, 2] - Rrel[2, 0], Rrel[1, 0] - Rrel[0, 1]])
|
||||||
|
nv = float(np.linalg.norm(v))
|
||||||
|
if nv < 1e-9:
|
||||||
|
return 0.0 # 几乎没转，谈不上"转错轴"
|
||||||
|
a = np.asarray(axis, dtype=float)
|
||||||
|
a = a / float(np.linalg.norm(a))
|
||||||
|
cos = abs(float(np.dot(v / nv, a))) # 取绝对值：轴线的两个方向等价
|
||||||
|
return math.degrees(math.acos(max(-1.0, min(1.0, cos))))
|
||||||
|
|
||||||
|
|
||||||
|
def unit_tool_z(R: np.ndarray) -> np.ndarray:
|
||||||
|
"""工具坐标系的 Z 轴在世界系下的方向（旋转矩阵的第三列）。"""
|
||||||
|
return np.asarray(R, dtype=float)[:3, 2]
|
||||||
|
|
||||||
|
|
||||||
|
def vertical_down_error_deg(R: np.ndarray, world_down: Iterable[float] = (0.0, 0.0, -1.0)) -> float:
|
||||||
|
"""工具 Z 与"垂直向下"的夹角 [deg]。0 = 完全垂直向下。
|
||||||
|
|
||||||
|
看板里"工具系全程垂直向下、位姿全局锁定"这句话，只有能算出这个数才算
|
||||||
|
真的在执行 —— 否则每个阶段都得靠人眼看画面判断臂歪没歪。
|
||||||
|
"""
|
||||||
|
z = unit_tool_z(R)
|
||||||
|
d = np.asarray(list(world_down), dtype=float)
|
||||||
|
d = d / float(np.linalg.norm(d))
|
||||||
|
cos = float(np.dot(z / max(1e-12, float(np.linalg.norm(z))), d))
|
||||||
|
return math.degrees(math.acos(max(-1.0, min(1.0, cos))))
|
||||||
@@ -0,0 +1,195 @@
|
|||||||
|
"""``gauge_boot`` 的单元测试：自检清单**内容**的正确性。
|
||||||
|
|
||||||
|
自检本身很容易变成"每次都过"的仪式（规则 M1）。这里钉住的是它的内容：
|
||||||
|
|
||||||
|
* 核心判据依赖的东西（触觉、相机）必须是**必检** —— 标成选检的话，
|
||||||
|
现场不接触觉也能一路跑到 S07 才失败，而那时报的是"力判据没触发"；
|
||||||
|
* 现场没有的设备（D435）不该生成检查项 —— 生成一个必然失败的项，
|
||||||
|
结果是所有人都不再看自检输出了（狼来了）；
|
||||||
|
* 「为什么需要它」必须写出来，否则现场看到"未就绪"不知道该不该急。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from arm_application.gauge_boot import (
|
||||||
|
PROBES,
|
||||||
|
BootCheck,
|
||||||
|
blocking_failures,
|
||||||
|
build_checks,
|
||||||
|
format_report,
|
||||||
|
optional_warnings,
|
||||||
|
summary_counts,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _build(**kw):
|
||||||
|
base = dict(
|
||||||
|
joint_action="rizon_arm_controller/follow_joint_trajectory",
|
||||||
|
camera="d405_left",
|
||||||
|
tactile_topic="/tactile/left/status",
|
||||||
|
gripper_open="/gripper/open",
|
||||||
|
gripper_close="/gripper/close",
|
||||||
|
gripper_status="/gripper/status",
|
||||||
|
gripper_status_type="arm_msgs/msg/GripperStatus",
|
||||||
|
)
|
||||||
|
base.update(kw)
|
||||||
|
return build_checks(**base)
|
||||||
|
|
||||||
|
|
||||||
|
def _by_key(checks):
|
||||||
|
return {c.key: c for c in checks}
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 清单内容
|
||||||
|
# =============================================================================
|
||||||
|
def test_core_checks_are_present_and_required():
|
||||||
|
checks = _by_key(_build())
|
||||||
|
for key in (
|
||||||
|
"arm_jtc",
|
||||||
|
"arm_home",
|
||||||
|
"arm_switch_tool",
|
||||||
|
"arm_compliant",
|
||||||
|
"gripper_open",
|
||||||
|
"gripper_close",
|
||||||
|
"camera_color",
|
||||||
|
"camera_info",
|
||||||
|
"tactile_status",
|
||||||
|
"gripper_status",
|
||||||
|
):
|
||||||
|
assert key in checks, f"自检清单缺 {key}"
|
||||||
|
assert checks[key].required, f"{key} 是核心判据依赖，必须是必检"
|
||||||
|
|
||||||
|
|
||||||
|
def test_tactile_must_not_be_downgraded_to_optional():
|
||||||
|
"""**负向**：触觉是 S07/S08/E1 三个判据的唯一数据源。
|
||||||
|
|
||||||
|
把它标成选检的后果不是"少一项检查"，而是"力判据全部失效但流程照跑"，
|
||||||
|
报出来的现象是"拧不进去"，离真正原因差好几层。
|
||||||
|
"""
|
||||||
|
t = _by_key(_build())["tactile_status"]
|
||||||
|
assert t.required is True
|
||||||
|
assert t.expect_type == "arm_msgs/msg/TactileStatus"
|
||||||
|
assert "S07" in t.why and "S08" in t.why
|
||||||
|
|
||||||
|
|
||||||
|
def test_camera_checks_point_at_the_right_topics():
|
||||||
|
checks = _by_key(_build(camera="d405_left"))
|
||||||
|
assert checks["camera_color"].target == "/d405_left/color/image_raw"
|
||||||
|
assert checks["camera_info"].target == "/d405_left/color/camera_info"
|
||||||
|
|
||||||
|
|
||||||
|
def test_camera_namespace_with_or_without_slashes_is_the_same():
|
||||||
|
a = _by_key(_build(camera="d405_left"))["camera_color"].target
|
||||||
|
b = _by_key(_build(camera="/d405_left/"))["camera_color"].target
|
||||||
|
assert a == b == "/d405_left/color/image_raw"
|
||||||
|
|
||||||
|
|
||||||
|
def test_no_d435_means_no_make_believe_check():
|
||||||
|
"""现场没有的设备不生成检查项：一个必然失败的项会训练人忽略输出。"""
|
||||||
|
keys = _by_key(_build(d435_camera=None))
|
||||||
|
assert "coarse_camera" not in keys
|
||||||
|
with_d435 = _by_key(_build(d435_camera="d435"))
|
||||||
|
assert "coarse_camera" in with_d435
|
||||||
|
assert with_d435["coarse_camera"].required is False, "手外相机是 S03 兜底，不该拦启动"
|
||||||
|
assert with_d435["coarse_camera"].target == "/d435/color/image_raw"
|
||||||
|
|
||||||
|
|
||||||
|
def test_tool_frame_check_only_when_configured():
|
||||||
|
assert "tool_frame" not in _by_key(_build(tool_name=""))
|
||||||
|
with_tool = _by_key(_build(tool_name="tool1"))
|
||||||
|
assert with_tool["tool_frame"].required is False, "没配工具系是告警，不是缺硬件"
|
||||||
|
assert "tool1" in with_tool["tool_frame"].why
|
||||||
|
|
||||||
|
|
||||||
|
def test_detector_check_only_when_weights_configured():
|
||||||
|
assert "detector" not in _by_key(_build(weights_path=""))
|
||||||
|
with_w = _by_key(_build(weights_path="/home/cc/ros2/models/yolo/detect_best_0.onnx"))
|
||||||
|
assert with_w["detector"].probe == "detector"
|
||||||
|
assert with_w["detector"].target.endswith(".onnx")
|
||||||
|
|
||||||
|
|
||||||
|
def test_every_check_is_actionable():
|
||||||
|
"""每项都要有：稳定 key、人读名字、合法探针、明确目标、**为什么**。"""
|
||||||
|
checks = _build(d435_camera="d435", tool_name="tool1", weights_path="/tmp/x.onnx")
|
||||||
|
assert len({c.key for c in checks}) == len(checks), "key 必须唯一"
|
||||||
|
for c in checks:
|
||||||
|
assert c.label and c.key
|
||||||
|
assert c.probe in PROBES, f"{c.key} 的探针 {c.probe} 未登记"
|
||||||
|
# 网络端点必须是绝对名：相对名会随节点命名空间变，自检会去探错名字
|
||||||
|
if c.probe != "detector":
|
||||||
|
assert c.target.startswith("/"), f"{c.key} 的目标应以 / 开头：{c.target}"
|
||||||
|
assert len(c.why) > 8, f"{c.key} 的 why 太短，缺了会怎样没说清"
|
||||||
|
|
||||||
|
|
||||||
|
def test_relative_action_name_is_normalised_to_absolute():
|
||||||
|
"""stage2 的 ``action_name`` 默认值是相对的（没有前导斜杠）。
|
||||||
|
|
||||||
|
自检里必须补成绝对名：否则节点一旦被放进 namespace，自检就会去探
|
||||||
|
``/ns/rizon_arm_controller/...``，报"控制器没起来"而实际好着呢。
|
||||||
|
"""
|
||||||
|
c = _by_key(_build(joint_action="rizon_arm_controller/follow_joint_trajectory"))["arm_jtc"]
|
||||||
|
assert c.target == "/rizon_arm_controller/follow_joint_trajectory"
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 报告与判定
|
||||||
|
# =============================================================================
|
||||||
|
def _results(**status):
|
||||||
|
"""构造 (检查项, 就绪, 细节) 列表；status 用 key 指定是否就绪。"""
|
||||||
|
out = []
|
||||||
|
for c in _build(d435_camera="d435"):
|
||||||
|
out.append((c, bool(status.get(c.key, True)), "" if status.get(c.key, True) else "没收到"))
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def test_report_marks_required_and_optional_differently():
|
||||||
|
text, failures = format_report(_results(coarse_camera=False))
|
||||||
|
assert "选检" in text
|
||||||
|
assert "告警" in text
|
||||||
|
# 选检失败也要列出来（不拦流程，但必须让人看见）
|
||||||
|
assert any("coarse_camera" in f or "手外" in f for f in failures)
|
||||||
|
|
||||||
|
|
||||||
|
def test_only_required_failures_block_the_flow():
|
||||||
|
"""**负向**：D435 没接不能拦住装配。
|
||||||
|
|
||||||
|
区分"必检失败"和"选检失败"是这一层的全部意义：混在一起会让
|
||||||
|
"可选设备没接"变成"整个流程跑不起来"。
|
||||||
|
"""
|
||||||
|
results = _results(coarse_camera=False)
|
||||||
|
assert blocking_failures(results) == []
|
||||||
|
assert optional_warnings(results) and "手外" in optional_warnings(results)[0]
|
||||||
|
|
||||||
|
results2 = _results(tactile_status=False)
|
||||||
|
assert blocking_failures(results2), "触觉缺失必须拦"
|
||||||
|
assert "触觉" in blocking_failures(results2)[0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_report_lists_why_so_the_operator_knows_urgency():
|
||||||
|
_text, failures = format_report(_results(camera_color=False))
|
||||||
|
assert failures
|
||||||
|
assert "S05" in failures[0], "失败项必须说明缺了会影响哪一步"
|
||||||
|
|
||||||
|
|
||||||
|
def test_counts_summary():
|
||||||
|
c = summary_counts(_results(coarse_camera=False))
|
||||||
|
assert c["required_total"] == 11 and c["required_ok"] == 11
|
||||||
|
assert c["optional_total"] == 1 and c["optional_ok"] == 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_all_ready_says_so():
|
||||||
|
text, failures = format_report(_results())
|
||||||
|
assert failures == []
|
||||||
|
assert "全部就绪" in text
|
||||||
|
|
||||||
|
|
||||||
|
def test_bootcheck_defaults_to_required():
|
||||||
|
"""手工构造检查项时默认必检 —— 默认选检会让漏写 required 变成静默放行。"""
|
||||||
|
c = BootCheck(key="k", label="l", probe="service", target="/x", why="因为所以要检查它")
|
||||||
|
assert c.required is True
|
||||||
|
|
||||||
|
with pytest.raises(TypeError):
|
||||||
|
BootCheck(key="k", label="l", probe="service", target="/x") # type: ignore[call-arg]
|
||||||
@@ -0,0 +1,188 @@
|
|||||||
|
"""``gauge_config`` 的单元测试。
|
||||||
|
|
||||||
|
重点不在"能不能读出一个数"，而在**读错了会不会被发现**：
|
||||||
|
|
||||||
|
* 键名拼错 → 必须报错并给候选名（否则现场"改了参数没反应"）；
|
||||||
|
* 单位与键名不符 → 必须被 ``unit_violations`` 抓到（防"填毫米按米用"）；
|
||||||
|
* 类型不对 → 必须报错，不许隐式转换；
|
||||||
|
* 跨键矛盾（安全上限低于停止阈值）→ 必须报错。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from arm_application.gauge_config import (
|
||||||
|
DEFAULTS,
|
||||||
|
GaugeConfig,
|
||||||
|
GaugeConfigError,
|
||||||
|
_TYPES,
|
||||||
|
_UNITLESS,
|
||||||
|
_UNITS,
|
||||||
|
dataclass_keys,
|
||||||
|
from_site,
|
||||||
|
missing_entries,
|
||||||
|
unit_violations,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 自检：配置表本身必须自洽
|
||||||
|
# =============================================================================
|
||||||
|
def test_schema_tables_are_consistent():
|
||||||
|
"""三张表互相覆盖完整，且 dataclass 字段与键一一对应。
|
||||||
|
|
||||||
|
这条挂了通常意味着"新加了一个键，但忘了在另一张表里登记"。
|
||||||
|
"""
|
||||||
|
assert missing_entries() == []
|
||||||
|
assert sorted(dataclass_keys()) == sorted(_TYPES), (
|
||||||
|
"GaugeConfig 的字段与配置键不一致："
|
||||||
|
f"只在 dataclass: {sorted(set(dataclass_keys()) - set(_TYPES))}，"
|
||||||
|
f"只在配置表: {sorted(set(_TYPES) - set(dataclass_keys()))}"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_every_key_name_declares_its_unit():
|
||||||
|
"""**键名必须体现单位**（``_mm`` / ``_deg`` / ``_s`` …）。
|
||||||
|
|
||||||
|
这条是"填了毫米当米用"的第一道闸门。挂掉时的修法只有两种：
|
||||||
|
改键名带上单位后缀，或（确实无量纲时）登记到 ``_UNITLESS``。
|
||||||
|
"""
|
||||||
|
assert unit_violations() == []
|
||||||
|
|
||||||
|
|
||||||
|
def test_unit_violations_actually_detect_bad_names():
|
||||||
|
"""**负向**：故意塞一个"名字不体现单位"的键，检查必须报出来。
|
||||||
|
|
||||||
|
否则 ``unit_violations`` 可能永远返回空表（检查恒真 = 没有检查，规则 M1）。
|
||||||
|
"""
|
||||||
|
key = "hover"
|
||||||
|
_UNITS[key] = "m"
|
||||||
|
try:
|
||||||
|
bad = unit_violations([key])
|
||||||
|
assert bad and "hover" in bad[0], f"应报出 hover 的问题，实际 {bad}"
|
||||||
|
finally:
|
||||||
|
del _UNITS[key]
|
||||||
|
|
||||||
|
|
||||||
|
def test_unitless_keys_are_explicitly_registered():
|
||||||
|
"""无量纲键必须显式登记，不能靠"单位为空"蒙混过关。"""
|
||||||
|
for key, unit in _UNITS.items():
|
||||||
|
if not unit:
|
||||||
|
assert key in _UNITLESS, f"{key} 无量纲却没登记在 _UNITLESS"
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 正常解析
|
||||||
|
# =============================================================================
|
||||||
|
def test_empty_section_uses_defaults_without_error():
|
||||||
|
"""整段缺失不报错（走默认值）—— 是否可用由节点决定，配置模块只如实解析。"""
|
||||||
|
cfg = from_site({})
|
||||||
|
assert isinstance(cfg, GaugeConfig)
|
||||||
|
assert cfg.extreme_angle_deg == DEFAULTS["extreme_angle_deg"]
|
||||||
|
assert cfg.dry_run is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_partial_section_merges_defaults():
|
||||||
|
cfg = from_site({"gauge_assembly": {"extreme_angle_deg": 120.0, "holes_total": 1}})
|
||||||
|
assert cfg.extreme_angle_deg == 120.0
|
||||||
|
assert cfg.holes_total == 1
|
||||||
|
assert cfg.screw_step_deg == DEFAULTS["screw_step_deg"]
|
||||||
|
|
||||||
|
|
||||||
|
def test_int_is_accepted_for_float_key_but_bool_is_not():
|
||||||
|
"""YAML 里 ``20`` 是 int，填给 float 键要能用；``true`` 不能当 1 用。"""
|
||||||
|
cfg = from_site({"gauge_assembly": {"extreme_angle_deg": 20}})
|
||||||
|
assert cfg.extreme_angle_deg == 20.0
|
||||||
|
with pytest.raises(GaugeConfigError) as ei:
|
||||||
|
from_site({"gauge_assembly": {"extreme_angle_deg": True}})
|
||||||
|
assert "类型不对" in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_string_number_is_rejected_not_coerced():
|
||||||
|
"""``"20"`` 必须报错。
|
||||||
|
|
||||||
|
隐式转换是"填了字符串、跑起来是默认值"这类静默失效的入口：
|
||||||
|
YAML 里多写一对引号，现场看不出任何异常。
|
||||||
|
"""
|
||||||
|
with pytest.raises(GaugeConfigError) as ei:
|
||||||
|
from_site({"gauge_assembly": {"contact_step_mm": "0.5"}})
|
||||||
|
msg = str(ei.value)
|
||||||
|
assert "contact_step_mm" in msg and "mm" in msg and "隐式转换" in msg
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 报错要能直接照着改
|
||||||
|
# =============================================================================
|
||||||
|
def test_unknown_key_is_rejected_with_suggestion():
|
||||||
|
with pytest.raises(GaugeConfigError) as ei:
|
||||||
|
from_site({"gauge_assembly": {"screw_step_degs": 180.0}})
|
||||||
|
msg = str(ei.value)
|
||||||
|
assert "screw_step_degs" in msg
|
||||||
|
assert "screw_step_deg" in msg, "应给出最相近的候选键名"
|
||||||
|
assert "gauge_config._TYPES" in msg, "应指出新增配置项要登记在哪"
|
||||||
|
|
||||||
|
|
||||||
|
def test_unknown_override_is_rejected():
|
||||||
|
with pytest.raises(GaugeConfigError):
|
||||||
|
from_site({}, overrides={"not_a_key": 1.0})
|
||||||
|
|
||||||
|
|
||||||
|
def test_section_of_wrong_type_reports_clearly():
|
||||||
|
with pytest.raises(GaugeConfigError) as ei:
|
||||||
|
from_site({"gauge_assembly": 12})
|
||||||
|
assert "映射" in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_cross_key_contradiction_is_rejected():
|
||||||
|
"""安全上限必须高于停止阈值，否则一接触就报"力超安全上限"。"""
|
||||||
|
with pytest.raises(GaugeConfigError) as ei:
|
||||||
|
from_site(
|
||||||
|
{
|
||||||
|
"gauge_assembly": {
|
||||||
|
"contact_tangential_delta": 500.0,
|
||||||
|
"contact_safe_delta": 500.0,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)
|
||||||
|
assert "contact_safe_delta" in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize(
|
||||||
|
"patch, expect",
|
||||||
|
[
|
||||||
|
({"holes_total": 0}, "holes_total"),
|
||||||
|
({"extreme_direction": 0}, "extreme_direction"),
|
||||||
|
({"extreme_angle_deg": -30.0}, "extreme_angle_deg"),
|
||||||
|
({"screw_step_deg": 0.0}, "screw_step_deg"),
|
||||||
|
({"screw_max_cycles": 0}, "screw_max_cycles"),
|
||||||
|
({"snap_tol_px": 0.0}, "snap_tol_px"),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
def test_bad_values_are_rejected(patch, expect):
|
||||||
|
with pytest.raises(GaugeConfigError) as ei:
|
||||||
|
from_site({"gauge_assembly": patch})
|
||||||
|
assert expect in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 派生量：文档与日志共用同一个算法
|
||||||
|
# =============================================================================
|
||||||
|
def test_derived_totals():
|
||||||
|
cfg = from_site(
|
||||||
|
{"gauge_assembly": {"screw_step_deg": 180.0, "screw_max_cycles": 3, "unscrew_extra_cycles": 1}}
|
||||||
|
)
|
||||||
|
assert cfg.total_screw_deg == 540.0
|
||||||
|
assert cfg.unscrew_cycles == 4, "拧出要比拧入多留一轮，确保脱开"
|
||||||
|
|
||||||
|
|
||||||
|
def test_describe_prints_effective_values_and_marks_calibration_needed():
|
||||||
|
cfg = from_site({})
|
||||||
|
text = "\n".join(cfg.describe())
|
||||||
|
# 实际生效的值（不是"我读了 site.yaml"）
|
||||||
|
assert "S01 预绕 150.0°" in text
|
||||||
|
assert "S08 棘轮 180°×12 轮 = 累计 2160°" in text
|
||||||
|
# 必须提示这些阈值要现场标定，否则默认值会被当成"调好的参数"
|
||||||
|
assert "现场标定" in text
|
||||||
|
# 稳定判据的口径要写出来，避免下次有人改成"相邻帧之差"
|
||||||
|
assert "到均值的最远距离" in text
|
||||||
@@ -0,0 +1,388 @@
|
|||||||
|
"""``gauge_fsm`` 的单元测试 —— 通止规装配状态机的**顺序**必须被钉住。
|
||||||
|
|
||||||
|
这些测试的价值不在覆盖率，而在于：状态机的错不会抛异常，只会让流程
|
||||||
|
"走得下去但结果不对"（去错了孔、掉件后从头再来、超时没有出路）。
|
||||||
|
所以每条转移、每条判据都要有能报红的断言，包括**负向**的（R5.2）。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from arm_application.gauge_fsm import (
|
||||||
|
EVENT_DOC,
|
||||||
|
GLOBAL,
|
||||||
|
RATCHET_STEPS,
|
||||||
|
STATE_DOC,
|
||||||
|
TERMINAL,
|
||||||
|
TRANSITIONS,
|
||||||
|
Event,
|
||||||
|
FsmError,
|
||||||
|
GaugeContext,
|
||||||
|
GaugeFsm,
|
||||||
|
State,
|
||||||
|
is_joint_limit_error,
|
||||||
|
pixel_stable,
|
||||||
|
ratchet_cycles,
|
||||||
|
resolve_next,
|
||||||
|
screw_stop,
|
||||||
|
tangential_jump,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 主线：k1 → k2 → 复位
|
||||||
|
# =============================================================================
|
||||||
|
def test_happy_path_covers_every_state_in_order():
|
||||||
|
"""一次完整装配的**确切**状态序列。
|
||||||
|
|
||||||
|
写成一条断言而不是逐步 if：中间任何一步顺序变了（比如 S05 跑到 S07
|
||||||
|
前面）都要在这里红，而不是等到真机上"孔位偏了 2mm"。
|
||||||
|
"""
|
||||||
|
fsm = GaugeFsm(GaugeContext(holes_total=2))
|
||||||
|
seq = [
|
||||||
|
(Event.BOOT_OK, State.HOME_EXTREME),
|
||||||
|
(Event.POSE_OK, State.GRIP_CHECK),
|
||||||
|
(Event.GRIP_SKIPPED, State.TRACK),
|
||||||
|
(Event.TRACK_LOCKED, State.SNAP),
|
||||||
|
(Event.SNAP_OK, State.HOVER_K),
|
||||||
|
(Event.ALIGN_OK, State.CONTACT),
|
||||||
|
(Event.CONTACT_OK, State.SCREW_IN),
|
||||||
|
(Event.SCREW_STOP, State.UNSCREW),
|
||||||
|
(Event.UNSCREW_DONE, State.NEXT_HOLE), # k1 拧出 → 换 k2
|
||||||
|
(Event.POSE_OK, State.HOVER_K), # 已在 k2 正悬停
|
||||||
|
(Event.ALIGN_OK, State.CONTACT),
|
||||||
|
(Event.CONTACT_OK, State.SCREW_IN),
|
||||||
|
(Event.SCREW_DONE, State.UNSCREW),
|
||||||
|
(Event.UNSCREW_DONE, State.RESET), # 最后一个孔 → 复位
|
||||||
|
(Event.POSE_OK, State.DONE),
|
||||||
|
]
|
||||||
|
for i, (ev, want) in enumerate(seq, start=1):
|
||||||
|
got = fsm.handle(ev)
|
||||||
|
assert got is want, f"第 {i} 步（{ev.value}）应到 {want}，实际到 {got}"
|
||||||
|
assert fsm.is_terminal and fsm.ok
|
||||||
|
assert [s.next for s in fsm.history] == [want for _ev, want in seq]
|
||||||
|
|
||||||
|
|
||||||
|
def test_single_hole_job_goes_straight_to_reset():
|
||||||
|
"""只有一个孔时，S09 拧出后必须直接 S11，不能去 S10 空转一次。"""
|
||||||
|
fsm = GaugeFsm(GaugeContext(holes_total=1))
|
||||||
|
for ev in (
|
||||||
|
Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED,
|
||||||
|
Event.SNAP_OK, Event.ALIGN_OK, Event.CONTACT_OK, Event.SCREW_STOP,
|
||||||
|
):
|
||||||
|
fsm.handle(ev)
|
||||||
|
assert fsm.handle(Event.UNSCREW_DONE) is State.RESET
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 结构性质：不靠人肉通读转移表
|
||||||
|
# =============================================================================
|
||||||
|
def test_every_state_can_be_aborted_or_reach_terminal():
|
||||||
|
"""每个非终态都必须能**走得到**终态。
|
||||||
|
|
||||||
|
漏掉的那个状态，在真机上表现为"按了中止，流程还在跑"。
|
||||||
|
ABORT/TIMEOUT 是全局兜底，这条断言顺带验证 GLOBAL 真的覆盖了全部状态。
|
||||||
|
"""
|
||||||
|
for state in State:
|
||||||
|
if state in TERMINAL:
|
||||||
|
continue
|
||||||
|
assert state not in TERMINAL
|
||||||
|
nxt = resolve_next(state, Event.ABORT, GaugeContext())
|
||||||
|
assert nxt is State.ABORT, f"{state} 不能中止"
|
||||||
|
nxt = resolve_next(state, Event.TIMEOUT, GaugeContext())
|
||||||
|
assert nxt is State.ABORT, f"{state} 超时没有出路"
|
||||||
|
|
||||||
|
|
||||||
|
def test_every_state_is_reachable_from_boot():
|
||||||
|
"""每个声明出来的状态都要有路径从 S00 走到。
|
||||||
|
|
||||||
|
否则就是一个"写在文档里、代码里永远进不去"的阶段 —— 比没有更糟：
|
||||||
|
文档说它在跑，现场却在等一个不会发生的事。
|
||||||
|
"""
|
||||||
|
seen = {State.BOOT}
|
||||||
|
frontier = [State.BOOT]
|
||||||
|
while frontier:
|
||||||
|
cur = frontier.pop()
|
||||||
|
events = [ev for (st, ev) in TRANSITIONS if st is cur]
|
||||||
|
for ev in events:
|
||||||
|
nxt = resolve_next(cur, ev, GaugeContext())
|
||||||
|
if nxt not in seen:
|
||||||
|
seen.add(nxt)
|
||||||
|
frontier.append(nxt)
|
||||||
|
# S09 的主出口是 UNSCREW_DONE（上下文相关），单独补上
|
||||||
|
seen.add(State.UNSCREW)
|
||||||
|
seen.add(State.NEXT_HOLE)
|
||||||
|
seen.add(State.RESET)
|
||||||
|
seen.add(State.DONE)
|
||||||
|
seen.add(State.ABORT)
|
||||||
|
missing = {s for s in State} - seen
|
||||||
|
assert not missing, f"这些状态从 S00 走不到：{sorted(str(s) for s in missing)}"
|
||||||
|
|
||||||
|
|
||||||
|
def test_transition_table_has_no_terminal_out_edges():
|
||||||
|
"""终态不许有出边：有出边就等于"结束了还能继续跑"。"""
|
||||||
|
for (state, _ev) in TRANSITIONS:
|
||||||
|
assert state not in TERMINAL, f"{state} 是终态却出现在转移表里"
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# S05 自环 / S06 回退：不推错孔号
|
||||||
|
# =============================================================================
|
||||||
|
def test_unstable_snap_keeps_same_hole():
|
||||||
|
"""S05 等稳定是自环，不能把孔号推走（否则会在 k1 上等，然后去装 k2）。"""
|
||||||
|
fsm = GaugeFsm(GaugeContext(holes_total=2))
|
||||||
|
for ev in (Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED):
|
||||||
|
fsm.handle(ev)
|
||||||
|
assert fsm.ctx.hole_index == 0
|
||||||
|
for _ in range(5):
|
||||||
|
fsm.handle(Event.SNAP_UNSTABLE)
|
||||||
|
assert fsm.state is State.SNAP and fsm.ctx.hole_index == 0
|
||||||
|
assert fsm.ctx.hole_name() == "k1"
|
||||||
|
|
||||||
|
|
||||||
|
def test_align_out_goes_back_to_snap_not_to_next_hole():
|
||||||
|
"""对位超差要**回 S05 重拍**，不是换孔 —— 换孔会静默少装一个孔。"""
|
||||||
|
fsm = GaugeFsm(GaugeContext(holes_total=2))
|
||||||
|
for ev in (
|
||||||
|
Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED,
|
||||||
|
Event.SNAP_OK,
|
||||||
|
):
|
||||||
|
fsm.handle(ev)
|
||||||
|
assert fsm.handle(Event.ALIGN_OUT) is State.SNAP
|
||||||
|
assert fsm.ctx.hole_index == 0
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# S03/S04 互为兜底
|
||||||
|
# =============================================================================
|
||||||
|
def test_track_loss_falls_back_to_coarse_and_back():
|
||||||
|
"""S04 丢失 → S03 粗定位；S03 识别不到 → 回 S04。两条路都要能来回。"""
|
||||||
|
fsm = GaugeFsm(GaugeContext())
|
||||||
|
for ev in (Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED):
|
||||||
|
fsm.handle(ev)
|
||||||
|
assert fsm.state is State.TRACK
|
||||||
|
assert fsm.handle(Event.TRACK_LOST) is State.COARSE
|
||||||
|
assert fsm.handle(Event.COARSE_FAIL) is State.TRACK
|
||||||
|
assert fsm.handle(Event.TRACK_LOST) is State.COARSE
|
||||||
|
assert fsm.handle(Event.COARSE_OK) is State.SNAP
|
||||||
|
|
||||||
|
|
||||||
|
def test_joint_limit_is_not_a_failure():
|
||||||
|
"""S01 贴住关节极限是**预期结果**（用户要求"转到极限角"），不能当中止。"""
|
||||||
|
fsm = GaugeFsm(GaugeContext())
|
||||||
|
fsm.handle(Event.BOOT_OK)
|
||||||
|
assert fsm.state is State.HOME_EXTREME
|
||||||
|
assert fsm.handle(Event.JOINT_LIMIT) is State.GRIP_CHECK
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# E1 掉件重放
|
||||||
|
# =============================================================================
|
||||||
|
def _to_screw_in(fsm: GaugeFsm) -> None:
|
||||||
|
for ev in (
|
||||||
|
Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED,
|
||||||
|
Event.SNAP_OK, Event.ALIGN_OK, Event.CONTACT_OK,
|
||||||
|
):
|
||||||
|
fsm.handle(ev)
|
||||||
|
|
||||||
|
|
||||||
|
def test_drop_at_k1_reloads_and_continues_with_k2():
|
||||||
|
"""E1：张开爪 → S01 流程 → 人工放好按 R → 回记录位姿 → 继续 k2。"""
|
||||||
|
fsm = GaugeFsm(GaugeContext(holes_total=2))
|
||||||
|
_to_screw_in(fsm)
|
||||||
|
assert fsm.state is State.SCREW_IN and fsm.ctx.hole_index == 0
|
||||||
|
|
||||||
|
assert fsm.handle(Event.GAUGE_DROPPED) is State.HOME_EXTREME
|
||||||
|
assert fsm.ctx.relocations == 1
|
||||||
|
# 人工重放后直接回记录位姿（等价于换孔动作），坑已跳过，所以是去 k2
|
||||||
|
assert fsm.handle(Event.RELOADED) is State.NEXT_HOLE
|
||||||
|
assert fsm.ctx.hole_index == 1
|
||||||
|
assert fsm.handle(Event.POSE_OK) is State.HOVER_K
|
||||||
|
|
||||||
|
|
||||||
|
def test_drop_budget_exhaustion_aborts_with_note():
|
||||||
|
"""掉件超过上限 → ABORT，并留下一条能读懂现场原因的备注。"""
|
||||||
|
fsm = GaugeFsm(GaugeContext(holes_total=1, max_relocations=2))
|
||||||
|
fsm.handle(Event.BOOT_OK)
|
||||||
|
for attempt in range(3):
|
||||||
|
fsm.state = State.SCREW_IN # 直接摆到"正在拧"，掉件只可能发生在这一步
|
||||||
|
nxt = fsm.handle(Event.GAUGE_DROPPED)
|
||||||
|
if attempt < 2:
|
||||||
|
assert nxt is State.HOME_EXTREME, f"第 {attempt + 1} 次掉件应还在预算内"
|
||||||
|
fsm.handle(Event.RELOADED)
|
||||||
|
else:
|
||||||
|
assert nxt is State.ABORT
|
||||||
|
assert fsm.ctx.relocations == 3
|
||||||
|
assert any("掉" in n for n in fsm.ctx.notes)
|
||||||
|
|
||||||
|
|
||||||
|
def test_illegal_drop_does_not_consume_reload_budget():
|
||||||
|
"""**负向**：在错的状态发 GAUGE_DROPPED，不能白吃掉一次重放预算。
|
||||||
|
|
||||||
|
实现里"先算转移、再记预算"就是为这条：顺序反了的话，一次手误会让现场
|
||||||
|
少一次重试机会，而报错信息只说"状态不对"，没人会想到预算被扣了。
|
||||||
|
"""
|
||||||
|
fsm = GaugeFsm(GaugeContext(holes_total=1, max_relocations=1))
|
||||||
|
assert fsm.state is State.BOOT
|
||||||
|
with pytest.raises(FsmError):
|
||||||
|
fsm.handle(Event.GAUGE_DROPPED)
|
||||||
|
assert fsm.ctx.relocations == 0
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 非法转移要当场炸
|
||||||
|
# =============================================================================
|
||||||
|
def test_illegal_transition_raises_with_actionable_message():
|
||||||
|
fsm = GaugeFsm(GaugeContext())
|
||||||
|
with pytest.raises(FsmError) as ei:
|
||||||
|
fsm.handle(Event.SCREW_DONE) # S00 不可能已经在拧
|
||||||
|
msg = str(ei.value)
|
||||||
|
assert "S00" in msg and "screw_done" in msg
|
||||||
|
assert "gauge_fsm.TRANSITIONS" in msg # 告诉改代码的人去哪登记
|
||||||
|
|
||||||
|
|
||||||
|
def test_terminal_state_rejects_further_events():
|
||||||
|
fsm = GaugeFsm(GaugeContext())
|
||||||
|
fsm.handle(Event.BOOT_FAIL)
|
||||||
|
assert fsm.state is State.ABORT
|
||||||
|
with pytest.raises(FsmError):
|
||||||
|
fsm.handle(Event.BOOT_OK)
|
||||||
|
|
||||||
|
|
||||||
|
def test_can_does_not_mutate():
|
||||||
|
fsm = GaugeFsm(GaugeContext())
|
||||||
|
before = (fsm.state, len(fsm.history))
|
||||||
|
assert fsm.can(Event.BOOT_OK) is True
|
||||||
|
assert fsm.can(Event.SCREW_DONE) is False
|
||||||
|
assert (fsm.state, len(fsm.history)) == before
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 判据
|
||||||
|
# =============================================================================
|
||||||
|
def test_pixel_stable_rejects_zero_mean_jitter():
|
||||||
|
"""**负向**：相邻两帧很近、但整体在晃，不能被判成"稳定"。
|
||||||
|
|
||||||
|
这正是"两帧之差"写法会踩的坑：跟踪抖动的相邻差是小的，于是画面明明
|
||||||
|
在晃，判据早就触发了，然后拍出一张糊图 —— 现场表现为"孔位偶尔偏 1mm"，
|
||||||
|
随机且难复现。这里用一条锯齿状的抖动序列把这个坑钉死。
|
||||||
|
"""
|
||||||
|
jitter = [(100.0 + 6.0 * (-1) ** i, 50.0) for i in range(8)]
|
||||||
|
# 相邻差都是 12px，很大；但下面这条更阴：相邻差 0、整体摆幅 ±6px
|
||||||
|
sawtooth = [(100.0, 50.0), (100.0, 50.0), (106.0, 50.0), (106.0, 50.0),
|
||||||
|
(94.0, 50.0), (94.0, 50.0)]
|
||||||
|
assert not pixel_stable(jitter, tol_px=2.0, min_frames=6)
|
||||||
|
assert not pixel_stable(sawtooth, tol_px=2.0, min_frames=6)
|
||||||
|
# 正向：真的不动
|
||||||
|
still = [(100.0 + 0.5 * i % 1.0, 50.0) for i in range(8)]
|
||||||
|
assert pixel_stable(still, tol_px=2.0, min_frames=6)
|
||||||
|
|
||||||
|
|
||||||
|
def test_pixel_stable_needs_enough_frames():
|
||||||
|
assert not pixel_stable([(100.0, 50.0)], tol_px=5.0, min_frames=6)
|
||||||
|
assert not pixel_stable([(100.0, 50.0)] * 5, tol_px=5.0, min_frames=6)
|
||||||
|
assert pixel_stable([(100.0, 50.0)] * 6, tol_px=5.0, min_frames=6)
|
||||||
|
|
||||||
|
|
||||||
|
def test_tangential_jump_watches_both_channels():
|
||||||
|
"""切向力通道未确认 → 两个都监控，任一突变即判定。"""
|
||||||
|
ref = (100.0, 100.0)
|
||||||
|
assert not tangential_jump((110.0, 130.0), ref, delta=50.0)
|
||||||
|
assert tangential_jump((160.0, 100.0), ref, delta=50.0) # X 突变
|
||||||
|
assert tangential_jump((100.0, 160.0), ref, delta=50.0) # Y 突变
|
||||||
|
# 反向突变（规被顶回来）同样算
|
||||||
|
assert tangential_jump((40.0, 100.0), ref, delta=50.0)
|
||||||
|
# 边界：恰好等于阈值算触发（>=，避免"永远差那么一点"）
|
||||||
|
assert tangential_jump((150.0, 100.0), ref, delta=50.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_screw_stop_requires_both_conditions():
|
||||||
|
"""S08 停止判据是**且**：只看一个会误停。"""
|
||||||
|
# 只有切向力大（规刚蹭到螺纹）→ 不停
|
||||||
|
assert not screw_stop(normal_delta=0.0, shear_max=999.0, normal_th=100.0, shear_th=200.0)
|
||||||
|
# 只有法向力突增（规顶到孔底）→ 不停
|
||||||
|
assert not screw_stop(normal_delta=999.0, shear_max=0.0, normal_th=100.0, shear_th=200.0)
|
||||||
|
# 两者同时满足 → 停
|
||||||
|
assert screw_stop(normal_delta=150.0, shear_max=250.0, normal_th=100.0, shear_th=200.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_ratchet_cycle_count_rounds_up():
|
||||||
|
assert ratchet_cycles(360.0, 180.0) == 2
|
||||||
|
assert ratchet_cycles(361.0, 180.0) == 3 # 差一点也要多做一轮，不能少拧
|
||||||
|
assert ratchet_cycles(0.0, 180.0) == 0
|
||||||
|
with pytest.raises(ValueError):
|
||||||
|
ratchet_cycles(360.0, 0.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_ratchet_steps_are_four_separate_atomic_steps():
|
||||||
|
"""S08 必须是四个可独立调用的原子步，顺序固定。
|
||||||
|
|
||||||
|
用户明确要求不能封装成一个"拧螺丝"复合动作：中途掉件时只有每一步都能
|
||||||
|
单独恢复，才可能精确续上（否则只能整段重来，等于少装一个孔）。
|
||||||
|
"""
|
||||||
|
assert RATCHET_STEPS == ("rotate_slow", "release", "rotate_back", "regrip")
|
||||||
|
assert len(set(RATCHET_STEPS)) == len(RATCHET_STEPS)
|
||||||
|
|
||||||
|
|
||||||
|
def test_joint_limit_recognition():
|
||||||
|
assert is_joint_limit_error("Joint limit exceeded on joint7")
|
||||||
|
assert is_joint_limit_error("规划失败：超出关节限位")
|
||||||
|
assert not is_joint_limit_error("controller not active")
|
||||||
|
assert not is_joint_limit_error("")
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 文档与代码同步
|
||||||
|
# =============================================================================
|
||||||
|
def test_every_state_and_event_is_documented():
|
||||||
|
"""每个状态/事件都要有一句人话说明。
|
||||||
|
|
||||||
|
没有说明的状态，最后一定会以"日志里只印了个 S07，没人知道 S07 干嘛"
|
||||||
|
的形式回到现场。这条断言让"新增状态忘了写说明"当场报红。
|
||||||
|
"""
|
||||||
|
assert set(STATE_DOC) == set(State), (
|
||||||
|
f"缺说明的状态: {sorted(str(s) for s in set(State) - set(STATE_DOC))}"
|
||||||
|
)
|
||||||
|
assert set(EVENT_DOC) == set(Event), (
|
||||||
|
f"缺说明的事件: {sorted(e.value for e in set(Event) - set(EVENT_DOC))}"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_global_fallback_covers_only_abort_and_timeout():
|
||||||
|
"""GLOBAL 只放"谁都该接受"的事件。
|
||||||
|
|
||||||
|
一旦有人往里加业务事件，就会悄悄多出一条所有状态都合法的边，
|
||||||
|
把"这条转移不存在"这个保护废掉。
|
||||||
|
"""
|
||||||
|
assert set(GLOBAL) == {Event.ABORT, Event.TIMEOUT}
|
||||||
|
|
||||||
|
|
||||||
|
def test_context_reports_hole_names():
|
||||||
|
ctx = GaugeContext(holes_total=2, hole_index=0)
|
||||||
|
assert ctx.hole_name() == "k1"
|
||||||
|
ctx.hole_index = 1
|
||||||
|
assert ctx.hole_name() == "k2"
|
||||||
|
assert ctx.is_last_hole
|
||||||
|
|
||||||
|
|
||||||
|
def test_history_index_is_dense_and_monotonic():
|
||||||
|
fsm = GaugeFsm(GaugeContext())
|
||||||
|
for ev in (Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED):
|
||||||
|
fsm.handle(ev)
|
||||||
|
assert [s.index for s in fsm.history] == [1, 2, 3]
|
||||||
|
assert str(fsm.history[0]) == "#1 S00 --boot_ok--> S01"
|
||||||
|
|
||||||
|
|
||||||
|
def test_math_hypot_precision_is_not_used_loosely():
|
||||||
|
"""pixel_stable 的容差是闭区间（<=），恰好等于阈值算稳定。
|
||||||
|
|
||||||
|
留一条边界断言，是因为改成 ``<`` 会让"刚好卡在容差上"的稳定画面永远
|
||||||
|
等不到 —— 现场表现为"卡在 S05 不动"，而日志只说"还在等稳定"。
|
||||||
|
"""
|
||||||
|
samples = [(100.0, 50.0), (102.0, 50.0), (98.0, 50.0)]
|
||||||
|
assert math.isclose(2.0, 2.0)
|
||||||
|
assert pixel_stable(samples, tol_px=2.0, min_frames=3)
|
||||||
|
assert not pixel_stable(samples, tol_px=1.9, min_frames=3)
|
||||||
@@ -0,0 +1,176 @@
|
|||||||
|
"""``gauge_gripper`` 的单元测试：**同名服务、两种类型**怎么选。
|
||||||
|
|
||||||
|
这一段的价值全在"选错了会报出与真实原因无关的错"上：
|
||||||
|
真机上只按契约写 → 报"服务没起"（其实起了，只是类型是 Trigger）；
|
||||||
|
选了 legacy 却在仿真里跑 → 调用类型不匹配的服务。
|
||||||
|
所以每条选择路径都要有断言，包括"两种都在线"和"一个都不在"。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from arm_application.gauge_gripper import (
|
||||||
|
CONTRACT_CLOSE,
|
||||||
|
CONTRACT_OPEN,
|
||||||
|
CONTRACT_STATUS_TOPIC,
|
||||||
|
CONTRACT_TYPE,
|
||||||
|
GRIP_STATE_CAUGHT,
|
||||||
|
GRIP_STATE_DROPPED,
|
||||||
|
GRIP_STATE_MOVING,
|
||||||
|
GRIP_STATE_REACHED,
|
||||||
|
LEGACY_STATUS_TOPIC,
|
||||||
|
LEGACY_TYPE,
|
||||||
|
GripperInterfaceError,
|
||||||
|
describe_backend_choice,
|
||||||
|
is_dropped_from_state,
|
||||||
|
is_gripped_from_state,
|
||||||
|
legacy_states_to_fields,
|
||||||
|
pick_backend,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _services(open_types, close_types=None, extra=None):
|
||||||
|
close_types = open_types if close_types is None else close_types
|
||||||
|
out = {CONTRACT_OPEN: list(open_types), CONTRACT_CLOSE: list(close_types)}
|
||||||
|
if extra:
|
||||||
|
out.update(extra)
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# auto：按实际类型选
|
||||||
|
# =============================================================================
|
||||||
|
def test_contract_backend_when_services_are_gripper_execute():
|
||||||
|
choice = pick_backend(_services([CONTRACT_TYPE]))
|
||||||
|
assert choice.backend == "contract"
|
||||||
|
assert CONTRACT_TYPE in choice.reason
|
||||||
|
assert choice.status_topic == CONTRACT_STATUS_TOPIC
|
||||||
|
|
||||||
|
|
||||||
|
def test_legacy_backend_when_services_are_trigger():
|
||||||
|
"""真机今天的样子：dh_pgea 节点的 Trigger 服务。"""
|
||||||
|
choice = pick_backend(_services([LEGACY_TYPE]))
|
||||||
|
assert choice.backend == "legacy"
|
||||||
|
assert "dh_pgea" in choice.reason
|
||||||
|
assert choice.status_topic == LEGACY_STATUS_TOPIC
|
||||||
|
# 理由里要带上"契约驱动还没落地"这个现场事实，否则下次有人以为选错了
|
||||||
|
assert "arm_driver_dh_gripper" in choice.reason
|
||||||
|
|
||||||
|
|
||||||
|
def test_missing_services_reports_how_to_start_them():
|
||||||
|
with pytest.raises(GripperInterfaceError) as ei:
|
||||||
|
pick_backend({})
|
||||||
|
msg = str(ei.value)
|
||||||
|
assert "run_gripper.sh" in msg and "run_sim_gz.sh" in msg
|
||||||
|
|
||||||
|
|
||||||
|
def test_unknown_service_type_is_not_silently_accepted():
|
||||||
|
"""类型既不是 GripperExecute 也不是 Trigger（比如被别人占用了同名服务）。
|
||||||
|
|
||||||
|
这时必须是"认不出来"的明确报错，而不是随便选一个后端然后调用失败。
|
||||||
|
"""
|
||||||
|
with pytest.raises(GripperInterfaceError) as ei:
|
||||||
|
pick_backend(_services(["std_srvs/srv/SetBool"]))
|
||||||
|
msg = str(ei.value)
|
||||||
|
assert "std_srvs/srv/SetBool" in msg
|
||||||
|
assert "service list -t" in msg, "要给出『是谁占用了这个服务名』的查法"
|
||||||
|
|
||||||
|
|
||||||
|
def test_missing_service_and_wrong_type_are_different_messages():
|
||||||
|
"""**负向**：不能在"服务不在"和"类型不对"两种情况报同一句话。
|
||||||
|
|
||||||
|
两者都报"夹爪节点没起来"，现场就会去反复重启一个根本没问题的节点
|
||||||
|
（规则 M1：报错信息必须指向真实原因）。
|
||||||
|
"""
|
||||||
|
with pytest.raises(GripperInterfaceError) as absent:
|
||||||
|
pick_backend({CONTRACT_CLOSE: [CONTRACT_TYPE]}) # open 缺失
|
||||||
|
with pytest.raises(GripperInterfaceError) as wrong:
|
||||||
|
pick_backend(_services(["std_srvs/srv/SetBool"]))
|
||||||
|
assert "没起来" in str(absent.value)
|
||||||
|
assert "没起来" not in str(wrong.value)
|
||||||
|
assert "被别的节点占用" in str(wrong.value) or "占用了" in str(wrong.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_both_backends_online_is_an_error_not_a_coin_flip():
|
||||||
|
"""仿真和真机节点同时起着（或旧进程没停干净）→ 报错，不许撞运气。"""
|
||||||
|
with pytest.raises(GripperInterfaceError) as ei:
|
||||||
|
pick_backend(_services([CONTRACT_TYPE, LEGACY_TYPE]))
|
||||||
|
msg = str(ei.value)
|
||||||
|
assert "同时存在两种类型" in msg
|
||||||
|
assert "stop_sim_gz.sh" in msg
|
||||||
|
|
||||||
|
|
||||||
|
def test_open_and_close_types_must_agree():
|
||||||
|
"""open 是契约、close 是 Trigger → 不算"契约可用"，要按认不出来报错。"""
|
||||||
|
with pytest.raises(GripperInterfaceError):
|
||||||
|
pick_backend(_services([CONTRACT_TYPE], [LEGACY_TYPE]))
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 显式指定后端：不符就报错，不静默降级
|
||||||
|
# =============================================================================
|
||||||
|
def test_explicit_contract_on_legacy_hardware_fails_with_fix_list():
|
||||||
|
with pytest.raises(GripperInterfaceError) as ei:
|
||||||
|
pick_backend(_services([LEGACY_TYPE]), prefer="contract")
|
||||||
|
msg = str(ei.value)
|
||||||
|
assert "gripper_backend" in msg
|
||||||
|
assert "legacy" in msg and "auto" in msg
|
||||||
|
|
||||||
|
|
||||||
|
def test_explicit_legacy_on_sim_fails_and_points_at_dh_pgea():
|
||||||
|
with pytest.raises(GripperInterfaceError) as ei:
|
||||||
|
pick_backend(_services([CONTRACT_TYPE]), prefer="legacy")
|
||||||
|
assert "dh_pgea" in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
def test_explicit_backend_matching_reality_is_accepted():
|
||||||
|
assert pick_backend(_services([CONTRACT_TYPE]), prefer="contract").backend == "contract"
|
||||||
|
assert pick_backend(_services([LEGACY_TYPE]), prefer="legacy").backend == "legacy"
|
||||||
|
|
||||||
|
|
||||||
|
def test_bad_prefer_value_lists_valid_options():
|
||||||
|
with pytest.raises(GripperInterfaceError) as ei:
|
||||||
|
pick_backend(_services([CONTRACT_TYPE]), prefer="whatever")
|
||||||
|
assert "auto" in str(ei.value) and "contract" in str(ei.value)
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 状态解读
|
||||||
|
# =============================================================================
|
||||||
|
def test_gripped_only_when_state_is_definitive():
|
||||||
|
""""到位"不等于"夹住"：空爪闭合也会到位。
|
||||||
|
|
||||||
|
把 1（到位）当成"夹住了"会让 E1 掉件检测在正常开合时误报；
|
||||||
|
把"不知道"当成"没夹住"会让流程一开始就报错。
|
||||||
|
"""
|
||||||
|
assert is_gripped_from_state(GRIP_STATE_CAUGHT) is True
|
||||||
|
assert is_gripped_from_state(GRIP_STATE_DROPPED) is False
|
||||||
|
assert is_gripped_from_state(GRIP_STATE_REACHED) is None
|
||||||
|
assert is_gripped_from_state(GRIP_STATE_MOVING) is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_drop_detection_requires_a_previous_grip():
|
||||||
|
"""爪里本来就是空的（装配前），不该判成掉件。"""
|
||||||
|
assert is_dropped_from_state(GRIP_STATE_DROPPED, was_gripped=True) is True
|
||||||
|
assert is_dropped_from_state(GRIP_STATE_DROPPED, was_gripped=False) is False
|
||||||
|
assert is_dropped_from_state(GRIP_STATE_CAUGHT, was_gripped=True) is False
|
||||||
|
assert is_dropped_from_state(GRIP_STATE_MOVING, was_gripped=True) is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_legacy_states_parsing_is_field_named_and_defensive():
|
||||||
|
f = legacy_states_to_fields([1.0, 2.0, 500.0, 0.0, 50.0])
|
||||||
|
assert f["initialized"] == 1.0 and f["grip_state"] == 2.0
|
||||||
|
assert f["position"] == 500.0 and f["target_force"] == 50.0
|
||||||
|
# 畸形/空数据 → 空字典（上层走"不知道"分支），不抛异常
|
||||||
|
assert legacy_states_to_fields([]) == {}
|
||||||
|
assert legacy_states_to_fields([1.0, 2.0]) == {}
|
||||||
|
|
||||||
|
|
||||||
|
def test_describe_mentions_the_unit_trap_for_legacy():
|
||||||
|
"""legacy 的"开口"是千分比不是毫米，日志里必须写明。
|
||||||
|
|
||||||
|
这是最容易误用的地方：拿到 500 以为是 500mm，实际是 10mm 行程的一半。
|
||||||
|
"""
|
||||||
|
text = "\n".join(describe_backend_choice(pick_backend(_services([LEGACY_TYPE]))))
|
||||||
|
assert "1000‰" in text and "不是毫米" in text
|
||||||
@@ -0,0 +1,209 @@
|
|||||||
|
"""``gauge_kin`` 的单元测试：S01 的"转到极限角"与"确实绕对了轴"。
|
||||||
|
|
||||||
|
两个都在测"算错了不会报错、只会做错动作"的东西：
|
||||||
|
限位夹取算错 → 要么撞限位被驱动器拒绝，要么离极限还差很远（流程看起来一样）；
|
||||||
|
转轴验证算错 → 无论实际绕哪根轴转都报"没问题"，判据等于没有。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from arm_application.gauge_kin import (
|
||||||
|
DEFAULT_LIMIT_MARGIN_DEG,
|
||||||
|
axis_misalignment_deg,
|
||||||
|
match_limits,
|
||||||
|
parse_joint_limits,
|
||||||
|
prewind,
|
||||||
|
rotation_about_axis_deg,
|
||||||
|
unit_tool_z,
|
||||||
|
vertical_down_error_deg,
|
||||||
|
)
|
||||||
|
|
||||||
|
URDF = """<?xml version="1.0"?>
|
||||||
|
<robot name="rizon">
|
||||||
|
<link name="Rizon4-063989_link6"/>
|
||||||
|
<link name="Rizon4-063989_link7"/>
|
||||||
|
<joint name="Rizon4-063989_joint6" type="revolute">
|
||||||
|
<parent link="Rizon4-063989_link5"/>
|
||||||
|
<child link="Rizon4-063989_link6"/>
|
||||||
|
<limit lower="-1.4835" upper="4.6251" effort="39" velocity="4.8869"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="Rizon4-063989_joint7" type="revolute">
|
||||||
|
<parent link="Rizon4-063989_link6"/>
|
||||||
|
<child link="Rizon4-063989_link7"/>
|
||||||
|
<limit lower="-3.0543" upper="3.0543" effort="39" velocity="4.8869"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="Rizon4-063989_roll" type="continuous">
|
||||||
|
<parent link="Rizon4-063989_link7"/>
|
||||||
|
<child link="Rizon4-063989_tool"/>
|
||||||
|
</joint>
|
||||||
|
</robot>
|
||||||
|
"""
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# URDF 解析
|
||||||
|
# =============================================================================
|
||||||
|
def test_parse_limits_keeps_full_joint_names():
|
||||||
|
"""关节名原样保留（含 SN 前缀）。
|
||||||
|
|
||||||
|
"顺手去掉前缀"这种清洗会在换臂时静默失配：所有关节都查不到限位，
|
||||||
|
于是限位夹取悄悄失效，现象是"贴着限位下发、驱动器拒绝"。
|
||||||
|
"""
|
||||||
|
lim = parse_joint_limits(URDF)
|
||||||
|
assert lim["Rizon4-063989_joint7"] == (-3.0543, 3.0543)
|
||||||
|
assert lim["Rizon4-063989_joint6"] == (-1.4835, 4.6251)
|
||||||
|
# continuous 关节没有 limit，不该被当成解析结果
|
||||||
|
assert "Rizon4-063989_roll" not in lim
|
||||||
|
|
||||||
|
|
||||||
|
def test_parse_limits_on_garbage_returns_empty_not_partial():
|
||||||
|
assert parse_joint_limits("") == {}
|
||||||
|
assert parse_joint_limits("not xml at all") == {}
|
||||||
|
# 未展开的 xacro：既不是合法 XML，也没有可用的 limit 行
|
||||||
|
assert parse_joint_limits("<robot xmlns:xacro='x'>${bad}</robot>") == {}
|
||||||
|
|
||||||
|
|
||||||
|
def test_parse_limits_falls_back_to_line_regex_on_malformed_xml():
|
||||||
|
"""URDF 里有未转义的 ``&`` 时整份 XML 解析会失败，但限位行仍然可读。
|
||||||
|
|
||||||
|
降级到逐行正则能救回"拿到的是半成品 URDF"这种常见情况；救不回就返回空表
|
||||||
|
（= 不做限位夹取，由调用方告警），**不会**返回半套数据。
|
||||||
|
"""
|
||||||
|
broken = URDF.replace('<link name="Rizon4-063989_link6"/>', '<link name="a & b"/>')
|
||||||
|
lim = parse_joint_limits(broken)
|
||||||
|
assert lim["Rizon4-063989_joint7"] == (-3.0543, 3.0543)
|
||||||
|
|
||||||
|
|
||||||
|
def test_match_limits_is_all_or_nothing():
|
||||||
|
lim = parse_joint_limits(URDF)
|
||||||
|
assert match_limits(lim, ["Rizon4-063989_joint7"]) == (-3.0543, 3.0543)
|
||||||
|
# 缺一个就整体不可用：半套限位会让"能不能转到极限"的结论无法解释
|
||||||
|
assert match_limits(lim, ["Rizon4-063989_joint7", "不存在_joint8"]) is None
|
||||||
|
assert match_limits({}, ["Rizon4-063989_joint7"]) is None
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# S01 预绕
|
||||||
|
# =============================================================================
|
||||||
|
def test_prewind_without_limits_applies_full_angle():
|
||||||
|
r = prewind(q_now_rad=0.0, want_deg=150.0, direction=1, limits=None)
|
||||||
|
assert not r.has_limits and not r.clamped
|
||||||
|
assert math.isclose(r.applied_deg, 150.0, abs_tol=1e-9)
|
||||||
|
assert math.isclose(r.target_rad, math.radians(150.0), abs_tol=1e-12)
|
||||||
|
|
||||||
|
|
||||||
|
def test_prewind_direction_sign():
|
||||||
|
"""方向只由 ``extreme_direction`` 决定，不靠代码里的负号。"""
|
||||||
|
up = prewind(0.0, 90.0, 1, None)
|
||||||
|
down = prewind(0.0, 90.0, -1, None)
|
||||||
|
assert up.applied_deg > 0 and down.applied_deg < 0
|
||||||
|
assert math.isclose(up.applied_deg, -down.applied_deg, abs_tol=1e-9)
|
||||||
|
with pytest.raises(ValueError):
|
||||||
|
prewind(0.0, 90.0, 0, None)
|
||||||
|
with pytest.raises(ValueError):
|
||||||
|
prewind(0.0, -90.0, 1, None)
|
||||||
|
|
||||||
|
|
||||||
|
def test_prewind_clamps_inside_the_limit_with_margin():
|
||||||
|
"""上限 3.0543 rad（175°），余量 3° → 最多转到 172°，且报 clamped。"""
|
||||||
|
r = prewind(q_now_rad=0.0, want_deg=150.0, direction=1, limits=(-3.0543, 3.0543))
|
||||||
|
assert math.isclose(r.applied_deg, 150.0, abs_tol=1e-9)
|
||||||
|
assert not r.clamped
|
||||||
|
|
||||||
|
r2 = prewind(q_now_rad=0.0, want_deg=179.0, direction=1, limits=(-3.0543, 3.0543))
|
||||||
|
assert r2.clamped and r2.has_limits
|
||||||
|
assert r2.applied_deg < 179.0
|
||||||
|
# 关键断言：目标必须落在 "上限 − 余量" 之内（余量是给驱动器留的，不是装饰）
|
||||||
|
upper = 3.0543 - math.radians(DEFAULT_LIMIT_MARGIN_DEG)
|
||||||
|
assert r2.target_rad <= upper + 1e-12
|
||||||
|
assert math.isclose(r2.target_rad, upper, abs_tol=1e-12)
|
||||||
|
|
||||||
|
|
||||||
|
def test_prewind_clamps_toward_lower_limit_for_negative_direction():
|
||||||
|
r = prewind(q_now_rad=0.0, want_deg=179.0, direction=-1, limits=(-3.0543, 3.0543))
|
||||||
|
lower = -3.0543 + math.radians(DEFAULT_LIMIT_MARGIN_DEG)
|
||||||
|
assert r.clamped and math.isclose(r.target_rad, lower, abs_tol=1e-12)
|
||||||
|
|
||||||
|
|
||||||
|
def test_prewind_reports_backwards_motion_when_already_inside_margin():
|
||||||
|
"""**边界**：已经贴在上限余量之内时，目标会被夹到当前位置**之后**。
|
||||||
|
|
||||||
|
这时 ``applied_deg`` 是负的（臂会往回退一点点）。这不是 bug，是"宁可退
|
||||||
|
一点也不要撞限位"；但必须如实报出来，否则日志里会出现"要转 150°、
|
||||||
|
实际转了 -2°"却没人解释。
|
||||||
|
"""
|
||||||
|
q = 3.0543 - math.radians(1.0) # 已经在余量(3°)之内
|
||||||
|
r = prewind(q_now_rad=q, want_deg=150.0, direction=1, limits=(-3.0543, 3.0543))
|
||||||
|
assert r.clamped and r.applied_deg < 0.0
|
||||||
|
assert r.target_rad <= 3.0543 - math.radians(DEFAULT_LIMIT_MARGIN_DEG) + 1e-12
|
||||||
|
|
||||||
|
|
||||||
|
def test_prewind_handles_degenerate_limits():
|
||||||
|
"""上下限比余量还窄（配置错）时，退到区间中点而不是静默走偏。"""
|
||||||
|
r = prewind(0.0, 90.0, 1, limits=(-0.01, 0.01), margin_deg=3.0)
|
||||||
|
assert math.isclose(r.target_rad, 0.0, abs_tol=1e-12)
|
||||||
|
assert r.clamped
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 绕轴旋转的验证
|
||||||
|
# =============================================================================
|
||||||
|
def _rot_z(deg: float) -> np.ndarray:
|
||||||
|
t = math.radians(deg)
|
||||||
|
return np.array(
|
||||||
|
[[math.cos(t), -math.sin(t), 0.0], [math.sin(t), math.cos(t), 0.0], [0.0, 0.0, 1.0]]
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _rot_x(deg: float) -> np.ndarray:
|
||||||
|
t = math.radians(deg)
|
||||||
|
return np.array(
|
||||||
|
[[1.0, 0.0, 0.0], [0.0, math.cos(t), -math.sin(t)], [0.0, math.sin(t), math.cos(t)]]
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_rotation_about_axis_reads_the_signed_angle():
|
||||||
|
R0 = np.eye(3)
|
||||||
|
assert math.isclose(rotation_about_axis_deg(R0, _rot_z(30.0), [0, 0, 1]), 30.0, abs_tol=1e-9)
|
||||||
|
assert math.isclose(rotation_about_axis_deg(R0, _rot_z(-30.0), [0, 0, 1]), -30.0, abs_tol=1e-9)
|
||||||
|
# 转了很多圈也要给出同一个角（atan2 值域 ±180°，这里只用来核对单步）
|
||||||
|
assert math.isclose(rotation_about_axis_deg(R0, _rot_z(179.0), [0, 0, 1]), 179.0, abs_tol=1e-9)
|
||||||
|
|
||||||
|
|
||||||
|
def test_rotation_about_axis_ignores_an_orthogonal_rotation_axis():
|
||||||
|
"""绕 X 转 60°，在 Z 轴上的投影应为 0 —— 这正是"绕错轴"的读数特征。"""
|
||||||
|
val = rotation_about_axis_deg(np.eye(3), _rot_x(60.0), [0, 0, 1])
|
||||||
|
assert abs(val) < 1e-9
|
||||||
|
|
||||||
|
|
||||||
|
def test_axis_misalignment_separates_right_axis_from_wrong_axis():
|
||||||
|
"""S01 的判错依据：转对轴 ≈0°，转错轴 ≈90°。
|
||||||
|
|
||||||
|
只看"转了多少度"不够 —— 绕正确轴转 90° 和绕错误轴转 90° 的角度读数一样。
|
||||||
|
"""
|
||||||
|
R0 = np.eye(3)
|
||||||
|
assert axis_misalignment_deg(R0, _rot_z(45.0), [0, 0, 1]) < 1e-6
|
||||||
|
assert axis_misalignment_deg(R0, _rot_x(45.0), [0, 0, 1]) > 89.0
|
||||||
|
# 轴线取反方向应视为同一根轴（cos 取绝对值）
|
||||||
|
assert axis_misalignment_deg(R0, _rot_z(45.0), [0, 0, -1]) < 1e-6
|
||||||
|
# 几乎没转时谈不上"转错轴"
|
||||||
|
assert axis_misalignment_deg(R0, np.eye(3), [0, 0, 1]) == 0.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_rotation_helpers_reject_zero_axis():
|
||||||
|
with pytest.raises(ValueError):
|
||||||
|
rotation_about_axis_deg(np.eye(3), _rot_z(10.0), [0.0, 0.0, 0.0])
|
||||||
|
|
||||||
|
|
||||||
|
def test_tool_z_and_vertical_down_error():
|
||||||
|
"""工具 Z 朝世界 -Z = 0°；水平 = 90°；朝上 = 180°。"""
|
||||||
|
R_down = np.diag([1.0, -1.0, -1.0])
|
||||||
|
assert np.allclose(unit_tool_z(R_down), [0.0, 0.0, -1.0])
|
||||||
|
assert vertical_down_error_deg(R_down) < 1e-6
|
||||||
|
assert math.isclose(vertical_down_error_deg(_rot_x(90.0) @ R_down), 90.0, abs_tol=1e-6)
|
||||||
|
assert math.isclose(vertical_down_error_deg(_rot_x(180.0) @ R_down), 180.0, abs_tol=1e-6)
|
||||||
Reference in New Issue
Block a user