From 83c7a897315805686cefa7250fde1d407173ac63 Mon Sep 17 00:00:00 2001
From: arm_dev <dev@example.com>
Date: Wed, 23 Sep 2026 15:46:08 +0800
Subject: [PATCH] =?UTF-8?q?P5c-5=20=E4=BB=BF=E7=9C=9F=E5=A4=B9=E7=88=AA?=
 =?UTF-8?q?=E5=8F=AF=E5=BC=80=E5=90=88=EF=BC=9AGazebo=20=E4=B8=8D=E6=94=AF?=
 =?UTF-8?q?=E6=8C=81=20mimic=EF=BC=8C=E6=94=B9=E4=B8=BA=E6=98=BE=E5=BC=8F?=
 =?UTF-8?q?=E9=A9=B1=E5=8A=A8=E5=85=A8=E9=83=A8=207=20=E4=B8=AA=E5=85=B3?=
 =?UTF-8?q?=E8=8A=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

三条独立的坑，前两条都在"看起来完全正常"的表象下静默失效：

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>
---
 devices/sim/gazebo/config/controllers.yaml    |  29 +++
 .../gazebo/urdf/arm.sim.ros2_control.xacro    |  46 ++++-
 devices/sim/gazebo/urdf/arm.sim.urdf.xacro    |   2 +-
 .../arm_sim_gazebo/urdf_normalize.py          | 168 +++++++++++++++++-
 .../launch/gazebo_sim.launch.py               |   2 +-
 .../test/test_urdf_normalize.py               | 103 +++++++++++
 6 files changed, 345 insertions(+), 5 deletions(-)

diff --git a/devices/sim/gazebo/config/controllers.yaml b/devices/sim/gazebo/config/controllers.yaml
index 077f91a..d29fc22 100644
--- a/devices/sim/gazebo/config/controllers.yaml
+++ b/devices/sim/gazebo/config/controllers.yaml
@@ -37,6 +37,11 @@ controller_manager:
     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 的位置。
@@ -76,3 +81,27 @@ rizon_arm_controller:
       # 与官方 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
diff --git a/devices/sim/gazebo/urdf/arm.sim.ros2_control.xacro b/devices/sim/gazebo/urdf/arm.sim.ros2_control.xacro
index ee641c9..74eda0c 100644
--- a/devices/sim/gazebo/urdf/arm.sim.ros2_control.xacro
+++ b/devices/sim/gazebo/urdf/arm.sim.ros2_control.xacro
@@ -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>
diff --git a/devices/sim/gazebo/urdf/arm.sim.urdf.xacro b/devices/sim/gazebo/urdf/arm.sim.urdf.xacro
index b425ac5..ce19498 100644
--- a/devices/sim/gazebo/urdf/arm.sim.urdf.xacro
+++ b/devices/sim/gazebo/urdf/arm.sim.urdf.xacro
@@ -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 标签：系统插件 + 材质 + 接触参数（同目录）
diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py b/src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py
index 15db9ce..3ab2ea5 100644
--- a/src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py
+++ b/src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py
@@ -28,6 +28,18 @@ sdformat 拒掉。两类问题，都属于"RViz 容忍、Gazebo 严格"的差异
         link[sim_left_finger_mount]/inertial/pose
     完全看不出是子 link 质量的问题。
 
+问题三：被 mimic 的关节 effort="0"，在 Gazebo 里出不了力
+    sdformat 转 SDF 时会**直接丢掉 <mimic>**（实测生成的 SDF 里
+    "mimic" 出现 0 次），gz-sim 也没有任何 mimic 实现，SDF 规范里
+    根本没有这个元素。于是夹爪那 6 个"靠机械耦合跟随"的手指关节
+    在 Gazebo 里变成互相独立的关节，谁都不跟随谁。
+    更麻烦的是它们的 effort="0"（上游语义：不由电机驱动）——
+    而 gz_ros2_control 的 position 接口是"位置误差 → 力"，
+    力被 effort 上限夹到 0，于是**永远出不了力**：
+    接口 [claimed]、控制器 active、话题有订阅、命令也发得出去，
+    但手指一动不动，只是自由地被重力拽着漂。
+    详见 MIMIC_EFFORT_FIX 处的注释。
+
 =============================================================================
 修法原则
 =============================================================================
@@ -60,6 +72,49 @@ ZERO_MASS_FIX_KG = "1e-4"
 # 惯量全 0 时补的对角值。极致小值只为让张量非退化，不引入可观测的转动惯量。
 ZERO_INERTIA_FIX = "1e-9"
 
+# 被 mimic 的关节在 Gazebo 里失去机械耦合，必须能自己出力，所以把 effort="0"
+# 补成这个值。取 80 Nm：与夹爪那个真正被驱动的 finger_width_joint 的 effort 一致
+# （grav_macro.xacro 里写的就是 80），所以不引入任何新的"凭空数据"，
+# 只是把"上游说这关节不由电机驱动"翻译成"仿真里它必须自己出力"。
+#
+# 为什么需要这条修复（实测，2026-09-23）：
+#   夹爪只有 finger_width_joint 是真关节（effort=80），其余 6 个手指关节
+#   全是 mimic、effort="0"（上游语义：它们靠机械耦合跟随，不由电机驱动）。
+#   而 sdformat **直接丢弃 <mimic>**，gz-sim 也不支持 mimic ——
+#   于是在 Gazebo 里它们是 7 个互相独立的关节，谁都不跟随谁。
+#
+#   症状极具误导性：命令下发成功（接口 [claimed]、控制器 active、话题有订阅），
+#   但手指一动不动，只是自由地被重力拽着漂。原因是 gz_ros2_control 的 position
+#   接口实现是"位置误差 → 力"（实测 position_proportional_gain=0.1），
+#   力被 effort 上限夹到 0，于是永远出不了力。
+#
+#   对照实验：同一套链路下 sim_joint1 收到 0.2 rad 的目标后精确到位（0.19998），
+#   所以控制链路本身没问题，差别只在这 6 个关节的 effort=0。
+MIMIC_EFFORT_FIX = "80"
+
+# =============================================================================
+# 为什么要把 <mimic> 整个从描述里删掉
+# =============================================================================
+# 除了上面的 effort 问题，<mimic> 留在描述里还有第二个隐患：
+#
+#   robot_state_publisher **支持** mimic —— 它会用"主关节的值 × multiplier + offset"
+#   覆盖从动关节的位置，而不是用 /joint_states 里广播的真实值。
+#   而 Gazebo 里这些关节是独立被我们驱动的，真实值与 mimic 公式算出来的值
+#   在**我们没按公式驱动时**（或主关节没跟上时）就不一样。
+#   后果是 TF 与 Gazebo 各说各话，而且不报错。
+#
+# 实测（2026-09-23）：当前版本的 RSP **没有**应用 mimic（TF 跟随真实关节值），
+# 所以现在两边碰巧一致。但这是 RSP 的实现细节，不是契约 —— 它一变，
+# TF 就会静默退回闭合位。
+#
+# 因此归一化时把 <mimic> 删掉，让描述与 Gazebo 的事实一致：
+#   "7 个独立关节，全部由我们显式驱动"。
+# 耦合关系（谁跟随谁、乘多少）由 sim_gripper_adapter 显式负责，
+# 它从**未归一化**的展开结果里读（见 robot_description.mimic_relations）。
+#
+# 删掉的信息不会丢：会记进 NormalizeReport.stripped_mimics 并被打印出来。
+# =============================================================================
+
 # 裸露的 origin + mass + inertia 三件套。
 #
 # ⚠️ 标签之间的空白必须是**可选**的（\s*），不能要求有换行 —— 这是个实测踩到的坑：
@@ -85,6 +140,12 @@ _TRIPLE = re.compile(
 
 _LINK_BLOCK = re.compile(r"<link\b[^>]*>.*?</link>", re.S)
 
+_JOINT_BLOCK = re.compile(r"<joint\b[^>]*>.*?</joint>", re.S)
+_MIMIC_TAG = re.compile(r"<mimic\b")
+
+# <limit effort="0" .../>：只认数值 0（或 0.0 / 0.00…），不碰其它值。
+_LIMIT_ZERO_EFFORT = re.compile(r"<limit\b(?P<attrs>[^>]*?)/>")
+
 _MASS_TAG = re.compile(r"<mass(?P<attrs>[^>]*?)/>")
 _INERTIA_TAG = re.compile(r"<inertia(?P<attrs>[^>]*?)/>")
 _INERTIA_COMPONENTS = ("ixx", "ixy", "ixz", "iyy", "iyz", "izz")
@@ -98,11 +159,18 @@ class NormalizeReport:
     wrapped_inertials: int = 0
     fixed_zero_mass: List[str] = field(default_factory=list)
     fixed_zero_inertia: List[str] = field(default_factory=list)
+    fixed_zero_effort: List[str] = field(default_factory=list)
+    #: 被删掉的 mimic 关系 (关节, 源关节, multiplier, offset)
+    stripped_mimics: List[Tuple[str, str, str, str]] = field(default_factory=list)
 
     @property
     def changed(self) -> bool:
         return bool(
-            self.wrapped_inertials or self.fixed_zero_mass or self.fixed_zero_inertia
+            self.wrapped_inertials
+            or self.fixed_zero_mass
+            or self.fixed_zero_inertia
+            or self.fixed_zero_effort
+            or self.stripped_mimics
         )
 
     def summary(self) -> str:
@@ -118,6 +186,16 @@ class NormalizeReport:
             )
         if self.fixed_zero_inertia:
             bits.append(f"零惯量张量补极小对角 {len(self.fixed_zero_inertia)} 个")
+        if self.fixed_zero_effort:
+            bits.append(
+                f"被 mimic 关节补 effort={MIMIC_EFFORT_FIX} {len(self.fixed_zero_effort)} 个"
+                f"({', '.join(self.fixed_zero_effort)})"
+            )
+        if self.stripped_mimics:
+            bits.append(
+                f"删除 mimic 关系 {len(self.stripped_mimics)} 条"
+                f"（Gazebo 不支持 mimic，改由 sim_gripper_adapter 显式驱动）"
+            )
         return "；".join(bits)
 
 
@@ -169,6 +247,52 @@ def _fix_zero_mass(block: str, report: NormalizeReport, link_name: str) -> str:
     return block
 
 
+def _fix_mimic_joint(block: str, report: NormalizeReport, joint_name: str) -> str:
+    """处理带 <mimic> 的关节：记下耦合关系、补 effort、删掉 mimic 标签。
+
+    为什么三件事一起做：它们都是同一个事实（"Gazebo 不支持 mimic"）的直接后果，
+    分散在三处代码里迟早会有人只改一半。
+      - 记下关系 → 适配器要知道谁跟随谁（信息不能丢，只是搬家）
+      - 补 effort → 关节要能自己出力（见 MIMIC_EFFORT_FIX）
+      - 删 mimic → 让描述与 Gazebo 的事实一致（见文件头那段说明）
+    """
+    m = _MIMIC_TAG.search(block)
+    if m is None:
+        return block
+
+    # 1) 记录耦合关系（从 <mimic .../> 标签上抠出来）
+    tag_end = block.find(">", m.start())
+    tag = block[m.start() : tag_end + 1] if tag_end != -1 else ""
+    attrs = _attrs_of(tag)
+    report.stripped_mimics.append(
+        (
+            joint_name,
+            attrs.get("joint", "?"),
+            attrs.get("multiplier", "1"),
+            attrs.get("offset", "0"),
+        )
+    )
+
+    # 2) 删掉 <mimic .../>（含它前面的缩进，免得留下孤零零的空白行）
+    line_start = block.rfind("\n", 0, m.start()) + 1
+    if block[line_start : m.start()].strip() == "":
+        block = block[:line_start] + block[tag_end + 1 :].lstrip("\n")
+    else:
+        block = block[: m.start()] + block[tag_end + 1 :]
+
+    # 3) effort="0" → 可用值（只动这一个属性，限位是上游的物理事实，不碰）
+    lm = _LIMIT_ZERO_EFFORT.search(block)
+    if lm is not None:
+        lattrs = _attrs_of(lm.group("attrs"))
+        if _num(lattrs.get("effort", "")) == 0.0:
+            report.fixed_zero_effort.append(joint_name)
+            lattrs["effort"] = MIMIC_EFFORT_FIX
+            new = "<limit " + " ".join(f'{k}="{v}"' for k, v in lattrs.items()) + "/>"
+            block = block[: lm.start()] + new + block[lm.end() :]
+
+    return block
+
+
 def normalize_urdf(urdf: str) -> Tuple[str, NormalizeReport]:
     """归一化入口。返回 (新 URDF 文本, 报告)。
 
@@ -214,7 +338,47 @@ def normalize_urdf(urdf: str) -> Tuple[str, NormalizeReport]:
         pos = m.end()
 
     parts.append(urdf[pos:])
-    return "".join(parts), report
+    out = "".join(parts)
+
+    # --- 问题三：被 mimic 的关节 effort="0" ---
+    # 放在 link 处理之后单独一遍：<joint> 不会嵌套在 <link> 里，顺序无关，
+    # 而且这样不必把两套正则揉进同一个循环。
+    def joint_repl(mm: re.Match) -> str:
+        block = mm.group(0)
+        if not _MIMIC_TAG.search(block):
+            return block
+        jm = re.match(r'<joint\b[^>]*name\s*=\s*"([^"]*)"', block)
+        return _fix_mimic_joint(block, report, jm.group(1) if jm else "?")
+
+    out = _JOINT_BLOCK.sub(joint_repl, out)
+    return out, report
+
+
+def mimic_relations(urdf: str) -> List[Tuple[str, str, str, str]]:
+    """从（**未归一化的**）URDF 文本里读出所有 mimic 关系。
+
+    返回 [(关节, 源关节, multiplier, offset)]，数值以字符串给出（原样保留，
+    由调用方决定怎么解释）。归一化会删掉 <mimic>，所以必须在归一化**之前**读 ——
+    这也是这个函数放在本模块而不是适配器里的原因：谁删的谁负责提供读取入口。
+    """
+    out: List[Tuple[str, str, str, str]] = []
+    for m in _JOINT_BLOCK.finditer(urdf):
+        block = m.group(0)
+        mm_ = _MIMIC_TAG.search(block)
+        if mm_ is None:
+            continue
+        jm = re.match(r'<joint\b[^>]*name\s*=\s*"([^"]*)"', block)
+        tag_end = block.find(">", mm_.start())
+        attrs = _attrs_of(block[mm_.start() : tag_end + 1] if tag_end != -1 else "")
+        out.append(
+            (
+                jm.group(1) if jm else "?",
+                attrs.get("joint", "?"),
+                attrs.get("multiplier", "1"),
+                attrs.get("offset", "0"),
+            )
+        )
+    return out
 
 
 def normalize_file(src: str, dst: str) -> NormalizeReport:
diff --git a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py
index 1819b63..0c2d9d2 100644
--- a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py
+++ b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py
@@ -60,7 +60,7 @@ from launch_ros.substitutions import FindPackageShare
 SPAWN_DELAY = 4.0
 CONTROLLER_DELAY = 8.0
 
-CONTROLLER_NAMES = ("joint_state_broadcaster", "rizon_arm_controller")
+CONTROLLER_NAMES = ("joint_state_broadcaster", "rizon_arm_controller", "gripper_controller")
 
 
 def _prepare(context, *args, **kwargs):
diff --git a/src/arm_sim_gazebo/test/test_urdf_normalize.py b/src/arm_sim_gazebo/test/test_urdf_normalize.py
index 52ddaa5..555ea18 100644
--- a/src/arm_sim_gazebo/test/test_urdf_normalize.py
+++ b/src/arm_sim_gazebo/test/test_urdf_normalize.py
@@ -347,3 +347,106 @@ def test_malformed_mass_value_does_not_crash():
 
     assert rep.fixed_zero_mass == []
     assert 'value="not-a-number"' in out
+
+
+# =============================================================================
+# 问题三：Gazebo 不支持 mimic（sdformat 直接丢弃 <mimic>）
+# =============================================================================
+# 背景：夹爪只有 finger_width_joint 是真关节，其余手指关节靠 mimic 跟随。
+# sdformat 丢掉 mimic 后它们成了独立关节，而它们的 <limit effort="0"> 让它们
+# 在 gz_ros2_control（位置误差 → 力）下永远出不了力。
+# 实测：命令下发成功、接口已 claim、控制器 active，但手指一动不动。
+# 归一化因此做两件事：补 effort、删 mimic（改由适配器显式驱动）。
+# =============================================================================
+
+MIMIC_JOINT = (
+    '<joint name="left_outer_knuckle_joint" type="revolute">'
+    '<parent link="gripper_base"/><child link="left_outer_bar"/>'
+    '<axis xyz="1 0 0"/>'
+    '<limit effort="0" lower="-0.155" upper="0.7854" velocity="2.0"/>'
+    '<mimic joint="finger_width_joint" multiplier="9.404" offset="-0.155"/>'
+    "</joint>"
+)
+
+
+def test_mimic_joint_gets_usable_effort():
+    """effort="0" 的 mimic 关节必须被补成可用的力上限。"""
+    from arm_sim_gazebo.urdf_normalize import MIMIC_EFFORT_FIX
+
+    out, rep = normalize_urdf(ROBOT_OPEN + MIMIC_JOINT + ROBOT_CLOSE)
+
+    assert rep.fixed_zero_effort == ["left_outer_knuckle_joint"]
+    assert f'effort="{MIMIC_EFFORT_FIX}"' in out
+    assert 'effort="0"' not in out
+
+
+def test_mimic_limits_are_preserved():
+    """只改 effort：限位是上游的物理事实，一个数字都不能动。"""
+    out, _ = normalize_urdf(ROBOT_OPEN + MIMIC_JOINT + ROBOT_CLOSE)
+
+    assert 'lower="-0.155"' in out
+    assert 'upper="0.7854"' in out
+    assert 'velocity="2.0"' in out
+
+
+def test_mimic_tag_is_stripped_and_recorded():
+    """mimic 标签要删掉，但耦合关系必须被记下来（信息只是搬家，不能丢）。"""
+    out, rep = normalize_urdf(ROBOT_OPEN + MIMIC_JOINT + ROBOT_CLOSE)
+
+    assert "<mimic" not in out
+    assert rep.stripped_mimics == [
+        ("left_outer_knuckle_joint", "finger_width_joint", "9.404", "-0.155")
+    ]
+
+
+def test_nonzero_effort_joint_is_untouched():
+    """effort 非 0 的关节不碰 —— 那只说明上游表达了别的东西。
+
+    特别是主关节 finger_width_joint（effort=80），它是 mimic 的**源**、
+    本身没有 <mimic> 标签，不该被本规则命中。
+    """
+    urdf = (
+        ROBOT_OPEN
+        + '<joint name="finger_width_joint" type="revolute">'
+        '<parent link="gripper_base"/><child link="unused"/>'
+        '<limit effort="80" lower="0" upper="0.100" velocity="0.2"/>'
+        "</joint>"
+        + ROBOT_CLOSE
+    )
+
+    out, rep = normalize_urdf(urdf)
+
+    assert rep.fixed_zero_effort == []
+    assert rep.stripped_mimics == []
+    assert 'effort="80"' in out
+
+
+def test_joint_without_mimic_is_never_touched_even_if_effort_zero():
+    """没有 mimic 的关节即便 effort=0 也不碰 —— 本轮修复只针对 mimic 场景，
+    别的关节为什么是 0 是另一回事，不该被这条规则悄悄改掉。"""
+    urdf = (
+        ROBOT_OPEN
+        + '<joint name="some_joint" type="revolute">'
+        '<parent link="a"/><child link="b"/>'
+        '<limit effort="0" lower="0" upper="1" velocity="1"/>'
+        "</joint>"
+        + ROBOT_CLOSE
+    )
+
+    out, rep = normalize_urdf(urdf)
+
+    assert rep.fixed_zero_effort == []
+    assert 'effort="0"' in out
+
+
+def test_mimic_relations_reads_from_unnormalized_urdf():
+    """mimic_relations 必须在归一化**之前**读 —— 归一化后标签已经没了。"""
+    from arm_sim_gazebo.urdf_normalize import mimic_relations
+
+    raw = ROBOT_OPEN + MIMIC_JOINT + ROBOT_CLOSE
+    normalized, _ = normalize_urdf(raw)
+
+    assert mimic_relations(raw) == [
+        ("left_outer_knuckle_joint", "finger_width_joint", "9.404", "-0.155")
+    ]
+    assert mimic_relations(normalized) == []  # 归一化后读不到了，这是预期的