arm_dev
01315773e7
链路：指尖关节 force_torque（持续发布）→ ros_gz_bridge → sim_tactile_adapter
→ /tactile/{left,right}/status。字段口径与真机 tactile_node 完全同构：
20Hz、frame_id=<side>_tactile、fnormal clamp 到非负、所有力都是"相对静止
基线的增量"。所以 compliant_descent 与 stage3_search_node 一行不改就能在
仿真里跑 —— 这是"同一套接口、换后端"契约的一部分。
新增 sim_tactile.py 把三件纯数据变换独立出来（轴向投影、桥接参数、基线
跟踪），因为这三处的错误全都**不会报错**，只会让力的语义静默错位：
轴向映射反了照样"有力、会响应、松开归零"，基线把接触吃掉了则表现为
"明明夹住了 fnormal 恒为 0"。抽出来才有单测可钉（新增 25 条）。
已实测验收（关闭夹爪夹静态薄板，60×120×80mm）：
空载 fnormal ~1e-8、contact=false（基线相减到位）
夹住 contact=true、fnormal=714.7/102.5，切向同时起变化
松开 contact 立刻回 false（靠 contact_timeout_s，因为 gz 的接触消息
是事件式的，不接触时一条都不发），力回落到 ~1e-8
启动期 check_tactile_collisions 无告警
check_tactile_collisions 顺带被负向单测逼出两处"会被静默骗过"的判据：
sdformat 对缺失的 contact/topic 会补 __default_topic__、对缺失的 collision
会补 __default__，所以只判"空"是抓不到的 —— 必须连占位值一起判。
force_scale 口径写入 config/tactile.yaml：仿真输出 = Gazebo 牛顿 × force_scale。
要共用真机那套 250/600 阈值必须先标定它，否则要么永不触发要么一碰就炸
（实测位置控制器硬压可到几百，量级与真机完全不同）。
Co-authored-by: Cursor <cursoragent@cursor.com>
272 lines
15 KiB
XML
272 lines
15 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 视触觉传感器
|
||
=======================================================================
|
||
真机上是视触觉：一块弹性体 + 内置相机，读出三向力、深度图、力矢量图。
|
||
仿真里没法真的模拟弹性体形变成像，采取的务实方案是**两个内置传感器协作**：
|
||
|
||
力的三分量 ← <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>
|