仿真夹爪适配器：服务接口对齐真机，开口按正运动学换算成真毫米
为什么
------
应用层只认 arm_msgs/srv/GripperExecute（开口毫米），真机与仿真必须同一套接口。
但 GN01 的 URDF 里 <mimic> 是**线性近似**：直接按线性系数下发 finger_width_joint，
"命令开 20 mm"在 Gazebo 里量出来是 21.75 mm —— 抓不住工件，而且看起来像算法问题。
实测（2026-09-24，Gazebo 内量两指尖间距）：
线性下发 20 mm → 实际 21.74 mm（偏差 1.74 mm，超 2 mm 抓取容差的前半步）
走 FK 反解 → 实际 20.00 mm（≤0.5 mm）
怎么改
------
- 新增 sim_gripper_kin.py：解析 URDF 正运动学，算出"主关节值 ↔ 真实开口 mm"的
单调曲线，反解用二分。曲线非单调/零行程时**主动报错**，不静默退化成线性。
- sim_gripper.py：开口换算全部走该曲线；容差统一改成毫米。
- sim_gripper_adapter.py：反馈改取**从动关节**（见下）；is_gripped 按毫米判定。
- 新增 check_sim_gripper.py：三层验收（服务/开口一致/指尖真实几何行程 + 对称性）。
踩到的坑
--------
- finger_width_joint 挂在无质量的 unused_finger_width_link 上，与手指**没有机械
连接**：命令到 0.100，它的 /joint_states 恒为 1.4e-08，6 个从动关节才准到
0.7854。反馈若取它，/gripper/status 会永远报"开口 0 mm"。
- 残留进程会伪造出"代码写错了"的现场：旧 sim_gripper_adapter 与新实例抢话题，
验收读到"指尖间距 0.03 mm"（真实 127 mm）；孤儿 static_transform_publisher
用 latched /tf_static 把旧相机外参按在新仿真上。
→ 新增 scripts/lib/sim_procs.sh；run_sim_gz.sh 用 setsid 独立进程组，
PGID 记进 log_sim/sim.pgid，stop 时整组 kill；启动前预检拒绝带病启动。
- 验收脚本自己也错过两次（把 TF 的米当毫米比 → 127 mm 显示成 0.13 mm；
用会转动指垫的中点漂移判对称 → 必然位移 16 mm）。判据改成"两侧各走多远"，
单位只在 _dist_mm 里换算一次。
验证
----
python3 -m pytest src/arm_sim_gazebo/test/ -q → 307 passed
python3 scripts/check_sim_gripper.py → 三层全过
第 2 层：请求/反馈开口最大偏差 < 0.5 mm
第 3 层：期望行程 vs 实际行程、左右行程差，均 < 1.5 mm
bash scripts/stop_sim_gz.sh --list → 整组列出（含 static tf）
文档
----
- 仿真手册：新增 §4.4（开口 mm 怎么换算）、坑 12（宽度关节不动、
反馈必须取从动关节）；停止一节改写成进程组语义。
- FRAMEWORK_RULES：新增 R4.11（残留进程会伪造现场）、R4.12（验收判据要挑
与实现无关的物理量）；R4.9 表补夹爪；清单补 13/14 两条。
- HANDBOOK：新增 §3.4「在仿真里开合夹爪」，补 §4.4 的两条新教训。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -0,0 +1,618 @@
|
||||
"""仿真夹爪的耦合关系、行程换算与命令向量拼装（纯逻辑，不起 ROS）。
|
||||
|
||||
=============================================================================
|
||||
为什么单独一个模块
|
||||
=============================================================================
|
||||
把"夹爪服务（mm）"变成"7 个关节的目标角（弧度/米）"，真正有内容的是四件事：
|
||||
|
||||
1. 耦合关系 从动关节 = multiplier × 主关节 + offset
|
||||
（上游 GN01 描述里主关节是 ``finger_width_joint``，
|
||||
6 个从动关节按各自的 multiplier 跟随）
|
||||
2. 行程换算 应用层说的是**开口毫米**，Gazebo 要的是**关节值**（米）
|
||||
3. 命令顺序 JointGroupPositionController 收到的数组顺序必须与
|
||||
controllers.yaml 里 ``joints:`` 完全一致
|
||||
4. 夹住判据 真机有状态字，仿真只能靠"指令到了下限、实际值没跟上"推断
|
||||
|
||||
四件都是纯数据变换，不需要 ROS 跑起来就能验。所以实现放这里、用普通函数表达，
|
||||
节点（sim_gripper_adapter.py）只做订阅/发布这层壳。这样"multiplier 用错"
|
||||
这种错误才能被单测钉住，而不是靠"看起来夹住了"。
|
||||
|
||||
=============================================================================
|
||||
为什么 Gazebo 里必须由适配器驱动 7 个关节
|
||||
=============================================================================
|
||||
sdformat 把 URDF 的 ``<mimic>`` **整个丢掉**，gz-sim 也不实现 mimic。
|
||||
所以仿真里 6 个从动关节不会跟随主关节，必须由适配器算出角度后**一起下发**。
|
||||
``urdf_normalize`` 在归一化时把 ``<mimic>`` 删掉（避免 robot_state_publisher
|
||||
和 Gazebo 各说各话），删掉的信息记在 ``NormalizeReport.stripped_mimics`` 里，
|
||||
launch 再把那张表作为参数交给本适配器 —— 这样"耦合关系"只有一个来源。
|
||||
|
||||
=============================================================================
|
||||
毫米 ↔ 关节值的口径（最容易搞错的一处）
|
||||
=============================================================================
|
||||
``finger_width_joint`` 是上游描述里的**虚拟宽度关节**（注释说它"本应是
|
||||
prismatic，但为了配合 mimic 只能是 revolute"），单位是**米**，限位 **0~0.100**，
|
||||
语义就是开口宽度：0 = 两指合拢，0.100 = 开口 100 mm。
|
||||
|
||||
本适配器采用**绝对（物理）映射**：命令"开口 8 mm"，仿真里两指就真的相距 8 mm。
|
||||
（而不是归一化映射 —— 那种把"100% 行程"当成全开，会让中间开口全部失真，
|
||||
仿真就失去"拿真几何试算法"的意义。）
|
||||
|
||||
⚠️ **但这里有个坑：不能简单地写成 `关节值 = 开口_mm / 1000`。**
|
||||
上游的耦合是**线性近似**，而机构本身是非线性的（手指是"转 + 再转"，指垫
|
||||
既平移又旋转）。按线性口径命令 20 mm，Gazebo 实测两指相距 **21.75 mm** ——
|
||||
抓 20 mm 的工件会空出 1.75 mm，没有接触力，触觉那一步在仿真里直接废掉。
|
||||
|
||||
所以开口↔关节值走**URDF 正运动学反解**（见 sim_gripper_kin.py）：
|
||||
|
||||
aperture_to_source(20.0) → 令两指间距 − 合拢间距 == 20.0 的关节值
|
||||
|
||||
这个正运动学与 Gazebo 的实测吻合到 0.01 mm（0~100 mm 全程），两边完全独立
|
||||
（一个是矩阵乘法，一个是物理引擎），所以"开口 20 mm"在仿真里是物理上真的 20 mm。
|
||||
顺带一个推论：GN01 的**实测**行程是 99.93 mm，不是名义的 100.0 ——
|
||||
启动日志里报的是实测值。
|
||||
|
||||
万一某个夹爪的指尖 link 命名不符合约定，正运动学建不出来，会**退回线性近似**
|
||||
并在启动日志里明确写出口径变了（中段偏差可达 4 mm/100mm 行程）。
|
||||
别在那种情况下拿仿真去标触觉阈值。
|
||||
|
||||
⚠️ 还要注意**仿真夹爪和真机夹爪不是同一个夹爪**：仿真是官方 GN01（行程
|
||||
100 mm），现场是 DH PGEA-100-20-W（``site.yaml`` 的 ``gripper.stroke_mm``
|
||||
= 20 mm）。同一个"8 mm 开口"在两者上都物理正确，但"全开"对应的开口
|
||||
完全不同。适配器启动时会把两者的行程打出来，避免有人默认它们一样。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Mapping, Optional, Sequence, Tuple
|
||||
|
||||
from .sim_gripper_kin import ApertureCurve, KinematicsError, UrdfTree, find_tip_pair
|
||||
|
||||
import yaml
|
||||
|
||||
# 默认控制器配置位置（相对仓库根）。与真机那份同名不同目录，别搞混。
|
||||
DEFAULT_CONTROLLERS_REL = "devices/sim/gazebo/config/controllers.yaml"
|
||||
|
||||
#: 适配器负责的控制器名。改这里要同步改 launch 与 controllers.yaml。
|
||||
DEFAULT_CONTROLLER = "gripper_controller"
|
||||
|
||||
#: 关节值（米）与开口（毫米）的换算系数。
|
||||
MM_PER_M = 1000.0
|
||||
|
||||
#: 参数里 mimic 表的序列化分隔符。关节名里不会有 ':'，所以安全。
|
||||
_MIMIC_SEP = ":"
|
||||
|
||||
|
||||
class GripperConfigError(RuntimeError):
|
||||
"""夹爪配置/描述有问题时抛出，消息面向使用者。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MimicRelation:
|
||||
"""一条 ``<mimic>`` 耦合关系：从动关节 = multiplier × 主关节 + offset。"""
|
||||
|
||||
joint: str
|
||||
source: str
|
||||
multiplier: float
|
||||
offset: float
|
||||
|
||||
def apply(self, source_value: float) -> float:
|
||||
"""按这条关系算出从动关节的值。"""
|
||||
return self.multiplier * source_value + self.offset
|
||||
|
||||
def to_param(self) -> str:
|
||||
"""序列化成 ROS 参数用的字符串（launch 与本模块必须用同一格式）。"""
|
||||
return format_mimic_param(self.joint, self.source, self.multiplier, self.offset)
|
||||
|
||||
|
||||
def format_mimic_param(joint: str, source: str, multiplier: object, offset: object) -> str:
|
||||
"""把一条耦合关系序列化成 ROS 参数用的字符串。
|
||||
|
||||
launch 从 ``NormalizeReport.stripped_mimics`` 拿到的是**四个字符串**，
|
||||
本模块解析的也是这个格式，所以格式化只留这一个实现 ——
|
||||
两边各写一份的话，改了格式就会一边发一边收不到（而且不报错）。
|
||||
"""
|
||||
return _MIMIC_SEP.join(
|
||||
(str(joint).strip(), str(source).strip(), repr(float(multiplier)), repr(float(offset)))
|
||||
)
|
||||
|
||||
|
||||
class GripperLayout:
|
||||
"""夹爪在仿真里的完整描述：关节顺序 + 耦合关系 + 主关节限位。
|
||||
|
||||
由 :func:`layout_from_parts` 构造（它做全部校验）；不要直接 new。
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
order: Sequence[str],
|
||||
relations: Mapping[str, MimicRelation],
|
||||
source: str,
|
||||
lower: float,
|
||||
upper: float,
|
||||
curve: Optional[ApertureCurve] = None,
|
||||
kin_note: str = "",
|
||||
) -> None:
|
||||
self.order: Tuple[str, ...] = tuple(order)
|
||||
self.relations: Dict[str, MimicRelation] = dict(relations)
|
||||
self.source = source
|
||||
self.lower = float(lower)
|
||||
self.upper = float(upper)
|
||||
self.feedback = self._pick_feedback()
|
||||
# 由 URDF 正运动学给出的"真实开口曲线"；None = 退回线性近似。
|
||||
self.curve: Optional[ApertureCurve] = curve
|
||||
#: 口径说明（给日志用）：是 FK 精确口径还是线性近似。
|
||||
self.kin_note: str = kin_note
|
||||
|
||||
# --- 反馈关节 ---------------------------------------------------------
|
||||
def _pick_feedback(self) -> str:
|
||||
"""挑一个**真的会动**的关节来当开口反馈源。
|
||||
|
||||
⚠️ 主关节 ``finger_width_joint`` 修不出来：上游描述里它挂在一个叫
|
||||
``unused_finger_width_link`` 的**孤立假 link** 上（注释原话：
|
||||
"The unused link cannot have 0 mass"），和手指没有任何机械连接。
|
||||
实测（2026-09-24，Gazebo Fortress + gz_ros2_control）：命令它到 0.100，
|
||||
它的状态**始终停在 0**，而 6 个从动关节准确到位。
|
||||
所以拿主关节当反馈源的话，``status`` 会永远报"开口 0 mm"，
|
||||
而夹爪明明开着 —— 这正是"看起来在工作"的静默失效。
|
||||
|
||||
于是改从**与主关节直接相连的那个从动关节**反推开口：
|
||||
state = m × source + o → source = (state - o) / m
|
||||
选 |m| 最大的那个（分辨率最好；m 为 0 的不能用）。
|
||||
|
||||
单关节夹爪（没有耦合关系）时退回到主关节自己。
|
||||
"""
|
||||
candidates = [
|
||||
(abs(rel.multiplier), joint)
|
||||
for joint, rel in self.relations.items()
|
||||
if rel.source == self.source and abs(rel.multiplier) > 1e-9
|
||||
]
|
||||
if not candidates:
|
||||
return self.source
|
||||
candidates.sort(reverse=True)
|
||||
return candidates[0][1]
|
||||
|
||||
def feedback_value(self, source_value: float) -> float:
|
||||
"""反馈关节在"主关节 = source_value"时应有的值（用于判"没跟上"）。"""
|
||||
if self.feedback == self.source:
|
||||
return float(source_value)
|
||||
return self.value_for(self.feedback, source_value)
|
||||
|
||||
def source_from_feedback(self, feedback_state: float) -> float:
|
||||
"""反馈关节实测值 → 等价的主关节值（米）。"""
|
||||
if self.feedback == self.source:
|
||||
return float(feedback_state)
|
||||
rel = self.relations[self.feedback]
|
||||
if abs(rel.multiplier) < 1e-9:
|
||||
raise GripperConfigError(
|
||||
f"反馈关节 {self.feedback!r} 的 multiplier 为 0，无法反推主关节值。"
|
||||
)
|
||||
return (float(feedback_state) - rel.offset) / rel.multiplier
|
||||
|
||||
def aperture_from_feedback(self, feedback_state: float) -> float:
|
||||
"""反馈关节的实测值 → 开口（毫米）。"""
|
||||
return self.source_to_aperture(self.source_from_feedback(feedback_state))
|
||||
|
||||
# --- 行程口径 ---------------------------------------------------------
|
||||
@property
|
||||
def stroke_mm(self) -> float:
|
||||
"""这个夹爪在仿真里**实际**能开的宽度（毫米）。
|
||||
|
||||
有 FK 曲线时用实测行程（GN01 ≈ 99.93 mm，不是名义的 100.0 ——
|
||||
上游把宽度关节限位定在 0~0.100 m，而线性耦合让指尖只走了 99.93，
|
||||
报名义值会让"全开时开口读数与几何对不上"变成一个查不出的矛盾）。
|
||||
没有 FK 曲线时只能按限位换算，并在 :attr:`kin_note` 里说明是近似。
|
||||
|
||||
真机那份行程在 ``site.yaml`` 的 ``gripper.stroke_mm``；两者不同是正常的
|
||||
（不同夹爪），但要显式打出来，见 :func:`stroke_note`。
|
||||
"""
|
||||
if self.curve is not None:
|
||||
return self.curve.stroke_mm
|
||||
return (self.upper - self.lower) * MM_PER_M
|
||||
|
||||
@property
|
||||
def closed_separation_mm(self) -> Optional[float]:
|
||||
"""合拢时两侧 tip frame 的固有间距（GN01 ≈ 27 mm）；无 FK 时为 None。
|
||||
|
||||
验收脚本用它把"指尖间距"折成"开口"（指垫合拢时间距不为 0，
|
||||
因为 tip frame 不在指垫表面）。见 scripts/check_sim_gripper.py。
|
||||
"""
|
||||
return None if self.curve is None else self.curve.closed_separation_mm
|
||||
|
||||
def aperture_to_source(self, aperture_mm: float) -> float:
|
||||
"""开口（毫米）→ 主关节值（米），并按限位截断。
|
||||
|
||||
**绝对物理映射**：夹爪真的开这么宽。越界不报错而是截断（撞到限位是
|
||||
正常操作），但调用方可以从 :meth:`is_clamped` 知道发生了截断。
|
||||
|
||||
⚠️ 有 FK 曲线时走**反解**，不是线性换算。因为上游的耦合是线性近似、
|
||||
而机构是非线性的：线性口径下"命令 20 mm"实际是 21.75 mm 的指垫间距，
|
||||
抓 20 mm 的工件会空出 1.75 mm（没有接触力），
|
||||
触觉那一步在仿真里就废了。细节见 sim_gripper_kin.py 文件头。
|
||||
"""
|
||||
if self.curve is not None:
|
||||
return self.curve.joint_value(aperture_mm)
|
||||
raw = self.lower + float(aperture_mm) / MM_PER_M
|
||||
return min(max(raw, self.lower), self.upper)
|
||||
|
||||
def source_to_aperture(self, source_value: float) -> float:
|
||||
"""主关节值（米）→ 开口（毫米）。与 :meth:`aperture_to_source` 互逆。"""
|
||||
if self.curve is not None:
|
||||
return self.curve.aperture_mm(source_value)
|
||||
return (float(source_value) - self.lower) * MM_PER_M
|
||||
|
||||
def is_clamped(self, aperture_mm: float) -> bool:
|
||||
"""这个开口请求是否会被限位截断（用来给调用方一句实话）。"""
|
||||
if self.curve is not None:
|
||||
return self.curve.is_clamped(aperture_mm)
|
||||
raw = self.lower + float(aperture_mm) / MM_PER_M
|
||||
return not (self.lower <= raw <= self.upper)
|
||||
|
||||
# --- 命令向量 ---------------------------------------------------------
|
||||
def command_vector(self, aperture_mm: float) -> List[float]:
|
||||
"""按 ``order`` 拼出一整组关节目标（主关节 + 从动关节）。
|
||||
|
||||
顺序必须与 controllers.yaml 的 ``joints:`` 一致 —— 顺序错了，
|
||||
JointGroupPositionController 不会报错，只会把角度发给**别的关节**，
|
||||
表现为"夹爪动得很奇怪"。
|
||||
"""
|
||||
return self.command_vector_from_source(self.aperture_to_source(aperture_mm))
|
||||
|
||||
def command_vector_from_source(self, source_value: float) -> List[float]:
|
||||
"""同上，但直接给主关节值（米），不经过"毫米"这一层来回换算。
|
||||
|
||||
``stop``（保持当前位置）这类命令手里只有关节状态值，走毫米会在
|
||||
两次换算里引入舍入误差，所以单独留这个入口。
|
||||
"""
|
||||
return [self.value_for(name, source_value) for name in self.order]
|
||||
|
||||
def value_for(self, joint: str, source_value: float) -> float:
|
||||
"""某个关节在"主关节 = source_value"时应有的值。
|
||||
|
||||
主关节自己就是 ``source_value``；从动关节沿**耦合链**逐级算出来。
|
||||
|
||||
⚠️ GN01 的耦合不是"6 个手指关节全部直挂主关节"，而是一条链：
|
||||
|
||||
finger_width_joint ─9.404,-0.155→ left_outer_knuckle_joint
|
||||
├─ 1, 0 → left_inner_knuckle
|
||||
├─ -1,0 → left_inner_finger
|
||||
├─ 1, 0 → right_outer_knuckle
|
||||
├─ 1, 0 → right_inner_knuckle
|
||||
└─ -1,0 → right_inner_finger
|
||||
|
||||
所以必须**逐级**套用 multiplier/offset（先套靠近根的、再套靠近叶的），
|
||||
不能只套一层。只套一层的话，指尖角度会错得不多但一直不对 ——
|
||||
表现为"夹爪动作看着像那么回事，就是夹不准"。
|
||||
"""
|
||||
if joint == self.source:
|
||||
return float(source_value)
|
||||
|
||||
# 从 joint 往上爬到根，把沿途的关系按"从根到叶"的顺序套用。
|
||||
chain: List[MimicRelation] = []
|
||||
seen = set()
|
||||
cur = joint
|
||||
while True:
|
||||
rel = self.relations.get(cur)
|
||||
if rel is None:
|
||||
break
|
||||
if cur in seen:
|
||||
raise GripperConfigError(
|
||||
f"耦合链成环：{cur!r} 又回到了自己（{' → '.join(sorted(seen))}）"
|
||||
)
|
||||
seen.add(cur)
|
||||
chain.append(rel)
|
||||
cur = rel.source
|
||||
if cur != self.source:
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint!r} 的耦合链终止在 {cur!r}，而不是主关节 "
|
||||
f"{self.source!r} —— 描述里的耦合表不完整。"
|
||||
)
|
||||
value = float(source_value)
|
||||
for rel in reversed(chain):
|
||||
value = rel.apply(value)
|
||||
return value
|
||||
|
||||
def source_index(self) -> int:
|
||||
"""主关节在命令数组里的下标。"""
|
||||
return self.order.index(self.source)
|
||||
|
||||
def _source_limits_note(self) -> str:
|
||||
return f"限位 [{self.lower:.4f}, {self.upper:.4f}] m"
|
||||
|
||||
def describe(self) -> str:
|
||||
"""启动时打给操作者看的一行摘要（含必须知道的口径）。"""
|
||||
note = f"；{self.kin_note}" if self.kin_note else ""
|
||||
return (
|
||||
f"主关节={self.source} {self._source_limits_note()} "
|
||||
f"→ 仿真行程 {self.stroke_mm:.2f} mm；"
|
||||
f"共 {len(self.order)} 个关节，{len(self.relations)} 条耦合{note}"
|
||||
)
|
||||
|
||||
def stroke_note(self, real_stroke_mm: Optional[float]) -> str:
|
||||
"""仿真行程与真机行程的对比说明（两者不同是正常的，但必须说出来）。"""
|
||||
if real_stroke_mm is None or real_stroke_mm <= 0:
|
||||
return (
|
||||
f"仿真夹爪行程 {self.stroke_mm:.1f} mm（site.yaml 里没读到真机行程，"
|
||||
f"无法对比）"
|
||||
)
|
||||
same = abs(real_stroke_mm - self.stroke_mm) < 0.5
|
||||
head = f"仿真行程 {self.stroke_mm:.1f} mm vs 真机 {real_stroke_mm:.1f} mm"
|
||||
if same:
|
||||
return head + "：一致。"
|
||||
return (
|
||||
head + "：**不一致**（不同型号的夹爪，这是正常的）。\n"
|
||||
" 开口毫米采用绝对物理映射，所以同一个开口在两边几何上都对；\n"
|
||||
" 但\"全开\"对应的开口不同，别把仿真里的全开当成真机的全开。\n"
|
||||
" 力/触觉阈值也要按各自夹爪重新标定（见 R3.5）。"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 解析与校验
|
||||
# ---------------------------------------------------------------------------
|
||||
def parse_mimics(entries: Sequence[str]) -> Dict[str, MimicRelation]:
|
||||
"""解析参数里的 mimic 表（每条 ``joint:source:multiplier:offset``）。
|
||||
|
||||
格式错就报错，不做"跳过这一条"的宽容处理：静默丢一条耦合关系，
|
||||
结果是某个手指关节永远不动，而 Gazebo 不会抱怨。
|
||||
"""
|
||||
out: Dict[str, MimicRelation] = {}
|
||||
for raw in entries:
|
||||
parts = str(raw).split(_MIMIC_SEP)
|
||||
if len(parts) != 4:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数格式不对：{raw!r}\n"
|
||||
f" 应为 '从动关节:主关节:multiplier:offset' 四段。"
|
||||
)
|
||||
joint, source, mult_s, off_s = (p.strip() for p in parts)
|
||||
if not joint or not source:
|
||||
raise GripperConfigError(f"mimic 参数里关节名为空：{raw!r}")
|
||||
try:
|
||||
mult, off = float(mult_s), float(off_s)
|
||||
except ValueError as exc:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数里的 multiplier/offset 不是数字：{raw!r}"
|
||||
) from exc
|
||||
if joint == source:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数里从动关节与主关节同名（{joint}）—— 描述有问题。"
|
||||
)
|
||||
if joint in out:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数里关节 {joint!r} 出现了两次，无法确定用哪条。"
|
||||
)
|
||||
out[joint] = MimicRelation(joint, source, mult, off)
|
||||
return out
|
||||
|
||||
|
||||
def gripper_joint_order(
|
||||
controllers_file: Path, controller: str = DEFAULT_CONTROLLER
|
||||
) -> List[str]:
|
||||
"""从 controllers.yaml 里读 ``<controller>.ros__parameters.joints``。
|
||||
|
||||
为什么解析而不是让调用方再配一份顺序：顺序是
|
||||
``JointGroupPositionController`` 的硬契约，配两份必然漂移，
|
||||
而漂移的症状是"角度发给了别的关节"、不报错。
|
||||
"""
|
||||
path = Path(controllers_file)
|
||||
try:
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except OSError as exc:
|
||||
raise GripperConfigError(f"读不到控制器配置 {path}：{exc}") from exc
|
||||
except yaml.YAMLError as exc:
|
||||
raise GripperConfigError(f"控制器配置 {path} 不是合法 YAML：{exc}") from exc
|
||||
|
||||
section = (data.get(controller) or {}).get("ros__parameters")
|
||||
if not isinstance(section, dict):
|
||||
raise GripperConfigError(
|
||||
f"{path} 里没有 {controller}.ros__parameters 段。\n"
|
||||
f" 仿真夹爪要靠这个控制器的 joints 顺序下发命令，缺了就无法工作。"
|
||||
)
|
||||
joints = section.get("joints")
|
||||
if not isinstance(joints, list) or not joints:
|
||||
raise GripperConfigError(
|
||||
f"{path} 的 {controller}.ros__parameters 里 joints 为空。"
|
||||
)
|
||||
names = [str(j).strip() for j in joints]
|
||||
if len(set(names)) != len(names):
|
||||
raise GripperConfigError(
|
||||
f"{path} 的 {controller} joints 里有重复项：{names}"
|
||||
)
|
||||
return names
|
||||
|
||||
|
||||
def joint_limits(urdf_xml: str, joint_name: str) -> Tuple[float, float]:
|
||||
"""从 URDF 文本里取某个关节的 (lower, upper)。取不到就报错。
|
||||
|
||||
为什么不从参数再传一遍限位：限位是描述里的**唯一真相**（也就是 Gazebo
|
||||
里真实的行程）。再配一份等于给自己一个"改了描述没改配置"的机会，
|
||||
而症状是开口悄悄不按预期走。
|
||||
"""
|
||||
# 只认 <joint name="..."> 那一段，避免匹配到别的关节。
|
||||
pat = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*[\"']%s[\"'][^>]*>(.*?)</joint>"
|
||||
% re.escape(joint_name),
|
||||
re.DOTALL,
|
||||
)
|
||||
m = pat.search(urdf_xml or "")
|
||||
if not m:
|
||||
raise GripperConfigError(
|
||||
f"在 robot_description 里找不到关节 {joint_name!r}。\n"
|
||||
f" 常见原因：描述没带夹爪（load_gripper:=false），或关节名前缀不对"
|
||||
f"（仿真是 sim_ 前缀）。"
|
||||
)
|
||||
body = m.group(1)
|
||||
if re.search(r"<mimic\b", body):
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint_name!r} 自己是从动关节（带 <mimic>），不能当主关节。"
|
||||
)
|
||||
lm = re.search(r"<limit\b[^>]*>", body)
|
||||
if not lm:
|
||||
raise GripperConfigError(f"关节 {joint_name!r} 的 <limit> 标签缺失。")
|
||||
|
||||
def _attr(name: str) -> Optional[float]:
|
||||
am = re.search(r"\b%s\s*=\s*[\"']([^\"']+)[\"']" % name, lm.group(0))
|
||||
if not am:
|
||||
return None
|
||||
try:
|
||||
return float(am.group(1))
|
||||
except ValueError:
|
||||
return None
|
||||
|
||||
lower, upper = _attr("lower"), _attr("upper")
|
||||
if lower is None or upper is None:
|
||||
# URDF 允许省略 limit 的上下限（continuous 关节就没有）。夹爪的宽度
|
||||
# 关节必须有，否则我们无从知道"全开"是多少 —— 这时报错比猜好。
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint_name!r} 的 <limit> 缺 lower/upper，"
|
||||
f"无法确定开口行程。原始标签：{lm.group(0)}"
|
||||
)
|
||||
if upper <= lower:
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint_name!r} 的限位不合法：lower={lower} upper={upper}"
|
||||
)
|
||||
return lower, upper
|
||||
|
||||
|
||||
def layout_from_parts(
|
||||
order: Sequence[str],
|
||||
mimics: Mapping[str, MimicRelation],
|
||||
urdf_xml: str,
|
||||
) -> GripperLayout:
|
||||
"""把三份数据拼成 :class:`GripperLayout`，并做全部一致性校验。
|
||||
|
||||
校验点（每一条都对应一种"不报错但行为错"的故障）：
|
||||
|
||||
* ``mimics`` 里的从动关节必须在 ``order`` 里 —— 否则控制器根本没声明它，
|
||||
我们发出去的这一路会被忽略；
|
||||
* 从动关节引用的 ``source`` 也必须在 ``order`` 里 —— 链上有个关节没被
|
||||
控制器声明的话，那一级的角度就没人驱动；
|
||||
* 主关节（不带 mimic 的那个）必须**唯一** —— 多个候选说明推断逻辑不成立，
|
||||
这时静默挑第一个会挑错；
|
||||
* 每条耦合链都必须能走到主关节（不终止、成环都报错）—— 只校验开头几层
|
||||
的话，链尾错一处只表现为"某个指尖角度一直不对"。
|
||||
"""
|
||||
order = [str(j) for j in order]
|
||||
if not order:
|
||||
raise GripperConfigError("夹爪控制器没有声明任何关节。")
|
||||
|
||||
followers = dict(mimics)
|
||||
unknown = sorted(set(followers) - set(order))
|
||||
if unknown:
|
||||
raise GripperConfigError(
|
||||
f"mimic 表里的关节不在控制器 joints 列表里：{unknown}\n"
|
||||
f" 控制器声明的是：{order}\n"
|
||||
f" 两边必须一致，否则我们算出来的角度会被控制器忽略。"
|
||||
)
|
||||
missing_src = sorted({r.source for r in followers.values()} - set(order))
|
||||
if missing_src:
|
||||
raise GripperConfigError(
|
||||
f"mimic 表里有关节的**主关节**不在控制器 joints 列表里：{missing_src}\n"
|
||||
f" 控制器声明的是：{order}\n"
|
||||
f" 耦合链上少一环，下面所有关节的角度都会算错。"
|
||||
)
|
||||
|
||||
candidates = [j for j in order if j not in followers]
|
||||
if len(candidates) != 1:
|
||||
raise GripperConfigError(
|
||||
f"无法唯一确定主关节：控制器里有 {len(candidates)} 个关节不带 mimic "
|
||||
f"（{candidates}），期望恰好 1 个。\n"
|
||||
f" 通常是描述里夹爪的 mimic 表没传进来（load_gripper 关了？），"
|
||||
f"或者上游描述换了耦合方式。"
|
||||
)
|
||||
source = candidates[0]
|
||||
|
||||
lower, upper = joint_limits(urdf_xml, source)
|
||||
layout = GripperLayout(order, followers, source, lower, upper)
|
||||
|
||||
# 逐条把耦合链走一遍：成环 / 不终止在这里就报出来，别等到运行期。
|
||||
for joint in followers:
|
||||
layout.value_for(joint, 0.0)
|
||||
|
||||
# 开口曲线要在 layout 之后建：它调 layout.value_for 拿每个关节的角度，
|
||||
# 而 value_for 是耦合链的**唯一**权威实现 —— 曲线里再抄一份迟早会漂移。
|
||||
# （所以是"先建后回填"，不是构造参数。）
|
||||
layout.curve, layout.kin_note = _build_aperture_curve(layout, urdf_xml)
|
||||
return layout
|
||||
|
||||
|
||||
def _build_aperture_curve(
|
||||
layout: GripperLayout, urdf_xml: str
|
||||
) -> Tuple[Optional[ApertureCurve], str]:
|
||||
"""建"开口毫米 ↔ 主关节值"的精确曲线；建不出来就退回线性并**说明白**。
|
||||
|
||||
返回 ``(曲线或 None, 给日志的一句话)``。
|
||||
|
||||
为什么允许退回：换一个夹爪时，如果它的指尖 link 命名不符合约定，
|
||||
硬报错会让整个仿真起不来；而线性近似虽然中段有几毫米偏差，但
|
||||
"能不能动、方向对不对"仍然可验。所以退回，但**必须在日志里说清楚
|
||||
口径变成了近似** —— 否则有人会拿它当毫米精度的夹爪去标定触觉阈值。
|
||||
"""
|
||||
try:
|
||||
tree = UrdfTree(urdf_xml)
|
||||
tips = find_tip_pair(tree)
|
||||
if tips is None:
|
||||
return None, (
|
||||
"⚠ 没在 URDF 里认出左右指尖 link（按 *left*finger*tip* / "
|
||||
"*right*finger*tip* 约定找）→ 开口退回**线性近似**"
|
||||
"（中段偏差可达 4 mm/100mm 行程，别拿它标触觉阈值）"
|
||||
)
|
||||
curve = ApertureCurve(
|
||||
tree,
|
||||
tips,
|
||||
angles_of=lambda v: {j: layout.value_for(j, v) for j in layout.order},
|
||||
lower=layout.lower,
|
||||
upper=layout.upper,
|
||||
)
|
||||
return curve, (
|
||||
f"开口由 URDF 正运动学反解（{tips[0]} / {tips[1]}；"
|
||||
f"合拢时间距 {curve.closed_separation_mm:.2f} mm）"
|
||||
)
|
||||
except KinematicsError as exc:
|
||||
return None, (
|
||||
f"⚠ 正运动学不可用（{exc}）→ 开口退回**线性近似**"
|
||||
f"（中段偏差可达 4 mm/100mm 行程）"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 夹住判据
|
||||
# ---------------------------------------------------------------------------
|
||||
#: 判"夹住了"的默认容差（**毫米开口**）。GN01 全行程 100 mm，2 mm 的开口差
|
||||
#: 只可能是被挡住了，不可能是控制精度问题。
|
||||
DEFAULT_GRASP_TOLERANCE_MM = 2.0
|
||||
|
||||
#: 判"还在动"的默认容差（毫米开口）。比上面小：它只是用来区分 MOVING / IDLE。
|
||||
DEFAULT_MOVING_TOLERANCE_MM = 0.5
|
||||
|
||||
|
||||
def is_grasping(
|
||||
target_aperture_mm: float,
|
||||
actual_aperture_mm: float,
|
||||
tolerance_mm: float = DEFAULT_GRASP_TOLERANCE_MM,
|
||||
) -> bool:
|
||||
"""由"指令到了合拢位、实际开口却更大"推断夹住了东西。
|
||||
|
||||
真机（DH）有状态字可以问 "object caught"，仿真没有。但仿真里有一个等价
|
||||
且更本质的信号：位置控制器把目标压到合拢位，若两指之间夹着东西，
|
||||
关节就**到不了**那个位置，稳态开口会停在工件宽度附近。
|
||||
|
||||
两个条件同时成立才算夹住：
|
||||
|
||||
1. 目标在合拢侧（``target_aperture_mm`` 小于容差）—— 否则"有误差"只是没走到；
|
||||
2. 实际开口比目标大出容差 —— 被挡住了。
|
||||
|
||||
⚠️ 口径统一用**毫米开口**，不是主关节值（米）。因为开口↔关节值在
|
||||
FK 口径下是非线性的（见 sim_gripper_kin.py），拿米当口径的话"2 mm 容差"
|
||||
在不同开度下含义不同，标定出来的阈值换台夹爪就废了。
|
||||
|
||||
这是**推断**，不是测量：工件被夹在两指之间、但关节仍走到位的特殊情况
|
||||
（例如工件很薄且被压进夹爪根部）会漏判。所以 P6 的最终判据仍然用触觉
|
||||
力（见 R3.1），这里只提供 ``GripperStatus.is_gripped`` 这个粗信号。
|
||||
"""
|
||||
near_closed = float(target_aperture_mm) <= float(tolerance_mm)
|
||||
blocked = (
|
||||
float(actual_aperture_mm) - float(target_aperture_mm)
|
||||
) > float(tolerance_mm)
|
||||
return bool(near_closed and blocked)
|
||||
@@ -0,0 +1,424 @@
|
||||
"""仿真夹爪适配器：``GripperExecute`` 服务 → ``JointGroupPositionController``。
|
||||
|
||||
=============================================================================
|
||||
它在链路上的位置
|
||||
=============================================================================
|
||||
应用层（P6 状态机 / 现场脚本）
|
||||
│ 服务 <ns>/open | <ns>/close | <ns>/move | <ns>/rotate
|
||||
│ （arm_msgs/srv/GripperExecute，与真机**同名同类型**）
|
||||
▼
|
||||
本节点
|
||||
│ ① 开口毫米 → 主关节值（绝对物理映射，见 sim_gripper.py 文件头）
|
||||
│ ② 按 mimic 耦合算出 6 个从动关节的目标角
|
||||
│ ③ 按 controllers.yaml 的 joints 顺序拼成数组
|
||||
▼
|
||||
话题 <controller>/commands (std_msgs/Float64MultiArray)
|
||||
▼
|
||||
JointGroupPositionController（gz_ros2_control，在 Gazebo 进程里）
|
||||
▼
|
||||
Gazebo 里的 7 个 sim_* 夹爪关节
|
||||
|
||||
同时订阅 /joint_states，把**反馈关节**的实际值换算回开口毫米，
|
||||
发布到 <ns>/status（arm_msgs/GripperStatus）。
|
||||
注意不是主关节 —— 主关节在 Gazebo 里压根不动，见下面第 4 个坑。
|
||||
|
||||
=============================================================================
|
||||
为什么必须由适配器驱动全部 7 个关节
|
||||
=============================================================================
|
||||
sdformat 把 URDF 的 ``<mimic>`` 整个丢掉，gz-sim 也不实现 mimic ——
|
||||
6 个从动关节在仿真里不会跟随主关节。所以适配器必须自己算。
|
||||
这件事同时解决了一个 TF 隐患：``robot_state_publisher`` 是按 /joint_states
|
||||
里的**真实值**发布 TF 的，只要 7 个关节都被驱动、且值符合耦合公式，
|
||||
RViz/TF 与 Gazebo 就一致（详见 urdf_normalize.py 里那段说明）。
|
||||
|
||||
=============================================================================
|
||||
⚠️ 仿真夹爪与真机夹爪不是同一个夹爪
|
||||
=============================================================================
|
||||
仿真是官方 Flexiv-GN01（行程 100 mm），现场是 DH PGEA-100-20-W
|
||||
（``site.yaml`` 的 ``gripper.stroke_mm`` = 20 mm）。开口用绝对物理映射，
|
||||
所以同一句"开 8 mm"两边都对；但"全开"对应的开口不同。启动时会把两者
|
||||
都打出来，差异大时会明确提示（见 :meth:`GripperLayout.stroke_note`）。
|
||||
|
||||
=============================================================================
|
||||
三个必须知道的坑
|
||||
=============================================================================
|
||||
1) **命令数组的顺序**取自 controllers.yaml，不是本节点里的常量。
|
||||
顺序错不会报错，只会把角度发给别的关节 —— 表现成"夹爪动得很奇怪"。
|
||||
所以本节点启动时先把顺序读出来并校验（从动关节必须在列表里）。
|
||||
|
||||
2) **``rotate`` 在平行夹爪上没有对应自由度**。真机那边 RGI 才有。这里明确
|
||||
返回 ``success=false`` 并说明原因，而不是假装成功 —— 假装成功会让上层
|
||||
的"拧入"流程在仿真里走到一个永远不动的分支上。
|
||||
|
||||
3) **``status`` 的 ``is_gripped`` 是推断值**（指令到合拢位但实际值没跟上），
|
||||
不是测量值。真机有 DH 状态字，仿真没有。最终夹持判据仍应看触觉力，
|
||||
见 sim_gripper.is_grasping 的说明。
|
||||
|
||||
4) **主关节 ``finger_width_joint`` 在 Gazebo 里不会动**。上游描述把它挂在
|
||||
孤立的 ``unused_finger_width_link`` 上（那个 link 的注释写着 "cannot have
|
||||
0 mass"，就是为了让 URDF 合法），它和手指没有任何机械连接。
|
||||
实测：命令到 0.100，它的 /joint_states 值恒为 ~0，而 6 个从动关节准确到位。
|
||||
所以：
|
||||
- 命令数组里**仍然要带上它**（controllers.yaml 的 joints 含它，少一个会错位）；
|
||||
- 但**反馈绝不能取它**，否则 status 永远报"开口 0 mm"而夹爪明明开着。
|
||||
适配器改用"与主关节直接相连、|multiplier| 最大的那个从动关节"反推开口
|
||||
（见 GripperLayout._pick_feedback / aperture_from_feedback）。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Float64MultiArray
|
||||
|
||||
from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from .sim_gripper import (
|
||||
DEFAULT_CONTROLLER,
|
||||
DEFAULT_CONTROLLERS_REL,
|
||||
DEFAULT_GRASP_TOLERANCE_MM,
|
||||
DEFAULT_MOVING_TOLERANCE_MM,
|
||||
GripperConfigError,
|
||||
GripperLayout,
|
||||
gripper_joint_order,
|
||||
is_grasping,
|
||||
layout_from_parts,
|
||||
parse_mimics,
|
||||
)
|
||||
|
||||
#: 主关节状态多少秒没更新就算"数据不新鲜"（仿真时间，不是墙钟）。
|
||||
#: 控制器 100Hz、JSB 也在这个量级，1 秒足够宽松。
|
||||
STATE_STALE_S = 1.0
|
||||
|
||||
#: 服务后缀 → GripperCommand.command 的映射。前四个就是 capabilities.py
|
||||
#: 里声明的接口名；execute 是通用入口（与 dual_gripper_driver 对齐）。
|
||||
_SERVICE_COMMANDS = ("open", "close", "move", "rotate")
|
||||
|
||||
|
||||
def _site_stroke_mm() -> Optional[float]:
|
||||
"""从 site.yaml 读真机夹爪行程（只为启动时打一句对比，读不到返回 None）。
|
||||
|
||||
读不到不影响工作 —— 它只是提示信息，不该让仿真起不来。
|
||||
"""
|
||||
try:
|
||||
from arm_interfaces import load_site # 延迟导入：只需要它一处
|
||||
except Exception:
|
||||
return None
|
||||
try:
|
||||
data = load_site()
|
||||
except Exception:
|
||||
return None
|
||||
grip = data.get("gripper") if isinstance(data, dict) else None
|
||||
if not isinstance(grip, dict):
|
||||
return None
|
||||
for key in ("stroke_mm", "stroke"):
|
||||
try:
|
||||
val = float(grip.get(key))
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
if val > 0:
|
||||
return val
|
||||
return None
|
||||
|
||||
|
||||
class SimGripperAdapter(Node):
|
||||
"""把 ``GripperExecute`` 服务翻译成 Gazebo 里 7 个夹爪关节的目标位置。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim_gripper_adapter")
|
||||
|
||||
# 参数：全部由 launch 从**同一份** prepare_urdf 结果与 controllers.yaml
|
||||
# 派生，避免这里自己再展开一次 xacro 造成两边不一致。
|
||||
self.declare_parameter("controllers_file", "")
|
||||
self.declare_parameter("controller", DEFAULT_CONTROLLER)
|
||||
self.declare_parameter("mimics", [""])
|
||||
# ⚠️ 不要 declare_parameter("use_sim_time", ...)：launch 会作为参数文件
|
||||
# 传进来，rclpy 那时已经自动声明过，再声明会抛异常让节点起不来。
|
||||
self.declare_parameter("namespace", "/gripper")
|
||||
self.declare_parameter("tolerance_mm", DEFAULT_GRASP_TOLERANCE_MM) # 夹住判据容差（毫米开口）
|
||||
self.declare_parameter("default_speed_pct", 50.0)
|
||||
self.declare_parameter("default_force_pct", 20.0)
|
||||
|
||||
controller = str(self.get_parameter("controller").value or DEFAULT_CONTROLLER)
|
||||
raw_ctrl = str(self.get_parameter("controllers_file").value or "")
|
||||
controllers_file = Path(raw_ctrl) if raw_ctrl else Path.cwd() / DEFAULT_CONTROLLERS_REL
|
||||
|
||||
# mimics 参数传进来的是字符串数组；空字符串要滤掉（ROS 对空数组不友好）。
|
||||
raw_mimics = [
|
||||
str(m).strip()
|
||||
for m in (self.get_parameter("mimics").value or [])
|
||||
if str(m).strip()
|
||||
]
|
||||
|
||||
urdf_xml = self._robot_description()
|
||||
try:
|
||||
order = gripper_joint_order(controllers_file, controller)
|
||||
relations = parse_mimics(raw_mimics)
|
||||
self.layout: GripperLayout = layout_from_parts(order, relations, urdf_xml)
|
||||
except GripperConfigError as exc:
|
||||
# 配置错就让节点直接失败：一个"什么都不动"的夹爪适配器比没有更糟 ——
|
||||
# 它看起来在工作，而上层会一直等一个永远不来的状态变化。
|
||||
self.get_logger().error(f"仿真夹爪配置有问题：\n{exc}")
|
||||
raise
|
||||
|
||||
self._controller = controller
|
||||
self._ns = str(self.get_parameter("namespace").value or "/gripper").rstrip("/")
|
||||
self._tolerance_mm = float(self.get_parameter("tolerance_mm").value)
|
||||
self._speed = float(self.get_parameter("default_speed_pct").value)
|
||||
self._force = float(self.get_parameter("default_force_pct").value)
|
||||
|
||||
# 当前目标（主关节值，米）与"最近一次是否有状态"。
|
||||
self._target = self.layout.aperture_to_source(self.layout.stroke_mm) # 默认全开
|
||||
self._feedback_state: Optional[float] = None
|
||||
self._state_stamp: Optional[float] = None
|
||||
# ⚠️ 反馈取 layout.feedback，**不是**主关节：上游的 finger_width_joint
|
||||
# 挂在孤立的 unused_finger_width_link 上，在 Gazebo 里根本不动
|
||||
# （实测：命令 0.100，状态恒为 0）。拿它当反馈会永远报"开口 0 mm"。
|
||||
# 详见 sim_gripper.GripperLayout._pick_feedback 的说明。
|
||||
self._state_name = f"{self.layout.feedback}"
|
||||
|
||||
self._pub_cmd = self.create_publisher(
|
||||
Float64MultiArray, f"/{controller}/commands", 10
|
||||
)
|
||||
self._pub_status = self.create_publisher(GripperStatus, f"{self._ns}/status", 10)
|
||||
self.create_subscription(JointState, "/joint_states", self._on_joint_state, 10)
|
||||
|
||||
self._srv: List[object] = []
|
||||
for name in _SERVICE_COMMANDS:
|
||||
self._srv.append(
|
||||
self.create_service(
|
||||
GripperExecute, f"{self._ns}/{name}", self._make_handler(name)
|
||||
)
|
||||
)
|
||||
self._srv.append(
|
||||
self.create_service(GripperExecute, f"{self._ns}/execute", self._on_execute)
|
||||
)
|
||||
|
||||
self.get_logger().info(f"夹爪：{self.layout.describe()}")
|
||||
if self.layout.feedback != self.layout.source:
|
||||
self.get_logger().info(
|
||||
f"夹爪开口反馈取自 {self.layout.feedback}（反推主关节 "
|
||||
f"{self.layout.source}；主关节本身在 Gazebo 里不跟随命令）"
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"夹爪行程口径：{self.layout.stroke_note(_site_stroke_mm())}"
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"服务 {self._ns}/{{{'|'.join(_SERVICE_COMMANDS)}|execute}} → "
|
||||
f"话题 /{controller}/commands"
|
||||
)
|
||||
|
||||
# 先把夹爪摆到默认全开，免得机器人一出场就是捏着的状态。
|
||||
self._publish_target(self._target)
|
||||
self.create_timer(0.1, self._tick_status)
|
||||
|
||||
# --- 参数与描述 -------------------------------------------------------
|
||||
def _robot_description(self) -> str:
|
||||
"""取归一化后的 robot_description（launch 与 RSP 用的是同一份）。"""
|
||||
if not self.has_parameter("robot_description"):
|
||||
self.declare_parameter("robot_description", "")
|
||||
xml = str(self.get_parameter("robot_description").value or "")
|
||||
if not xml.strip():
|
||||
raise GripperConfigError(
|
||||
"没拿到 robot_description 参数，无法确定夹爪关节的限位。\n"
|
||||
" 正常由 gazebo_sim.launch.py 传入（它与发给 robot_state_publisher "
|
||||
"的是同一份归一化描述）。单独手跑本节点时要显式给：\n"
|
||||
" ros2 run arm_sim_gazebo sim_gripper_adapter "
|
||||
"--ros-args -p robot_description:=\"$(xacro ...)\""
|
||||
)
|
||||
return xml
|
||||
|
||||
# --- 下发 -------------------------------------------------------------
|
||||
def _publish_target(self, source_value: float) -> None:
|
||||
"""把一整组关节目标（主 + 从动）按顺序发出去。"""
|
||||
self._target = source_value
|
||||
msg = Float64MultiArray()
|
||||
msg.data = [float(v) for v in self.layout.command_vector_from_source(source_value)]
|
||||
self._pub_cmd.publish(msg)
|
||||
|
||||
def _move_to_aperture(self, aperture_mm: float) -> None:
|
||||
self._publish_target(self.layout.aperture_to_source(aperture_mm))
|
||||
|
||||
# --- 状态 -------------------------------------------------------------
|
||||
def _on_joint_state(self, msg: JointState) -> None:
|
||||
for name, pos in zip(msg.name, msg.position):
|
||||
if name == self._state_name:
|
||||
self._feedback_state = float(pos)
|
||||
self._state_stamp = self._now()
|
||||
return
|
||||
|
||||
def _now(self) -> float:
|
||||
return self.get_clock().now().nanoseconds * 1e-9
|
||||
|
||||
def _fresh_state(self) -> Optional[float]:
|
||||
"""反馈关节的当前值；太久没更新或从没收到过就返回 None。"""
|
||||
if self._feedback_state is None or self._state_stamp is None:
|
||||
return None
|
||||
if (self._now() - self._state_stamp) > STATE_STALE_S:
|
||||
return None
|
||||
return self._feedback_state
|
||||
|
||||
def _status_msg(self) -> GripperStatus:
|
||||
state = self._fresh_state()
|
||||
msg = GripperStatus()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
# frame_id 留空：真机那边填的是 "<side>_gripper"，而仿真里没有任何东西
|
||||
# 发布那个坐标系（TF 只来自 robot_state_publisher）。
|
||||
# 填一个查不到的 frame 比留空更糟 —— tf2 会一直报 frame 不存在。
|
||||
msg.header.frame_id = ""
|
||||
msg.name = "gn01_sim"
|
||||
msg.gripper_type = "parallel"
|
||||
msg.connected = state is not None
|
||||
msg.initialized = state is not None
|
||||
msg.velocity = self._speed
|
||||
msg.force = self._force
|
||||
|
||||
if state is None:
|
||||
# 没状态就别编：position 保持 0 会让上层以为夹爪是合拢的。
|
||||
msg.state = "ERROR"
|
||||
msg.error_msg = (
|
||||
f"{STATE_STALE_S:.1f}s 内没收到 {self._state_name} 的 /joint_states，"
|
||||
f"无法反馈开口。检查 joint_state_broadcaster 是否 active。"
|
||||
)
|
||||
return msg
|
||||
|
||||
aperture = self.layout.aperture_from_feedback(state)
|
||||
target_aperture = self.layout.source_to_aperture(self._target)
|
||||
# 判"还在动"用**开口口径**（毫米），不用主关节值：开口↔关节值在 FK
|
||||
# 口径下是非线性的，拿米当口径的话同一个容差在不同开度下含义不同。
|
||||
moving = abs(aperture - target_aperture) > DEFAULT_MOVING_TOLERANCE_MM
|
||||
msg.position = float(aperture)
|
||||
# 真机这里填 DH 的千分比；仿真的"宽度关节"本身就是千分比语义
|
||||
#（0.100 m ≙ 1000‰），所以直接换算，口径与真机一致。
|
||||
msg.finger_position_per_mille = float(
|
||||
max(0.0, min(1000.0, aperture / max(1e-9, self.layout.stroke_mm) * 1000.0))
|
||||
)
|
||||
msg.is_moving = bool(moving)
|
||||
# 判据：指令在合拢侧 + 实际开口比指令大出容差（被工件挡住）。
|
||||
msg.is_gripped = is_grasping(
|
||||
target_aperture, aperture, self._tolerance_mm
|
||||
)
|
||||
if msg.is_gripped:
|
||||
msg.state = "GRIPPING"
|
||||
elif moving:
|
||||
msg.state = "MOVING"
|
||||
else:
|
||||
msg.state = "IDLE"
|
||||
return msg
|
||||
|
||||
def _tick_status(self) -> None:
|
||||
self._pub_status.publish(self._status_msg())
|
||||
|
||||
# --- 服务 -------------------------------------------------------------
|
||||
def _make_handler(self, verb: str):
|
||||
def handler(request: GripperExecute.Request, response: GripperExecute.Response):
|
||||
# 服务名本身就是命令；但请求里也可能带 command（通用入口风格）。
|
||||
# 若两者冲突，以**请求里的**为准并记一条 info —— 免得上层以为
|
||||
# 服务名能覆盖请求内容。
|
||||
req_verb = str(getattr(request.command, "command", "") or "").strip()
|
||||
if req_verb and req_verb != verb:
|
||||
self.get_logger().info(
|
||||
f"{self._ns}/{verb} 收到 command={req_verb!r}，按请求执行。"
|
||||
)
|
||||
return self._execute_verb(req_verb, request, response)
|
||||
return self._execute_verb(verb, request, response)
|
||||
|
||||
return handler
|
||||
|
||||
def _on_execute(
|
||||
self, request: GripperExecute.Request, response: GripperExecute.Response
|
||||
):
|
||||
verb = str(getattr(request.command, "command", "") or "").strip()
|
||||
if not verb:
|
||||
return self._fail(response, "execute 需要 request.command.command（如 open/close/move）")
|
||||
return self._execute_verb(verb, request, response)
|
||||
|
||||
def _execute_verb(
|
||||
self,
|
||||
verb: str,
|
||||
request: GripperExecute.Request,
|
||||
response: GripperExecute.Response,
|
||||
):
|
||||
cmd = request.command
|
||||
try:
|
||||
if verb == "open":
|
||||
self._move_to_aperture(self.layout.stroke_mm)
|
||||
note = f"全开（{self.layout.stroke_mm:.1f} mm）"
|
||||
elif verb == "close":
|
||||
self._move_to_aperture(0.0)
|
||||
note = "全闭合（开口 0 mm）"
|
||||
elif verb == "move":
|
||||
want = float(cmd.position)
|
||||
clamped = self.layout.is_clamped(want)
|
||||
self._move_to_aperture(want)
|
||||
note = f"开口 → {want:.2f} mm"
|
||||
if clamped:
|
||||
note += (
|
||||
f"（**被限位截断**到 "
|
||||
f"{min(max(want, 0.0), self.layout.stroke_mm):.2f} mm）"
|
||||
)
|
||||
elif verb == "rotate":
|
||||
return self._fail(
|
||||
response,
|
||||
"仿真夹爪（GN01）是平行夹爪，没有旋转自由度；rotate 只在真机 RGI 上有效。",
|
||||
)
|
||||
elif verb == "stop":
|
||||
# 停在当前位置：把"当前实际值"当目标发回去。
|
||||
state = self._fresh_state()
|
||||
if state is None:
|
||||
return self._fail(response, "没有关节状态，stop 无法保持位置")
|
||||
# state 是**反馈关节**的值，得先换算回主关节口径再下发。
|
||||
self._publish_target(self.layout.source_from_feedback(state))
|
||||
note = (
|
||||
"保持当前位置（"
|
||||
f"{self.layout.aperture_from_feedback(state):.2f} mm）"
|
||||
)
|
||||
else:
|
||||
return self._fail(
|
||||
response,
|
||||
f"仿真夹爪不支持命令 {verb!r}；支持 "
|
||||
f"{list(_SERVICE_COMMANDS) + ['stop']}。",
|
||||
)
|
||||
except GripperConfigError as exc: # pragma: no cover - 构造期已挡住
|
||||
return self._fail(response, str(exc))
|
||||
|
||||
response.success = True
|
||||
response.message = note
|
||||
response.status = self._status_msg()
|
||||
self.get_logger().info(f"{self._ns}/{verb}：{note}")
|
||||
return response
|
||||
|
||||
def _fail(self, response: GripperExecute.Response, why: str):
|
||||
response.success = False
|
||||
response.message = why
|
||||
# status 仍然填：调用方拿到 success=false 时也需要知道当前开口，
|
||||
# 否则只能靠猜（真机的返回结构也是这个语义）。
|
||||
response.status = self._status_msg()
|
||||
self.get_logger().warn(f"{self._ns}：{why}")
|
||||
return response
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = None
|
||||
try:
|
||||
node = SimGripperAdapter()
|
||||
rclpy.spin(node)
|
||||
except GripperConfigError:
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(2)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,368 @@
|
||||
"""由 URDF 做夹爪正运动学，把"开口毫米"换算成**真的**关节角（纯逻辑，不起 ROS）。
|
||||
|
||||
=============================================================================
|
||||
为什么需要它：上游的 <mimic> 是线性近似
|
||||
=============================================================================
|
||||
GN01 的描述用一条线性耦合把 6 个从动关节挂在宽度关节上：
|
||||
|
||||
从动关节 = 9.404 × 主关节 − 0.155
|
||||
|
||||
系数 9.404 是按**两端**拟合出来的，所以两端准、中段不准 —— 因为真实的
|
||||
手指不是"平移"，而是"绕着 knuckle 转 + 指掌再转"，指垫的间距
|
||||
``|tip_L(θ) − tip_R(θ)|`` 是 θ 的非线性函数。
|
||||
|
||||
实测（2026-09-24，Gazebo Fortress，见 test_sim_gripper_kin.py 里的黄金表）：
|
||||
|
||||
命令开口 指尖间距实测 与命令之差
|
||||
0 mm 27.09 mm （固有间距，指垫此时接触）
|
||||
8 mm 35.76 mm +0.67 mm
|
||||
20 mm 48.84 mm +1.75 mm
|
||||
50 mm 80.96 mm +3.87 mm
|
||||
100 mm 127.02 mm −0.07 mm
|
||||
|
||||
也就是说：**命令"开 20 mm"，两指实际相距 21.75 mm**。抓一个 20 mm 宽的
|
||||
通止规时，指垫之间会空出 1.75 mm —— 没有接触、没有接触力，触觉那一步
|
||||
在仿真里直接失效。这和真机（DH，毫米制、开多少就是多少）的口径不一致，
|
||||
仿真就失去了"拿真几何试算法"的意义。
|
||||
|
||||
所以本模块用 URDF 自己算正运动学，把 mm 反解成关节角：
|
||||
|
||||
aperture_to_source(20.0) → 令 |tip_L − tip_R| − 闭合间距 == 20.0 的关节值
|
||||
|
||||
这样"开口 20 mm"在仿真里就是**物理上真的 20 mm**，与真机口径一致。
|
||||
|
||||
=============================================================================
|
||||
为什么可靠
|
||||
=============================================================================
|
||||
本文件的正运动学与 Gazebo（DART 物理 + sdformat 转换后的模型）在
|
||||
0~100 mm 全程 **吻合到 0.01 mm**（上面那张表就是两边对比的结果）。
|
||||
两者完全独立：一个是我们的矩阵乘法，一个是物理引擎。这个一致性同时证明：
|
||||
|
||||
- URDF → SDF 的转换没有丢掉这些 link/joint 的偏移；
|
||||
- 我们对耦合公式的理解与 Gazebo 实际执行的一致。
|
||||
|
||||
=============================================================================
|
||||
通用性（换夹爪时）
|
||||
=============================================================================
|
||||
本模块不写死 GN01 的名字：
|
||||
- 树结构完全从 URDF 里读；
|
||||
- 两个"指尖"link 按命名约定找（``*left*finger*tip*`` / ``*right*...``），
|
||||
找不到就让调用方退回线性近似（并在日志里**明确说出口径变了**）；
|
||||
- 计算指尖间距用的参考系取两者的**最近公共祖先**，不需要知道夹爪 base
|
||||
叫什么名字（GN01 是 ``grav_base_link``，别的夹爪叫别的）。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import re
|
||||
import xml.etree.ElementTree as ET
|
||||
from dataclasses import dataclass
|
||||
from typing import Callable, Dict, List, Mapping, Optional, Sequence, Tuple
|
||||
|
||||
Mat3 = Tuple[Tuple[float, float, float], ...]
|
||||
Vec3 = Tuple[float, float, float]
|
||||
|
||||
|
||||
class KinematicsError(ValueError):
|
||||
"""URDF 结构与预期不符（缺 link、链断开、不是树等）。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointNode:
|
||||
"""URDF 里的一条 joint（父 → 子），带固定偏移与转动轴。"""
|
||||
|
||||
name: str
|
||||
type: str
|
||||
parent: str
|
||||
child: str
|
||||
xyz: Vec3
|
||||
rpy: Vec3
|
||||
axis: Vec3
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 3×3 矩阵小工具（只有旋转，位置单独带着走 —— 比 4×4 好读）
|
||||
# ---------------------------------------------------------------------------
|
||||
def _rot_rpy(rpy: Vec3) -> Mat3:
|
||||
r, p, y = rpy
|
||||
cr, sr = math.cos(r), math.sin(r)
|
||||
cp, sp = math.cos(p), math.sin(p)
|
||||
cy, sy = math.cos(y), math.sin(y)
|
||||
return (
|
||||
(cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr),
|
||||
(sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr),
|
||||
(-sp, cp * sr, cp * cr),
|
||||
)
|
||||
|
||||
|
||||
def _rot_axis(axis: Vec3, angle: float) -> Mat3:
|
||||
x, y, z = axis
|
||||
norm = math.sqrt(x * x + y * y + z * z)
|
||||
if norm < 1e-12:
|
||||
raise KinematicsError("关节 axis 是零向量，无法确定转动轴")
|
||||
x, y, z = x / norm, y / norm, z / norm
|
||||
c, s, k = math.cos(angle), math.sin(angle), 1.0 - math.cos(angle)
|
||||
return (
|
||||
(x * x * k + c, x * y * k - z * s, x * z * k + y * s),
|
||||
(y * x * k + z * s, y * y * k + c, y * z * k - x * s),
|
||||
(z * x * k - y * s, z * y * k + x * s, z * z * k + c),
|
||||
)
|
||||
|
||||
|
||||
def _mat_mul(a: Mat3, b: Mat3) -> Mat3:
|
||||
return tuple(
|
||||
tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3))
|
||||
for i in range(3)
|
||||
)
|
||||
|
||||
|
||||
def _mat_vec(a: Mat3, v: Vec3) -> Vec3:
|
||||
return tuple(sum(a[i][k] * v[k] for k in range(3)) for i in range(3)) # type: ignore[return-value]
|
||||
|
||||
|
||||
def _dist(a: Vec3, b: Vec3) -> float:
|
||||
return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# URDF 树
|
||||
# ---------------------------------------------------------------------------
|
||||
def _floats(text: Optional[str], default: Sequence[float]) -> Vec3:
|
||||
if not text:
|
||||
return tuple(default) # type: ignore[return-value]
|
||||
parts = text.split()
|
||||
if len(parts) != 3:
|
||||
raise KinematicsError(f"URDF 里的三元组长度不对: {text!r}")
|
||||
return (float(parts[0]), float(parts[1]), float(parts[2]))
|
||||
|
||||
|
||||
class UrdfTree:
|
||||
"""URDF 的 joint 树；只关心运动学（不碰 inertial / visual / collision）。"""
|
||||
|
||||
def __init__(self, xml: str) -> None:
|
||||
root = ET.fromstring(xml)
|
||||
self.by_child: Dict[str, JointNode] = {}
|
||||
self.parent_of: Dict[str, str] = {}
|
||||
for j in root.findall("joint"):
|
||||
origin = j.find("origin")
|
||||
axis_el = j.find("axis")
|
||||
parent_el, child_el = j.find("parent"), j.find("child")
|
||||
if parent_el is None or child_el is None or j.get("name") is None:
|
||||
raise KinematicsError("URDF 里有缺 name/parent/child 的 joint")
|
||||
node = JointNode(
|
||||
name=j.get("name", ""),
|
||||
type=j.get("type", "fixed"),
|
||||
parent=parent_el.get("link", ""),
|
||||
child=child_el.get("link", ""),
|
||||
xyz=_floats(origin.get("xyz") if origin is not None else None, (0, 0, 0)),
|
||||
rpy=_floats(origin.get("rpy") if origin is not None else None, (0, 0, 0)),
|
||||
axis=_floats(axis_el.get("xyz") if axis_el is not None else None, (1, 0, 0)),
|
||||
)
|
||||
if node.child in self.by_child:
|
||||
raise KinematicsError(f"link {node.child!r} 有两条父关节，不是树")
|
||||
self.by_child[node.child] = node
|
||||
self.parent_of[node.child] = node.parent
|
||||
|
||||
# --- 结构查询 ---------------------------------------------------------
|
||||
def ancestors(self, link: str) -> List[str]:
|
||||
"""从 link 一路向上到根（含自身）。"""
|
||||
out: List[str] = []
|
||||
seen = set()
|
||||
cur = link
|
||||
while True:
|
||||
out.append(cur)
|
||||
if cur not in self.parent_of:
|
||||
return out
|
||||
if cur in seen:
|
||||
raise KinematicsError(f"URDF 的 link 链里有环（{cur}）")
|
||||
seen.add(cur)
|
||||
cur = self.parent_of[cur]
|
||||
|
||||
def common_ancestor(self, a: str, b: str) -> str:
|
||||
"""最近公共祖先 link（用 A 的祖先链去查 B 的，第一个命中的就是）。"""
|
||||
b_chain = set(self.ancestors(b))
|
||||
for link in self.ancestors(a):
|
||||
if link in b_chain:
|
||||
return link
|
||||
raise KinematicsError(f"{a!r} 与 {b!r} 没有公共祖先（URDF 断了？）")
|
||||
|
||||
def chain(self, link: str, root: str) -> List[JointNode]:
|
||||
"""root → link 的关节序列。"""
|
||||
out: List[JointNode] = []
|
||||
cur = link
|
||||
while cur != root:
|
||||
if cur not in self.by_child:
|
||||
raise KinematicsError(f"从 {link!r} 上溯不到 {root!r}")
|
||||
node = self.by_child[cur]
|
||||
out.append(node)
|
||||
cur = node.parent
|
||||
return list(reversed(out))
|
||||
|
||||
# --- 正运动学 ---------------------------------------------------------
|
||||
def position(self, link: str, root: str, angles: Mapping[str, float]) -> Vec3:
|
||||
"""link 原点在 root 坐标系里的位置。angles 缺省视为 0。"""
|
||||
rot: Mat3 = ((1.0, 0, 0), (0, 1.0, 0), (0, 0, 1.0))
|
||||
pos: Vec3 = (0.0, 0.0, 0.0)
|
||||
for node in self.chain(link, root):
|
||||
offset = _mat_vec(rot, node.xyz)
|
||||
pos = (pos[0] + offset[0], pos[1] + offset[1], pos[2] + offset[2])
|
||||
rot = _mat_mul(rot, _rot_rpy(node.rpy))
|
||||
if node.type != "fixed":
|
||||
rot = _mat_mul(rot, _rot_axis(node.axis, float(angles.get(node.name, 0.0))))
|
||||
return pos
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 找指尖 link（按命名约定，不写死 GN01）
|
||||
# ---------------------------------------------------------------------------
|
||||
_LEFT_TIP_PATTERNS = (
|
||||
r".*left.*finger.*tip$",
|
||||
r".*left.*tip$",
|
||||
r".*left.*tcp$",
|
||||
)
|
||||
_RIGHT_TIP_PATTERNS = (
|
||||
r".*right.*finger.*tip$",
|
||||
r".*right.*tip$",
|
||||
r".*right.*tcp$",
|
||||
)
|
||||
|
||||
|
||||
def _pick(links: Sequence[str], patterns: Sequence[str]) -> Optional[str]:
|
||||
for pat in patterns:
|
||||
rx = re.compile(pat) # 大小写敏感：URDF 里普遍是小写
|
||||
hits = sorted(link for link in links if rx.match(link))
|
||||
if hits:
|
||||
# 多个命中时取名字最长的（``sim_left_finger_tip`` 比 ``left_tip``
|
||||
# 更可能是真正的指垫），并保持确定性（已排序）。
|
||||
return max(hits, key=len)
|
||||
return None
|
||||
|
||||
|
||||
def find_tip_pair(tree: UrdfTree) -> Optional[Tuple[str, str]]:
|
||||
"""按命名约定找左右指垫 link；找不到返回 None（调用方退回线性近似）。"""
|
||||
links = sorted(set(tree.by_child))
|
||||
left = _pick(links, _LEFT_TIP_PATTERNS)
|
||||
right = _pick(links, _RIGHT_TIP_PATTERNS)
|
||||
if left is None or right is None or left == right:
|
||||
return None
|
||||
return (left, right)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 开口曲线：mm ↔ 关节值
|
||||
# ---------------------------------------------------------------------------
|
||||
class ApertureCurve:
|
||||
"""由正运动学给出的"关节值 → 两指间距(mm)"曲线，以及它的反解。
|
||||
|
||||
``joint_value`` 是**主关节**的值（GN01 就是宽度关节，单位米）。
|
||||
调用方通过 ``angles_of(joint_value)`` 告诉本类"主关节取这个值时，
|
||||
每个关节分别是多少" —— 耦合关系归调用方（sim_gripper.py）管，
|
||||
本类只管几何，这样换夹爪/换耦合方式都不用动几何代码。
|
||||
"""
|
||||
|
||||
#: 反解用的二分次数（40 次足够把 100 mm 行程收敛到 1e-9 mm 以内）。
|
||||
BISECT_ITERS = 60
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
tree: UrdfTree,
|
||||
tips: Tuple[str, str],
|
||||
angles_of: Callable[[float], Mapping[str, float]],
|
||||
lower: float,
|
||||
upper: float,
|
||||
) -> None:
|
||||
self.tree = tree
|
||||
self.tips = tips
|
||||
self.angles_of = angles_of
|
||||
self.lower = float(lower)
|
||||
self.upper = float(upper)
|
||||
self.root = tree.common_ancestor(tips[0], tips[1])
|
||||
# 闭合位置（主关节到下限）的指尖间距 —— 指垫此时贴在一起，
|
||||
# 所以"开口"一律相对它算，这样开口 0 就是"真的合拢"。
|
||||
self._closed = self._raw_separation(self.lower)
|
||||
self._stroke = self._raw_separation(self.upper) - self._closed
|
||||
# ⚠️ 这里不能只判 `stroke <= 0`。实测过一次：把右侧 knuckle 的符号写反时，
|
||||
# 两指**同步**摆动，间距始终 45.0000000000 mm —— 差值是 4e-13，
|
||||
# 一个裸的 `<= 0` 会放过它。而放过的后果是**静默失效**：开口曲线是
|
||||
# 水平的，`joint_value()` 的二分会一路收敛到下限，
|
||||
# 于是"开 20 mm"变成"不动"，且没有任何报错。
|
||||
# 所以阈值取"名义行程的 1%"：真实夹爪不可能只有这么点行程，
|
||||
# 而浮点噪声远小于它。
|
||||
nominal_mm = abs(self.upper - self.lower) * 1000.0
|
||||
min_stroke = max(1e-6, 0.01 * nominal_mm)
|
||||
if self._stroke <= min_stroke:
|
||||
raise KinematicsError(
|
||||
f"正运动学算出两指行程只有 {self._stroke:.6f} mm"
|
||||
f"（名义 {nominal_mm:.1f} mm）—— 耦合方向或 tip link 选错了："
|
||||
f"关节在动，但两指间距几乎不变"
|
||||
)
|
||||
self._check_monotonic()
|
||||
|
||||
# --- 基础量 -----------------------------------------------------------
|
||||
def _raw_separation(self, joint_value: float) -> float:
|
||||
angles = self.angles_of(joint_value)
|
||||
a = self.tree.position(self.tips[0], self.root, angles)
|
||||
b = self.tree.position(self.tips[1], self.root, angles)
|
||||
return 1000.0 * _dist(a, b)
|
||||
|
||||
def _check_monotonic(self, samples: int = 24) -> None:
|
||||
"""间距必须随主关节单调增 —— 否则反解会给出"看起来对但跳变"的结果。
|
||||
|
||||
真实夹爪（含 GN01）都满足；不满足说明耦合表填错了（比如某条
|
||||
multiplier 正负号写反），这时**必须报错**而不是继续，
|
||||
否则抓取时开口会忽大忽小。
|
||||
"""
|
||||
prev = None
|
||||
for i in range(samples + 1):
|
||||
v = self.lower + (self.upper - self.lower) * i / samples
|
||||
cur = self._raw_separation(v)
|
||||
if prev is not None and cur < prev - 1e-6:
|
||||
raise KinematicsError(
|
||||
f"指尖间距不随主关节单调变化（{v:.4f} 处 {cur:.3f} mm < "
|
||||
f"前一点 {prev:.3f} mm）—— 检查耦合表的正负号"
|
||||
)
|
||||
prev = cur
|
||||
|
||||
# --- 对外 -------------------------------------------------------------
|
||||
@property
|
||||
def closed_separation_mm(self) -> float:
|
||||
"""合拢时两侧 tip frame 之间的固有间距（GN01 约 27 mm）。
|
||||
|
||||
注意它不是开口：指垫此时贴在一起。验收脚本要拿它把"间距"换成
|
||||
"开口"，见 scripts/check_sim_gripper.py。
|
||||
"""
|
||||
return self._closed
|
||||
|
||||
@property
|
||||
def stroke_mm(self) -> float:
|
||||
"""两指的**实测**行程（毫米）。GN01 ≈ 99.93，而不是名义的 100.0。
|
||||
|
||||
这个差不是误差：上游把宽度关节的限位定在 0~0.100 m，而线性耦合
|
||||
让指尖实际只走了 99.93 mm。启动日志里报的是这个实测值 ——
|
||||
报名义值会让"全开时开口读数与几何对不上"变成一个查不出的矛盾。
|
||||
"""
|
||||
return self._stroke
|
||||
|
||||
def aperture_mm(self, joint_value: float) -> float:
|
||||
"""主关节值 → 开口（毫米）。
|
||||
|
||||
超出限位时按限位截断（位置控制器本来也会截断，这里提前对齐口径）。
|
||||
"""
|
||||
v = min(max(float(joint_value), self.lower), self.upper)
|
||||
return self._raw_separation(v) - self._closed
|
||||
|
||||
def joint_value(self, aperture_mm: float) -> float:
|
||||
"""开口（毫米）→ 主关节值。二分反解，超出行程按端点截断。"""
|
||||
want = min(max(float(aperture_mm), 0.0), self._stroke)
|
||||
lo, hi = self.lower, self.upper
|
||||
for _ in range(self.BISECT_ITERS):
|
||||
mid = 0.5 * (lo + hi)
|
||||
if self.aperture_mm(mid) < want:
|
||||
lo = mid
|
||||
else:
|
||||
hi = mid
|
||||
return 0.5 * (lo + hi)
|
||||
|
||||
def is_clamped(self, aperture_mm: float) -> bool:
|
||||
"""请求的开口是否超出这个夹爪能开的范围（用来提醒上层）。"""
|
||||
return not (0.0 <= float(aperture_mm) <= self._stroke)
|
||||
@@ -492,6 +492,55 @@ def _prepare(context, *args, **kwargs):
|
||||
]
|
||||
actions.extend(controllers)
|
||||
|
||||
# --- 6b) 仿真夹爪适配器 --------------------------------------------------
|
||||
# 为什么必须有它：sdformat 把 URDF 的 <mimic> 整个丢掉，gz-sim 也不实现，
|
||||
# 所以 6 个从动关节不会跟随主关节。适配器按耦合公式算出 7 个关节的目标值
|
||||
# 一起下发，并对外提供与真机**同名同类型**的 GripperExecute 服务
|
||||
#（<ns>/open|close|move|rotate + status），上层代码一行都不用改。
|
||||
#
|
||||
# 耦合表来自 report.stripped_mimics —— 也就是 urdf_normalize 删掉 <mimic>
|
||||
# 时记下来的那几张表。让 launch 传给适配器，而不是让适配器自己再展开一次
|
||||
# xacro：两次展开在参数不同时会静默不一致，而症状是"手指角度不对"。
|
||||
if load_gripper != "false":
|
||||
from arm_sim_gazebo.sim_gripper import format_mimic_param
|
||||
|
||||
mimics = [format_mimic_param(*t) for t in report.stripped_mimics]
|
||||
if not mimics:
|
||||
# 这里必须出声：拿不到耦合表说明夹爪没被加载（或上游改了耦合方式），
|
||||
# 而适配器会因此拒绝启动 —— 早知道比晚知道好。
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg="[gazebo_sim] ⚠ 归一化报告里没有 mimic 关系，"
|
||||
"仿真夹爪适配器将无法启动（它会明确报错）。"
|
||||
"检查 load_gripper 与 flexiv_description 的 grav 宏。"
|
||||
)
|
||||
)
|
||||
actions.append(
|
||||
TimerAction(
|
||||
# 等 gripper_controller 起来再发命令：控制器还没 active 时
|
||||
# 发出去的 /commands 会被丢弃（不报错），夹爪就一直停在初始位。
|
||||
period=CONTROLLER_DELAY + len(CONTROLLER_NAMES) * 2.0 + 1.0,
|
||||
actions=[
|
||||
Node(
|
||||
package="arm_sim_gazebo",
|
||||
executable="sim_gripper_adapter",
|
||||
name="sim_gripper_adapter",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"controllers_file": str(default_controllers(root)),
|
||||
"controller": "gripper_controller",
|
||||
"mimics": mimics,
|
||||
"robot_description": urdf,
|
||||
"namespace": "/gripper",
|
||||
},
|
||||
{"use_sim_time": True},
|
||||
],
|
||||
)
|
||||
],
|
||||
)
|
||||
)
|
||||
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg=(
|
||||
|
||||
@@ -46,6 +46,10 @@ setup(
|
||||
# arm_msgs/TactileStatus（轴向投影 + 基线相减 + 恒定发布）。
|
||||
# 由 gazebo_sim.launch.py 拉起；单独调试时可以用。
|
||||
'sim_tactile_adapter = arm_sim_gazebo.sim_tactile_adapter:main',
|
||||
# 仿真夹爪适配器：把 GripperExecute 服务（开口 mm）变成
|
||||
# 7 个夹爪关节的目标位置（含 mimic 耦合换算），让仿真能走
|
||||
# 真机那一套夹爪接口。由 gazebo_sim.launch.py 拉起。
|
||||
'sim_gripper_adapter = arm_sim_gazebo.sim_gripper_adapter:main',
|
||||
# 独立小工具：只看归一化做了什么，不启动 Gazebo。
|
||||
# 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。
|
||||
'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',
|
||||
|
||||
@@ -0,0 +1,497 @@
|
||||
"""sim_gripper 的单测：耦合链、毫米↔关节值、命令向量、夹住判据。
|
||||
|
||||
这些测试锁定的都是**这个适配器真正会出错的地方**，不是凭空设想的边界情况：
|
||||
|
||||
1. GN01 的耦合是**一条链**，不是"6 个手指关节直挂主关节"。
|
||||
只套一层 multiplier 的实现看起来完全正常（夹爪会动、角度也是那个量级），
|
||||
只是指尖角度一直不对 —— 这种错误只能靠测试钉住。
|
||||
本文件第一个版本就踩了这个坑，所以下面有专门的链式测试。
|
||||
2. 命令数组的顺序取自 controllers.yaml，顺序错了 Gazebo 不报错，
|
||||
只是把角度发给别的关节。
|
||||
3. 米 / 毫米 差 1000 倍。仿真里 100 mm 行程写成 0.100，真机是 20 mm；
|
||||
混用不会报错，只会让夹爪"开到别的地方"。
|
||||
"""
|
||||
import os
|
||||
import sys
|
||||
|
||||
import pytest
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
|
||||
from arm_sim_gazebo.sim_gripper import ( # noqa: E402
|
||||
DEFAULT_CONTROLLER,
|
||||
DEFAULT_GRASP_TOLERANCE_MM,
|
||||
GripperConfigError,
|
||||
GripperLayout,
|
||||
MimicRelation,
|
||||
format_mimic_param,
|
||||
gripper_joint_order,
|
||||
is_grasping,
|
||||
joint_limits,
|
||||
layout_from_parts,
|
||||
parse_mimics,
|
||||
)
|
||||
|
||||
# 真实的 GN01 耦合表（从 flexiv_description 的 grav_macro.xacro 里读出来的）。
|
||||
# 注意它是链：finger_width → left_outer_knuckle → 其余 5 个。
|
||||
GN01_MIMICS = [
|
||||
("sim_left_outer_knuckle_joint", "sim_finger_width_joint", "9.404", "-0.155"),
|
||||
("sim_left_inner_knuckle_joint", "sim_left_outer_knuckle_joint", "1", "0"),
|
||||
("sim_left_inner_finger_joint", "sim_left_outer_knuckle_joint", "-1", "0"),
|
||||
("sim_right_outer_knuckle_joint", "sim_left_outer_knuckle_joint", "1", "0"),
|
||||
("sim_right_inner_knuckle_joint", "sim_left_outer_knuckle_joint", "1", "0"),
|
||||
("sim_right_inner_finger_joint", "sim_left_outer_knuckle_joint", "-1", "0"),
|
||||
]
|
||||
|
||||
GN01_ORDER = [
|
||||
"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",
|
||||
]
|
||||
|
||||
# 与真实描述一致的最小 URDF 片段：主关节限位 0~0.100（米），语义是开口。
|
||||
URDF = (
|
||||
'<?xml version="1.0"?>\n<robot name="t">\n'
|
||||
' <joint name="sim_finger_width_joint" type="revolute">\n'
|
||||
' <parent link="a"/><child link="b"/>\n'
|
||||
' <limit lower="0" upper="0.100" effort="80" velocity="0.2"/>\n'
|
||||
' </joint>\n'
|
||||
"</robot>\n"
|
||||
)
|
||||
|
||||
|
||||
def _gn01_layout():
|
||||
return layout_from_parts(
|
||||
GN01_ORDER,
|
||||
parse_mimics([format_mimic_param(*t) for t in GN01_MIMICS]),
|
||||
URDF,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# mimic 参数解析
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestParseMimics:
|
||||
def test_round_trip(self):
|
||||
rel = parse_mimics([format_mimic_param(*GN01_MIMICS[0])])
|
||||
assert rel["sim_left_outer_knuckle_joint"].source == "sim_finger_width_joint"
|
||||
assert rel["sim_left_outer_knuckle_joint"].multiplier == pytest.approx(9.404)
|
||||
assert rel["sim_left_outer_knuckle_joint"].offset == pytest.approx(-0.155)
|
||||
|
||||
def test_negative_offset_survives_the_split(self):
|
||||
"""offset 是负数（-0.155）—— 分隔符与负号不能被搞混。"""
|
||||
rel = parse_mimics(["a:b:1:-0.5"])["a"]
|
||||
assert rel.offset == pytest.approx(-0.5)
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"bad",
|
||||
[
|
||||
"a:b:1", # 段数不够
|
||||
"a:b:1:0:0", # 段数太多
|
||||
"a::1:0", # 主关节名为空
|
||||
":b:1:0", # 从动关节名为空
|
||||
"a:b:x:0", # multiplier 不是数字
|
||||
"a:b:1:y", # offset 不是数字
|
||||
],
|
||||
)
|
||||
def test_malformed_entry_raises(self, bad):
|
||||
with pytest.raises(GripperConfigError):
|
||||
parse_mimics([bad])
|
||||
|
||||
def test_self_reference_raises(self):
|
||||
with pytest.raises(GripperConfigError, match="同名"):
|
||||
parse_mimics(["a:a:1:0"])
|
||||
|
||||
def test_duplicate_joint_raises(self):
|
||||
with pytest.raises(GripperConfigError, match="两次"):
|
||||
parse_mimics(["a:b:1:0", "a:b:2:0"])
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 控制器顺序
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestGripperJointOrder:
|
||||
def test_reads_real_controllers_yaml(self):
|
||||
"""真配置里必须读到 7 个关节，且第一个是主关节。"""
|
||||
root = os.environ.get("ROS2_WS") or os.path.dirname(
|
||||
os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
)
|
||||
path = os.path.join(root, "devices/sim/gazebo/config/controllers.yaml")
|
||||
if not os.path.isfile(path):
|
||||
pytest.skip(f"找不到 {path}")
|
||||
order = gripper_joint_order(path, DEFAULT_CONTROLLER)
|
||||
assert len(order) == 7
|
||||
assert order[0] == "sim_finger_width_joint"
|
||||
assert len(set(order)) == len(order)
|
||||
|
||||
def test_missing_controller_raises(self, tmp_path):
|
||||
f = tmp_path / "c.yaml"
|
||||
f.write_text("other_controller:\n ros__parameters:\n joints: [a]\n", encoding="utf-8")
|
||||
with pytest.raises(GripperConfigError, match="ros__parameters"):
|
||||
gripper_joint_order(f, "gripper_controller")
|
||||
|
||||
def test_empty_joints_raises(self, tmp_path):
|
||||
f = tmp_path / "c.yaml"
|
||||
f.write_text("gripper_controller:\n ros__parameters:\n joints: []\n", encoding="utf-8")
|
||||
with pytest.raises(GripperConfigError, match="joints 为空"):
|
||||
gripper_joint_order(f, "gripper_controller")
|
||||
|
||||
def test_duplicate_joints_raise(self, tmp_path):
|
||||
f = tmp_path / "c.yaml"
|
||||
f.write_text(
|
||||
"gripper_controller:\n ros__parameters:\n joints: [a, a]\n", encoding="utf-8"
|
||||
)
|
||||
with pytest.raises(GripperConfigError, match="重复"):
|
||||
gripper_joint_order(f, "gripper_controller")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 关节限位
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestJointLimits:
|
||||
def test_reads_limits(self):
|
||||
assert joint_limits(URDF, "sim_finger_width_joint") == (0.0, pytest.approx(0.100))
|
||||
|
||||
def test_missing_joint_raises(self):
|
||||
with pytest.raises(GripperConfigError, match="找不到关节"):
|
||||
joint_limits(URDF, "sim_nope_joint")
|
||||
|
||||
def test_missing_limit_raises(self):
|
||||
xml = (
|
||||
'<robot><joint name="j" type="revolute"><limit effort="1" velocity="1"/>'
|
||||
"</joint></robot>"
|
||||
)
|
||||
with pytest.raises(GripperConfigError, match="upper"):
|
||||
joint_limits(xml, "j")
|
||||
|
||||
def test_inverted_limits_raise(self):
|
||||
xml = '<robot><joint name="j"><limit lower="1" upper="0"/></joint></robot>'
|
||||
with pytest.raises(GripperConfigError, match="限位不合法"):
|
||||
joint_limits(xml, "j")
|
||||
|
||||
def test_mimic_joint_is_rejected_as_source(self):
|
||||
"""从动关节不能当主关节 —— 选错了整条链都没人驱动。"""
|
||||
xml = (
|
||||
'<robot><joint name="j"><limit lower="0" upper="1"/><mimic joint="k"/></joint></robot>'
|
||||
)
|
||||
with pytest.raises(GripperConfigError, match="从动关节"):
|
||||
joint_limits(xml, "j")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# layout 组装与校验
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestLayoutFromParts:
|
||||
def test_gn01_layout(self):
|
||||
layout = _gn01_layout()
|
||||
assert layout.source == "sim_finger_width_joint"
|
||||
assert layout.lower == 0.0
|
||||
assert layout.upper == pytest.approx(0.100)
|
||||
assert layout.stroke_mm == pytest.approx(100.0)
|
||||
assert len(layout.order) == 7
|
||||
assert len(layout.relations) == 6
|
||||
|
||||
def test_follower_not_in_order_raises(self):
|
||||
rels = parse_mimics(["sim_ghost:sim_finger_width_joint:1:0"])
|
||||
with pytest.raises(GripperConfigError, match="不在控制器 joints"):
|
||||
layout_from_parts(GN01_ORDER, rels, URDF)
|
||||
|
||||
def test_source_not_in_order_raises(self):
|
||||
"""链上某一环的主关节没被控制器声明 —— 那一级角度没人驱动。"""
|
||||
rels = parse_mimics(["sim_left_outer_knuckle_joint:sim_missing_root:1:0"])
|
||||
with pytest.raises(GripperConfigError, match="主关节.*不在控制器"):
|
||||
layout_from_parts(GN01_ORDER, rels, URDF)
|
||||
|
||||
def test_multiple_roots_raise(self):
|
||||
"""两个关节都不带 mimic → 推断不出谁是被命令的那个，必须报错。"""
|
||||
with pytest.raises(GripperConfigError, match="唯一确定主关节"):
|
||||
layout_from_parts(GN01_ORDER, {}, URDF)
|
||||
|
||||
def test_cycle_raises(self):
|
||||
"""成环必须报错。
|
||||
|
||||
构造要小心：``layout_from_parts`` 先检查"不带 mimic 的关节是否唯一"，
|
||||
所以随手写两条互为 source 的关系通常会先撞上那个检查。这里用一组
|
||||
"除 root 外全都有 mimic" 的数据，才能真的走到成环检测。
|
||||
"""
|
||||
order = ["root", "a", "b"]
|
||||
urdf = (
|
||||
'<robot><joint name="root"><limit lower="0" upper="1"/></joint></robot>'
|
||||
)
|
||||
rels = parse_mimics(["a:b:1:0", "b:a:1:0"])
|
||||
with pytest.raises(GripperConfigError, match="成环"):
|
||||
layout_from_parts(order, rels, urdf)
|
||||
|
||||
def test_chain_not_reaching_root_raises(self):
|
||||
"""链走到一个"既不是主关节、也没有 mimic"的关节 → 描述不完整。
|
||||
|
||||
⚠️ 这条在 ``layout_from_parts`` 里**到不了**：那个"不带 mimic 的关节
|
||||
必须唯一"的检查已经把它挡住了（走到一个无 mimic 的非主关节，就说明
|
||||
存在第二个不带 mimic 的关节）。所以这里直接构造 ``GripperLayout``
|
||||
来钉住这个防御性分支 —— 免得将来有人调整校验顺序时把它变成真空洞。
|
||||
"""
|
||||
layout = GripperLayout(
|
||||
["root", "a"],
|
||||
{"a": MimicRelation("a", "elsewhere", 1.0, 0.0)},
|
||||
"root",
|
||||
0.0,
|
||||
1.0,
|
||||
)
|
||||
with pytest.raises(GripperConfigError, match="终止在"):
|
||||
layout.value_for("a", 0.5)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 耦合链：这是本模块最容易写错的地方
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestCouplingChain:
|
||||
def test_chain_is_applied_level_by_level(self):
|
||||
"""两级链：先套靠近根的，再套靠近叶的。
|
||||
|
||||
left_inner_finger = -1 × left_outer_knuckle
|
||||
left_outer_knuckle = 9.404 × width - 0.155
|
||||
|
||||
只套一层（漏掉第二级）会得到 -1×(9.404w-0.155) 之外的值，
|
||||
这正是"夹爪会动但夹不准"的成因。
|
||||
"""
|
||||
layout = _gn01_layout()
|
||||
s = 0.04 # 40 mm 开口
|
||||
knuckle = 9.404 * s - 0.155
|
||||
assert layout.value_for("sim_left_inner_finger_joint", s) == pytest.approx(-knuckle)
|
||||
assert layout.value_for("sim_left_outer_knuckle_joint", s) == pytest.approx(knuckle)
|
||||
|
||||
def test_root_maps_written_limit_to_knuckle_limits(self):
|
||||
"""开口全开/全闭应当正好撞上 from-knuckle 关节的上下限。
|
||||
|
||||
这条把"绝对物理映射"这个口径钉住：如果哪天改成归一化映射，
|
||||
这里立刻会红。
|
||||
"""
|
||||
layout = _gn01_layout()
|
||||
assert layout.value_for("sim_left_outer_knuckle_joint", layout.upper) == pytest.approx(
|
||||
0.7854, abs=1e-4
|
||||
)
|
||||
assert layout.value_for("sim_left_outer_knuckle_joint", layout.lower) == pytest.approx(
|
||||
-0.155, abs=1e-4
|
||||
)
|
||||
|
||||
@pytest.mark.parametrize("aperture", [0.0, 5.0, 8.0, 20.0, 50.0, 100.0])
|
||||
def test_command_vector_matches_the_urdf_formulas(self, aperture):
|
||||
"""命令向量里的每个值都用**独立实现**重算一遍。
|
||||
|
||||
独立实现直接按原始耦合表递归求值（不依赖 GripperLayout），
|
||||
所以它能抓住"链的顺序搞反""只套一层""用错 source"这类错误。
|
||||
"""
|
||||
layout = _gn01_layout()
|
||||
raw = {
|
||||
j: (s, float(m), float(o))
|
||||
for j, s, m, o in GN01_MIMICS
|
||||
}
|
||||
|
||||
def expected(joint: str) -> float:
|
||||
value = layout.aperture_to_source(aperture)
|
||||
chain = []
|
||||
cur = joint
|
||||
while cur in raw:
|
||||
src, mult, off = raw[cur]
|
||||
chain.append((mult, off))
|
||||
cur = src
|
||||
for mult, off in reversed(chain):
|
||||
value = mult * value + off
|
||||
return value
|
||||
|
||||
vec = layout.command_vector(aperture)
|
||||
assert len(vec) == len(GN01_ORDER)
|
||||
for name, got in zip(GN01_ORDER, vec):
|
||||
assert got == pytest.approx(expected(name), abs=1e-12), f"{name} 对不上"
|
||||
|
||||
def test_vector_order_follows_controllers_yaml(self):
|
||||
"""数组顺序必须与传入的 joints 顺序一致（控制器按顺序取）。"""
|
||||
layout = _gn01_layout()
|
||||
vec = layout.command_vector(50.0)
|
||||
# 主关节在下标 0；把它改成另一个位置，向量里的值也必须跟着换位。
|
||||
assert vec[layout.source_index()] == pytest.approx(0.050)
|
||||
shuffled = [GN01_ORDER[3]] + [j for j in GN01_ORDER if j != GN01_ORDER[3]]
|
||||
other = layout_from_parts(
|
||||
shuffled,
|
||||
parse_mimics([format_mimic_param(*t) for t in GN01_MIMICS]),
|
||||
URDF,
|
||||
)
|
||||
assert other.command_vector(50.0)[0] == pytest.approx(vec[3])
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 毫米 ↔ 关节值
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestApertureConversion:
|
||||
def test_round_trip(self):
|
||||
layout = _gn01_layout()
|
||||
for mm in (0.0, 7.5, 20.0, 100.0):
|
||||
joint = layout.aperture_to_source(mm)
|
||||
assert layout.source_to_aperture(joint) == pytest.approx(mm)
|
||||
|
||||
def test_clamped_at_both_ends(self):
|
||||
layout = _gn01_layout()
|
||||
assert layout.aperture_to_source(-5.0) == pytest.approx(0.0)
|
||||
assert layout.aperture_to_source(250.0) == pytest.approx(0.100)
|
||||
assert layout.is_clamped(-5.0)
|
||||
assert layout.is_clamped(250.0)
|
||||
assert not layout.is_clamped(50.0)
|
||||
|
||||
def test_mm_is_not_confused_with_m(self):
|
||||
"""100 mm 必须是 0.100 m，不是 100 m。差 1000 倍是这里唯一的量纲。"""
|
||||
layout = _gn01_layout()
|
||||
assert layout.aperture_to_source(100.0) == pytest.approx(0.100)
|
||||
assert layout.source_to_aperture(0.100) == pytest.approx(100.0)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 行程口径提示
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestStrokeNote:
|
||||
def test_reports_the_real_difference(self):
|
||||
"""仿真 100 mm vs 真机 20 mm 必须**说出来**，不能默认一致。"""
|
||||
note = _gn01_layout().stroke_note(20.0)
|
||||
assert "不一致" in note
|
||||
assert "100.0" in note and "20.0" in note
|
||||
|
||||
def test_says_so_when_equal(self):
|
||||
note = _gn01_layout().stroke_note(100.0)
|
||||
assert "一致" in note and "不一致" not in note
|
||||
|
||||
def test_handles_missing_real_stroke(self):
|
||||
assert "无法对比" in _gn01_layout().stroke_note(None)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 夹住判据
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestIsGrasping:
|
||||
"""夹住判据走**毫米开口**口径（不是主关节值）。
|
||||
|
||||
为什么强调口径：开口↔关节值在 FK 口径下是非线性的。用米当口径的话，
|
||||
"2 mm 的容差"在不同开度下含义不同 —— 标定出来的阈值换台夹爪就废了。
|
||||
"""
|
||||
|
||||
def test_blocked_at_lower_is_grasping(self):
|
||||
"""指令到合拢位，实际开口停在 20 mm → 夹住了东西。"""
|
||||
assert is_grasping(0.0, 20.0)
|
||||
|
||||
def test_reached_lower_is_not_grasping(self):
|
||||
assert not is_grasping(0.0, 0.0)
|
||||
|
||||
def test_far_from_lower_is_not_grasping(self):
|
||||
"""目标是全开（100 mm）—— 实际只走了 80 mm 只是还没走到，不是夹住。"""
|
||||
assert not is_grasping(100.0, 80.0)
|
||||
|
||||
def test_within_tolerance_is_not_grasping(self):
|
||||
"""开口差在容差内不算夹住（否则控制精度差一点就误报夹住）。"""
|
||||
assert not is_grasping(0.0, DEFAULT_GRASP_TOLERANCE_MM * 0.5)
|
||||
|
||||
def test_respects_custom_tolerance(self):
|
||||
assert not is_grasping(0.0, 3.0, tolerance_mm=5.0)
|
||||
assert is_grasping(0.0, 3.0, tolerance_mm=1.0)
|
||||
|
||||
def test_realistic_case(self):
|
||||
"""抓 20 mm 通止规：指令合拢，实际停在 20 mm 附近 → 夹住。"""
|
||||
assert is_grasping(0.0, 20.0)
|
||||
assert not is_grasping(0.0, 1.0) # 只是没走到
|
||||
assert not is_grasping(50.0, 20.0) # 目标是 50 mm，谈不上"夹"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 开口反馈源：为什么不能取主关节
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestFeedbackJoint:
|
||||
"""锁住一个**实测**出来的坑，不是设想。
|
||||
|
||||
2026-09-24 实测（Gazebo Fortress + gz_ros2_control）：把
|
||||
``finger_width_joint`` 命令到 0.100，它的 /joint_states 值恒为 ~0
|
||||
（1.4e-08），而 6 个从动关节准确到位（0.7854 = 9.404×0.100-0.155）。
|
||||
|
||||
原因是上游描述里它挂在一个孤立的 ``unused_finger_width_link`` 上
|
||||
（那个 link 的注释写着 "cannot have 0 mass"，纯粹为了 URDF 合法），
|
||||
和手指没有机械连接，Gazebo 里推不动也测不着。
|
||||
|
||||
拿它当反馈的后果是**静默失效**：夹爪明明开着，status 永远报"开口 0 mm"。
|
||||
"""
|
||||
|
||||
def test_feedback_is_not_the_source_joint(self):
|
||||
layout = _gn01_layout()
|
||||
assert layout.feedback != layout.source
|
||||
|
||||
def test_picks_the_direct_follower_with_biggest_gain(self):
|
||||
"""与主关节直接相连的只有 left_outer_knuckle（增益 9.404），选它。"""
|
||||
assert _gn01_layout().feedback == "sim_left_outer_knuckle_joint"
|
||||
|
||||
def test_feedback_value_is_the_driven_command(self):
|
||||
"""反馈关节的目标值就是命令向量里发给它的那个值。"""
|
||||
layout = _gn01_layout()
|
||||
source = layout.aperture_to_source(30.0)
|
||||
assert layout.feedback_value(source) == pytest.approx(
|
||||
layout.value_for(layout.feedback, source)
|
||||
)
|
||||
|
||||
def test_aperture_can_be_recovered_from_the_follower(self):
|
||||
"""从从动关节反推开口，和直接换算必须一致（否则开口读数会整体偏）。"""
|
||||
layout = _gn01_layout()
|
||||
for mm in (0.0, 8.0, 33.0, 100.0):
|
||||
source = layout.aperture_to_source(mm)
|
||||
follower = layout.feedback_value(source)
|
||||
assert layout.aperture_from_feedback(follower) == pytest.approx(mm)
|
||||
|
||||
def test_source_from_feedback_inverts_the_relation(self):
|
||||
layout = _gn01_layout()
|
||||
rel = layout.relations[layout.feedback]
|
||||
assert layout.source_from_feedback(rel.apply(0.037)) == pytest.approx(0.037)
|
||||
|
||||
def test_moving_detection_uses_the_follower_not_the_source(self):
|
||||
"""端到端复现那个坑：用主关节值（恒 0）判"在动"会永远说"在动"。
|
||||
|
||||
生产路径在适配器里：开口 = aperture_from_feedback(反馈关节状态)，
|
||||
再和 target 的开口比。这里把那条链路走一遍。
|
||||
"""
|
||||
layout = _gn01_layout()
|
||||
target = layout.aperture_to_source(100.0) # 全开
|
||||
target_mm = layout.source_to_aperture(target)
|
||||
# 主关节实测恒为 0 → 若拿它当反馈，开口会被算成 0 mm，与 100 mm 差一大截
|
||||
assert abs(layout.source_to_aperture(0.0) - target_mm) > DEFAULT_GRASP_TOLERANCE_MM
|
||||
# 用反馈关节：已经在全开位，开口差为 0
|
||||
follower = layout.feedback_value(target)
|
||||
assert abs(layout.aperture_from_feedback(follower) - target_mm) == pytest.approx(0.0)
|
||||
|
||||
def test_single_joint_gripper_falls_back_to_the_source(self):
|
||||
"""没有耦合关系的单关节夹爪：反馈只能是主关节自己。"""
|
||||
layout = layout_from_parts(
|
||||
["sim_only_joint"], {}, URDF.replace("sim_finger_width_joint", "sim_only_joint")
|
||||
)
|
||||
assert layout.feedback == layout.source
|
||||
assert layout.feedback_value(0.02) == pytest.approx(0.02)
|
||||
assert layout.aperture_from_feedback(0.02) == pytest.approx(20.0)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 口径：线性近似 vs 正运动学
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestApertureCurveFallback:
|
||||
"""测试用的最小 URDF 里没有指尖 link，所以走**线性近似**这条路。
|
||||
|
||||
这不是缺陷而是刻意的：真实 GN01 的 FK 曲线与 Gazebo 的对照表在
|
||||
test_sim_gripper_kin.py 里；这里只保证"没有 FK 时行为不变、且口径
|
||||
在日志里说明白了"。
|
||||
"""
|
||||
|
||||
def test_no_tip_links_means_linear_fallback(self):
|
||||
layout = _gn01_layout()
|
||||
assert layout.curve is None
|
||||
assert layout.closed_separation_mm is None
|
||||
assert "线性近似" in layout.kin_note
|
||||
# 线性口径下行程 = 限位区间
|
||||
assert layout.stroke_mm == pytest.approx(100.0)
|
||||
assert layout.aperture_to_source(20.0) == pytest.approx(0.020)
|
||||
|
||||
def test_describe_mentions_the_口径(self):
|
||||
assert "线性近似" in _gn01_layout().describe()
|
||||
@@ -0,0 +1,288 @@
|
||||
"""sim_gripper_kin 的单测：URDF 正运动学 vs Gazebo 实测（黄金表）。
|
||||
|
||||
=============================================================================
|
||||
这个文件里最值钱的东西是 GOLDEN 那张表
|
||||
=============================================================================
|
||||
它是 2026-09-24 在 Gazebo Fortress + gz_ros2_control 上**实测**出来的：
|
||||
启动仿真、用 GripperExecute 依次开合、再查 /joint_states 与 TF 量两指间距。
|
||||
|
||||
命令开口(主关节值) 请求开口 Gazebo 实测指尖间距
|
||||
0.000 m 0 mm 27.09 mm
|
||||
0.008 m 8 mm 35.76 mm
|
||||
0.020 m 20 mm 48.84 mm
|
||||
0.050 m 50 mm 80.96 mm
|
||||
0.100 m 100 mm 127.02 mm
|
||||
|
||||
本文件用**自己的矩阵乘法**算同一批数字，要求吻合到 0.05 mm。
|
||||
两者完全独立（一个是物理引擎 + sdformat 转换，一个是我们的 FK），
|
||||
所以这个对照同时钉住了三件事：
|
||||
|
||||
1. URDF → SDF 的转换没有丢掉手指链上的偏移；
|
||||
2. 我们对耦合公式（含 9.404/-0.155 那条链）的理解与 Gazebo 实际执行一致；
|
||||
3. 开口↔关节值必须走反解而不是线性换算 —— 表里 20 mm 那行差 1.75 mm，
|
||||
线性口径会让"抓 20 mm 工件"在仿真里抓空。
|
||||
|
||||
以后改耦合表、换 Gazebo 版本、换夹爪描述后，如果这张表开始红，
|
||||
说明仿真里的手指几何真的变了 —— 那时必须重新实测并更新表，
|
||||
而不是把容差放大。
|
||||
"""
|
||||
import os
|
||||
import sys
|
||||
|
||||
import pytest
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
|
||||
from arm_sim_gazebo.sim_gripper_kin import ( # noqa: E402
|
||||
ApertureCurve,
|
||||
KinematicsError,
|
||||
UrdfTree,
|
||||
find_tip_pair,
|
||||
)
|
||||
|
||||
# (主关节值 m, 请求开口 mm, Gazebo 实测指尖间距 mm)
|
||||
GOLDEN = [
|
||||
(0.000, 0, 27.09),
|
||||
(0.008, 8, 35.76),
|
||||
(0.020, 20, 48.84),
|
||||
(0.050, 50, 80.96),
|
||||
(0.100, 100, 127.02),
|
||||
]
|
||||
|
||||
#: 与 Gazebo 的允许差（毫米）。实测最高吻合到 0.01 mm，留 0.05 的余量。
|
||||
TOL_MM = 0.05
|
||||
|
||||
# GN01 的耦合链（与 flexiv_description 的 grav_macro.xacro 一致）
|
||||
MIMICS = {
|
||||
"sim_left_outer_knuckle_joint": ("sim_finger_width_joint", 9.404, -0.155),
|
||||
"sim_left_inner_knuckle_joint": ("sim_left_outer_knuckle_joint", 1.0, 0.0),
|
||||
"sim_left_inner_finger_joint": ("sim_left_outer_knuckle_joint", -1.0, 0.0),
|
||||
"sim_right_outer_knuckle_joint": ("sim_left_outer_knuckle_joint", 1.0, 0.0),
|
||||
"sim_right_inner_knuckle_joint": ("sim_left_outer_knuckle_joint", 1.0, 0.0),
|
||||
"sim_right_inner_finger_joint": ("sim_left_outer_knuckle_joint", -1.0, 0.0),
|
||||
}
|
||||
|
||||
ORDER = [
|
||||
"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",
|
||||
]
|
||||
|
||||
# 一个"够像 GN01"的最小 URDF：只保留手指链上的 offset，
|
||||
# 数字全部取自归一化后的真实描述（/tmp/norm.urdf 同源）。
|
||||
FINGER_URDF = """<?xml version="1.0"?>
|
||||
<robot name="gn01_finger_chain">
|
||||
<!-- 下面 6 条 joint 是**从真实归一化描述里逐字抄下来**的手指链
|
||||
（devices/sim 的 GN01 夹爪，flexiv_description 的 grav 宏）。
|
||||
|
||||
为什么抄而不是自己写一个"像 GN01"的：这一版测试最初是手写几何，
|
||||
结果和 Gazebo 实测差了 78 mm —— 手写时把 knuckle 的安装偏移
|
||||
(0 -0.0325 0.0825) 和右侧那条 rpy="0 0 π" 的镜像都猜错了。
|
||||
黄金表要能证明"我们的 FK == 物理引擎"，夹具就必须是真几何。 -->
|
||||
<link name="sim_grav_base_link"/>
|
||||
<link name="sim_left_outer_bar"/>
|
||||
<link name="sim_left_finger_mount"/>
|
||||
<link name="sim_left_finger_tip"/>
|
||||
<link name="sim_right_outer_bar"/>
|
||||
<link name="sim_right_finger_mount"/>
|
||||
<link name="sim_right_finger_tip"/>
|
||||
<joint name="sim_left_outer_knuckle_joint" type="revolute"><origin rpy="0 0 0" xyz="0 -0.0325 0.0825" /><parent link="sim_grav_base_link" /><child link="sim_left_outer_bar" /><axis xyz="1 0 0" /><limit effort="80" lower="-0.155" upper="0.7854" velocity="2.0" /></joint>
|
||||
<joint name="sim_left_inner_finger_joint" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.058" /><parent link="sim_left_outer_bar" /><child link="sim_left_finger_mount" /><axis xyz="1 0 0" /><limit effort="80" lower="-0.7854" upper="0.155" velocity="2.0" /></joint>
|
||||
<joint name="sim_left_inner_finger_pad_joint" type="fixed"><origin rpy="0 0 0" xyz="0 0.010 0.0235" /><parent link="sim_left_finger_mount" /><child link="sim_left_finger_tip" /><axis xyz="0 0 1" /></joint>
|
||||
<joint name="sim_right_outer_knuckle_joint" type="revolute"><origin rpy="0 0 3.141592653589793" xyz="0 0.0325 0.0825" /><parent link="sim_grav_base_link" /><child link="sim_right_outer_bar" /><axis xyz="1 0 0" /><limit effort="80" lower="-0.155" upper="0.7854" velocity="2.0" /></joint>
|
||||
<joint name="sim_right_inner_finger_joint" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.058" /><parent link="sim_right_outer_bar" /><child link="sim_right_finger_mount" /><axis xyz="1 0 0" /><limit effort="80" lower="-0.7854" upper="0.155" velocity="2.0" /></joint>
|
||||
<joint name="sim_right_inner_finger_pad_joint" type="fixed"><origin rpy="0 0 0" xyz="0 0.010 0.0235" /><parent link="sim_right_finger_mount" /><child link="sim_right_finger_tip" /><axis xyz="0 0 1" /></joint>
|
||||
<joint name="sim_finger_width_joint" type="revolute">
|
||||
<parent link="sim_grav_base_link"/><child link="sim_unused_finger_width_link"/>
|
||||
<axis xyz="1 0 0"/><limit lower="0" upper="0.100" effort="80" velocity="0.2"/>
|
||||
</joint>
|
||||
<link name="sim_unused_finger_width_link"/>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
def _angles_of(main_value: float):
|
||||
"""按耦合链算每个关节的角度（GN01 那条链）。"""
|
||||
|
||||
def value_for(joint: str):
|
||||
if joint == "sim_finger_width_joint":
|
||||
return main_value
|
||||
src, mul, off = MIMICS[joint]
|
||||
return mul * value_for(src) + off
|
||||
|
||||
return {j: value_for(j) for j in ORDER}
|
||||
|
||||
|
||||
def _curve(angles_of=_angles_of, xml: str = FINGER_URDF, upper: float = 0.100):
|
||||
"""建一条开口曲线。``angles_of`` 可替换，用来做"耦合表写错"的负例。"""
|
||||
tree = UrdfTree(xml)
|
||||
tips = find_tip_pair(tree)
|
||||
assert tips is not None
|
||||
return ApertureCurve(tree, tips, angles_of, lower=0.0, upper=upper)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 树结构
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestUrdfTree:
|
||||
def test_common_ancestor(self):
|
||||
tree = UrdfTree(FINGER_URDF)
|
||||
assert tree.common_ancestor("sim_left_finger_tip", "sim_right_finger_tip") == \
|
||||
"sim_grav_base_link"
|
||||
|
||||
def test_chain_order_is_root_first(self):
|
||||
tree = UrdfTree(FINGER_URDF)
|
||||
names = [j.name for j in tree.chain("sim_left_finger_tip", "sim_grav_base_link")]
|
||||
assert names == [
|
||||
"sim_left_outer_knuckle_joint",
|
||||
"sim_left_inner_finger_joint",
|
||||
"sim_left_inner_finger_pad_joint",
|
||||
]
|
||||
|
||||
def test_duplicate_child_is_not_a_tree(self):
|
||||
bad = FINGER_URDF.replace(
|
||||
'<child link="sim_left_finger_tip" />',
|
||||
'<child link="sim_left_finger_mount" />',
|
||||
1,
|
||||
)
|
||||
with pytest.raises(KinematicsError):
|
||||
UrdfTree(bad)
|
||||
|
||||
def test_zero_axis_is_rejected(self):
|
||||
"""axis 为零向量 → 无法确定转动轴，必须报错而不是算出 NaN。"""
|
||||
bad = FINGER_URDF.replace(
|
||||
'<axis xyz="1 0 0" /><limit effort="80" lower="-0.155"',
|
||||
'<axis xyz="0 0 0" /><limit effort="80" lower="-0.155"',
|
||||
1,
|
||||
)
|
||||
tree = UrdfTree(bad)
|
||||
with pytest.raises(KinematicsError):
|
||||
tree.position("sim_left_finger_tip", "sim_grav_base_link", {})
|
||||
|
||||
|
||||
class TestFindTipPair:
|
||||
def test_finds_left_and_right(self):
|
||||
assert find_tip_pair(UrdfTree(FINGER_URDF)) == (
|
||||
"sim_left_finger_tip",
|
||||
"sim_right_finger_tip",
|
||||
)
|
||||
|
||||
def test_returns_none_when_convention_not_met(self):
|
||||
xml = FINGER_URDF.replace("sim_left_finger_tip", "sim_a").replace(
|
||||
"sim_right_finger_tip", "sim_b"
|
||||
)
|
||||
assert find_tip_pair(UrdfTree(xml)) is None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 黄金表：我们的 FK vs Gazebo 的物理
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestGoldenAgainstGazebo:
|
||||
def test_separation_matches_gazebo_everywhere(self):
|
||||
curve = _curve()
|
||||
for source, _request, gazebo_mm in GOLDEN:
|
||||
mine = curve.aperture_mm(source) + curve.closed_separation_mm
|
||||
assert abs(mine - gazebo_mm) < TOL_MM, (
|
||||
f"主关节 {source} m：FK 算得 {mine:.3f} mm，"
|
||||
f"Gazebo 实测 {gazebo_mm:.2f} mm"
|
||||
)
|
||||
|
||||
def test_closed_separation(self):
|
||||
"""合拢时两指尖间距 27.09 mm（指垫此时贴在一起，所以它不是开口）。"""
|
||||
assert _curve().closed_separation_mm == pytest.approx(27.09, abs=TOL_MM)
|
||||
|
||||
def test_stroke_is_measured_not_nominal(self):
|
||||
"""实测行程 99.93 mm，不是名义的 100.0 —— 差在非线性上，不是误差。"""
|
||||
assert _curve().stroke_mm == pytest.approx(99.93, abs=0.05)
|
||||
|
||||
def test_linear_mapping_would_be_wrong(self):
|
||||
"""把"线性口径错在哪"钉成断言，免得以后有人又把它改回去。
|
||||
|
||||
线性口径下 20 mm 请求 → 主关节 0.020 → 实测间距 48.84 mm，
|
||||
即开口 21.75 mm，比请求多 1.75 mm。
|
||||
"""
|
||||
curve = _curve()
|
||||
linear_aperture = (0.020 - 0.0) * 1000.0 # 线性口径认为它是 20.00
|
||||
real = curve.aperture_mm(0.020) # 实际是 21.75
|
||||
assert linear_aperture == pytest.approx(20.0)
|
||||
assert real == pytest.approx(21.75, abs=TOL_MM)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 反解
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestApertureInversion:
|
||||
def test_round_trip(self):
|
||||
curve = _curve()
|
||||
for mm in (0.0, 0.5, 8.0, 20.0, 33.3, 50.0, 99.0):
|
||||
joint = curve.joint_value(mm)
|
||||
assert curve.aperture_mm(joint) == pytest.approx(mm, abs=1e-6)
|
||||
|
||||
def test_inverted_values_are_smaller_than_linear(self):
|
||||
"""中段反解出来的关节值必须**小于**线性值（非线性开口更快）。"""
|
||||
curve = _curve()
|
||||
for mm in (8.0, 20.0, 50.0):
|
||||
assert curve.joint_value(mm) < mm / 1000.0
|
||||
|
||||
def test_clamps_at_both_ends(self):
|
||||
curve = _curve()
|
||||
assert curve.joint_value(-5.0) == pytest.approx(0.0)
|
||||
assert curve.aperture_mm(curve.joint_value(-5.0)) == pytest.approx(0.0)
|
||||
assert curve.joint_value(500.0) == pytest.approx(0.100)
|
||||
assert curve.is_clamped(-5.0) and curve.is_clamped(500.0)
|
||||
assert not curve.is_clamped(50.0)
|
||||
|
||||
def test_grasping_a_20mm_part_actually_touches(self):
|
||||
"""这条是"为什么要做 FK 反解"的验收：抓 20 mm 工件时指垫真的贴住。
|
||||
|
||||
判据：命令开口 20 mm → 反解出的关节值 → 正运动学算回的开口 == 20 mm。
|
||||
线性口径下这一步会是 21.75 mm（差 1.75 mm，等于没碰到工件）。
|
||||
"""
|
||||
curve = _curve()
|
||||
joint = curve.joint_value(20.0)
|
||||
assert curve.aperture_mm(joint) == pytest.approx(20.0, abs=1e-6)
|
||||
# 顺带把"线性口径会差多少"写在这里，读代码的人一眼能看到动机
|
||||
assert curve.aperture_mm(0.020) - 20.0 > 1.5
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 老实报错：不单调 / 行程为 0
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestRefusesBadCoupling:
|
||||
def test_wrong_coupling_direction_is_rejected(self):
|
||||
"""耦合表方向写错 → 指尖不再相向运动 → 必须报错。
|
||||
|
||||
做法：把右侧 knuckle 的符号翻掉（镜像丢了）。实测这种情况下两指
|
||||
**同步**摆动，间距恒定在 45.00 mm —— 关节都在动、`/joint_states`
|
||||
一切正常，只有"开口"这个量不动。
|
||||
|
||||
⚠️ 这一条正是"静默失效"的典型：开口曲线水平时，
|
||||
``joint_value()`` 的二分会一路收敛到下限，"开 20 mm"变成"不动"，
|
||||
而且没有任何报错。所以曲线构造期就必须拒绝它 —— 判据不是
|
||||
``stroke > 0``（这个例子的浮点差是 4e-13，能骗过 ``> 0``），
|
||||
而是"行程至少占名义行程的 1%"。
|
||||
"""
|
||||
|
||||
def wrong(main_value: float):
|
||||
left_knuckle = 9.404 * main_value - 0.155
|
||||
return {
|
||||
"sim_finger_width_joint": main_value,
|
||||
"sim_left_outer_knuckle_joint": left_knuckle,
|
||||
"sim_left_inner_finger_joint": -left_knuckle,
|
||||
"sim_right_outer_knuckle_joint": -left_knuckle, # ← 少了镜像
|
||||
"sim_right_inner_finger_joint": left_knuckle,
|
||||
}
|
||||
|
||||
with pytest.raises(KinematicsError):
|
||||
_curve(angles_of=wrong)
|
||||
|
||||
def test_zero_stroke_is_rejected(self):
|
||||
"""主关节取任何值都不动（耦合表空了）→ 报错，而不是"所有开口都是 0"。"""
|
||||
tree = UrdfTree(FINGER_URDF)
|
||||
tips = find_tip_pair(tree)
|
||||
assert tips is not None
|
||||
with pytest.raises(KinematicsError):
|
||||
ApertureCurve(tree, tips, lambda _v: {}, 0.0, 0.100)
|
||||
Reference in New Issue
Block a user