arm_dev
d32e6ae53f
一、场景物料（新增）
devices/sim/gazebo/models/workpiece/ 带三个孔的铝板。碰撞体是"带方孔的平板"，
视觉额外加两个 φ14/φ7 亮环 —— 因为真机的 HoughCircles 就是靠这两个亮环找孔，
只按碰撞体建视觉会让"孔检测"在仿真里永远失败，而那看起来像算法问题。
devices/sim/gazebo/models/gauge/ 通止规：通端/套环/握柄/止端四段，各段异色
便于肉眼判断方向。接触刚度刻意比台面软 —— 位置控制器会"刚性压进去"，
用默认刚度会被弹飞（注释里写了原因，不是随手抄的参数）。
devices/sim/gazebo/models/gauge_stand/ 规座（static）。规直接站在台面上会立刻倒，
加个方孔底座把它立住。
三个模型都是 model.sdf + model.config，原点在**底面中心**，单位米。
二、场景配置与校验（新增）
config/scene.yaml 物体不再写死在 world 里，改为 scene.yaml + 启动时动态 spawn。
理由：world 是"环境"（地面/台面/相机/系统插件），场景是"这一批活做什么"。
动 world 的风险面太大 —— 它只要丢一个系统声明，所有模型都 spawn 不进去（见旧坑 2）。
含 operating（相机到工件的距离等作业条件）与 gates_px（像素门槛）。
sim_scene.py 摆放校验：从 SDF **逐碰撞体**解析 AABB（不抄尺寸，避免漂移），
检查原点在底面 / 不穿台面 / 不出界 / 物体之间不嵌入。
重叠判定必须逐碰撞体三维判，不能拿总包围盒相交 ——
正常装配里本来就有嵌套（规立在规座方孔里），粗判会误报，
而误报的后果是人把检查整个关掉，之后真重叠也抓不到。
另有成像几何：把亮环/孔间距按相机距离换算成像素，与门槛比对。
scripts/check_sim_scene.py 验收工具（退出码可用）：摆放 + 门槛漂移 + 成像几何 +
悬停高度 vs 工作空间 + 手外相机是否**完整**看得见工件 + Gazebo 实际位姿。
三、修掉两处静默失效（都是"看起来一切正常"）
1) parse_spawned_poses 一直是**死代码**：它按 "x: N / y: N / z: N" 解析，
而 Fortress 实际输出是方括号两行（XYZ / RPY）。对不上 → 永远返回空 →
调用方按设计"降级成跳过位姿核对" → 位姿检查**从来没跑过**，
而日志只说"跳过"，看起来像正常降级。
现在两种格式都支持，并用真实抓下来的输出样本做回归测试。
教训：宽容降级会把"没检查"伪装成"检查通过"。
2) stop_sim_gz.sh 的模式表里漏了 robot_state_publisher（而注释里写着它有覆盖）：
脚本报"已停止"，它却还在发上一份 /robot_description 与 TF，
下一次启动就会"改了描述却不生效"。
顺带修掉同源的两个问题：awk 用 -v 传模式导致它匹配到自己、每次复查都多报
一个空命令行的幽灵 PID；以及"杀的时候"和"复查的时候"两份扫描实现会各自漂移。
现在共用同一个 scan_pids()，模式改用环境变量传递，并在杀完之后**再扫一遍**：
有残留就报出来并非零退出 —— 下次再有漏项，不再静默。
实测：--list 列出全套 9 个进程；停止后 rc=0 且 pgrep 干净。
四、相机：D405 分辨率对齐真机
仿真的 D405 此前渲染 640x480，而真机跑 848x480（fx 337 而非 447）。后果是
58mm 的两孔间距成像只剩 85px，低于 hole_distance_min=90 ——
孔检测在仿真里根本不可能通过，而现象看起来像"算法不行"。
已把 xacro 与 cameras.yaml 一起改成 848x480，并把启动时的比对从"只比视场"
扩成"视场 + 图幅"（sim_cameras.check_camera_consistency），
不一致时说明它对像素门槛的影响。
五、文档
devices/sim/gazebo/README.md 重写为中文手册：使用方法 / 建模要求 / 导入格式 /
配置方式（含"改哪里才真的生效"的接线表）/ 11 条实测坑（每条都有"怎么发现"）。
manifest.yaml 的进度、assets、description、坑清单与本轮状态对齐。
scripts/README.md 补两个验收工具与停止脚本；根 README 补仿真入口与手册链接。
cameras.yaml 里一处过期的函数名（check_fov_consistency）修正。
六、测试
新增 test_sim_scene.py（79 条，此前 sim_scene.py 完全没有单测 —— 这正是
位姿解析死掉却无人发现的原因）与 test_check_sim_scene.py（21 条），
含三条回归：world_boxes 的偏移碰撞体不能被对称化放大（假重叠）、
center_bore 不能被 _bore 后缀吃掉、Fortress 位姿输出格式。
全量：src/arm_sim_gazebo + arm_interfaces + arm_application 共 515 通过。
同时附上 config/site.example.yaml 的重新生成（此前 site.yaml 的注释改动没同步，
它的 parity 测试是红的）。
Co-authored-by: Cursor <cursoragent@cursor.com>
194 lines
9.4 KiB
XML
194 lines
9.4 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.4：真机夹爪指垫是橡胶/聚氨酯，比金属-金属的 0.25 高。
|
||
给太低（<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>
|
||
<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>
|
||
</model>
|
||
</sdf>
|