orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

P5c-2：仿真世界 + 机器人描述 + 控制器配置，并解决两处官方描述与 Gazebo 的不兼容

Browse Source 
这一步解决的是"官方 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
2026-09-23 14:19:39 +08:00
parent d93dab31c9
commit 25635e21f8
15 changed files with 1506 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/site.example.yaml
+8 -4
View File
@@ -480,10 +480,14 @@ network:
# 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
# 只有物理引擎自身的参数放在这里，避免「仿真调好的值搬到真机不生效」。
sim:
# ⚠️ 这一段目前没有任何代码读取 —— 它是对目标平台的声明，不是能生效的开关。
# 改这里的值不会有任何效果，直到 Gazebo 真正接线（见 devices/sim/gazebo/README.md）。
# 现在跑 `run_sim.sh` 得到的是 mock 驱动（假数据 + RViz），不是物理仿真。
# ⚠️ 这一段目前**没有代码读取**（gazebo_sim_launcher 还没实现，见
# devices/sim/gazebo/manifest.yaml 的进度段）。现在跑 `run_sim.sh` 得到的
# 仍是 mock 驱动（假数据 + RViz），不是物理仿真。
#
# 之所以先把 world 填上：世界文件与机器人描述**已经做好并通过 sdformat 校验**，
# 填在这里等于把"参数该从哪来"确定下来，等启动器一接上就自动生效，
# 不用再改配置。路径相对仓库根。
platform: "gazebo"
world: "" # 世界文件路径；留空用 Gazebo 默认空场景
world: "devices/sim/gazebo/worlds/assembly.world"
physics_enable: true
joint_prefix: "sim_" # 仿真关节前缀，必须与真实臂前缀区分
devices/sim/gazebo/config/controllers.yaml
+78
View File
@@ -0,0 +1,78 @@
# =============================================================================
# devices/sim/gazebo/config/controllers.yaml
#
# Gazebo 里 controller_manager 的控制器配置。由 arm.sim.gazebo.xacro 的系统插件
# 通过 <parameters> 指过来（路径由启动器按仓库根拼成绝对路径）。
#
# -----------------------------------------------------------------------------
# 硬约束：控制器名不能改
# -----------------------------------------------------------------------------
# 应用层（arm_application）按 rizon_arm_controller/follow_joint_trajectory
# 发轨迹，真机、mock、moveit 三个后端都是这个名字。仿真沿用同名，
# 应用层代码一行都不用改 —— 这是"同一套接口、换后端"的核心契约。
# 想让仿真改名，等于让应用层感知后端，那就退回到"每个后端一套代码"了。
#
# -----------------------------------------------------------------------------
# 关节名必须是 sim_ 前缀
# -----------------------------------------------------------------------------
# 与 arm.sim.urdf.xacro 的 arm_prefix:=sim 对应。
# 这里写错（比如写成 Rizon4-063989_joint1）的症状：
# controller_manager 起来了，但控制器报
# "Joint 'xxx' not found in the robot model"
# 而且 ros2 control list_hardware_interfaces 里明明有 7 个关节 ——
# 因为硬件接口是按 sim_ 前缀注册的，两边名字对不上。
# =============================================================================
controller_manager:
ros__parameters:
# 仿真步进与控制器更新率的比例。Gazebo 的物理步长在 world 文件里（1ms），
# 控制器跑 100Hz（10ms）—— 物理比控制细十倍，接触力的瞬态才不会被采丢。
# 两者是独立的：调这里不会改变物理精度，只是控制器下发的频率。
update_rate: 100
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
# 名字与真机一致，勿改（见文件头说明）
rizon_arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
# --- 关节状态广播 -------------------------------------------------------------
# TF 与 /joint_states 的来源。robot_state_publisher 靠 /joint_states 算 TF，
# 少了它 RViz / 跟踪节点全都查不到 tcp 的位置。
joint_state_broadcaster:
ros__parameters: {}
# --- 机械臂轨迹控制器 ---------------------------------------------------------
rizon_arm_controller:
ros__parameters:
joints:
- sim_joint1
- sim_joint2
- sim_joint3
- sim_joint4
- sim_joint5
- sim_joint6
- sim_joint7
# 只下发位置。Gazebo 侧 position 可写；应用层也不做力矩控制
# （柔顺下压是靠应用层按力反馈改位置目标实现的，不是靠力矩接口）。
command_interfaces:
- position
# velocity 要读回来：handeye 的"稳定判定"、柔顺下压的限速都依赖关节速度。
state_interfaces:
- position
- velocity
# false = 轨迹必须包含全部 7 个关节才接受。
# 应用层发的都是全关节轨迹，设 false 能在漏关节时直接报错而不是补零乱动。
allow_partial_joints_goal: false
# 开环：Gazebo 是理想执行器，不需要靠状态反馈纠正，开环反而更平滑。
open_loop_control: true
constraints:
# 与官方 joint_limits.yaml 的 soft 限位对齐（真机软限位）。
# 不设的话 JTC 只按硬限位卡，仿真里会走到真机不敢走的姿态。
stopped_velocity_tolerance: 0.05
devices/sim/gazebo/manifest.yaml
+30 -8
View File
@@ -3,21 +3,40 @@
#
# Gazebo 仿真平台清单。
#
# 重要：本项目的 Gazebo 目前【尚未接线】。详见本目录 README.md。
# 这份清单声明的是「接线完成之后」应有的配置项，并把 world 设为非必填，
# 这样在仿真还没搭建好时不会阻塞 device_check。
# 进度（截至 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
version: "待填 —— 接线后记录 Gazebo 版本"
# 实测版本。注意：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: >
物理仿真平台。当前项目的「仿真」实际是 mock 驱动（假数据），Gazebo 尚未接线：
arm.gazebo.xacro 零引用、stack.launch.py 的 isaac 分支只有日志、没有 world 文件。
接线步骤见本目录 README.md 的第一部分。
物理仿真平台。环境已装好、世界与机器人描述已就绪并通过 sdformat 校验；
启动器、场景模型（工件/通止规）与 Orisys 触觉插件尚未实现，
因此仿真目前还不能替代 run_sim.sh 的 mock 路径。
注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前，
仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。
@@ -59,7 +78,10 @@ driver:
executable: gazebo_sim_launcher
build: colcon
notes: >
当前尚未实现。接线前 run_sim.sh 走的是 arm_bringup 的 mock 驱动路径。
包已建（含经单测的 URDF 归一化），但 gazebo_sim_launcher 本身**尚未实现**，
因此 setup.py 里暂未注册这个入口点 —— 注册了模块不存在，构建能过、
一运行就 ImportError。P5c-3 补上实现后再注册。
在它可用之前，run_sim.sh 仍走 arm_bringup 的 mock 驱动路径。
# --- 配置项声明 ------------------------------------------------------------
# 设计约束：仿真与真机共用同一套 site.yaml 参数（servo.* / hole_detect.stage2.*），
devices/sim/gazebo/urdf/arm.sim.gazebo.xacro
+90
View File
@@ -0,0 +1,90 @@
<?xml version="1.0"?>
<!--
=============================================================================
Gazebo 专属标签（<gazebo>）。
URDF 本身是"几何+运动学"的描述，Gazebo 需要额外信息才能正确仿真：
<plugin> 把 ros2_control 拉进 Gazebo 进程（没有它就没有 controller_manager）
<material> 让可视化能区分各个连杆（纯观感，但调试时非常有用）
阻尼/摩擦 纯 URDF 的 <dynamics> 会被 Gazebo 部分忽略，这里显式覆盖
为什么单独一个文件而不是塞进 arm.sim.urdf.xacro：
传感器（相机、触觉）也要往 <gazebo> 里挂东西（见 arm.sim.sensors.xacro）。
分开后"机器人本体"与"外挂设备"互不干扰，加传感器不用动本体文件。
=============================================================================
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="arm_sim_gazebo" params="prefix controllers_file">
<!-- =====================================================================
ros2_control 的系统插件。
filename 必须是 Gazebo 能在库路径里找到的 .so 名。本机实测该文件为
$CONDA_PREFIX/lib/libgz_ros2_control-system.so，故写 libgz_ros2_control-system.so。
踩坑提示：网上教程常见 libignition-gazebo-ros2-control-system.so
（Gazebo 改名前的老名字）。名字写错时 Gazebo 只打印一行
"Failed to load system plugin"，不报具体原因，很容易卡在这里。
换版本后这样确认：
ls $CONDA_PREFIX/lib/libgz_ros2_control*.so
ls $CONDA_PREFIX/lib/libignition-gazebo*ros2-control*.so
parameters 指向控制器配置文件，控制器名与话题名由它决定。
应用层按 rizon_arm_controller/follow_joint_trajectory 发轨迹
（与真机、mock 完全一致），所以 controllers.yaml 里的控制器名必须是
rizon_arm_controller —— 这是三个后端共享的接口契约，不能改。
===================================================================== -->
<gazebo>
<plugin filename="libgz_ros2_control-system.so"
name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<parameters>${controllers_file}</parameters>
</plugin>
</gazebo>
<!-- =====================================================================
连杆外观色。只为在 Gazebo 里一眼看清是哪一节在动。
官方 URDF 的 visual 里带 mesh，但 Gazebo 渲染 mesh 更吃性能，
用纯色覆盖能显著提速，调试运动时也更直观。
===================================================================== -->
<gazebo reference="${prefix}base_link"><material>Gazebo/DarkGrey</material></gazebo>
<gazebo reference="${prefix}link1"><material>Gazebo/Red</material></gazebo>
<gazebo reference="${prefix}link2"><material>Gazebo/Blue</material></gazebo>
<gazebo reference="${prefix}link3"><material>Gazebo/Green</material></gazebo>
<gazebo reference="${prefix}link4"><material>Gazebo/Yellow</material></gazebo>
<gazebo reference="${prefix}link5"><material>Gazebo/Orange</material></gazebo>
<gazebo reference="${prefix}link6"><material>Gazebo/Purple</material></gazebo>
<gazebo reference="${prefix}link7"><material>Gazebo/Grey</material></gazebo>
<!-- =====================================================================
末端两个关键 link 的接触参数。
通止规装配全靠"接触"：下压到碰到工件、旋拧时切向力突变。
这些判据在仿真里能不能复现，取决于接触刚度/阻尼是否合理。
默认值（stiffness=1e6）对金属-金属插入偏软，会有明显穿透感，
这里调到 1e7 并给足阻尼，让接触"硬"一点，力信号更干净。
min_depth / max_vel 是 Gazebo 用来抑制穿透弹跳的收敛参数：
调不好会出现"接触瞬间被弹飞"的假象，进而让力判据误触发。
===================================================================== -->
<gazebo reference="${prefix}flange">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<kp>1e7</kp>
<kd>1e4</kd>
<minDepth>0.001</minDepth>
<maxVel>0.01</maxVel>
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="${prefix}tcp">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<kp>1e7</kp>
<kd>1e4</kd>
<minDepth>0.001</minDepth>
<maxVel>0.01</maxVel>
</gazebo>
</xacro:macro>
</robot>
devices/sim/gazebo/urdf/arm.sim.ros2_control.xacro
+63
View File
@@ -0,0 +1,63 @@
<?xml version="1.0"?>
<!--
=============================================================================
仿真版 ros2_control 硬件描述。
为什么不复用官方 flexiv_description 的 rizon.ros2_control.xacro：
那个宏只认两种硬件插件 —— 要么 mock_components/GenericSystem，
要么 flexiv_hardware/FlexivHardwareInterface（真机 RDK）。
Gazebo 需要第三种：gz_ros2_control/GazeboSimSystem。官方宏里没有这个分支，
而且改官方文件等于分叉上游描述，上游一升级就冲突。所以仿真自己写一份。
两处必须对齐，否则 controller_manager 会报 "no hardware interface found"：
1. 关节名 —— 必须是 sim_joint1..sim_joint7（前缀 sim_，见 naming 约定）
2. command/state interface 名字 —— 必须与 gz_ros2_control 实际提供的接口一致
Gazebo 侧：position/velocity 可读可写，effort 只能读。
我们只下 position 指令（应用层走 JTC 位置轨迹），所以 command 只声明 position。
验证插件名的方法（换 Gazebo/ros2_control 版本后建议重跑一遍确认）：
strings $CONDA_PREFIX/lib/libgz_ros2_control-system.so | grep GazeboSimSystem
cat $CONDA_PREFIX/share/gz_ros2_control/gz_hardware_plugins.xml
本机实测（gz_ros2_control 0.7.15）：硬件名为 gz_ros2_control/GazeboSimSystem。
=============================================================================
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="arm_sim_ros2_control" params="prefix joint_count:=7">
<ros2_control name="GazeboSimSystem" type="system">
<hardware>
<!--
Gazebo 的 ros2_control 硬件插件。它由 Gazebo 侧的系统插件
（arm.sim.gazebo.xacro 里声明的 GazeboSimROS2ControlPlugin）加载，
controller_manager 在 Gazebo 进程内被拉起来。
注意：硬件参数（update_rate 等）不写在这里，而是在 Gazebo 的
<plugin> 标签里配置（见 arm.sim.gazebo.xacro）。写在 <hardware> 下
不会生效，这是 gz_ros2_control 与真机 hardware 的一个区别。
-->
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
</hardware>
<!-- 关节 1..7。全部只给 position 指令 + position/velocity 状态。 -->
<xacro:macro name="arm_sim_joint" params="name">
<joint name="${name}">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</xacro:macro>
<xacro:arm_sim_joint name="${prefix}joint1"/>
<xacro:arm_sim_joint name="${prefix}joint2"/>
<xacro:arm_sim_joint name="${prefix}joint3"/>
<xacro:arm_sim_joint name="${prefix}joint4"/>
<xacro:arm_sim_joint name="${prefix}joint5"/>
<xacro:arm_sim_joint name="${prefix}joint6"/>
<xacro:arm_sim_joint name="${prefix}joint7"/>
</ros2_control>
</xacro:macro>
</robot>
devices/sim/gazebo/urdf/arm.sim.sensors.xacro
+189
View File
@@ -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>
devices/sim/gazebo/urdf/arm.sim.urdf.xacro
+152
View File
@@ -0,0 +1,152 @@
<?xml version="1.0" encoding="utf-8"?>
<!--
=============================================================================
仿真版机器人描述（Gazebo 用）。
用法（由 arm_sim_gazebo 的 gazebo_sim_launcher 调用，一般不需要手敲）：
xacro /abs/path/devices/sim/gazebo/urdf/arm.sim.urdf.xacro \
controllers_file:=/abs/path/devices/sim/gazebo/config/controllers.yaml
注意：本文件在 devices/ 下，而 devices/ **不是 ROS 包**，所以不能用
$(find ...) 找同目录的兄弟文件。这里用 xacro 的相对包含 —— xacro 会把
相对路径按"当前正在处理的文件所在目录"解析，因此同目录文件直接写文件名即可。
（绝对路径也可以，启动器就是这么传 controllers_file 的。）
============================================================================
设计：复用官方描述，只补仿真缺的那部分
============================================================================
连杆几何 / 惯性 / 碰撞体全部来自官方 flexiv_description，一行都不复制：
<xacro:rizon_robot ...> 会用 Rizon4 的四个 YAML 生成
joint_limits.yaml → 关节限位（与真机一致，仿真才有意义）
default_kinematics.yaml → 各关节原点位姿（决定 TCP 能不能够到工件）
inertia_parameters.yaml → 真实质量与惯量张量（Gazebo 物理必需）
visual_parameters.yaml → mesh 视觉 + 凸包碰撞
这一点很关键：如果仿真自己造一套物理参数，那"仿真里调好的力阈值搬到真机
就不成立"，整个仿真也就失去价值了。
仿真额外要补的只有三样，都在本文件里显式写清：
1. gz_ros2_control 硬件 + 系统插件（官方宏只认 mock / 真机两种）
2. tcp / tool0 两个应用层帧（官方描述只有 flange）
3. 外挂传感器（相机/触觉，见 arm.sim.sensors.xacro）
============================================================================
关节前缀为什么是 sim_
============================================================================
真实臂前缀是 Rizon4-063989_。仿真若沿用同一前缀，两边同时跑时 TF 和
/joint_states 会互相污染 —— 症状是 RViz 里模型乱跳、控制器收到别人的关节角。
仿真用独立的 sim_ 前缀 → 天然隔离，可以真机仿真同开对比。
注意这不止是"改个名字"：控制器名（rizon_arm_controller）**故意保持与真机一致**，
因为那是应用层依赖的接口契约；隔离的是关节/TF 层，不是接口层。
=============================================================================
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rizon_sim">
<!-- 机型。Rizon4 / Rizon4s / Rizon4M / Rizon4R / Rizon10 / Rizon10s -->
<xacro:arg name="rizon_type" default="Rizon4"/>
<!--
仿真关节前缀。改成空串则与真机同前缀（只在单跑仿真、且不介意与真机串味时用）。
robot_sn 传空串，配合 arm_prefix=sim，官方宏算出的前缀就是 "sim_"。
官方前缀算法：(arm_prefix + '_' + robot_sn + '_') 若两者都非空，
否则 (arm_prefix + robot_sn + '_')。
-->
<xacro:arg name="arm_prefix" default="sim"/>
<xacro:arg name="robot_sn" default=""/>
<!-- 控制器配置文件的绝对路径（由启动器按仓库根动态拼出来传入） -->
<xacro:arg name="controllers_file" default=""/>
<!-- 是否挂夹爪。换夹爪品牌时这里改（或由启动器从 site.yaml 映射过来）。
注意官方描述里夹爪只有 Flexiv-GN01 一种（grav_macro），换别的品牌
需要另写一份描述 —— 见 devices/grippers/ 的说明。 -->
<xacro:arg name="load_gripper" default="true"/>
<!-- 是否挂外挂传感器（相机/触觉）。关掉可做纯运动学验证，跑得更快。 -->
<xacro:arg name="with_sensors" default="true"/>
<!--
传感器参数直接由启动器传入，不在这里读 YAML。
原因：读 YAML 需要绝对路径，而 xacro 无法自己推导仓库根；且"配置→参数"的
映射统一放在启动器里（那是唯一知道 site.yaml 和仓库布局的地方），
调试时看一处就够，不用在 xacro / YAML / Python 三处来回找。
-->
<xacro:arg name="sensors_file" default=""/>
<xacro:property name="ap" value="$(arg arm_prefix)"/>
<xacro:property name="sn" value="$(arg robot_sn)"/>
<xacro:property name="prefix"
value="${(ap + '_' + sn + '_') if ap != '' and sn != '' else (ap + sn + '_') if (ap + sn) != '' else ''}"/>
<!-- =========================================================================
官方描述：7 轴本体 + 真实惯性 + 凸包碰撞
=========================================================================
ros2_control:=false —— 官方宏的 ros2_control 分支只能给 mock 或真机插件，
仿真要 gz_ros2_control，所以这里关掉、下面自己写。
add_world_link / create_base_joint 保持 true：
它们会生成一个空的 world link 和 world→base_link 的固定关节，
把机械臂焊在世界上。这是必要的 —— 不焊住，仿真一开始整条臂就砸到地面。
注：ros_gz_sim create 没有 =static 之类选项（实测确认），
所以"固定基座"只能在描述文件里做。
-->
<xacro:include filename="$(find flexiv_description)/urdf/common/rizon_robot.xacro"/>
<xacro:rizon_robot
rizon_type="$(arg rizon_type)"
robot_sn="$(arg robot_sn)"
arm_prefix="$(arg arm_prefix)"
joint_limits_parameters_file="$(find flexiv_description)/config/$(arg rizon_type)/joint_limits.yaml"
kinematics_parameters_file="$(find flexiv_description)/config/$(arg rizon_type)/default_kinematics.yaml"
inertia_parameters_file="$(find flexiv_description)/config/$(arg rizon_type)/inertia_parameters.yaml"
visual_parameters_file="$(find flexiv_description)/config/$(arg rizon_type)/visual_parameters.yaml"
load_gripper="$(arg load_gripper)"
gripper_name="Flexiv-GN01"
load_mounted_ft_sensor="false"
ros2_control="false"
add_world_link="true"
create_base_joint="true"
/>
<!-- =========================================================================
应用层帧：tcp / tool0
=========================================================================
与真机 arm.urdf.xacro 的定义保持一致（都是 flange 上的零偏移固定帧）。
应用层代码硬编码查 tcp / tool0（见 arm_application 的 TF 查询），
仿真里这两个帧必须存在，否则跟踪和柔顺下压会在查 TF 时失败。
tool0 是 tcp 的别名，历史原因两者都在用，别删任何一个 ——
删了会有一部分节点静默查不到帧。
-->
<joint name="${prefix}tcp_joint" type="fixed">
<parent link="${prefix}flange"/>
<child link="${prefix}tcp"/>
</joint>
<link name="${prefix}tcp"/>
<joint name="${prefix}tool0_joint" type="fixed">
<parent link="${prefix}tcp"/>
<child link="${prefix}tool0"/>
</joint>
<link name="${prefix}tool0"/>
<!-- =========================================================================
ros2_control：gz_ros2_control 硬件（同目录）
========================================================================= -->
<xacro:include filename="arm.sim.ros2_control.xacro"/>
<xacro:arm_sim_ros2_control prefix="${prefix}"/>
<!-- =========================================================================
Gazebo 标签：系统插件 + 材质 + 接触参数（同目录）
========================================================================= -->
<xacro:include filename="arm.sim.gazebo.xacro"/>
<xacro:arm_sim_gazebo prefix="${prefix}"
controllers_file="$(arg controllers_file)"/>
<!-- =========================================================================
外挂传感器（相机 + 触觉，同目录）
========================================================================= -->
<xacro:if value="$(arg with_sensors)">
<xacro:include filename="arm.sim.sensors.xacro"/>
<xacro:arm_sim_sensors prefix="${prefix}"
sensors_file="$(arg sensors_file)"/>
</xacro:if>
</robot>
devices/sim/gazebo/worlds/assembly.world
+238
View File
@@ -0,0 +1,238 @@
<?xml version="1.0"?>
<!--
=============================================================================
devices/sim/gazebo/worlds/assembly.world
通止规装配场景。
由 site.yaml 的 sim.world 指向（留空则用 Gazebo 默认空世界）：
sim:
world: "devices/sim/gazebo/worlds/assembly.world"
============================================================================
物理引擎参数为什么这么调
============================================================================
装配任务的难点全在"接触"上：下压到碰、旋拧时切向力突变。这些判据能不能在
仿真里复现，几乎完全由物理步长和接触求解器决定：
max_step_size 1ms 步长太大时穿透会累积，接触力变成一坨噪声，
柔顺下压的"突变检测"就废了。1ms 是精度与速度的折中。
real_time_factor 仿真时间/真实时间的上限。设 1.0 表示最多实时；
调低能让物理更稳（掉步少），调试接触时可设 0.5。
solver_iters 接触求解迭代次数。插入/旋拧这种持续接触场景，
默认 50 会"软"，接触力抖动大；提到 150 明显更干净，
代价是 CPU。若机器吃力，先降 real_time_factor，
不要降 solver_iters —— 降了力信号就没法用了。
=============================================================================
-->
<sdf version="1.9">
<world name="assembly">
<!-- = 物理 -->
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
<!--
物理引擎 = DART（Fortress 默认），碰撞检测器用 bullet。
说明：引擎配置直接写在 <physics> 下（dart 子元素），不要写成
<ignition::physics> —— SDF 的元素名里不允许出现 "::"，
sdformat 会直接报 XML not well-formed。"::" 只出现在插件名的
属性值里（如 name="ignition::gazebo::systems::Sensors"），那是合法的。
-->
<dart>
<collision_detector>bullet</collision_detector>
</dart>
<solver>
<type>quick</type>
<iters>150</iters>
<sor>1.3</sor>
</solver>
<constraints>
<cfm>0.0</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>0.1</contact_max_correcting_vel>
<contact_surface_layer>0.001</contact_surface_layer>
</constraints>
</physics>
<!-- = 光照 -->
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<!-- = 地面 -->
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane><normal>0 0 1</normal><size>10 10</size></plane>
</geometry>
<surface>
<friction>
<ode><mu>0.8</mu><mu2>0.8</mu2></ode>
</friction>
</surface>
</collision>
<visual name="visual">
<geometry>
<plane><normal>0 0 1</normal><size>10 10</size></plane>
</geometry>
<material>
<ambient>0.7 0.7 0.7 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
</material>
</visual>
</link>
</model>
<!-- =========================================================================
工件台
=========================================================================
高度 0.75m 是常见工作台高度。工件就放在台面上（P5c-6 里会加），
台面高度决定机械臂要不要伸到底 —— 先在仿真里确认工作空间够得着，
再上真机试，能省掉很多次现场调试。
台面做成 static：台子本身不该被机械臂推着走。
摩擦系数给足（0.8），否则柔顺下压时工件会被顶得滑动，
仿真里看到"工件跑了"会让人误以为是跟踪算法的问题。
========================================================================= -->
<model name="work_table">
<static>true</static>
<pose>0.55 0 0 0 0 0</pose>
<link name="top">
<pose>0 0 0.375 0 0 0</pose>
<collision name="collision">
<geometry><box><size>0.8 1.2 0.05</size></box></geometry>
<surface>
<friction>
<ode><mu>0.8</mu><mu2>0.8</mu2></ode>
</friction>
<contact>
<ode>
<kp>1e7</kp>
<kd>1e4</kd>
<max_vel>0.01</max_vel>
<min_depth>0.001</min_depth>
</ode>
</contact>
</surface>
</collision>
<visual name="visual">
<geometry><box><size>0.8 1.2 0.05</size></box></geometry>
<material>
<ambient>0.45 0.4 0.35 1</ambient>
<diffuse>0.55 0.5 0.42 1</diffuse>
</material>
</visual>
</link>
<!-- 四条腿，纯视觉：碰撞只保留台面，省掉不必要的接触计算 -->
<link name="legs">
<pose>0 0 0.175 0 0 0</pose>
<visual name="visual">
<geometry><box><size>0.7 1.1 0.35</size></box></geometry>
<material>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.35 0.35 0.35 1</diffuse>
</material>
</visual>
</link>
</model>
<!-- =========================================================================
通止规起始位（留给人工/机械手放置用的标记区）
=========================================================================
真机上通止规是被人放到夹爪里的（P6 里有"提示用户放置"的交互）。
仿真里这里放一个视觉标记，方便对着它调整"返回极值角是否到位"。
真正会被夹爪抓取的通止规模型在 P5c-6 里加到这个位置。
========================================================================= -->
<model name="gauge_marker">
<static>true</static>
<pose>0.15 0.35 0.755 0 0 0</pose>
<link name="link">
<visual name="visual">
<geometry><cylinder><radius>0.04</radius><length>0.004</length></cylinder></geometry>
<material>
<ambient>0.1 0.6 0.9 1</ambient>
<diffuse>0.1 0.6 0.9 1</diffuse>
</material>
</visual>
</link>
</model>
<!-- =========================================================================
传感器渲染系统插件（必须，且必须在 world 级）
=========================================================================
没有它，所有 <sensor> 标签被静默忽略 —— 退出码 0、不报错、话题永远
没数据。排查"相机不出图"时，第一件事就是确认这个插件真的加载了：
ign gazebo -v 3 ... 2>&1 | grep -i sensors
render_engine 用 ogre2：本机装了 libignition-rendering6-ogre2.so，
且只有 ogre2 支持深度相机（老的 ogre 拿不到深度）。
注：无头环境（没有显示器）下 ogre2 通常仍可用（走 EGL/软件渲染），
但若报渲染相关错误，先试 `export LIBGL_ALWAYS_SOFTWARE=1`。
========================================================================= -->
<plugin filename="libignition-gazebo6-sensors-system.so"
name="ignition::gazebo::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<!-- =========================================================================
RealSense D435 —— 眼在手外（俯视工作台）
=========================================================================
放在 world 而不是机械臂 URDF 里，因为这个相机在物理上就属于场景：
它装在工作台支架上，不跟着机械臂动。
（早期把它挂在 URDF 的 world link 上，结果转 SDF 时整台相机消失 ——
sdformat 丢弃无质量根 link 时连带丢了整棵子树，且不报错。）
位姿：pitch≈50° 俯视，yaw=180° 让光轴转向机械臂一侧。
视野必须完整覆盖台面 —— 粗定位失败要回退到这个视角，看不全工件就等于
没有兜底方案。验收方法：
ros2 topic hz /d435/color/image_raw # 有数
rqt_image_view /d435/color/image_raw # 工件三个孔都在画面内
========================================================================= -->
<model name="d435">
<static>true</static>
<pose>-0.45 0.0 1.35 0.0 0.872665 3.14159</pose>
<link name="link">
<visual name="body">
<geometry><box><size>0.09 0.025 0.025</size></box></geometry>
<material>
<ambient>0.15 0.15 0.15 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
</material>
</visual>
<sensor name="d435_rgbd" type="rgbd_camera">
<update_rate>30</update_rate>
<always_on>true</always_on>
<visualize>false</visualize>
<camera name="d435">
<!-- D435 视场较宽（约 87°×58°），粗定位需要比 D405 更宽的视野 -->
<horizontal_fov>1.5184</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip><near>0.1</near><far>10.0</far></clip>
<depth_camera>
<clip><near>0.1</near><far>10.0</far></clip>
</depth_camera>
</camera>
<!-- 话题名对齐真机 ros_realsense，应用层两边通用 -->
<topic>/d435</topic>
</sensor>
</link>
</model>
</world>
</sdf>
src/arm_sim_gazebo/arm_sim_gazebo/__init__.py
+12
View File
@@ -0,0 +1,12 @@
"""Gazebo 仿真后端。
只暴露纯函数级别的能力，不在这里 import rclpy —— 这样
urdf_normalize 可以被单测直接导入，不需要 ROS 运行时。
"""
from .urdf_normalize import ( # noqa: F401
NormalizeReport,
normalize_file,
normalize_urdf,
)
__all__ = ["NormalizeReport", "normalize_file", "normalize_urdf"]
src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py
+250
View File
@@ -0,0 +1,250 @@
"""把厂商给的 URDF 归一化成"Gazebo 能接受"的形态。
=============================================================================
为什么需要这一步（全是实测踩出来的）
=============================================================================
官方 flexiv_description 是给 RViz / MoveIt 用的，直接喂 Gazebo 会被
sdformat 拒掉。两类问题，都属于"RViz 容忍、Gazebo 严格"的差异，
所以这个框架在有 Gazebo 之前一直没暴露：
问题一：7 轴本体的质量/惯量是"孤儿标签"
rizon_arm.xacro 里这样调：
<link name="${prefix}base_link">
<xacro:get_inertias name="base_link" />
get_inertias 宏只吐 <origin> / <mass> / <inertia> 三个碎片，**外面没有
<inertial> 包裹**。URDF 规范里 <link> 不能直接有这三个子元素。
RViz/MoveIt 忽略不认识的子元素 → 看起来一切正常
sdformat 判定 "link has no <inertial> block"，
然后把 **link 和它的父关节一起丢掉**
症状很误导：机械臂凭空消失或只剩夹爪，报错只有一句
"link[sim_link1] is not modeled in sdf"，看不出根因在上游描述文件里。
问题二：夹爪有零质量 link，导致 sdformat 惯性叠加出 NaN
grav_macro.xacro 给 left_finger_tip / left_finger_tcp / grav_tcp 等
六个纯坐标系 dummy link 写了 mass=0、惯量全 0。
sdformat 对 fixed 关节会把子 link 的惯性"叠加(lump)"到父 link，
这个叠加要除以总质量 → 质量 0 就出 NaN。而报错指向的是**父** link：
Pose values must be finite.
link[sim_left_finger_mount]/inertial/pose
完全看不出是子 link 质量的问题。
=============================================================================
修法原则
=============================================================================
只做"重新分组 + 补极小量"，不发明任何物理数据，也不掩盖真问题：
- 孤儿三件套 → 只是把已有元素包进 <inertial>，一个数字都不改
- 零质量 → 补成 1e-4 kg（与夹爪其他配件同量级，0.1g 对动力学无影响），
惯量如果是全 0 也补成极小的对角值，避免张量退化
若某个 link 上游真没给质量信息，问题一这里什么都不会包，
sdformat 会照实报错 —— 这是我们想要的：归一化不是止错器。
=============================================================================
为什么在启动时做，而不是改上游文件
=============================================================================
1. 上游文件由 scripts/fetch_flexiv.sh 从 GitHub 获取，改了会被下次 fetch 覆盖
2. 这个框架要支持"换成别的臂" —— 别的厂商描述大概率有别的坑，
在启动时统一过一道"归一化"，比逐个去改厂商文件更可持续
3. 纯字符串→字符串的变换，可以脱开 ROS 单测（见 test/test_urdf_normalize.py）
=============================================================================
"""
from __future__ import annotations
import re
from dataclasses import dataclass, field
from typing import List, Tuple
# 零质量 link 补成的质量。取 1e-4 kg：与夹爪上 other 配件（内外杆、指座）
# 同量级，0.1g 在动力学上完全可忽略，但足以让惯性叠加不除零。
ZERO_MASS_FIX_KG = "1e-4"
# 惯量全 0 时补的对角值。极致小值只为让张量非退化，不引入可观测的转动惯量。
ZERO_INERTIA_FIX = "1e-9"
# 裸露的 origin + mass + inertia 三件套（自闭合标签，连续三行）。
# 夹爪的 <inertial> 块内部也是这三行，所以必须按 link 块处理并跳过
# 已含 <inertial> 的块，否则会二次包裹 —— sdformat 会报
# "Inertial element must have a mass element"（已踩过）。
_TRIPLE = re.compile(
r"(?P<ind>[ \t]*)<origin(?P<o>[^>]*?)/>[ \t]*\n"
r"[ \t]*<mass(?P<m>[^>]*?)/>[ \t]*\n"
r"[ \t]*<inertia(?P<i>[^>]*?)/>"
)
_LINK_BLOCK = re.compile(r"<link\b[^>]*>.*?</link>", re.S)
_MASS_TAG = re.compile(r"<mass(?P<attrs>[^>]*?)/>")
_INERTIA_TAG = re.compile(r"<inertia(?P<attrs>[^>]*?)/>")
_INERTIA_COMPONENTS = ("ixx", "ixy", "ixz", "iyy", "iyz", "izz")
@dataclass
class NormalizeReport:
"""归一化做了什么。启动器会打印它 —— 静默修改用户描述是不可接受的，
必须让人一眼看到"我改了你给的文件里的哪几处、改成什么"。"""
wrapped_inertials: int = 0
fixed_zero_mass: List[str] = field(default_factory=list)
fixed_zero_inertia: List[str] = field(default_factory=list)
@property
def changed(self) -> bool:
return bool(
self.wrapped_inertials or self.fixed_zero_mass or self.fixed_zero_inertia
)
def summary(self) -> str:
if not self.changed:
return "URDF 无需归一化"
bits = []
if self.wrapped_inertials:
bits.append(f"补 <inertial> 包裹 {self.wrapped_inertials} 处")
if self.fixed_zero_mass:
bits.append(
f"零质量 link 补 {ZERO_MASS_FIX_KG}kg {len(self.fixed_zero_mass)} 个"
f"({', '.join(self.fixed_zero_mass)})"
)
if self.fixed_zero_inertia:
bits.append(f"零惯量张量补极小对角 {len(self.fixed_zero_inertia)} 个")
return "；".join(bits)
def _num(attr: str) -> float:
"""把 URDF 里的数值属性转 float，失败返回 nan（调用方据此跳过）。"""
try:
return float(attr)
except (TypeError, ValueError):
return float("nan")
def _attrs_of(tag: str) -> dict:
"""从标签文本里取出 name="value" 形式的属性。"""
return dict(re.findall(r'([A-Za-z_][\w.-]*)\s*=\s*"([^"]*)"', tag))
def _fix_zero_mass(block: str, report: NormalizeReport, link_name: str) -> str:
"""把 <mass value="0"/> 补成极小正值。
两个前提：这个 link 有 <inertial>（否则不是"质量为零"而是"没有质量信息"，
不归这里管），且质量为数值 0。
"""
if "<inertial>" not in block:
return block
m = _MASS_TAG.search(block)
if m is None:
return block
attrs = _attrs_of(m.group("attrs"))
if _num(attrs.get("value", "")) != 0.0:
return block
report.fixed_zero_mass.append(link_name)
fixed = f'<mass value="{ZERO_MASS_FIX_KG}"/>'
block = block[: m.start()] + fixed + block[m.end():]
# 惯量张量若也全是 0，补极小对角值，避免退化
im = _INERTIA_TAG.search(block)
if im is not None:
iattrs = _attrs_of(im.group("attrs"))
if all(_num(iattrs.get(c, "")) == 0.0 for c in _INERTIA_COMPONENTS):
report.fixed_zero_inertia.append(link_name)
new_attrs = " ".join(
f'{c}="{ZERO_INERTIA_FIX if c in ("ixx", "iyy", "izz") else "0"}"'
for c in _INERTIA_COMPONENTS
)
block = block[: im.start()] + f"<inertia {new_attrs}/>" + block[im.end():]
return block
def normalize_urdf(urdf: str) -> Tuple[str, NormalizeReport]:
"""归一化入口。返回 (新 URDF 文本, 报告)。
纯文本替换，不改动输入里其它任何字节 —— 重写整份 XML（如用
ElementTree 再序列化）会顺带改变空白、属性顺序甚至语义，风险不值得担。
"""
report = NormalizeReport()
parts: List[str] = []
pos = 0
for m in _LINK_BLOCK.finditer(urdf):
parts.append(urdf[pos:m.start()])
block = m.group(0)
link_name = "?"
# --- 问题一：孤儿 inertial 三件套 ---
if "<inertial>" not in block:
def repl(mm: re.Match) -> str:
ind = mm.group("ind")
return (
f"{ind}<inertial>\n"
f"{ind} <origin{mm.group('o')}/>\n"
f"{ind} <mass{mm.group('m')}/>\n"
f"{ind} <inertia{mm.group('i')}/>\n"
f"{ind}</inertial>"
)
block, n = _TRIPLE.subn(repl, block)
report.wrapped_inertials += n
# --- 问题二：零质量 link ---
lm = re.match(r'<link\b[^>]*name\s*=\s*"([^"]*)"', block)
if lm:
link_name = lm.group(1)
block = _fix_zero_mass(block, report, link_name)
parts.append(block)
pos = m.end()
parts.append(urdf[pos:])
return "".join(parts), report
def normalize_file(src: str, dst: str) -> NormalizeReport:
"""读文件 → 归一化 → 写文件。"""
with open(src, "r", encoding="utf-8") as fh:
text = fh.read()
out, report = normalize_urdf(text)
with open(dst, "w", encoding="utf-8") as fh:
fh.write(out)
return report
def main(argv: List[str] | None = None) -> int:
"""命令行入口：只看归一化做了什么，不启动 Gazebo。
排查"Gazebo 说我的 link 不存在"时，先跑这个比直接起仿真快得多：
ros2 run arm_sim_gazebo urdf_normalize <urdf> [输出路径]
"""
import argparse
import sys
parser = argparse.ArgumentParser(
prog="urdf_normalize",
description="把 URDF 归一化成 Gazebo 可接受的形态（补 <inertial> 包裹、补零质量）。",
)
parser.add_argument("src", help="输入 URDF 路径")
parser.add_argument(
"dst", nargs="?", help="输出路径（默认写到 stdout，便于直接 grep）"
)
parser.add_argument(
"--quiet", action="store_true", help="不打印归一化摘要"
)
args = parser.parse_args(argv)
with open(args.src, "r", encoding="utf-8") as fh:
text = fh.read()
out, report = normalize_urdf(text)
if not args.quiet:
print(f"[urdf_normalize] {report.summary()}", file=sys.stderr)
if args.dst:
with open(args.dst, "w", encoding="utf-8") as fh:
fh.write(out)
else:
sys.stdout.write(out)
return 0
if __name__ == "__main__":
raise SystemExit(main())
src/arm_sim_gazebo/package.xml
+48
View File
@@ -0,0 +1,48 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>arm_sim_gazebo</name>
<version>0.1.0</version>
<description>
Gazebo 仿真后端。兑现 devices/sim/gazebo/manifest.yaml 里声明的
package: arm_sim_gazebo / executable: gazebo_sim_launcher。
职责：
1. 把 devices/sim/gazebo/urdf 下的仿真描述展开成 URDF，
并归一化成 Gazebo 能接受的形态（见 urdf_normalize.py 的长注释）
2. 起 Gazebo、spawn 机械臂、拉控制器、拉 ros_gz 桥
3. 把应用层需要的接口对齐成与真机/mock 完全一致
（控制器名 rizon_arm_controller、服务与话题名不变），
这样应用层代码不需要知道后端是谁
与真机环境严格隔离：本包只在 ros_gz_sim 环境构建（scripts/build_sim.sh），
真机环境不装它。反过来它也不依赖任何真机专属包。
</description>
<maintainer email="dev@example.com">arm_dev</maintainer>
<license>Apache-2.0</license>
<exec_depend>rclpy</exec_depend>
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
<exec_depend>ament_index_python</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>xacro</exec_depend>
<!-- Gazebo 侧（仅仿真环境有） -->
<exec_depend>ros_gz_sim</exec_depend>
<exec_depend>ros_gz_bridge</exec_depend>
<exec_depend>gz_ros2_control</exec_depend>
<exec_depend>controller_manager</exec_depend>
<exec_depend>joint_state_broadcaster</exec_depend>
<exec_depend>joint_trajectory_controller</exec_depend>
<!-- 描述与参数 -->
<exec_depend>flexiv_description</exec_depend>
<exec_depend>arm_control</exec_depend>
<exec_depend>arm_application</exec_depend>
<test_depend>python3-pytest</test_depend>
<test_depend>python3-yaml</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/arm_sim_gazebo/resource/arm_sim_gazebo
View File
src/arm_sim_gazebo/setup.cfg
+12
View File
@@ -0,0 +1,12 @@
[develop]
script_dir=$base/lib/arm_sim_gazebo
[install]
install_scripts=$base/lib/arm_sim_gazebo
[tool:pytest]
# 与 arm_application 同样的处理：Humble 的 launch_testing / launch_ros 两个
# pytest 插件用了旧版 hook 签名，当前 pytest 在加载插件阶段就抛
# PluginValidationError，导致连测试收集都进不去。
# 本包单测不涉及 launch，直接禁掉。
addopts = -p no:launch_testing -p no:launch_ros
testpaths = test
src/arm_sim_gazebo/setup.py
+32
View File
@@ -0,0 +1,32 @@
from setuptools import find_packages, setup
package_name = 'arm_sim_gazebo'
setup(
name=package_name,
version='0.1.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools', 'PyYAML'],
zip_safe=True,
maintainer='arm_dev',
maintainer_email='dev@example.com',
description='Gazebo 仿真后端：描述归一化 + 启动 Gazebo 与 ros2_control 桥接',
license='Apache-2.0',
entry_points={
'console_scripts': [
# 独立小工具：只看归一化做了什么，不启动 Gazebo。
# 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。
'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',
# TODO(P5c-3): manifest 声明的启动器
# devices/sim/gazebo/manifest.yaml -> driver.executable: gazebo_sim_launcher
# 尚未实现，所以这里**先不注册** —— 注册了但模块不存在，
# 构建能过、一运行就 ImportError，不如老实缺着。
# P5c-3 会补上 gazebo_sim_launcher.py 并在此注册。
# 'gazebo_sim_launcher = arm_sim_gazebo.gazebo_sim_launcher:main',
],
},
)
src/arm_sim_gazebo/test/test_urdf_normalize.py
+304
View File
@@ -0,0 +1,304 @@
"""urdf_normalize 的单测。
这些测试锁定的都是**实测踩过的上游缺陷**，不是凭空设想的边界情况：
1. 官方 flexiv_description 的 rizon_arm.xacro 漏了 <inertial> 包裹
→ sdformat 丢掉 link 和它的父关节
2. grav_macro.xacro 里六个 dummy link 质量 0
→ sdformat 惯性叠加除零出 NaN，报的却是父 link
每加一条都对应一个真实现象，所以别把这里的断言当成"实现细节"随手改。
"""
import os
import sys
import pytest
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from arm_sim_gazebo.urdf_normalize import ( # noqa: E402
ZERO_INERTIA_FIX,
ZERO_MASS_FIX_KG,
normalize_urdf,
)
ROBOT_OPEN = (
'<?xml version="1.0"?>\n'
'<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="t">\n'
)
ROBOT_CLOSE = "</robot>\n"
# ---------------------------------------------------------------------------
# 问题一：孤儿 inertial 三件套
# ---------------------------------------------------------------------------
def test_wraps_orphan_triple_in_inertial():
"""漏了 <inertial> 包裹的三件套要被包起来，且一个数字都不能变。"""
urdf = ROBOT_OPEN + (
' <link name="base_link">\n'
' <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0008 0.0883"/>\n'
' <mass value="3.863"/>\n'
' <inertia ixx="0.0188" ixy="0.0" ixz="0.0" iyy="0.019" iyz="0.0002" izz="0.0103"/>\n'
' <visual name="shell"/>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.wrapped_inertials == 1
assert "<inertial>" in out and "</inertial>" in out
# 数值原样保留 —— 归一化只重新分组，绝不发明物理数据
assert 'xyz="0.0 -0.0008 0.0883"' in out
assert 'value="3.863"' in out
assert 'ixx="0.0188"' in out
assert 'iyz="0.0002"' in out
def test_wrapped_block_is_not_touched():
"""已有 <inertial> 的 link 不能被二次包裹。
这是回归测试：早期用一条全局正则做替换，把夹爪那种本来就正确的
<inertial> 又包了一层，sdformat 报
"Inertial element must have a mass element"，整模型被拒。
"""
urdf = ROBOT_OPEN + (
' <link name="grav_base_link">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0.057"/>\n'
' <mass value="1.186"/>\n'
' <inertia ixx="0.003" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.001"/>\n'
' </inertial>\n'
' <visual name="v"/>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.wrapped_inertials == 0
assert out.count("<inertial>") == 1
assert out == urdf, "已规范的 link 应逐字节不变"
def test_link_without_any_inertial_data_is_left_alone():
"""纯坐标系 link（没有 inertial 也没有三件套）不该被动手，
更不该被凭空补出一个 inertial。"""
urdf = ROBOT_OPEN + ' <link name="tool0"/>\n' + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.wrapped_inertials == 0
assert "<inertial>" not in out
assert out == urdf
def test_indentation_is_preserved():
"""包裹后的缩进要跟原文件一致，否则 diff 里全是噪声。"""
urdf = ROBOT_OPEN + (
' <link name="l1">\n'
' <origin xyz="0 0 0" rpy="0 0 0"/>\n'
' <mass value="1.0"/>\n'
' <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>\n'
' </link>\n'
) + ROBOT_CLOSE
out, _ = normalize_urdf(urdf)
assert " <inertial>" in out
assert " <mass" in out # 内部再进一级
# ---------------------------------------------------------------------------
# 问题二：零质量 / 零惯量
# ---------------------------------------------------------------------------
def test_zero_mass_link_gets_tiny_positive_mass():
"""质量 0 的 dummy link 要补成极小正值，否则 sdformat 惯性叠加出 NaN。"""
urdf = ROBOT_OPEN + (
' <link name="left_finger_tip">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="0"/>\n'
' <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.fixed_zero_mass == ["left_finger_tip"]
assert f'<mass value="{ZERO_MASS_FIX_KG}"/>' in out
assert 'mass value="0"' not in out
def test_zero_inertia_tensor_gets_minimal_diagonal():
"""惯量全 0 的张量要补极小对角值，避免退化。"""
urdf = ROBOT_OPEN + (
' <link name="dummy">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="0"/>\n'
' <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.fixed_zero_inertia == ["dummy"]
assert f'ixx="{ZERO_INERTIA_FIX}"' in out
assert f'iyy="{ZERO_INERTIA_FIX}"' in out
assert f'izz="{ZERO_INERTIA_FIX}"' in out
def test_positive_mass_is_never_rewritten():
"""有正常质量的 link 必须逐字节不变 —— 归一化不能偷偷改物理参数。"""
urdf = ROBOT_OPEN + (
' <link name="link1">\n'
' <origin rpy="0 0 0" xyz="0 0 0.1451"/>\n'
' <mass value="3.85"/>\n'
' <inertia ixx="0.0308" ixy="0" ixz="0" iyy="0.031" iyz="0" izz="0.0051"/>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.wrapped_inertials == 1 # 只包了外层
assert rep.fixed_zero_mass == []
assert rep.fixed_zero_inertia == []
assert 'value="3.85"' in out
assert 'ixx="0.0308"' in out
def test_nonzero_offdiagonal_inertia_is_preserved():
"""张量只有部分分量为 0 时不能补 —— 那不是退化张量。"""
urdf = ROBOT_OPEN + (
' <link name="l">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="0"/>\n'
' <inertia ixx="0.01" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.fixed_zero_mass == ["l"]
assert rep.fixed_zero_inertia == [], "ixx 非零，不是退化张量"
assert 'ixx="0.01"' in out
# ---------------------------------------------------------------------------
# 性质
# ---------------------------------------------------------------------------
def test_normalize_is_idempotent():
"""跑两遍等于跑一遍。否则启动器里任何一次重复调用都会把 URDF 越改越乱。"""
urdf = ROBOT_OPEN + (
' <link name="base_link">\n'
' <origin rpy="0 0 0" xyz="0 0 0.0883"/>\n'
' <mass value="3.863"/>\n'
' <inertia ixx="0.0188" ixy="0" ixz="0" iyy="0.019" iyz="0" izz="0.0103"/>\n'
' </link>\n'
' <link name="tip">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="0"/>\n'
' <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
once, rep1 = normalize_urdf(urdf)
twice, rep2 = normalize_urdf(once)
assert once == twice
assert rep1.changed and not rep2.changed
def test_xml_stays_well_formed_after_normalize():
"""归一化后必须是合法 XML —— 这是 sdformat 的第一道关。"""
import xml.etree.ElementTree as ET
urdf = ROBOT_OPEN + (
' <link name="l1">\n'
' <origin xyz="0 0 0" rpy="0 0 0"/>\n'
' <mass value="1.5"/>\n'
' <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>\n'
' </link>\n'
' <joint name="j1" type="fixed">\n'
' <parent link="l1"/><child link="l2"/>\n'
' </joint>\n'
' <link name="l2"/>\n'
) + ROBOT_CLOSE
out, _ = normalize_urdf(urdf)
ET.fromstring(out) # 抛异常即失败
def test_report_summary_is_human_readable():
"""摘要要能直接打印给用户看，说明改了什么。"""
urdf = ROBOT_OPEN + (
' <link name="a">\n'
' <origin xyz="0 0 0" rpy="0 0 0"/>\n'
' <mass value="0"/>\n'
' <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </link>\n'
) + ROBOT_CLOSE
_, rep = normalize_urdf(urdf)
s = rep.summary()
assert "补 <inertial> 包裹 1 处" in s
assert "a" in s
def test_clean_urdf_reports_no_change():
urdf = ROBOT_OPEN + (
' <link name="l">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="2.0"/>\n'
' <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
_, rep = normalize_urdf(urdf)
assert not rep.changed
assert rep.summary() == "URDF 无需归一化"
@pytest.mark.parametrize("mass_text", ["0", "0.0", "0.000"])
def test_all_zero_mass_spellings_are_caught(mass_text):
"""上游可能把 0 写成 0 / 0.0 / 0.000，都要认出来。"""
urdf = ROBOT_OPEN + (
' <link name="l">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
f' <mass value="{mass_text}"/>\n'
' <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
_, rep = normalize_urdf(urdf)
assert rep.fixed_zero_mass == ["l"]
def test_malformed_mass_value_does_not_crash():
"""数值解析失败时不能崩，也不能乱改。"""
urdf = ROBOT_OPEN + (
' <link name="l">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="not-a-number"/>\n'
' <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.fixed_zero_mass == []
assert 'value="not-a-number"' in out