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>
91 lines
4.6 KiB
XML
91 lines
4.6 KiB
XML
<?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>
|