arm_dev
c1cd862eea
这批是 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。
253 lines
14 KiB
XML
253 lines
14 KiB
XML
<?xml version="1.0"?>
|
||
<!--
|
||
=============================================================================
|
||
devices/sim/gazebo/models/gauge/model.sdf
|
||
通止规（螺纹通止规）。被夹爪夹住、往工件孔里旋入的那个"规"。
|
||
|
||
位姿由 config/scene.yaml 决定，不在本文件里写死（见 workpiece 的同类说明）。
|
||
|
||
============================================================================
|
||
结构（局部坐标：原点在最下端的规尖，+Z 朝上，与真机"竖直朝下装配"一致）
|
||
============================================================================
|
||
|
||
┌──────┐ ← 止端 NO-GO φ8.0 × 8mm z 0.078~0.086
|
||
│ │ 通不过 φ7 的孔（过盈 0.5mm 半径）→ 判断"孔大了"
|
||
├──────┤
|
||
│ │
|
||
│ │ ← 夹持段 GROUND φ30 × 45mm z 0.033~0.078
|
||
│ │ 夹爪夹这里。φ30 是**按夹爪的实际开合范围选的**：
|
||
│ │ 实测两指指尖间距 fw=0 时约 27~29mm、fw=0.10 时约 128mm
|
||
│ │ （见 README 的实测表）。取 30mm 让夹爪在接近闭合处
|
||
│ │ 夹住 —— 既能夹稳，又不会一上来就顶到行程末端。
|
||
├──────┤ ← 台肩 COLLAR φ16 × 3mm z 0.030~0.033
|
||
│ │ 进入孔前会先碰到工件表面，是"到底了"的硬限位
|
||
└──────┘
|
||
││││ ← 通端 GO φ6.0 × 30mm z 0~0.030
|
||
││││ 要通过 φ7 的光孔（单边间隙 0.5mm）→ 判断"孔小了"
|
||
▼▼▼▼
|
||
|
||
通端/止端的方向是有讲究的：**朝下的是通端**。因为装配时规是竖直朝下插入的，
|
||
真正进入孔里的必须是通端；止端朝上、只在人工复核（把规拔出来调头再试）时用。
|
||
若两者装反，仿真里会表现成"规怎么都插不进孔"，而看模型外观完全正常。
|
||
|
||
============================================================================
|
||
⚠️ 螺纹没有建模 —— 这是有意的，不是说简化
|
||
============================================================================
|
||
SDF 里没有螺纹几何，本框架也**不需要**它：
|
||
S08 的旋入是状态机按 site.yaml 的 screw_pitch_mm 同步下发"绕世界 Z 旋转
|
||
+ 按螺距下降"（见 stage3_search_node 的 insert 段）。物理上就是一根光轴
|
||
在光孔里转，螺距是**运动学参数**，不是几何参数。
|
||
所以仿真能验证的是"旋入动作/时序/力判据的联动对不对"，
|
||
**不能**验证螺纹啮合本身（会不会卡牙、螺纹精度够不够）。
|
||
|
||
============================================================================
|
||
⚠️ 单位：米。坐标原点在规尖，所以场景里 z = 台面高度 就是"规立在台面上"。
|
||
=============================================================================
|
||
-->
|
||
<sdf version="1.9">
|
||
<model name="gauge">
|
||
|
||
<static>false</static>
|
||
<self_collide>false</self_collide>
|
||
|
||
<link name="body">
|
||
|
||
<!-- =====================================================================
|
||
惯量
|
||
=====================================================================
|
||
钢件（ρ≈7850）。按各段体积算：
|
||
通端 π·0.003²·0.030 = 8.5e-7
|
||
台肩 π·0.008²·0.003 = 6.0e-7
|
||
夹持段 π·0.015²·0.045 = 3.18e-5
|
||
止端 π·0.004²·0.008 = 4.0e-7
|
||
合计 ≈ 3.38e-5 m³ → 质量 ≈ 0.265kg
|
||
惯量按"等效圆柱"(r=15mm, h=86mm)算，够用 —— 接触仿真里
|
||
规的姿态由接触约束主导，惯量的精确值不影响结论。
|
||
===================================================================== -->
|
||
<inertial>
|
||
<mass>0.265</mass>
|
||
<inertia>
|
||
<ixx>1.784e-04</ixx>
|
||
<ixy>0</ixy>
|
||
<ixz>0</ixz>
|
||
<iyy>1.784e-04</iyy>
|
||
<iyz>0</iyz>
|
||
<izz>2.980e-05</izz>
|
||
</inertia>
|
||
</inertial>
|
||
|
||
<!-- =====================================================================
|
||
通端 GO —— φ6.0 × 30mm（朝下）
|
||
=====================================================================
|
||
⚠️ 接触刚度（kp/kd）故意给得**软**，这是本模型最关键的一处调参：
|
||
夹爪是**位置控制**的（config/controllers.yaml 只有 position 接口），
|
||
夹持力不是"设"出来的，而是由位置误差乘接触刚度决定的。
|
||
用默认硬接触（world 里台面是 kp=1e7）时，夹爪指令闭合的几毫米
|
||
会变成几百牛的挤压力 —— 实测夹一块薄板时指尖力传感器读到过
|
||
700N 量级。那种力量下规会被弹飞，整条装配流程没法看。
|
||
把规这一侧的 kp 降到 5e4（比台面低 200 倍）：夹持 0.5mm 的
|
||
指令过盈 ≈ 25N，既夹得住（不掉），又不会弹飞。
|
||
想让夹持更紧/更松，首选的旋钮是**指令过盈量**，而不是继续改 kp。
|
||
===================================================================== -->
|
||
<collision name="go_stud">
|
||
<pose>0 0 0.015 0 0 0</pose>
|
||
<geometry><cylinder><radius>0.003</radius><length>0.030</length></cylinder></geometry>
|
||
<surface>
|
||
<friction><ode><mu>0.25</mu><mu2>0.25</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>
|
||
</surface>
|
||
</collision>
|
||
|
||
<!-- 台肩 -->
|
||
<collision name="collar">
|
||
<pose>0 0 0.0315 0 0 0</pose>
|
||
<geometry><cylinder><radius>0.008</radius><length>0.003</length></cylinder></geometry>
|
||
<surface>
|
||
<friction><ode><mu>0.25</mu><mu2>0.25</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>
|
||
</surface>
|
||
</collision>
|
||
|
||
<!-- 夹持段：夹爪夹这里，同样用软接触。
|
||
摩擦给 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.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>
|
||
</surface>
|
||
</collision>
|
||
|
||
<!-- 止端 NO-GO -->
|
||
<collision name="nogo_stud">
|
||
<pose>0 0 0.082 0 0 0</pose>
|
||
<geometry><cylinder><radius>0.004</radius><length>0.008</length></cylinder></geometry>
|
||
<surface>
|
||
<friction><ode><mu>0.25</mu><mu2>0.25</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>
|
||
</surface>
|
||
</collision>
|
||
|
||
<!-- =====================================================================
|
||
视觉
|
||
===================================================================== -->
|
||
<!-- 通端：深色（发黑处理的量规就是黑的），与工件的灰形成对比，
|
||
便于在 D405 画面里看清"规尖是不是对准了孔"。 -->
|
||
<visual name="go_stud_vis">
|
||
<pose>0 0 0.015 0 0 0</pose>
|
||
<geometry><cylinder><radius>0.003</radius><length>0.030</length></cylinder></geometry>
|
||
<material>
|
||
<ambient>0.08 0.08 0.09 1</ambient>
|
||
<diffuse>0.12 0.12 0.13 1</diffuse>
|
||
<specular>0.1 0.1 0.1 1</specular>
|
||
</material>
|
||
</visual>
|
||
<visual name="collar_vis">
|
||
<pose>0 0 0.0315 0 0 0</pose>
|
||
<geometry><cylinder><radius>0.008</radius><length>0.003</length></cylinder></geometry>
|
||
<material>
|
||
<ambient>0.7 0.7 0.68 1</ambient>
|
||
<diffuse>0.8 0.8 0.78 1</diffuse>
|
||
</material>
|
||
</visual>
|
||
<!-- 夹持段：亮色 + 滚花的感觉只用颜色区分（不建真实滚花网格，
|
||
它对物理与视觉判据都没有影响，只会让模型变重）。 -->
|
||
<visual name="grip_vis">
|
||
<pose>0 0 0.0555 0 0 0</pose>
|
||
<geometry><cylinder><radius>0.015</radius><length>0.045</length></cylinder></geometry>
|
||
<material>
|
||
<ambient>0.75 0.75 0.72 1</ambient>
|
||
<diffuse>0.85 0.85 0.82 1</diffuse>
|
||
<specular>0.3 0.3 0.3 1</specular>
|
||
</material>
|
||
</visual>
|
||
<!-- 止端：做成红色，一眼能区分通端/止端。
|
||
这不是装饰 —— 现场最容易犯的错就是把规装反（通端朝上），
|
||
红色让"哪头朝下"在预览里瞬间可判。 -->
|
||
<visual name="nogo_stud_vis">
|
||
<pose>0 0 0.082 0 0 0</pose>
|
||
<geometry><cylinder><radius>0.004</radius><length>0.008</length></cylinder></geometry>
|
||
<material>
|
||
<ambient>0.6 0.05 0.05 1</ambient>
|
||
<diffuse>0.8 0.08 0.08 1</diffuse>
|
||
</material>
|
||
</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>
|