# =============================================================================
# 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