986 lines
37 KiB
Markdown
986 lines
37 KiB
Markdown
# 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`节点。
|
||
|
||
这些边界应保留在数据报告、实验结论和对外精度声明中。
|