arm_dev
83c7a89731
三条独立的坑，前两条都在"看起来完全正常"的表象下静默失效：
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>
108 lines
6.0 KiB
XML
108 lines
6.0 KiB
XML
<?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 load_gripper:=true">
|
||
|
||
<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"/>
|
||
|
||
<!-- =====================================================================
|
||
夹爪：所有 7 个关节都要接口 —— 因为 Gazebo 不支持 mimic
|
||
=====================================================================
|
||
⚠️ 这是本项目第二深的坑（第一是触觉的 contact-system），务必看懂：
|
||
|
||
官方描述里夹爪**只有 1 个真关节**，其余 6 个都是 mimic：
|
||
finger_width_joint ← 唯一被驱动的"宽度"关节
|
||
├─ left_outer_knuckle_joint = 9.404 * width - 0.155 (mimic)
|
||
│ ├─ left_inner_knuckle_joint = 1 * 左外指节 (mimic)
|
||
│ ├─ left_inner_finger_joint = -1 * 左外指节 (mimic)
|
||
│ ├─ right_outer_knuckle_joint = 1 * 左外指节 (mimic)
|
||
│ ├─ right_inner_knuckle_joint = 1 * 左外指节 (mimic)
|
||
│ └─ right_inner_finger_joint = -1 * 左外指节 (mimic)
|
||
（见 src/flexiv_description/urdf/common/grav_macro.xacro:244 起）
|
||
|
||
问题：**sdformat 转 SDF 时会直接丢掉 <mimic>**（实测：生成的 SDF 里
|
||
"mimic" 出现 0 次），gz-sim 也没有任何 mimic 实现，SDF 规范里压根
|
||
没有这个元素。所以：
|
||
|
||
在 Gazebo 里，这 7 个关节是**互相独立**的转动关节；
|
||
在 RViz/TF 里，robot_state_publisher **支持** mimic，手指会跟随。
|
||
|
||
只驱动 finger_width_joint 的后果：RViz 里夹爪在开合、Gazebo 里
|
||
手指一动不动（还挂在重力下垂着），而两边**都不报错**。抓取必然失败，
|
||
且现象极具误导性。
|
||
|
||
因此这里把 7 个关节全部暴露成可驱动，由 sim_gripper_adapter 在启动时
|
||
从 URDF **读出 mimic 的 multiplier/offset**，自己算出 6 个从动关节的
|
||
角度一起下发（不写死数值 —— 上游改参数就自动跟随）。
|
||
这样 Gazebo 与 RViz 的行为重新一致，且不依赖 sdformat 的 mimic 支持。
|
||
|
||
验证方法：launch 后 /joint_states 里应能看到这 7 个夹爪关节；
|
||
让适配器开到最大，Gazebo 里两根手指必须真的张开（RViz 里也应一致）。
|
||
===================================================================== -->
|
||
<xacro:if value="${load_gripper}">
|
||
<xacro:arm_sim_joint name="${prefix}finger_width_joint"/>
|
||
<xacro:arm_sim_joint name="${prefix}left_outer_knuckle_joint"/>
|
||
<xacro:arm_sim_joint name="${prefix}left_inner_knuckle_joint"/>
|
||
<xacro:arm_sim_joint name="${prefix}left_inner_finger_joint"/>
|
||
<xacro:arm_sim_joint name="${prefix}right_outer_knuckle_joint"/>
|
||
<xacro:arm_sim_joint name="${prefix}right_inner_knuckle_joint"/>
|
||
<xacro:arm_sim_joint name="${prefix}right_inner_finger_joint"/>
|
||
</xacro:if>
|
||
|
||
</ros2_control>
|
||
|
||
</xacro:macro>
|
||
|
||
</robot>
|