arm_dev
b29339e522
三块补齐，目标是"同一个流程节点，仿真/真机都不改代码"：
1. 真值孔位（truth_holes.py）：把 scene.yaml 与模型 SDF 里已知的孔几何
直接投影到相机像素坐标，替代 HoughCircles。perception.source 选：
yolo / truth / external —— truth 就走这条路。
为什么不用 Hough：仿真里孔是"方孔 + 贴片亮环"的近似（SDF 没有布尔运算），
靠图像门槛去凑 radus 区间很脆；而孔位本来就是场景里已知量，直接投影
既准又稳，还能当真实检测算法的对照基线。
2. 真值相机（sim_truth_perception.py）：同理，直接算物体位姿，供
visual_servo/filtered_point 用，绕开 ONNX。
3. 补 /home 与 set_named_pose 服务（sim_home_service.py）：真机上这两个
由 Flexiv 驱动提供，仿真里没有 —— 缺了 S01 直接超时。
4. sim_tactile_adapter 容错导入 ros_gz_interfaces：那一路只是"接触点"，
力本身走 geometry_msgs/Wrench。typesupport 加载不了时退化成
contact 恒 False + centroid 恒 0，三力判据（S07 用的）不受影响 ——
宁可用退化模式跑，也不要整个适配器起不来。
5. repo_root 解析（已装包要能找回源码仓库根，见 test_repo_root.py 的用例）
与 use_sim_time 向子 launch 的传递。
另：scripts/gen_sim_handeye.py 把 URDF 真值导出手眼文件（仿真不标定）。
Co-authored-by: Cursor <cursoragent@cursor.com>
156 lines
8.7 KiB
YAML
156 lines
8.7 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
|
||
# 指腹内侧的"夹持点"（= tool center point）。**上料脚本要用它**：
|
||
# 两指 TCP 的中点就是工具轴，规尖垂在这条轴线上。
|
||
# 与 frame 的区别：frame 是力传感器的参照系（指尖关节的 child link），
|
||
# TCP 在它的子链再往下 0.036m（见 flexiv_description 的 finger_tcp_joint）。
|
||
# 名字不能靠 frame 加后缀推 —— 实测这两条链分别是
|
||
# sim_left_finger_mount / sim_left_finger_tcp（父链与子链并列命名）。
|
||
tcp_frame: sim_left_finger_tcp
|
||
|
||
- side: right
|
||
wrench_topic: /tactile/right/wrench
|
||
contact_topic: /tactile/right/raw
|
||
frame: sim_right_finger_mount
|
||
tcp_frame: sim_right_finger_tcp
|
||
|
||
# --- 轴向映射（已实测，不是猜的）----------------------------------------
|
||
# 力传感器挂在指尖关节上，给出的是该关节 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
|