diff --git a/devices/sim/gazebo/config/tactile.yaml b/devices/sim/gazebo/config/tactile.yaml new file mode 100644 index 0000000..d112d52 --- /dev/null +++ b/devices/sim/gazebo/config/tactile.yaml @@ -0,0 +1,147 @@ +# ============================================================================= +# devices/sim/gazebo/config/tactile.yaml +# +# 仿真触觉传感器的标定与口径。sim_tactile_adapter 与启动文件都从这里读。 +# +# 与 cameras.yaml 放在一起、同样属于「物料而非代码」：改这里不需要重新编译， +# 也不需要改任何 Python。真机的对应物是 config/site.yaml 的 tactile: 段。 +# ============================================================================= +# +# ----------------------------------------------------------------------------- +# 单位口径：为什么仿真与真机不能直接共用阈值 +# ----------------------------------------------------------------------------- +# 真机 Orisys 输出的是**形变任意单位**（非牛顿），厂商没有给牛顿换算， +# 且基线会漂移 —— 所以真机侧所有阈值都是「相对静止均值的增量」 +# （见 devices/sensors/orisys/manifest.yaml）。 +# +# 仿真侧 Gazebo 给的是**真正的牛顿**。两者数值不在一个尺度上， +# 直接把真机的 touch_delta=250 拿到仿真里用是错的（要么永远不触发， +# 要么一碰就超上限）。框架的做法是： +# +# 声明本框架的触觉力单位 = force_scale 定义的那个尺度 +# 仿真输出 = Gazebo 牛顿 × force_scale +# +# 于是有两种用法，选哪种由 force_scale 的值决定： +# +# a) force_scale = 1.0（默认）—— 输出就是牛顿。 +# 适合：仿真里自己一套阈值（按牛顿设），或只想验证状态机逻辑。 +# 注意：此时**不能**直接套用真机那套 250/600 之类的阈值。 +# +# b) force_scale = 实测的「Orisys 单位 / 牛顿」—— 输出换算到 Orisys 单位。 +# 适合：希望仿真与真机共用同一组阈值。 +# 怎么测：真机上用已知重量的砝码压传感器（或静态夹持已知刚度的物体）， +# 读 /tactile//status 的 fnormal 增量 Δu，force_scale = Δu / (m·g)。 +# +# ⚠️ 这是一处**必须在验收时确认**的口径，因为选错不会报任何错： +# 阈值只是"不触发"或"一碰就炸"，看起来像算法问题，实际是单位问题。 +# ----------------------------------------------------------------------------- + +# 框架声明的单位名。仅用于日志与文档，代码不解释它。 +unit: "N" + +# --- 发布 --------------------------------------------------------------- +# 与真机 site.yaml 的 tactile.publish_hz 保持一致：下游（stage3_search_node +# 的 _tac_fresh）按"0.4s 内必须收到新鲜帧"判活，发布率太低的后果与掉线一样。 +publish_hz: 20.0 + +# --- 基线 --------------------------------------------------------------- +# 真机启动时采 baseline_seconds 秒静止均值，之后所有读数都是相对它取增量。 +# 仿真必须做同样的事，原因有二： +# 1) force_torque 读的是**关节传递力**，静止时它等于远端那段手指的自重 +# （实测 3×1e-4 kg × 9.81 = 2.94 mN），不减掉就是一个恒定偏置； +# 2) 更重要的是语义一致 —— 下游阈值本来就是"增量"，不减基线就对不上。 +baseline_seconds: 2.0 + +# 基线慢跟踪：不接触时用时间常数 tau 慢慢更新基线。 +# 为什么需要：上面那个自重项会随**机械臂姿态**变化（换姿态后"零力"读数会漂移）， +# 一次性基线会在运动后留下残余。只在确认无接触时更新，避免把真实接触吃掉。 +rebaseline_when_idle: true +rebaseline_tau_s: 3.0 + +# --- 接触布尔 ----------------------------------------------------------- +# ⚠️ 接触传感器的消息是**事件式**的：只在接触时发，不接触时一条都不发。 +# 所以"最近 contact_timeout_s 内收到过消息"才是接触判据； +# 若写成"收到消息=接触、没收到=不管"，那么离开工件后 contact 会永远停在 True。 +contact_timeout_s: 0.25 + +# 接触面积/点数少于此值视为噪声（求解器偶发单点接触）。 +min_contact_points: 1 + +# --- 标度 --------------------------------------------------------------- +# 见文件头「单位口径」。1.0 = 输出牛顿。 +force_scale: 1.0 + +# --- 视触觉图像--------------------------------------------------------- +# 仿真里没有弹性体成像，深度图/力矢量图**只能是占位**：话题与消息结构保留 +# （下游不用改代码），内容为空/NaN，并打一次明确日志说明它不是真实数据。 +# 0 = 不发布（默认）。真机默认也是 0，因为图像会挤占三力的时序。 +image_hz: 0.0 + +# 占位图的尺寸（像素）。随便取一个小尺寸即可，它没有物理含义。 +image_width: 64 +image_height: 64 + +# --- 两指 --------------------------------------------------------------- +# wrench_topic / contact_topic 必须与 arm.sim.sensors.xacro 里写的 一致； +# 名字对不上时的症状同样是静默：适配器订阅一个永远不存在的话题， +# 它不报错，只是 status 里永远是零。启动时会做一次存在性检查。 +sides: + - side: left + # 三向力来自指尖关节的 force_torque 传感器（持续发布） + wrench_topic: /tactile/left/wrench + # 接触布尔来自指尖 link 的 contact 传感器（只在接触时发布） + contact_topic: /tactile/left/raw + # 力传感器所在的坐标系（即 *_inner_finger_joint 的 child link）。 + # 接触点会转到这个系里，用来算 centroid_u/v，也是轴的参照系。 + frame: sim_left_finger_mount + + - side: right + wrench_topic: /tactile/right/wrench + contact_topic: /tactile/right/raw + frame: sim_right_finger_mount + +# --- 轴向映射（已实测，不是猜的）---------------------------------------- +# 力传感器挂在指尖关节上，给出的是该关节 child link 坐标系里的三向力。 +# 哪个轴对应"指腹法向"必须实测 —— 猜错不会报错，只会让所有力的语义静默错位。 +# +# 实测方法（2026-09-23，Fortress 6.16）： +# 把一块 40×4×40mm 的**大平面薄板**夹在两指之间，等 6 秒进入稳态， +# 取 20 个样本求均值。用小方块不行：它与指腹是点/边接触，法向会偏、 +# 摩擦混进来，读数方向不干净（实测左指甚至出现 z 主导）。 +# +# 实测结果（相对无接触基线的增量）： +# left : ΔFx=+1.52 ΔFy=+140.74 ΔFz=-0.18 ← 主导轴 y +# right: ΔFx=+1.27 ΔFy= +66.59 ΔFz=-1.33 ← 主导轴 y +# +# 结论： +# * 法向 = y（两指一致）。x/z 在纯法向挤压下都近似为零，证明它们是切向。 +# * **两指的符号相同、压下去都是正**，所以 sign 两指都是 +1。 +# 这正合下游预期（真机 fnormal 也是压下去为正、且被 clamp 到非负）。 +# +# 复测（同日，换成 60×120×80mm 的静态薄板、从张开 128mm 收到 80mm）： +# left : fnormal=+714.7 fshearx=-142.6 fsheary=+408.8 +# right: fnormal=+102.5 fshearx= -91.8 fsheary=+146.9 +# 复测确认了上面两条结论（法向=y、两指同号为正），并给出两点现场经验： +# a) 量级可以到几百 —— 位置控制器是"刚性地压进去"的，没有力控限幅， +# 所以 force_scale=1.0 时仿真里的数字**通常远大于真机阈值 250/600**。 +# 要共用真机阈值必须按上面 b) 的方式标定 force_scale，不能想当然。 +# b) 薄板没夹正时两指读数差一个量级（714 vs 102），切向也会很大。 +# 这不是链路问题，是几何问题 —— 验收触觉时应让工件居中， +# 否则会把"夹偏了"误判成"传感器标定错了"。 +# +# 为什么两指符号会相同：左右指的 mimic 链带镜像（右外指节 rpy z=π）， +# 补偿后压同一块板得到同号读数。这一点已由实测确认，不必再从 URDF 推导。 +# +# 两个切向轴的命名是**框架约定**（真机上 fshearx/fsheary 对应哪个物理方向 +# 取决于传感器贴装，本框架不假设二者一致）： +# shear_x ← child z（指尖"前进"方向，home 姿态下约等于世界 +x） +# shear_y ← child x（与前者正交，home 姿态下约为世界竖直方向） +axis: + normal: "y" + shear_x: "z" + shear_y: "x" + +sign: + normal: 1.0 + shear_x: 1.0 + shear_y: 1.0 diff --git a/devices/sim/gazebo/urdf/arm.sim.sensors.xacro b/devices/sim/gazebo/urdf/arm.sim.sensors.xacro index fa15c90..c7b7f74 100644 --- a/devices/sim/gazebo/urdf/arm.sim.sensors.xacro +++ b/devices/sim/gazebo/urdf/arm.sim.sensors.xacro @@ -166,7 +166,12 @@ 所有阈值都按"相对静止均值的增量"使用（见 devices/sensors/orisys/manifest.yaml）。仿真给的是牛顿， 所以两者的数值**不能直接共用阈值** —— 标度换算由 - config/tactile.yaml 的 force_scale 负责，详见该文件与 README。 + devices/sim/gazebo/config/tactile.yaml 的 force_scale 负责， + 该文件头把两种用法讲清楚了。 + + ⚠️ 三向力的轴映射（哪根轴是指腹法向）是**实测**出来的，不是读代码推的， + 结论与实测方法记在同一个 yaml 的 axis: 段里。改夹爪/换传感器后 + 要重测，否则力的语义会静默错位（有数值、但方向没意义）。 ======================================================================= --> @@ -225,7 +230,8 @@ → 读"父 link 施加给子 link 的力"。 静止时它等于 -(接触力 + 手指自重)， 所以真正符号如何、哪根轴对应"法向"， - 以实测为准（见 config/tactile.yaml 的 + 以实测为准（见 devices/sim/gazebo/config/ + tactile.yaml 的 axis_* 映射项与 README 的实测记录）。 ⚠️ 这个传感器是**持续发布**的（即使没有任何接触，也按 update_rate diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py b/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py index f6d8679..0b3faef 100644 --- a/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py +++ b/src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py @@ -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 里就只会表现为"话题不出现、 + 日志空白"，而本检查能在启动前直接指出是哪个话题缺了。 + + 已实测确认能抓到的情形： + * 引用的名字在 SDF 里不存在（sdformat 命名规则变了） + * 里没有 （sdformat 会补一个占位值 __default_topic__， + 所以这一项不能只判"空"，要连占位值一起判 —— 这是单测逼出来的） + * 传感器标签没有产生出任何 force_torque/contact 传感器 + * 配置里声明的话题在 SDF 里找不到 + + ========================================================================= + 为什么必须有这个检查 + ========================================================================= + 接触传感器的 里写的**不是我们起的名字**，而是 sdformat 从 + URDF 转 SDF 时按规则自动生成的： + + + "_fixed_joint_lump__" + <本 link> + "_" + collision 名 + + 本机实测（Fortress 6.16）得到 + sim_left_finger_mount_fixed_joint_lump__sim_left_finger_tip_collision + 而 URDF 的 不能命名，所以没法给它一个稳定名字。代价是： + **换一个 sdformat 版本、命名规则一变，传感器就永远收不到数据 —— 而 + Gazebo 不会为此报任何错**（话题在、没输出，看起来像"还没接触"）。 + + 同样容易静默失效的两处，这里一并核对： + * force_torque 传感器挂的关节是否存在于 SDF（写错关节名 = 永不发布） + * 接触传感器的话题是否在 里（写在 层会被忽略， + 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 的**限定名**。 + # ⚠️ 不要再拼一次 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} 没有 块") + continue + ref = (contact.findtext("collision") or "").strip() + if not ref or ref == "__default__": + problems.append( + f"接触传感器 {sname} 的 里没有可用的 " + 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 里的 。" + ) + # ⚠️ 同样要防 sdformat 的占位值：缺 时它会补一个 + # __default_topic__（实测），于是"有没有 "这种检查会被骗过去。 + # __default_topic__ 不是可用话题名 —— gz 实际发的是 + # /world//model//link//sensor//contact。 + ctopic = (contact.findtext("topic") or "").strip() + if ctopic in ("", "__default_topic__"): + problems.append( + f"接触传感器 {sname} 的 里没有可用 " + f"（实测值: {ctopic or '空'}）。\n" + f" 话题必须写在 contact/topic（写在 层会被静默忽略，" + f"gz 改用默认 scoped 话题名）。" + ) + elif stype == "force_torque": + n_ft += 1 + parent = _sdf_parent_joint(root, sensor) + if parent is None: + problems.append( + f"力传感器 {sname} 不在任何 里。\n" + f" force_torque 在 SDF 里只能属于关节；用 " + f" 表达，" + f"sdformat 会把它移进对应的 。" + ) + elif parent not in joint_names: + problems.append(f"力传感器 {sname} 挂在未知关节 {parent} 上") + if (sensor.findtext("topic") or "").strip() == "": + problems.append( + f"力传感器 {sname} 没有 （它不会用默认话题名，" + f"而是完全不发布）。" + ) + + # 传感器是否被整个丢掉 —— 这一项最容易被忽略，也最难现场排查。 + if n_contact == 0 and n_ft == 0: + problems.append( + "SDF 里**一个触觉传感器都没有**。\n" + " 说明 arm.sim.sensors.xacro 里的 名字写错、" + "或传感器标签写到了错误的位置 —— 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" 常见原因： 的关节名不存在" + f"（整个传感器被丢掉），或 contact 传感器少写了 。\n" + f" SDF 里实际有的话题: {sorted(t for t in sdf_topics if t)}" + ) + return problems + + +def _sdf_parent_joint(root, sensor) -> Optional[str]: + """在 SDF 树里找 sensor 所属的 名。""" + 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, diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_tactile.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_tactile.py new file mode 100644 index 0000000..edbe28d --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_tactile.py @@ -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" 完全一致 —— 对不上时适配器只是订阅了一个不存在的话题，\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) diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_tactile_adapter.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_tactile_adapter.py new file mode 100644 index 0000000..28006b0 --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_tactile_adapter.py @@ -0,0 +1,361 @@ +"""仿真触觉适配器：Gazebo 的关节力/接触传感器 → arm_msgs/TactileStatus。 + +============================================================================= +它在链路上的位置 +============================================================================= + gz /tactile//wrench (gz.msgs.Wrench, 持续 ~100Hz) + gz /tactile//raw (gz.msgs.Contacts, 只在接触时) + │ + ├─ ros_gz_bridge parameter_bridge（只搬类型，不改语义） + │ + └─ 本节点 + ├ 轴向投影 三向力 → fnormal/fshearx/fsheary（映射实测，见 config） + ├ 基线相减 静止时关节要承担手指自重，不减就是一个恒定偏置 + ├ 接触判据 超时判"已离开"（接触传感器是事件式发布） + └ 变送 /tactile//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 声明了任何 ， + 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 声明了任何 ，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() diff --git a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py index 0c2d9d2..80bc97c 100644 --- a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py +++ b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py @@ -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" diff --git a/src/arm_sim_gazebo/package.xml b/src/arm_sim_gazebo/package.xml index 68ee959..7dee945 100644 --- a/src/arm_sim_gazebo/package.xml +++ b/src/arm_sim_gazebo/package.xml @@ -30,6 +30,11 @@ ros_gz_sim ros_gz_bridge + + ros_gz_interfaces + arm_msgs + tf2_ros gz_ros2_control controller_manager joint_state_broadcaster diff --git a/src/arm_sim_gazebo/setup.py b/src/arm_sim_gazebo/setup.py index ffb456e..f78f972 100644 --- a/src/arm_sim_gazebo/setup.py +++ b/src/arm_sim_gazebo/setup.py @@ -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', diff --git a/src/arm_sim_gazebo/test/test_robot_description.py b/src/arm_sim_gazebo/test/test_robot_description.py index 64e8b8b..640f388 100644 --- a/src/arm_sim_gazebo/test/test_robot_description.py +++ b/src/arm_sim_gazebo/test/test_robot_description.py @@ -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. 写的是 sdformat 自动生成的名字，命名规则一变就 +# 永远收不到数据，而 Gazebo 不报任何错； +# 2. 接触传感器的话题必须写在 里，写在外层 会被忽略， +# 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：把话题从 挪到 层，必须被报出来。 + + 这条是"看起来更合理"的写法（force_torque 就是写在外层 的）， + 所以极易在重构时被顺手改过去。改完话题从 /tactile/left/raw 变成 + gz 的 scoped 名字，消费者永远等不到消息。 + """ + import re + + from arm_sim_gazebo.robot_description import check_tactile_collisions + + urdf = _strip_tactile_expected() + # 精准摘掉 里的那个 （不是注释里提到的那处）。 + # ⚠️ 不能用「带换行」的正则：xacro 会把 压掉换行，真实的 + # 紧跟在前一个注释的 --> 后面，行尾是 而不是 \n。 + # 第一版就是这么写的，结果正则没匹配上、断言却照样通过（见下面的 + # sanity check）—— 测试假绿比没测试更危险。 + surgically_removed = re.sub( + r"(.*?)[^<]*", r"\1", urdf, count=1, flags=re.S + ) + assert surgically_removed != urdf, "字符串手术没生效" + # sanity：元素形式的 已消失（注释里那处文本不算） + assert not re.search(r"/tactile/left/raw", 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'.*?', 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(" 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 里的 一致"。""" + 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 里写的 逐字一致。 + + 这条是防"改了一处忘了另一处"的：配置说订阅 /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"{s.wrench_topic}" in xacro, s.wrench_topic + assert f"{s.contact_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} 不在描述里"