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/ —— 设备与外部文件总目录
这个目录存在的唯一目的：把「换个硬件」从改代码变成改配置。
设计原则（务必先读）
1. 选型与数值分离
| 关注点 | 放在哪 | 谁改 |
|---|---|---|
| 用哪个品牌的哪个驱动 | devices/registry.yaml |
集成工程师（很少改） |
| 设备号 / 速度 / 阈值 / 悬高 | config/site.yaml |
现场操作人员（经常改） |
devices/ 下不允许出现任何现场数值。 数值只有 config/site.yaml 一个来源。
这样可以避免现在这种「同一个值三处定义、改了不生效」的问题。
2. 目录约定
devices/
├── registry.yaml # 选型开关（唯一入口）
├── robots/ # 机械臂：RDK + 适配器
├── grippers/ # 夹爪
├── sensors/
│ └── orisys/ # ★ 视触觉核心，长期迭代，单独占一个大文件夹
├── cameras/ # 相机
└── sim/ # 仿真平台（当前 Gazebo）
每个品牌一个子目录，子目录内固定结构：
<类别>/<品牌>/
├── manifest.yaml # 我是谁 / 支持什么能力 / 需要哪些配置 / 外部文件放哪
├── driver/ # 适配器（把厂商 API 翻译成框架接口）
└── <厂商物料>/ # 如 rdk/、sdk/，原样放置，不要改名
3. 一个新品牌要接进来，只需要四步
- 丢物料：把厂商的 RDK / SDK / 驱动包放进
<类别>/<品牌>/下对应文件夹 （格式和命名要求写在该类别的README.md里，必须按它要求放）。 - 填 manifest：复制同类别的
manifest.yaml改四个字段（id / label / driver.package / config_schema）。 - 写适配器：在
driver/下实现框架的接口契约（见src/arm_interfaces/）。 这一步的工作量取决于厂商 —— 如果厂商本来就提供符合契约的 ROS 2 驱动，这一步几乎为零。 - 改一行选型：把
registry.yaml里对应类别的值改成新品牌 id。
诚实说明：第 3 步无法完全避免。Flexiv 的 RDK 是 C++/Python 库而不是 ROS 2 驱动， 所以它外面永远需要一层薄适配器。框架能做的是把这层适配器写到最薄（只做 API 翻译， 不含任何业务逻辑），而不是让你零代码接入。
4. 启动时的校验
框架启动会依次读 registry.yaml → 各 manifest.yaml → config/site.yaml，
校验三件事：
- 选中的品牌目录存在，且
manifest.yaml可解析； - manifest 里
assets声明的必需物料文件都在； config_schema里标记required: true的配置键，在site.yaml里都有值。
任何一项不满足立即报错并明确指出该去哪个文件补什么，不允许静默降级。
这是替代现在「run_handeye.sh 传了 5 个节点没声明的参数、调参静默失效」那类问题的机制。