arm_dev
25635e21f8
这一步解决的是"官方 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>
devices/sim/gazebo/ —— Gazebo 仿真平台
先读这段：当前仿真是假的
现在项目里的「仿真」并不是 Gazebo。 实际情况是：
src/arm_description/urdf/arm.gazebo.xacro是零引用的死文件；arm_bringup/stack.launch.py的isaac分支只有一行日志，没有任何实际动作；run_sim.sh走的是sim.launch.py，底层是 mock 驱动（mock_arm_driver.py）， 即用假数据模拟机器人状态，不是物理仿真；- Gazebo 从未真正接线。
所以这份文档分两部分：第一部分是怎么把 Gazebo 真正跑起来（当前必须做的前置工作）， 第二部分才是日常怎么用。不要跳过第一部分。
第一部分：把 Gazebo 真正接起来（前置工作）
要让 Gazebo 跑起来，缺的是这几块，按顺序补齐：
1. URDF 加 Gazebo 标签
现有 arm_description/urdf/arm.gazebo.xacro 是死文件，需要重写。每个 link 必须补：
<!-- 视觉碰撞体 -->
<gazebo reference="${prefix}link1">
<material>Gazebo/Grey</material>
</gazebo>
并在 arm.ros2_control.xacro 里把 ros2_control 标签的 hardware 插件从
flexiv_hardware/FlexivHardware 换成 gazebo_ros2_control/GazeboSystem。
2. 关节传动与控制器
Gazebo 需要 transmission 标签把关节映射到 ros2_control。控制器配置可以直接复用
arm_control/config/controllers.yaml 里的 rizon_arm_controller，只需把关节名前缀
从真实 SN（如 Rizon4-063989_）换成仿真专用的前缀（如 sim_）。
3. 传感器插件
| 仿真对象 | 插件 | 对应真实设备 |
|---|---|---|
| 腕部相机 | libgazebo_ros_camera.so |
D405 |
| 手外相机 | libgazebo_ros_camera.so |
D435 |
| 深度 | libgazebo_ros_depth_camera.so |
对齐深度流 |
| 触觉力 | 需要自己写插件 | Orisys |
最关键的一条：Orisys 视触觉没有现成的 Gazebo 插件。 3 路切向力 + 1 路法向力 + 深度图 + 力矢量图 这套数据需要自定义 Gazebo 插件来模拟。 在它做出来之前，仿真里无法验证通止规装配的力控部分（S07 接触、S08 拧紧判定）。 建议的自定义插件做法：订阅 Gazebo 的接触传感器数据，按接触深度和相对滑动速度合成 出一组「类触觉」读数，再按真实传感器的量纲（任意单位增量）发布到同样的内部话题上。
4. 工件与通止规模型
需要给工件（含 k0 中心孔、k1/k2 螺纹孔）和通止规建简化网格，并保证：
- 工件的 k0/k1/k2 三个点在模型坐标系里的位置与实际一致 —— 否则推理出的坐标映射到仿真里会对不上；
- 螺纹孔要能表达「拧入」的几何阻力，否则 S08 的力反馈无法触发。
第二部分：日常怎么用
放置格式与命名要求
| 类型 | 放哪 | 格式 | 命名要求 |
|---|---|---|---|
| 世界文件 | worlds/ |
.world 或 .sdf |
<场景名>.world，如 assembly.world |
| 机器人模型 | urdf/ |
.urdf / .xacro |
必须以 sim_ 为关节前缀，与真实臂区分 |
| 工件/夹具网格 | models/<模型名>/ |
<模型名>.sdf + meshes/ |
模型名必须与推理用的类别名一致，如 workpiece |
| 传感器插件 | sensor_plugins/ |
.so 源码 + CMakeLists.txt |
插件类名以 Orisys 开头 |
| 网格文件 | models/<模型名>/meshes/ |
.stl / .dae |
单位必须是米。用毫米建模的必须先缩放到米 |
导入一个新场景
- 把
.world放进worlds/，网格放进models/<模型名>/ - 改
config/site.yaml的sim.world指向该文件 - 改
devices/registry.yaml的sim: gazebo（通常已是） - 启动
scripts/run_sim.sh
修改哪个配置才会生效
| 想改什么 | 改哪里 |
|---|---|
| 用哪个场景 | site.yaml → sim.world |
| 仿真是否启用物理 | site.yaml → sim.physics_enable |
| 仿真里的悬高/速度 | 复用真实的那套参数（servo.* / hole_detect.stage2.*），不要另建一套，否则仿真调好的值搬到真机不生效 |
| 关节前缀 | site.yaml → sim.joint_prefix（默认 sim_） |
设计约束：仿真与真机必须共用同一套
site.yaml参数。任何「仿真专用参数」都是未来 仿真调参搬到真机失效的隐患。只允许物理引擎本身的参数（如重力、步长）放在sim.*下。