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>
300 lines
17 KiB
XML
300 lines
17 KiB
XML
<?xml version="1.0"?>
|
||
<!--
|
||
=============================================================================
|
||
外挂传感器：眼在手上的相机 + 双指触觉。
|
||
|
||
分工（按"装在哪"划分，不是按品牌）：
|
||
D405 眼在手上（eye-in-hand）—— 装在法兰上，跟着手走 → 属于**机器人**，
|
||
所以在这个文件里，跟着 URDF 一起展开
|
||
D435 眼在手外（eye-to-hand）—— 装在工作台支架上，俯视台面 → 属于**场景**，
|
||
所以在 worlds/assembly.world 里（每台相机只出一份定义，不重复）
|
||
|
||
为什么 D435 不能挂在 URDF 的 world link 上（实测踩过）：
|
||
挂了之后整台相机在转 SDF 时凭空消失。sdformat 会丢弃无质量的根 link，
|
||
连带它整棵子树；而它**退出码 0、不报任何错**，只是 SDF 里再也找不到
|
||
d435 的 frame 和 sensor，启动后话题永远没数据。
|
||
另一个理由：渲染系统插件必须是 world 级的，相机和插件放一起最不容易错配。
|
||
|
||
=============================================================================
|
||
和真机的对应关系（重要）
|
||
=============================================================================
|
||
真机上是 RealSense，出 RGB + 深度。仿真用 rgbd_camera 同时给两者，
|
||
话题名对齐 ros_realsense，应用层（跟踪、粗定位）才能不加分支地两边通用：
|
||
真机 / 仿真： /d405/color/image_raw + /d405/depth/image_rect_raw
|
||
换相机型号时只改这里的尺寸/内参，应用层不用动。
|
||
|
||
============================================================================
|
||
插件文件名
|
||
============================================================================
|
||
Fortress 的传感器系统插件是 libignition-gazebo6-sensors-system.so，
|
||
名称 ignition::gazebo::systems::Sensors。实测确认过：
|
||
ls $CONDA_PREFIX/lib/libignition-gazebo6-sensors-system.so
|
||
换到更新版本（Harmonic+）后名字会变成 libgz-sim*-sensors-system.so，
|
||
且插件名去掉 ignition:: 前缀 —— 那时要改这里。
|
||
=============================================================================
|
||
-->
|
||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||
|
||
<xacro:macro name="arm_sim_sensors"
|
||
params="
|
||
prefix
|
||
sensors_file:=''
|
||
d405_enable:=true
|
||
tactile_enable:=true
|
||
d405_x:=0.0
|
||
d405_y:=-0.06
|
||
d405_z:=0.05
|
||
d405_roll:=0.0
|
||
d405_pitch:=0.0
|
||
d405_yaw:=0.0
|
||
image_width:=848
|
||
image_height:=480
|
||
image_hz:=30
|
||
">
|
||
|
||
<!-- =======================================================================
|
||
图像尺寸（image_width / image_height）—— **必须与真机一致**
|
||
=======================================================================
|
||
⚠️ 注意：这段说明**不能**写成上面 params="..." 里的 XML 注释 ——
|
||
XML 不允许在属性值里放注释，那样整个文件会 not well-formed，
|
||
xacro 直接展开失败（踩过）。所以只能放在这里。
|
||
|
||
真机 D405 跑 848x480@30（见 config/site.yaml 的 camera.width/height）。
|
||
仿真也必须 848x480，理由是**像素尺度的可迁移性**：
|
||
|
||
阶段二的孔检测（hole_detect_hough）直接在**原始分辨率**图上跑
|
||
HoughCircles，不做缩放。所以 hole_detect.yaml 里那些以像素为单位的
|
||
门槛（hough_min_radius 10 / min_radius 10 / hole_distance_min 90 …）
|
||
只有在"每毫米对应多少像素"与真机相同时才有意义：
|
||
fx = width / (2·tan(hfov/2))
|
||
视场角两边都是 87°，所以像素尺度完全由 width 决定：
|
||
真机 848 → fx ≈ 447
|
||
曾用 640 → fx ≈ 337（每毫米像素数只有真机的 75%）
|
||
用 640 的后果很具体：58mm 的两孔间距在真机上成像约 152px（过 90 的门槛），
|
||
在仿真里只剩 115px —— 仍是能过，但整套像素门槛的余量都被吃掉 25%，
|
||
而"看起来不对"的表现只是"孔检测偶尔不认"，极难归因。
|
||
所以在仿真侧就把尺度对齐，别留给调试。
|
||
|
||
启动时会比对这里与 cameras.yaml 的 width/height，不一致会告警
|
||
（见 sim_cameras.check_camera_consistency）—— 图幅写错不报错，
|
||
只会让像素门槛静默失效，所以要主动查。
|
||
======================================================================= -->
|
||
|
||
<!-- =======================================================================
|
||
传感器系统插件放在 **world 文件**里，不放这里
|
||
=======================================================================
|
||
渲染是所有传感器共用的世界级能力，而且 D435 在 world 里（见下）。
|
||
插件与相机分处两地的话，很容易出现"插件在模型里、相机在世界里"
|
||
这种错配 —— 症状是话题永远没数据且不报错。
|
||
所以统一由 worlds/assembly.world 声明一次，机器人上的 D405
|
||
和触觉也由它一并渲染。
|
||
======================================================================= -->
|
||
|
||
<!-- =======================================================================
|
||
RealSense D405 —— 眼在手上
|
||
=======================================================================
|
||
位姿默认值说明（法兰坐标系，z 朝法兰外侧）：
|
||
z=+0.05 相机略微伸出法兰，避免看到夹爪本体
|
||
y=-0.06 偏置到一侧，与真机上的支架安装位置一致
|
||
这些值**不是**真机的标定结果，只是仿真里的合理起始位。
|
||
仿真里"手眼关系"本来就精确已知（我们直接定义了相机的安装位姿），
|
||
所以不需要跑手眼标定 —— 但保留了可调位姿，方便模拟"标定不准"的情况，
|
||
验证跟踪算法对误差的容忍度。
|
||
======================================================================= -->
|
||
<xacro:if value="${d405_enable}">
|
||
<link name="${prefix}d405_link"/>
|
||
|
||
<joint name="${prefix}d405_joint" type="fixed">
|
||
<parent link="${prefix}flange"/>
|
||
<child link="${prefix}d405_link"/>
|
||
<origin xyz="${d405_x} ${d405_y} ${d405_z}"
|
||
rpy="${d405_roll} ${d405_pitch} ${d405_yaw}"/>
|
||
</joint>
|
||
|
||
<gazebo reference="${prefix}d405_link">
|
||
<sensor name="d405_rgbd" type="rgbd_camera">
|
||
<update_rate>${image_hz}</update_rate>
|
||
<always_on>true</always_on>
|
||
<!-- visualize=false：headless 下没有 GUI，开着只会白耗渲染资源 -->
|
||
<visualize>false</visualize>
|
||
<camera name="d405">
|
||
<!-- 水平视场角。D405 的实际值约 87°(H) × 58°(V)，这里取近似 -->
|
||
<horizontal_fov>1.5184</horizontal_fov>
|
||
<image>
|
||
<width>${image_width}</width>
|
||
<height>${image_height}</height>
|
||
<format>R8G8B8</format>
|
||
</image>
|
||
<clip>
|
||
<!-- 近裁剪面必须小于相机到工件的最近距离，否则工件会被裁掉 -->
|
||
<near>0.02</near>
|
||
<far>10.0</far>
|
||
</clip>
|
||
<depth_camera>
|
||
<clip><near>0.02</near><far>10.0</far></clip>
|
||
</depth_camera>
|
||
</camera>
|
||
<!-- 话题名对齐真机 ros_realsense 的命名，应用层两边通用 -->
|
||
<topic>/d405</topic>
|
||
</sensor>
|
||
</gazebo>
|
||
</xacro:if>
|
||
|
||
<!-- =======================================================================
|
||
RealSense D435 —— 眼在手外：**不在这个文件里**
|
||
=======================================================================
|
||
它挂过 world link（URDF 里那个空 link），结果整台相机在转换到 SDF 时
|
||
凭空消失：sdformat 会丢掉无质量的根 link，连带它整棵子树。
|
||
现象极具误导性 —— sdformat 退出码 0、不报任何错，只是 SDF 里
|
||
d435 的 frame 和 sensor 都不存在，启动后话题永远没数据。
|
||
|
||
正确位置是 world 文件（worlds/assembly.world）：眼在手外的相机在物理上
|
||
就是装在工作台支架上、不属于机械臂。放那里还有个好处 ——
|
||
渲染系统插件必须是 world 级的，相机和插件在一起，不会再出现
|
||
"插件在模型里、相机在世界里"这种错配。
|
||
======================================================================= -->
|
||
|
||
<!-- =======================================================================
|
||
Orisys 视触觉传感器
|
||
=======================================================================
|
||
真机上是视触觉：一块弹性体 + 内置相机，读出三向力、深度图、力矢量图。
|
||
仿真里没法真的模拟弹性体形变成像，采取的务实方案是**两个内置传感器协作**：
|
||
|
||
力的三分量 ← <sensor type="force_torque"> 挂在**指尖关节**上
|
||
是否接触 ← <sensor type="contact"> 挂在**指尖 link** 上
|
||
接触位置 ← 同上，取接触点坐标 → 映射成 centroid_u/v
|
||
|
||
⚠️ 为什么力必须用 force_torque，而不是从接触传感器拿 —— 这条很反直觉，
|
||
而且已经为此走过一次弯路，务必看完：
|
||
|
||
gz.msgs.Contacts（接触传感器发的消息）**不带任何力**。它的
|
||
Contact 消息体里确实有 wrench/force 字段（ros_gz_interfaces/Contact.msg
|
||
也照着声明了 wrenches），但 Fortress 的 Contact 系统**从不填充它们**：
|
||
实测一条接触消息全文只有 header + 碰撞体名 + 接触点坐标，
|
||
连 normal / depth 都是空的。而 Contact 系统的二进制里连一个
|
||
"wrench" 字符串都搜不到 —— 所以这不是配置问题，是它设计上就只有位置。
|
||
|
||
正确的力来源是关节：**转动关节在物理上要承受三个方向的约束力**
|
||
（它只放开绕轴转动这一个自由度）。所以挂在指尖关节上的
|
||
force_torque 传感器，读到的就是"整条手指链传过来的力" ≈ 接触力
|
||
（相差一个手指自身重力的偏置，正好由适配器做基线相减消掉，
|
||
这与真机 Orisys "相对静止基线取增量"的口径天然一致）。
|
||
由 Gazebo 内置的 forcetorque-system 实现，**无需自写 C++ 插件**。
|
||
|
||
⚠️ URDF 语法里没有"关节传感器"。这里靠 sdformat 的扩展写法实现：
|
||
<gazebo reference="关节名"><sensor .../></gazebo>
|
||
sdformat 的 URDF 解析器会把这类 sensor **移进对应的 <joint> 块**
|
||
（已实测确认，生成的 SDF 里传感器确实在 joint 内）。
|
||
|
||
为什么要挂两个（left/right）：
|
||
真机是双指各贴一片，tangential_1/2 + normal 三路力分别来自两片，
|
||
单指的数据无法复现"旋拧时切向力从哪一侧来"这个判据。
|
||
|
||
⚠️ 单位不是牛顿！真机读数是**形变任意单位**、基线会漂移，
|
||
所有阈值都按"相对静止均值的增量"使用（见
|
||
devices/sensors/orisys/manifest.yaml）。仿真给的是牛顿，
|
||
所以两者的数值**不能直接共用阈值** —— 标度换算由
|
||
devices/sim/gazebo/config/tactile.yaml 的 force_scale 负责，
|
||
该文件头把两种用法讲清楚了。
|
||
|
||
⚠️ 三向力的轴映射（哪根轴是指腹法向）是**实测**出来的，不是读代码推的，
|
||
结论与实测方法记在同一个 yaml 的 axis: 段里。改夹爪/换传感器后
|
||
要重测，否则力的语义会静默错位（有数值、但方向没意义）。
|
||
======================================================================= -->
|
||
<xacro:if value="${tactile_enable}">
|
||
<gazebo reference="${prefix}left_finger_tip">
|
||
<sensor name="tactile_left" type="contact">
|
||
<always_on>true</always_on>
|
||
<update_rate>100</update_rate>
|
||
<contact>
|
||
<!-- ⚠️ 这个名字不是我们起的，是 sdformat 从 URDF 转 SDF 时**自动生成**的：
|
||
规则 =「fixed 关节链的父 link」+ "_fixed_joint_lump__" +「本 link」
|
||
+ "_" + collision 名。本机实测生成名（2026-09-23，Fortress 6.16）：
|
||
sim_left_finger_mount_fixed_joint_lump__sim_left_finger_tip_collision
|
||
|
||
为什么不得不写死一个"自动生成的名字"（很脆，但有理由）：
|
||
a) URDF 的 <collision> 不能命名，所以没法给它一个稳定名字；
|
||
b) 留空 <contact/> 不会"监测全部"，而是被补成 SDF 规范的默认值
|
||
`collision=__default__` —— 一个不存在的 collision。
|
||
症状是传感器**永远没有输出**，gz 不报错、话题也不出现。
|
||
已实测确认（这是本节最大的一条坑）。
|
||
所以只能写死，并用启动检查兜住改名：
|
||
robot_description.check_tactile_collisions() 会把展开后的
|
||
URDF 转成 SDF，核对这两个名字是否存在 —— 一旦 sdformat 改了命名
|
||
规则，启动时就会明确报错，而不是静默失效。 -->
|
||
<collision>${prefix}left_finger_mount_fixed_joint_lump__${prefix}left_finger_tip_collision</collision>
|
||
<!-- ⚠️ 话题必须写在**这里**（contact/topic），不能写在外层的
|
||
<sensor><topic> 里 —— 接触传感器只认 contact/topic。
|
||
写错位置不会报错：gz 会静默改用默认的 scoped 话题名
|
||
/world/<world>/model/<model>/link/<link>/sensor/<sensor>/contact
|
||
于是"按 /tactile/left/raw 去订阅"永远收不到东西。实测确认。 -->
|
||
<topic>/tactile/left/raw</topic>
|
||
</contact>
|
||
</sensor>
|
||
</gazebo>
|
||
|
||
<gazebo reference="${prefix}right_finger_tip">
|
||
<sensor name="tactile_right" type="contact">
|
||
<always_on>true</always_on>
|
||
<update_rate>100</update_rate>
|
||
<contact>
|
||
<collision>${prefix}right_finger_mount_fixed_joint_lump__${prefix}right_finger_tip_collision</collision>
|
||
<topic>/tactile/right/raw</topic>
|
||
</contact>
|
||
</sensor>
|
||
</gazebo>
|
||
|
||
<!-- =====================================================================
|
||
三向力：指尖关节上的 force_torque 传感器
|
||
=====================================================================
|
||
挂在**指尖关节**（left/right_inner_finger_joint）上，而不是指尖 link ——
|
||
force_torque 在 SDF 里只能属于关节。这个关节是手指链上最靠近指尖的
|
||
一个（child = finger_mount，tip 用固定关节连在它下面），
|
||
所以它传过去的力就是整条手指链的力 ≈ 接触力。
|
||
|
||
三个非轴方向的约束力就是我们要的"三向力"：
|
||
frame=child → 力的表达在 finger_mount 坐标系里
|
||
measure_direction=parent_to_child
|
||
→ 读"父 link 施加给子 link 的力"。
|
||
静止时它等于 -(接触力 + 手指自重)，
|
||
所以真正符号如何、哪根轴对应"法向"，
|
||
以实测为准（见 devices/sim/gazebo/config/
|
||
tactile.yaml 的
|
||
axis_* 映射项与 README 的实测记录）。
|
||
|
||
⚠️ 这个传感器是**持续发布**的（即使没有任何接触，也按 update_rate
|
||
发零力）。这一点很关键：接触传感器只在接触时发消息，如果适配器
|
||
只靠接触消息驱动，那么"离开工件"这段时间下游会认为话题断流。
|
||
真机 Orisys 也是持续发布的，所以这里必须与真机一致。
|
||
===================================================================== -->
|
||
<gazebo reference="${prefix}left_inner_finger_joint">
|
||
<sensor name="tactile_left_wrench" type="force_torque">
|
||
<always_on>true</always_on>
|
||
<update_rate>100</update_rate>
|
||
<!-- force_torque 的话题写在外层 <topic> 即可（这点与 contact 不同：
|
||
contact 只认 contact/topic，写外层会被静默忽略）。 -->
|
||
<topic>/tactile/left/wrench</topic>
|
||
<force_torque>
|
||
<frame>child</frame>
|
||
<measure_direction>parent_to_child</measure_direction>
|
||
</force_torque>
|
||
</sensor>
|
||
</gazebo>
|
||
|
||
<gazebo reference="${prefix}right_inner_finger_joint">
|
||
<sensor name="tactile_right_wrench" type="force_torque">
|
||
<always_on>true</always_on>
|
||
<update_rate>100</update_rate>
|
||
<topic>/tactile/right/wrench</topic>
|
||
<force_torque>
|
||
<frame>child</frame>
|
||
<measure_direction>parent_to_child</measure_direction>
|
||
</force_torque>
|
||
</sensor>
|
||
</gazebo>
|
||
</xacro:if>
|
||
|
||
</xacro:macro>
|
||
|
||
</robot>
|