admin/linkerhand_retarget_ros2

Fork 0

Files

T

admin 0d92e5f998 大拇指安全限制去除

2026-07-27 17:40:57 +08:00

37 KiB

Raw Blame History

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

1. 文档定位

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

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

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：

文件	职责
`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，双手机型启动文件为 ffg_dual_g20_o6.launch.py，可提交的机械手种子profile位于 resource/linkerforce_v2/profiles。

3. 总体架构

                         ┌──────────────────────────────┐
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仿真弧度目标

架构的关键是分成三层：

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

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

4. 数据契约

4.1 FFG 21维原始数据

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

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：

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 鲁棒采样统计

每次采集保留：

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

对第 j 维：

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 基础语义特征拟合

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

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

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

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

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) 平面中取得四个锚点：

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

建立双线性标定面：

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 动态屈伸对侧摆的串扰补偿

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

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]，运行时执行：

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

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

5.5 捏合证据

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

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

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

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

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

5.6 快速佩戴检查

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

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顺序。每个姿势保存：

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，使用张手、桌面、钩拳和握拳的实机命令拟合。

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

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时，不直接使用抽象规范姿势坐标，而是：

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

姿势选择规则为：

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

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

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

完成构造后，mapping_mode为 factorized_paired_v2。

7.2 基础通道映射

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

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

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

cmd_base[index_k] = piecewise_linear(a_k, paired_points_k)

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

7.3 竞争式局部捏合修正

7.3.1 标定自适应阈值

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

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

对所有非捏合姿势记录：

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

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

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 单赢家连续门控

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

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 局部残差

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

residual = robot_pinch_target - base_command_at_glove_pinch

运行时只把 pinch_gate * residual加到：

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

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

7.4 握持时的拇指协调

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

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

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

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

7.5 安全范围与保留通道

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

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

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

8. 滤波与实时执行

8.1 输入Kalman

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

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的有效执行路径：

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：

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

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

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

9. 独立仿真目标

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

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包含：

{
  "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`	立即撤销两个型号的命令发布权限

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

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

11. 实机使能门控

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

手套profile已加载且 approved_for_runtime=true；
wear-check已通过启动时校验；
机械手profile已加载且 approved_for_control=true；
启动参数中的期望SN非空，并与profile SN完全一致；
profile CAN接口与启动配置一致；
mapper为 factorized_paired_v2；
FFG最近一帧未超过 glove_timeout，默认0.35秒；
对应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哈希不是原文件字节哈希，而是：

排除加载器添加的 _profile_path和 _profile_sha256；
JSON key排序；
使用紧凑分隔符和UTF-8；
计算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后：

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

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

17. 扩展和维护约束

17.1 增加新的机械手型号

至少需要：

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

不要通过复制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，覆盖：

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

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

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节点。

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

37 KiB Raw Blame History Unescape Escape