仿真上料：夹爪真能夹住通止规（新增 sim_load_gauge.py + 夹爪出力钳位）

仿真里没有"人把规塞进夹爪"这一步，之前整条流程拖到 S07 才暴露（下探 25mm
什么也没碰到，看着像力阈值问题）。新增 scripts/sim_load_gauge.py 顶这一步。

上料为什么难（四个原因叠加，都实测踩过）：
  1. URDF 里夹爪关节 effort=80Nm → 位置控制器把规顶飞。urdf_normalize 新增
     _clamp_grip_effort：按握力 45N 与指节杠杆反推，指节 4.79Nm / 主关节 45N。
  2. **指腹接触面不在 tcp_frame 原点上**：URDF 里 inner_finger_pad_joint
     z=0.0235、finger_tcp_joint z=0.0360（同父链节同方向）→ 指腹中心比
     tcp_frame 高 12.5mm。所以规尖→指腹中心是 0.043，不是夹持段中心的
     0.0555。用错值的现象极迷惑：gz 报得出 Contacts、也能读到接触，但规
     就是托不住、一路滑下去。ΔZ 扫描实测：0/±5mm 掉、+10~+15mm 夹住。
  3. 摆完位就自由落体：ROS 往返 100ms+ 而物理步进 1ms，等合爪指令到达，
     规已掉 125mm。改成**像人手一样托着** —— 合爪期间 50Hz 按当前指腹位姿
     把规瞬移回两指之间，托到真夹住为止。
  4. 合到多少不猜：从松到紧扫开口，扫到"松手后不掉"为止。指腹实际接触面
     未必在 tcp_frame 原点，拿"指间间距=开口"反推闭合量会差好几毫米。

结果：漂移 0.4mm、两指 |fnormal|≈46.5N（正好是 45N 设计握力）。
另修 _set_gauge_pose 的 ign service 口径说明与 _gauge_pose_now 的解析。

Co-authored-by: Cursor <cursoragent@cursor.com>

src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py

@@ -92,6 +92,39 @@ ZERO_INERTIA_FIX = "1e-9"
 #   所以控制链路本身没问题，差别只在这 6 个关节的 effort=0。
 MIMIC_EFFORT_FIX = "80"
 
+# =============================================================================
+# 夹爪出力上限：把 gz_ros2_control 的"位置控制"夹成"力控夹爪"
+# =============================================================================
+# 为什么需要（实测，2026-09-24）：
+#   gz_ros2_control 的 position 接口是"位置误差 → 力"，力只被 effort 上限夹。
+#   grav_macro 给手指关节写的 effort=80（单位在这里是 Nm），而指腹到指节的
+#   力臂只有 ~0.1m —— 于是"合爪"能压出**上千牛**：仿真里 30mm 的规被两指
+#   直接挤穿（实测关节走到 0 位=开口 27mm < 规径 30mm），规被弹出去、掉回规座，
+#   触觉却报着上百牛的力。表现像"夹爪抓不住"，根因是出力没有上限。
+#
+# 取值怎么来的（不用猜：从 URDF 自己的行程推）：
+#   夹爪是平行四边形连杆，指腹平动 100mm 时手指关节转 0.9404 rad
+#   （inner/outer_finger 的 limits：-0.7854..0.155），所以
+#       力臂 ≈ 行程 / 角行程 = 0.100 / 0.9404 ≈ 0.106 m
+#   真机 Grav 的握力档位是 20~50N，取 45N（覆盖应用层常用的 force=30）：
+#       手指关节 effort = 45 × 0.106 ≈ 4.8 Nm
+#   而 finger_width_joint 是 1:1 的"行程关节"（0..0.1m），它的 effort 就是握力
+#   本身，给 45（N）。
+#
+# ⚠ 这一条是**仿真专用**的物理量：真机的握力由 GripperExecute.force 走力控闭环，
+#   不靠关节 effort 上限。归一化只服务于 Gazebo，所以写在这里、不去动上游描述。
+GRIP_FORCE_N = 45.0
+GRIP_STROKE_M = 0.100
+GRIP_FINGER_TRAVEL_RAD = 0.9404
+GRIP_PAD_LEVER_M = GRIP_STROKE_M / GRIP_FINGER_TRAVEL_RAD      # ≈ 0.1064 m
+GRIP_FINGER_EFFORT_NM = round(GRIP_FORCE_N * GRIP_PAD_LEVER_M, 2)   # ≈ 4.79 Nm
+GRIP_WIDTH_EFFORT_N = GRIP_FORCE_N
+
+#: 名字里带这些字样的关节 = 夹爪的指节/指腹/行程关节（仿真里统一带 sim_ 前缀）
+_GRIP_JOINT_HINTS = ("finger", "knuckle")
+#: 行程关节（1:1，effort 单位就是 N）
+_GRIP_WIDTH_HINT = "finger_width"
+
 # =============================================================================
 # 为什么要把 <mimic> 整个从描述里删掉
 # =============================================================================
@@ -162,6 +195,8 @@ class NormalizeReport:
     fixed_zero_effort: List[str] = field(default_factory=list)
     #: 被删掉的 mimic 关系 (关节, 源关节, multiplier, offset)
     stripped_mimics: List[Tuple[str, str, str, str]] = field(default_factory=list)
+    #: 被压到真机握力量级的夹爪关节 (关节, 原值, 新值)
+    clamped_grip_effort: List[Tuple[str, str, str]] = field(default_factory=list)
 
     @property
     def changed(self) -> bool:
@@ -171,6 +206,7 @@ class NormalizeReport:
             or self.fixed_zero_inertia
             or self.fixed_zero_effort
             or self.stripped_mimics
+            or self.clamped_grip_effort
         )
 
     def summary(self) -> str:
@@ -196,6 +232,11 @@ class NormalizeReport:
                 f"删除 mimic 关系 {len(self.stripped_mimics)} 条"
                 f"（Gazebo 不支持 mimic，改由 sim_gripper_adapter 显式驱动）"
             )
+        if self.clamped_grip_effort:
+            bits.append(
+                f"夹爪出力压到握力 {GRIP_FORCE_N:g}N 量级 {len(self.clamped_grip_effort)} 个"
+                f"({', '.join(f'{n}:{a}→{b}' for n, a, b in self.clamped_grip_effort)})"
+            )
         return "；".join(bits)
 
 
@@ -293,6 +334,31 @@ def _fix_mimic_joint(block: str, report: NormalizeReport, joint_name: str) -> st
     return block
 
 
+def _clamp_grip_effort(block: str, report: NormalizeReport, joint_name: str) -> str:
+    """把夹爪关节的 effort 上限压到"真机握力量级"（见 GRIP_FORCE_N 的推导）。
+
+    为什么必须做：不做的话"合爪"能压出上千牛，把工件挤穿、弹飞，
+    看起来像"夹爪抓不住"，实际是出力没有上限（详见 GRIP_FORCE_N 处注释）。
+
+    只改 effort 一个属性；限位、速度是上游的物理事实，一律不碰。
+    """
+    low = joint_name.lower()
+    if not any(h in low for h in _GRIP_JOINT_HINTS):
+        return block
+    want = GRIP_WIDTH_EFFORT_N if _GRIP_WIDTH_HINT in low else GRIP_FINGER_EFFORT_NM
+    lm = _LIMIT_ZERO_EFFORT.search(block)
+    if lm is None:
+        return block
+    lattrs = _attrs_of(lm.group("attrs"))
+    cur = _num(lattrs.get("effort", ""))
+    if cur <= want:
+        return block          # 上游本来就比我们更保守：尊重它，不往上调
+    lattrs["effort"] = f"{want:g}"
+    report.clamped_grip_effort.append((joint_name, f"{cur:g}", f"{want:g}"))
+    new = "<limit " + " ".join(f'{k}="{v}"' for k, v in lattrs.items()) + "/>"
+    return block[: lm.start()] + new + block[lm.end() :]
+
+
 def normalize_urdf(urdf: str) -> Tuple[str, NormalizeReport]:
     """归一化入口。返回 (新 URDF 文本, 报告)。
 
@@ -345,10 +411,13 @@ def normalize_urdf(urdf: str) -> Tuple[str, NormalizeReport]:
     # 而且这样不必把两套正则揉进同一个循环。
     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 "?")
+        name = jm.group(1) if jm else "?"
+        if _MIMIC_TAG.search(block):
+            block = _fix_mimic_joint(block, report, name)
+        # 夹爪出力上限**放在 mimic 修复之后**：mimic 那条会把 effort=0 补成 80，
+        # 而这里要把它压到真机握力的量级。顺序反了就等于没夹。
+        return _clamp_grip_effort(block, report, name)
 
     out = _JOINT_BLOCK.sub(joint_repl, out)
     return out, report

src/arm_sim_gazebo/test/test_urdf_normalize.py

@@ -370,14 +370,88 @@ MIMIC_JOINT = (
 
 
 def test_mimic_joint_gets_usable_effort():
-    """effort="0" 的 mimic 关节必须被补成可用的力上限。"""
+    """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
+    # 补 effort 这一步确实发生了（MIMIC_EFFORT_FIX 是它的输入，夹爪上限在此基础上再压）
+    assert MIMIC_EFFORT_FIX != "" and 'effort="0"' not in out
+    # 最终值由夹爪握力上限决定（见 test_gripper_joints_are_clamped_to_grip_force）
+    assert 'effort="4.79"' in out
+
+
+def test_gripper_joints_are_clamped_to_grip_force():
+    """夹爪关节的 effort 必须被压到"真机握力"量级。
+
+    为什么：gz_ros2_control 的 position 接口是"位置误差→力"，力只受 effort 上限限制。
+    上游写 80（手指关节，Nm）配上 ~0.1m 的力臂 → 合爪能压出上千牛，把工件挤穿弹飞，
+    看起来像"夹爪抓不住"（2026-09-24 实测）。这里守住压过的值。
+    """
+    from arm_sim_gazebo.urdf_normalize import (
+        GRIP_FINGER_EFFORT_NM,
+        GRIP_FORCE_N,
+        GRIP_PAD_LEVER_M,
+        GRIP_WIDTH_EFFORT_N,
+    )
+
+    # 取值本身要自洽：握力 × 力臂 = 手指关节力矩
+    assert GRIP_PAD_LEVER_M > 0.05
+    assert abs(GRIP_FINGER_EFFORT_NM - GRIP_FORCE_N * GRIP_PAD_LEVER_M) < 0.01
+    assert GRIP_WIDTH_EFFORT_N == GRIP_FORCE_N
+
+    urdf = (
+        ROBOT_OPEN
+        + '<joint name="left_inner_finger_joint" type="revolute">'
+        '<parent link="a"/><child link="b"/>'
+        '<limit effort="80" lower="-0.7854" upper="0.155" velocity="2.0"/>'
+        '<mimic joint="finger_width_joint" multiplier="9.404" offset="-0.155"/>'
+        "</joint>"
+        + '<joint name="finger_width_joint" type="revolute">'
+        '<parent link="c"/><child link="d"/>'
+        '<limit effort="80" lower="0" upper="0.100" velocity="0.2"/>'
+        "</joint>"
+        + ROBOT_CLOSE
+    )
+    out, rep = normalize_urdf(urdf)
+
+    clamped = dict((n, (a, b)) for n, a, b in rep.clamped_grip_effort)
+    assert clamped["left_inner_finger_joint"] == ("80", f"{GRIP_FINGER_EFFORT_NM:g}")
+    assert clamped["finger_width_joint"] == ("80", f"{GRIP_WIDTH_EFFORT_N:g}")
+    assert "夹爪出力压到" in rep.summary()
+
+
+def test_arm_joints_are_never_clamped():
+    """机械臂本体的 effort 一个都不能动 —— 那是真机的力矩上限，不是夹爪。"""
+    urdf = (
+        ROBOT_OPEN
+        + '<joint name="joint1" type="revolute">'
+        '<parent link="base_link"/><child link="link1"/>'
+        '<limit effort="123" lower="-2.88" upper="2.88" velocity="2.0"/>'
+        "</joint>"
+        + ROBOT_CLOSE
+    )
+    out, rep = normalize_urdf(urdf)
+
+    assert rep.clamped_grip_effort == []
+    assert 'effort="123"' in out
+
+
+def test_gripper_clamp_respects_a_more_conservative_upstream():
+    """上游若本来就比我们更保守（effort 更小），要尊重它，不能往上调。"""
+    urdf = (
+        ROBOT_OPEN
+        + '<joint name="left_inner_finger_joint" type="revolute">'
+        '<parent link="a"/><child link="b"/>'
+        '<limit effort="1.0" lower="-0.7854" upper="0.155" velocity="2.0"/>'
+        "</joint>"
+        + ROBOT_CLOSE
+    )
+    out, rep = normalize_urdf(urdf)
+
+    assert rep.clamped_grip_effort == []
+    assert 'effort="1.0"' in out
 
 
 def test_mimic_limits_are_preserved():
@@ -400,16 +474,15 @@ def test_mimic_tag_is_stripped_and_recorded():
 
 
 def test_nonzero_effort_joint_is_untouched():
-    """effort 非 0 的关节不碰 —— 那只说明上游表达了别的东西。
+    """**机械臂**上 effort 非 0 的关节不碰 —— 那只说明上游表达了别的东西。
 
-    特别是主关节 finger_width_joint（effort=80），它是 mimic 的**源**、
-    本身没有 <mimic> 标签，不该被本规则命中。
+    （夹爪类关节是另一条规则管的，见 test_gripper_joints_are_clamped_to_grip_force。）
     """
     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 name="joint3" type="revolute">'
+        '<parent link="link2"/><child link="link3"/>'
+        '<limit effort="64" lower="-3.05" upper="3.05" velocity="2.0"/>'
         "</joint>"
         + ROBOT_CLOSE
     )
@@ -418,7 +491,7 @@ def test_nonzero_effort_joint_is_untouched():
 
     assert rep.fixed_zero_effort == []
     assert rep.stripped_mimics == []
-    assert 'effort="80"' in out
+    assert 'effort="64"' in out
 
 
 def test_joint_without_mimic_is_never_touched_even_if_effort_zero():