P5c-5 触觉适配器落地：Gazebo 关节力 → arm_msgs/TactileStatus
链路：指尖关节 force_torque（持续发布）→ ros_gz_bridge → sim_tactile_adapter
→ /tactile/{left,right}/status。字段口径与真机 tactile_node 完全同构：
20Hz、frame_id=<side>_tactile、fnormal clamp 到非负、所有力都是"相对静止
基线的增量"。所以 compliant_descent 与 stage3_search_node 一行不改就能在
仿真里跑 —— 这是"同一套接口、换后端"契约的一部分。
新增 sim_tactile.py 把三件纯数据变换独立出来（轴向投影、桥接参数、基线
跟踪），因为这三处的错误全都**不会报错**，只会让力的语义静默错位：
轴向映射反了照样"有力、会响应、松开归零"，基线把接触吃掉了则表现为
"明明夹住了 fnormal 恒为 0"。抽出来才有单测可钉（新增 25 条）。
已实测验收（关闭夹爪夹静态薄板，60×120×80mm）：
空载 fnormal ~1e-8、contact=false（基线相减到位）
夹住 contact=true、fnormal=714.7/102.5，切向同时起变化
松开 contact 立刻回 false（靠 contact_timeout_s，因为 gz 的接触消息
是事件式的，不接触时一条都不发），力回落到 ~1e-8
启动期 check_tactile_collisions 无告警
check_tactile_collisions 顺带被负向单测逼出两处"会被静默骗过"的判据：
sdformat 对缺失的 contact/topic 会补 __default_topic__、对缺失的 collision
会补 __default__，所以只判"空"是抓不到的 —— 必须连占位值一起判。
force_scale 口径写入 config/tactile.yaml：仿真输出 = Gazebo 牛顿 × force_scale。
要共用真机那套 250/600 阈值必须先标定它，否则要么永不触发要么一碰就炸
（实测位置控制器硬压可到几百，量级与真机完全不同）。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -25,7 +25,7 @@ import os
|
||||
import re
|
||||
import subprocess
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
from typing import Dict, List, Optional, Sequence, Tuple
|
||||
|
||||
from .urdf_normalize import NormalizeReport, normalize_urdf
|
||||
|
||||
@@ -320,6 +320,197 @@ def check_base_plugins(world_file: Path) -> List[str]:
|
||||
return missing
|
||||
|
||||
|
||||
def check_tactile_collisions(
|
||||
urdf: str, expected_topics: Optional[Sequence[str]] = None
|
||||
) -> List[str]:
|
||||
"""把展开后的 URDF 转成 SDF，核对触觉传感器的引用是否真的存在。
|
||||
|
||||
返回问题列表（空 = 没问题）。``expected_topics`` 若给出，还会核对这些话题
|
||||
在 SDF 里是否真的被声明了 —— 这是**最有价值的一项**：传感器标签一旦没进
|
||||
SDF（写错位置、被 sdformat 静默丢掉），Gazebo 里就只会表现为"话题不出现、
|
||||
日志空白"，而本检查能在启动前直接指出是哪个话题缺了。
|
||||
|
||||
已实测确认能抓到的情形：
|
||||
* <contact><collision> 引用的名字在 SDF 里不存在（sdformat 命名规则变了）
|
||||
* <contact> 里没有 <topic>（sdformat 会补一个占位值 __default_topic__，
|
||||
所以这一项不能只判"空"，要连占位值一起判 —— 这是单测逼出来的）
|
||||
* 传感器标签没有产生出任何 force_torque/contact 传感器
|
||||
* 配置里声明的话题在 SDF 里找不到
|
||||
|
||||
=========================================================================
|
||||
为什么必须有这个检查
|
||||
=========================================================================
|
||||
接触传感器的 <collision> 里写的**不是我们起的名字**，而是 sdformat 从
|
||||
URDF 转 SDF 时按规则自动生成的：
|
||||
|
||||
<fixed 关节链的父 link> + "_fixed_joint_lump__" + <本 link> + "_" + collision 名
|
||||
|
||||
本机实测（Fortress 6.16）得到
|
||||
sim_left_finger_mount_fixed_joint_lump__sim_left_finger_tip_collision
|
||||
而 URDF 的 <collision> 不能命名，所以没法给它一个稳定名字。代价是：
|
||||
**换一个 sdformat 版本、命名规则一变，传感器就永远收不到数据 —— 而
|
||||
Gazebo 不会为此报任何错**（话题在、没输出，看起来像"还没接触"）。
|
||||
|
||||
同样容易静默失效的两处，这里一并核对：
|
||||
* force_torque 传感器挂的关节是否存在于 SDF（写错关节名 = 永不发布）
|
||||
* 接触传感器的话题是否在 <contact> 里（写在 <sensor> 层会被忽略，
|
||||
gz 转而使用默认的 scoped 话题名，于是"按 /tactile/left/raw 订阅"
|
||||
永远收不到东西）
|
||||
|
||||
纯文本 + 一次 `ign sdf -p`，不需要 Gazebo 在跑，所以放进启动前的检查里。
|
||||
"""
|
||||
try:
|
||||
sdf = _urdf_to_sdf(urdf)
|
||||
except SimDescriptionError as exc:
|
||||
return [f"无法把 URDF 转成 SDF 做触觉检查：{exc}"]
|
||||
|
||||
problems: List[str] = []
|
||||
try:
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
root = ET.fromstring(sdf)
|
||||
except ET.ParseError as exc:
|
||||
return [f"SDF 解析失败，无法核对触觉传感器: {exc}"]
|
||||
|
||||
# SDF 里接触传感器的 <collision> 引用的是 collision 的**限定名**。
|
||||
# ⚠️ 不要再拼一次 link 名前缀：sdformat 为 fixed 关节融合出来的 collision
|
||||
# 名字本身就已经含 link 前缀
|
||||
# （sim_left_finger_mount_fixed_joint_lump__sim_left_finger_tip_collision），
|
||||
# 拼了反而变成一个不存在的名字 —— 于是这个检查会**误报**，
|
||||
# 而误报比不检查更糟：真出问题时没人信它。两种写法都收。
|
||||
collision_names = set()
|
||||
for link in root.iter("link"):
|
||||
lname = link.get("name", "")
|
||||
for col in link.findall("collision"):
|
||||
cname = col.get("name", "")
|
||||
if cname:
|
||||
collision_names.add(cname)
|
||||
collision_names.add(f"{lname}::{cname}")
|
||||
elif lname:
|
||||
collision_names.add(lname)
|
||||
|
||||
joint_names = {j.get("name", "") for j in root.iter("joint")}
|
||||
sdf_topics = {(t.text or "").strip() for t in root.iter("topic")}
|
||||
n_contact = 0
|
||||
n_ft = 0
|
||||
|
||||
for sensor in root.iter("sensor"):
|
||||
stype = sensor.get("type", "")
|
||||
sname = sensor.get("name", "?")
|
||||
if stype == "contact":
|
||||
n_contact += 1
|
||||
contact = sensor.find("contact")
|
||||
if contact is None:
|
||||
problems.append(f"接触传感器 {sname} 没有 <contact> 块")
|
||||
continue
|
||||
ref = (contact.findtext("collision") or "").strip()
|
||||
if not ref or ref == "__default__":
|
||||
problems.append(
|
||||
f"接触传感器 {sname} 的 <contact> 里没有可用的 <collision>"
|
||||
f"（实测值: {ref or '空'}）。\n"
|
||||
f" 留空不会「监测全部」：sdformat 会补成 __default__"
|
||||
f"（一个不存在的 collision），症状是永远没有输出。"
|
||||
)
|
||||
elif ref not in collision_names:
|
||||
problems.append(
|
||||
f"接触传感器 {sname} 引用的 collision 在 SDF 里不存在: {ref}\n"
|
||||
f" 这几乎总是 sdformat 的自动命名规则变了。"
|
||||
f"现有 collision: {sorted(collision_names)[:4]} ...\n"
|
||||
f" 用 `ign sdf -p <展开后的 urdf>` 看实际名字，"
|
||||
f"改 devices/sim/gazebo/urdf/arm.sim.sensors.xacro 里的 <collision>。"
|
||||
)
|
||||
# ⚠️ 同样要防 sdformat 的占位值：缺 <topic> 时它会补一个
|
||||
# __default_topic__（实测），于是"有没有 <topic>"这种检查会被骗过去。
|
||||
# __default_topic__ 不是可用话题名 —— gz 实际发的是
|
||||
# /world/<w>/model/<m>/link/<l>/sensor/<s>/contact。
|
||||
ctopic = (contact.findtext("topic") or "").strip()
|
||||
if ctopic in ("", "__default_topic__"):
|
||||
problems.append(
|
||||
f"接触传感器 {sname} 的 <contact> 里没有可用 <topic>"
|
||||
f"（实测值: {ctopic or '空'}）。\n"
|
||||
f" 话题必须写在 contact/topic（写在 <sensor> 层会被静默忽略，"
|
||||
f"gz 改用默认 scoped 话题名）。"
|
||||
)
|
||||
elif stype == "force_torque":
|
||||
n_ft += 1
|
||||
parent = _sdf_parent_joint(root, sensor)
|
||||
if parent is None:
|
||||
problems.append(
|
||||
f"力传感器 {sname} 不在任何 <joint> 里。\n"
|
||||
f" force_torque 在 SDF 里只能属于关节；用 "
|
||||
f"<gazebo reference=「关节名」><sensor .../></gazebo> 表达，"
|
||||
f"sdformat 会把它移进对应的 <joint>。"
|
||||
)
|
||||
elif parent not in joint_names:
|
||||
problems.append(f"力传感器 {sname} 挂在未知关节 {parent} 上")
|
||||
if (sensor.findtext("topic") or "").strip() == "":
|
||||
problems.append(
|
||||
f"力传感器 {sname} 没有 <topic>（它不会用默认话题名，"
|
||||
f"而是完全不发布）。"
|
||||
)
|
||||
|
||||
# 传感器是否被整个丢掉 —— 这一项最容易被忽略，也最难现场排查。
|
||||
if n_contact == 0 and n_ft == 0:
|
||||
problems.append(
|
||||
"SDF 里**一个触觉传感器都没有**。\n"
|
||||
" 说明 arm.sim.sensors.xacro 里的 <gazebo reference=...> 名字写错、"
|
||||
"或传感器标签写到了错误的位置 —— sdformat 会把整块**静默丢掉**：\n"
|
||||
" 转换成功、无警告、Gazebo 里话题不出现、日志空白。"
|
||||
)
|
||||
if expected_topics:
|
||||
missing = [t for t in expected_topics if t not in sdf_topics]
|
||||
if missing:
|
||||
problems.append(
|
||||
f"以下触觉话题在 SDF 里不存在: {', '.join(missing)}\n"
|
||||
f" 配置（tactile.yaml）里声明了这些话题，但描述里没有产生它们。\n"
|
||||
f" 常见原因：<gazebo reference=「关节名」> 的关节名不存在"
|
||||
f"（整个传感器被丢掉），或 contact 传感器少写了 <topic>。\n"
|
||||
f" SDF 里实际有的话题: {sorted(t for t in sdf_topics if t)}"
|
||||
)
|
||||
return problems
|
||||
|
||||
|
||||
def _sdf_parent_joint(root, sensor) -> Optional[str]:
|
||||
"""在 SDF 树里找 sensor 所属的 <joint> 名。"""
|
||||
for joint in root.iter("joint"):
|
||||
for s in joint.findall("sensor"):
|
||||
if s is sensor:
|
||||
return joint.get("name", "")
|
||||
return None
|
||||
|
||||
|
||||
def _urdf_to_sdf(urdf: str) -> str:
|
||||
"""调 `ign sdf -p` 把 URDF 文本转成 SDF 文本。
|
||||
|
||||
走命令行而不是 Python 绑定：sdformat 的 Python 绑定在这个环境里没有装，
|
||||
而 ign CLI 是仿真环境的必备件（sim_env.sh 里就有）。
|
||||
"""
|
||||
import tempfile
|
||||
|
||||
with tempfile.NamedTemporaryFile("w", suffix=".urdf", delete=False,
|
||||
encoding="utf-8") as fh:
|
||||
fh.write(urdf)
|
||||
tmp = fh.name
|
||||
try:
|
||||
r = subprocess.run(
|
||||
["ign", "sdf", "-p", tmp], capture_output=True, text=True, timeout=120
|
||||
)
|
||||
except FileNotFoundError as exc:
|
||||
raise SimDescriptionError(
|
||||
"找不到 ign 命令，无法做 SDF 转换检查。修法: source scripts/sim_env.sh"
|
||||
) from exc
|
||||
finally:
|
||||
try:
|
||||
os.unlink(tmp)
|
||||
except OSError:
|
||||
pass
|
||||
if r.returncode != 0:
|
||||
raise SimDescriptionError(
|
||||
f"ign sdf 转换失败（退出码 {r.returncode}）: {(r.stderr or r.stdout).strip()[:600]}"
|
||||
)
|
||||
return r.stdout
|
||||
|
||||
|
||||
def expand_xacro(
|
||||
xacro_path: Path,
|
||||
mappings: Optional[Dict[str, object]] = None,
|
||||
|
||||
@@ -0,0 +1,317 @@
|
||||
"""仿真触觉的配置解析、轴向投影与基线跟踪。
|
||||
|
||||
=============================================================================
|
||||
为什么单独一个模块
|
||||
=============================================================================
|
||||
把 Gazebo 的关节力传感器变成 arm_msgs/TactileStatus，真正有内容的是三件事：
|
||||
|
||||
1. 轴向投影 关节坐标系的三向力 → (fnormal, fshearx, fsheary)
|
||||
2. 基线相减 静止时关节要承担手指自重，不减掉就是一个恒定偏置
|
||||
3. 接触判据 接触传感器是**事件式**发布，必须靠超时判"已离开"
|
||||
|
||||
这三件事都是纯数据变换，不需要 ROS 跑起来就能验。所以实现放在这里、用普通
|
||||
函数/类表达，节点（sim_tactile_adapter.py）只做订阅/发布这层壳。
|
||||
好处是它们能被单测钉死 —— 轴向映射搞反、基线算错这类错误，
|
||||
在"力有数值变化"的直观观察下完全看不出异常，只能靠测试兜住。
|
||||
|
||||
轴映射的实测依据与口径见 devices/sim/gazebo/config/tactile.yaml 的注释。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Sequence, Tuple
|
||||
|
||||
import yaml
|
||||
|
||||
# 默认配置位置（相对仓库根）。与 cameras.yaml 同目录，属"物料而非代码"。
|
||||
DEFAULT_TACTILE_REL = "devices/sim/gazebo/config/tactile.yaml"
|
||||
|
||||
# 轴向只认这三根轴。写成别的一律报错 —— 静默地把 'Y' 当成未知轴处理，
|
||||
# 会让力的语义整体错位而没有任何提示。
|
||||
_VALID_AXES = ("x", "y", "z")
|
||||
|
||||
|
||||
class TactileConfigError(RuntimeError):
|
||||
"""触觉配置有问题时抛出，消息面向使用者。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AxisSpec:
|
||||
"""把关节力传感器的三根轴映射成 Orisys 的三路力。
|
||||
|
||||
``normal`` 是**指尖法向**（压下去的方向）；另两根是切向。
|
||||
符号单独给：实测确认两指压同一个物体读数同号，但保留符号项是为了
|
||||
换夹爪/换传感器时不必改代码。
|
||||
"""
|
||||
|
||||
normal: str = "y"
|
||||
shear_x: str = "z"
|
||||
shear_y: str = "x"
|
||||
sign_normal: float = 1.0
|
||||
sign_shear_x: float = 1.0
|
||||
sign_shear_y: float = 1.0
|
||||
|
||||
def project(self, force: Sequence[float], scale: float = 1.0) -> Tuple[float, float, float]:
|
||||
"""三向力（关节坐标系，牛顿）→ (fnormal, fshearx, fsheary)。
|
||||
|
||||
``scale`` 是 config 里的 force_scale，见该文件头的「单位口径」。
|
||||
"""
|
||||
idx = {a: i for i, a in enumerate(_VALID_AXES)}
|
||||
fx, fy, fz = (float(v) for v in force)
|
||||
raw = {"x": fx, "y": fy, "z": fz}
|
||||
return (
|
||||
self.sign_normal * raw[self.normal] * scale,
|
||||
self.sign_shear_x * raw[self.shear_x] * scale,
|
||||
self.sign_shear_y * raw[self.shear_y] * scale,
|
||||
)
|
||||
|
||||
def shear_indices(self) -> Tuple[int, int]:
|
||||
"""两个切向轴在 (x,y,z) 里的下标，用于把接触点转成 centroid_u/v。"""
|
||||
idx = {a: i for i, a in enumerate(_VALID_AXES)}
|
||||
return idx[self.shear_x], idx[self.shear_y]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TactileSide:
|
||||
"""一"指"的声明。"""
|
||||
|
||||
side: str
|
||||
wrench_topic: str
|
||||
contact_topic: str
|
||||
frame: str
|
||||
|
||||
# --- ROS 侧话题名：与真机 tactile_node 完全一致，应用层不用改 -----------
|
||||
@property
|
||||
def status_topic(self) -> str:
|
||||
return f"/tactile/{self.side}/status"
|
||||
|
||||
@property
|
||||
def image_topic(self) -> str:
|
||||
return f"/tactile/{self.side}/image"
|
||||
|
||||
@property
|
||||
def vector_topic(self) -> str:
|
||||
return f"/tactile/{self.side}/vector"
|
||||
|
||||
# --- Gazebo 侧：桥接的输入 ---------------------------------------------
|
||||
@property
|
||||
def gz_wrench_msg(self) -> str:
|
||||
return "gz.msgs.Wrench"
|
||||
|
||||
@property
|
||||
def gz_contact_msg(self) -> str:
|
||||
return "gz.msgs.Contacts"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TactileSpec:
|
||||
"""整个仿真触觉的声明。"""
|
||||
|
||||
sides: Tuple[TactileSide, ...]
|
||||
axis: AxisSpec
|
||||
publish_hz: float
|
||||
baseline_seconds: float
|
||||
rebaseline_when_idle: bool
|
||||
rebaseline_tau_s: float
|
||||
contact_timeout_s: float
|
||||
min_contact_points: int
|
||||
force_scale: float
|
||||
image_hz: float
|
||||
image_width: int
|
||||
image_height: int
|
||||
unit: str = "N"
|
||||
|
||||
def by_side(self, side: str) -> Optional[TactileSide]:
|
||||
for s in self.sides:
|
||||
if s.side == side:
|
||||
return s
|
||||
return None
|
||||
|
||||
|
||||
def load_tactile(path: Optional[Path] = None, root: Optional[Path] = None) -> TactileSpec:
|
||||
"""读触觉配置。找不到或写错时抛 TactileConfigError，消息里给出修法。"""
|
||||
if path is None:
|
||||
base = root if root is not None else Path.cwd()
|
||||
path = Path(base) / DEFAULT_TACTILE_REL
|
||||
path = Path(path)
|
||||
|
||||
if not path.is_file():
|
||||
raise TactileConfigError(
|
||||
f"触觉配置不存在: {path}\n"
|
||||
f" 它是仿真相机之外另一份「物料配置」：话题名、轴向映射、force_scale、\n"
|
||||
f" 基线口径全在这里（桥接与适配器都从它派生）。缺了它仿真里的触觉\n"
|
||||
f" 完全没有输出。确认仓库根下存在 {DEFAULT_TACTILE_REL}。"
|
||||
)
|
||||
|
||||
try:
|
||||
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except yaml.YAMLError as exc:
|
||||
raise TactileConfigError(f"触觉配置 YAML 解析失败: {path}\n 原因: {exc}") from exc
|
||||
|
||||
raw_sides = doc.get("sides")
|
||||
if not isinstance(raw_sides, list) or not raw_sides:
|
||||
raise TactileConfigError(
|
||||
f"{path} 里没有 sides 列表（或为空）。双指方案要写 left 与 right 两项 ——\n"
|
||||
f" 单指无法复现「旋拧时切向力从哪一侧来」这个判据。"
|
||||
)
|
||||
|
||||
sides: List[TactileSide] = []
|
||||
seen: Dict[str, str] = {}
|
||||
for i, item in enumerate(raw_sides):
|
||||
where = f"{path} 的 sides[{i}]"
|
||||
if not isinstance(item, dict):
|
||||
raise TactileConfigError(f"{where} 不是映射（key: value 形式）。")
|
||||
missing = [
|
||||
k for k in ("side", "wrench_topic", "contact_topic", "frame")
|
||||
if not str(item.get(k, "")).strip()
|
||||
]
|
||||
if missing:
|
||||
raise TactileConfigError(
|
||||
f"{where} 缺字段: {', '.join(missing)}。\n"
|
||||
f" wrench_topic/contact_topic 必须与 arm.sim.sensors.xacro 里写的\n"
|
||||
f" <topic> 完全一致 —— 对不上时适配器只是订阅了一个不存在的话题，\n"
|
||||
f" 不报错、status 永远是零。"
|
||||
)
|
||||
side = str(item["side"]).strip()
|
||||
if side in seen:
|
||||
raise TactileConfigError(f"{where} 的 side 重复: {side}")
|
||||
seen[side] = where
|
||||
sides.append(
|
||||
TactileSide(
|
||||
side=side,
|
||||
wrench_topic=str(item["wrench_topic"]).strip(),
|
||||
contact_topic=str(item["contact_topic"]).strip(),
|
||||
frame=str(item["frame"]).strip(),
|
||||
)
|
||||
)
|
||||
|
||||
raw_axis = doc.get("axis") or {}
|
||||
raw_sign = doc.get("sign") or {}
|
||||
if not isinstance(raw_axis, dict) or not isinstance(raw_sign, dict):
|
||||
raise TactileConfigError(f"{path} 的 axis / sign 必须是映射。")
|
||||
|
||||
axis = AxisSpec(
|
||||
normal=_axis(raw_axis.get("normal", "y"), path, "axis.normal"),
|
||||
shear_x=_axis(raw_axis.get("shear_x", "z"), path, "axis.shear_x"),
|
||||
shear_y=_axis(raw_axis.get("shear_y", "x"), path, "axis.shear_y"),
|
||||
sign_normal=_num(raw_sign.get("normal", 1.0), path, "sign.normal"),
|
||||
sign_shear_x=_num(raw_sign.get("shear_x", 1.0), path, "sign.shear_x"),
|
||||
sign_shear_y=_num(raw_sign.get("shear_y", 1.0), path, "sign.shear_y"),
|
||||
)
|
||||
if len({axis.normal, axis.shear_x, axis.shear_y}) != 3:
|
||||
raise TactileConfigError(
|
||||
f"{path} 的 axis 里三根轴必须互不相同（当前 normal={axis.normal}, "
|
||||
f"shear_x={axis.shear_x}, shear_y={axis.shear_y}）。\n"
|
||||
f" 重复意味着有两个分量取自同一根轴，另有一根轴被丢掉 —— "
|
||||
f"力的语义会静默错位。"
|
||||
)
|
||||
|
||||
spec = TactileSpec(
|
||||
sides=tuple(sides),
|
||||
axis=axis,
|
||||
publish_hz=_num(doc.get("publish_hz", 20.0), path, "publish_hz", positive=True),
|
||||
baseline_seconds=_num(doc.get("baseline_seconds", 2.0), path, "baseline_seconds"),
|
||||
rebaseline_when_idle=bool(doc.get("rebaseline_when_idle", True)),
|
||||
rebaseline_tau_s=_num(doc.get("rebaseline_tau_s", 3.0), path, "rebaseline_tau_s"),
|
||||
contact_timeout_s=_num(
|
||||
doc.get("contact_timeout_s", 0.25), path, "contact_timeout_s", positive=True
|
||||
),
|
||||
min_contact_points=int(doc.get("min_contact_points", 1)),
|
||||
force_scale=_num(doc.get("force_scale", 1.0), path, "force_scale"),
|
||||
image_hz=_num(doc.get("image_hz", 0.0), path, "image_hz"),
|
||||
image_width=int(doc.get("image_width", 64)),
|
||||
image_height=int(doc.get("image_height", 64)),
|
||||
unit=str(doc.get("unit", "N")),
|
||||
)
|
||||
return spec
|
||||
|
||||
|
||||
def _axis(value: object, path: Path, key: str) -> str:
|
||||
axis = str(value).strip().lower()
|
||||
if axis not in _VALID_AXES:
|
||||
raise TactileConfigError(
|
||||
f"{path} 的 {key} 必须是 x/y/z 之一，当前是 {value!r}。"
|
||||
)
|
||||
return axis
|
||||
|
||||
|
||||
def _num(value: object, path: Path, key: str, positive: bool = False) -> float:
|
||||
try:
|
||||
out = float(value)
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise TactileConfigError(f"{path} 的 {key} 不是数字: {value!r}") from exc
|
||||
if positive and out <= 0.0:
|
||||
raise TactileConfigError(f"{path} 的 {key} 必须为正，当前是 {out}")
|
||||
return out
|
||||
|
||||
|
||||
def bridge_args(spec: TactileSpec) -> List[str]:
|
||||
"""生成 ros_gz_bridge parameter_bridge 的话题规则。
|
||||
|
||||
只做**传输**：gz 消息类型 → ROS 消息类型，话题名保持 gz 侧原样。
|
||||
改名与语义（轴向、基线、单位）全在适配器里做 —— 桥接器改不了 frame_id，
|
||||
也做不了基线相减，越权只会把"在哪一层改"这件事弄乱。
|
||||
"""
|
||||
args: List[str] = []
|
||||
for s in spec.sides:
|
||||
args.append(
|
||||
f"{s.wrench_topic}@geometry_msgs/msg/Wrench[{s.gz_wrench_msg}"
|
||||
)
|
||||
args.append(
|
||||
f"{s.contact_topic}@ros_gz_interfaces/msg/Contacts[{s.gz_contact_msg}"
|
||||
)
|
||||
return args
|
||||
|
||||
|
||||
class BaselineTracker:
|
||||
"""按"相对静止基线取增量"的口径跟踪零力读数。
|
||||
|
||||
两段式：
|
||||
预热期（前 baseline_seconds 秒）：等权均值。用均值而不是 EMA 是因为
|
||||
一开始没有任何先验，EMA 从 0 爬升需要很久。
|
||||
稳态期：仅在**确认无接触**时用时间常数 tau 做慢跟踪。
|
||||
为什么需要慢跟踪而不是"一次性基线"：force_torque 读的是关节传递力，
|
||||
静止时它等于远端那段手指的自重，而**这个自重项随机械臂姿态变化** ——
|
||||
一次性基线会在换姿态后留下残余。只在无接触时更新，是为了不把真实
|
||||
接触当成新基线吃掉（那会让接触力永远回到零，症状是"夹住了但力为 0"）。
|
||||
"""
|
||||
|
||||
def __init__(self, baseline_seconds: float, tau_s: float, enabled: bool = True) -> None:
|
||||
self._warmup = max(0.0, float(baseline_seconds))
|
||||
self._tau = max(0.0, float(tau_s))
|
||||
self._enabled = bool(enabled)
|
||||
self._sum: Dict[str, float] = {}
|
||||
self._count: Dict[str, int] = {}
|
||||
self._value: Dict[str, float] = {}
|
||||
self._elapsed = 0.0
|
||||
|
||||
@property
|
||||
def warmed_up(self) -> bool:
|
||||
return self._elapsed >= self._warmup
|
||||
|
||||
def add(self, key: str, raw: float, dt: float, in_contact: bool) -> None:
|
||||
"""喂一个原始读数。``dt`` 是距上次的秒数。"""
|
||||
if in_contact:
|
||||
# 接触期间既不预热也不跟踪：接触力绝不能进基线。
|
||||
return
|
||||
if not self.warmed_up:
|
||||
self._sum[key] = self._sum.get(key, 0.0) + float(raw)
|
||||
self._count[key] = self._count.get(key, 0) + 1
|
||||
self._value[key] = self._sum[key] / max(1, self._count[key])
|
||||
return
|
||||
if not self._enabled or self._tau <= 0.0 or key not in self._value:
|
||||
return
|
||||
alpha = min(1.0, max(0.0, dt) / self._tau)
|
||||
self._value[key] += (float(raw) - self._value[key]) * alpha
|
||||
|
||||
def advance(self, dt: float) -> None:
|
||||
"""推进预热计时。必须在每轮循环调用一次（即使这一轮没有读数）。"""
|
||||
self._elapsed += max(0.0, float(dt))
|
||||
|
||||
def value(self, key: str) -> float:
|
||||
return self._value.get(key, 0.0)
|
||||
|
||||
def delta(self, key: str, raw: float) -> float:
|
||||
"""原始读数 → 相对基线的增量。"""
|
||||
return float(raw) - self.value(key)
|
||||
@@ -0,0 +1,361 @@
|
||||
"""仿真触觉适配器：Gazebo 的关节力/接触传感器 → arm_msgs/TactileStatus。
|
||||
|
||||
=============================================================================
|
||||
它在链路上的位置
|
||||
=============================================================================
|
||||
gz /tactile/<side>/wrench (gz.msgs.Wrench, 持续 ~100Hz)
|
||||
gz /tactile/<side>/raw (gz.msgs.Contacts, 只在接触时)
|
||||
│
|
||||
├─ ros_gz_bridge parameter_bridge（只搬类型，不改语义）
|
||||
│
|
||||
└─ 本节点
|
||||
├ 轴向投影 三向力 → fnormal/fshearx/fsheary（映射实测，见 config）
|
||||
├ 基线相减 静止时关节要承担手指自重，不减就是一个恒定偏置
|
||||
├ 接触判据 超时判"已离开"（接触传感器是事件式发布）
|
||||
└ 变送 /tactile/<side>/status （arm_msgs/TactileStatus）
|
||||
|
||||
话题名、消息类型、字段口径全部与真机 tactile_node 对齐，所以下游
|
||||
（compliant_descent、stage3_search_node）**一行都不用改**就能吃仿真的数据，
|
||||
这正是"同一套接口、换后端"的契约。
|
||||
|
||||
=============================================================================
|
||||
三个必须知道的坑（都会静默）
|
||||
=============================================================================
|
||||
1) 接触传感器的消息是**事件式**的：不接触时一条都不发。
|
||||
若适配器只在收到消息时才发布 status，那么"离开工件"期间下游会认为
|
||||
话题断流（stage3_search_node 的 _tac_fresh 会直接 SearchAbort）。
|
||||
所以本节点**按 publish_hz 恒定发布**，接触状态由"最近是否收到过"决定。
|
||||
真机 Orisys 也是恒定发布的，这里必须与真机一致。
|
||||
|
||||
2) 力的话题来自 force_torque 传感器，而它依赖 world 里的
|
||||
forcetorque-system 插件被显式声明（只要 world 声明了任何 <plugin>，
|
||||
gz 就不再加载默认系统列表）。插件漏了的话**话题根本不出现**、
|
||||
日志一片空白。所以本节点在启动几秒后若一个 wrench 都没收到，
|
||||
会明确报出来并指出该查哪里。
|
||||
|
||||
3) 单位不是牛顿等价值就可能是错的：Gazebo 给的是牛顿，真机给的是形变
|
||||
任意单位。换算靠 config 里的 force_scale，见 tactile.yaml 文件头的
|
||||
「单位口径」。选错不报错，只表现为阈值永远不触发或一碰就炸。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Dict, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.time import Time
|
||||
|
||||
from arm_msgs.msg import TactileStatus
|
||||
from geometry_msgs.msg import Wrench
|
||||
from ros_gz_interfaces.msg import Contacts
|
||||
from sensor_msgs.msg import Image
|
||||
from tf2_ros import Buffer, TransformListener
|
||||
|
||||
from .sim_tactile import (
|
||||
DEFAULT_TACTILE_REL,
|
||||
BaselineTracker,
|
||||
TactileConfigError,
|
||||
TactileSide,
|
||||
TactileSpec,
|
||||
load_tactile,
|
||||
)
|
||||
|
||||
# 启动后多久检查"到底有没有收到力数据"。要给桥接器与 gz 一点时间。
|
||||
TOPIC_CHECK_DELAY_S = 8.0
|
||||
|
||||
|
||||
def _depth_placeholder(width: int, height: int) -> np.ndarray:
|
||||
"""占位深度图：32FC1、全 NaN。
|
||||
|
||||
为什么是 NaN 而不是全 0：全 0 在深度语义里是"距离 0 米"，会被下游当成
|
||||
一个贴脸的平面；NaN 是"无读数"，不会伪装成有效数据。
|
||||
"""
|
||||
return np.full((height, width), np.nan, dtype=np.float32)
|
||||
|
||||
|
||||
def _vector_placeholder(width: int, height: int) -> np.ndarray:
|
||||
"""占位力矢量图：32FC2（每像素 dx, dy），全 NaN。"""
|
||||
return np.full((height, width, 2), np.nan, dtype=np.float32)
|
||||
|
||||
|
||||
class SimTactileAdapter(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim_tactile_adapter")
|
||||
|
||||
self.declare_parameter("tactile_file", "")
|
||||
# ⚠️ 不要 declare_parameter("use_sim_time", ...)：launch 会把它作为
|
||||
# 参数文件传进来，rclpy 那时**已经自动声明过**，再声明一次会直接抛
|
||||
# ParameterAlreadyDeclaredException 让节点起不来（实测踩过）。
|
||||
# 相机适配器同样只声明自己的参数。
|
||||
|
||||
raw_path = str(self.get_parameter("tactile_file").value or "")
|
||||
path = Path(raw_path) if raw_path else Path.cwd() / DEFAULT_TACTILE_REL
|
||||
try:
|
||||
self.spec: TactileSpec = load_tactile(path)
|
||||
except TactileConfigError as exc:
|
||||
# 配置错误必须让节点直接失败：静默启动一个"永远零力"的适配器
|
||||
# 比不启动更糟 —— 它看起来在工作。
|
||||
self.get_logger().error(f"触觉配置有问题：\n{exc}")
|
||||
raise
|
||||
|
||||
self.get_logger().info(
|
||||
f"触觉：单位={self.spec.unit} force_scale={self.spec.force_scale} "
|
||||
f"发布率={self.spec.publish_hz}Hz 基线={self.spec.baseline_seconds}s"
|
||||
f" 轴向 normal={self.spec.axis.normal} "
|
||||
f"shear_x={self.spec.axis.shear_x} shear_y={self.spec.axis.shear_y}"
|
||||
)
|
||||
|
||||
self._tf_buffer = Buffer()
|
||||
self._tf_listener = TransformListener(self._tf_buffer, self)
|
||||
|
||||
# 每指的状态。键都是 side。
|
||||
self._wrench: Dict[str, Tuple[float, float, float]] = {}
|
||||
self._wrench_stamp: Dict[str, float] = {}
|
||||
self._contact_point: Dict[str, Optional[Tuple[float, float, float]]] = {}
|
||||
self._contact_stamp: Dict[str, float] = {}
|
||||
self._got_wrench: Dict[str, bool] = {}
|
||||
self._got_contact: Dict[str, bool] = {}
|
||||
|
||||
self._baseline = BaselineTracker(
|
||||
self.spec.baseline_seconds,
|
||||
self.spec.rebaseline_tau_s,
|
||||
enabled=self.spec.rebaseline_when_idle,
|
||||
)
|
||||
self._last_tick = time.monotonic()
|
||||
|
||||
self._pub_status = {}
|
||||
self._pub_image: Dict[str, Tuple[object, object, float, float]] = {}
|
||||
for s in self.spec.sides:
|
||||
self._got_wrench[s.side] = False
|
||||
self._got_contact[s.side] = False
|
||||
self._pub_status[s.side] = self.create_publisher(
|
||||
TactileStatus, s.status_topic, 10
|
||||
)
|
||||
self.create_subscription(
|
||||
Wrench,
|
||||
s.wrench_topic,
|
||||
self._make_wrench_cb(s.side),
|
||||
10,
|
||||
)
|
||||
self.create_subscription(
|
||||
Contacts,
|
||||
s.contact_topic,
|
||||
self._make_contact_cb(s.side),
|
||||
10,
|
||||
)
|
||||
if self.spec.image_hz > 0.0:
|
||||
self._pub_image[s.side] = (
|
||||
self.create_publisher(Image, s.image_topic, 3),
|
||||
self.create_publisher(Image, s.vector_topic, 3),
|
||||
self.spec.image_hz,
|
||||
0.0,
|
||||
)
|
||||
self.get_logger().warn(
|
||||
f"仿真里没有弹性体成像：{s.image_topic} 与 {s.vector_topic} "
|
||||
f"发布的是**占位图**（32FC1 / 32FC2 全 NaN），话题与消息结构"
|
||||
f"与真机一致，但内容没有物理含义。需要真实图像时请关掉 "
|
||||
f"image_hz（设为 0）。"
|
||||
)
|
||||
|
||||
period = 1.0 / max(1e-3, self.spec.publish_hz)
|
||||
self.create_timer(period, self._tick)
|
||||
self._check_at = time.monotonic() + TOPIC_CHECK_DELAY_S
|
||||
self._checked = False
|
||||
|
||||
# --- 订阅回调 ---------------------------------------------------------
|
||||
def _make_wrench_cb(self, side: str):
|
||||
def cb(msg: Wrench) -> None:
|
||||
self._got_wrench[side] = True
|
||||
self._wrench[side] = (
|
||||
float(msg.force.x),
|
||||
float(msg.force.y),
|
||||
float(msg.force.z),
|
||||
)
|
||||
self._wrench_stamp[side] = time.monotonic()
|
||||
|
||||
return cb
|
||||
|
||||
def _make_contact_cb(self, side: str):
|
||||
def cb(msg: Contacts) -> None:
|
||||
pts = []
|
||||
for c in msg.contacts:
|
||||
for p in c.positions:
|
||||
pts.append((float(p.x), float(p.y), float(p.z)))
|
||||
if len(pts) < max(1, self.spec.min_contact_points):
|
||||
return
|
||||
n = float(len(pts))
|
||||
self._got_contact[side] = True
|
||||
self._contact_point[side] = (
|
||||
sum(p[0] for p in pts) / n,
|
||||
sum(p[1] for p in pts) / n,
|
||||
sum(p[2] for p in pts) / n,
|
||||
)
|
||||
self._contact_stamp[side] = time.monotonic()
|
||||
|
||||
return cb
|
||||
|
||||
# --- 主循环 -----------------------------------------------------------
|
||||
def _tick(self) -> None:
|
||||
now = time.monotonic()
|
||||
dt = now - self._last_tick
|
||||
self._last_tick = now
|
||||
self._baseline.advance(dt)
|
||||
|
||||
for s in self.spec.sides:
|
||||
raw = self._wrench.get(s.side)
|
||||
in_contact = self._in_contact(s, now)
|
||||
|
||||
if raw is None:
|
||||
# 还没收到过力数据。仍然发布（下游靠"有没有帧"判活），
|
||||
# 但发的是零 + connected=False，让下游能区分"没数据"与"零力"。
|
||||
self._publish(s.side, connected=False, contact=False,
|
||||
forces=(0.0, 0.0, 0.0), stamp=now)
|
||||
continue
|
||||
|
||||
for i, axis in enumerate(("x", "y", "z")):
|
||||
self._baseline.add(f"{s.side}_{axis}", raw[i], dt, in_contact)
|
||||
|
||||
delta = tuple(
|
||||
self._baseline.delta(f"{s.side}_{axis}", raw[i]) for i, axis in enumerate(("x", "y", "z"))
|
||||
)
|
||||
fn, fsx, fsy = self.spec.axis.project(delta, self.spec.force_scale)
|
||||
# 真机 fnormal 也是非负的（法向只能压不能拉），这里保持一致。
|
||||
fn = max(0.0, fn)
|
||||
self._publish(s.side, connected=True, contact=in_contact,
|
||||
forces=(fn, fsx, fsy), stamp=now, side_spec=s)
|
||||
|
||||
self._maybe_check_topics(now)
|
||||
|
||||
def _in_contact(self, s: TactileSide, now: float) -> bool:
|
||||
"""接触 = 最近 contact_timeout_s 内收到过接触消息。
|
||||
|
||||
⚠️ 不能写成"收到消息就一直算接触"：接触传感器是事件式的，
|
||||
离开工件后不会发"取消接触"的消息，那样 contact 会永远停在 True。
|
||||
"""
|
||||
stamp = self._contact_stamp.get(s.side)
|
||||
return stamp is not None and (now - stamp) <= self.spec.contact_timeout_s
|
||||
|
||||
def _centroid(self, s: TactileSide) -> Tuple[float, float]:
|
||||
"""接触点在指尖坐标系里沿两个切向轴的偏移（毫米）→ (u, v)。
|
||||
|
||||
真机的 centroid_u/v 是形变图的像素坐标；仿真没有成像，用"接触点在指腹
|
||||
平面上的位置"表达同一件事 —— 同样是"接触发生在指腹的哪里"。
|
||||
下游目前不消费这两个字段，所以量纲不同不会造成影响；
|
||||
真要跨仿真/真机共用，这里是最需要先统一的一处。
|
||||
"""
|
||||
pt = self._contact_point.get(s.side)
|
||||
if pt is None:
|
||||
return (0.0, 0.0)
|
||||
try:
|
||||
tr = self._tf_buffer.lookup_transform(s.frame, "world", Time())
|
||||
except Exception:
|
||||
return (0.0, 0.0)
|
||||
t = tr.transform.translation
|
||||
q = tr.transform.rotation
|
||||
# world → frame：位置平移 + 用共轭四元数旋转
|
||||
px, py, pz = pt[0] - t.x, pt[1] - t.y, pt[2] - t.z
|
||||
# 逆旋转：q* v q
|
||||
qx, qy, qz, qw = -q.x, -q.y, -q.z, q.w
|
||||
ix = qw * px + qy * pz - qz * py
|
||||
iy = qw * py + qz * px - qx * pz
|
||||
iz = qw * pz + qx * py - qy * px
|
||||
iw = -qx * px - qy * py - qz * pz
|
||||
vx = ix * qw + iw * -qx + iy * -qz - iz * -qy
|
||||
vy = iy * qw + iw * -qy + iz * -qx - ix * -qz
|
||||
vz = iz * qw + iw * -qz + ix * -qy - iy * -qx
|
||||
i_sx, i_sy = self.spec.axis.shear_indices()
|
||||
comp = (vx, vy, vz)
|
||||
return (comp[i_sx] * 1000.0, comp[i_sy] * 1000.0)
|
||||
|
||||
def _publish(self, side: str, connected: bool, contact: bool,
|
||||
forces: Tuple[float, float, float], stamp: float,
|
||||
side_spec: Optional[TactileSide] = None) -> None:
|
||||
msg = TactileStatus()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = f"{side}_tactile"
|
||||
msg.side = side
|
||||
msg.connected = connected
|
||||
msg.contact = contact
|
||||
msg.fnormal = float(forces[0])
|
||||
msg.fshearx = float(forces[1])
|
||||
msg.fsheary = float(forces[2])
|
||||
# 仿真没有光学帧率。填 0 而不是编一个 30 —— 假数据比缺数据更难查。
|
||||
msg.fps = 0.0
|
||||
if contact and side_spec is not None:
|
||||
msg.centroid_u, msg.centroid_v = (float(v) for v in self._centroid(side_spec))
|
||||
else:
|
||||
msg.centroid_u = 0.0
|
||||
msg.centroid_v = 0.0
|
||||
msg.error_msg = ""
|
||||
self._pub_status[side].publish(msg)
|
||||
|
||||
pub = self._pub_image.get(side)
|
||||
if pub is not None:
|
||||
p_img, p_vec, hz, last = pub
|
||||
now = time.monotonic()
|
||||
if now - last >= 1.0 / max(1e-3, hz):
|
||||
self._pub_image[side] = (p_img, p_vec, hz, now)
|
||||
self._publish_placeholders(side, p_img, p_vec)
|
||||
|
||||
def _publish_placeholders(self, side: str, p_img, p_vec) -> None:
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
w, h = self.spec.image_width, self.spec.image_height
|
||||
|
||||
depth = _depth_placeholder(w, h)
|
||||
img = Image()
|
||||
img.header.stamp = stamp
|
||||
img.header.frame_id = f"{side}_tactile"
|
||||
img.height, img.width = h, w
|
||||
img.encoding = "32FC1"
|
||||
img.is_bigendian = False
|
||||
img.step = w * 4
|
||||
img.data = depth.tobytes()
|
||||
p_img.publish(img)
|
||||
|
||||
vec = _vector_placeholder(w, h)
|
||||
vimg = Image()
|
||||
vimg.header.stamp = stamp
|
||||
vimg.header.frame_id = f"{side}_tactile"
|
||||
vimg.height, vimg.width = h, w
|
||||
vimg.encoding = "32FC2"
|
||||
vimg.is_bigendian = False
|
||||
vimg.step = w * 8
|
||||
vimg.data = vec.tobytes()
|
||||
p_vec.publish(vimg)
|
||||
|
||||
def _maybe_check_topics(self, now: float) -> None:
|
||||
"""启动几秒后确认力话题真的有数据。
|
||||
|
||||
这一步存在的唯一理由：漏声明 forcetorque-system 插件时，gz 不报错、
|
||||
话题不出现、本节点也照常发零力 —— 整条链路"看起来在工作"。
|
||||
主动检查并指出可能原因是唯一能打破这种静默的地方。
|
||||
"""
|
||||
if self._checked or now < self._check_at:
|
||||
return
|
||||
self._checked = True
|
||||
missing = [s.side for s in self.spec.sides if not self._got_wrench[s.side]]
|
||||
if not missing:
|
||||
return
|
||||
self.get_logger().error(
|
||||
f"启动 {TOPIC_CHECK_DELAY_S:.0f}s 后仍未收到力数据（{'/'.join(missing)}）。\n"
|
||||
f" 最可能的原因：world 文件里没有声明 forcetorque-system 插件。\n"
|
||||
f" 只要 world 声明了任何 <plugin>，gz 就不再加载默认系统列表，于是\n"
|
||||
f" force_torque 传感器**静默失效**：话题不出现、日志空白。\n"
|
||||
f" 检查 devices/sim/gazebo/worlds/assembly.world 里是否有\n"
|
||||
f" libignition-gazebo6-forcetorque-system.so 那一项。"
|
||||
)
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = SimTactileAdapter()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
@@ -76,6 +76,7 @@ def _prepare(context, *args, **kwargs):
|
||||
default_controllers,
|
||||
default_world,
|
||||
default_xacro,
|
||||
check_tactile_collisions,
|
||||
model_link_pose,
|
||||
prepare_urdf,
|
||||
repo_root,
|
||||
@@ -300,6 +301,55 @@ def _prepare(context, *args, **kwargs):
|
||||
)
|
||||
)
|
||||
|
||||
# 4d) 触觉：桥接 + 适配器 -------------------------------------------
|
||||
# 链路与相机同构：gz 只负责"有数据"，语义（轴向、基线、单位、接触超时）
|
||||
# 全在适配器里做。见 sim_tactile.py / sim_tactile_adapter.py 顶部。
|
||||
from arm_sim_gazebo.sim_tactile import (
|
||||
DEFAULT_TACTILE_REL,
|
||||
TactileConfigError,
|
||||
)
|
||||
from arm_sim_gazebo.sim_tactile import bridge_args as tactile_bridge_args
|
||||
from arm_sim_gazebo.sim_tactile import load_tactile
|
||||
|
||||
try:
|
||||
tac = load_tactile(root=root)
|
||||
except TactileConfigError as exc:
|
||||
raise RuntimeError(f"\n[gazebo_sim] 仿真触觉配置有问题：\n{exc}\n") from exc
|
||||
|
||||
# 描述级静态检查：接触传感器引用的 collision 名是 sdformat **自动生成**的，
|
||||
# 换个 sdformat 版本就可能变，而变了之后传感器只是"永远没输出"、不报错。
|
||||
# 这里把它和"传感器挂错关节""话题写在错误的层级""传感器被整个丢掉"
|
||||
# 一起在启动时报出来。expected_topics 用配置里声明的话题做对照 ——
|
||||
# 这是唯一能发现"传感器被静默丢弃"的办法。
|
||||
for _w in check_tactile_collisions(
|
||||
urdf,
|
||||
expected_topics=[s.wrench_topic for s in tac.sides]
|
||||
+ [s.contact_topic for s in tac.sides],
|
||||
):
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 触觉描述检查: {_w}"))
|
||||
|
||||
actions.append(
|
||||
Node(
|
||||
package="ros_gz_bridge",
|
||||
executable="parameter_bridge",
|
||||
name="tactile_bridge",
|
||||
output="screen",
|
||||
arguments=tactile_bridge_args(tac),
|
||||
)
|
||||
)
|
||||
actions.append(
|
||||
Node(
|
||||
package="arm_sim_gazebo",
|
||||
executable="sim_tactile_adapter",
|
||||
name="sim_tactile_adapter",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{"tactile_file": str(Path(root) / DEFAULT_TACTILE_REL)},
|
||||
{"use_sim_time": True},
|
||||
],
|
||||
)
|
||||
)
|
||||
|
||||
# --- 5) spawn 机器人 -----------------------------------------------------
|
||||
# -world 必须给对：world 名写错时 Gazebo **不报错**，只是实体没被创建，
|
||||
# 表现成"launch 一切正常但 RViz 里什么都没有"。
|
||||
@@ -357,6 +407,7 @@ def _prepare(context, *args, **kwargs):
|
||||
" ros2 control list_controllers # 两个控制器 active\n"
|
||||
" ros2 topic echo /joint_states --once # 7 个 sim_joint\n"
|
||||
" ros2 run tf2_ros tf2_echo world sim_tcp # 末端位姿可查\n"
|
||||
" ros2 topic echo /tactile/left/status --once # 触觉三力在发\n"
|
||||
" python3 scripts/check_sim_cameras.py # ★ 相机的真正验收\n"
|
||||
" 机械臂应在重力下**保持站住**。若它瘫下去或消失，说明描述有问题，"
|
||||
"参考 arm_sim_gazebo/urdf_normalize.py 顶部列的两个上游缺陷。\n"
|
||||
|
||||
@@ -30,6 +30,11 @@
|
||||
<!-- Gazebo 侧（仅仿真环境有） -->
|
||||
<exec_depend>ros_gz_sim</exec_depend>
|
||||
<exec_depend>ros_gz_bridge</exec_depend>
|
||||
<!-- 触觉适配器订阅 ros_gz_interfaces/msg/Contacts（接触传感器的原始消息），
|
||||
发布 arm_msgs/msg/TactileStatus。两者都是运行期硬依赖。 -->
|
||||
<exec_depend>ros_gz_interfaces</exec_depend>
|
||||
<exec_depend>arm_msgs</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
<exec_depend>gz_ros2_control</exec_depend>
|
||||
<exec_depend>controller_manager</exec_depend>
|
||||
<exec_depend>joint_state_broadcaster</exec_depend>
|
||||
|
||||
@@ -42,6 +42,10 @@ setup(
|
||||
# 话题（改名 + 光学系 frame_id + 深度 米→毫米）。
|
||||
# 由 gazebo_sim.launch.py 拉起，一般不手跑；单独调试时可以用。
|
||||
'sim_camera_adapter = arm_sim_gazebo.sim_camera_adapter:main',
|
||||
# 仿真触觉适配器：把桥接过来的 gz 关节力/接触传感器变成
|
||||
# arm_msgs/TactileStatus（轴向投影 + 基线相减 + 恒定发布）。
|
||||
# 由 gazebo_sim.launch.py 拉起；单独调试时可以用。
|
||||
'sim_tactile_adapter = arm_sim_gazebo.sim_tactile_adapter:main',
|
||||
# 独立小工具：只看归一化做了什么，不启动 Gazebo。
|
||||
# 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。
|
||||
'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',
|
||||
|
||||
@@ -19,6 +19,7 @@ sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
from arm_sim_gazebo.robot_description import ( # noqa: E402
|
||||
SimDescriptionError,
|
||||
check_base_plugins,
|
||||
check_tactile_collisions,
|
||||
default_world,
|
||||
expand_xacro,
|
||||
model_link_pose,
|
||||
@@ -478,3 +479,123 @@ def test_real_assembly_world_d435_pose():
|
||||
# yaw 必须是 0：曾经写成 3.14159，把视线从"+X 俯视"翻成"-X 俯视"，
|
||||
# 相机于是背对台面、只看得见地面。这条断言就是防止它被改回去。
|
||||
assert rpy == pytest.approx((0.0, 0.872665, 0.0), abs=1e-6)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 触觉传感器描述检查（check_tactile_collisions）
|
||||
#
|
||||
# 这四条测试锁的是**三个真的踩过、且都是静默失效**的坑：
|
||||
# 1. <contact><collision> 写的是 sdformat 自动生成的名字，命名规则一变就
|
||||
# 永远收不到数据，而 Gazebo 不报任何错；
|
||||
# 2. 接触传感器的话题必须写在 <contact> 里，写在外层 <sensor> 会被忽略，
|
||||
# gz 改用默认的 scoped 话题名，"按 /tactile/left/raw 订阅"永远收不到；
|
||||
# 3. 整块传感器没进 SDF（写错位置 / 被丢掉）时，唯一的外部症状就是
|
||||
# "话题不出现"，所以必须拿配置里声明的话题去对照。
|
||||
#
|
||||
# 前两条用"故意改坏再检查"的负向测试来证明检查真的会报 ——
|
||||
# 只测"正常情况下没问题"是不够的，那种测试在检查退化成 return [] 后照样通过。
|
||||
# ---------------------------------------------------------------------------
|
||||
TACTILE_TOPICS = [
|
||||
"/tactile/left/wrench",
|
||||
"/tactile/left/raw",
|
||||
"/tactile/right/wrench",
|
||||
"/tactile/right/raw",
|
||||
]
|
||||
|
||||
|
||||
def _strip_tactile_expected():
|
||||
"""展开仓库里的描述，失败时 skip（与其它 needs_flexiv 测试同一口径）。"""
|
||||
try:
|
||||
urdf, _ = prepare_urdf()
|
||||
except SimDescriptionError as exc:
|
||||
pytest.skip(f"无法展开仿真描述：{exc}")
|
||||
return urdf
|
||||
|
||||
|
||||
@needs_flexiv
|
||||
def test_tactile_check_passes_on_repo_description():
|
||||
"""仓库自带的描述必须通过 —— 否则启动日志里会一直挂着没用的告警。"""
|
||||
problems = check_tactile_collisions(
|
||||
_strip_tactile_expected(), expected_topics=TACTILE_TOPICS
|
||||
)
|
||||
assert problems == [], "\n".join(problems)
|
||||
|
||||
|
||||
@needs_flexiv
|
||||
def test_tactile_check_catches_bad_collision_name():
|
||||
"""坑 1：collision 名与实现不符时必须报出来。
|
||||
|
||||
这是最像"灵异事件"的一种失效：话题存在、桥接正常、日志干净，
|
||||
就是永远没有数据。真出问题时人只会怀疑传感器或桥接器，
|
||||
不会想到是 sdformat 的自动命名变了。
|
||||
"""
|
||||
from arm_sim_gazebo.robot_description import check_tactile_collisions
|
||||
|
||||
urdf = _strip_tactile_expected().replace(
|
||||
"sim_left_finger_mount_fixed_joint_lump__sim_left_finger_tip_collision",
|
||||
"a_name_that_does_not_exist",
|
||||
)
|
||||
problems = check_tactile_collisions(urdf, expected_topics=TACTILE_TOPICS)
|
||||
assert any("a_name_that_does_not_exist" in p for p in problems), problems
|
||||
|
||||
|
||||
@needs_flexiv
|
||||
def test_tactile_check_catches_contact_topic_in_wrong_place():
|
||||
"""坑 2：把话题从 <contact> 挪到 <sensor> 层，必须被报出来。
|
||||
|
||||
这条是"看起来更合理"的写法（force_torque 就是写在外层 <topic> 的），
|
||||
所以极易在重构时被顺手改过去。改完话题从 /tactile/left/raw 变成
|
||||
gz 的 scoped 名字，消费者永远等不到消息。
|
||||
"""
|
||||
import re
|
||||
|
||||
from arm_sim_gazebo.robot_description import check_tactile_collisions
|
||||
|
||||
urdf = _strip_tactile_expected()
|
||||
# 精准摘掉 <contact> 里的那个 <topic>（不是注释里提到的那处）。
|
||||
# ⚠️ 不能用「带换行」的正则：xacro 会把 <!-- --> 压掉换行，真实的
|
||||
# <topic> 紧跟在前一个注释的 --> 后面，行尾是 </contact> 而不是 \n。
|
||||
# 第一版就是这么写的，结果正则没匹配上、断言却照样通过（见下面的
|
||||
# sanity check）—— 测试假绿比没测试更危险。
|
||||
surgically_removed = re.sub(
|
||||
r"(<contact>.*?)<topic>[^<]*</topic>", r"\1", urdf, count=1, flags=re.S
|
||||
)
|
||||
assert surgically_removed != urdf, "字符串手术没生效"
|
||||
# sanity：元素形式的 <topic> 已消失（注释里那处文本不算）
|
||||
assert not re.search(r"<topic>/tactile/left/raw</topic>", surgically_removed)
|
||||
|
||||
problems = check_tactile_collisions(
|
||||
surgically_removed, expected_topics=TACTILE_TOPICS
|
||||
)
|
||||
assert any("tactile_left" in p for p in problems), problems
|
||||
# 话题缺失也必须被同时指出：这是"话题不出现"这个症状的直接线索
|
||||
assert any("/tactile/left/raw" in p for p in problems), problems
|
||||
|
||||
|
||||
@needs_flexiv
|
||||
def test_tactile_check_catches_whole_sensor_dropped():
|
||||
"""坑 3：整块接触传感器被丢掉时，只能靠"配置里有、SDF 里没有"抓出来。"""
|
||||
import re
|
||||
|
||||
from arm_sim_gazebo.robot_description import check_tactile_collisions
|
||||
|
||||
urdf = _strip_tactile_expected()
|
||||
block = re.search(
|
||||
r'<sensor name="tactile_left" type="contact">.*?</sensor>', urdf, re.S
|
||||
)
|
||||
assert block, "没找到 tactile_left contact 传感器，测试需要跟着描述更新"
|
||||
problems = check_tactile_collisions(
|
||||
urdf.replace(block.group(0), ""), expected_topics=TACTILE_TOPICS
|
||||
)
|
||||
assert any("/tactile/left/raw" in p for p in problems), problems
|
||||
|
||||
|
||||
def test_tactile_check_reports_bad_urdf_instead_of_raising():
|
||||
"""垃圾输入要变成一条可读的问题，不能抛异常。
|
||||
|
||||
它跑在 launch 的启动路径上：抛异常 = 整个 launch 直接崩，
|
||||
于是"触觉有点小毛病"升级成"什么都起不来"。宁可少报，不可崩。
|
||||
"""
|
||||
problems = check_tactile_collisions("<robot><joint", expected_topics=["/x"])
|
||||
assert len(problems) == 1
|
||||
assert "SDF" in problems[0] or "URDF" in problems[0]
|
||||
|
||||
@@ -0,0 +1,341 @@
|
||||
"""sim_tactile 的单测。
|
||||
|
||||
这些测试锁的是三类**静默**错误 —— 它们的共同点是"力有数值变化"看起来完全
|
||||
正常，只有对着具体数字才能发现，而现场没有"正确值"可比：
|
||||
|
||||
1. 轴向映射错（把切向当法向）—— 数值照样在动，只是语义整体错位；
|
||||
2. 基线把接触吃掉 —— 症状是"明明夹住了，fnormal 却是 0"；
|
||||
3. 配置里的话题名与 xacro 漂移 —— 适配器订阅一个不存在的话题，
|
||||
不报错，status 里永远是零。
|
||||
|
||||
前两类在纯数据层就能钉死，所以放在这里；第三类是跨文件一致性，
|
||||
用"真读仓库里的两份文件"的方式测（与 test_sim_cameras 的 FOV 对照同一思路）。
|
||||
|
||||
最后一类（真读仓库文件）在环境不具备时 skip，其余任何时候都能跑。
|
||||
"""
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
|
||||
from arm_sim_gazebo.sim_tactile import ( # noqa: E402
|
||||
AxisSpec,
|
||||
BaselineTracker,
|
||||
TactileConfigError,
|
||||
bridge_args,
|
||||
load_tactile,
|
||||
)
|
||||
|
||||
|
||||
def _write(tmp_path: Path, body: str) -> Path:
|
||||
p = tmp_path / "tactile.yaml"
|
||||
p.write_text(body, encoding="utf-8")
|
||||
return p
|
||||
|
||||
|
||||
MINIMAL = """
|
||||
publish_hz: 20.0
|
||||
sides:
|
||||
- side: left
|
||||
wrench_topic: /tactile/left/wrench
|
||||
contact_topic: /tactile/left/raw
|
||||
frame: sim_left_finger_mount
|
||||
"""
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 轴向投影 —— 第 1 类静默错误
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_project_maps_axes_to_fnormal_and_shears():
|
||||
"""实测口径：法向=y、shear_x=z、shear_y=x（见 config/tactile.yaml 注释）。
|
||||
|
||||
这是全套触觉里最"不能靠眼睛验"的一处：映射错了以后力照样是"有数值、
|
||||
会响应接触、松开归零"，只有把三根轴的读数拆开看才发现法向拿的是切向。
|
||||
"""
|
||||
spec = AxisSpec(normal="y", shear_x="z", shear_y="x")
|
||||
fnormal, fshearx, fsheary = spec.project((1.0, 2.0, 3.0))
|
||||
assert (fnormal, fshearx, fsheary) == (2.0, 3.0, 1.0)
|
||||
|
||||
|
||||
def test_project_applies_sign_and_scale():
|
||||
"""符号与 force_scale 都要作用在输出上。
|
||||
|
||||
单测里之所以把 sign 显式测出来：真机两指实测**同号**（都是压下去为正），
|
||||
如果哪天有人按"左右镜像"的直觉把一边改成 -1，这条会立刻报警。
|
||||
"""
|
||||
spec = AxisSpec(
|
||||
normal="y",
|
||||
shear_x="z",
|
||||
shear_y="x",
|
||||
sign_normal=1.0,
|
||||
sign_shear_x=-1.0,
|
||||
sign_shear_y=-1.0,
|
||||
)
|
||||
out = spec.project((1.0, 2.0, 3.0), scale=0.5)
|
||||
assert out == (1.0, -1.5, -0.5)
|
||||
|
||||
|
||||
def test_project_requires_exactly_three_components():
|
||||
"""给两个或四个分量必须报错，不能"尽量取前三个"。
|
||||
|
||||
静默补零会让某根轴永远是 0，而那看起来像"这个方向本来就没有力"。
|
||||
"""
|
||||
spec = AxisSpec()
|
||||
with pytest.raises(ValueError):
|
||||
spec.project((1.0, 2.0))
|
||||
with pytest.raises(ValueError):
|
||||
spec.project((1.0, 2.0, 3.0, 4.0))
|
||||
|
||||
|
||||
def test_shear_indices_match_project():
|
||||
"""centroid 用的下标必须和 project 用的是同一套映射，否则接触点会错位。"""
|
||||
spec = AxisSpec(normal="y", shear_x="z", shear_y="x")
|
||||
raw = (10.0, 20.0, 30.0)
|
||||
_, fshearx, fsheary = spec.project(raw)
|
||||
ix, iy = spec.shear_indices()
|
||||
assert (raw[ix], raw[iy]) == (fshearx, fsheary)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 配置解析
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_load_minimal_config(tmp_path):
|
||||
spec = load_tactile(_write(tmp_path, MINIMAL))
|
||||
assert [s.side for s in spec.sides] == ["left"]
|
||||
assert spec.sides[0].status_topic == "/tactile/left/status"
|
||||
assert spec.sides[0].image_topic == "/tactile/left/image"
|
||||
# 默认值：发布率/基线对齐真机 site.yaml，图像默认关闭
|
||||
assert spec.publish_hz == 20.0
|
||||
assert spec.baseline_seconds == 2.0
|
||||
assert spec.force_scale == 1.0
|
||||
assert spec.image_hz == 0.0
|
||||
|
||||
|
||||
def test_load_missing_file_error_is_actionable(tmp_path):
|
||||
"""缺文件时必须说出它是谁、放哪、缺了会怎样。
|
||||
|
||||
实际报错现场是 launch 日志，用户不该还需要去翻代码才知道找什么文件。
|
||||
"""
|
||||
with pytest.raises(TactileConfigError) as ei:
|
||||
load_tactile(tmp_path / "nope.yaml")
|
||||
msg = str(ei.value)
|
||||
assert "tactile.yaml" in msg
|
||||
assert "devices/sim/gazebo/config/tactile.yaml" in msg
|
||||
|
||||
|
||||
def test_load_without_sides_is_rejected(tmp_path):
|
||||
with pytest.raises(TactileConfigError) as ei:
|
||||
load_tactile(_write(tmp_path, "publish_hz: 20.0\nsides: []\n"))
|
||||
assert "sides" in str(ei.value)
|
||||
|
||||
|
||||
def test_load_missing_side_field_is_rejected(tmp_path):
|
||||
"""少字段要报出字段名，并点明"必须与 xacro 里的 <topic> 一致"。"""
|
||||
body = MINIMAL.replace(" frame: sim_left_finger_mount\n", "")
|
||||
with pytest.raises(TactileConfigError) as ei:
|
||||
load_tactile(_write(tmp_path, body))
|
||||
msg = str(ei.value)
|
||||
assert "frame" in msg
|
||||
assert "xacro" in msg
|
||||
|
||||
|
||||
def test_load_duplicate_side_is_rejected(tmp_path):
|
||||
body = MINIMAL + """ - side: left
|
||||
wrench_topic: /tactile/left/wrench2
|
||||
contact_topic: /tactile/left/raw2
|
||||
frame: sim_left_finger_mount
|
||||
"""
|
||||
with pytest.raises(TactileConfigError) as ei:
|
||||
load_tactile(_write(tmp_path, body))
|
||||
assert "重复" in str(ei.value)
|
||||
|
||||
|
||||
def test_load_duplicate_axis_is_rejected(tmp_path):
|
||||
"""三根轴必须互不相同。
|
||||
|
||||
写重了不会报错、也不会崩：只是有两路力取自同一根轴、另有一根轴被丢掉，
|
||||
于是"切向拖拽"和"法向压力"变成同一个数 —— 力闭环会照着错误的量动作。
|
||||
"""
|
||||
body = MINIMAL + "axis:\n normal: y\n shear_x: y\n shear_y: x\n"
|
||||
with pytest.raises(TactileConfigError) as ei:
|
||||
load_tactile(_write(tmp_path, body))
|
||||
assert "互不相同" in str(ei.value)
|
||||
|
||||
|
||||
def test_load_bad_axis_name_is_rejected(tmp_path):
|
||||
"""轴名写成 w 之类的必须当场报错，不能当未知轴静默处理。"""
|
||||
body = MINIMAL + "axis:\n normal: w\n shear_x: z\n shear_y: x\n"
|
||||
with pytest.raises(TactileConfigError) as ei:
|
||||
load_tactile(_write(tmp_path, body))
|
||||
assert "x/y/z" in str(ei.value)
|
||||
|
||||
|
||||
def test_load_accepts_uppercase_axis(tmp_path):
|
||||
"""大小写不敏感 —— YAML 里手写 'Y' 很常见，不该因此启动失败。"""
|
||||
body = MINIMAL + "axis:\n normal: Y\n shear_x: Z\n shear_y: X\n"
|
||||
assert load_tactile(_write(tmp_path, body)).axis.normal == "y"
|
||||
|
||||
|
||||
def test_load_rejects_nonpositive_publish_hz(tmp_path):
|
||||
"""发布率为 0 时下游判活（0.4s 内要有新鲜帧）会整体失效。"""
|
||||
with pytest.raises(TactileConfigError) as ei:
|
||||
load_tactile(_write(tmp_path, MINIMAL.replace("20.0", "0.0", 1)))
|
||||
assert "publish_hz" in str(ei.value)
|
||||
|
||||
|
||||
def test_load_rejects_nonpositive_contact_timeout(tmp_path):
|
||||
"""超时 <=0 会让接触判定永远为假（或永远为真），两种都属静默失效。"""
|
||||
body = MINIMAL + "contact_timeout_s: 0.0\n"
|
||||
with pytest.raises(TactileConfigError) as ei:
|
||||
load_tactile(_write(tmp_path, body))
|
||||
assert "contact_timeout_s" in str(ei.value)
|
||||
|
||||
|
||||
def test_load_rejects_non_numeric(tmp_path):
|
||||
body = MINIMAL + "force_scale: 很大\n"
|
||||
with pytest.raises(TactileConfigError) as ei:
|
||||
load_tactile(_write(tmp_path, body))
|
||||
assert "force_scale" in str(ei.value)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 桥接规则 —— 与 ros_gz_bridge 的契约
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_bridge_args_types(tmp_path):
|
||||
"""消息类型串写错时 parameter_bridge 只是**静默跳过**那条规则。
|
||||
|
||||
症状就是"话题根本不存在"，而它和"传感器没配好"长得一模一样。
|
||||
"""
|
||||
spec = load_tactile(_write(tmp_path, MINIMAL))
|
||||
args = bridge_args(spec)
|
||||
assert "/tactile/left/wrench@geometry_msgs/msg/Wrench[gz.msgs.Wrench" in args
|
||||
assert "/tactile/left/raw@ros_gz_interfaces/msg/Contacts[gz.msgs.Contacts" in args
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 基线跟踪 —— 第 2 类静默错误
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_warmup_averages_samples():
|
||||
"""预热期取等权均值（不用 EMA：从 0 爬升太久，运动后残留很大）。"""
|
||||
t = BaselineTracker(baseline_seconds=1.0, tau_s=3.0)
|
||||
t.add("fn", 100.0, 0.1, in_contact=False)
|
||||
t.add("fn", 200.0, 0.1, in_contact=False)
|
||||
assert t.value("fn") == pytest.approx(150.0)
|
||||
assert t.delta("fn", 200.0) == pytest.approx(50.0)
|
||||
|
||||
|
||||
def test_contact_readings_never_enter_baseline():
|
||||
"""接触期间的读数绝不能进基线。
|
||||
|
||||
这是本模块最重要的一条：如果进了，基线会被抬到接触力上，
|
||||
于是 delta 恒为 0 —— 症状是"明明夹住了，fnormal 却一直是 0"，
|
||||
而下游的触面判定/力闭环全部失效。看上去像传感器坏了。
|
||||
"""
|
||||
t = BaselineTracker(baseline_seconds=0.5, tau_s=3.0)
|
||||
t.add("fn", 10.0, 0.1, in_contact=False)
|
||||
# 预热期内的接触读数：既不加和也不计数
|
||||
t.add("fn", 9999.0, 0.1, in_contact=True)
|
||||
assert t.value("fn") == pytest.approx(10.0)
|
||||
# 预热结束、进入稳态后同样不接受
|
||||
t.advance(0.5)
|
||||
t.add("fn", 9999.0, 1.0, in_contact=True)
|
||||
assert t.value("fn") == pytest.approx(10.0)
|
||||
|
||||
|
||||
def test_idle_tracking_follows_drift():
|
||||
"""无接触时的慢跟踪要真的跟上漂移（姿态一变零点就变，靠它消残余）。"""
|
||||
t = BaselineTracker(baseline_seconds=0.1, tau_s=1.0)
|
||||
t.add("fn", 0.0, 0.1, in_contact=False)
|
||||
t.advance(0.1)
|
||||
# dt/tau = 1.0 → 一步就到新值
|
||||
t.add("fn", 50.0, 1.0, in_contact=False)
|
||||
assert t.value("fn") == pytest.approx(50.0)
|
||||
|
||||
|
||||
def test_warmed_up_only_after_baseline_seconds():
|
||||
"""预热没结束就不该对外声称基线可用（否则启动瞬间的读数是脏的）。"""
|
||||
t = BaselineTracker(baseline_seconds=2.0, tau_s=3.0)
|
||||
assert not t.warmed_up
|
||||
t.advance(1.9)
|
||||
assert not t.warmed_up
|
||||
t.advance(0.2)
|
||||
assert t.warmed_up
|
||||
|
||||
|
||||
def test_disabled_tracker_keeps_warmup_value():
|
||||
"""关掉慢跟踪（rebaseline_when_idle=false）时，基线停在预热均值。"""
|
||||
t = BaselineTracker(baseline_seconds=0.1, tau_s=1.0, enabled=False)
|
||||
t.add("fn", 10.0, 0.1, in_contact=False)
|
||||
t.advance(0.1)
|
||||
t.add("fn", 500.0, 1.0, in_contact=False)
|
||||
assert t.value("fn") == pytest.approx(10.0)
|
||||
|
||||
|
||||
def test_zero_tau_keeps_warmup_value():
|
||||
"""tau<=0 等价于关闭（除零/负 alpha 都不能出现）。"""
|
||||
t = BaselineTracker(baseline_seconds=0.1, tau_s=0.0)
|
||||
t.add("fn", 10.0, 0.1, in_contact=False)
|
||||
t.advance(0.1)
|
||||
t.add("fn", 500.0, 1.0, in_contact=False)
|
||||
assert t.value("fn") == pytest.approx(10.0)
|
||||
|
||||
|
||||
def test_unknown_key_defaults_to_zero():
|
||||
"""没喂过的键按 0 处理：不能让 delta 抛异常（订阅还没来数据时就要发布）。"""
|
||||
t = BaselineTracker(baseline_seconds=1.0, tau_s=3.0)
|
||||
assert t.value("never_seen") == 0.0
|
||||
assert t.delta("never_seen", 7.0) == pytest.approx(7.0)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 跨文件一致性 —— 第 3 类静默错误
|
||||
# ---------------------------------------------------------------------------
|
||||
def _repo_root():
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
return root if (root / "devices/sim/gazebo/config/tactile.yaml").is_file() else None
|
||||
|
||||
|
||||
def test_real_repo_config_topics_match_xacro():
|
||||
"""仓库里 tactile.yaml 的话题名必须与 xacro 里写的 <topic> 逐字一致。
|
||||
|
||||
这条是防"改了一处忘了另一处"的：配置说订阅 /tactile/left/wrench，
|
||||
而 xacro 里写的是别的名字时，节点能正常起来、话题列表里也有东西，
|
||||
只是 status 恒为零 —— 看起来完全像传感器/桥接的问题。
|
||||
"""
|
||||
root = _repo_root()
|
||||
if root is None:
|
||||
pytest.skip("找不到仓库根")
|
||||
|
||||
spec = load_tactile(root=root)
|
||||
xacro = (root / "devices/sim/gazebo/urdf/arm.sim.sensors.xacro").read_text(
|
||||
encoding="utf-8"
|
||||
)
|
||||
assert {s.side for s in spec.sides} == {"left", "right"}, "双指方案必须是 left+right"
|
||||
for s in spec.sides:
|
||||
assert f"<topic>{s.wrench_topic}</topic>" in xacro, s.wrench_topic
|
||||
assert f"<topic>{s.contact_topic}</topic>" in xacro, s.contact_topic
|
||||
|
||||
|
||||
def test_real_repo_config_frames_exist_in_description():
|
||||
"""配置里的 frame 必须是描述里真实存在的 link 名。
|
||||
|
||||
frame 用错时力数据本身没问题，但接触点会转到不存在的坐标系里 ——
|
||||
tf2 只是抛异常/返回空，centroid 静默变成 0。
|
||||
"""
|
||||
root = _repo_root()
|
||||
if root is None:
|
||||
pytest.skip("找不到仓库根")
|
||||
|
||||
try:
|
||||
from arm_sim_gazebo.robot_description import prepare_urdf
|
||||
|
||||
urdf, _ = prepare_urdf()
|
||||
except Exception as exc: # 环境没有 xacro/flexiv_description
|
||||
pytest.skip(f"无法展开仿真描述：{exc}")
|
||||
|
||||
spec = load_tactile(root=root)
|
||||
for s in spec.sides:
|
||||
assert f'name="{s.frame}"' in urdf, f"{s.side}: {s.frame} 不在描述里"
|
||||
Reference in New Issue
Block a user