From 25635e21f8ac4b61fcbb518fa6da7834462d5d6f Mon Sep 17 00:00:00 2001 From: arm_dev Date: Wed, 23 Sep 2026 14:19:39 +0800 Subject: [PATCH] =?UTF-8?q?P5c-2=EF=BC=9A=E4=BB=BF=E7=9C=9F=E4=B8=96?= =?UTF-8?q?=E7=95=8C=20+=20=E6=9C=BA=E5=99=A8=E4=BA=BA=E6=8F=8F=E8=BF=B0?= =?UTF-8?q?=20+=20=E6=8E=A7=E5=88=B6=E5=99=A8=E9=85=8D=E7=BD=AE=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E8=A7=A3=E5=86=B3=E4=B8=A4=E5=A4=84=E5=AE=98=E6=96=B9?= =?UTF-8?q?=E6=8F=8F=E8=BF=B0=E4=B8=8E=20Gazebo=20=E7=9A=84=E4=B8=8D?= =?UTF-8?q?=E5=85=BC=E5=AE=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 这一步解决的是"官方 Flexiv 描述能不能喂给 Gazebo"的问题。结论是**不能直接用**， 有两处不兼容，且都只有 Gazebo 会暴露 —— RViz/MoveIt 完全正常，所以此前从未发现。 问题一：7 轴本体的质量/惯量是"孤儿标签" rizon_arm.xacro 调 get_inertias 时没写 包裹，而那个宏只吐 // 三个碎片。RViz/MoveIt 忽略不认识的子元素所以看着 没事；sdformat 判定 "link has no 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 项单测，含"已有 不能被二次包裹"（早期用全局正则踩过， 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 的 —— 所以启动器必须先起 robot_state_publisher 且发布归一化后的同一份 URDF，否则控制器注册的关节名 会和 Gazebo 里的对不上。这条已写进 P5c-3 的实现要求。 Co-authored-by: Cursor --- config/site.example.yaml | 12 +- devices/sim/gazebo/config/controllers.yaml | 78 +++++ devices/sim/gazebo/manifest.yaml | 38 ++- devices/sim/gazebo/urdf/arm.sim.gazebo.xacro | 90 ++++++ .../gazebo/urdf/arm.sim.ros2_control.xacro | 63 ++++ devices/sim/gazebo/urdf/arm.sim.sensors.xacro | 189 +++++++++++ devices/sim/gazebo/urdf/arm.sim.urdf.xacro | 152 +++++++++ devices/sim/gazebo/worlds/assembly.world | 238 ++++++++++++++ src/arm_sim_gazebo/arm_sim_gazebo/__init__.py | 12 + .../arm_sim_gazebo/urdf_normalize.py | 250 ++++++++++++++ src/arm_sim_gazebo/package.xml | 48 +++ src/arm_sim_gazebo/resource/arm_sim_gazebo | 0 src/arm_sim_gazebo/setup.cfg | 12 + src/arm_sim_gazebo/setup.py | 32 ++ .../test/test_urdf_normalize.py | 304 ++++++++++++++++++ 15 files changed, 1506 insertions(+), 12 deletions(-) create mode 100644 devices/sim/gazebo/config/controllers.yaml create mode 100644 devices/sim/gazebo/urdf/arm.sim.gazebo.xacro create mode 100644 devices/sim/gazebo/urdf/arm.sim.ros2_control.xacro create mode 100644 devices/sim/gazebo/urdf/arm.sim.sensors.xacro create mode 100644 devices/sim/gazebo/urdf/arm.sim.urdf.xacro create mode 100644 devices/sim/gazebo/worlds/assembly.world create mode 100644 src/arm_sim_gazebo/arm_sim_gazebo/__init__.py create mode 100644 src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py create mode 100644 src/arm_sim_gazebo/package.xml create mode 100644 src/arm_sim_gazebo/resource/arm_sim_gazebo create mode 100644 src/arm_sim_gazebo/setup.cfg create mode 100644 src/arm_sim_gazebo/setup.py create mode 100644 src/arm_sim_gazebo/test/test_urdf_normalize.py diff --git a/config/site.example.yaml b/config/site.example.yaml index a0c91e0..327844c 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -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_" # 仿真关节前缀，必须与真实臂前缀区分 diff --git a/devices/sim/gazebo/config/controllers.yaml b/devices/sim/gazebo/config/controllers.yaml new file mode 100644 index 0000000..077f91a --- /dev/null +++ b/devices/sim/gazebo/config/controllers.yaml @@ -0,0 +1,78 @@ +# ============================================================================= +# devices/sim/gazebo/config/controllers.yaml +# +# Gazebo 里 controller_manager 的控制器配置。由 arm.sim.gazebo.xacro 的系统插件 +# 通过 指过来（路径由启动器按仓库根拼成绝对路径）。 +# +# ----------------------------------------------------------------------------- +# 硬约束：控制器名不能改 +# ----------------------------------------------------------------------------- +# 应用层（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 diff --git a/devices/sim/gazebo/manifest.yaml b/devices/sim/gazebo/manifest.yaml index 6d77a7f..cba5e19 100644 --- a/devices/sim/gazebo/manifest.yaml +++ b/devices/sim/gazebo/manifest.yaml @@ -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 轴本体缺 包裹；夹爪有零质量 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.*）， diff --git a/devices/sim/gazebo/urdf/arm.sim.gazebo.xacro b/devices/sim/gazebo/urdf/arm.sim.gazebo.xacro new file mode 100644 index 0000000..7263c55 --- /dev/null +++ b/devices/sim/gazebo/urdf/arm.sim.gazebo.xacro @@ -0,0 +1,90 @@ + + + + + + + + + + ${controllers_file} + + + + + Gazebo/DarkGrey + Gazebo/Red + Gazebo/Blue + Gazebo/Green + Gazebo/Yellow + Gazebo/Orange + Gazebo/Purple + Gazebo/Grey + + + + 0.6 + 0.6 + 1e7 + 1e4 + 0.001 + 0.01 + Gazebo/Black + + + 0.6 + 0.6 + 1e7 + 1e4 + 0.001 + 0.01 + + + + + diff --git a/devices/sim/gazebo/urdf/arm.sim.ros2_control.xacro b/devices/sim/gazebo/urdf/arm.sim.ros2_control.xacro new file mode 100644 index 0000000..ee641c9 --- /dev/null +++ b/devices/sim/gazebo/urdf/arm.sim.ros2_control.xacro @@ -0,0 +1,63 @@ + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/devices/sim/gazebo/urdf/arm.sim.sensors.xacro b/devices/sim/gazebo/urdf/arm.sim.sensors.xacro new file mode 100644 index 0000000..b6b63f7 --- /dev/null +++ b/devices/sim/gazebo/urdf/arm.sim.sensors.xacro @@ -0,0 +1,189 @@ + + + + + + + + + + + + + + + + + + + + + ${image_hz} + true + + false + + + 1.5184 + + ${image_width} + ${image_height} + R8G8B8 + + + + 0.02 + 10.0 + + + 0.0210.0 + + + + /d405 + + + + + + + + + + + + + + + + + + + + + + + + + true + 100 + + + ${prefix}tactile_left_link_collision + + /tactile/left/raw + + + + + + true + 100 + + ${prefix}tactile_right_link_collision + + /tactile/right/raw + + + + + + + diff --git a/devices/sim/gazebo/urdf/arm.sim.urdf.xacro b/devices/sim/gazebo/urdf/arm.sim.urdf.xacro new file mode 100644 index 0000000..b425ac5 --- /dev/null +++ b/devices/sim/gazebo/urdf/arm.sim.urdf.xacro @@ -0,0 +1,152 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/devices/sim/gazebo/worlds/assembly.world b/devices/sim/gazebo/worlds/assembly.world new file mode 100644 index 0000000..d24133c --- /dev/null +++ b/devices/sim/gazebo/worlds/assembly.world @@ -0,0 +1,238 @@ + + + + + + + + 0.001 + 1.0 + 1000 + + + bullet + + + quick + 150 + 1.3 + + + 0.0 + 0.2 + 0.1 + 0.001 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + true + + + + 0 0 110 10 + + + + 0.80.8 + + + + + + 0 0 110 10 + + + 0.7 0.7 0.7 1 + 0.7 0.7 0.7 1 + + + + + + + + true + 0.55 0 0 0 0 0 + + 0 0 0.375 0 0 0 + + 0.8 1.2 0.05 + + + 0.80.8 + + + + 1e7 + 1e4 + 0.01 + 0.001 + + + + + + 0.8 1.2 0.05 + + 0.45 0.4 0.35 1 + 0.55 0.5 0.42 1 + + + + + + 0 0 0.175 0 0 0 + + 0.7 1.1 0.35 + + 0.3 0.3 0.3 1 + 0.35 0.35 0.35 1 + + + + + + + + true + 0.15 0.35 0.755 0 0 0 + + + 0.040.004 + + 0.1 0.6 0.9 1 + 0.1 0.6 0.9 1 + + + + + + + + ogre2 + + + + + true + -0.45 0.0 1.35 0.0 0.872665 3.14159 + + + 0.09 0.025 0.025 + + 0.15 0.15 0.15 1 + 0.2 0.2 0.2 1 + + + + 30 + true + false + + + 1.5184 + + 640 + 480 + R8G8B8 + + 0.110.0 + + 0.110.0 + + + + /d435 + + + + + + diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/__init__.py b/src/arm_sim_gazebo/arm_sim_gazebo/__init__.py new file mode 100644 index 0000000..12c0498 --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/__init__.py @@ -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"] diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py b/src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py new file mode 100644 index 0000000..b8caec4 --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py @@ -0,0 +1,250 @@ +"""把厂商给的 URDF 归一化成"Gazebo 能接受"的形态。 + +============================================================================= +为什么需要这一步（全是实测踩出来的） +============================================================================= +官方 flexiv_description 是给 RViz / MoveIt 用的，直接喂 Gazebo 会被 +sdformat 拒掉。两类问题，都属于"RViz 容忍、Gazebo 严格"的差异， +所以这个框架在有 Gazebo 之前一直没暴露： + +问题一：7 轴本体的质量/惯量是"孤儿标签" + rizon_arm.xacro 里这样调： + + + get_inertias 宏只吐 / / 三个碎片，**外面没有 + 包裹**。URDF 规范里 不能直接有这三个子元素。 + RViz/MoveIt 忽略不认识的子元素 → 看起来一切正常 + sdformat 判定 "link has no 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 质量的问题。 + +============================================================================= +修法原则 +============================================================================= +只做"重新分组 + 补极小量"，不发明任何物理数据，也不掩盖真问题： + - 孤儿三件套 → 只是把已有元素包进 ，一个数字都不改 + - 零质量 → 补成 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 三件套（自闭合标签，连续三行）。 +# 夹爪的 块内部也是这三行，所以必须按 link 块处理并跳过 +# 已含 的块，否则会二次包裹 —— sdformat 会报 +# "Inertial element must have a mass element"（已踩过）。 +_TRIPLE = re.compile( + r"(?P[ \t]*)[^>]*?)/>[ \t]*\n" + r"[ \t]*[^>]*?)/>[ \t]*\n" + r"[ \t]*[^>]*?)/>" +) + +_LINK_BLOCK = re.compile(r"]*>.*?", re.S) + +_MASS_TAG = re.compile(r"[^>]*?)/>") +_INERTIA_TAG = re.compile(r"[^>]*?)/>") +_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"补 包裹 {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: + """把 补成极小正值。 + + 两个前提：这个 link 有 （否则不是"质量为零"而是"没有质量信息"， + 不归这里管），且质量为数值 0。 + """ + if "" 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'' + 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"" + 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 "" not in block: + def repl(mm: re.Match) -> str: + ind = mm.group("ind") + return ( + f"{ind}\n" + f"{ind} \n" + f"{ind} \n" + f"{ind} \n" + f"{ind}" + ) + + block, n = _TRIPLE.subn(repl, block) + report.wrapped_inertials += n + + # --- 问题二：零质量 link --- + lm = re.match(r']*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 [输出路径] + """ + import argparse + import sys + + parser = argparse.ArgumentParser( + prog="urdf_normalize", + description="把 URDF 归一化成 Gazebo 可接受的形态（补 包裹、补零质量）。", + ) + 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()) diff --git a/src/arm_sim_gazebo/package.xml b/src/arm_sim_gazebo/package.xml new file mode 100644 index 0000000..68ee959 --- /dev/null +++ b/src/arm_sim_gazebo/package.xml @@ -0,0 +1,48 @@ + + + + arm_sim_gazebo + 0.1.0 + + 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）， + 真机环境不装它。反过来它也不依赖任何真机专属包。 + + arm_dev + Apache-2.0 + + rclpy + launch + launch_ros + ament_index_python + robot_state_publisher + xacro + + ros_gz_sim + ros_gz_bridge + gz_ros2_control + controller_manager + joint_state_broadcaster + joint_trajectory_controller + + flexiv_description + arm_control + arm_application + + python3-pytest + python3-yaml + + + ament_python + + diff --git a/src/arm_sim_gazebo/resource/arm_sim_gazebo b/src/arm_sim_gazebo/resource/arm_sim_gazebo new file mode 100644 index 0000000..e69de29 diff --git a/src/arm_sim_gazebo/setup.cfg b/src/arm_sim_gazebo/setup.cfg new file mode 100644 index 0000000..9c2916c --- /dev/null +++ b/src/arm_sim_gazebo/setup.cfg @@ -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 diff --git a/src/arm_sim_gazebo/setup.py b/src/arm_sim_gazebo/setup.py new file mode 100644 index 0000000..e38d24a --- /dev/null +++ b/src/arm_sim_gazebo/setup.py @@ -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', + ], + }, +) diff --git a/src/arm_sim_gazebo/test/test_urdf_normalize.py b/src/arm_sim_gazebo/test/test_urdf_normalize.py new file mode 100644 index 0000000..ecb76d8 --- /dev/null +++ b/src/arm_sim_gazebo/test/test_urdf_normalize.py @@ -0,0 +1,304 @@ +"""urdf_normalize 的单测。 + +这些测试锁定的都是**实测踩过的上游缺陷**，不是凭空设想的边界情况： + 1. 官方 flexiv_description 的 rizon_arm.xacro 漏了 包裹 + → 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 = ( + '\n' + '\n' +) +ROBOT_CLOSE = "\n" + + +# --------------------------------------------------------------------------- +# 问题一：孤儿 inertial 三件套 +# --------------------------------------------------------------------------- +def test_wraps_orphan_triple_in_inertial(): + """漏了 包裹的三件套要被包起来，且一个数字都不能变。""" + urdf = ROBOT_OPEN + ( + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ) + ROBOT_CLOSE + + out, rep = normalize_urdf(urdf) + + assert rep.wrapped_inertials == 1 + assert "" in out and "" 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(): + """已有 的 link 不能被二次包裹。 + + 这是回归测试：早期用一条全局正则做替换，把夹爪那种本来就正确的 + 又包了一层，sdformat 报 + "Inertial element must have a mass element"，整模型被拒。 + """ + urdf = ROBOT_OPEN + ( + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ) + ROBOT_CLOSE + + out, rep = normalize_urdf(urdf) + + assert rep.wrapped_inertials == 0 + assert out.count("") == 1 + assert out == urdf, "已规范的 link 应逐字节不变" + + +def test_link_without_any_inertial_data_is_left_alone(): + """纯坐标系 link（没有 inertial 也没有三件套）不该被动手， + 更不该被凭空补出一个 inertial。""" + urdf = ROBOT_OPEN + ' \n' + ROBOT_CLOSE + + out, rep = normalize_urdf(urdf) + + assert rep.wrapped_inertials == 0 + assert "" not in out + assert out == urdf + + +def test_indentation_is_preserved(): + """包裹后的缩进要跟原文件一致，否则 diff 里全是噪声。""" + urdf = ROBOT_OPEN + ( + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ) + ROBOT_CLOSE + + out, _ = normalize_urdf(urdf) + + assert " " in out + assert " \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ) + ROBOT_CLOSE + + out, rep = normalize_urdf(urdf) + + assert rep.fixed_zero_mass == ["left_finger_tip"] + assert f'' in out + assert 'mass value="0"' not in out + + +def test_zero_inertia_tensor_gets_minimal_diagonal(): + """惯量全 0 的张量要补极小对角值，避免退化。""" + urdf = ROBOT_OPEN + ( + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \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 + ( + ' \n' + ' \n' + ' \n' + ' \n' + ' \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 + ( + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \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 + ( + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \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 + ( + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ) + ROBOT_CLOSE + + out, _ = normalize_urdf(urdf) + ET.fromstring(out) # 抛异常即失败 + + +def test_report_summary_is_human_readable(): + """摘要要能直接打印给用户看，说明改了什么。""" + urdf = ROBOT_OPEN + ( + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ) + ROBOT_CLOSE + + _, rep = normalize_urdf(urdf) + s = rep.summary() + + assert "补 包裹 1 处" in s + assert "a" in s + + +def test_clean_urdf_reports_no_change(): + urdf = ROBOT_OPEN + ( + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \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 + ( + ' \n' + ' \n' + ' \n' + f' \n' + ' \n' + ' \n' + ' \n' + ) + ROBOT_CLOSE + + _, rep = normalize_urdf(urdf) + + assert rep.fixed_zero_mass == ["l"] + + +def test_malformed_mass_value_does_not_crash(): + """数值解析失败时不能崩，也不能乱改。""" + urdf = ROBOT_OPEN + ( + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ' \n' + ) + ROBOT_CLOSE + + out, rep = normalize_urdf(urdf) + + assert rep.fixed_zero_mass == [] + assert 'value="not-a-number"' in out