linkerhand_retarget_ros2/src/linkerhand_calibration

LinkerHand 专业标定包

O6 右手局部标定（o6_right_8/v1）

O6 使用与 L6 相同的三机位八 Tag 观测拓扑，但保留 O6 自己的六通道协议与 URDF 关节名：正面 ID0/1/2 标定 rh_thumb_cmc_pitch 与 rh_thumb_ip，侧面 ID3/4/5 标定 rh_pinky_mcp_pitch 与 rh_pinky_dip，上面 ID6/7 标定 rh_thumb_cmc_yaw。六路 baseline 均为 255；每次只扫描通道 0、1 或 5，其他通道保持 255。经硬件确认，食指、中指、 无名指与小指同机构，因此小指 MCP/DIP 的实测结果会以迁移来源标记后用于其余 三指。O6 实测 IP/DIP 均存在稳定的非线性，因此使用通过独立 holdout 的双向 运行曲线和二次耦合模型。标准 URDF 无法表示二次 mimic，因此修正 URDF 与 L6 一样保留端点对齐的线性 <mimic>，使普通 URDF/RViz 中五个被动关节能正常联动， 同时把被动关节 limit 更新为实测范围；中间行程的精确双向非线性轨迹由下述标定桥 发布。

ros2 run linkerhand_calibration calibrate_hand --config \
  src/linkerhand_calibration/config/o6_right_product.yaml --validate-only

ros2 run linkerhand_calibration calibrate_hand --config \
  src/linkerhand_calibration/config/o6_right_product.yaml

由于 O6 的 FRONT/TOP 光轴接近正交，同一平面棋盘的同步视角天然更倾斜。O6 外参 允许最终批次 RMS 不超过 1.5 px，但仍保持 0.3° 旋转、1.5 mm 平移稳定性门限， 并在标定发布前额外用 20 mm 跨机位实体轴线 RMS 粗差门限拦截移动相机等明显错误：

ros2 launch linkerhand_calibration three_camera_extrinsics.launch.py \
  output_file:=$PWD/config/o6_three_camera_extrinsics.yaml \
  checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027 \
  maximum_reprojection_rms_px:=1.5

结果发布到 calibration_output/<O6串号>/latest_partial_passed，运行时 JSON、 修正 URDF 与 correction-input JSON 均使用 o6_right_ 前缀。产品 YAML 内的实物 串号、相机身份、外参和四项输入哈希必须在启动硬件前通过校验。

标定完成后，以修正 URDF 启动 robot state publisher，并用同一会话中的 JSON 把 O6 六路反馈转换为 11 个 URDF 关节：

ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
  hand_type:=right \
  calibration_file:=$PWD/calibration_output/O6_RIGHT_001/latest_partial_passed/o6_right_O6_RIGHT_001_partial_calibration.json

L6 右手局部标定（l6_right_8/v1）

本版只发布 rh_thumb_cmc_pitch、rh_thumb_cmc_roll、 rh_pinky_mcp_pitch 的静态零位与动态曲线，以及 rh_thumb_dip、 rh_pinky_dip 的视觉动态曲线。拇指 DIP 通过线性 mimic；小指 DIP 使用实测 双方向运行曲线和 MuJoCo 二次 equality，因为它的传动比会随屈曲角变化。生成的 修正 URDF 保留 rh_pinky_dip 的 <mimic>，因此在普通 URDF/RViz 中仍会跟随 MCP 运动；该线性回退精确对齐实测零位和闭合端点。中间行程的准确非线性轨迹由 MuJoCo equality 或下述标定桥提供。根据 L6 四指同机构的实机确认，食指、中指、 无名指的 MCP 零偏、行程、双向反馈曲线及 DIP 耦合从小指迁移；每指自己的 origin.xyz、axis、mesh 和惯量保持 CAD，不把迁移结果标成 Tag 实测。四指 MCP 以反馈 255 的展开端作为 CAD lower/zero 锚点；实测行程不会再被强制压回较短的 CAD upper，因此不会向四指 origin 写入系统性的负零偏。 结果指针为 latest_partial_passed，不会被当作六路主动关节的完整标定。

拇指 pitch/roll 根据两组已记录六路反馈值的实机/仿真姿态对比，以反馈 255 保持源 CAD joint zero，不叠加端点推断的静态偏置；小指及三指迁移仍以反馈 0 机械闭合姿态对齐源 CAD upper。该策略只改变坐标锚点，不改变视觉实测的双方向 行程曲线。现场姿态对比必须同时记录对应的六路反馈值。

先启动 SDK 和 GUI 做手动检查时使用：

ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \
  -p hand_type:=right -p hand_joint:=L6 -p can:=can0 -p topic_prefix:=/l6

ros2 run gui_control gui_control

正式一键标定由 runner 自己启动 SDK、三相机、AprilTag 和标定节点，不要同时 运行上面的 SDK/GUI 控制命令：

ros2 run linkerhand_calibration calibrate_hand --config \
  src/linkerhand_calibration/config/l6_right_product.yaml

只检查 Profile、8 张 16 mm Tag、相机/外参哈希和只读源 URDF：

ros2 run linkerhand_calibration calibrate_hand --config \
  src/linkerhand_calibration/config/l6_right_product.yaml --validate-only

离线回放与在线发布使用同一拟合/URDF写回路径：

ros2 run linkerhand_calibration calibrate_hand --config \
  src/linkerhand_calibration/config/l6_right_product.yaml \
  --offline-raw calibration_output/L6_RIGHT_001/<时间戳>/raw_samples.jsonl

拟合通过后，程序先原子写入并重新校验 l6_right_<序列号>_urdf_correction_input.json，验证串号、Profile 和源 URDF SHA-256 后，才把其中的原精度零偏、行程和 DIP 耦合参数交给现有 URDF 写回器。 面向运行时的 schema-v6 *_partial_calibration.json 及修正 URDF 的字段、数值和 格式保持兼容；单独的 correction-input JSON 是可审计的 URDF 生成依据，不是运行桥 的输入文件。

运行前将产品 YAML 中的 serial_number 改成实物串号。通道顺序固定为 pitch、 roll、index、middle、ring、pinky；旧 SDK 反馈中的 thumb_cmc_yaw 仅作为第二路 兼容别名读取，新产物和运行桥始终输出 thumb_cmc_roll / rh_* URDF 名。 预检和正式扫描均使用 L6 硬件速度 1 作为上限，并由 100 Hz 余弦缓入缓出 轨迹把完整 255↔0 行程固定为 6 s；短行程按距离同比缩短。SDK 会过滤 L6 在同一 CAN ID 回送的位置命令回显，标定只使用状态查询返回的真实反馈。标定 启动时还会检查重复 SDK/GUI 发布者，避免两个进程同时访问同一只手。

标定完成后，用生成的 JSON 将六路硬件反馈转换成包括小指 DIP 在内的 11 关节 JointState：

ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
  hand_type:=right \
  calibration_file:=$PWD/calibration_output/L6_RIGHT_001/latest_partial_passed/l6_right_L6_RIGHT_001_partial_calibration.json

schema v6 默认订阅 /l6/cb_right_hand_state，发布 /sim/mujoco/l6/right/joint_state。标准 URDF 的 <mimic> 本身只支持线性关系， 所以只查看 URDF 时小指 DIP 中间行程是端点对齐的近似；需要实测轨迹时使用该桥 或修正 URDF 内的 MuJoCo equality。

G20右手正式一键标定

固定三相机和19张Tag安装完成后，用户只运行：

ros2 run linkerhand_calibration calibrate_hand

旧 executable calibrate_g20_right 在本发行版内保留为同一入口的别名； 旧 ROS 包名前缀不再提供。新脚本和部署配置统一使用 calibrate_hand。

构建和正式调用统一为：

colcon build --packages-select linkerhand_calibration
ros2 run linkerhand_calibration calibrate_hand --config <产品配置.yaml>

代码边界

• core/：无 ROS、无具体型号，包含领域类型、PnP/旋转数学、拟合接口、统一样本 契约、TaskEvaluator/SessionSolver 协议和 UrdfCorrectionPlan。其中 core/urdf/patch.py 是所有型号共用的声明式 URDF patch engine，统一负责属性级 文本修改、MuJoCo equality、mesh 安全复制、禁止覆盖和原子发布。
• runtime/：通用会话状态机与注册 Profile 分发；ROS 消息和硬件适配只能位于 runtime/nodes、runtime/adapters。
• models/g20/、models/l6/：只保留型号 Profile、拟合/零位策略以及把拟合结果 转成 UrdfPatchSet 的薄适配层，不再各自实现 XML/mesh 文件写入器。新增 O6 时 优先新增 Profile；只有测量链或传动模型不同的部分才增加小型拟合插件。 后续型号或左右手作为新的独立 Profile 加入 models/，不在通用层增加分支。
• compat/：v1 配置、旧路径、旧会话与旧单相机逻辑。旧 Python 包名仅保留 一版最小转发 shim，不包含算法副本。

产品配置在启动硬件前通过本地 ProfileRegistry 完成命令索引、任务、视角、 Tag、零位目标、URDF关节和文件哈希校验。v1 配置原文不改；v2 配置使用 profile_id: MODEL/side/layout/vREVISION。视角名和数量由 Profile 声明，通用层 不要求 front/side/top，也不假设固定 20 个命令。

仓库中存在已审定硬件会话时，可执行只读金标准检查（不会覆盖任何产物）：

ros2 run linkerhand_calibration validate_g20_goldens \
  calibration_output/G20_RIGHT_001

它严格核对完整整手、合并拇指、独立拇指、拟合失败和零位失败五个会话；完整 整手还会重新离线求解并要求 JSON、URDF 的 SHA-256 与正式产物一致。

完全独立地只标定大拇指4项任务时，使用：

ros2 run linkerhand_calibration calibrate_hand \
  --scope thumb

该模式不读取任何已标定四指数据。拇指零位求解只使用5条拇指轴、两条顶部同相机 方向观测以及拇指自己的机械端点；输出URDF从原始CAD生成，只修改4个拇指主动 关节，12个四指关节保持CAD零位。独立结果发布到latest_thumb_passed，其JSON是 拇指标定/诊断产物，不冒充可直接运行的完整整手曲线JSON。

如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定，才额外使用：

ros2 run linkerhand_calibration calibrate_hand \
  --scope thumb \
  --base-session calibration_output/G20_RIGHT_001/latest_passed

合并模式会冻结基础会话中的12个四指主动零位；发布前再次读取基础会话JSON核对， 任何四指零位变化都会拒绝发布。基础四指数据仍不参与4个拇指零位的数值求解。

拇指专项结果重复性通过后，不必再做原来的16项整手扫描。以该拇指会话为基础， 只重新采集12项四指任务并合成完整整手URDF：

ros2 run linkerhand_calibration calibrate_hand \
  --scope fingers \
  --base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳>

fingers模式严格冻结基础会话中的4个拇指主动零位；最终完整整手URDF中的拇指 零位与专项会话schema-v4数值完全一致。默认full也先调用与thumb完全相同的 独立拇指内核，再冻结这4个结果求解12个四指零位，因此四指数据不能反向改写 拇指结果。原来的默认full仍保留，用于需要16项全部重新采集的情况。

采样文件中的运动域是显式且不可混用的： requested_command_u8 表示下发命令，feedback_u8 表示电机反馈。 在线拟合和离线重放通过同一个数据契约投影到曲线索引；新会话不会把含糊的 command_u8 写入 raw_samples.jsonl。基础会话导入期间状态会显示为 IMPORTING_BASE 和 REVALIDATING_INHERITED，完成复核后才允许机械手运动。

提供--base-session时，它必须解析到同一序列号目录下的完整PASS会话；启动前会校验源CAD URDF、相机外参和标定配置哈希。新会话从原始CAD重新生成完整URDF，不在旧校准 URDF上叠加。两种thumb模式都只重采thumb_cmc_pitch、thumb_cmc_roll、 thumb_cmc_yaw、thumb_mcp/thumb_ip四项物理任务，其余任务的原始记录导入后仍按 数据契约和产物哈希检查，但不会以历史四指拟合结果否决本次拇指专项标定。

开发阶段若上一次会话失败，同一命令会自动校验硬件/几何哈希，并恢复已经 完整提交的关节任务；失败中的当前任务始终丢弃重做，位于它后面但已经完整通过的 独立任务仍会复用，不再因“连续前缀”限制整段重采。导入的任务会立即用与 最终验收相同的硬门限复检（不含视口实时有效率）：只以预警带余量通过的旧数据 当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收 失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记； 恢复时只读取该标记之后的替代采集，不能把同一尝试编号下重扫前后的稳态点合并。 因此已经在线硬门限验收的任务保持已完成，暂停中的任务从任务开头重采，不会因 日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面主测量和侧面校验测量 独立保留；单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面 轴线位置若也能明确定位为单轮异常，同样只补扫该轮；补扫会保留任务预检和前次 采集确定的PnP分支参考，不会因重新初始化切换到另一组平面Tag镜像解。侧面 校验视角的任务级有效率只记录为诊断；G20右手预检若逐帧识别率低于标称值， 但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞，也按完整 轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查，轴线、曲线和模型质量 门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一 采集事务：相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用 产品审定速度，不再根据单次识别密度自动提速，确保不同会话测量的是同一动态 过程。顶部单目thumb_cmc_yaw在最终求解后另做零偏轮次重复性检查：前三轮 极差默认不得超过0.5°，95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的 紧密簇、仅另一轮越界，自动补采该轮两个方向；无法明确定位时只重采完整yaw任务， 不会回退重采整手。与latest_passed中上一正式结果相差超过0.75°时另写入 thumb_yaw_cross_session_diagnostic提示检查机械手位置和Tag安装，但该历史差值 不直接否决当前会话，也不会用旧结果约束新零位。需要强制从第一个关节 重新采集时使用 --no-resume。升级前已经分别完成的正面/侧面roll也会合并为 一个完整同步任务断点；只有两边数据都完整时才复用。

命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、 16个会话数据求解主动关节URDF零位修正、 21条视觉实测命令曲线发布；四指PIP/DIP的动态曲线均实测，四指DIP静态零位保留CAD。 终端只显示中文进度和问题；失败时复制“请复制以下内容给开发者”块即可。

四指末端的16 mm Tag允许使用刚性延长杆避挡；软件不假设末端Tag平面与中节Tag 平面平行。延长杆和Tag在一次标定期间必须完全刚性，不能晃动、扭转或重新调整。 侧面掌部基准Tag（ID 4）与各活动指节Tag也不要求安装面平行：首次联合PnP使用 静态多帧刚性、重投影误差和跨轮任务参考选择分支，不再用固定15°安装角门限阻断扫描。 单Tag独立位姿仍保留75°倾角保护；对包含锁定掌部基准和完整父子链的任务，倾角保护只 限制独立选择，不会在联合选择前删除正深度、低重投影的IPPE候选。联合跟踪继续用相邻帧 绝对/相对位姿连续性约束这些斜视候选，最终轴线残差、四轮重复性和隔离留出门限不放宽。 终端中的Tag计数表示“可见”；等待扫描起点时会另列PnP初始化进度和累计拒绝原因。 若联合候选仍然失败，raw_samples.jsonl 会按8个反馈计数的区间保存 group_pnp_candidate_event，其中包含缺失角色、逐Tag候选数/倾角/重投影、角点和相机内参 哈希，可直接定位运动到哪个机械位置后开始失效，而不需要再次盲扫整条流程。

正式结果位于 calibration_output/G20_RIGHT_001/latest_passed。该指针只在 JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证 全部通过后更新。

G20右手19-Tag底层调试入口

以下内容仅保留给旧会话回放和开发调试；正式一键命令只发布上面的精简 schema v4 JSON，不再生成schema v5运行文件。

新布局用独立参数启用，原有左右手11-Tag流程仍默认使用 tag_layout:=legacy_11，两套配置和结果schema互不覆盖：

ros2 launch linkerhand_calibration three_camera_calibration.launch.py \
  hand_type:=right \
  tag_layout:=g20_right_19 \
  serial_number:=G20_RIGHT_001 \
  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
  source_urdf_expected_sha256:=<CAD负责人确认的G20右手源URDF_SHA256>

当前产品布局共19张tag36h11，所有Tag的黑色码区边长均为16 mm；四指末节ID为 7,14,16,18。正面为0,1,2,3,10,11,12,13，侧面为 4,5,6,7,14,15,16,17,18，上面为8,9。详细角色和逐ID尺寸以 config/three_camera_tags_g20_right_19.yaml为唯一软件配置源。程序执行4项拇指任务， 以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、 侧面PIP/DIP联合任务，共16个物理运动任务。同步roll只驱动电机一次，但两台相机仍分别拟合并通过 各自的观测质量门限。每个PIP任务只驱动一次对应电机，同时用“手掌→中节Tag”实测PIP、 用“中节Tag→末节Tag”实测被动DIP；四个DIP不再由URDF mimic系数生成，并参加完整视觉 拟合和质量门限。每项正式四轮之前自动低速往返预检 0/127/255可见性；

基准形态恢复完成后，程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的 固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0，因此四指正面+侧面同步roll中 允许ID 0暂时不可见，并使用本会话基准锁定值；运动连杆Tag仍必须实时可见，门限不 放宽。终端用锁表示该固定参考有效，例如正面[0锁,12✓]，✗才表示需要处理的 实时Tag。锁定后本次标定运行中不得再移动相机、手掌底座或整只手，否则缓存参考 失效，必须重新启动标定；两次独立会话之间轻微调整整只手的位置不会改变机械端点 零位基准。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计； 任务结束后角色要求会切回预检全套标签，静止期帧不参与该门限，避免把 采集质量良好的任务误判为可见性失败。

恢复末端Tag后，程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次 相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差；实体手在反馈0端 能够触掌是独立的机械端点约束。三个thumb CMC主动轴、thumb_mcp、四指MCP pitch 和PIP的URDF静态零位均由本次实测全行程与CAD机械端点之差求出，不再把跨相机的 平面PnP绝对相位直接当作编码器零位，也不写死为0。生成URDF时同步修正这些关节的 坐标上限以及相关被动关节的mimic坐标偏置， 保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和 第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、thumb_mcp、 四指MCP roll、MCP pitch及PIP，共16个。thumb_mcp与四指屈伸关节一样使用 实测全屈曲行程和CAD机械端点联合求解，不直接采用MCP/IP耦合运动的单目PnP相位。 侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入，并按逆序分阶段退出； 同类辅助电机（全部邻指滚转、全部PIP、全部MCP pitch）合并为同一个并行航点同时 运动，被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP” 等已评审不变量保持不变，过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。 parallel_pose_transitions（默认true）置false可回退旧的逐电机顺序。 同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行，只在任务切换时退出， 不再每轮重复展开/弯曲辅助手指。跨手指组切换时，下一组避障姿态仍然需要、且 当前已经在位（含反馈容差）的辅助电机保持原位，只有下一组不再使用的避障电机 退回基准，避免"先展开回基准、马上又折回"的多余动作；已评审的 "滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺 不超过8，只作为采集能力诊断。G20右手四轮正式速度始终使用产品配置的固定值， 不会因本次预检帧率或识别密度而改变；旧11-Tag布局仍保留自适应速度兼容逻辑。 四指roll不再把同一反馈127误当成方向无关的唯一机械姿态：以255→127为标准物理 零位，反向到达127的实测偏差保留在increasing_rad中。方向分支间隙上限1.5°、 四轮间隙极差上限0.3°；其他关节仍使用严格的0.5°baseline回差门限。 预检、正式四轮和拟合重扫始终使用速度5。 正式roll的每个方向会在经过127时先到位保持0.5秒，再独立保存至少10帧静止Tag/反馈； 方向分支检查和动态曲线的127相位都使用这两组双向静止数据，运动中经过127的帧不再 替代静态保持姿态。 前三轮只用于训练，第四轮完全留出；留出轮不参与显著性、Student-t置信区间或最终重拟合。 每个任务只在低速递减预检起点执行一次8帧PnP静态初始化；预检往返和四轮正式 扫描连续复用同一帧间分支与任务参考，不再让每一轮独立选择平面Tag解。同一任务第1轮 已确立的端点相对姿态作为后3轮的分支锚点，防止独立初始化选到相反的 IPPE镜像解。baseline标准接近和全部质量门限保持不变。 电机15任务会利用源URDF中已确认的thumb_ip mimic=1.03，只在逐帧IPPE双解中 排除与MCP同步运动明显矛盾（残差超过7.5°）的ID3镜像候选。该先验不生成或缩放 thumb_ip曲线；通过分支选择后的ID2→ID3姿态仍独立拟合并接受完整留出验证。

当前19-Tag产品流程发布精简schema v4：21条运行时曲线全部来自当前会话的视觉实测。 URDF零位字段覆盖拇指4个主动关节和四指各自的mcp_roll/mcp_pitch/pip，共16个， 其中三个thumb CMC轴、thumb_mcp及四指mcp_pitch/pip共12个字段由实测旋转行程 与机械端点联合求解； 写出非零thumb_mcp零偏时同步平移其关节坐标上限，并更新被动thumb_ip的 mimic offset，因此不会改变CAD定义的最大屈曲实体姿态； thumb_ip及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容， 当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合；方向、 轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中 相对侧相机视线倾斜约13°~20°，平面标签的单目IPPE姿态二义性会给侧视姿态引入 数度的系统性"绕视线"偏差（亚像素重投影无法发现，会话20260820_105535实测 前后轴向稳定相差11.4°），因此侧视PIP连杆姿态不再参与MCP轴向融合或角曲线验收， 正侧姿态差只写入cross_view_roll_axis_diagnostic。四根MCP侧摆轴在产品URDF中 严格平行：小指作为先采集的参考轴，其余三指复用该公共方向并各自独立拟合轴线位置， 避免平面PnP分支在不同会话中改变轴向。前视侧摆连杆受丝杆平移影响，其纯旋转拟合得到的是 随手指结构变化的伪轴线，不能与侧视PIP连杆的物理轴线使用统一距离门限； 两者线距仅记录在诊断中。侧视校验通道 （*_mcp_roll_side）的 姿态分支间隙跨轮极差和独立姿态轴方向极差只作诊断，不触发重复采集；这两个量来自 近掠射平面Tag的非发布姿态分量。绝对分支间隙1.5°上限保持不变，真正发布的正面主轴 仍使用原跨轮严格门限。侧视逐帧axis_pose_line_rms同样只作诊断，组合轴线改用四轮 位置RMS验收；径向、平面、圆一致性及可见性门限全部保留。 侧面端视roll的圆轨迹方向已经受 姿态轴约束，因此自由三维圆平面与姿态轴的夹角只保留诊断，不再被重复作为硬门限； 径向残差和四轮轴线位置RMS仍是硬门限。正式MCP动态曲线统一使用正面Tag中心的 二维投影圆角度，侧面姿态曲线仅保留为诊断；任一正式视角自身四轮不重复或第四轮 留出失败仍会拒绝发布。任一静态目标、第四轮留出、 遮挡、PnP或跨机位检查失败时，只保留原始轨迹和passed:false诊断，不发布正式URDF。 8个组合姿态仅保留为开发诊断，正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔 位置验收的手基座变换，因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用 a609d521验证过的完整四轮相对旋转曲线；全部实测关节均由隔离第四轮逐关节验收。 现场需要区分某根手指的roll机构回差与单机位误差时，可设置 cross_view_roll_diagnostic_finger:=pinky|ring|middle|index。该会话只执行目标手指的一次 正面+侧面同步roll，共10个预检/正式方向；任一机位数据不足会重扫同一物理任务， 双机位数据齐全后即使存在轴质量失败也不再自动重采，而是把失败项随双机位结果 一起写入cross_view_roll_diagnostic并立即暂停。诊断会话永久 锁定URDF发布，不能用resume转换成正式标定。 schema v5明确声明曲线输入域为真实反馈u8；运行桥默认订阅 /g20/cb_right_hand_state，并按反馈增减方向选择正程/反程曲线，停止时锁存最后运动 方向。尚未观察到运动方向时使用255→127标准分支，不使用两个机械分支的平均值。 schema v4继续兼容旧 命令域。两者都只发布动态角度，不重复叠加已写入URDF的静态偏移。

三机位三维关节轴零位标定（schema v4）

正式入口同时使用三台海康 MV-CS020-10U/10UM 黑白全局快门相机，只有 /g20_calibration 一个节点拥有机械手命令发布权。相机不需要水平，Tag方向也不需要 贴正；相机和Tag在一次标定中必须固定。默认绑定为：

front = DB2163742，Tag 0/1/2/3/10
side  = DB2163749，Tag 4/5/6/7
top   = DB2163739，Tag 8/9

11 张 tag36h11 的程序角色必须与贴纸所在刚性件一致：

ID	机位	固定位置/运动件
0	正面	正面掌壳固定基准
1	正面；右手电机0时也由侧面观测	拇指 CMC 后连杆
2	正面	拇指 MCP 后连杆
3	正面	拇指 IP 后末节
4	侧面	掌壳侧面固定基准（最底下）
5	侧面	左手食指/右手小指 MCP 后连杆
6	侧面	左手食指/右手小指 PIP 后连杆
7	侧面	左手食指/右手小指 DIP 后末节
8	上面	上面相机可见的掌壳/底座固定基准
9	上面	拇指 CMC yaw 运动件
10	正面	左手食指/右手小指根部侧摆运动件

ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且可见， ID 9 必须在拇指横摆的完整行程中持续可见。 贴纸不能跨关节、贴在软胶上或在扫描过程中翘起。

每台相机必须有独立内参文件：

~/.ros/camera_info/hikrobot_DB2163742.yaml
~/.ros/camera_info/hikrobot_DB2163749.yaml
~/.ros/camera_info/hikrobot_DB2163739.yaml

1. 一次性三相机外参

三相机第一次安装、任何相机移动、镜头重新聚焦或内参变化后，必须重标外参。使用 8x5 内角点、实测方格边长 27 mm、粘在硬质平板上的棋盘：

mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
ros2 launch linkerhand_calibration \
  three_camera_extrinsics.launch.py \
  output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
  checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027

启动后默认打开 G20 Three-Camera Extrinsics 交互窗口。可切换 FRONT + SIDE 和 FRONT + TOP；窗口实时显示棋盘角点、单相机/组合 RMS、时间差、联合拟合稳定性和候选/内点数量。单张只要棋盘完整、 同步、RMS和姿态差异合格，ADD CANDIDATE 就会变绿；不再用单张 PnP的最终外参偏差锁死采集。点击 AUTO ON 后，棋盘稳定1秒会自动 采集，移到新姿态后再自动采下一组。

外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中，每改变一次位置和倾角添加 一个候选；随后以相同方法采集正面/上面。程序使用固定内参的 stereoCalibrate 联合优化唯一旋转/平移。采集准入和最终验收分离：FRONT和 配对相机的单帧RMS分别不得超过1.5 px，候选组合RMS不得超过1.5 px；界面中 单相机1.2 px以内显示绿色、1.2～1.5 px显示黄色且仍可采集、超过1.5 px显示红色。 新姿态会与全部已采姿态比较，避免在少数姿态间反复采集。拟合先剔除粗大异常组， 再在不低于15个内点的前提下有界裁剪联合误差最高的候选，只有最终批次组合RMS 不超过1.2 px才允许保存。两组均得到 至少15个内点且联合RMS、三折稳定性合格后 SAVE 才变绿。

ros2 service call /g20_camera_extrinsics/capture_front_side std_srvs/srv/Trigger {}
ros2 service call /g20_camera_extrinsics/capture_front_top std_srvs/srv/Trigger {}
ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}

采集时可分别查看 /g20_extrinsics/{front,side,top}/camera/image_rect。界面始终 显示当前配对的整批RMS、旋转稳定性和平移稳定性；BATCH FAIL 后会直接列出 INLIERS、RMS、ROT 或 TRANS 失败项。保存门限为：联合重投影RMS不超过 1.2 px、三折重拟外参最大旋转差不超过0.3°、最大平移差不超过1.5 mm。文件同时 绑定三台相机序列号、分辨率和内参哈希；不满足任一项时不会保存通过结果，正式 标定也不会运动。

2. 预检和正式标定

先使用禁止运动模式检查三个机位、外参、内参和标签：

ros2 launch linkerhand_calibration \
  three_camera_calibration.launch.py \
  hand_type:=left \
  serial_number:=G20_LEFT_001 \
  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
  commands_enabled:=false

分别查看正式流程的三个画面：

ros2 run image_view image_view --ros-args \
  --remap image:=/g20_calibration/front/camera/image_rect
ros2 run image_view image_view --ros-args \
  --remap image:=/g20_calibration/side/camera/image_rect
ros2 run image_view image_view --ros-args \
  --remap image:=/g20_calibration/top/camera/image_rect

确认全行程安全、MVS客户端已关闭且没有其他命令发布者后，重启正式流程：

ros2 launch linkerhand_calibration \
  three_camera_calibration.launch.py \
  hand_type:=left \
  serial_number:=G20_LEFT_001 \
  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
  can_interface:=can0

原始URDF及其mesh随linkerhand_calibration安装，默认根据hand_type自动选择。 如需调试其他CAD版本，仍可通过source_urdf_path:=<绝对路径>显式覆盖。 右手使用同一入口，并自动选择右手SDK话题和原始URDF：

ros2 launch linkerhand_calibration \
  three_camera_calibration.launch.py \
  hand_type:=right \
  serial_number:=G20_RIGHT_001 \
  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
  can_interface:=can0

状态显示三个机位均“就绪”后只调用一次：

ros2 topic echo /g20_calibration/status_text
ros2 service call /g20_calibration/start std_srvs/srv/Trigger {}

收到 start 后，程序先下发并确认以下20通道基准姿态，稳定保持0.5秒后才开始 第一条轨迹扫描：

[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]

左手依次扫描电机 0/5/15/6/1/16/10，右手依次扫描 0/5/15/9/4/19/10，每项三轮 255→0→255。轨迹角由父/子Tag完整相对 四元数的旋转向量投影到三维拟合轴得到。 轴方向使用相对姿态旋转轴和可信上游轴约束；仅对斜视、非约束关节将中心圆作为独立 交叉检查并参与融合。轴线上一点则由整段 相对SE(3)轨迹的 (I-R)p=t 方程鲁棒拟合，不再把单目Tag中心自由三维圆的圆心直接 当成机械轴心。正面/侧面端视关节只使用图像平面内可观分量，丢弃无法由单目确定的 光轴深度；斜视轨迹仍保留姿态轴和独立三维圆轴的交叉检查。每条主动曲线在其baseline命令 严格归零：普通通道255，四指侧摆127。左手将食指动态轨迹、右手将小指动态轨迹继承 给其余三指。11-Tag布局只能可靠恢复参考指的动态命令—角度曲线，不能证明四根独立 电机的绝对装配相位相同；因此四指全部MCP侧摆、MCP屈伸和PIP静态URDF零偏都保留 原始CAD的0，只继承动态曲线，避免参考指弯曲或四指整体同向倾斜。

零位求解使用行程更充分的根轴方向和保持原始CAD直立的参考指MCP pitch实测轴方向确定 掌部朝向；两条平行根轴线只确定平移，不再用其单目三维深度间距确定绕根轴的旋转，避免 稳定PnP深度偏差被写成拇指roll零偏。另一条短行程根轴方向只作诊断。随后按两条运动链逐关节 进行一维鲁棒求解，避免远端异常把已确定的上游零位一起拖到边界。非平行相邻轴先将上下游 轴投影到父轴法平面，再计算精确有符号方位角；父子轴夹角是零位无法改变的几何不变量，偏差 超过5°直接判定模型失败。平行相邻轴比较两轴之间的径向相位，三维路径忽略连杆长度和沿轴 Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目PnP深度。轴线SE(3)拟合RMS超过 1 mm也不允许写URDF。四指静态零位不参与相机相位覆盖，拇指可观测零偏上限20°； 小于0.3°或未超过3倍不确定度的稳定偏移保留原始零位0。

thumb_mcp 的动态角度曲线仍由电机15的三轮轨迹直接测量，但其绝对静态零位只可通过 被动 thumb_ip 的轴线圆心相位间接推断。固定正面单目机位下这条浅圆弧的姿态轴/圆轨迹轴 偏差可达数十度，重复性不能排除稳定系统误差，因此不得把该相位写入URDF；左右手 thumb_mcp 都保留原始CAD零位0。该保护只冻结静态 origin.rpy，不会冻结或复制其 angle_rad[256] 实测轨迹。

前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位 差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的 训练周期Student-t 95%改善下界检查。最终门限不会因自动重试而放宽。

单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动 耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差， 不会替代三轮轴方向≤0.75°和第三轮MAE/P95留出验证。

四指参考源的MCP pitch虽有约70°大行程，但侧面机位接近沿转轴观察，单目PnP深度偏差 仍可能把低残差的Tag中心圆平面稳定地倾斜。因此MCP pitch与其他端视关节一样，始终用 完整相对姿态确定轴方向，Tag中心轨迹只参与轴线位置拟合；不再按10°分界在两种轴模型 之间切换。固定Tag安装旋转会在相对旋转中抵消，不需要中心圆回退。被动PIP/DIP继承 上游轴方向时不重复报告同一项跨轮轴失败。

四指MCP侧摆的动态曲线仍由参考指三轮实测并继承，但绝对静态侧摆零位固定使用原始CAD 的0。仅凭下游pitch轴相对CAD掌坐标反推roll相位，会把稳定的跨视角/固定几何偏差写成 约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。

四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断， 但拟合出的共同掌坐标相位不写入四指 origin.rpy；只发布各指相对四指中值的实测 装配偏差。拇指CMC roll/yaw/pitch的非零修正来自当前会话的完整相对旋转行程与机械 端点，视觉轴链继续用于轴线、PnP和留出诊断；代码和配置中不保存任何按左右手或 序列号写死的拇指零位角。电机5的256点动态曲线同样使用本机四轮实测结果。

视觉依赖链仍为：yaw轴检查拇指roll、pitch轴检查拇指yaw、MCP轴线相位检查拇指 pitch、IP轴线相位检查拇指MCP。该链用于几何和PnP诊断，不再决定四个具有机械端点 的拇指主动关节绝对零位；四指PIP/DIP轴线相位也继续用于机构诊断。 原始URDF的 origin.xyz、axis.xyz、连杆长度、mesh和被动结构固定。yaw扫描时电机5 保持145，求解器使用实测 angle_rad[145] 还原该条件，不会把145误当成baseline。 偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL 会显示实际估计值及配置上限，而不是把所有超限结果都截断成恰好±20°或±3°；该诊断搜索 不会放宽正式结果的硬门限。

生成修正URDF时只修改通过验收的主动关节 origin.rpy，不会修改任何关节的 origin.xyz、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终 JSON；任一曲线点越过CAD限位都会阻止正式发布，程序不会自动扩大URDF限位。

坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、 重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%， 端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时 连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并 暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫 完整关节，每关节最多自动重采2轮。过程指标落在最终门限的1.25倍内时会标记为黄色 预警，但只要仍超过硬门限，就在当前关节立即使用剩余重试预算 （provisional_fit_warning_rescan）；第三次仍超限则当场暂停，不允许预警数据继续到 后续关节。最终拟合仍按原硬门限验收，因此不会在全部任务采完后才回头重采靠前 关节。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序 只暂停一次且不再自动重扫，防止重复运动；此时也拒绝resume形成死循环。其他可恢复 失败在预算耗尽后才暂停，resume从最小失败单元继续，已通过数据保留。所有失败尝试 仍保存在 raw_samples.jsonl。若连续两次完整重扫出现轮次和数值都重复的 PnP双簇行程，程序将它判为系统性分支失败并当场停止，不再浪费第3次全关节重扫。

每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。 程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组 分支；侧面Tag 4/5/6/7贴面在该端点应近似平行，初始化还会比较相邻Tag法向，避免 错误镜像分支虽然8帧稳定且重投影很小仍被选中。每轮 255→0 前都会在静止端点独立 重置并重新选择分支，使第三轮同时成为PnP初始化留出，而不是三轮共享同一错误分支。 再开始正式轨迹采集。初始化帧不写入轨迹；最终单轴、跨轮和留出门限不变。

左手测食指roll时将电机7/8/9固定到0；右手测小指roll时因左右手侧摆机构镜像， 将电机6/7/8固定到255。两者均为相机画面向右的物理避挡方向，速度分别为 [15,5,15,15,15] 和 [15,15,15,15,5]。参考指MCP pitch/PIP扫描分别使用 电机1/16（左）或4/19（右），参考指速度10。

右手扫描拇指CMC俯仰（电机0）前，程序将拇指横摆电机10和拇指侧摆电机5都固定到 255，确认两个辅助关节到位后才允许电机0执行全行程。该关节由正面机位使用掌部 Tag 0和运动Tag 1同帧测量。每帧都会保留两个辅助关节 的实测条件值，轴线经外参转换到公共坐标系，零位求解按URDF上游关节链补偿；左手 仍沿用原有正面机位和基准姿态。

标定 thumb_cmc_yaw（电机10）时，程序将 thumb_cmc_roll（电机5）固定为145，并在它到位后才开始采样，以保持运动Tag ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试和人工 resume 都保持 电机5为145，只让电机10返回待重扫方向的起点；电机10全部三轮完成后，电机5才 恢复基准值255。自动恢复直接发送恢复目标，不会短暂发送保持当前位置命令；操作员 暂停/终止、恢复预算耗尽或机械停滞时仍保持当前位置。最终JSON的 baseline_command_u8 不变。

右手标定小指PIP（电机19）时，命令0对应的固件反馈可能稳定饱和在5。只有电机19 的0端使用±5反馈容差，并将该实测机械端点归入命令0端点分箱；255端和其他电机仍 使用默认±2。轨迹仍须覆盖至少240个u8并通过完整拟合门限，所以中途卡滞不会被误判 为端点到达。

右手拇指横摆电机10在命令255时多次实测稳定饱和在250，因此仅右手电机10的255端 使用±5反馈容差；其命令0端实测反馈为4，仍使用±4，其他电机和中间位置不放宽。基准姿态、作为 电机0辅助避挡姿态以及电机10自身扫描端点都使用同一条专用判定。

三机位流程默认 validation_enabled:=false，即不增加随机机械动作，但第三轮留出验证 始终启用且不能关闭；最终 quality.validation_mae_rad/p95_rad 正是第三轮轨迹误差。 状态中的扫描进度和总体进度分开显示：42/42只表示计划轨迹已采完，总体进度在拟合和 验证完成、正式JSON与URDF成功写入之前不会显示100%。

上面机位在Tag二维质量合格但PnP连续无效达到1秒时，会自动重置该机位的单Tag 和双Tag连续性跟踪器，并从下一帧重新建链，早于3秒采集超时。暂停恢复时只要求 当前活动机位就绪；首次调用 start 仍要求三个机位全部通过预检。

3. 输出

通过后生成精简JSON和一个新URDF：

calibration_output/G20_LEFT_001/<时间戳>/
  g20_left_G20_LEFT_001_calibration.json
  g20_left_G20_LEFT_001_calibration_urdf_correction_input.json
  linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf
  meshes/*.STL

calibration_output/G20_RIGHT_001/<时间戳>/
  g20_right_G20_RIGHT_001_calibration.json
  g20_right_G20_RIGHT_001_calibration_urdf_correction_input.json
  linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
  meshes/*.STL

*_urdf_correction_input.json 会在修正 URDF 之前落盘并重新读取，且绑定源 URDF 哈希、型号、侧别、layout 和序列号。公开 schema-v4 运行 JSON 仍保持原格式，原有 G20 运行桥、金标准哈希和 URDF 写回数值不因该交接层改变。

文件包含21个关节的256项 angle_rad、16个主动关节的 zero_command_u8、 zero_angles.urdf_zero_offset_rad、5个被动标记、模板来源和总体质量。新URDF每次从 指定原始CAD文件生成，采用 T_original × Rot(axis, offset)，绝不叠加旧校准文件或 覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 origin.rpy 可能 改变；当前19-Tag右手的16个主动静态零位字段全部由本会话数据求解；四指DIP和 thumb_ip为被动关节，发布实测动态曲线并保留CAD静态零位，其mimic坐标偏置只随 上游主动关节坐标系变换作等价调整。 未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到 同一会话，保证会话内URDF可独立加载，并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、 图像时间戳、20通道状态、同步误差和PnP误差只进入 raw_samples.jsonl。

完整 raw_samples.jsonl 已存在时，可以按当前算法离线重放，不连接相机、不发送电机 命令。--output-tag 为新产物增加安全后缀，已有JSON、URDF和验证报告不会被覆盖：

python3 -m linkerhand_calibration.offline_replay \
  calibration_output/G20_RIGHT_001/20260811_120146 \
  --output-tag AXIS_FRAME_V3 \
  --write

下面保留原有正面拇指独立标定说明和兼容入口。

4. 修正URDF的运行时关节映射

修正URDF已经把 zero_angles.urdf_zero_offset_rad 写入关节 origin.rpy。仿真运行时只能再使用同一台、同一侧机械手JSON中的256点 angle_rad 动态曲线，不能把 urdf_zero_offset_rad 再加一次，也不能把左手曲线 用于右手URDF。可用桥接节点将GUI的20通道u8命令转换为完整21关节 JointState（包括5个被动关节）：

ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
  hand_type:=right \
  calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json

schema v5默认订阅 /g20/cb_right_hand_state；schema v4默认订阅 /g20/cb_right_hand_control_cmd。两者均发布 /sim/mujoco/g20/right/joint_state。启动前必须停止任何旧的同名话题桥，避免两个 发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令， 因此不会静默退回旧标定。

该包启动海康机器人 MVS USB3 Vision 黑白相机、图像校正、apriltag_ros、 Linker Hand SDK 和标定状态机， 只扫描 G20 左手命令下标 0、15。默认使用单终点连续模式：每个方向只发送一次 终点命令，速度保持在固件能稳定响应的 15。SDK 以独立时间戳反馈实际 20 维位置， 程序把每帧 AprilTag 三维中心与同一时刻的实际电机位置插值配对并按整数位置分箱。 完成 255→0→255 后分别拟合正反方向并检查回差，最终运行时 JSON 将两条曲线逐点 平均，只为每个关节保存一个 256 项 angle_rad。最后用 5 个随机静态命令复测精度。

当前默认使用 trajectory_center_3d。节点由四个亚像素角点和 CameraInfo.P 计算每张 Tag 的三维中心，但不把小尺寸平面 Tag 的 PnP 朝向直接当作关节角：

• 根部扫描先减去掌心 T0 的位置，再用 T3/T4/T5 三条圆轨迹共同拟合 CMC 旋转轴； 每帧三个角度取中位数。
• 尖部扫描用 T4 相对 T3 的圆轨迹直接拟合 MCP。G20 只有电机 15 这一个尖部输入， URDF 将被动 IP 定义为 thumb_ip = 1.02 × thumb_mcp，因此运行时 IP 曲线严格按 这个机械耦合生成。这样不会把不同相机角度下 T5 的平面 PnP 深度偏差误认为 IP 真实运动。
• 程序仍会按 MCP 角将 T5 反向旋转并拟合剩余小圆，但该结果只用于 trajectory_center_quality.tip 中的观测一致性诊断，不参与最终 IP 数组。
• 每条曲线都减去命令 255 的测量角，所以最终文件严格满足 angle_rad[255] == 0.0；angle_rad[0] 是该关节相对零位的最大角度。

这种方法对固定的相机摆放角度、Tag 在同一刚性连杆上的固定位置和贴纸朝向更不敏感。 但相机或贴纸在一次扫描过程中移动、Tag 翘起、角点严重抖动仍会破坏圆轨迹。程序会 检查平面残差、圆残差、轨迹半径、实际弧长和根部三个轨迹点的角度一致性。

PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维中心及辅助质量检查，不再 直接生成运行时角度。根部扫描用固定的 T3–T4、T4–T5 中心间距共同选择分支； 尖部扫描用固定的 T0–T3 中心间距约束非目标部分。中心间距漂移超过阈值仍会暂停， 避免错误中心进入圆拟合，但 Tag 的 PnP 朝向抖动不会触发该门限。

1. 标记和安全检查

• T0 必须保留并固定在掌壳，作为整体平移参考；T3 固定在拇指根部运动连杆，T4 固定在 MCP 后的连杆， T5 固定在最末节。四张 Tag 必须与所在刚性件完全固定，不能跨关节或贴在软胶上。
• 当前实物使用 tag36h11 的 ID 0/1/2/3，依次对应 T0/T3/T4/T5。如果实物 ID 改变，同时修改 config/front_tags.yaml 里检测节点和标定节点的两组数组。
• tag.sizes/tag_sizes_m 必须填写每张 Tag 的实测有效边长（米），当前实物黑色正方形实测为 16 mm，因此配置为 0.016。 测量检测角点所围成的正方形边长，不包含外围白色留边。
• 当前试标定允许四张 Tag 的有效边长至少 30 px（实测静态约 32～38 px），最终仍由 静止角度 RMS 和随机复测误差决定是否合格。四张 Tag 必须在全行程内均可见。需要短时检查标记时， 启动参数增加 publish_debug_image:=true，再订阅 /g20_thumb_calibration/debug_image；正式长时间扫描建议保持默认关闭。
• 执行全行程前清空拇指周围空间并准备断开电机电源。确认这只手的下标 0 和 15 均可安全走完整 255→0→255。标定节点发现命令话题上另有发布者时不会解锁扫描。

2. 安装与构建

sudo apt-get update
sudo apt-get install -y \
  ros-jazzy-image-pipeline \
  ros-jazzy-apriltag-ros \
  ros-jazzy-apriltag-msgs \
  ros-jazzy-camera-calibration \
  python3-yaml

cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install \
  --packages-select linker_hand_ros2_sdk linkerhand_calibration
source install/setup.bash

相机节点直接使用海康 MVS SDK。当前机器的默认安装位置是 /opt/MVS，需要存在：

/opt/MVS/lib/64/libMvCameraControl.so
/opt/MVS/Samples/64/Python/MvImport/MvCameraControl_class.py

正面相机默认按序列号 DB2163742 绑定（MVS 显示的 GUID 是 2BDFB2163742），型号校验为 MV-CS020-10UM。三台相机同时连接时程序不会按枚举 顺序猜测机位。启动 ROS 节点前必须关闭 MVS 客户端中的相机连接，否则设备可能被占用。

1624x1240 mono8 每帧约 2.0 MB，超过 Fast DDS 2.14 默认约 512 KB 的共享内存段。 三相机标定 launch 会固定使用 rmw_fastrtps_cpp，并通过新旧两个 Fast DDS 环境变量 加载 config/fastdds_large_images.xml，使用 64 MB 共享内存段；否则相机内部虽为 30 Hz， 大图订阅端通常只能收到约 1～4 Hz。修改配置后必须重启相关 ROS 进程才能生效。

首次使用必须先标定该相机和当前镜头的内参。主 launch 默认从 ~/.ros/camera_info/hikrobot_DB2163742.yaml 加载标准 ROS CameraInfo YAML；文件缺失时 仍可预览 mono8 原图，但发布的内参无效，轨迹标定预检不会解锁运动。

先单独启动相机（不会连接机械手，也不会发送关节命令）：

ros2 run linkerhand_calibration hikrobot_camera_node --ros-args \
  --remap __ns:=/camera/camera/color \
  -p serial_number:=DB2163742 \
  -p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml

测速时优先检查同帧发布的小消息和原图；两者正常值都应接近 30 Hz：

ros2 topic hz /camera/camera/color/camera_info
ros2 topic hz /camera/camera/color/image_raw

使用标定板采集内参。下面的 8x6 是内角点数量、0.020 是单格边长 20 mm，必须按 实际标定板修改：

ros2 run camera_calibration cameracalibrator \
  --size 8x6 --square 0.020 \
  --camera_name hikrobot_front_DB2163742 \
  --ros-args \
  --remap image:=/camera/camera/color/image_raw \
  --remap camera/set_camera_info:=/camera/camera/color/set_camera_info

在标定界面完成采样后点击 CALIBRATE，确认重投影误差，再点击 COMMIT。相机节点会 原子写入上述 YAML，并立即开始发布有效内参。内参只适用于标定时的镜头焦距、对焦、 分辨率和 ROI；改变任何一项都要重新标定。

连接 CAN 后先确认 can0 已启动。不要同时运行其他会发布 /g20/cb_left_hand_control_cmd 的程序。

3. 启动和操作

首次使用时可先用 commands_enabled:=false 做预检；SDK 仍会设置速度/扭矩并读取状态， 但标定节点不会发送位置运动命令，也不会允许解锁全行程扫描：

ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
  serial_number:=G20_LEFT_001 \
  camera_serial_number:=DB2163742 \
  commands_enabled:=false

确认 T0、T3、T4、T5 在根部和尖部全行程中不会被遮挡，且拇指运动不会碰撞后， 停止预检并启动一个新的正式会话。默认使用 AprilTag 内部 decimate=1.5 提升检测 速度，并使用单终点连续运动：

ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
  serial_number:=G20_LEFT_001 \
  camera_serial_number:=DB2163742 \
  can_interface:=can0 \
  calibration_speed:=15 \
  continuous_motion_mode:=endpoint \
  angle_estimation_mode:=trajectory_center_3d \
  apriltag_decimate:=1.5 \
  use_roi:=false

默认关闭 ROI，AprilTag 使用完整的 1624×1240 校正画面。查看实际送入 AprilTag 的完整画面：

ros2 run image_view image_view --ros-args \
  --remap image:=/camera/camera/color/image_rect

图像检测链路使用 sensor_data（BEST_EFFORT）QoS，只保留最新帧，避免完整分辨率 下可靠队列积压反压相机；这不会裁剪图像，也不会降低相机分辨率。

若以后需要以帧率优先，可传入 use_roi:=true；默认 ROI 是原图中的 x=128, y=192, width=1024, height=528，也可用 roi_x、roi_y、 roi_width、roi_height 覆盖。

监控状态：

ros2 topic echo /g20_thumb_calibration/status

预检通过后状态为 WAIT_ROOT_CONFIRM，reason 为 call_start。只需调用一次：

ros2 service call /g20_thumb_calibration/start std_srvs/srv/Trigger {}

节点随后自动完成下标 0 的 255→0→255、下标 15 的 255→0→255 和 5 点随机复测， 正常结束状态为 COMPLETE，无需在根部和尖部之间再次确认。为安全起见，调用 start 前必须一次性确认两个关节的完整行程都已清空。原来的 confirm_root_full_range、confirm_tip_full_range 服务仍保留用于兼容。

暂停、恢复和终止：

ros2 service call /g20_thumb_calibration/pause std_srvs/srv/Trigger {}
ros2 service call /g20_thumb_calibration/resume std_srvs/srv/Trigger {}
ros2 service call /g20_thumb_calibration/abort std_srvs/srv/Trigger {}

预检要求四 Tag 有效帧率至少 95%，且检测消息频率至少 15 Hz。PnP 有效率也必须 至少 95%，每个候选解的重投影 RMS 不超过 1.5 px。中心轨迹模式以三组相对中心 的静止 RMS 不超过 2 mm、5 mm 范围内位置内点不少于 90% 为硬判据；PnP 朝向抖动 只作为诊断，不会阻止静态捕获。 状态中的 pnp_rejections 会指出当前是哪张 Tag 因丢失、重投影/倾角超限或姿态跳变而被拒绝， pnp_reprojection_error_px 显示四张 Tag 最近一次有效解的误差。连续扫描要求 图像与状态的时间差不超过 150 ms、全行程至少得到 40 个有效帧、 至少覆盖 32 个整数位置且相邻实测位置间隔不超过 16。Tag 或同步状态持续丢失 3 秒、 90 秒内未到达终点，或覆盖不足时，节点保持当前命令并进入 PAUSED。恢复时会先回到 该方向的起点，再完整重扫这个方向，避免把半程数据混入结果。abort 也只停止队列， 不会主动移动机械手。正常扫描和随机复测最后一项均为命令 255。

PnP 跟踪在整个会话中对四张 Tag 都优先保持同一个 IPPE 平面分支；最多 5 秒的短暂检测 间隔不会重新初始化分支。随机复测只有在同步电机反馈与目标相差不超过 2、且稳定 窗口与捕获窗口内三个相对中心的最大偏差都不超过 3 mm 时才会写入，否则继续等待并最终暂停， 不会再生成明知不可靠但字段完整的结果。

单终点连续模式共有 4 个端到端命令：根部和尖部各一个往返。每个方向运动前会先用 实际电机反馈确认已经到达起点，再做一次短暂静态确认；随机验证的“接近位置”只等待 电机反馈到位，不再重复采图。若实际 AprilTag 检测仍低于 15 Hz，先优化检测链路， 不要降低到固件低速区。必须临时回退时可启动 continuous_motion_mode:=paced，该模式按步长 8 到位即发下一段。

连续扫描中的主要状态字段：

• state_zh/reason_zh/action_zh：当前阶段、失败原因和下一步操作的中文说明； 原有 state/reason 英文机器码继续保留。
• tag_quality：逐张显示 T0/T3/T4/T5 的边长、hamming、识别置信度、重投影误差、 是否有效和具体中文问题，不再需要手工解析 /apriltag/detections。
• /g20_thumb_calibration/status_text：适合终端直接查看的多行中文状态。使用 ros2 topic echo --once /g20_thumb_calibration/status_text --field data 即可看到原因、建议及四张标签的质量。
• scan_progress：4 个方向的完成比例，依次约为 0、0.25、0.5、0.75、1.0。
• sweep_valid_frames_seen：当前连续方向已收到的同步有效帧数。
• sweep_state_span_u8：当前方向实际覆盖的电机范围，接近 255 才算完整。
• active_phase/active_direction：当前是根部或尖部、下降或上升方向。
• pnp_branch_corrections：四张 Tag 联合跟踪为维持相邻关节姿态连续，而没有选择 单张 Tag 最小重投影分支的累计次数。
• pnp_trajectory_quality：最近一个完整方向的整段分支修正帧数，以及相对整段稳健 参考的旋转、相对平移和中心间距漂移。中心轨迹模式只按欧氏中心间距判断： P95 超过 3 mm 或单帧最大值超过 6 mm 时暂停；旋转及随 Tag 坐标轴表达的相对平移 只保留为诊断。
• trajectory_center_quality：四个方向完成并拟合后，显示三维平面/圆残差、拟合半径、 实际弧长、T0/T3 锚点漂移和根部三个轨迹点的角度一致性。其中 tip.ip_observed_vs_constrained_* 显示T5残余小圆与URDF被动耦合之间的差异； 它用于发现T5识别误差、标签松动或机构异常，但不会改变最终IP曲线。

根部扫描中 T3/T4/T5 作为完整刚性组共同选择 IPPE 分支，不再把 T3 固定为在线解； 尖部扫描仍固定 T3，只用静止的 T0/T3 约束修正非目标根部姿态。

4. 中断恢复和输出

默认会话目录是启动命令当前目录下：

calibration_output/<序列号>/<时间戳>/

恢复时必须显式复用原目录，否则会创建新会话：

ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
  serial_number:=G20_LEFT_001 \
  session_dir:=/绝对路径/calibration_output/G20_LEFT_001/20260727_120000

恢复会校验序列号、Tag 配置、基准命令、扫描模式、采集参数和代码哈希； 任一项变化都会拒绝混用旧样本， 此时应新建会话。

目录内文件：

• raw_samples.jsonl：连续帧按实际整数电机位置分箱后的 Tag 三维中心、姿态辅助统计及复测点；每完成一个 扫描方向后落盘。
• checkpoint.json：当前状态和进度。
• session_manifest.json：Tag、相机内参、SDK、代码哈希和会话信息。
• validation.json：随机复测及全部质量判据。
• rosbag/：仅在 record_bag:=true 时生成，用于保存相机、检测、命令和状态等诊断数据。
• g20_left_<序列号>_thumb_angle.json：精简后的运行时标定文件。

最终文件使用 schema_version: 2。每个关节只包含：

{
  "motor_index": 0,
  "angle_rad": ["按命令0～255索引的256个弧度值"]
}

thumb_ip.angle_rad 由 thumb_mcp.angle_rad 乘 ip_coupling.multiplier（默认 1.02）得到，二者在命令255处都严格为零。

thumb_ip 另外包含 "passive": true。正反方向原始曲线不进入最终 JSON，但仍保留 在 raw_samples.jsonl 中，并用于最大回差和质量判定。

零位和最大角度可直接读取：

import json
from pathlib import Path

data = json.loads(Path("g20_left_G20_LEFT_001_thumb_angle.json").read_text())
for name, joint in data["joints"].items():
    print(name, "zero(rad)=", joint["angle_rad"][255],
          "max(rad)=", joint["angle_rad"][0])

如果相机或 SDK 已由外部进程启动，可传 start_camera:=false 或 start_sdk:=false。camera_serial_number 同时接受 MVS 序列号和 GUID，但推荐使用稳定且简短的序列号 DB2163742。

海康相机默认输出 1624x1240@30Hz mono8，全局快门，曝光时间 5000us、增益 0dB，并使用“只取最新帧”策略避免视觉延迟。现场亮度不足时优先增加照明；必要时可用 exposure_time_us、gain_db 调整，或临时传 auto_exposure:=true。正式轨迹采集建议固定 曝光，避免自动曝光在运动过程中改变角点质量。rosbag 默认关闭；需要诊断留档时增加 record_bag:=true。

默认对完整 1624×1240 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用 image_rect 的角点及同一条处理链对应的 CameraInfo，启动文件已自动保证二者配对。 校正和 AprilTag 组件运行 在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。 可选 ROI 模式会额外在同一容器内加入裁剪组件并同步修正 CameraInfo。标定节点默认 不订阅整幅图像，只订阅检测结果和 TF。 若启用调试图，预览会缩放到 50%、限速 10 Hz 并使用最新帧优先的传输方式， 不影响 AprilTag 的 ROI 输入。

静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。 当前末节16 mm Tag如果中心位置 RMS 持续不合格，应优先增加照明、缩短 相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。

启用 rosbag 后保存裁剪后的原始图像和配套 CameraInfo，避免新增一个全分辨率图像 订阅者；同时使用 MCAP zstd_fast 压缩并按 10 GiB 分卷。快速标定通常不需要录制； 若用于正式可追溯验收，再启用并检查磁盘空间。

5. CMC Pitch 零位角测量

只测量命令 255 时 thumb_cmc_pitch 的画面水平投影零位角时，使用独立启动文件。 它只拟合 CMC 的二维零位轨迹圆，不运行完整 0～255 角度映射，也不会生成或修改 URDF：

ros2 launch linkerhand_calibration \
  front_cmc_pitch_zero.launch.py \
  serial_number:=G20_LEFT_001

该流程只要求 T0（ID 0）和 T3（ID 1）有效。T4/T5 可以留在手上，但丢失不会阻塞。 预检完成后查看中文状态：

ros2 topic echo --once --full-length \
  /g20_thumb_cmc_pitch_zero/status_text \
  --field data

状态显示“等待开始”后启动三轮测量：

ros2 service call \
  /g20_thumb_cmc_pitch_zero/start \
  std_srvs/srv/Trigger {}

查看带红色画面水平线、T0/T3标签中心、青色轨迹点、紫色拟合圆心和径向零位线 的调试画面：

ros2 run image_view image_view --ros-args \
  --remap image:=/g20_thumb_cmc_pitch_zero/debug_image

画面底部红线是固定的相机水平与构图目标。程序会在画面下部自动寻找一条足够长、 接近水平的物理桌边或高对比参考直线，并画成蓝线。开始前调整相机，使蓝线与红线 重合；画面和 status_text 会实时显示红蓝线夹角及垂直偏差。±0.5° 和 ±12 px 只用于显示 ALIGNED/ADJUST，完全不参与预检或 start 服务判断。 由操作人员确认相机位置后手动开始标定。参考直线应清晰、连续并尽量横跨画面； 该检测不使用 T0/T3 标签朝向。

一条二维直线只能确认相机滚转角和上下构图位置，不能单独证明相机的距离、俯仰、 偏航或完整三维位置。若需要严格复现这些量，还应使用固定相机支架或专用标定板。

每轮只控制电机 0 执行一次 255→64 端点运动和一次 64→255 返回运动。运动期间 连续采集 T3中心−T0中心，按机械手状态分箱后拟合图像平面圆；返回 255 后使用 “T3零位中心→拟合圆心”的固定内向径向矢量计算角度。运动前和返回后各采集30帧静态零位， 三轮轨迹合并后得到最终圆心。其他 19 个命令保持固定基准。任何其他节点同时发布 /g20/cb_left_hand_control_cmd 时，start 服务会拒绝启动。

T0中心用于消除相机或整只手的平移抖动。T0和T3标签自身的朝向与角点 +x 都不参与零位或行程计算；标签可以任意平面内旋转或反贴180°，只需标签平整、 固定且中心始终可见。若轨迹跨度、圆弧、半径、径向RMS/P95或回零误差不合格， 节点暂停或写出 quality.passed=false。

完成后只生成：

calibration_output/G20_LEFT_001/<时间戳>/
  g20_left_G20_LEFT_001_thumb_cmc_pitch_zero.json

核心字段是：

zero_angles.table_projected_zero_rad

该值是内向径向零位矢量相对相机画面水平向右方向的角度。它不使用 T0 的方向， 但会用 T0 中心抵消平移；它仍不是真实三维桌面检测，因此会随相机滚转和机械手 摆放改变。

6. CMC Roll 零位与行程标定

thumb_cmc_roll 复用上节的 T0 平移补偿、T3 中心轨迹分箱和稳健圆拟合， 但控制的是电机 5。每轮执行 255→0→255：在 255 零位、0 行程端点和返回 255 后各静态采集 30 帧，因此可以同时测量零位角和完整 0～255 实际角行程。 命令 0 是完整行程端点，开始前必须确认拇指没有机械碰撞或硬限位顶死风险。 Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量，不读取T3标签朝向。

ros2 launch linkerhand_calibration \
  front_cmc_roll_calibration.launch.py \
  serial_number:=G20_LEFT_001

预检通过后启动三轮标定：

ros2 topic echo --once --full-length \
  /g20_thumb_cmc_roll_calibration/status_text \
  --field data

ros2 service call \
  /g20_thumb_cmc_roll_calibration/start \
  std_srvs/srv/Trigger {}

调试画面：

ros2 run image_view image_view --ros-args \
  --remap image:=/g20_thumb_cmc_roll_calibration/debug_image

Roll 使用与 Pitch 相同的红蓝参考线显示，但是否对齐由操作人员确认，程序不会用 蓝线状态阻止 start 进入电机运动。

完成后生成：

calibration_output/G20_LEFT_001/<时间戳>/
  g20_left_G20_LEFT_001_thumb_cmc_roll_zero_travel.json

核心输出字段：

zero_angles.table_projected_zero_rad
travel.signed_rad
travel.range_rad

travel.signed_rad 是从命令 255 到 0 的有符号转角，travel.range_rad 是三轮 行程大小的中值。只有轨迹圆质量、T0/T3 检出率、三轮零位/行程一致性、端点径向 误差和回零误差全部通过时，quality.passed 才为 true。