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。
371 lines
22 KiB
XML
371 lines
22 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
|
||
wrist_ft_enable:=true
|
||
wrist_ft_topic:='/sim/ft_sensor_wrench'
|
||
d405_x:=0.0
|
||
d405_y:=-0.06
|
||
d405_z:=0.05
|
||
d405_roll:=0.0
|
||
d405_pitch:=-1.5708
|
||
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 —— 眼在手上
|
||
=======================================================================
|
||
位姿默认值说明（法兰坐标系）：
|
||
xyz: x=0, y=-0.06, z=+0.05
|
||
z=+0.05 相机略微伸出法兰，避免看到夹爪本体
|
||
y=-0.06 偏置到一侧，与真机上的支架安装位置一致
|
||
rpy: roll=0, **pitch=-1.5708(-90°)**, yaw=0
|
||
这些值**不是**真机的标定结果，只是仿真里的合理起始位。
|
||
仿真里"手眼关系"本来就精确已知（我们直接定义了相机的安装位姿），
|
||
所以不需要跑手眼标定 —— 但保留了可调位姿，方便模拟"标定不准"的情况，
|
||
验证跟踪算法对误差的容忍度。
|
||
|
||
⚠️⚠️ pitch 这一项是**必须**的，不是"随手调的角度"。原因在 Gazebo 的
|
||
相机惯例：**传感器/模型的视轴是 +X**（不是 +Z）。世界相机 d435 就是靠
|
||
在 world 文件里给它模型 pitch=50° 才瞄准台面的 —— 同一个惯例。
|
||
腕相机要在法兰垂直朝下时看工件，就得让 d405_link 的 +X 对上法兰的 +Z，
|
||
即绕 Y 转 -90°。
|
||
|
||
为什么"少转这 90°"极难发现（曾经默认 0，踩过）：
|
||
· 话题有数据、预览有图像、camera_info 内参也对；
|
||
· 深度反投影甚至"通过"了检查 —— 因为它看到的是**远处的地面**，
|
||
反投影出来同样是 z≈0 的水平面（check_sim_cameras 当时只看
|
||
"命中不用平面"，判据太松，现在已改成核对视轴朝向）；
|
||
· 症状只出现在应用侧：S04 跟踪里真值/检测永远说"目标不在相机前方"，
|
||
伺服一直"寻找工件"原地转圈 —— 看起来像算法或标定问题。
|
||
所以：改这里之前先想清楚视轴指向，改完跑
|
||
python3 scripts/check_sim_cameras.py
|
||
它会核对"腕部相机光学系的视轴是否与法兰 +Z 一致"。
|
||
======================================================================= -->
|
||
<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>
|
||
|
||
<!-- =======================================================================
|
||
腕部力扭矩（wrist force/torque）—— 与手指触觉是**两回事**，别互相替代
|
||
=======================================================================
|
||
为什么必须有它（2026-09-24 踩过，代价是整整两轮排查方向错）：
|
||
|
||
"通止规"是**靠夹持摩擦抱住**的工具。轴向力（往下压的力）要传到手指上，
|
||
必须经过"指垫↔规"的摩擦面，而摩擦能传的上限是 µ·N：
|
||
夹持段 µ=0.25、实测握力 N≈46N → 单指 ~11.5N、两指 ~23N
|
||
台肩一落面，接触刚度 5e4 N/m 意味着**过压 0.5mm 就是 25N** —— 一步
|
||
步进量就足以让规在爪里打滑。所以手指触觉**根本读不到**"台肩落面"那
|
||
一下的力（读数停在 0，而它是对的）。拿手指触觉当"到底了"的判据，
|
||
阈值调多大都不对：调小了被噪声骗，调大了物理上永远达不到。
|
||
|
||
腕部 FT 装在这条力路的**上游**：工件→规→夹爪→法兰→腕部关节。
|
||
工具打不打滑都要经过它，所以它测得到真实的轴向力。真机同理 ——
|
||
真机上这个量由 Flexiv 的 flexiv_robot_states_broadcaster 发在
|
||
/<sn>/ft_sensor_wrench（geometry_msgs/WrenchStamped，法兰系）。
|
||
|
||
为什么挂在 ${prefix}joint7 而不是 flange_joint：
|
||
flange_joint 是 **fixed** 关节，物理引擎不会把它当独立关节求解
|
||
（它是刚体的一部分），挂上去的 force_torque 传感器永远没有输出、
|
||
而且**不报任何错**。joint7 是最后一个 revolute 关节，它下游挂着
|
||
link7 + flange + 夹爪 + 工具 —— 正好是"整个腕部以下"的全部载荷，
|
||
这才是工程意义上的腕部 FT。
|
||
为什么是 frame=child：
|
||
child 是关节的下游连杆（link7）。工具竖直朝下时 link7 的 z 轴
|
||
就是工具轴，所以 **z 分量 = 轴向力**，与真机 ft_sensor_wrench
|
||
（法兰系，z=法兰轴）口径一致 —— 应用层因此可以两边共用一套判据。
|
||
|
||
⚠️ 话题名必须与"应用层按 sn 推出的那个名字"一致：
|
||
仿真臂的 sn 固定是 `sim`（见 config/site.yaml 的 robot.arms[sim]），
|
||
应用层算 `/{sn}/ft_sensor_wrench` = /sim/ft_sensor_wrench。
|
||
改 sn 时必须同步改这里，否则只是"话题上没有数据"（不报错）。
|
||
-->
|
||
<xacro:if value="${wrist_ft_enable}">
|
||
<gazebo reference="${prefix}joint7">
|
||
<sensor name="wrist_ft" type="force_torque">
|
||
<always_on>true</always_on>
|
||
<!-- 与真机 flexiv_robot_states_broadcaster 的发布率(100Hz)对齐：
|
||
下游按"够不够新"判活，率太低会被当成掉线。 -->
|
||
<update_rate>100</update_rate>
|
||
<topic>${wrist_ft_topic}</topic>
|
||
<force_torque>
|
||
<frame>child</frame>
|
||
<measure_direction>parent_to_child</measure_direction>
|
||
</force_torque>
|
||
</sensor>
|
||
</gazebo>
|
||
</xacro:if>
|
||
|
||
</xacro:macro>
|
||
|
||
</robot>
|