# FFG多手势标定映射与遥操操作说明

## 1. 文档目的

本文档说明当前 `linkerforce_v2` 无Marker方案的工作原理、标定流程和实机操作方法。
当前主要使用场景是一只左手FFG控制左手G20，也支持在配置对应profile后同时生成O6目标。

当前方案属于 `provisional` 阶段：

- 可以验证手套语义、机械手通道、方向、动作范围和连续性；
- 可以用于演示和临时数采；
- 不能把G20/O6的 `0～255` 电机命令当作真实关节角；
- 没有Marker、编码器或独立角度传感器时，不能给出实机与仿真的真实角度误差。

旧入口 `handretarget` 仍然保留；本文档只描述新入口 `ffg_dual_retarget`。

## 2. 当前映射架构

```text
FFG左手套21维原始数据
        ↓
hand_intent：模型无关的人手语义（0～1）
        ↓
        ├─→ G20 actuation_target
        │      ├─→ G20单机profile → 20维cmd_u8 → G20实机
        │      └─→ G20名义URDF范围 → q_nominal → 仿真
        │
        └─→ O6 actuation_target
               ├─→ O6单机profile → 6维cmd_u8 → O6实机
               └─→ O6名义URDF范围 → q_nominal → 仿真
```

实机命令和仿真目标是两条独立标尺：

- `cmd_u8`：设备电机空间命令，范围为0～255；
- `q_nominal`：根据URDF名义限位生成的仿真弧度目标；
- `state_u8`：SDK返回的设备状态，只用于运行诊断，不是真实关节角GT。

仿真不应直接把 `cmd_u8` 当作真实角度。需要接近实机外观时，可以使用同一
`actuation_target`，再通过实测角度标定完善仿真标尺。

## 3. 多手势标定解决什么问题

### 3.1 FFG静态姿势

完整手套标定采集11个静态姿势：

1. 五指自然张开、自然分开；
2. 五指伸直并拢；
3. 桌面手势：四指根部弯曲、末端伸直；
4. 钩拳：四指根部伸直、末端弯曲；
5. 自然握拳；
6. 拇指最大外展；
7. 拇指横跨掌心；
8. 拇指—食指捏合；
9. 拇指—中指捏合；
10. 拇指—无名指捏合；
11. 拇指—小指捏合。

每个静态姿势默认采集2秒、重复3次，每次至少50帧。profile保留全部原始帧、
每次中位数、MAD和有效帧数，而不是只保存一个平均值。

### 3.2 FFG动态轨迹

完整标定还采集7组短时往返轨迹：

- 食指独立弯曲往返；
- 中指独立弯曲往返；
- 无名指独立弯曲往返；
- 小指独立弯曲往返；
- 拇指弯曲往返；
- 拇指对掌往返；
- 四指开合往返。

动态轨迹主要用于发现和补偿同一手指屈伸对侧摆语义的传感器串扰，并检查非目标
手指是否跟随运动。它们不是额外的离散手势模板。

### 3.3 连续映射原则

当前运行时不会把整只手吸附到“最相似的标定手势”：

- 每根普通手指只读取自身的根部、末端和侧摆语义；
- 张手、桌面、钩拳和握拳构成四指根部—末端标定面，连续解耦传感器串扰；
- 普通屈伸映射保持连续，不在曲线内部加入停止平台；
- 四种捏合分别进行竞争判断；
- 捏合只局部修正拇指和目标手指，不替换整只手命令；
- 捏合证据不明确时，连续退回普通逐关节映射；
- 握拳只增加必要的拇指协调，不把相似动作强制变成握拳模板。

### 3.4 当前实时执行策略

当前默认执行节奏与旧版左手G20的有效路径一致：

```text
publish_rate=30Hz
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
repeat_position_commands=true
```

也就是直接使用最新手套帧，将连续电机目标交给G20固件插值，并在每个30Hz控制心跳
重复发送最新目标。待发送队列深度为1，来不及发送时只保留最新目标，不重放旧命令。

Kalman和EMA仍然可以显式启用，但不要同时启用两层滤波。两层滤波叠加后再进行整数
取整，容易表现为小幅运动停顿、累计后跳变。

## 4. profile与运行文件

| 文件 | 内容 | 是否提交Git |
|---|---|---|
| `glove_<ID>_left_<operator>.json` | 个人佩戴下的FFG完整标定 | 否 |
| `*.wear_check.json` | 绑定手套profile哈希的快速佩戴检查 | 否 |
| `hand_G20_left_<SN>_provisional.json` | 指定G20实机的姿势命令profile | 否 |
| `*.checkpoint.json` | 实机姿势标定中断续标检查点 | 否 |
| `*.verification.json` | 实机姿势人工复核报告 | 否 |
| `g20_seed_profile.json` | G20 GUI安全初值和标定结构 | 是 |
| `o6_seed_profile.json` | O6 GUI安全初值和标定结构 | 是 |

根目录 `profiles/` 已加入 `.gitignore`。可复现的种子profile位于：

```text
src/linkerhand_retarget/resource/linkerforce_v2/profiles/
```

手套profile与操作者、手套和佩戴方式相关；机械手profile与型号、序列号、固件和CAN
接口相关。换手套、换操作者、明显改变佩戴位置时，应重新完整标定FFG。换机械手本体
或固件导致电机响应变化时，应重新标定机械手profile。

## 5. 构建与环境准备

在每个新终端中都要加载ROS和工作空间：

```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash
```

源码修改后重新构建：

```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash

colcon build --symlink-install --packages-select \
  linker_hand_ros2_sdk gui_control linkerhand_retarget

source install/setup.bash
```

如果出现 `Package 'linkerhand_retarget' not found`，通常是当前终端没有执行上述两个
`source`，或者源码修改后还没有构建。

## 6. 完整标定FFG左手套

### 6.1 启动FFG原始数据发布

终端A启动只读FFG节点。只连接左手套即可，不要求右手套存在：

```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash

ros2 run linkerhand_retarget ffg_dual_retarget --ros-args \
  -p serial_port:=/dev/ttyUSB0 \
  -p baudrate:=2000000 \
  -p auto_scan:=true
```

确认原始话题有数据：

```bash
ros2 topic hz /ffg/left/raw_joint_state
```

如果标定提示“0个有效帧”，不要继续重复按Enter。先确认：

- 终端A仍在运行；
- 日志显示左手FFG已连接；
- `/dev/ttyUSB0`没有被另一个FFG进程占用；
- `/ffg/left/raw_joint_state`有稳定数据。

### 6.2 执行完整手套标定

终端B执行：

```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash

ros2 run linkerhand_retarget ffg_calibrate -- \
  --glove-id FFG_LEFT_SN \
  --operator lxp \
  --firmware 2.1.4 \
  --output /home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp.json
```

静态姿势的正确操作：

1. 先摆好终端提示的固定姿势；
2. 姿势稳定后按Enter；
3. 按Enter后继续保持不动约2秒；
4. 终端显示保存帧数后再放松；
5. 同一姿势按相同方法重复3次。

动态往返轨迹的正确操作：

1. 先回到该动作的自然起始位置；
2. 按Enter后立即开始连续、缓慢地往返运动；
3. 在默认3秒采集窗口内完成若干次完整往返；
4. 非目标手指尽量保持稳定；
5. 不要先弯好后全程静止，否则采不到动态关系。

标定成功应显示：

```text
approved_for_runtime=True
sha256=<手套profile哈希>
```

不要使用 `--skip-dynamic` 生成正式运行profile。该参数只适合调试。

## 7. 快速佩戴检查

每次正式实机启动前，对当前准备使用的手套profile执行快速检查：

```bash
ros2 run linkerhand_retarget ffg_calibrate -- \
  --quick-check /home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp.json \
  --quick-output /home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp.wear_check.json
```

依次检查张手、握拳和食指捏合。每个姿势也是“先摆好，再按Enter，然后保持2秒”。

快速检查凭据：

- 默认12小时有效；
- 必须显示 `passed=true`；
- 必须与启动时使用的手套profile SHA-256完全一致；
- 切换v4、v5等手套profile时，必须同时切换到对应的wear-check文件。

快速检查失败时，先重新调整手套佩戴位置并重试。如果多次失败，说明当前佩戴与完整
标定差异过大，应重新完整标定，不要通过增大阈值静默放行实机。

## 8. 标定G20实机姿势

### 8.1 安全要求

- 标定时使用低速、低力矩；
- 配备软件急停，并保证机械手周围无障碍物；
- 不得使用旧手套映射把机械手带到标定姿势；
- 不得在带电状态强行手掰；
- 使用GUI逐通道调整，并确认通道方向正确。

### 8.2 启动G20 SDK

终端A：

```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash

ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \
  -p hand_type:=left \
  -p hand_joint:=G20 \
  -p is_touch:=false \
  -p can:=can0 \
  -p modbus:=None \
  -p topic_prefix:=/g20 \
  -p startup_speed:=30 \
  -p startup_torque:=80 \
  -p move_on_startup:=false \
  -p state_poll_rate:=10.0
```

### 8.3 启动G20标定GUI

终端B：

```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash

ros2 run gui_control gui_control --ros-args \
  -r __node:=g20_calibration_gui \
  -p hand_type:=left \
  -p hand_joint:=G20 \
  -p topic_prefix:=/g20
```

### 8.4 捕获11个G20姿势

终端C：

```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash

ros2 run linkerhand_retarget hand_pose_capture -- \
  --model G20 \
  --seed /home/lxp/projects/linkerhand_retarget_ros2/install/linkerhand_retarget/share/linkerhand_retarget/linkerforce_v2/profiles/g20_seed_profile.json \
  --serial-number G20_LEFT_001 \
  --operator lxp \
  --firmware unknown \
  --can-interface can0 \
  --output /home/lxp/projects/linkerhand_retarget_ros2/profiles/hand_G20_left_G20_LEFT_001_provisional.json
```

每个姿势的操作：

1. 用GUI低速调整机械手；
2. 目视确认目标手指、通道方向和最终姿势；
3. 等待实机稳定；
4. 点击GUI“保存当前标定姿势”；
5. CLI收到快照后选择姿势状态。

状态含义：

- `exact`：机械手能够准确实现该姿势；
- `approximate`：受机构自由度限制，只能实现最佳近似；
- `unsupported`：该型号不能可靠实现，不用于对应姿势约束；
- 输入 `r`：放弃刚才的快照，重新调整和保存。

CLI每完成一个姿势都会立即写入 `*.checkpoint.json`。程序中断后，使用完全相同的
命令会自动恢复并跳过已保存姿势，不需要从头开始。

只有确实要放弃原进度时才增加：

```text
--fresh
```

全部姿势完成后，只有输入 `y` 批准，最终profile才会包含：

```text
approved_for_control=True
```

O6操作相同，但使用 `--model O6`、O6 seed、`can1`和 `/o6` 命名空间。O6自由度较少，
桌面、钩拳及部分捏合通常应标为 `approximate`。

## 9. 复核机械手profile

姿势复核可以避免手工把JSON中的20维命令复制到GUI。

复核前：

- 停止 `ffg_dual_retarget`；
- 停止GUI，避免命令话题存在其他发布者；
- 只保留低速、低力矩的G20 SDK。

执行：

```bash
ros2 run linkerhand_retarget hand_pose_verify -- \
  --profile /home/lxp/projects/linkerhand_retarget_ros2/profiles/hand_G20_left_G20_LEFT_001_provisional.json \
  --operator lxp \
  --topic-prefix /g20
```

只复核单个姿势：

```bash
ros2 run linkerhand_retarget hand_pose_verify -- \
  --profile /home/lxp/projects/linkerhand_retarget_ros2/profiles/hand_G20_left_G20_LEFT_001_provisional.json \
  --operator lxp \
  --topic-prefix /g20 \
  --pose pinch_index
```

按照提示输入 `VERIFY`、`MOVE`，再选择：

- `p`：目视通过；
- `f`：目视未通过；
- `r`：重放；
- `s`：跳过。

工具会低速平滑过渡，并保存独立的 `*.verification.json`，不会修改原始机械手profile。

## 10. 离线检查映射质量

不连接实机即可回放profile中的静态与动态数据：

```bash
ros2 run linkerhand_retarget retarget_profile_check -- \
  --glove /home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp.json \
  --robot /home/lxp/projects/linkerhand_retarget_ros2/profiles/hand_G20_left_G20_LEFT_001_provisional.json \
  --model G20 \
  --output /home/lxp/projects/linkerhand_retarget_ros2/profiles/g20_mapping_quality.json
```

重点查看：

- `passed`和`hard_failures`；
- 静态有效通道复现误差；
- 四种捏合的winner和gate；
- 四指动态轨迹的非目标通道跨度；
- 小指、侧摆等动作是否有明显串扰；
- 帧间命令变化是否存在异常突跳。

离线检查通过不等于实机角度准确，只表示profile内部逻辑一致。

## 11. 启动G20正式遥操

正式启动前，停止旧SDK、标定GUI、姿势捕获工具和占用FFG串口的只读节点。每种节点
只保留一个实例。

### 11.1 终端A：启动G20驱动

```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash

ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \
  -p hand_type:=left \
  -p hand_joint:=G20 \
  -p is_touch:=false \
  -p can:=can0 \
  -p modbus:=None \
  -p topic_prefix:=/g20 \
  -p startup_speed:=255 \
  -p startup_torque:=80 \
  -p move_on_startup:=false \
  -p state_poll_rate:=10.0 \
  -p velocity_poll_rate:=10.0 \
  -p defer_state_reads_while_commanding:=true \
  -p repeat_position_commands:=true
```

`startup_speed`控制电机最大运动速度；`startup_torque`控制最大输出力矩。提高力矩不会
解决映射卡顿。建议先使用80，在确有负载需要并完成安全评估后再提高。

确认状态话题已有发布者：

```bash
ros2 topic info /g20/cb_left_hand_state
```

应至少显示：

```text
Publisher count: 1
```

### 11.2 终端B：启动FFG映射节点

以下示例使用当前v4手套profile：

```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash

ros2 run linkerhand_retarget ffg_dual_retarget --ros-args \
  -p serial_port:=/dev/ttyUSB0 \
  -p baudrate:=2000000 \
  -p auto_scan:=true \
  -p publish_rate:=30.0 \
  -p input_filter_enabled:=false \
  -p command_filter_mode:=passthrough \
  -p command_filter_ema_alpha:=1.0 \
  -p command_filter_max_step_u8:=255.0 \
  -p command_filter_deadband_u8:=0.0 \
  -p glove_profile:=/home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp_v4.json \
  -p wear_check:=/home/lxp/projects/linkerhand_retarget_ros2/profiles/glove_FFG_LEFT_SN_left_lxp_v4.wear_check.json \
  -p g20_profile:=/home/lxp/projects/linkerhand_retarget_ros2/profiles/hand_G20_left_G20_LEFT_001_provisional.json \
  -p g20_serial_number:=G20_LEFT_001 \
  -p g20_can_interface:=can0
```

正常日志应包含：

```text
FFG profile已加载
三姿势快速佩戴检查有效
G20 profile已加载(可申请实机使能)
G20映射=factorized_paired_v2
执行滤波=passthrough alpha=1.0, max_step=255.0
左手FFG已连接
```

节点启动后默认处于PREVIEW，不会立即控制实机。

### 11.3 PREVIEW检查

在使能前观察：

```text
/ffg/left/raw_joint_state
/ffg/left/filtered_joint_state
/retarget/left/hand_intent
/retarget/g20/left/actuation_target
/retarget/g20/left/joint_target_nominal
/retarget/g20/left/cmd_u8_preview
```

检查要求：

- 张手、半握、握拳过程中命令连续；
- 弯曲一根手指时，主要变化的是对应手指通道；
- 食指捏合主要影响拇指与食指；
- 不应锁定在某个历史捏合模板；
- 不应出现明显越限或突跳。

### 11.4 使能G20

终端C：

```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash

ros2 service call /ffg_dual_retarget/enable_g20 \
  std_srvs/srv/SetBool "{data: true}"
```

成功返回：

```text
success=True
message='G20已使能'
```

停用G20：

```bash
ros2 service call /ffg_dual_retarget/enable_g20 \
  std_srvs/srv/SetBool "{data: false}"
```

软件急停：

```bash
ros2 service call /ffg_dual_retarget/emergency_stop \
  std_srvs/srv/Trigger "{}"
```

急停、FFG断开、驱动状态超时或profile错误后，都需要排除问题并重新显式使能。

## 12. ROS话题说明

| 话题 | 说明 |
|---|---|
| `/ffg/left/raw_joint_state` | 21维FFG原始数据 |
| `/ffg/left/filtered_joint_state` | 实际送入语义提取器的数据；默认与raw相同 |
| `/retarget/left/hand_intent` | 模型无关的0～1人手语义 |
| `/retarget/g20/left/actuation_target` | G20归一化目标 |
| `/retarget/g20/left/joint_target_nominal` | G20仿真名义弧度目标 |
| `/retarget/g20/left/cmd_u8_preview` | 未使能时也持续发布的20维预览命令 |
| `/g20/cb_left_hand_control_cmd` | 使能后发送给G20 SDK的20维命令 |
| `/g20/cb_left_hand_state` | G20 SDK状态心跳 |
| `/ffg_dual_retarget/status` | profile、使能、超时、滤波和延迟诊断 |

所有向量都带名称。仿真桥和其他消费者必须按 `JointState.name` 匹配，不得依赖裸下标。

## 13. 安全与自动停用

- 默认PREVIEW，必须通过服务显式使能；
- FFG超过0.35秒没有新帧：撤销全部实机使能；
- 对应驱动状态超过1秒未更新：只撤销该型号；
- profile缺失、未批准、SN/CAN不匹配：拒绝实机使能；
- wear-check缺失、失败、过期或哈希不匹配：拒绝实机使能；
- 所有命令检查长度、名称、有限值和0～255范围；
- G20四个保留通道保持安全固定值；
- 带电机械手不得强行手掰。

## 14. 常见问题排查

### 14.1 `Package 'linkerhand_retarget' not found`

```bash
source /opt/ros/jazzy/setup.bash
source /home/lxp/projects/linkerhand_retarget_ros2/install/setup.bash
```

如果仍然找不到，重新执行第5节的构建命令。

### 14.2 手套标定只有0个有效帧

原因通常是没有单独启动FFG原始数据发布节点。检查：

```bash
ros2 topic info /ffg/left/raw_joint_state
ros2 topic hz /ffg/left/raw_joint_state
```

### 14.3 快速佩戴检查失败

- 确认使用的是正确版本profile；
- 调整手套位置、腕带和手指传感器；
- 每个姿势先摆好再按Enter；
- 按Enter后保持不动2秒；
- 多次失败则重新完整标定。

### 14.4 `三姿势快速佩戴检查缺失、失败或过期`

重新对启动时使用的同一个手套profile执行第7节命令。不能拿v5的wear-check启动v4。

### 14.5 `G20驱动状态无效或已超时`

先执行：

```bash
ros2 topic info /g20/cb_left_hand_state
```

如果 `Publisher count: 0`，说明G20 SDK未启动或没有使用 `/g20` 命名空间。按第11.1节
启动驱动。如果有发布者，再检查：

```bash
ros2 topic echo /g20/cb_left_hand_state --once
```

状态必须是20维、名称与G20 profile一致、数值有限且位于0～255。

### 14.6 服务一直显示 `waiting for service`

检查：

```bash
ros2 node list
ros2 service list | grep ffg_dual_retarget
```

常见原因是 `ffg_dual_retarget`没有启动、当前终端未source，或者服务名称中多写了反斜杠。

### 14.7 服务成功但机械手不动

检查命令话题：

```bash
ros2 topic info /g20/cb_left_hand_control_cmd
ros2 topic hz /g20/cb_left_hand_control_cmd
```

使能后应同时存在发布者和订阅者，并接近30Hz。还要检查SDK终端是否报告CAN错误。

### 14.8 机械手运动卡顿

确认运行参数：

```bash
ros2 param get /ffg_dual_retarget input_filter_enabled
ros2 param get /ffg_dual_retarget command_filter_mode
ros2 param get /linker_hand_sdk repeat_position_commands
```

当前推荐结果：

```text
False
passthrough
True
```

同时检查：

- 只运行一个G20 SDK和一个映射节点；
- 命令话题稳定接近30Hz；
- CAN状态查询在遥操期间已延后；
- 不要用提高力矩解决卡顿；
- 如果只有某一根手指异常，运行离线profile质量检查，重点看该手指动态轨迹。

### 14.9 某根手指张手不到位或发生串指

依次比较：

```text
raw_joint_state
→ hand_intent
→ cmd_u8_preview
→ cb_left_hand_state
```

- raw异常：佩戴或FFG采集问题；
- hand_intent异常：手套标定/语义解耦问题；
- intent正确但preview异常：映射/profile问题；
- preview正确但实机异常：机械手profile、驱动、固件或机构问题。

不要直接通过修改某个写死系数掩盖问题。

### 14.10 实机姿势标定中断

使用完全相同的 `hand_pose_capture` 命令重新启动，会自动读取检查点并跳过已保存姿势。
不要增加 `--fresh`，除非明确要删除当前标定进度并从头开始。

## 15. 当前精度边界与后续优化

当前多手势方案可以继续优化：

- 重采质量较差的小指、侧摆或拇指动态轨迹；
- 改善21维传感器到人体语义的连续解耦；
- 完善拇指对掌和四种捏合的局部连续映射；
- 为每台机械手建立方向相关、非线性的电机命令曲线；
- 记录输入、映射、发布、CAN和状态时间戳，量化延迟与丢帧。

要得到可量化的真实角度精度，仍需增加Marker、编码器或独立角度传感器，建立：

```text
hand_intent
    → 实机真实关节角q_target
    → 单机关节角—cmd_u8标定
```

在此之前，验收结论只能是动作语义、通道、连续性和外观接近程度，不能声明实机与仿真
达到某个真实关节角误差。

## 16. 正式运行前检查清单

- [ ] ROS与工作空间已source；
- [ ] 当前只有一个FFG读取进程；
- [ ] 当前只有一个G20 SDK，使用`can0`和`/g20`；
- [ ] FFG profile显示`approved_for_runtime=True`；
- [ ] wear-check通过、未过期且哈希匹配；
- [ ] G20 profile显示`approved_for_control=True`；
- [ ] profile中的SN和CAN与启动参数一致；
- [ ] `/g20/cb_left_hand_state`有有效发布者；
- [ ] PREVIEW下逐指、握拳和四种捏合动作正确；
- [ ] 默认实时参数为关闭输入滤波、直通命令、30Hz重复目标；
- [ ] 周围安全、急停可用；
- [ ] 最后才调用`enable_g20`。