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
5.1 KiB
YAML
108 lines
5.1 KiB
YAML
# =============================================================================
|
||
# devices/sim/gazebo/config/controllers.yaml
|
||
#
|
||
# Gazebo 里 controller_manager 的控制器配置。由 arm.sim.gazebo.xacro 的系统插件
|
||
# 通过 <parameters> 指过来（路径由启动器按仓库根拼成绝对路径）。
|
||
#
|
||
# -----------------------------------------------------------------------------
|
||
# 硬约束：控制器名不能改
|
||
# -----------------------------------------------------------------------------
|
||
# 应用层（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
|
||
|
||
# 夹爪。名字是仿真内部的事（应用层通过 GripperExecute 服务访问夹爪，
|
||
# 不直接碰这个控制器），所以可以自由命名。
|
||
gripper_controller:
|
||
type: position_controllers/JointGroupPositionController
|
||
|
||
# --- 关节状态广播 -------------------------------------------------------------
|
||
# 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
|
||
|
||
# --- 夹爪控制器 ---------------------------------------------------------------
|
||
# ⚠️ 为什么这里要 7 个关节，而真机只需要 1 个"宽度"关节：
|
||
# Gazebo 不支持 URDF 的 <mimic>（sdformat 转 SDF 时直接丢掉），
|
||
# 所以 6 个从动关节在仿真里不会自动跟随，必须由 sim_gripper_adapter
|
||
# 算出角度后**一起下发**。详见 arm.sim.ros2_control.xacro 里的长注释。
|
||
#
|
||
# 用 JointGroupPositionController（一次收一组位置）而不是 JTC：
|
||
# 夹爪是"两态"动作（开/合），不需要轨迹插值和时间参数化，
|
||
# JointGroupPositionController 的语义更直接、也少一层容错。
|
||
# 它订阅 ~/commands (std_msgs/Float64MultiArray)，顺序 = 下面的 joints 顺序。
|
||
#
|
||
# 顺序约定：第 0 个是主关节 finger_width_joint，其余 6 个从动关节按
|
||
# 左右、由外到内排列。适配器按同一顺序填数组（它自己也会校验）。
|
||
gripper_controller:
|
||
ros__parameters:
|
||
joints:
|
||
- sim_finger_width_joint
|
||
- sim_left_outer_knuckle_joint
|
||
- sim_left_inner_knuckle_joint
|
||
- sim_left_inner_finger_joint
|
||
- sim_right_outer_knuckle_joint
|
||
- sim_right_inner_knuckle_joint
|
||
- sim_right_inner_finger_joint
|