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>
This commit is contained in:
@@ -23,7 +23,7 @@
|
||||
-->
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="arm_sim_ros2_control" params="prefix joint_count:=7">
|
||||
<xacro:macro name="arm_sim_ros2_control" params="prefix joint_count:=7 load_gripper:=true">
|
||||
|
||||
<ros2_control name="GazeboSimSystem" type="system">
|
||||
<hardware>
|
||||
@@ -56,6 +56,50 @@
|
||||
<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>
|
||||
|
||||
@@ -131,7 +131,7 @@
|
||||
ros2_control:gz_ros2_control 硬件(同目录)
|
||||
========================================================================= -->
|
||||
<xacro:include filename="arm.sim.ros2_control.xacro"/>
|
||||
<xacro:arm_sim_ros2_control prefix="${prefix}"/>
|
||||
<xacro:arm_sim_ros2_control prefix="${prefix}" load_gripper="$(arg load_gripper)"/>
|
||||
|
||||
<!-- =========================================================================
|
||||
Gazebo 标签:系统插件 + 材质 + 接触参数(同目录)
|
||||
|
||||
Reference in New Issue
Block a user