diff --git a/docs/FFG多手势标定映射与遥操技术实现.md b/docs/FFG多手势标定映射与遥操技术实现.md new file mode 100644 index 0000000..4c07d15 --- /dev/null +++ b/docs/FFG多手势标定映射与遥操技术实现.md @@ -0,0 +1,985 @@ +# 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` | +| 四指屈伸 | 每指的 `_root`、`_tip` | +| 四指侧摆 | 每指的 `_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连续命令 `//cb_left_hand_control_cmd`; +- SDK状态 `//cb_left_hand_state`; +- GUI保存快照 `//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`节点。 + +这些边界应保留在数据报告、实验结论和对外精度声明中。 diff --git a/src/gui_control/gui_control/gui_control.py b/src/gui_control/gui_control/gui_control.py index 320ee73..bc24ce1 100644 --- a/src/gui_control/gui_control/gui_control.py +++ b/src/gui_control/gui_control/gui_control.py @@ -38,7 +38,7 @@ _CANONICAL_COMMAND_NAMES = { _CANONICAL_COMMAND_BOUNDS = { "G20": [ *[(0, 255)] * 10, - (80, 255), + (0, 255), *[(255, 255)] * 4, *[(0, 255)] * 5, ], diff --git a/src/linkerhand_retarget/resource/linkerforce_v2/README_zh.md b/src/linkerhand_retarget/resource/linkerforce_v2/README_zh.md index 30a9254..bbe20b1 100644 --- a/src/linkerhand_retarget/resource/linkerforce_v2/README_zh.md +++ b/src/linkerhand_retarget/resource/linkerforce_v2/README_zh.md @@ -174,8 +174,8 @@ CLI将GUI保存动作和状态选择共同视为人工确认,并立即写入 提示当前姿势,不会退出。程序被关闭或异常中断后,使用完全相同的命令 会校验型号、seed哈希、SN、固件、CAN、操作者和输出路径,并自动跳过已保存姿势。 若旧检查点中的某些命令超出profile安全范围,CLI会保留其他有效姿势,只移除并 -重拍超限姿势。G20 GUI同时把 `thumb_cmc_yaw` 限制在80~255,并将4个保留通道 -固定为255;不得为了复现超限手势而放宽这些安全范围。 +重拍超限姿势。G20的 `thumb_cmc_yaw` 使用完整0~255命令范围,4个保留通道仍固定 +为255;不得修改保留通道的固定值。 只有明确希望放弃旧进度时才在原命令末尾增加 `--fresh`;该选项会覆盖旧检查点, 从第一个姿势重新开始。 diff --git a/src/linkerhand_retarget/resource/linkerforce_v2/profiles/g20_seed_profile.json b/src/linkerhand_retarget/resource/linkerforce_v2/profiles/g20_seed_profile.json index 2a187d0..96076c3 100644 --- a/src/linkerhand_retarget/resource/linkerforce_v2/profiles/g20_seed_profile.json +++ b/src/linkerhand_retarget/resource/linkerforce_v2/profiles/g20_seed_profile.json @@ -93,7 +93,7 @@ "command_bounds": [ [0, 255], [0, 255], [0, 255], [0, 255], [0, 255], [0, 255], [0, 255], [0, 255], [0, 255], [0, 255], - [80, 255], [255, 255], [255, 255], [255, 255], [255, 255], + [0, 255], [255, 255], [255, 255], [255, 255], [255, 255], [0, 255], [0, 255], [0, 255], [0, 255], [0, 255] ], "reserved_channels": { diff --git a/src/linkerhand_retarget/test/test_linkerforce_v2.py b/src/linkerhand_retarget/test/test_linkerforce_v2.py index f3e3354..bf5739d 100644 --- a/src/linkerhand_retarget/test/test_linkerforce_v2.py +++ b/src/linkerhand_retarget/test/test_linkerforce_v2.py @@ -248,6 +248,24 @@ def test_seed_profiles_map_named_bounded_commands(model, length): assert result.command[11:15] == (255, 255, 255, 255) +def test_g20_thumb_yaw_uses_full_command_range(): + profile = load_robot_profile( + PROFILE_DIR / "g20_seed_profile.json", "G20" + ) + assert profile["command_bounds"][10] == [0.0, 255.0] + yaw_channel = next( + channel + for channel in profile["channels"] + if channel["name"] == "thumb_cmc_yaw" + ) + yaw_channel["points"] = [[0.0, 255.0], [1.0, 0.0]] + intent = {name: 0.0 for name in BASE_INTENT_NAMES} + intent["thumb_rotate"] = 1.0 + result = RobotMapper(profile).map(intent) + assert result.raw_command[10] == 0.0 + assert result.command[10] == 0 + + def test_paired_pinch_anchors_only_constrain_thumb_and_target_finger(): glove_profile = build_glove_profile( _glove_captures(), @@ -429,9 +447,7 @@ def test_thumb_mapping_ignores_four_finger_only_pose_commands(): altered_profile = copy.deepcopy(robot_profile) for pose_name in ("fingers_together", "tabletop", "hook"): for command_index in (0, 5, 10, 15): - altered_profile["poses"][pose_name]["cmd_u8"][command_index] = ( - 80.0 if command_index == 10 else 0.0 - ) + altered_profile["poses"][pose_name]["cmd_u8"][command_index] = 0.0 extractor = HandIntentExtractor(glove_profile) reference_mapper = RobotMapper(robot_profile, glove_profile) @@ -629,9 +645,9 @@ def test_robot_pose_checkpoint_resumes_and_rejects_identity_mismatch(tmp_path): def test_robot_capture_rejects_commands_outside_profile_safety_bounds(): seed = load_robot_profile(PROFILE_DIR / "g20_seed_profile.json", "G20") command = list(seed["poses"]["open_spread"]["cmd_u8"]) - command[10] = 79.0 + command[10] = -1.0 assert _command_bound_violations(seed, command) == ( - "thumb_cmc_yaw=79,允许[80, 255]", + "thumb_cmc_yaw=-1,允许[0, 255]", )