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
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>