ros_contorl

orisys/ros_contorl

Fork 0

Commit Graph

Author	SHA1	Message	Date
arm_dev	83c7a89731	P5c-5 仿真夹爪可开合：Gazebo 不支持 mimic，改为显式驱动全部 7 个关节三条独立的坑，前两条都在"看起来完全正常"的表象下静默失效： 1) 夹爪在仿真里是死的（关节级）夹爪只有 finger_width_joint 是真关节（effort=80），其余 6 个手指关节全是 mimic、effort="0"（上游语义：靠机械耦合跟随，不由电机驱动）。而 sdformat 转 SDF 时直接丢弃 <mimic>（实测生成 SDF 里 "mimic" 出现 0 次），gz-sim 也没有任何 mimic 实现，SDF 规范里就没有这个元素。于是 7 个关节在 Gazebo 里互相独立、谁都不跟随谁。 2) 补了 effort 才有用（力度） gz_ros2_control 的 position 接口是"位置误差 → 力" （实测 position_proportional_gain=0.1），力被 effort 上限夹到 0 → 永远出不了力。症状极隐蔽：接口 [claimed]、控制器 active、话题有订阅、命令也发得出去，但手指一动不动，只是自由地被重力拽着漂。对照实验：同一套链路下 sim_joint1 收到 0.2 rad 后精确到位（0.19998），所以链路本身没问题，差别只在这 6 个关节的 effort=0。 3) 描述与 Gazebo 的事实不一致（语义） robot_state_publisher 支持 mimic，会用主关节的值覆盖从动关节；而 Gazebo 里它们是独立被驱动的。实测当前 RSP 没有应用 mimic（TF 跟随真实值），所以碰巧一致 —— 但这是实现细节，它一变 TF 就静默退回闭合位。改动： - ros2_control 增加夹爪 7 个关节的接口（含 load_gripper 开关） - controllers.yaml 增加 gripper_controller（JointGroupPositionController） - 启动器 spawn 它；并重命名归一化第三条修复 - urdf_normalize 第三条修复：被 mimic 关节补 effort=80、删除 <mimic> 并记录耦合关系；新增 mimic_relations() 供适配器在归一化前读取（信息只是搬家，不能丢） - 新增 6 项单测锁定上述行为验收（实测）：命令 width 0→0.1，指尖间距 27.1mm → 127.0mm（约 100mm 行程，与真机 GRAV 规格吻合），7 个关节实测值均命中命令值，TF 与之一致。 Co-authored-by: Cursor <cursoragent@cursor.com>	2026-09-23 15:46:08 +08:00
arm_dev	25635e21f8	P5c-2：仿真世界 + 机器人描述 + 控制器配置，并解决两处官方描述与 Gazebo 的不兼容这一步解决的是"官方 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>	2026-09-23 14:19:39 +08:00

Author

SHA1

Message

Date

arm_dev

83c7a89731

P5c-5 仿真夹爪可开合：Gazebo 不支持 mimic，改为显式驱动全部 7 个关节

三条独立的坑，前两条都在"看起来完全正常"的表象下静默失效：

1) 夹爪在仿真里是死的（关节级）
   夹爪只有 finger_width_joint 是真关节（effort=80），其余 6 个手指关节
   全是 mimic、effort="0"（上游语义：靠机械耦合跟随，不由电机驱动）。
   而 sdformat 转 SDF 时**直接丢弃 <mimic>**（实测生成 SDF 里 "mimic"
   出现 0 次），gz-sim 也没有任何 mimic 实现，SDF 规范里就没有这个元素。
   于是 7 个关节在 Gazebo 里互相独立、谁都不跟随谁。

2) 补了 effort 才有用（力度）
   gz_ros2_control 的 position 接口是"位置误差 → 力"
   （实测 position_proportional_gain=0.1），力被 effort 上限夹到 0 →
   永远出不了力。症状极隐蔽：接口 [claimed]、控制器 active、话题有订阅、
   命令也发得出去，但手指一动不动，只是自由地被重力拽着漂。
   对照实验：同一套链路下 sim_joint1 收到 0.2 rad 后精确到位（0.19998），
   所以链路本身没问题，差别只在这 6 个关节的 effort=0。

3) 描述与 Gazebo 的事实不一致（语义）
   robot_state_publisher 支持 mimic，会用主关节的值覆盖从动关节；而 Gazebo
   里它们是独立被驱动的。实测当前 RSP 没有应用 mimic（TF 跟随真实值），
   所以碰巧一致 —— 但这是实现细节，它一变 TF 就静默退回闭合位。

改动：
  - ros2_control 增加夹爪 7 个关节的接口（含 load_gripper 开关）
  - controllers.yaml 增加 gripper_controller（JointGroupPositionController）
  - 启动器 spawn 它；并重命名归一化第三条修复
  - urdf_normalize 第三条修复：被 mimic 关节补 effort=80、删除 <mimic>
    并记录耦合关系；新增 mimic_relations() 供适配器在归一化前读取
    （信息只是搬家，不能丢）
  - 新增 6 项单测锁定上述行为

验收（实测）：命令 width 0→0.1，指尖间距 27.1mm → 127.0mm（约 100mm 行程，
与真机 GRAV 规格吻合），7 个关节实测值均命中命令值，TF 与之一致。

Co-authored-by: Cursor <cursoragent@cursor.com>

2026-09-23 15:46:08 +08:00

arm_dev

25635e21f8

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

这一步解决的是"官方 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>

2026-09-23 14:19:39 +08:00

2 Commits