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>
153 lines
8.4 KiB
XML
153 lines
8.4 KiB
XML
<?xml version="1.0" encoding="utf-8"?>
|
||
<!--
|
||
=============================================================================
|
||
仿真版机器人描述（Gazebo 用）。
|
||
|
||
用法（由 arm_sim_gazebo 的 gazebo_sim_launcher 调用，一般不需要手敲）：
|
||
xacro /abs/path/devices/sim/gazebo/urdf/arm.sim.urdf.xacro \
|
||
controllers_file:=/abs/path/devices/sim/gazebo/config/controllers.yaml
|
||
|
||
注意：本文件在 devices/ 下，而 devices/ **不是 ROS 包**，所以不能用
|
||
$(find ...) 找同目录的兄弟文件。这里用 xacro 的相对包含 —— xacro 会把
|
||
相对路径按"当前正在处理的文件所在目录"解析，因此同目录文件直接写文件名即可。
|
||
（绝对路径也可以，启动器就是这么传 controllers_file 的。）
|
||
|
||
============================================================================
|
||
设计：复用官方描述，只补仿真缺的那部分
|
||
============================================================================
|
||
连杆几何 / 惯性 / 碰撞体全部来自官方 flexiv_description，一行都不复制：
|
||
<xacro:rizon_robot ...> 会用 Rizon4 的四个 YAML 生成
|
||
joint_limits.yaml → 关节限位（与真机一致，仿真才有意义）
|
||
default_kinematics.yaml → 各关节原点位姿（决定 TCP 能不能够到工件）
|
||
inertia_parameters.yaml → 真实质量与惯量张量（Gazebo 物理必需）
|
||
visual_parameters.yaml → mesh 视觉 + 凸包碰撞
|
||
这一点很关键：如果仿真自己造一套物理参数，那"仿真里调好的力阈值搬到真机
|
||
就不成立"，整个仿真也就失去价值了。
|
||
|
||
仿真额外要补的只有三样，都在本文件里显式写清：
|
||
1. gz_ros2_control 硬件 + 系统插件（官方宏只认 mock / 真机两种）
|
||
2. tcp / tool0 两个应用层帧（官方描述只有 flange）
|
||
3. 外挂传感器（相机/触觉，见 arm.sim.sensors.xacro）
|
||
|
||
============================================================================
|
||
关节前缀为什么是 sim_
|
||
============================================================================
|
||
真实臂前缀是 Rizon4-063989_。仿真若沿用同一前缀，两边同时跑时 TF 和
|
||
/joint_states 会互相污染 —— 症状是 RViz 里模型乱跳、控制器收到别人的关节角。
|
||
仿真用独立的 sim_ 前缀 → 天然隔离，可以真机仿真同开对比。
|
||
注意这不止是"改个名字"：控制器名（rizon_arm_controller）**故意保持与真机一致**，
|
||
因为那是应用层依赖的接口契约；隔离的是关节/TF 层，不是接口层。
|
||
=============================================================================
|
||
-->
|
||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rizon_sim">
|
||
|
||
<!-- 机型。Rizon4 / Rizon4s / Rizon4M / Rizon4R / Rizon10 / Rizon10s -->
|
||
<xacro:arg name="rizon_type" default="Rizon4"/>
|
||
|
||
<!--
|
||
仿真关节前缀。改成空串则与真机同前缀（只在单跑仿真、且不介意与真机串味时用）。
|
||
robot_sn 传空串，配合 arm_prefix=sim，官方宏算出的前缀就是 "sim_"。
|
||
官方前缀算法：(arm_prefix + '_' + robot_sn + '_') 若两者都非空，
|
||
否则 (arm_prefix + robot_sn + '_')。
|
||
-->
|
||
<xacro:arg name="arm_prefix" default="sim"/>
|
||
<xacro:arg name="robot_sn" default=""/>
|
||
|
||
<!-- 控制器配置文件的绝对路径（由启动器按仓库根动态拼出来传入） -->
|
||
<xacro:arg name="controllers_file" default=""/>
|
||
|
||
<!-- 是否挂夹爪。换夹爪品牌时这里改（或由启动器从 site.yaml 映射过来）。
|
||
注意官方描述里夹爪只有 Flexiv-GN01 一种（grav_macro），换别的品牌
|
||
需要另写一份描述 —— 见 devices/grippers/ 的说明。 -->
|
||
<xacro:arg name="load_gripper" default="true"/>
|
||
|
||
<!-- 是否挂外挂传感器（相机/触觉）。关掉可做纯运动学验证，跑得更快。 -->
|
||
<xacro:arg name="with_sensors" default="true"/>
|
||
|
||
<!--
|
||
传感器参数直接由启动器传入，不在这里读 YAML。
|
||
原因：读 YAML 需要绝对路径，而 xacro 无法自己推导仓库根；且"配置→参数"的
|
||
映射统一放在启动器里（那是唯一知道 site.yaml 和仓库布局的地方），
|
||
调试时看一处就够，不用在 xacro / YAML / Python 三处来回找。
|
||
-->
|
||
<xacro:arg name="sensors_file" default=""/>
|
||
|
||
<xacro:property name="ap" value="$(arg arm_prefix)"/>
|
||
<xacro:property name="sn" value="$(arg robot_sn)"/>
|
||
<xacro:property name="prefix"
|
||
value="${(ap + '_' + sn + '_') if ap != '' and sn != '' else (ap + sn + '_') if (ap + sn) != '' else ''}"/>
|
||
|
||
<!-- =========================================================================
|
||
官方描述：7 轴本体 + 真实惯性 + 凸包碰撞
|
||
=========================================================================
|
||
ros2_control:=false —— 官方宏的 ros2_control 分支只能给 mock 或真机插件，
|
||
仿真要 gz_ros2_control，所以这里关掉、下面自己写。
|
||
add_world_link / create_base_joint 保持 true：
|
||
它们会生成一个空的 world link 和 world→base_link 的固定关节，
|
||
把机械臂焊在世界上。这是必要的 —— 不焊住，仿真一开始整条臂就砸到地面。
|
||
注：ros_gz_sim create 没有 =static 之类选项（实测确认），
|
||
所以"固定基座"只能在描述文件里做。
|
||
-->
|
||
<xacro:include filename="$(find flexiv_description)/urdf/common/rizon_robot.xacro"/>
|
||
|
||
<xacro:rizon_robot
|
||
rizon_type="$(arg rizon_type)"
|
||
robot_sn="$(arg robot_sn)"
|
||
arm_prefix="$(arg arm_prefix)"
|
||
joint_limits_parameters_file="$(find flexiv_description)/config/$(arg rizon_type)/joint_limits.yaml"
|
||
kinematics_parameters_file="$(find flexiv_description)/config/$(arg rizon_type)/default_kinematics.yaml"
|
||
inertia_parameters_file="$(find flexiv_description)/config/$(arg rizon_type)/inertia_parameters.yaml"
|
||
visual_parameters_file="$(find flexiv_description)/config/$(arg rizon_type)/visual_parameters.yaml"
|
||
load_gripper="$(arg load_gripper)"
|
||
gripper_name="Flexiv-GN01"
|
||
load_mounted_ft_sensor="false"
|
||
ros2_control="false"
|
||
add_world_link="true"
|
||
create_base_joint="true"
|
||
/>
|
||
|
||
<!-- =========================================================================
|
||
应用层帧：tcp / tool0
|
||
=========================================================================
|
||
与真机 arm.urdf.xacro 的定义保持一致（都是 flange 上的零偏移固定帧）。
|
||
应用层代码硬编码查 tcp / tool0（见 arm_application 的 TF 查询），
|
||
仿真里这两个帧必须存在，否则跟踪和柔顺下压会在查 TF 时失败。
|
||
tool0 是 tcp 的别名，历史原因两者都在用，别删任何一个 ——
|
||
删了会有一部分节点静默查不到帧。
|
||
-->
|
||
<joint name="${prefix}tcp_joint" type="fixed">
|
||
<parent link="${prefix}flange"/>
|
||
<child link="${prefix}tcp"/>
|
||
</joint>
|
||
<link name="${prefix}tcp"/>
|
||
|
||
<joint name="${prefix}tool0_joint" type="fixed">
|
||
<parent link="${prefix}tcp"/>
|
||
<child link="${prefix}tool0"/>
|
||
</joint>
|
||
<link name="${prefix}tool0"/>
|
||
|
||
<!-- =========================================================================
|
||
ros2_control：gz_ros2_control 硬件（同目录）
|
||
========================================================================= -->
|
||
<xacro:include filename="arm.sim.ros2_control.xacro"/>
|
||
<xacro:arm_sim_ros2_control prefix="${prefix}" load_gripper="$(arg load_gripper)"/>
|
||
|
||
<!-- =========================================================================
|
||
Gazebo 标签：系统插件 + 材质 + 接触参数（同目录）
|
||
========================================================================= -->
|
||
<xacro:include filename="arm.sim.gazebo.xacro"/>
|
||
<xacro:arm_sim_gazebo prefix="${prefix}"
|
||
controllers_file="$(arg controllers_file)"/>
|
||
|
||
<!-- =========================================================================
|
||
外挂传感器（相机 + 触觉，同目录）
|
||
========================================================================= -->
|
||
<xacro:if value="$(arg with_sensors)">
|
||
<xacro:include filename="arm.sim.sensors.xacro"/>
|
||
<xacro:arm_sim_sensors prefix="${prefix}"
|
||
sensors_file="$(arg sensors_file)"/>
|
||
</xacro:if>
|
||
|
||
</robot>
|