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>
79 lines
3.6 KiB
YAML
79 lines
3.6 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
|
||
|
||
# --- 关节状态广播 -------------------------------------------------------------
|
||
# 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
|