P5c-2：仿真世界 + 机器人描述 + 控制器配置，并解决两处官方描述与 Gazebo 的不兼容
这一步解决的是"官方 Flexiv 描述能不能喂给 Gazebo"的问题。结论是**不能直接用**，
有两处不兼容，且都只有 Gazebo 会暴露 —— RViz/MoveIt 完全正常，所以此前从未发现。
问题一：7 轴本体的质量/惯量是"孤儿标签"
rizon_arm.xacro 调 get_inertias 时没写 <inertial> 包裹，而那个宏只吐
<origin>/<mass>/<inertia> 三个碎片。RViz/MoveIt 忽略不认识的子元素所以看着
没事；sdformat 判定 "link has no <inertial> block"，然后把 **link 和它的父
关节一起丢掉**。症状极具误导性：机械臂凭空消失或只剩夹爪，报错只有一句
"link[sim_link1] is not modeled in sdf"。
问题二：夹爪有零质量 link，导致 sdformat 惯性叠加出 NaN
grav_macro.xacro 给六个纯坐标系 dummy link 写了 mass=0、惯量全 0。
sdformat 对 fixed 关节会把子 link 惯性叠加到父 link，叠加要除以总质量 →
质量 0 就出 NaN。而报错指向的是**父** link：
Pose values must be finite. link[sim_left_finger_mount]/inertial/pose
完全看不出根因在子 link 的质量上。
修法：启动时归一化（不走改上游文件的路，因为上游由 fetch_flexiv.sh 获取、改了会被
覆盖；而且换别的臂时大概率有别的坑，在启动时统一过一道更可持续）
- 孤儿三件套 → 只重新分组已有元素，一个数字都不改
- 零质量 → 补 1e-4 kg（与夹爪其他配件同量级，0.1g 对动力学无影响），
惯量全 0 时补极小对角值避免张量退化
不发明物理数据，也不掩盖真问题：上游真没给质量时这里什么都不会包。
16 项单测，含"已有 <inertial> 不能被二次包裹"（早期用全局正则踩过，
sdformat 报 "Inertial element must have a mass element"）与幂等性。
新增/改动
- devices/sim/gazebo/urdf/：arm.sim.urdf.xacro（主）、.ros2_control（gz 硬件）、
.gazebo（系统插件/材质/接触刚度）、.sensors（D405 + 双指触觉）
- devices/sim/gazebo/worlds/assembly.world：1ms 步长 + 150 次接触迭代（力信号
要干净才谈得上"切向力突变"判据）、工作台、world 级渲染插件、D435
- devices/sim/gazebo/config/controllers.yaml：控制器名 rizon_arm_controller
**故意与真机一致** —— 隔离的是关节/TF 层（sim_ 前缀），不是应用层依赖的接口
- src/arm_sim_gazebo/：新包，含 urdf_normalize（库 + CLI）与单测
一处设计纠正：D435 从 URDF 挪进 world。原本挂在 URDF 的 world link 上，结果
整台相机转 SDF 时凭空消失（sdformat 丢弃无质量根 link 时连带丢了整棵子树，
且退出码 0、不报错）。眼在手外的相机物理上就属于场景，放 world 才对；
渲染插件也一并归 world 级，避免"插件在模型里、相机在世界里"的错配。
验证（不是只看退出码）
- 带夹爪全量：sdformat 退出码 0、0 报错、15 link / 15 joint / 26 frame
- 传感器 d405_rgbd + tactile_left/right 均在；控制器插件与参数路径正确
- 世界文件 sdformat 通过
- 包在 ros_gz_sim 环境构建通过，CLI 实跑结果与库调用一致
- 未注册 gazebo_sim_launcher 入口点：模块还没写，注册了构建能过但一运行就
ImportError，不如老实缺着（manifest 里如实标注为未实现）
注：实测确认 gz_ros2_control 的硬件/接口声明**不在 SDF 里**，插件是从
/robot_description 话题读 URDF 的 <ros2_control> —— 所以启动器必须先起
robot_state_publisher 且发布归一化后的同一份 URDF，否则控制器注册的关节名
会和 Gazebo 里的对不上。这条已写进 P5c-3 的实现要求。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -0,0 +1,189 @@
|
||||
<?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 取接触点的三向力，
|
||||
再由自定义插件（devices/sim/gazebo/sensor_plugins/）把它
|
||||
按 Orisys 的 TactileStatus 消息格式发布出去，
|
||||
含切向/法向分解，使 P6 的力判据在仿真里可跑。
|
||||
|
||||
这里只挂 contact sensor 与自定义插件的位置占位；
|
||||
插件的实现与消息映射在 P5c-5 完成。
|
||||
|
||||
为什么要挂两个（left/right）：
|
||||
真机是双指各贴一片，tangential_1/2 + normal 三路力分别来自两片，
|
||||
单指的数据无法复现"旋拧时切向力从哪一侧来"这个判据。
|
||||
======================================================================= -->
|
||||
<xacro:if value="${tactile_enable}">
|
||||
<link name="${prefix}tactile_left_link"/>
|
||||
<link name="${prefix}tactile_right_link"/>
|
||||
|
||||
<joint name="${prefix}tactile_left_joint" type="fixed">
|
||||
<parent link="${prefix}tcp"/>
|
||||
<child link="${prefix}tactile_left_link"/>
|
||||
<origin xyz="0 0.035 0.005" rpy="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="${prefix}tactile_right_joint" type="fixed">
|
||||
<parent link="${prefix}tcp"/>
|
||||
<child link="${prefix}tactile_right_link"/>
|
||||
<origin xyz="0 -0.035 0.005" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<gazebo reference="${prefix}tactile_left_link">
|
||||
<sensor name="tactile_left" type="contact">
|
||||
<always_on>true</always_on>
|
||||
<update_rate>100</update_rate>
|
||||
<!-- 话题名与真机 tactile_node 发布的完全一致：
|
||||
/tactile/left/status (arm_msgs/TactileStatus)
|
||||
这样 P6 的状态机在仿真/真机之间不用改一行判断。 -->
|
||||
<contact>
|
||||
<collision>${prefix}tactile_left_link_collision</collision>
|
||||
</contact>
|
||||
<topic>/tactile/left/raw</topic>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="${prefix}tactile_right_link">
|
||||
<sensor name="tactile_right" type="contact">
|
||||
<always_on>true</always_on>
|
||||
<update_rate>100</update_rate>
|
||||
<contact>
|
||||
<collision>${prefix}tactile_right_link_collision</collision>
|
||||
</contact>
|
||||
<topic>/tactile/right/raw</topic>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
</xacro:if>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
Reference in New Issue
Block a user