# FFG多手势标定映射与遥操技术实现

## 1. 文档定位

本文面向 `linkerforce_v2` 的开发、联调和维护人员，说明新版FFG手套映射遥操链路的
软件架构、标定拟合方法、实时映射算法、ROS 2接口、profile约束和安全门控。

实机标定与启动步骤见
[《FFG多手势标定映射与遥操操作说明》](./FFG多手势标定映射与遥操操作说明.md)。
本文不重复完整操作流程，而是回答以下实现问题：

- 21维FFG数据如何变成模型无关的手部语义；
- 张手、桌面、钩拳、握拳四个锚点如何解耦根部和末端屈伸；
- G20和O6如何共用手套语义、同时保持各自的机械执行标尺；
- 捏合与握持为什么不会把整只手锁定到离散模板；
- `cmd_u8`、`actuation_target`、`q_nominal`三类目标有什么区别；
- profile如何生成、校验、配对和追踪；
- 节点在什么条件下允许或撤销实机控制。

当前实现基于：

- profile schema：`schema_version=1`；
- 手套侧：单只左手FFG，21维输入；
- 机械手侧：左手G20和左手O6；
- 映射模式：`factorized_paired_v2`；
- 机械手profile策略：`paired_continuous_v1`；
- 仿真策略：`semantic_urdf_v1`；
- 标定等级：`provisional`，没有真实关节角GT。

## 2. 代码组织

核心实现位于
[`linkerforce_v2`](../src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/)：

| 文件 | 职责 |
|---|---|
| `constants.py` | FFG关节名、手部语义名、静态/动态手势集合和默认参数 |
| `calibrate_glove.py` | 订阅FFG原始话题，交互采集完整标定和快速佩戴检查 |
| `calibrate_robot.py` | 从GUI命令、快照和SDK状态生成G20/O6实机profile |
| `calibration.py` | 鲁棒统计、FFG特征拟合、机械手通道权重和分段曲线拟合 |
| `profiles.py` | profile加载、严格校验、规范化哈希和原子保存 |
| `mapping.py` | 人手语义提取、连续配对映射、捏合/握持修正和命令滤波 |
| `node.py` | ROS 2实时节点、话题、服务、定时循环和安全门控 |
| `safety.py` | 与ROS无关的超时撤权判定 |
| `simulation.py` | 按关节名重排仿真目标并执行限位检查 |
| `quality.py` | 静态、捏合、动态轨迹的离线回放质量检查 |
| `verify_robot_profile.py` | 低速回放机械手标定姿势并生成独立人工复核报告 |
| `session_manifest.py` | 生成provisional数采清单并绑定profile、URDF和设备信息 |

ROS 2入口在
[`setup.py`](../src/linkerhand_retarget/setup.py)，双手机型启动文件为
[`ffg_dual_g20_o6.launch.py`](../src/linkerhand_retarget/launch/ffg_dual_g20_o6.launch.py)，
可提交的机械手种子profile位于
[`resource/linkerforce_v2/profiles`](../src/linkerhand_retarget/resource/linkerforce_v2/profiles/)。

## 3. 总体架构

```text
                         ┌──────────────────────────────┐
FFG串口 / ROS JointState│  21维左手套原始弧度 raw[21] │
                         └──────────────┬───────────────┘
                                        │ 可选逐维Kalman
                                        ▼
                         ┌──────────────────────────────┐
                         │ HandIntentExtractor          │
                         │ 21维 → 22维0～1人手语义     │
                         └──────────────┬───────────────┘
                                        │ 同一hand_intent
                    ┌───────────────────┴───────────────────┐
                    ▼                                       ▼
          ┌───────────────────┐                   ┌───────────────────┐
          │ G20 RobotMapper   │                   │ O6 RobotMapper    │
          │ 16个主动执行语义  │                   │ 6个主动执行语义   │
          └─────────┬─────────┘                   └─────────┬─────────┘
                    │                                       │
          ┌─────────┴─────────┐                   ┌─────────┴─────────┐
          ▼                   ▼                   ▼                   ▼
   20维cmd_u8          16维q_nominal       6维cmd_u8           6维q_nominal
   G20实机电机空间      G20仿真弧度目标      O6实机电机空间       O6仿真弧度目标
```

架构的关键是分成三层：

1. **传感器层**：FFG原始值和佩戴差异由手套profile吸收；
2. **解剖语义层**：`hand_intent`只描述人的手部动作，不依赖G20或O6；
3. **执行器层**：每个机械手profile独立定义语义到本机电机命令和URDF目标的映射。

因此，换机械手通常不需要改手套语义提取器；换手套或操作者也不需要改G20/O6的
通道定义，只需重新建立对应profile并在运行时配对。

## 4. 数据契约

### 4.1 FFG 21维原始数据

原始输入使用 `sensor_msgs/msg/JointState`。名称和规范顺序为：

```text
thumb_0 ... thumb_4
index_0 ... index_3
middle_0 ... middle_3
ring_0 ... ring_3
pinky_0 ... pinky_3
```

串口解析器将设备角度转换为弧度。实时节点只接受长度为21、全部有限的数据。

- 串口模式直接使用上述顺序；
- topic模式允许输入名称顺序不同，但要求名称集合完整且唯一，节点按规范顺序重排；
- 标定工具要求输入消息已经使用规范顺序；
- 不支持右手FFG，也不会因右手套缺失而退出。

普通四指每指4个原始量：第0维主要用于侧摆，第1～3维共同参与根部和末端屈伸拟合。
拇指5个原始量由不同组合共同拟合旋转、外展、对掌、根部屈伸和末端屈伸。

### 4.2 人手语义

`hand_intent`共22维，所有值限制在 `[0, 1]`：

| 类别 | 名称 |
|---|---|
| 拇指 | `thumb_rotate`、`thumb_abduction`、`thumb_opposition`、`thumb_root`、`thumb_tip` |
| 四指屈伸 | 每指的 `<finger>_root`、`<finger>_tip` |
| 四指侧摆 | 每指的 `<finger>_splay` |
| 捏合证据 | `pinch_index`、`pinch_middle`、`pinch_ring`、`pinch_pinky` |
| 整体握持 | `power_grasp` |

这里的0和1是由个人手套标定定义的语义端点，不是机械手角度，也不代表统一的物理角度。

### 4.3 G20命令空间

G20输出完整20维 `cmd_u8`：

| 下标 | 通道 |
|---:|---|
| 0 | `thumb_cmc_pitch` |
| 1～4 | `index/middle/ring/pinky_mcp_pitch` |
| 5 | `thumb_cmc_roll` |
| 6～9 | `index/middle/ring/pinky_mcp_roll` |
| 10 | `thumb_cmc_yaw` |
| 11～14 | `reserved_11`～`reserved_14`，固定为255 |
| 15 | `thumb_mcp` |
| 16～19 | `index/middle/ring/pinky_pip` |

其中16个通道是主动映射通道，4个保留通道不参与映射。新版种子profile中
`thumb_cmc_yaw`使用完整的 `[0, 255]` 命令范围，不再继承旧版的80下限。

### 4.4 O6命令空间

O6输出6维 `cmd_u8`：

```text
thumb_cmc_pitch
thumb_cmc_yaw
index_mcp_pitch
middle_mcp_pitch
ring_mcp_pitch
pinky_mcp_pitch
```

O6没有独立的四指PIP和侧摆执行通道，因此每个普通手指的单一屈伸通道由
`root`和`tip`语义融合得到。

### 4.5 三种输出标尺

| 输出 | 范围/单位 | 含义 |
|---|---|---|
| `actuation_target` | `[0,1]` | 当前型号各主动通道的归一化语义激活量 |
| `cmd_u8_preview` / 实机命令 | `[0,255]` | 设备电机命令空间，包含机械耦合和本机标定 |
| `joint_target_nominal` | rad | 根据语义激活量和URDF名义端点生成的仿真目标 |

`state_u8`是SDK返回的设备状态，仍属于设备空间。它既不是编码器关节角，也不能作为
`q_nominal`或真实物理关节角的GT。

## 5. FFG手套profile的生成

### 5.1 鲁棒采样统计

每次采集保留：

```text
sample_count
median[21]
mad[21]
raw_frames[N][21]
```

对第 `j` 维：

```text
median_j = median(raw[:, j])
MAD_j    = median(abs(raw[:, j] - median_j))
```

每个静态姿势和动态轨迹还保存3次独立重复的上述统计。`approved_for_runtime=true`
要求：

- 11个静态姿势全部存在；
- 7个动态轨迹全部存在；
- 每项恰好3次重复；
- 每次至少50帧；
- 汇总帧数等于3次重复的帧数之和。

因此，CLI虽然允许修改静态 `--repeats`，但不是3次时生成的profile只能用于预览。

### 5.2 基础语义特征拟合

每个语义特征定义一组原始下标和带标签的标定姿势。以某个特征为例：

1. 从语义标签为0的姿势求原始端点 `low`；
2. 从语义标签为1的姿势求原始端点 `high`；
3. 将各标定姿势归一化为：

```text
n_j = clip((raw[index_j] - low_j) / (high_j - low_j), 0, 1)
```

4. 用最小二乘拟合各原始维度对语义标签的贡献；
5. 将负权重截为0，再归一化为权重和1；
6. 运行时计算：

```text
feature = clip(sum(weight_j * n_j), 0, 1)
```

无有效跨度的维度不参与归一化。若拟合后所有权重都接近0，则回退为等权。

普通四指的 `root`和`tip`故意使用同一组3个屈伸传感器，但使用不同姿势标签：

| 姿势 | root目标 | tip目标 |
|---|---:|---:|
| 张手/并拢 | 0 | 0 |
| 桌面 | 1 | 0 |
| 钩拳 | 0 | 1 |
| 握拳 | 1 | 1 |

这一设计先得到两个可能仍有耦合的初始特征，再由下一步二维标定面解耦。

### 5.3 根部—末端双线性解耦

对每个普通手指，在初始 `(root_feature, tip_feature)` 平面中取得四个锚点：

```text
p00 = 张手
p10 = 桌面
p01 = 钩拳
p11 = 握拳
```

建立双线性标定面：

```text
p(u, v) = p00
        + (p10 - p00) * u
        + (p01 - p00) * v
        + (p11 - p10 - p01 + p00) * u * v
```

其中 `u`是解耦后的根部屈伸，`v`是解耦后的末端屈伸。运行时先用线性最小二乘得到
初值，再执行最多5次Newton迭代反解 `(u, v)`，最后限制到 `[0,1]`。

只有标定四边形在四角的Jacobian行列式符号一致，且最小绝对值不小于 `1e-3` 时才启用
该解码器。退化或发生折叠的标定面不会用于反解，此时保留基础特征结果。

### 5.4 动态屈伸对侧摆的串扰补偿

每个普通手指的独立屈伸往返轨迹假设该手指侧摆应基本不变。对每次重复：

```text
x = 0.5 * (root + tip)
y = splay
x, y分别减去各自中位数
coefficient = dot(x, y) / (dot(x, x) + ridge)
```

其中 `ridge = 1e-3 * max(dot(x,x), 1e-6)`。

以下情况拒绝学习该次轨迹：

- 少于10帧、长度错误或存在非有限值；
- 屈伸变化范围小于0.05；
- 侧摆几乎没有变化，无法估计；
- 补偿后残差方差仍大于原方差的80%；
- 三次重复的有效系数方向互相矛盾。

最终系数取各有效重复的中位数并限制到 `[-1,1]`，运行时执行：

```text
splay_corrected = clip(
    splay - coefficient * 0.5 * root
          - coefficient * 0.5 * tip,
    0,
    1
)
```

补偿只发生在人手语义层，不直接学习或修改任何G20/O6电机系数。

### 5.5 捏合证据

每种捏合只使用拇指5维和目标手指4维。标定时保存：

```text
center = 目标捏合姿势中位数
scale  = max(abs(center - open), 6 * pinch_pose_MAD, 0.02)
```

并计算张手到捏合中心的归一化距离 `open_distance`。运行时：

```text
distance = RMS((raw_selected - center) / scale)
pinch_strength = clip(1 - distance / open_distance, 0, 1)
```

这4个值是候选证据，不直接等于4个离散状态；最终是否施加捏合修正还要经过竞争门控。

### 5.6 快速佩戴检查

快速检查重新采集张手、握拳和食指捏合。每个姿势计算：

```text
normalized_error =
    RMS((observed_median - reference_median) / max(6 * MAD, 0.05))
```

默认要求每个误差不大于4.0。凭据保存当前手套profile的规范化SHA-256、检查时间、
阈值、各姿势误差和通过状态。

实时节点只在加载profile时检查凭据：

- `kind=ffg_wear_check`；
- `passed=true`；
- 绑定哈希等于当前手套profile哈希；
- 凭据年龄在配置范围内，默认12小时。

节点不会在长时间运行期间周期性重新读取凭据或重新计算年龄。需要跨时段运行时，应按
作业流程主动重启节点并重新执行佩戴检查。

## 6. 机械手profile的生成

### 6.1 捕获数据

`hand_pose_capture`同时监听：

- GUI连续命令 `/<model>/cb_left_hand_control_cmd`；
- SDK状态 `/<model>/cb_left_hand_state`；
- GUI保存快照 `/<model>/calibration_pose_snapshot`。

消息名称允许任意顺序，但必须与seed中的 `command_names`集合完全一致；保存前统一重排为
profile顺序。每个姿势保存：

```text
cmd_u8
command_names
state_u8
state_names
status = exact | approximate | unsupported
confirmed
captured_at
```

每次人工确认后立即原子写入checkpoint。恢复时会核对seed哈希、型号、输出路径、
SN、固件、CAN、操作者、命令名和姿势列表；身份不一致时拒绝续标。若旧checkpoint中
某个命令超出新的安全范围，只删除该姿势并要求重拍。

最终 `approved_for_control=true` 同时要求：

- 用户在最后明确批准；
- SN、CAN和操作者非空；
- 所有必需姿势均已确认；
- 命令与状态名称完整；
- 所有命令位于profile安全范围。

`unsupported`表示该姿势不参与对应运行时约束，但该姿势记录本身仍需人工确认并保存。

### 6.2 多源执行通道权重

一个机械手主动通道可以融合多个人手语义。对seed中列出的 `fit_sources`，使用
张手、桌面、钩拳和握拳的实机命令拟合。

先以张手和握拳命令归一化该通道：

```text
y_pose = (cmd_pose - cmd_open) / (cmd_fist - cmd_open)
```

设计矩阵来自各姿势的规范语义目标，然后执行最小二乘；负权重截为0并归一化。

- G20大部分主动通道只有一个语义源；
- O6普通手指通道同时使用对应的 `root`和`tip`，权重由实机捕获结果拟合；
- 若张手和握拳命令跨度退化，回退为等权。

### 6.3 分段曲线与单调约束

profile生成时，根据通道语义激活量和各标定姿势的 `cmd_u8`产生曲线点。

- `piecewise`：同一激活量的命令取中位数，然后按激活量排序；
- `monotonic_piecewise`：在上述基础上使用相邻违例合并算法执行等距单调回归；
- 曲线至少需要两个不同的激活量；
- 命令点必须位于 `[0,255]`和该通道 `command_bounds`内。

该profile曲线是一条可独立验证和追踪的型号级基线，也是在运行时手套锚点退化时的
单通道回退曲线。

## 7. 运行时分解式配对映射

### 7.1 配对曲线构造

`RobotMapper`同时收到手套profile和机械手profile时，不直接使用抽象规范姿势坐标，
而是：

1. 用当前手套profile的静态姿势中位数重新计算真实 `hand_intent`；
2. 找出手套和机械手共有且未标为 `unsupported` 的姿势；
3. 对每个机械手主动通道，选择真正定义该解剖通道的姿势；
4. 以当前手套语义激活量为横轴、当前实机profile命令为纵轴重建分段曲线；
5. 对单调通道再次执行单调回归。

姿势选择规则为：

| 通道 | 使用的基础姿势 |
|---|---|
| 普通四指屈伸 | 张手、并拢、桌面、钩拳、握拳中双方共有的姿势 |
| G20普通四指侧摆 | 并拢、张手 |
| 拇指基础通道 | 最大外展、张手、横跨掌心 |

捏合姿势不进入普通通道曲线，握拳也不直接进入拇指基础曲线；它们分别由局部残差分支
处理。这样，某个捏合捕获中的非目标手指残留命令不会污染普通手指曲线。

如果某个通道的实际手套锚点退化为少于两个不同激活量，该通道使用机械手profile中
已经校验的曲线；其他通道仍可保持配对曲线。

完成构造后，`mapping_mode`为 `factorized_paired_v2`。

### 7.2 基础通道映射

对第 `k` 个主动通道，其来源权重为 `w_ki`，当前人手语义为 `h_i`：

```text
a_k = clip(sum(w_ki * h_i) / sum(w_ki), 0, 1)
```

`a_k`组成 `actuation_target`。基础电机命令由该通道配对曲线分段线性插值得到：

```text
cmd_base[index_k] = piecewise_linear(a_k, paired_points_k)
```

完整命令向量先以张手命令初始化，主动通道逐个覆盖；未映射的保留通道之后强制写回
固定值。

### 7.3 竞争式局部捏合修正

#### 7.3.1 标定自适应阈值

映射器先对手套profile中的所有静态姿势计算4种捏合分数。对每个真实捏合姿势记录：

- 目标分数；
- 目标分数相对其他3种分数的领先量。

对所有非捏合姿势记录：

- 最大误触分数；
- 第一名相对第二名的误触领先量。

满激活阈值取4个目标姿势中的最弱值，起始阈值位于最大负样本与满激活阈值之间的20%：

```text
score_onset  = negative_score  + 0.2 * (score_full  - negative_score)
margin_onset = negative_margin + 0.2 * (margin_full - negative_margin)
```

若当前手套profile无法在正负样本间形成有效分数或领先量间隔，所有捏合门均保持0。

#### 7.3.2 单赢家连续门控

运行时仅选择当前分数最高的候选，并计算：

```text
score_gate  = smoothstep((top_score - score_onset) / score_span)
margin_gate = smoothstep((top_score - second_score - margin_onset) / margin_span)
pinch_gate  = score_gate * margin_gate
```

`smoothstep(x)=x²(3-2x)`，输入先限制到 `[0,1]`。其他3种捏合门为0。

因此：

- 证据不足时保持普通连续映射；
- 两种捏合证据接近时，领先量门控将修正降到0；
- 不存在确认帧数、进入/退出滞回或历史姿势锁存；
- 捏合切换只依赖当前帧，且权重连续变化。

#### 7.3.3 局部残差

对每种捏合，在该手套捏合中位数处先计算基础命令，再与机械手目标捏合命令做差：

```text
residual = robot_pinch_target - base_command_at_glove_pinch
```

运行时只把 `pinch_gate * residual`加到：

- 所有拇指主动通道；
- 当前目标手指的主动通道。

其他3根手指不参与该分支。G20的保留通道也不参与。

### 7.4 握持时的拇指协调

`power_grasp`是8个普通四指 `root/tip`语义的平均值。握持分数进一步要求拇指主动折叠：

```text
grasp_score = min(
    power_grasp,
    thumb_opposition,
    thumb_root,
    thumb_tip
)
```

满分取手套握拳姿势，负样本取其他静态姿势的最高分，门控同样使用从负样本到握拳分数
20%处开始的 `smoothstep`。

握持残差是机械手握拳目标与握拳处基础命令的差，但只施加到拇指主动通道。四指仍由
各自连续屈伸曲线决定，普通拇指动作也不会仅因四指弯曲而被强制成握拳拇指。

### 7.5 安全范围与保留通道

局部修正完成后依次执行：

1. 写回保留通道固定值；
2. 按每通道 `command_bounds`裁剪；
3. 执行可选命令滤波；
4. 再次裁剪并再次写回保留通道；
5. 最终四舍五入为整数命令。

`raw_command`保留滤波前浮点目标，当前ROS节点不发布该字段；`cmd_u8_preview`发布滤波后
并取整的最终目标。

## 8. 滤波与实时执行

### 8.1 输入Kalman

输入滤波是21个互相独立的一维Kalman滤波器，共享参数：

```text
P_pred = P + process_variance
K      = P_pred / (P_pred + measurement_variance)
x      = x + K * (z - x)
P      = (1 - K) * P_pred
```

首帧、时间倒退或帧间隔超过 `input_filter_reset_gap` 时直接重置到当前测量，避免断流后
从旧状态缓慢追赶。默认关闭。

### 8.2 输出命令滤波

`CommandFilter`支持：

| 模式 | 行为 |
|---|---|
| `passthrough` | 直接使用本帧目标，仅应用deadband |
| `ema` | `step=clip(alpha*(target-last), ±max_step)` |
| `acceleration_limited` | 同时限制速度、帧间加速度，并根据剩余距离提前制动 |

默认参数匹配旧版左手G20的有效执行路径：

```text
input_filter_enabled=false
command_filter_mode=passthrough
command_filter_ema_alpha=1.0
command_filter_max_step_u8=255
command_filter_deadband_u8=0
```

实时节点没有单独暴露 `command_filter_max_acceleration_u8_per_frame2` 参数；
`acceleration_limited`模式下它使用与 `command_filter_max_step_u8`相同的值。

### 8.3 30 Hz最新帧策略

实时节点的处理定时器默认30 Hz：

1. 串口模式从线程安全快照取得最新序列号、数据和接收时刻；
2. 只有出现新FFG序列时，才发布/更新raw、filtered、intent和frame metadata；
3. 每个定时周期都用最近一次有效intent重新计算两个型号目标；
4. 预览始终发布，只有已使能型号才发布到SDK命令话题；
5. 硬件命令QoS为 `RELIABLE + KEEP_LAST(depth=1)`。

固定控制心跳不会排队重放旧手套帧。FFG停止更新时，节点可在超时窗口内短暂复用最后
intent，随后watchdog撤销使能。

使能某型号时，命令滤波器会重置到该型号最新有效SDK状态，而不是张手或上一次内部
目标，从而降低重新使能的第一帧跳变。

## 9. 独立仿真目标

仿真目标不从 `cmd_u8`反解。对主动通道激活量 `a_k`：

```text
q_nominal_k = clip(
    q_open_k + a_k * (q_closed_k - q_open_k),
    q_lower_k,
    q_upper_k
)
```

其输入是基础解剖通道激活量，不使用电机命令曲线，也不直接使用捏合或握持的电机残差。
因此实机姿势捕获中的机械耦合、偶然残留值和保留通道不会污染仿真弧度目标。

仿真消费者必须按 `JointState.name`建立映射。`simulation.py`提供：

- `build_name_mapping()`：检查空名、重名、缺名和多余名称；
- `reorder_named_target()`：按仿真模型顺序重排，检查有限值并应用仿真限位。

名称合同不满足时抛出 `JointNameMismatch`，不得按裸下标猜测。

profile中的 `urdf_sha256`用于追踪生成名义端点时对应的URDF版本，但实时映射节点本身
不读取或重新计算URDF文件哈希；数采manifest工具会执行文件哈希核对。

## 10. ROS 2实时节点

### 10.1 输入与输出话题

| 话题 | 类型 | 维度 | 发布条件 |
|---|---|---:|---|
| `/ffg/left/raw_joint_state` | `JointState` | 21 | 串口模式收到新帧；topic模式直接使用上游话题 |
| `/ffg/left/filtered_joint_state` | `JointState` | 21 | 有有效手套profile和新帧 |
| `/retarget/left/hand_intent` | `JointState` | 22 | 有有效手套profile和新帧 |
| `/retarget/left/frame_meta` | `String(JSON)` | - | 每个新映射手套帧 |
| `/retarget/g20/left/actuation_target` | `JointState` | 16 | G20 mapper有效 |
| `/retarget/o6/left/actuation_target` | `JointState` | 6 | O6 mapper有效 |
| `/retarget/g20/left/joint_target_nominal` | `JointState` | 16 | G20 mapper有效 |
| `/retarget/o6/left/joint_target_nominal` | `JointState` | 6 | O6 mapper有效 |
| `/retarget/g20/left/cmd_u8_preview` | `JointState` | 20 | G20 mapper有效 |
| `/retarget/o6/left/cmd_u8_preview` | `JointState` | 6 | O6 mapper有效 |
| `/g20/cb_left_hand_control_cmd` | `JointState` | 20 | G20已使能 |
| `/o6/cb_left_hand_control_cmd` | `JointState` | 6 | O6已使能 |
| `/ffg_dual_retarget/status` | `String(JSON)` | - | 1 Hz |

节点订阅：

| 话题 | 说明 |
|---|---|
| `raw_input_topic` | `input_mode=topic`时的FFG输入，默认 `/ffg/left/raw_joint_state` |
| `/g20/cb_left_hand_state` | G20驱动状态心跳 |
| `/o6/cb_left_hand_state` | O6驱动状态心跳 |

topic输入模式不会再次向raw话题发布收到的消息，避免默认同名话题形成反馈。

驱动状态的名称必须已经按profile `command_names`规范顺序排列；这里与FFG topic输入不同，
不会对驱动状态按集合重排。

### 10.2 帧元数据

`frame_meta`包含：

```json
{
  "timestamp_ns": 0,
  "sequence": 0,
  "calibration": "provisional",
  "q_gt": null
}
```

一个新手套帧产生的filtered、intent、frame_meta及该次定时周期的型号目标共用ROS时间戳。
定时器复用旧intent时，型号目标使用新的当前时间戳，但不会重复发布intent和frame_meta。

### 10.3 状态诊断

1 Hz状态JSON包含：

- 当前输入模式、FFG帧序号和数据年龄；
- G20/O6分别是否使能；
- glove、G20、O6的批准状态和SHA-256；
- `mapping_mode`、`simulation_mapping_mode`；
- 输入和命令滤波配置；
- 当前捏合/握持局部分支权重 `anchor_weights`；
- wear-check有效性；
- 驱动状态年龄；
- profile和URDF哈希；
- profile加载错误、最近故障和p95调度延迟。

`latency_p95_ms`以本机接收手套数据的单调时钟为起点，表示接收至映射调度的延迟，
不是基于设备硬件时间戳的端到端链路延迟。

### 10.4 服务与状态转换

| 服务 | 类型 | 作用 |
|---|---|---|
| `~/enable_g20` | `SetBool` | 单独申请/撤销G20实机控制 |
| `~/enable_o6` | `SetBool` | 单独申请/撤销O6实机控制 |
| `~/enable_all` | `SetBool` | 原子检查两台后同时使能，或同时撤销 |
| `~/emergency_stop` | `Trigger` | 立即撤销两个型号的命令发布权限 |

```text
                         显式SetBool(true)且全部检查通过
              ┌──────────────────────────────────────────┐
              │                                          ▼
         PREVIEW / DISABLED                         ENABLED(model)
              ▲                                          │
              └──────────────────────────────────────────┘
                SetBool(false)、超时、映射异常或软件急停
```

撤销使能的含义是停止向SDK命令话题发布新命令，不会主动发送张手、零位或其他安全姿势。
驱动/固件将保持最后命令相关行为；物理急停仍应由系统级安全链路负责。

## 11. 实机使能门控

某个型号从PREVIEW进入ENABLED前依次检查：

1. 手套profile已加载且 `approved_for_runtime=true`；
2. wear-check已通过启动时校验；
3. 机械手profile已加载且 `approved_for_control=true`；
4. 启动参数中的期望SN非空，并与profile SN完全一致；
5. profile CAN接口与启动配置一致；
6. mapper为 `factorized_paired_v2`；
7. FFG最近一帧未超过 `glove_timeout`，默认0.35秒；
8. 对应SDK状态名称、长度、数值和范围有效，且未超过 `driver_timeout`，默认1秒。

运行时身份门控比较SN和CAN接口，不比较profile中的固件版本。固件兼容性目前依赖操作
流程和数采manifest的可选校验；若固件变更会改变电机响应，应重新标定机械手profile。

`enable_all`先检查G20和O6两者，任意一个失败都不会使能任何一个。单型号服务互相独立。

## 12. Watchdog与故障策略

watchdog周期为50 ms：

| 故障 | 动作 |
|---|---|
| 任意型号已使能且FFG超时 | 同时撤销G20和O6 |
| 某型号SDK状态无效或超时 | 只撤销该型号，另一型号保持 |
| 映射计算出现数值/形状错误 | 同时撤销G20和O6 |
| 软件急停 | 同时撤销G20和O6 |
| profile加载失败 | 启动时降级；不创建对应mapper或只保留raw |

故障恢复不会自动重新使能。排除原因后必须再次调用对应使能服务。

节点启动和profile加载采用fail-closed策略：

- 无手套profile：只发布原始FFG；
- 手套profile有效但未获运行批准：允许完整预览，拒绝实机；
- seed机械手profile `approved_for_control=false`：允许预览，拒绝实机；
- 单个型号profile无效：另一个有效型号仍可生成目标和独立使能。

## 13. 主要ROS参数

### 13.1 FFG输入

| 参数 | 默认值 | 说明 |
|---|---|---|
| `input_mode` | `serial` | `serial`或`topic` |
| `raw_input_topic` | `/ffg/left/raw_joint_state` | topic模式输入 |
| `serial_port` | 空 | 指定串口；为空时可自动扫描 |
| `baudrate` | `0` | 大于0时优先尝试该波特率 |
| `baudrates` | `[2000000,460800,1000000,921600]` | 探测候选 |
| `auto_scan` | `true` | 指定端口失败或为空时扫描 |
| `serial_debug` | `false` | 串口调试日志 |

### 13.2 Profile与身份

| 参数 | 默认值 | 说明 |
|---|---|---|
| `glove_profile` | 空 | FFG完整标定profile |
| `wear_check` | 空 | 快速佩戴检查凭据 |
| `wear_check_max_age_hours` | `12.0` | 启动加载时允许的最大年龄 |
| `g20_profile` / `o6_profile` | 空 | 单机机械手profile |
| `g20_serial_number` / `o6_serial_number` | 空 | 运行期望SN |
| `g20_can_interface` | `can0` | G20身份核对 |
| `o6_can_interface` | `can1` | O6身份核对 |

### 13.3 时序和滤波

| 参数 | 默认值 | 说明 |
|---|---:|---|
| `publish_rate` | `30.0` | 固定映射/命令心跳Hz |
| `glove_timeout` | `0.35` | FFG超时秒数 |
| `driver_timeout` | `1.0` | SDK状态超时秒数 |
| `input_filter_enabled` | `false` | 是否启用逐维Kalman |
| `input_filter_process_variance` | `1e-5` | Kalman过程噪声 |
| `input_filter_measurement_variance` | `5e-4` | Kalman测量噪声 |
| `input_filter_reset_gap` | `0.35` | 断流重置阈值 |
| `command_filter_mode` | `passthrough` | 输出滤波模式 |
| `command_filter_ema_alpha` | `1.0` | EMA/限加速度目标增益 |
| `command_filter_max_step_u8` | `255.0` | 每帧最大速度尺度 |
| `command_filter_deadband_u8` | `0.0` | 小于该差值时保持上一目标 |

启动文件还负责创建两个SDK节点，并设置启动速度、力矩、状态轮询和G20控制期间延迟状态
读取等驱动参数；这些不是 `ffg_dual_retarget`自身参数。

## 14. Profile校验与可追踪性

### 14.1 严格加载

`profiles.py`在构造mapper之前检查：

- schema、profile类型、型号和左手侧；
- 固定的FFG关节名或机械手命令名；
- 所有数组长度、有限值和范围；
- 特征权重非负且和为1；
- 分段曲线激活量、命令范围和单调性；
- 主动通道与保留通道完整覆盖命令向量；
- 仿真名称顺序、端点、限位和URDF哈希格式；
- 已批准profile的设备身份、人工确认、状态和名称完整性。

profile错误不会被静默修正为另一种型号或旧映射策略。

### 14.2 规范化哈希

profile哈希不是原文件字节哈希，而是：

1. 排除加载器添加的 `_profile_path`和 `_profile_sha256`；
2. JSON key排序；
3. 使用紧凑分隔符和UTF-8；
4. 计算SHA-256。

因此仅缩进或JSON键顺序变化不会改变profile身份，持久字段变化会改变哈希。

保存使用同目录临时文件加原子替换，避免中途退出留下半个JSON。

### 14.3 相关运行文件

| 文件 | 技术作用 |
|---|---|
| glove profile | 原始帧、鲁棒统计、特征参数和捏合锚点 |
| wear-check | 绑定glove profile哈希的短期佩戴凭据 |
| robot profile | 设备身份、姿势、通道曲线、安全范围和仿真端点 |
| checkpoint | 绑定seed和设备元数据的可恢复捕获进度 |
| verification | 绑定robot profile哈希的独立人工复核结果 |
| session manifest | 绑定profile、wear-check、URDF、设备和rosbag话题 |

`session_manifest.py`当前要求G20和O6都已批准，并按 `can0/can1`核对；它适用于标准双手
型号数采拓扑，不是任意单型号或任意CAN配置的通用manifest生成器。

## 15. 离线质量检查

`retarget_profile_check`不启动ROS、不连接机械手，直接回放profile中的原始帧和姿势。

### 15.1 静态复现

对每个共有姿势，只比较该姿势真正定义的相关通道：

- 捏合：拇指和目标手指；
- 拇指姿势：拇指通道；
- 并拢：侧摆通道；
- 桌面/钩拳：普通四指屈伸通道；
- 握拳：除普通侧摆外的通道。

任一相关通道最大误差大于5个u8单位，记为hard failure。

### 15.2 捏合混淆

四个捏合中位数必须：

- 竞争winner等于目标手指；
- 目标门控不小于0.95。

否则记为hard failure。

### 15.3 动态连续性和局部性

每组动态重复记录：

- 目标通道跨度；
- 非目标通道跨度；
- 原始浮点命令帧间步长p95和最大值；
- 取整后整帧不变比例；
- 应用profile执行滤波后的同类指标。

普通手指屈伸轨迹中，若非目标通道跨度中位数大于
`max(15, 0.2 * target_span)`，生成warning。四指开合轨迹中若任一屈伸语义范围中位数
大于0.5，也生成warning。

动态步长目前只报告统计量，没有统一hard-failure阈值；应结合采样率、动作速度和设备
允许步长分析。

离线通过只证明profile内部复现和分解逻辑满足这些判据，不证明实机物理角度精度。

## 16. 实机姿势复核实现

`hand_pose_verify`加载已批准机械手profile后：

1. 查询命令话题是否已有其他发布者，有则拒绝开始；
2. 要求显式输入安全确认；
3. 从最新SDK状态而不是上一次目标开始；
4. 将目标分成每通道步长不超过 `max_step_u8` 的线性序列；
5. 默认30 Hz发送，运动中周期检查新竞争发布者；
6. 稳定后读取命名状态并计算设备空间绝对误差；
7. 保存人工通过/失败、备注、目标、状态和误差摘要。

默认 `max_step_u8=4`，CLI硬限制不超过8。复核报告明确记录
`state_is_angle_ground_truth=false`，并且不修改原机械手profile。

## 17. 扩展和维护约束

### 17.1 增加新的机械手型号

至少需要：

1. 在 `MODEL_COMMAND_LENGTHS`登记型号和命令长度；
2. 定义唯一、稳定的 `command_names`；
3. 创建seed profile，包括姿势、安全范围、主动通道、保留通道和仿真端点；
4. 明确每个执行通道的解剖语义源；
5. 扩展profile校验器的必需姿势集合；
6. 扩展节点的话题、身份参数、状态和服务；
7. 增加静态复现、局部性、限位和名称合同测试。

不要通过复制G20的裸下标映射来接入新型号；名称、主动通道和保留通道必须显式定义。

### 17.2 增加新的手套语义

需要同步更新：

- `BASE_INTENT_NAMES`或派生语义列表；
- 标定姿势标签和原始下标；
- glove profile生成及严格校验；
- `HandIntentExtractor.extract()`输出顺序；
- 使用该语义的机械手seed和测试；
- rosbag/下游消费者的数据合同。

修改名称或顺序会影响profile兼容性，应升级schema而不是让旧profile静默通过。

### 17.3 修改手势或阈值

捏合和握持阈值由当前手套profile自动推导。优先修复标定数据或距离定义，不要增加隐藏
的全局常量绕过竞争判据。若确需改变门控公式，应同时更新：

- 正/负样本定义；
- 连续性和混淆测试；
- 离线质量报告；
- `mapping_mode`或schema版本，以便数据可追踪。

### 17.4 线程与实时性

- 串口读取在线程中更新带锁快照；
- ROS节点定时器只消费最新快照，不阻塞等待串口；
- 运行时没有无界命令队列；
- 标定和复核CLI可使用后台executor线程，因为它们包含交互式终端等待；
- 映射主要是小向量NumPy运算，不包含在线优化或模型推理。

## 18. 测试与验收建议

核心单元/集成测试集中在
[`test_linkerforce_v2.py`](../src/linkerhand_retarget/test/test_linkerforce_v2.py)，覆盖：

- 鲁棒统计和profile完整性；
- 根部/末端解耦和曲线内部无平台；
- 捏合局部性、连续切换和无历史锁存；
- 普通手指对拇指动作的独立性；
- 动态侧摆串扰补偿；
- 仿真目标与电机残差隔离；
- 滤波步长、加速度、重置和默认直通行为；
- profile身份、checkpoint、安全范围和命名合同；
- 重使能重基准和超时撤权；
- G20/O6输出长度、名称和限位。

修改核心算法后至少执行：

```bash
source /opt/ros/jazzy/setup.bash
source install/setup.bash

python3 -m pytest -q \
  src/linkerhand_retarget/test/test_linkerforce_v2.py
```

完成profile标定后再分别执行G20和O6离线质量回放。涉及ROS接口、SDK命名或launch参数的
修改，还应在PREVIEW状态检查实际话题长度、名称、频率和status JSON，再进入低速实机
验收。

## 19. 已知边界

- 当前只支持左手FFG到左手G20/O6；
- `provisional`不提供真实物理关节角精度声明；
- `cmd_u8`和SDK `state_u8`不能转换成可靠的真实关节弧度；
- 仿真目标只代表语义—URDF名义映射，不是实机测量；
- 捏合竞争无时间滞回，连续性依赖当前帧证据质量和可选输入滤波；
- wear-check只在节点加载时验证，不在长时间运行中自动过期撤权；
- 运行时身份门控不核对固件版本；
- 软件急停只撤销发布权限，不替代硬件急停或独立安全控制器；
- 默认双型号launch会同时创建两个SDK驱动；单型号系统可直接启动所需驱动和
  `ffg_dual_retarget`节点。

这些边界应保留在数据报告、实验结论和对外精度声明中。