arm_dev
01315773e7
链路：指尖关节 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>
148 lines
8.2 KiB
YAML
148 lines
8.2 KiB
YAML
# =============================================================================
|
||
# 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/<side>/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 里写的 <topic> 一致；
|
||
# 名字对不上时的症状同样是静默：适配器订阅一个永远不存在的话题，
|
||
# 它不报错，只是 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
|