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>
114 lines
5.2 KiB
YAML
114 lines
5.2 KiB
YAML
# =============================================================================
|
||
# devices/sim/gazebo/manifest.yaml
|
||
#
|
||
# Gazebo 仿真平台清单。
|
||
#
|
||
# 进度（截至 P5c-2，如实记录，不要凭印象改）：
|
||
# ✔ 环境 独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble）
|
||
# 安装见 scripts/install_sim_env.sh，激活见 scripts/sim_env.sh
|
||
# ✔ 世界文件 worlds/assembly.world（sdformat 校验通过）
|
||
# ✔ 机器人描述 urdf/（复用官方 flexiv_description，已通过 sdformat 校验）
|
||
# ✔ 控制器 config/controllers.yaml（控制器名与真机一致）
|
||
# ✔ 描述归一化 arm_sim_gazebo 包的 urdf_normalize（16 项单测）
|
||
# ✘ 启动器 driver.executable 声明的 gazebo_sim_launcher 尚未实现
|
||
# ✘ 场景模型 models/ 还是空的（工件/通止规模型未做）
|
||
# ✘ 触觉插件 sensor_plugins/ 还是空的（Orisys 三向力插件未做）
|
||
# ✘ 相机出图 D405 已建模，但话题出图未联调验证
|
||
#
|
||
# 所以：现在**还不能**靠 run_sim.sh 跑起完整仿真，run_sim.sh 走的仍是
|
||
# arm_bringup 的 mock 驱动（假数据）。接线步骤见本目录 README.md。
|
||
#
|
||
# 一个重要结论（接线过程中实测得出，值得记住）：
|
||
# 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉，有两处不兼容
|
||
# （7 轴本体缺 <inertial> 包裹；夹爪有零质量 link 导致惯性叠加出 NaN）。
|
||
# 两者都只有 Gazebo 会暴露，RViz/MoveIt 完全正常，所以此前从未被发现。
|
||
# 解决方式是在启动时归一化，见 arm_sim_gazebo/urdf_normalize.py 的长注释。
|
||
# =============================================================================
|
||
|
||
id: gazebo
|
||
label: Gazebo 仿真
|
||
kind: sim
|
||
vendor: openrobotics
|
||
# 实测版本。注意：Humble 配的是 Fortress（ignition-gazebo6），不是现代 gz-sim ——
|
||
# 后者要 libprotobuf>=6.31，与 RoboStack 给 Humble 钉的 5.29.3 解不开依赖。
|
||
version: "Fortress 6.16 (ignition-gazebo6, ros_gz + gz_ros2_control 0.7.15)"
|
||
|
||
description: >
|
||
物理仿真平台。环境已装好、世界与机器人描述已就绪并通过 sdformat 校验；
|
||
启动器、场景模型（工件/通止规）与 Orisys 触觉插件尚未实现，
|
||
因此仿真目前还不能替代 run_sim.sh 的 mock 路径。
|
||
注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前，
|
||
仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。
|
||
|
||
# --- 物料放置要求 ----------------------------------------------------------
|
||
assets:
|
||
- path: worlds
|
||
required: false
|
||
what: Gazebo 世界文件（.world / .sdf）
|
||
format: Gazebo 原生格式
|
||
naming: 场景名.world，如 assembly.world
|
||
where_used: 由 site.yaml 的 sim.world 指向
|
||
|
||
- path: models
|
||
required: false
|
||
what: 工件 / 夹具 / 通止规模型
|
||
format: 每个模型一个子目录，内含 <模型名>.sdf 与 meshes/
|
||
naming: >
|
||
模型名必须与推理用的类别名一致（如 workpiece），否则仿真里算出的坐标
|
||
与推理结果对不上。网格单位必须是米，用毫米建模的必须先缩放。
|
||
where_used: Gazebo 场景加载
|
||
|
||
- path: urdf
|
||
required: false
|
||
what: 仿真版机器人描述
|
||
format: .urdf / .xacro，需含 gazebo 与 ros2_control 标签
|
||
naming: 关节前缀必须以 sim_ 开头，与真实臂（Rizon4-xxxxxx_）区分
|
||
where_used: 仿真机器人加载
|
||
|
||
- path: sensor_plugins
|
||
required: false
|
||
what: 传感器插件（含 Orisys 触觉自定义插件）
|
||
format: 插件源码 + CMakeLists.txt
|
||
naming: 插件类名以 Orisys 开头
|
||
where_used: 仿真传感器数据发布
|
||
|
||
# --- 适配器 ----------------------------------------------------------------
|
||
driver:
|
||
package: arm_sim_gazebo
|
||
executable: gazebo_sim_launcher
|
||
build: colcon
|
||
notes: >
|
||
包已建（含经单测的 URDF 归一化），但 gazebo_sim_launcher 本身**尚未实现**，
|
||
因此 setup.py 里暂未注册这个入口点 —— 注册了模块不存在，构建能过、
|
||
一运行就 ImportError。P5c-3 补上实现后再注册。
|
||
在它可用之前，run_sim.sh 仍走 arm_bringup 的 mock 驱动路径。
|
||
|
||
# --- 配置项声明 ------------------------------------------------------------
|
||
# 设计约束：仿真与真机共用同一套 site.yaml 参数（servo.* / hole_detect.stage2.*），
|
||
# 避免「仿真调好的值搬到真机不生效」。只有物理引擎自身的参数放在 sim.* 下。
|
||
config_schema:
|
||
- key: world
|
||
site_path: sim.world
|
||
type: str
|
||
required: false
|
||
default: ""
|
||
doc: >
|
||
仿真世界文件路径。留空表示使用 Gazebo 默认空场景。
|
||
Gazebo 接线完成后填真实路径，如 devices/sim/gazebo/worlds/assembly.world。
|
||
|
||
- key: physics_enable
|
||
site_path: sim.physics_enable
|
||
type: bool
|
||
required: false
|
||
default: true
|
||
doc: 是否启用物理引擎。关闭可用于纯运动学验证。
|
||
|
||
- key: joint_prefix
|
||
site_path: sim.joint_prefix
|
||
type: str
|
||
required: false
|
||
default: sim_
|
||
doc: >
|
||
仿真关节名前缀。必须与真实臂的前缀（Rizon4-xxxxxx_）不同，
|
||
否则仿真与真机的 TF 会互相污染。
|