arm_dev
02516fe567
两条独立的静默坑，都不报错、不告警，只会"永远收不到数据"：
1) world 里没声明 contact-system 系统插件。
gz-sim 的 <sensor type="contact"> 不由 ignition-sensors 库实现，而是由
ignition-gazebo6-contact-system 插件实现 —— 所以环境里根本没有
libignition-sensors6-contact.so（相机/IMU/lidar 的后端都在，唯独缺它），
那不是包缺失。不声明插件的症状：话题不出现、日志空白。
实测：加上插件后 /tactile/left/raw、/tactile/right/raw 立刻出现。
2) 接触传感器的三个必备写法，错一个就静默失效：
- <contact><collision> 必须写 URDF→SDF 自动生成的 collision 名；
留空 <contact/> 会被补成 SDF 规范的默认值 __default__（不存在的
collision）。实测生成名：
sim_left_finger_mount_fixed_joint_lump__sim_left_finger_tip_collision
- 话题必须写在 <contact><topic> 里，写在外层 <sensor><topic> 无效，
gz 会静默改用默认 scoped 话题名。
- 传感器必须挂在有碰撞体的 link 上（指尖），不是另建的空 link。
两处均已按实测结果修正，并把"为什么这么写"写进文件注释。
Co-authored-by: Cursor <cursoragent@cursor.com>
205 lines
11 KiB
XML
205 lines
11 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:=640
|
||
image_height:=480
|
||
image_hz:=30
|
||
">
|
||
|
||
<!-- =======================================================================
|
||
传感器系统插件放在 **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 视触觉传感器
|
||
=======================================================================
|
||
真机上是视触觉：一块弹性体 + 内置相机，读出三向力、深度图、力矢量图。
|
||
仿真里没法真的模拟弹性体形变成像，采取的务实方案是：
|
||
用 Gazebo 内置的 contact sensor 取接触处的真实接触力，
|
||
再由 sim_tactile_adapter 把力投影到指尖坐标系，
|
||
按 Orisys 的 TactileStatus 消息格式发布出去。
|
||
|
||
⚠️ 这里**不再**新建空的 tactile link。
|
||
曾经的做法是建两个空的 sim_tactile_*_link（既无 visual 也无
|
||
collision），挂在 **法兰** 的 tcp 上（不是指尖），而 <contact> 里
|
||
又引用了一个 <collision>sim_tactile_left_link_collision</collision>
|
||
—— 那个 collision 元素根本不存在。三重错叠在一起，
|
||
结果是传感器**永远收不到任何数据**，而 Gazebo 不会为此报任何错：
|
||
话题在、没有输出、看起来像"还没接触"。极难查。
|
||
这也正是把传感器挂到真实碰撞体所在的 link（指尖）上的理由 ——
|
||
"接触到的就是读到的"，中间不再有需要对齐的第二套几何。
|
||
|
||
为什么要挂两个（left/right）：
|
||
真机是双指各贴一片，tangential_1/2 + normal 三路力分别来自两片，
|
||
单指的数据无法复现"旋拧时切向力从哪一侧来"这个判据。
|
||
|
||
⚠️ 单位不是牛顿！真机读数是**形变任意单位**、基线会漂移，
|
||
所有阈值都按"相对静止均值的增量"使用（见
|
||
devices/sensors/orisys/manifest.yaml）。仿真给的是牛顿，
|
||
所以两者的数值**不能直接共用阈值** —— 标度换算由
|
||
config/tactile.yaml 的 force_scale 负责，详见该文件与 README。
|
||
======================================================================= -->
|
||
<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>
|
||
</xacro:if>
|
||
|
||
</xacro:macro>
|
||
|
||
</robot>
|