真机落地前的仿真补齐:夹持/腕力/真值位姿,让仿真能验证 S07–S09

这批是 09-24 那轮仿真工作的收尾,一直没提交。内容按目的分四块:

1) 仿真真能「夹住」工件
   sim_grasp_lock:给规与夹爪之间加接触锁(默认关,run_gauge_assembly --sim 才开)。
   没有它时仿真里规只靠摩擦,S08 棘轮一转就滑出指间,于是把「仿真不真」误读成
   「力判据不对」。新增 test_sim_grasp_lock.py。

2) 腕部力传感器进仿真
   sim_wrist_ft(+ config/wrist_ft.yaml):S07 的首选判据是腕部 FT 的轴向突变,
   而仿真里以前根本没有腕部力,S07 只能退到触觉兜底,两套判据从没在同一场景下比过。
   现在两条通道都在,接触判定可对照。

3) 真值位姿可查(不再靠猜)
   sim_live_pose / sim_pose_service:把 Gazebo 里刚体的实测位姿发布出来,
   stage2 的真值路径按**实测**位姿算孔位,而不是按场景文件里的标称位姿。
   顺带把真值孔位的推导(truth_holes)独立出来并补测。

4) 工具几何成为可配置项
   config/sim_tool_geometry.yaml + stage2 的 tool_tip_below_flange_m 启动参数:
   仿真里规会在指间下滑,工具长每次装夹都不同(实测见过 243/254mm),
   悬停高度与 S07 下探预算必须按实测值算,不能再写死。

应用层同时跟进:gauge_fsm / gauge_config 把「悬停时工具尖端入没入工件」的判据
(hover_z_guard)与拖件判定(drag_verdict)收成纯函数并补测;stage3 搜孔、
site 导出、run_gauge_assembly 与若干文档同步。
新增 test_gauge_exit_lift / test_gauge_gripper_client / test_node_handler_collisions。

测试:全仓库 1085 passed。
This commit is contained in:
arm_dev
2026-09-28 10:17:06 +08:00
parent 1b4d6168d9
commit c1cd862eea
41 changed files with 5327 additions and 115 deletions
+62 -3
View File
@@ -118,13 +118,24 @@
</collision>
<!-- 夹持段:夹爪夹这里,同样用软接触。
摩擦给 0.4:真机夹爪指垫是橡胶/聚氨酯,比金属-金属的 0.25 高。
给太低(<0.2)规会在指间打滑自转,而现象看起来像"夹爪没夹紧"。 -->
摩擦给 0.8:真机夹爪指垫是橡胶/聚氨酯,对钢件的 µ 在 0.6~1.0 之间。
这个数**直接决定"能不能把轴向力传给手指"**:能传的上限 = µ·N
(单指),握力 N≈46N → 0.8 时两指合计 ≈74N,0.4 时只有 ≈37N。
为什么原来是 0.4、为什么必须改(2026-09-24):
台肩一落面,接触刚度 5e4 N/m 意味着**过压 0.5mm 就是 25N**。µ=0.4
时两指上限 ≈37N,看着够;但一步步进就是 0.5mm,下一步就 50N > 37N
→ 规在爪里打滑,力就再也传不到指垫触觉上(读数停在 0,而它是对的)。
把 µ 改成接近真实的 0.8,连同"改用腕部 FT 当判据",两条一起才成立:
- 腕部 FT 在摩擦上游,打滑也测得准(首选判据);
- 摩擦接近真实值,才不会出现"规被顶得往上溜 20mm 顶到掌心"的
仿真特有现象(那会让 S07 在 25mm 处顶死、报假原因)。
给太低(<0.2)还会让规在指间打滑自转,现象看起来像"夹爪没夹紧"。 -->
<collision name="grip">
<pose>0 0 0.0555 0 0 0</pose>
<geometry><cylinder><radius>0.015</radius><length>0.045</length></cylinder></geometry>
<surface>
<friction><ode><mu>0.4</mu><mu2>0.4</mu2></ode></friction>
<friction><ode><mu>0.8</mu><mu2>0.8</mu2></ode></friction>
<contact>
<ode><kp>5e4</kp><kd>5e2</kd><max_vel>0.01</max_vel><min_depth>0.0005</min_depth></ode>
</contact>
@@ -189,5 +200,53 @@
</visual>
</link>
<!-- =====================================================================
抓取锁 GRASP LOCK —— 只存在于仿真,真机不需要
=====================================================================
做什么:夹爪一旦确认夹住,就用一个**固定关节**把规锁在手指上;
夹爪一张开就解锁。规与指垫之间的接触对仍然存在,
所以触觉/腕部 FT 的读数照旧是真实接触力。
为什么必须有它(2026-09-24 用一整轮排查换来的):
仿真里的夹持力不是"设"出来的,而是**位置误差 × 接触刚度**
(见上面 kp 那段说明)。规站着不动时够用,可机械臂一动就不行:
· S01 要绕工具轴快转 150°;
· pre_lift / 粗定位是一次 60~100mm 的连续轨迹;
这两步里规在指间**打滑**(实测工具长度 0.243m → 0.285m,规歪了 62°),
于是 S07 拿到的几何全是错的 —— 现象是"S07 判不到接触"或"规掉了(E1)",
而日志里看不到真因。
真机不会这样:握力 46N、µ≈0.8、规 0.27kg,摩擦余量两个数量级。
所以这把锁补的是**Gazebo 缺的那一小块物理**,不是绕过物理:
刚性接触求解器给不出静摩擦迟滞,也不会模拟指垫橡胶的贴合,
而现实的夹持恰恰靠这两件事。补上之后,仿真才配得上"验证装配逻辑"。
⚠️ 四处必须对上,否则它**静默失效**(不报错,只是规不跟着走):
1. <parent_link> = 本模型(规)里要被抓的那根连杆:body;
2. <child_model> = 机器人 spawn 时的 -name(launch 里是 rizon_sim);
3. <child_link> = Gazebo 里**真实存在**的连杆名,且必须是**不跟着
张爪动**的那一根 —— 这里填的是 `sim_link7`(法兰;夹爪本体
`sim_grav_base_link` 在 sdformat 里被合并进去了,见
`/sim/world_pose_info` 里的 `sim_link7_fixed_joint_lump__...`)。
⚠️ 2026-09-24 实测:填成 `sim_left_finger_mount`(手指)会出事 ——
S08 棘轮每轮"松爪 → 回转 → 夹紧",张爪时手指外移 40mm 而规被
刚性锁着、又插在孔里,两处约束打架把规**从孔里拽出来**
(规尖跳 +17.5mm、偏摆 64°)→ 下一步掉地 → 只报"通止规掉了(E1)"。
真名怎么查:
ros2 topic echo /sim/world_pose_info (列全部 child_frame_id)
4. attach/detach 话题与 sim_gripper_adapter 下发的名字一致;
两侧名字对不上时,锁只会"永远不触发"。
===================================================================== -->
<plugin filename="ignition-gazebo-detachable-joint-system"
name="ignition::gazebo::systems::DetachableJoint">
<parent_link>body</parent_link>
<child_model>rizon_sim</child_model>
<child_link>sim_link7</child_link>
<attach_topic>/sim/grasp_lock/attach</attach_topic>
<detach_topic>/sim/grasp_lock/detach</detach_topic>
<output_topic>/sim/grasp_lock/state</output_topic>
</plugin>
</model>
</sdf>