O6gui预设动作修改、O12标定初始化提交

.gitignore

@@ -50,6 +50,7 @@ Thumbs.db
 
 # Runtime and calibration scratch files
 /logs/
+/MvSdkLog/
 *.tmp
 *.log
 *.bak
@@ -61,13 +62,23 @@ Thumbs.db
 # src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
 /profiles/
 /calibration_output/
+/config/*_three_camera_extrinsics.yaml
 *.wear_check.json
 *.checkpoint.json
 *.verification.json
 *_mapping_quality.json
 
-# Device-specific robot descriptions derived from local CMC pitch calibration
+# Device-specific robot descriptions derived from local calibration runs
+# Includes full/partial zero-calibration outputs and local copies.
+/src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf
+/src/linkerhand_calibration/urdf/*/*_transferred_from_*.urdf
 /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_cmc_pitch_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_zero_calibrated_*.urdf
+/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_G20_RIGHT_tag.urdf
 
 # ROS bag / MCAP recordings and CAN captures
 rosbag2_*/
@@ -84,3 +95,5 @@ candump-*
 # Local Codex/agent workspace metadata
 /.agents/
 /.codex/
+/.codebuddy/
+/.zcode/

src/g20_thumb_apriltag_calibration/README.md

@@ -1,398 +0,0 @@
-# G20 左手拇指正面 AprilTag 标定
-
-该包启动 RealSense、彩色图像校正、`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 的实测有效边长（米），当前配置为 `0.010`。
-  测量检测角点所围成的正方形边长，不包含外围白色留边。
-- 当前试标定允许四张 Tag 的有效边长至少 30 px（实测静态约 32～38 px），最终仍由
-  静止角度 RMS 和随机复测误差决定是否合格。四张 Tag 必须在全行程内均可见。需要短时检查标记时，
-  启动参数增加 `publish_debug_image:=true`，再订阅
-  `/g20_thumb_calibration/debug_image`；正式长时间扫描建议保持默认关闭。
-- 执行全行程前清空拇指周围空间并准备断开电机电源。确认这只手的下标 0 和 15
-  均可安全走完整 `255→0→255`。标定节点发现命令话题上另有发布者时不会解锁扫描。
-
-## 2. 安装与构建
-
-```bash
-sudo apt-get update
-sudo apt-get install -y \
-  ros-jazzy-realsense2-camera \
-  ros-jazzy-realsense2-description \
-  ros-jazzy-image-pipeline \
-  ros-jazzy-apriltag-ros \
-  ros-jazzy-apriltag-msgs
-
-cd /home/lxp/projects/linkerhand_retarget_ros2
-source /opt/ros/jazzy/setup.bash
-colcon build --symlink-install \
-  --packages-select linker_hand_ros2_sdk g20_thumb_apriltag_calibration
-source install/setup.bash
-```
-
-连接 CAN 后先确认 `can0` 已启动。不要同时运行其他会发布
-`/g20/cb_left_hand_control_cmd` 的程序。
-
-## 3. 启动和操作
-
-首次使用时可先用 `commands_enabled:=false` 做预检；SDK 仍会设置速度/扭矩并读取状态，
-但标定节点不会发送位置运动命令，也不会允许解锁全行程扫描：
-
-```bash
-ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
-  serial_number:=G20_LEFT_001 \
-  commands_enabled:=false
-```
-
-确认 T0、T3、T4、T5 在根部和尖部全行程中不会被遮挡，且拇指运动不会碰撞后，
-停止预检并启动一个新的正式会话。默认使用 AprilTag 内部 `decimate=1.5` 提升检测
-速度，并使用单终点连续运动：
-
-```bash
-ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
-  serial_number:=G20_LEFT_001 \
-  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 使用完整的 1280×720 校正画面。查看实际送入 AprilTag
-的完整画面：
-
-```bash
-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` 覆盖。
-
-监控状态：
-
-```bash
-ros2 topic echo /g20_thumb_calibration/status
-```
-
-预检通过后状态为 `WAIT_ROOT_CONFIRM`，`reason` 为 `call_start`。只需调用一次：
-
-```bash
-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` 服务仍保留用于兼容。
-
-暂停、恢复和终止：
-
-```bash
-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. 中断恢复和输出
-
-默认会话目录是启动命令当前目录下：
-
-```text
-calibration_output/<序列号>/<时间戳>/
-```
-
-恢复时必须显式复用原目录，否则会创建新会话：
-
-```bash
-ros2 launch g20_thumb_apriltag_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`。每个关节只包含：
-
-```json
-{
-  "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` 中，并用于最大回差和质量判定。
-
-零位和最大角度可直接读取：
-
-```python
-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:=<序列号>`
-可绑定指定 RealSense。
-
-D405 的彩色流来自 `depth_module`，启动文件会同时设置
-`depth_module.color_profile` 和 `rgb_camera.color_profile`，默认均为
-`1280x720x30`。连续模式默认关闭深度和 rosbag，以减少 USB、CPU 和磁盘负担；
-它们都不参与角度计算。需要完整诊断留档时可增加
-`enable_depth:=true record_bag:=true`。
-
-默认对完整 1280×720 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用
-`image_rect` 的角点及同一条处理链对应的 `CameraInfo`，启动文件已自动保证二者配对。
-校正和 AprilTag 组件运行
-在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。
-可选 ROI 模式会额外在同一容器内加入裁剪组件并同步修正 `CameraInfo`。标定节点默认
-不订阅整幅图像，只订阅检测结果和 TF。
-若启用调试图，预览会缩放到 50%、限速 10 Hz 并使用最新帧优先的传输方式，
-不影响 AprilTag 的 ROI 输入。
-
-静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。
-当前 30～38 px 的 10 mm Tag 属于试标定尺寸，如果中心位置 RMS 持续不合格，应优先增加照明、缩短
-相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。
-
-启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像
-订阅者；同时使用 MCAP `zstd_fast` 压缩并按 10 GiB 分卷。快速标定通常不需要录制；
-若用于正式可追溯验收，再启用并检查磁盘空间。
-
-## 5. CMC Pitch 零位角测量
-
-只测量命令 255 时 `thumb_cmc_pitch` 的画面水平投影零位角时，使用独立启动文件。
-它只拟合 CMC 的二维零位轨迹圆，不运行完整 0～255 角度映射，也不会生成或修改
-URDF：
-
-```bash
-ros2 launch g20_thumb_apriltag_calibration \
-  front_cmc_pitch_zero.launch.py \
-  serial_number:=G20_LEFT_001
-```
-
-该流程只要求 T0（ID 0）和 T3（ID 1）有效。T4/T5 可以留在手上，但丢失不会阻塞。
-预检完成后查看中文状态：
-
-```bash
-ros2 topic echo --once --full-length \
-  /g20_thumb_cmc_pitch_zero/status_text \
-  --field data
-```
-
-状态显示“等待开始”后启动三轮测量：
-
-```bash
-ros2 service call \
-  /g20_thumb_cmc_pitch_zero/start \
-  std_srvs/srv/Trigger {}
-```
-
-查看带红色画面水平线、T0/T3标签中心、青色轨迹点、紫色拟合圆心和径向零位线
-的调试画面：
-
-```bash
-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`。
-
-完成后只生成：
-
-```text
-calibration_output/G20_LEFT_001/<时间戳>/
-  g20_left_G20_LEFT_001_thumb_cmc_pitch_zero.json
-```
-
-核心字段是：
-
-```text
-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标签朝向。
-
-```bash
-ros2 launch g20_thumb_apriltag_calibration \
-  front_cmc_roll_calibration.launch.py \
-  serial_number:=G20_LEFT_001
-```
-
-预检通过后启动三轮标定：
-
-```bash
-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 {}
-```
-
-调试画面：
-
-```bash
-ros2 run image_view image_view --ros-args \
-  --remap image:=/g20_thumb_cmc_roll_calibration/debug_image
-```
-
-Roll 使用与 Pitch 相同的红蓝参考线显示，但是否对齐由操作人员确认，程序不会用
-蓝线状态阻止 `start` 进入电机运动。
-
-完成后生成：
-
-```text
-calibration_output/G20_LEFT_001/<时间戳>/
-  g20_left_G20_LEFT_001_thumb_cmc_roll_zero_travel.json
-```
-
-核心输出字段：
-
-```text
-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`。

src/g20_thumb_apriltag_calibration/config/realsense_color_qos.yaml

@@ -1,5 +0,0 @@
-# This is a flat parameter mapping consumed by realsense2_camera/rs_launch.py.
-# Keep image and CameraInfo durability identical so image_transport can
-# synchronize them for image_proc and apriltag_ros.
-color_qos: DEFAULT
-color_info_qos: DEFAULT

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/__init__.py

@@ -1,5 +0,0 @@
-"""Front-camera AprilTag calibration for the left LinkerHand G20 thumb."""
-
-from .core import BASELINE_COMMAND, COMMAND_NAMES
-
-__all__ = ["BASELINE_COMMAND", "COMMAND_NAMES"]

src/g20_thumb_apriltag_calibration/setup.cfg

@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/g20_thumb_apriltag_calibration
-[install]
-install_scripts=$base/lib/g20_thumb_apriltag_calibration

src/g20_thumb_apriltag_calibration/setup.py

@@ -1,41 +0,0 @@
-from glob import glob
-
-from setuptools import find_packages, setup
-
-
-package_name = "g20_thumb_apriltag_calibration"
-
-setup(
-    name=package_name,
-    version="0.1.0",
-    packages=find_packages(),
-    data_files=[
-        (
-            "share/ament_index/resource_index/packages",
-            ["resource/" + package_name],
-        ),
-        ("share/" + package_name, ["package.xml", "README.md"]),
-        ("share/" + package_name + "/config", glob("config/*.yaml")),
-        ("share/" + package_name + "/launch", glob("launch/*.launch.py")),
-    ],
-    install_requires=["setuptools", "numpy", "scipy"],
-    tests_require=["pytest"],
-    zip_safe=True,
-    maintainer="lxp",
-    maintainer_email="support@linker-robotics.com",
-    description="Front RealSense AprilTag calibration for the left G20 thumb",
-    license="MIT",
-    entry_points={
-        "console_scripts": [
-            "calibration_node = g20_thumb_apriltag_calibration.node:main",
-            (
-                "cmc_pitch_zero_node = "
-                "g20_thumb_apriltag_calibration.zero_node:main"
-            ),
-            (
-                "cmc_roll_calibration_node = "
-                "g20_thumb_apriltag_calibration.zero_node:main"
-            ),
-        ],
-    },
-)

src/g20_thumb_apriltag_calibration/test/test_config.py

@@ -1,184 +0,0 @@
-from pathlib import Path
-
-import yaml
-
-
-PACKAGE_ROOT = Path(__file__).resolve().parents[1]
-
-
-def test_front_tag_parameters_match_namespaced_detector() -> None:
-    config = yaml.safe_load(
-        (PACKAGE_ROOT / "config" / "front_tags.yaml").read_text()
-    )
-
-    detector = config["/apriltag/apriltag"]["ros__parameters"]
-    calibration = config["g20_thumb_calibration"]["ros__parameters"]
-
-    assert detector["tag"]["ids"] == calibration["tag_ids"]
-    assert detector["tag"]["frames"] == calibration["tag_frames"]
-    assert detector["tag"]["sizes"] == calibration["tag_sizes_m"]
-    assert detector["tag"]["ids"] == [0, 1, 2, 3]
-    assert detector["qos_profile"] == "sensor_data"
-    assert detector["detector"]["decimate"] == 1.5
-    assert detector["detector"]["refine"] is True
-    assert detector["detector"]["debug"] is False
-
-
-def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None:
-    config = yaml.safe_load(
-        (PACKAGE_ROOT / "config" / "calibration.yaml").read_text()
-    )
-    parameters = config["g20_thumb_calibration"]["ros__parameters"]
-
-    assert parameters["angle_estimation_mode"] == "trajectory_center_3d"
-    assert parameters["passive_ip_multiplier"] == 1.02
-    assert parameters["pnp_minimum_valid_rate"] == 0.95
-    assert parameters["pnp_maximum_reprojection_error_px"] <= 1.5
-    assert parameters["pnp_reprojection_tie_px"] == 1.5
-    assert parameters["pnp_tracker_reset_seconds"] == 5.0
-    assert parameters["pnp_group_relative_rotation_scale_deg"] == 5.0
-    assert parameters["pnp_group_relative_translation_scale_m"] == 0.01
-    assert parameters["pnp_group_reprojection_weight"] == 0.05
-    assert parameters["pnp_trajectory_reprojection_scale_px"] == 0.1
-    assert parameters["pnp_rigid_rotation_scale_deg"] == 5.0
-    assert parameters["pnp_rigid_p95_accepted_drift_deg"] == 8.0
-    assert parameters["pnp_rigid_maximum_accepted_drift_deg"] == 15.0
-    assert (
-        parameters["pnp_rigid_p95_accepted_distance_drift_m"]
-        <= 0.003
-    )
-    assert (
-        parameters["pnp_rigid_maximum_accepted_distance_drift_m"]
-        <= 0.006
-    )
-    assert parameters["pnp_maximum_pose_jump_deg"] <= 35.0
-    assert parameters["trajectory_maximum_plane_rms_m"] <= 0.004
-    assert parameters["trajectory_maximum_radial_rms_m"] <= 0.004
-    assert parameters["trajectory_minimum_radius_m"] >= 0.005
-    assert parameters["trajectory_minimum_arc_deg"] >= 15.0
-    assert (
-        parameters["trajectory_maximum_root_role_disagreement_deg"]
-        <= 5.0
-    )
-    assert parameters["trajectory_maximum_anchor_drift_m"] <= 0.005
-    assert parameters["trajectory_static_translation_outlier_m"] <= 0.005
-    assert (
-        parameters["trajectory_maximum_static_translation_rms_m"]
-        <= 0.002
-    )
-    assert parameters["minimum_edge_pixels"] == 30.0
-    assert parameters["maximum_static_std_deg"] == 3.0
-    assert parameters["minimum_pose_inlier_rate"] == 0.90
-    assert parameters["repetitions"] == 1
-    assert parameters["command_step"] == 8
-    assert parameters["scan_mode"] == "continuous"
-    assert parameters["continuous_motion_mode"] == "endpoint"
-    assert parameters["auto_start_tip"] is True
-    assert parameters["minimum_detection_hz"] == 15.0
-    assert parameters["stable_frames"] == 5
-    assert parameters["capture_frames"] == 8
-    assert parameters["maximum_stable_spread_deg"] == 3.0
-    assert parameters["maximum_stable_translation_spread_m"] <= 0.003
-    assert parameters["validation_command_count"] == 5
-    assert parameters["continuous_minimum_bins"] >= 32
-    assert parameters["continuous_maximum_bin_gap"] <= 16
-    assert parameters["continuous_segment_minimum_seconds"] >= 0.1
-    assert parameters["continuous_segment_timeout_seconds"] >= 5.0
-    assert parameters["continuous_prepare_timeout_seconds"] >= 30.0
-
-
-def test_realsense_color_and_camera_info_use_matching_qos() -> None:
-    config = yaml.safe_load(
-        (PACKAGE_ROOT / "config" / "realsense_color_qos.yaml").read_text()
-    )
-
-    assert config["color_qos"] == "DEFAULT"
-    assert config["color_info_qos"] == "DEFAULT"
-
-
-def test_cmc_pitch_zero_config_uses_three_trajectory_circle_rounds() -> None:
-    config = yaml.safe_load(
-        (PACKAGE_ROOT / "config" / "cmc_pitch_zero.yaml").read_text()
-    )
-    parameters = config["g20_thumb_cmc_pitch_zero"]["ros__parameters"]
-
-    assert parameters["t0_id"] == 0
-    assert parameters["t3_id"] == 1
-    assert parameters["joint_name"] == "thumb_cmc_pitch"
-    assert parameters["motor_index"] == 0
-    assert parameters["zero_command_u8"] == 255
-    assert parameters["measure_travel"] is False
-    assert parameters["baseline_command_u8"] == [
-        255,
-        255,
-        255,
-        255,
-        255,
-        255,
-        193,
-        148,
-        105,
-        42,
-        245,
-        255,
-        255,
-        255,
-        255,
-        255,
-        255,
-        255,
-        255,
-        255,
-    ]
-    assert parameters["repetitions"] == 3
-    assert parameters["zero_capture_frames"] == 30
-    assert parameters["trajectory_command_u8"] <= 64
-    assert parameters["trajectory_minimum_state_span_u8"] >= 160.0
-    assert parameters["trajectory_minimum_bins"] >= 18
-    assert parameters["trajectory_minimum_arc_deg"] >= 20.0
-    assert parameters["trajectory_maximum_radial_rms_px"] <= 2.0
-    assert parameters["trajectory_maximum_p95_radial_error_px"] <= 3.5
-    assert parameters["minimum_detection_rate"] == 0.95
-    assert parameters["minimum_edge_pixels"] == 40.0
-    assert parameters["maximum_static_position_rms_px"] <= 1.5
-    assert "maximum_axis_alignment_deg" not in parameters
-    assert "maximum_static_std_deg" not in parameters
-    assert "maximum_anchor_drift_deg" not in parameters
-    assert parameters["camera_alignment_enabled"] is True
-    assert parameters["camera_alignment_max_angle_deg"] <= 0.5
-    assert parameters["camera_alignment_max_vertical_offset_px"] <= 12.0
-    assert parameters["camera_alignment_required_frames"] >= 10
-    assert parameters["camera_alignment_minimum_detection_rate"] <= 0.8
-
-
-def test_cmc_roll_config_measures_full_endpoint_travel() -> None:
-    config = yaml.safe_load(
-        (PACKAGE_ROOT / "config" / "cmc_roll_zero_travel.yaml").read_text()
-    )
-    parameters = config[
-        "g20_thumb_cmc_roll_calibration"
-    ]["ros__parameters"]
-
-    assert parameters["t0_id"] == 0
-    assert parameters["t3_id"] == 1
-    assert parameters["joint_name"] == "thumb_cmc_roll"
-    assert parameters["motor_index"] == 5
-    assert parameters["zero_command_u8"] == 255
-    assert parameters["trajectory_command_u8"] == 0
-    assert parameters["measure_travel"] is True
-    assert parameters["baseline_command_u8"][5] == 255
-    assert parameters["repetitions"] == 3
-    assert parameters["zero_capture_frames"] == 30
-    assert parameters["trajectory_minimum_state_span_u8"] >= 240.0
-    assert parameters["trajectory_minimum_bins"] >= 30
-    assert parameters["minimum_travel_deg"] >= 20.0
-    assert parameters["maximum_travel_difference_deg"] <= 1.0
-    assert parameters["maximum_round_difference_deg"] == 1.0
-    assert "maximum_axis_alignment_deg" not in parameters
-    assert "maximum_static_std_deg" not in parameters
-    assert "maximum_anchor_drift_deg" not in parameters
-    assert parameters["camera_alignment_enabled"] is True
-    assert parameters["camera_alignment_max_angle_deg"] <= 0.5
-    assert parameters["camera_alignment_max_vertical_offset_px"] <= 12.0
-    assert parameters["camera_alignment_required_frames"] >= 10
-    assert parameters["camera_alignment_minimum_detection_rate"] <= 0.8

src/gui_control/gui_control/config/constants.py

@@ -9,6 +9,20 @@ class HandConfig:
     joint_names_en: Optional[List[str]] = None
     init_pos: List[int] = field(default_factory=list)
     preset_actions: Optional[Dict[str, List[int]]] = None
+    preset_action_overrides: Optional[Dict[str, Dict[str, List[int]]]] = None
+    # Integer GUI values are divided by this scale before publishing.
+    # Legacy hands use raw u8 values (scale=1); O12 uses milliradians.
+    position_scale: int = 1
+    position_unit: str = "u8"
+
+    def get_preset_actions(self, hand_type: str) -> Dict[str, List[int]]:
+        """Return preset actions with hand-specific values applied."""
+        actions = dict(self.preset_actions or {})
+        if self.preset_action_overrides:
+            actions.update(
+                self.preset_action_overrides.get(hand_type.lower(), {})
+            )
+        return actions
 
 # ------------------------------------------------------------------
 # 常量字典（仅构建一次）
@@ -75,10 +89,10 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
             "点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
             "握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
             "张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
-            "OK": [148, 110, 255, 255, 255, 44, 164, 100, 114, 127, 178, 255, 255, 255, 255, 94, 71, 255, 255, 255],
-            "拇指对中指": [191, 255, 55, 255, 255, 96, 95, 100, 114, 127, 105, 255, 255, 255, 255, 94, 255, 108, 255, 255],
-            "拇指对无名指": [191, 255, 255, 72, 255, 115, 95, 100, 114, 127, 60, 255, 255, 255, 255, 94, 255, 255, 97, 255],
-            "拇指对小指": [191, 255, 255, 255, 55, 0, 95, 100, 114, 121, 70, 255, 255, 255, 255, 94, 255, 255, 255, 100],
+            "OK": [0, 0, 255, 255, 255, 138, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 211, 255, 255, 255],
+            "拇指对中指": [0, 255, 0, 255, 255, 107, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 202, 255, 255],
+            "拇指对无名指": [0, 255, 255, 0, 255, 88, 171, 148, 105, 42, 59, 255, 255, 255, 255, 255, 255, 255, 206, 254],
+            "拇指对小指": [0, 255, 255, 255, 0, 32, 170, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 203],
             "准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
             "壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0],
             "贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],
@@ -224,13 +238,20 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
             "叁": [92, 87, 255, 255, 255, 0],
             "肆": [92, 87, 255, 255, 255, 255],
             "伍": [255, 255, 255, 255, 255, 255],
-            "OK": [139, 91, 103, 250, 250, 250],
+            "OK": [95, 75, 116, 255, 255, 255],
+            "拇指对中指": [88, 2, 255, 114, 255, 255],
             "点赞": [250, 79, 0, 0, 0, 0],
             "握拳": [102, 18, 0, 0, 0, 0],
-        }
+        },
+        preset_action_overrides={
+            "right": {
+                "OK": [95, 83, 122, 255, 255, 255],
+                "拇指对中指": [95, 8, 255, 114, 255, 255],
+            },
+        },
     ),
     "L6": HandConfig(
-        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
+        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"],
         joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
         init_pos=[250] * 6,
         preset_actions={
@@ -258,5 +279,33 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
             "拇指压感准备2": [139, 18, 130, 0, 0, 0]
         }
     ),
+    "O12": HandConfig(
+        # O12 active-angle API order.  Keep this independent from the
+        # physical motor order exposed by some SDK metadata.
+        joint_names_en=[
+            "thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
+            "index_abad", "index_mcp", "index_pip",
+            "middle_abad", "middle_mcp", "middle_pip",
+            "ring_mcp", "pinky_mcp",
+        ],
+        joint_names=[
+            "拇指旋转", "拇指侧摆", "拇指根部", "拇指中部",
+            "食指侧摆", "食指根部", "食指中部",
+            "中指侧摆", "中指根部", "中指中部",
+            "无名指根部", "小指根部",
+        ],
+        # Values are milliradians so QSlider can retain 0.001 rad resolution.
+        init_pos=[0] * 12,
+        preset_actions={
+            "展开": [0] * 12,
+            "食指轻弯": [0, 0, 0, 0, 0, 300, 400, 0, 0, 0, 0, 0],
+            "半握": [
+                300, -200, -200, -500,
+                0, 650, 700, 0, 650, 800, 700, 700,
+            ],
+        },
+        position_scale=1000,
+        position_unit="rad",
+    ),
 }
 HAND_CONFIGS = MappingProxyType(_HAND_CONFIGS)

src/gui_control/gui_control/gui_control.py

@@ -1,5 +1,6 @@
 import sys
 import time, json
+import math
 import threading
 from dataclasses import dataclass
 from typing import List, Dict
@@ -33,6 +34,16 @@ _CANONICAL_COMMAND_NAMES = {
         "thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch",
         "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
     ],
+    "L6": [
+        "thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch",
+        "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+    ],
+    "O12": [
+        "thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
+        "index_abad", "index_mcp", "index_pip",
+        "middle_abad", "middle_mcp", "middle_pip",
+        "ring_mcp", "pinky_mcp",
+    ],
 }
 
 _CANONICAL_COMMAND_BOUNDS = {
@@ -43,17 +54,38 @@ _CANONICAL_COMMAND_BOUNDS = {
         *[(0, 255)] * 5,
     ],
     "O6": [(0, 255)] * 6,
+    "L6": [(0, 255)] * 6,
+}
+
+# Integer slider bounds in milliradians, derived from the O12 active-angle
+# limits.  Right and left thumb signs differ; all other limits are shared.
+_O12_COMMAND_BOUNDS_MRAD = {
+    "right": [
+        (0, 942), (-1387, 0), (-827, 0), (-1291, 0),
+        (-261, 261), (0, 1352), (0, 1530), (-261, 261),
+        (0, 1357), (0, 1815), (0, 1535), (0, 1535),
+    ],
+    "left": [
+        (-942, 0), (0, 1387), (-827, 0), (-1291, 0),
+        (-261, 261), (0, 1352), (0, 1530), (-261, 261),
+        (0, 1357), (0, 1815), (0, 1535), (0, 1535),
+    ],
 }
 
 
 class ROS2NodeManager(QObject):
     """ROS2节点管理器，处理ROS通信"""
     status_updated = pyqtSignal(str, str)  # 状态类型, 消息内容
+    feedback_updated = pyqtSignal(object)
 
     def __init__(self, node_name: str = "hand_control_node"):
         super().__init__()
         self.node = None
         self.publisher = None
+        self.publisher_arc = None
+        self.state_subscription = None
+        self.speed_pub = None
+        self.torque_pub = None
         self.joint_state = JointState()
         self.joint_state.header = Header()
         
@@ -83,14 +115,26 @@ class ROS2NodeManager(QObject):
                 self.node.get_parameter('topic_prefix').value
             )
             
-            if self.is_arc == True:
+            if self.hand_joint == "O12":
+                topic_base = f'/o12/{self.hand_type}'
+                self.publisher = self.node.create_publisher(
+                    JointState, f'{topic_base}/joint_cmd', 10
+                )
+                self.state_subscription = self.node.create_subscription(
+                    JointState,
+                    f'{topic_base}/joint_states',
+                    self._o12_feedback_callback,
+                    10,
+                )
+            elif self.is_arc == True:
                 # 创建发布者
                 self.publisher_arc = self.node.create_publisher(
                     JointState,
                     self.topic(f'/cb_{self.hand_type}_hand_control_cmd_arc'),
                     10
                 )
-            # 创建发布者
+            # 创建旧型号发布者
+            if self.hand_joint != "O12":
                 self.publisher = self.node.create_publisher(
                     JointState,
                     self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
@@ -100,6 +144,7 @@ class ROS2NodeManager(QObject):
                 JointState, self.topic('/calibration_pose_snapshot'), 10
             )
                     # 新增 speed / torque 发布者
+            if self.hand_joint != "O12":
                 self.speed_pub = self.node.create_publisher(
                     String, self.topic('/cb_hand_setting_cmd'), 10)
                 self.torque_pub = self.node.create_publisher(
@@ -136,15 +181,36 @@ class ROS2NodeManager(QObject):
         while rclpy.ok() and self.node:
             rclpy.spin_once(self.node, timeout_sec=0.1)
 
-    def bound_positions(self, positions: List[int]) -> List[int]:
+    def _o12_feedback_callback(self, msg: JointState):
+        """Forward 12-channel O12 radian feedback safely into the Qt thread."""
+        if len(msg.position) != 12:
+            self.status_updated.emit(
+                "error", f"O12位置反馈长度错误: {len(msg.position)}，应为12"
+            )
+            return
+        self.feedback_updated.emit([float(value) for value in msg.position])
+
+    def command_bounds(self, count: int):
+        if self.hand_joint == "O12":
+            bounds = _O12_COMMAND_BOUNDS_MRAD.get(self.hand_type)
+        else:
             bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
-        if not bounds or len(bounds) != len(positions):
-            return list(positions)
+        if not bounds or len(bounds) != count:
+            return [(0, 255)] * count
+        return bounds
+
+    def bound_positions(self, positions: List[int]) -> List[int]:
+        bounds = self.command_bounds(len(positions))
         return [
             max(minimum, min(maximum, int(value)))
             for value, (minimum, maximum) in zip(positions, bounds)
         ]
 
+    def wire_positions(self, positions: List[int]) -> List[float]:
+        positions = self.bound_positions(positions)
+        scale = _HAND_CONFIGS[self.hand_joint].position_scale
+        return [float(value) / float(scale) for value in positions]
+
     def publish_joint_state(self, positions: List[int]):
         """发布关节状态消息"""
         if not self.publisher or not self.node:
@@ -153,15 +219,21 @@ class ROS2NodeManager(QObject):
             
         try:
             positions = self.bound_positions(positions)
+            wire_positions = self.wire_positions(positions)
             self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
-            self.joint_state.position = [float(pos) for pos in positions]
+            self.joint_state.position = wire_positions
             # self.joint_state.velocity = [0.1] * len(positions)
             # self.joint_state.effort = [0.01] * len(positions)
             # 如果有关节名称，添加到消息中
             #hand_config = HandConfig.from_hand_type(self.hand_joint)
             hand_config = _HAND_CONFIGS[self.hand_joint]
             canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
-            if canonical_names and len(canonical_names) == len(positions):
+            # O12 SDK 1.1.8 may expose names in physical motor order while the
+            # position array uses active-angle order.  An empty name array makes
+            # the documented position order unambiguous to the driver.
+            if self.hand_joint == "O12":
+                self.joint_state.name = []
+            elif canonical_names and len(canonical_names) == len(positions):
                 self.joint_state.name = canonical_names
             elif len(hand_config.joint_names) == len(positions):
                 if hand_config.joint_names_en != None:
@@ -170,7 +242,7 @@ class ROS2NodeManager(QObject):
                     self.joint_state.name = hand_config.joint_names
                 
             self.publisher.publish(self.joint_state)
-            if self.is_arc == True:
+            if self.is_arc == True and self.hand_joint != "O12":
                 if self.hand_joint == "O6":
                     if self.hand_type == "left":
                         pose = range_to_arc_left(positions,self.hand_joint)
@@ -205,14 +277,21 @@ class ROS2NodeManager(QObject):
         positions = self.bound_positions(positions)
         self.publish_joint_state(positions)
         self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
-        self.joint_state.position = [float(pos) for pos in positions]
+        self.joint_state.position = self.wire_positions(positions)
         canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
-        if canonical_names:
+        if canonical_names and self.hand_joint != "O12":
             self.joint_state.name = canonical_names
+        elif self.hand_joint == "O12":
+            self.joint_state.name = []
         self.snapshot_publisher.publish(self.joint_state)
         self.status_updated.emit("info", "当前标定姿势快照已发布")
 
     def publish_speed(self, val: int):
+        if self.hand_joint == "O12":
+            self.status_updated.emit(
+                "warning", "O12不使用旧版u8速度接口；请通过角度轨迹限制速度"
+            )
+            return
         if self.hand_joint.upper() in ("O6", "L6"):
             joint_len = 6
         elif self.hand_joint == "L7":
@@ -232,6 +311,11 @@ class ROS2NodeManager(QObject):
         self.speed_pub.publish(msg)
 
     def publish_torque(self, val: int):
+        if self.hand_joint == "O12":
+            self.status_updated.emit(
+                "warning", "O12不使用旧版u8扭矩接口；位置GUI已禁用该设置"
+            )
+            return
         if self.hand_joint.upper() in ("O6", "L6"):
             joint_len = 6
         elif self.hand_joint == "L7":
@@ -273,11 +357,17 @@ class HandControlGUI(QWidget):
         # 设置ROS管理器
         self.ros_manager = ros_manager
         self.ros_manager.status_updated.connect(self.update_status)
+        self.ros_manager.feedback_updated.connect(self.on_feedback_updated)
         
         # 获取手部配置
         self.hand_joint = self.ros_manager.hand_joint
         self.hand_type = self.ros_manager.hand_type
         self.hand_config = _HAND_CONFIGS[self.hand_joint]
+        self.preset_actions = self.hand_config.get_preset_actions(self.hand_type)
+        self.feedback_positions = None
+        # O12 only publishes after an explicit slider/preset action.  This
+        # prevents opening or closing the real hand merely by launching GUI.
+        self.command_dirty = self.hand_joint != "O12"
         
         # 初始化UI
         self.init_ui()
@@ -288,6 +378,20 @@ class HandControlGUI(QWidget):
         self.publish_timer.timeout.connect(self.publish_joint_state)
         self.publish_timer.start()
 
+    def format_position(self, value: int) -> str:
+        """Format an integer GUI value in the hand's actual command unit."""
+        scale = self.hand_config.position_scale
+        if self.hand_config.position_unit == "rad":
+            radians = float(value) / float(scale)
+            return f"{radians:.3f} rad / {math.degrees(radians):.1f}°"
+        return str(value)
+
+    def command_positions(self) -> List[float]:
+        """Return current slider values converted to wire units."""
+        return self.ros_manager.wire_positions(
+            [slider.value() for slider in self.sliders]
+        )
+
     def init_ui(self):
         """初始化用户界面"""
         # 设置窗口属性
@@ -465,13 +569,11 @@ class HandControlGUI(QWidget):
         for i, (name, value) in enumerate(zip(
             self.hand_config.joint_names, self.hand_config.init_pos
         )):
-            bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
-            minimum, maximum = (
-                bounds[i] if bounds and i < len(bounds) else (0, 255)
-            )
+            bounds = self.ros_manager.command_bounds(len(self.hand_config.init_pos))
+            minimum, maximum = bounds[i]
             value = max(minimum, min(maximum, int(value)))
             # 创建标签
-            label = QLabel(f"{name}: {value}")
+            label = QLabel(f"{name}: {self.format_position(value)}")
             label.setMinimumWidth(120)
             
             # 创建滑动条
@@ -535,9 +637,9 @@ class HandControlGUI(QWidget):
     def create_system_preset_buttons(self, parent_layout):
         """创建系统预设动作按钮"""
         self.preset_buttons = []  # 清空按钮列表
-        if self.hand_config.preset_actions:
+        if self.preset_actions:
             buttons = []
-            for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()):
+            for idx, (name, positions) in enumerate(self.preset_actions.items()):
                 button = QPushButton(name)
                 button.setProperty("category", "preset")
                 button.clicked.connect(
@@ -564,6 +666,9 @@ class HandControlGUI(QWidget):
 
         # —— 2. 新增：速度与扭矩设置（每行一个）——
         quick_set_gb = QGroupBox("快速设置")
+        if self.hand_joint == "O12":
+            quick_set_gb.setEnabled(False)
+            quick_set_gb.setToolTip("O12不使用旧版0-255速度/扭矩设置接口")
         qv_layout = QVBoxLayout(quick_set_gb)
 
         # 速度行
@@ -692,7 +797,10 @@ class HandControlGUI(QWidget):
         """滑动条值改变事件处理"""
         if 0 <= index < len(self.slider_labels):
             joint_name = self.hand_config.joint_names[index]
-            self.slider_labels[index].setText(f"{joint_name}: {value}")
+            self.slider_labels[index].setText(
+                f"{joint_name}: {self.format_position(value)}"
+            )
+        self.command_dirty = True
             
         # 更新数值显示
         self.update_value_display()
@@ -700,10 +808,23 @@ class HandControlGUI(QWidget):
     def update_value_display(self):
         """更新数值显示面板内容"""
         # 获取所有滑动条的当前值
-        values = [slider.value() for slider in self.sliders]
+        command = self.command_positions()
+        command_text = ", ".join(f"{value:.3f}" for value in command)
+        lines = [f"目标({self.hand_config.position_unit}): [{command_text}]"]
+        if self.feedback_positions is not None:
+            feedback_text = ", ".join(
+                f"{value:.3f}" for value in self.feedback_positions
+            )
+            lines.append(f"反馈(rad): [{feedback_text}]")
+        self.value_display.setText("\n".join(lines))
 
-        # 格式化显示为列表形式
-        self.value_display.setText(f"{values}")
+    def on_feedback_updated(self, positions):
+        """Display actual O12 joint feedback without moving command sliders."""
+        self.feedback_positions = list(positions)
+        self.update_value_display()
+        if hasattr(self, "connection_status"):
+            self.connection_status.setText("O12右手已连接并收到位置反馈")
+            self.connection_status.setObjectName("StatusInfo")
 
     def on_preset_action_clicked(self, positions: List[int]):
         """预设动作按钮点击事件处理"""
@@ -742,7 +863,25 @@ class HandControlGUI(QWidget):
             self.cycle_button.setText("循环运行预设动作")
             self.reset_preset_buttons_color()
             
-        self.status_updated.emit("warning", "已停止所有动作")
+        if self.hand_joint == "O12" and self.feedback_positions is not None:
+            # Hold the latest measured pose instead of merely stopping topic
+            # publication while a previous position target is still active.
+            scale = self.hand_config.position_scale
+            hold_values = [
+                int(round(value * scale)) for value in self.feedback_positions
+            ]
+            hold_values = self.ros_manager.bound_positions(hold_values)
+            for slider, value in zip(self.sliders, hold_values):
+                slider.blockSignals(True)
+                slider.setValue(value)
+                slider.blockSignals(False)
+            self.command_dirty = True
+            self.publish_joint_state()
+            self.update_value_display()
+            self.status_updated.emit("warning", "已下发当前反馈位置并保持")
+        else:
+            self.command_dirty = False
+            self.status_updated.emit("warning", "已停止连续动作和新的位置发布")
 
     def on_save_pose_clicked(self):
         """发布当前滑块姿势，供hand_pose_capture写入profile。"""
@@ -751,7 +890,7 @@ class HandControlGUI(QWidget):
 
     def on_cycle_clicked(self):
         """循环运行预设动作按钮点击事件处理"""
-        if not self.hand_config.preset_actions:
+        if not self.preset_actions:
             QMessageBox.warning(self, "无预设动作", "当前手部型号没有预设动作可循环运行")
             return
             
@@ -774,19 +913,19 @@ class HandControlGUI(QWidget):
 
     def run_next_action(self):
         """运行下一个预设动作"""
-        if not self.hand_config.preset_actions:
+        if not self.preset_actions:
             return
             
         # 重置所有按钮颜色
         self.reset_preset_buttons_color()
         
         # 计算下一个动作索引
-        self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions)
+        self.current_action_index = (self.current_action_index + 1) % len(self.preset_actions)
         
         # 获取下一个动作
-        action_names = list(self.hand_config.preset_actions.keys())
+        action_names = list(self.preset_actions.keys())
         action_name = action_names[self.current_action_index]
-        action_positions = self.hand_config.preset_actions[action_name]
+        action_positions = self.preset_actions[action_name]
         
         # 执行动作
         self.on_preset_action_clicked(action_positions)
@@ -811,6 +950,7 @@ class HandControlGUI(QWidget):
         """关节类型改变事件处理"""
         self.hand_joint = joint_type
         self.hand_config = _HAND_CONFIGS[self.hand_joint]
+        self.preset_actions = self.hand_config.get_preset_actions(self.hand_type)
         
         # 更新手部信息
         info_text = f"""手部类型: {self.hand_type}
@@ -829,8 +969,11 @@ class HandControlGUI(QWidget):
 
     def publish_joint_state(self):
         """发布当前关节状态"""
+        if self.hand_joint == "O12" and not self.command_dirty:
+            return
         positions = [slider.value() for slider in self.sliders]
         self.ros_manager.publish_joint_state(positions)
+        self.command_dirty = False
 
     def update_status(self, status_type: str, message: str):
         """更新状态显示"""

src/gui_control/launch/o12_right_gui.launch.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+"""Start the HCAN-connected O12 right hand and its safe position GUI."""
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, TimerAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    o12_driver = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare("omnihand_node"),
+                "launch",
+                "omnihand_pro_2025_node.launch.py",
+            ])
+        )
+    )
+
+    o12_gui = Node(
+        package="gui_control",
+        executable="gui_control",
+        name="o12_right_gui_control",
+        output="screen",
+        parameters=[{
+            "hand_type": "right",
+            "hand_joint": "O12",
+            # Commands are coalesced, so dragging a slider sends at most 10 Hz.
+            "topic_hz": 10,
+            "is_arc": False,
+            "topic_prefix": "",
+        }],
+    )
+
+    return LaunchDescription([
+        o12_driver,
+        # Give the CANFD driver a moment to claim and initialise the adapter.
+        TimerAction(period=1.5, actions=[o12_gui]),
+    ])

src/gui_control/package.xml

@@ -13,6 +13,7 @@
   <test_depend>python3-pytest</test_depend>
 
   <exec_depend>linker_hand_ros2_sdk</exec_depend>
+  <exec_depend>omnihand_node</exec_depend>
 
   <export>
     <build_type>ament_python</build_type>

src/gui_control/test/test_l6_config.py

@@ -0,0 +1,12 @@
+from gui_control.config.constants import HAND_CONFIGS
+
+
+def test_l6_gui_uses_the_sdk_channel_order() -> None:
+    assert HAND_CONFIGS["L6"].joint_names_en == [
+        "thumb_cmc_pitch",
+        "thumb_cmc_roll",
+        "index_mcp_pitch",
+        "middle_mcp_pitch",
+        "ring_mcp_pitch",
+        "pinky_mcp_pitch",
+    ]

src/gui_control/test/test_o12_config.py

@@ -0,0 +1,16 @@
+from gui_control.config.constants import HAND_CONFIGS
+
+
+def test_o12_gui_uses_active_angle_order_and_milliradians() -> None:
+    config = HAND_CONFIGS["O12"]
+
+    assert config.joint_names_en == [
+        "thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
+        "index_abad", "index_mcp", "index_pip",
+        "middle_abad", "middle_mcp", "middle_pip",
+        "ring_mcp", "pinky_mcp",
+    ]
+    assert config.position_scale == 1000
+    assert config.position_unit == "rad"
+    assert len(config.init_pos) == 12
+    assert all(len(pose) == 12 for pose in config.preset_actions.values())

src/gui_control/test/test_o6_config.py

@@ -0,0 +1,18 @@
+from gui_control.config.constants import HAND_CONFIGS
+
+
+def test_o6_ok_preset_is_hand_specific() -> None:
+    config = HAND_CONFIGS["O6"]
+
+    assert config.get_preset_actions("left")["OK"] == [
+        95, 75, 116, 255, 255, 255,
+    ]
+    assert config.get_preset_actions("right")["OK"] == [
+        95, 83, 122, 255, 255, 255,
+    ]
+    assert config.get_preset_actions("left")["拇指对中指"] == [
+        88, 2, 255, 114, 255, 255,
+    ]
+    assert config.get_preset_actions("right")["拇指对中指"] == [
+        95, 8, 255, 114, 255, 255,
+    ]

src/linker_hand_ros2_sdk/launch/test.bak

@@ -1,31 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-def generate_launch_description():
-    return LaunchDescription([
-        Node(
-            package='linker_hand_ros2_sdk',
-            executable='linker_hand_sdk',
-            name='linker_hand_sdk_left',
-            output='screen',
-            parameters=[{
-                'hand_type': 'left',
-                'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
-                'is_touch': True, # 是否带有压力传感器
-                'can': 'can0', # 这里需要修改为实际的CAN总线名称
-            }],
-        ),
-
-        Node(
-            package='linker_hand_ros2_sdk',
-            executable='linker_hand_sdk',
-            name='linker_hand_sdk_right',
-            output='screen',
-            parameters=[{
-                'hand_type': 'right',
-                'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
-                'is_touch': True, # 是否带有压力传感器
-                'can': 'can0', # 这里需要修改为实际的CAN总线名称
-            }],
-        ),
-    ])

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py

@@ -1,3 +1,5 @@
+from collections import deque
+
 import can
 import time, sys
 import threading
@@ -57,6 +59,14 @@ class LinkerHandL6Can:
         self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
         self.is_lock = False
         self.version = None
+        # L6 replies to a six-byte 0x01 position command with an immediate
+        # byte-for-byte echo on the same CAN ID. A zero-payload 0x01 state
+        # query also replies on that ID, but with the measured positions.
+        # Keep the two transactions distinct so command echoes never enter
+        # the published feedback stream used by calibration.
+        self._position_echo_lock = threading.Lock()
+        self._pending_position_echoes = deque(maxlen=32)
+        self._position_echo_timeout_seconds = 0.02
         # Start the receiving thread
         self.running = True
         self.receive_thread = threading.Thread(target=self.receive_response)
@@ -111,6 +121,11 @@ class LinkerHandL6Can:
         frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
         data = [frame_property_value] + [int(val) for val in data_list]
         msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+        if frame_property_value == 0x01 and len(data_list) == 6:
+            with self._position_echo_lock:
+                self._pending_position_echoes.append(
+                    (time.monotonic(), tuple(int(value) for value in data_list))
+                )
         try:
             self.bus.send(msg)
         except can.CanError as e:
@@ -201,7 +216,23 @@ class LinkerHandL6Can:
             except:
                 return
             if frame_type == 0x01:   # 0x01
-                self.x01 = list(response_data)
+                response = tuple(int(value) for value in response_data)
+                now = time.monotonic()
+                is_position_echo = False
+                with self._position_echo_lock:
+                    while (
+                        self._pending_position_echoes
+                        and now - self._pending_position_echoes[0][0]
+                        > self._position_echo_timeout_seconds
+                    ):
+                        self._pending_position_echoes.popleft()
+                    for pending in tuple(self._pending_position_echoes):
+                        if pending[1] == response:
+                            self._pending_position_echoes.remove(pending)
+                            is_position_echo = True
+                            break
+                if not is_position_echo:
+                    self.x01 = list(response)
             elif frame_type == 0x02:    # 0x02
                 self.x02 = list(response_data)
             elif frame_type == 0x05: # Set speed
@@ -391,7 +422,10 @@ class LinkerHandL6Can:
         return self.x35
     
     def get_finger_order(self):
-        return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+        # L6 channel 1 is the physical CMC roll actuator.  Older SDK releases
+        # exposed the channel as ``thumb_cmc_yaw`` even though the wire order
+        # and mechanism have always been roll.
+        return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
 
     def show_fun_table(self):
         pass

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py

@@ -372,7 +372,7 @@ class LinkerHandL6RS485:
         return [0] * 6
 
     def get_finger_order(self):
-        return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+        return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
 
     # --------------------------------------------------
     # 便捷方法

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak

@@ -1,444 +0,0 @@
-#!/usr/bin/env python3
-import os
-import time
-from pymodbus.client import ModbusSerialClient
-from typing import List, Dict
-import numpy as np
-
-_INTERVAL = 0.006  # 8 ms
-
-class LinkerHandL6RS485:
-    """L6机械手 Modbus-RTU 控制类"""
-    
-    # 6个关节名称
-    JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch", 
-                   "middle_pitch", "ring_pitch", "little_pitch"]
-    
-    # 手指名称
-    FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"]
-    
-    def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
-        """
-        初始化L6机械手
-        hand_id: 右手0x27(39), 左手0x28(40)
-        modbus_port: 串口设备路径
-        baudrate: 波特率，固定115200
-        """
-        self.slave = hand_id
-        self.cli = ModbusSerialClient(
-            port=modbus_port, 
-            baudrate=baudrate,
-            bytesize=8, 
-            parity="N", 
-            stopbits=1,
-            timeout=0.05
-        )
-        # pymodbus 3.5.1 需要显式连接
-        self.connected = self.cli.connect()
-        if not self.connected:
-            raise ConnectionError(f"RS485连接失败，端口: {modbus_port}")
-
-    def _read_input_registers(self, address: int, count: int) -> List[int]:
-        """读取输入寄存器"""
-        time.sleep(_INTERVAL)
-        result = self.cli.read_input_registers(address=address, count=count, slave=self.slave)
-        if result.isError():
-            raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}")
-        return result.registers
-
-    def _write_register(self, address: int, value: int):
-        """写入单个寄存器"""
-        time.sleep(_INTERVAL)
-        result = self.cli.write_register(address=address, value=value, slave=self.slave)
-        if result.isError():
-            raise RuntimeError(f"写入寄存器失败: address={address}, value={value}")
-
-    def _write_registers(self, address: int, values: List[int]):
-        """写入多个寄存器"""
-        time.sleep(_INTERVAL)
-        result = self.cli.write_registers(address=address, values=values, slave=self.slave)
-        if result.isError():
-            raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}")
-
-    # --------------------------------------------------
-    # 基础读取接口
-    # --------------------------------------------------
-    
-    def read_angles(self) -> List[int]:
-        """读取6个关节角度 (输入寄存器 0-5)"""
-        return self._read_input_registers(0, 6)
-
-    def read_torques(self) -> List[int]:
-        """读取6个关节转矩 (输入寄存器 6-11)"""
-        return self._read_input_registers(6, 6)
-
-    def read_speeds(self) -> List[int]:
-        """读取6个关节速度 (输入寄存器 12-17)"""
-        return self._read_input_registers(12, 6)
-
-    def read_temperatures(self) -> List[int]:
-        """读取6个关节温度 (输入寄存器 18-23)"""
-        return self._read_input_registers(18, 6)
-
-    def read_error_codes(self) -> List[int]:
-        """读取6个关节错误码 (输入寄存器 24-29)"""
-        return self._read_input_registers(24, 6)
-
-    # --------------------------------------------------
-    # 压力传感器接口
-    # --------------------------------------------------
-    
-    # def _pressure(self, finger: int) -> List[int]:
-    #     """内部：选手指 → 读压力数据"""
-    #     # 选择手指 (保持寄存器 36)
-    #     self._write_register(36, finger)
-    #     time.sleep(_INTERVAL)
-    #     # 读取压力数据 (输入寄存器 52-122)
-    #     return np.array(self._read_input_registers(52, 71))
-    def _pressure(self, finger: int) -> np.ndarray:
-        """
-        6x12 (72点) 矩阵尺寸。
-        Modbus 地址 60/62。
-        """
-        rows = 12  # 12 行
-        cols = 6   # 6 列
-        finger_size = rows * cols  # 72 个数据点
-        
-        # modbus 地址和计数
-        write_address = 60  # 写入手指选择
-        read_address = 62   # 读取压力数据
-        read_count = 96     # 读取 96 个寄存器
-        skip_count = 10     # 跳过前 10 个校验点
-        
-        # 0. 参数校验和手指写入值确定
-        if finger < 1 or finger > 5:
-            raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
-            
-        finger_write_value = finger 
-        
-        # 1. 写入手指选择寄存器 (地址 60)
-        time.sleep(0.008)
-        wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
-        if wrsp.isError():
-            raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
-
-        # 写入后等待片刻
-        time.sleep(0.008) 
-        
-        # 2. 读取地址 62 的数据
-        rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
-        
-        if rrsp.isError():
-            raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
-            
-        registers_16bit: List[int] = rrsp.registers 
-        
-        # 3. 核心数据处理
-        # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
-        final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
-        
-        # b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点)
-        effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8)
-        # c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点)
-        start_idx = 0 
-        end_idx = finger_size  # 72
-        
-        finger_data_flat = effective_data[start_idx:end_idx]
-        
-        # d. 验证数据长度
-        if finger_data_flat.size != finger_size:
-            raise ValueError(
-                f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
-                f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。"
-            )
-            
-        # e. 重塑为二维矩阵 (12 行 6 列)
-        finger_matrix = finger_data_flat.reshape((rows, cols))
-                
-        return finger_matrix
-
-    def read_pressure_thumb(self) -> np.ndarray:
-        """读取大拇指压力数据"""
-        return np.array(self._pressure(1), dtype=np.uint8)
-
-    def read_pressure_index(self) -> np.ndarray:
-        """读取食指压力数据"""
-        return np.array(self._pressure(2), dtype=np.uint8)
-
-    def read_pressure_middle(self) -> np.ndarray:
-        """读取中指压力数据"""
-        return np.array(self._pressure(3), dtype=np.uint8)
-
-    def read_pressure_ring(self) -> np.ndarray:
-        """读取无名指压力数据"""
-        return np.array(self._pressure(4), dtype=np.uint8)
-
-    def read_pressure_little(self) -> np.ndarray:
-        """读取小拇指压力数据"""
-        return np.array(self._pressure(5), dtype=np.uint8)
-
-    # --------------------------------------------------
-    # 版本信息接口
-    # --------------------------------------------------
-    
-    def read_versions(self) -> Dict[str, int]:
-        """读取版本信息 (输入寄存器 148-155)"""
-        result = self._read_input_registers(148, 8)
-        
-        return {
-            "hand_freedom": result[0],
-            "hand_version": result[1],
-            "hand_number": result[2],
-            "hand_direction": result[3],
-            "software_version_major": result[4],
-            "software_version_minor": result[5] if len(result) > 5 else 0,
-            "software_version_revision": result[6] if len(result) > 6 else 0,
-            "hardware_version": result[7] if len(result) > 7 else 0
-        }
-
-    # --------------------------------------------------
-    # 写入接口
-    # --------------------------------------------------
-    
-    def write_angles(self, vals: List[int]):
-        """设置6个关节角度 (保持寄存器 0-5)"""
-        vals = [int(x) for x in vals]
-        if not self.is_valid_6xuint8(vals):
-            raise ValueError("需要6个0-255的整数")
-        self._write_registers(0, vals)
-
-    def write_torques(self, vals: List[int]):
-        """设置6个关节转矩 (保持寄存器 6-11)"""
-        vals = [int(x) for x in vals]
-        if not self.is_valid_6xuint8(vals):
-            raise ValueError("需要6个0-255的整数")
-        self._write_registers(6, vals)
-
-    def write_speeds(self, vals: List[int]):
-        """设置6个关节速度 (保持寄存器 12-17)"""
-        vals = [int(x) for x in vals]
-        if not self.is_valid_6xuint8(vals):
-            raise ValueError("需要6个0-255的整数")
-        self._write_registers(12, vals)
-
-    # --------------------------------------------------
-    # 上下文管理
-    # --------------------------------------------------
-    
-    def close(self):
-        """关闭连接"""
-        if self.connected:
-            self.cli.close()
-            self.connected = False
-    
-    def __enter__(self):
-        return self
-    
-    def __exit__(self, exc_type, exc_val, exc_tb):
-        self.close()
-
-    # --------------------------------------------------
-    # API固定接口函数
-    # --------------------------------------------------
-    
-    def is_valid_6xuint8(self, lst) -> bool:
-        """验证6个0-255的整数列表"""
-        if len(lst) != 6:
-            return False
-        return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
-    
-    def set_joint_positions(self, joint_angles=None):
-        """设置关节位置"""
-        joint_angles = joint_angles or [0] * 6
-        self.write_angles(joint_angles)
-
-    def set_speed(self, speed=None):
-        """设置速度"""
-        speed = speed or [200] * 6
-        self.write_speeds(speed)
-    
-    def set_torque(self, torque=None):
-        """设置扭矩"""
-        torque = torque or [200] * 6
-        self.write_torques(torque)
-
-    def set_current(self, current=None):
-        """设置电流 (L6不支持)"""
-        print("当前L6不支持设置电流", flush=True)
-
-    def get_version(self) -> list:
-        """获取版本信息"""
-        versions = self.read_versions()
-        return [
-            versions.get("hand_freedom", 0),
-            versions.get("hand_version", 0),
-            versions.get("hand_number", 0),
-            versions.get("hand_direction", 0),
-            versions.get("software_version_major", 0),
-            versions.get("hardware_version", 0)
-        ]
-
-    def get_current(self):
-        """获取电流 (L6不支持)"""
-        print("当前L6不支持获取电流", flush=True)
-        return []
-
-    def get_state(self) -> list:
-        """获取关节状态"""
-        return self.read_angles()
-    
-    def get_state_for_pub(self) -> list:
-        return self.get_state()
-
-    def get_current_status(self) -> list:
-        return self.get_state()
-    
-    def get_speed(self) -> list:
-        """获取当前速度"""
-        return self.read_speeds()
-    
-    def get_joint_speed(self) -> list:
-        return self.get_speed()
-    
-    def get_touch_type(self) -> int:
-        """获取压感类型 (2=矩阵式)"""
-        return 2
-    
-    def get_normal_force(self) -> list:
-        """获取压感数据：点式"""
-        return [-1] * 5
-    
-    def get_tangential_force(self) -> list:
-        """获取压感数据：点式"""
-        return [-1] * 5
-    
-    def get_approach_inc(self) -> list:
-        """获取压感数据：点式"""
-        return [-1] * 5
-    
-    def get_touch(self) -> list:
-        return [-1] * 5
-
-    def get_thumb_matrix_touch(self,sleep_time=0):
-        return self._pressure(1)
-
-    def get_index_matrix_touch(self,sleep_time=0):
-        return self._pressure(2)
-
-    def get_middle_matrix_touch(self,sleep_time=0):
-        return self._pressure(3)
-
-    def get_ring_matrix_touch(self,sleep_time=0):
-        return self._pressure(4)
-
-    def get_little_matrix_touch(self,sleep_time=0):
-        return self._pressure(5)
-        
-    def get_matrix_touch(self) -> list:
-        """获取压感数据：矩阵式"""
-        return [self._pressure(1), self._pressure(2), self._pressure(3), 
-                self._pressure(4), self._pressure(5)]
-    
-    def get_matrix_touch_v2(self) -> list:
-        """获取压感数据：矩阵式"""
-        return self.get_matrix_touch()
-    
-    def get_torque(self) -> list:
-        """获取当前扭矩"""
-        return self.read_torques()
-    
-    def get_temperature(self) -> list:
-        """获取当前电机温度"""
-        return self.read_temperatures()
-    
-    def get_fault(self) -> list:
-        """获取当前电机故障码"""
-        return self.read_error_codes()
-    
-    def get_serial_number(self):
-        return [0] * 6
-
-    # --------------------------------------------------
-    # 便捷方法
-    # --------------------------------------------------
-    
-    def relax(self):
-        """所有手指伸直"""
-        self.set_joint_positions([255] * 6)
-    
-    def fist(self):
-        """所有手指握拳"""
-        self.set_joint_positions([0] * 6)
-    
-    def dump_status(self):
-        """打印状态信息"""
-        print("=" * 50)
-        print("L6机械手状态信息")
-        print("=" * 50)
-        
-        try:
-            # 关节状态
-            angles = self.read_angles()
-            torques = self.read_torques()
-            speeds = self.read_speeds()
-            temps = self.read_temperatures()
-            errors = self.read_error_codes()
-            
-            print("关节状态:")
-            for i, name in enumerate(self.JOINT_NAMES):
-                print(f"  {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, "
-                      f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}")
-            
-            # 版本信息
-            versions = self.read_versions()
-            print("\n版本信息:")
-            for key, value in versions.items():
-                print(f"  {key:20s}: {value}")
-            
-            # 压力传感器测试
-            print("\n压力传感器测试:")
-            thumb_pressure = self.read_pressure_thumb()
-            print(f"大拇指压力数据长度: {len(thumb_pressure)}")
-            
-        except Exception as e:
-            print(f"读取状态时出错: {e}")
-        
-        print("=" * 50)
-
-
-# ------------------- 演示程序 -------------------
-if __name__ == "__main__":
-    # 使用示例
-    try:
-        with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
-            print("连接成功!")
-            
-            # 打印状态信息
-            hand.dump_status()
-            
-            # 测试基本控制
-            print("\n测试控制功能...")
-            print("伸直手指...")
-            hand.relax()
-            time.sleep(2)
-            
-            print("握拳...")
-            hand.fist()
-            time.sleep(2)
-            
-            print("恢复伸直...")
-            hand.relax()
-            
-            # 测试压力传感器
-            print("\n测试压力传感器...")
-            thumb_matrix = hand.get_thumb_matrix_touch()
-            print(f"大拇指压力数据: {len(thumb_matrix)}个点")
-            
-            # 获取所有手指压力数据
-            all_matrices = hand.get_matrix_touch()
-            for i, name in enumerate(hand.FINGER_NAMES):
-                matrix = all_matrices[i]
-                print(f"{name}手指压力数据长度: {len(matrix)}")
-            
-    except Exception as e:
-        print(f"错误: {e}")

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak

@@ -1,414 +0,0 @@
-#!/usr/bin/env python3 
-# -*- coding: utf-8 -*-
-'''
-编译: colcon build --symlink-install
-启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
-'''
-from re import A
-import rclpy,sys                                     # ROS2 Python接口库
-import time
-import numpy as np
-from rclpy.node import Node                      # ROS2 节点类
-from rclpy.clock import Clock
-from std_msgs.msg import String, Header, Float32MultiArray
-from sensor_msgs.msg import JointState, PointCloud2, PointField
-import time, json, threading
-from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
-from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
-from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
-
-
-class LinkerHand(Node):
-    def __init__(self, name):
-        super().__init__(name)
-        # 声明参数（带默认值）
-        self.declare_parameter('hand_type', 'left')
-        self.declare_parameter('hand_joint', 'L6')
-        self.declare_parameter('is_touch', False)
-        self.declare_parameter('can', 'can0')
-        self.declare_parameter('modbus', "None")        
-
-        # ros时间获取
-        self.stamp_clock = Clock()
-        # 获取参数值
-        self.hand_type = self.get_parameter('hand_type').value
-        self.hand_joint = self.get_parameter('hand_joint').value
-        self.is_touch = self.get_parameter('is_touch').value
-        self.can = self.get_parameter('can').value
-        self.modbus = self.get_parameter('modbus').value
-        self.sdk_v = 2
-        self.sleep_time = 0.005
-        self.cmd_lock = False
-        self.last_hand_post_cmd = None # 最新手指位置命令
-        self.last_hand_vel_cmd = None # 最新手指速度命令
-        self.last_hand_eff_cmd = None # 最新手指力矩命令
-
-        self.last_hand_state = [-1] * 10
-        self.last_hand_vel = [-1] * 10
-        self.force = [[-1] * 5] * 4
-        self.matrix_dic = {
-            "stamp":{
-                "sec": 0,
-                "nanosec": 0,
-            },
-            "thumb_matrix":[[-1] * 6 for _ in range(12)],
-            "index_matrix":[[-1] * 6 for _ in range(12)],
-            "middle_matrix":[[-1] * 6 for _ in range(12)],
-            "ring_matrix":[[-1] * 6 for _ in range(12)],
-            "little_matrix":[[-1] * 6 for _ in range(12)]
-        }
-        # 压感矩阵合值，单位g 克
-        self.matrix_mass_dic = {
-            "stamp":{
-                "secs": 0,
-                "nsecs": 0,
-            },
-            "thumb_mass":[-1],
-            "index_mass":[-1],
-            "middle_mass":[-1],
-            "ring_mass":[-1],
-            "little_mass":[-1]
-        }
-        self.last_hand_info = {
-            "version": [-1], # Dexterous hand version number
-            "hand_joint": self.hand_joint, # Dexterous hand joint type
-            "speed": [-1] * 10, # Current speed threshold of the dexterous hand
-            "current": [-1] * 10, # Current of the dexterous hand
-            "fault": [-1] * 10, # Current fault of the dexterous hand
-            "motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand
-            "torque": [-1] * 10, # Current torque of the dexterous hand
-            "is_touch":self.is_touch,
-            "touch_type": -1,
-            "finger_order": None # Finger motor order
-        }
-        self.version = []
-        self.touch_type = -1
-        self.hz = 1.0/60.0
-
-        self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10)
-        self._init_hand()
-        time.sleep(1)
-        self.run_count = 0 # 计数器，用于记录运行次数
-        self.timer = self.create_timer(0.01, self.run)  # 100 Hz
-        self.thread_pub_state = threading.Thread(target=self.pub_state)
-        self.thread_pub_state.daemon = True
-        self.thread_pub_state.start()
-
-    def _init_hand(self):
-        self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
-        time.sleep(0.1)
-        self.touch_type = self.api.get_touch_type()
-        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
-        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
-        self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10)
-        if self.is_touch == True:
-            if self.touch_type > 1:
-                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
-                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
-                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
-                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
-            elif self.touch_type != -1:
-                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
-                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
-            else:
-                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
-                self.is_touch = False
-        self.embedded_version = self.api.get_embedded_version()
-        pose = None
-        torque = [200, 200, 200, 200, 200]
-        speed = [200, 250, 250, 250, 250]
-        if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P":
-            pose = [200, 255, 255, 255, 255, 180]
-            torque = [250, 250, 250, 250, 250, 250]
-            # O6 最大速度阈值
-            speed = [200, 250, 250, 250, 250, 250]
-        elif self.hand_joint == "L7":
-            # The data length of L7 is 7, reinitialize here
-            pose = [255, 200, 255, 255, 255, 255, 180]
-            torque = [250, 250, 250, 250, 250, 250, 250]
-            speed = [120, 250, 250, 250, 250, 250, 250]
-        elif self.hand_joint == "L10":
-            torque = [255] * 10
-            pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
-            speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
-        elif self.hand_joint == "L20":
-            pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
-        elif self.hand_joint == "L21":
-            pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
-        elif self.hand_joint == "L25":
-            pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
-        if pose is not None:
-            for i in range(1): 
-                self.api.set_speed(speed=speed)
-                time.sleep(0.1)
-                self.api.set_torque(torque=torque)
-                time.sleep(0.1)
-                self.api.finger_move(pose=pose)
-                time.sleep(0.1)
-
-    def list_check(self,pose):
-        if isinstance(pose, list) == False:
-            return False
-        if len(self.last_hand_post_cmd) != len(pose):
-            return False
-        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
-
-    def hand_control_cb(self, msg):
-        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
-            self.last_hand_post_cmd = msg.position
-        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
-            self.last_hand_vel_cmd = msg.velocity
-        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
-            self.last_hand_eff_cmd = msg.effort
-
-    def run(self):
-        if self.sdk_v == 1:
-            self.sleep_time = 0.009
-        if self.hand_state_pub.get_subscription_count() > 0:
-            # 优先获取手指状态并且发布
-            self.last_hand_state = self.api.get_state()
-            time.sleep(0.003)
-            self.last_hand_vel = self.api.get_joint_speed()
-            time.sleep(0.002)
-        if self.cmd_lock == False:
-            if self.last_hand_post_cmd != None:
-                self.api.finger_move(pose=self.last_hand_post_cmd)
-                self.last_hand_post_cmd = None
-            if self.last_hand_vel_cmd != None:
-                vel = list(self.last_hand_vel_cmd)
-                if all(x == 0 for x in vel):
-                    pass
-                else:
-                    if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L7" and len(vel) == 7:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L10" and len(vel) == 10:
-                        speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L20" and len(vel) == 20:
-                        speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L21" and len(vel) == 25:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L25" and len(vel) == 25:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                self.last_hand_vel_cmd = None
-            time.sleep(0.003)
-            if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
-                """单点式压力传感器"""
-                self.force = self.api.get_force()
-            if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
-                """矩阵式压力传感器"""
-                if self.run_count == 3:
-                    self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 4:
-                    self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 5:
-                    self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 6:
-                    self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist()
-                if self.run_count == 7:
-                    self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
-                time.sleep(0.005)
-            if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
-                """手部信息"""
-                self.last_hand_info = {
-                    "version": self.embedded_version, # Dexterous hand version number
-                    "hand_joint": self.hand_joint, # Dexterous hand joint type
-                    "speed": self.api.get_speed(), # Current speed threshold of the dexterous hand
-                    "current": self.api.get_current(), # Current of the dexterous hand
-                    "fault": self.api.get_fault(), # Current fault of the dexterous hand
-                    "motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand
-                    "torque": self.api.get_torque(), # Current torque of the dexterous hand
-                    "is_touch":self.is_touch,
-                    "touch_type": self.touch_type,
-                    "finger_order": self.api.get_finger_order() # Finger motor order
-                }
-            if self.run_count == 9:
-                self.run_count = 0
-            self.run_count += 1
-            time.sleep(0.003)
-
-
-    def pub_state(self):
-        while True:
-            if self.hand_state_pub.get_subscription_count() > 0:
-                msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
-                self.hand_state_pub.publish(msg)
-            if self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
-                msg = Float32MultiArray()
-                msg.data = [float(val) for sublist in self.force for val in sublist]
-                self.touch_pub.publish(msg)
-            if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
-                # 发布矩阵压感数据JSON格式
-                self.pub_matrix_dic()
-                # 发布矩阵压感和值JSON格式
-                self.pub_matrix_mass(dic=self.matrix_dic)
-                # 发布矩阵压感点云格式
-                self.pub_matrix_point_cloud()
-            if self.hand_info_pub.get_subscription_count() > 0:
-                msg = String()
-                msg.data = json.dumps(self.last_hand_info)
-                self.hand_info_pub.publish(msg)
-            time.sleep(self.hz)
-
-    def pub_matrix_mass(self, dic):
-        """发布矩阵数据合值 单位g 克 JSON格式"""
-        msg = String()
-        # 获取当前的 ROS 时间
-        current_time = self.stamp_clock.now()
-        # 提取 secs 和 nsecs
-        t_secs = current_time.to_msg().sec
-        t_nsecs = current_time.to_msg().nanosec
-        self.matrix_mass_dic["stamp"]["secs"] = t_secs
-        self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
-        self.matrix_mass_dic["unit"] = "g"
-        self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
-        self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
-        self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
-        self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
-        self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
-        msg.data = json.dumps(self.matrix_mass_dic)
-        self.matrix_touch_mass_pub.publish(msg)
-
-    def pub_matrix_point_cloud(self):
-        """发布矩阵数据点云格式"""
-        tmp_dic = self.matrix_dic.copy()
-        del tmp_dic['stamp']               # 去掉时间戳字段
-        all_matrices = list(tmp_dic.values())  # 5 帧，每帧 6×12=72 个数
-        # 摊平到一维：360 个 float
-        flat_list = [v for frame in all_matrices for v in frame]  # 360
-        flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
-        fields = [PointField(
-            name='val',
-            offset=0,
-            datatype=PointField.UINT8,
-            count=1
-        )]
-        pc = PointCloud2()
-        pc.header.stamp = self.stamp_clock.now().to_msg()
-        pc.header.frame_id = ''
-        pc.height = 1
-        pc.width = flat.size         # 360
-        pc.fields = fields
-        pc.is_bigendian = False
-        pc.point_step = 1            # 1 个 float32
-        pc.row_step = pc.point_step * pc.width
-        pc.data = flat.tobytes()     # 1440 字节
-        self.matrix_touch_pub_pc.publish(pc)
-
-    def pub_matrix_dic(self):
-        """发布矩阵数据JSON格式"""
-        msg = String()
-        # 获取当前的 ROS 时间
-        current_time = self.stamp_clock.now()
-        # 提取 secs 和 nsecs
-        t_secs = current_time.to_msg().sec
-        t_nsecs = current_time.to_msg().nanosec
-        self.matrix_dic["stamp"]["secs"] = t_secs
-        self.matrix_dic["stamp"]["nsecs"] = t_nsecs
-        msg.data = json.dumps(self.matrix_dic)
-        self.matrix_touch_pub.publish(msg)
-
-    def joint_state_msg(self, pose,vel=[]):
-        joint_state = JointState()
-        joint_state.header = Header()
-        joint_state.header.stamp = self.get_clock().now().to_msg()
-        joint_state.name = self.api.get_finger_order()
-        joint_state.position = [float(x) for x in pose]
-        if len(vel) > 1:
-            joint_state.velocity = [float(x) for x in vel]
-        else:
-            joint_state.velocity = [0.0] * len(pose)
-        joint_state.effort = [0.0] * len(pose)
-        return joint_state
-    
-    
-
-    
-
-    def hand_setting_cb(self,msg):
-        '''控制命令回调'''
-        data = json.loads(msg.data)
-        print(f"Received setting command: {data['setting_cmd']}",flush=True)
-        try:
-            if data["params"]["hand_type"] == "left":
-                hand = self.api
-                hand_left = True
-            elif data["params"]["hand_type"] == "right":
-                hand = self.api
-                hand_right = True
-            else:
-                print("Please specify the hand part to be set",flush=True)
-                return
-            self.cmd_lock = True
-            # Set maximum torque
-            if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque
-                torque = list(data["params"]["torque"])
-                hand.set_torque(torque=torque)
-                
-            if data["setting_cmd"] == "set_speed": # Set speed
-                if isinstance(data["params"]["speed"], list) == True:
-                    speed = data["params"]["speed"]
-                    hand.set_speed(speed=speed)
-                else:
-                    ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red")
-            if data["setting_cmd"] == "clear_faults": # Clear faults
-                if hand_left == True and self.hand_joint == "L10" :
-                    ColorMsg(msg=f"L10 left hand cannot clear faults")
-                elif hand_right == True and self.hand_joint == "L10" :
-                    ColorMsg(msg=f"L10 right hand cannot clear faults")
-                else:
-                    hand.clear_faults()
-            if data["setting_cmd"] == "get_faults": # Get faults
-                f = hand.get_fault()
-                ColorMsg(msg=f"Get faults: {f}")
-            if data["setting_cmd"] == "electric_current": # Get current
-                ColorMsg(msg=f"Get current: {hand.get_current()}")
-            if data["setting_cmd"] == "set_electric_current": # Set current
-                if isinstance(data["params"]["current"], list) == True:
-                    hand.set_current(data["params"]["current"])
-            if data["setting_cmd"] == "show_fun_table": # Get faults
-                f = hand.show_fun_table()
-        except:
-            print("命令参数错误")
-            self.cmd_lock = False
-        finally:
-            self.cmd_lock = False
-
-
-    def close_can(self):
-        self.api.open_can.close_can(can=self.can)
-        sys.exit(0)
-
-        
-def main(args=None):
-    try:
-        rclpy.init(args=args)
-        node = LinkerHand("linker_hand_sdk")
-        embedded_version = node.embedded_version
-        if len(embedded_version) == 3 or node.hand_joint.upper() == "O6" or node.hand_joint.upper() == "L6" or node.hand_joint.upper() == "G20":
-            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
-            node.sdk_v = 2
-        elif len(embedded_version) == 6 and node.hand_joint == "L10":
-            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
-            node.sdk_v = 2
-        elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)):
-            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
-            node.sdk_v = 2
-        else:
-            ColorMsg(msg=f"SDK V1", color="green")
-            node.sdk_v = 1
-        rclpy.spin(node)         # 主循环，监听 ROS 回调
-    except KeyboardInterrupt:
-        print("收到 Ctrl+C，准备退出...")
-    finally:
-        # node.close_can()         # 关闭 CAN 或其他硬件资源
-        # node.destroy_node()      # 销毁 ROS 节点
-        # rclpy.shutdown()         # 关闭 ROS
-        print("程序已退出。")

src/linker_hand_ros2_sdk/test/test_l6_channel_order.py

@@ -0,0 +1,72 @@
+import ast
+from collections import deque
+from pathlib import Path
+import sys
+import threading
+import time
+from types import SimpleNamespace
+
+
+PACKAGE = Path(__file__).resolve().parents[1] / "linker_hand_ros2_sdk/LinkerHand/core"
+EXPECTED = [
+    "thumb_cmc_pitch",
+    "thumb_cmc_roll",
+    "index_mcp_pitch",
+    "middle_mcp_pitch",
+    "ring_mcp_pitch",
+    "pinky_mcp_pitch",
+]
+
+
+def _finger_order(path: Path, class_name: str) -> list[str]:
+    module = ast.parse(path.read_text(encoding="utf-8"))
+    selected = next(
+        item
+        for item in module.body
+        if isinstance(item, ast.ClassDef) and item.name == class_name
+    )
+    method = next(
+        item
+        for item in selected.body
+        if isinstance(item, ast.FunctionDef) and item.name == "get_finger_order"
+    )
+    returned = next(item for item in method.body if isinstance(item, ast.Return))
+    return ast.literal_eval(returned.value)
+
+
+def test_l6_can_and_rs485_publish_the_same_physical_channel_order() -> None:
+    assert _finger_order(PACKAGE / "can/linker_hand_l6_can.py", "LinkerHandL6Can") == EXPECTED
+    assert _finger_order(
+        PACKAGE / "rs485/linker_hand_l6_rs485.py", "LinkerHandL6RS485"
+    ) == EXPECTED
+
+
+def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None:
+    linker_hand_root = PACKAGE.parent
+    sys.path.insert(0, str(linker_hand_root))
+    try:
+        from core.can.linker_hand_l6_can import LinkerHandL6Can
+    finally:
+        sys.path.remove(str(linker_hand_root))
+
+    hand = LinkerHandL6Can.__new__(LinkerHandL6Can)
+    hand.can_id = 0x27
+    hand.x01 = [10, 20, 30, 40, 50, 60]
+    hand._position_echo_lock = threading.Lock()
+    command = (255, 2, 253, 253, 253, 253)
+    hand._pending_position_echoes = deque(
+        [(time.monotonic(), command)], maxlen=32
+    )
+    hand._position_echo_timeout_seconds = 0.02
+
+    hand.process_response(
+        SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command)))
+    )
+    assert hand.x01 == [10, 20, 30, 40, 50, 60]
+    assert not hand._pending_position_echoes
+
+    measured = (250, 3, 252, 252, 252, 252)
+    hand.process_response(
+        SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured)))
+    )
+    assert hand.x01 == list(measured)

src/linkerhand_calibration/config/calibration.yaml

@@ -44,7 +44,7 @@ g20_thumb_calibration:
     minimum_detection_hz: 15.0
     maximum_hamming: 0
     minimum_decision_margin: 30.0
-    # Trial threshold for the current 10 mm tags (observed at 32-38 px).
+    # Trial threshold for small/far tags (historically observed at 32-38 px).
     # Final acceptance is still guarded by static RMS and random validation.
     minimum_edge_pixels: 30.0
     # Current 30 px tags measure about 0.50-0.53 deg RMS while stationary.

src/linkerhand_calibration/config/fastdds_large_images.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<dds>
+  <profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
+    <transport_descriptors>
+      <transport_descriptor>
+        <transport_id>g20_udp_transport</transport_id>
+        <type>UDPv4</type>
+        <sendBufferSize>10485760</sendBufferSize>
+        <receiveBufferSize>10485760</receiveBufferSize>
+      </transport_descriptor>
+      <transport_descriptor>
+        <transport_id>g20_shm_transport</transport_id>
+        <type>SHM</type>
+        <segment_size>67108864</segment_size>
+        <port_queue_capacity>512</port_queue_capacity>
+        <healthy_check_timeout_ms>1000</healthy_check_timeout_ms>
+      </transport_descriptor>
+    </transport_descriptors>
+
+    <participant
+      profile_name="g20_large_image_participant"
+      is_default_profile="true">
+      <rtps>
+        <userTransports>
+          <transport_id>g20_udp_transport</transport_id>
+          <transport_id>g20_shm_transport</transport_id>
+        </userTransports>
+        <useBuiltinTransports>false</useBuiltinTransports>
+      </rtps>
+    </participant>
+  </profiles>
+</dds>

src/linkerhand_calibration/config/front_tags.yaml

@@ -6,7 +6,7 @@
     # from back-pressuring image_proc's reliable image publisher.
     qos_profile: sensor_data
     family: 36h11
-    size: 0.01
+    size: 0.016
     profile: false
     max_hamming: 0
     detector:
@@ -20,11 +20,11 @@
     tag:
       ids: [0, 1, 2, 3]
       frames: [tag_t0, tag_t3, tag_t4, tag_t5]
-      sizes: [0.010, 0.010, 0.010, 0.010]
+      sizes: [0.016, 0.016, 0.016, 0.016]
 
 g20_thumb_calibration:
   ros__parameters:
     tag_roles: [t0, t3, t4, t5]
     tag_ids: [0, 1, 2, 3]
     tag_frames: [tag_t0, tag_t3, tag_t4, tag_t5]
-    tag_sizes_m: [0.010, 0.010, 0.010, 0.010]
+    tag_sizes_m: [0.016, 0.016, 0.016, 0.016]

src/linkerhand_calibration/config/g20_right_product.yaml

@@ -0,0 +1,37 @@
+schema_version: 1
+model: G20
+side: right
+tag_layout: g20_right_19
+serial_number: G20_RIGHT_001
+can_interface: can0
+output_root: calibration_output
+
+cameras:
+  front:
+    serial_number: DB2163742
+    camera_name: hikrobot_front_DB2163742
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
+  side:
+    serial_number: DB2163749
+    camera_name: hikrobot_side_DB2163749
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
+  top:
+    serial_number: DB2163739
+    camera_name: hikrobot_top_DB2163739
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
+
+artifacts:
+  source_urdf: package://linkerhand_calibration/urdf/g20_right/linkerhand_g20_right.urdf
+  source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
+  camera_extrinsics: config/g20_three_camera_extrinsics.yaml
+  camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
+  calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
+  calibration_config_sha256: 0faaf891ebb616c4c8a3bb3052c48fa4b6c8aa0c5fdc5abaaa89f4fc29cca1c3
+  tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
+  tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
+
+release:
+  # Each task already contains three training cycles plus an isolated fourth
+  # holdout, so a second complete hardware session duplicates hours of motion.
+  required_independent_passes: 1
+  static_repeatability_deg: 1.0

src/linkerhand_calibration/config/l6_right_8_tags.yaml

@@ -0,0 +1,59 @@
+/l6_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2]
+      frames: [front_base, thumb_pitch, thumb_dip]
+      sizes: [0.016, 0.016, 0.016]
+
+/l6_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [3, 4, 5]
+      frames: [side_base, pinky_pitch, pinky_dip]
+      sizes: [0.016, 0.016, 0.016]
+
+/l6_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [6, 7]
+      frames: [top_base, thumb_roll]
+      sizes: [0.016, 0.016]

src/linkerhand_calibration/config/l6_right_product.yaml

@@ -0,0 +1,37 @@
+schema_version: 2
+profile_id: L6/right/l6_right_8/v1
+model: L6
+side: right
+tag_layout: l6_right_8
+namespace: /l6_calibration
+serial_number: L6_RIGHT_001
+can_interface: can0
+output_root: calibration_output
+
+cameras:
+  front:
+    serial_number: DB2163742
+    camera_name: hikrobot_front_DB2163742
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
+  side:
+    serial_number: DB2163749
+    camera_name: hikrobot_side_DB2163749
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
+  top:
+    serial_number: DB2163739
+    camera_name: hikrobot_top_DB2163739
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
+
+artifacts:
+  source_urdf: package://linkerhand_calibration/urdf/l6_right/linkerhand_l6v3.1_right.urdf
+  source_urdf_sha256: 298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666
+  camera_extrinsics: config/g20_three_camera_extrinsics.yaml
+  camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
+  calibration_config: package://linkerhand_calibration/config/l6_three_camera_calibration.yaml
+  calibration_config_sha256: 0934699c8225891e748deefef6791eb28355821b89aeadd1f7ff0b7f7b4d265f
+  tag_config: package://linkerhand_calibration/config/l6_right_8_tags.yaml
+  tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5
+
+release:
+  required_independent_passes: 1
+  static_repeatability_deg: 1.0

src/linkerhand_calibration/config/l6_three_camera_calibration.yaml

@@ -0,0 +1,67 @@
+l6_calibration:
+  ros__parameters:
+    command_topic: /l6/cb_right_hand_control_cmd
+    state_topic: /l6/cb_right_hand_state
+    setting_topic: /l6/cb_hand_setting_cmd
+    front_camera_info_topic: /l6_calibration/front/camera/camera_info
+    front_detections_topic: /l6_calibration/front/apriltag/detections
+    side_camera_info_topic: /l6_calibration/side/camera/camera_info
+    side_detections_topic: /l6_calibration/side/apriltag/detections
+    top_camera_info_topic: /l6_calibration/top/camera/camera_info
+    top_detections_topic: /l6_calibration/top/apriltag/detections
+
+    baseline_command_u8: [255, 255, 255, 255, 255, 255]
+    # L6_RIGHT_001 measured a 250->5 travel of only ~0.9 s at speed 10,
+    # which left fewer than 32 useful feedback bins. Speed 1 is still only a
+    # firmware ceiling: different L6 motors complete a full stroke in 0.7-1.3 s.
+    # A 100 Hz cosine trajectory therefore sets the actual, model-level pace.
+    preflight_speed_u8: 1
+    formal_speed_u8: 1
+    speed_settle_seconds: 0.2
+    command_trajectory_full_range_seconds: 6.0
+    torque_u8: 80
+    repetitions: 4
+    preflight_checkpoints_u8: [255, 127, 0]
+    tag_size_m: 0.016
+    tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7]
+    tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+    minimum_detection_rate: 0.95
+    # Per-Tag quality remains >=95%. With three independently detected Tags,
+    # the fully joined frame rate may be 0.95^3 ~= 85.7%.
+    minimum_joint_frame_rate: 0.85
+    minimum_feedback_hz: 25.0
+    maximum_state_image_skew_ms: 50.0
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 30.0
+    pnp_maximum_reprojection_error_px: 1.5
+    pnp_reprojection_tie_px: 1.5
+    pnp_maximum_pose_jump_deg: 35.0
+    pnp_maximum_translation_jump_m: 0.04
+    pnp_maximum_tag_tilt_deg: 75.0
+    pnp_tracker_reset_seconds: 5.0
+
+    minimum_sweep_frames: 40
+    minimum_state_span_u8: 240.0
+    minimum_sweep_bins: 32
+    maximum_bin_gap: 16
+    maximum_monotonic_correction_deg: 2.0
+    passive_maximum_monotonic_correction_deg: 3.0
+    maximum_validation_mae_deg: 1.0
+    maximum_validation_p95_deg: 2.0
+    maximum_validation_error_deg: 3.0
+    mimic_minimum_multiplier: 0.5
+    mimic_maximum_multiplier: 1.5
+    mimic_maximum_cycle_range: 0.03
+    mimic_maximum_residual_p95_deg: 2.0
+
+    endpoint_tolerance_u8: 2.0
+    endpoint_hold_seconds: 1.0
+    motor_stall_timeout_seconds: 2.0
+    position_timeout_seconds: 30.0
+    sweep_timeout_seconds: 90.0
+    automatic_sweep_retry_limit: 2
+    non_target_motion_tolerance_u8: 3.0
+    fixed_base_maximum_corner_drift_px: 2.0
+    fixed_base_movement_confirmation_frames: 5

src/linkerhand_calibration/config/o12_right_16_tags.yaml

@@ -0,0 +1,41 @@
+/o12_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false}
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3, 12, 13]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_dip, middle_roll, index_roll]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+/o12_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false}
+    pose_estimation_method: pnp
+    tag:
+      ids: [4, 5, 6, 7, 8, 9, 10, 11]
+      frames: [side_base, pinky_mcp, pinky_pip, pinky_dip, middle_pip, middle_dip, index_pip, index_dip]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+/o12_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false}
+    pose_estimation_method: pnp
+    tag:
+      ids: [14, 15]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/linkerhand_calibration/config/o12_right_product.yaml

@@ -0,0 +1,43 @@
+schema_version: 3
+profile_id: O12/right/o12_right_16/v1
+model: O12
+side: right
+tag_layout: o12_right_16
+namespace: /o12_calibration
+serial_number: O12_RIGHT_001
+output_root: calibration_output
+
+sdk:
+  driver: omnihand_pro_2025_node
+  transport: hcan
+  setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash
+  config: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/share/omnihand_node/config/omnihand_pro_2025_node.yaml
+  config_sha256: ca1791c822bf1e99f25db8806397c3f877100c9252fcce6c53a7a6b8cef7694a
+
+cameras:
+  front:
+    serial_number: DB2163742
+    camera_name: hikrobot_front_DB2163742
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
+  side:
+    serial_number: DB2163749
+    camera_name: hikrobot_side_DB2163749
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
+  top:
+    serial_number: DB2163739
+    camera_name: hikrobot_top_DB2163739
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
+
+artifacts:
+  source_urdf: package://linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703.urdf
+  source_urdf_sha256: 75b3c18992a3640d2d7d36719da5ff75428ded477b97d9cf29adbe43a6eb27eb
+  camera_extrinsics: config/g20_three_camera_extrinsics.yaml
+  camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
+  calibration_config: package://linkerhand_calibration/config/o12_three_camera_calibration.yaml
+  calibration_config_sha256: 9defe39a3f4b31c3764cbdcd9ee5b570da70e810215844515e8cc01e56414728
+  tag_config: package://linkerhand_calibration/config/o12_right_16_tags.yaml
+  tag_config_sha256: 41001c3afba74cc01eb524a75dc58561a37e9364fab029ea12d879156a008dab
+
+release:
+  required_independent_passes: 1
+  static_repeatability_deg: 1.0

src/linkerhand_calibration/config/o12_three_camera_calibration.yaml

@@ -0,0 +1,50 @@
+o12_calibration:
+  ros__parameters:
+    command_topic: /o12/right/joint_cmd
+    state_topic: /o12/right/joint_states
+    front_camera_info_topic: /o12_calibration/front/camera/camera_info
+    front_detections_topic: /o12_calibration/front/apriltag/detections
+    side_camera_info_topic: /o12_calibration/side/camera/camera_info
+    side_detections_topic: /o12_calibration/side/apriltag/detections
+    top_camera_info_topic: /o12_calibration/top/camera/camera_info
+    top_detections_topic: /o12_calibration/top/apriltag/detections
+
+    # O12 standard interface is radians. Raw 0..2000 mixed control is forbidden.
+    command_rate_hz: 20.0
+    repetitions: 4
+    tag_size_m: 0.016
+    tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
+    tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+    minimum_detection_rate: 0.95
+    minimum_joint_frame_rate: 0.85
+    minimum_feedback_hz: 15.0
+    maximum_state_image_skew_ms: 50.0
+    minimum_sweep_frames: 40
+    minimum_state_span_fraction: 0.90
+    minimum_sweep_bins: 32
+    maximum_bin_gap: 2
+    endpoint_tolerance_rad: 0.01
+    endpoint_hold_seconds: 1.0
+    motor_stall_timeout_seconds: 2.0
+    position_timeout_seconds: 60.0
+    sweep_timeout_seconds: 180.0
+    automatic_sweep_retry_limit: 2
+    non_target_motion_tolerance_rad: 0.015
+    maximum_temperature_c: 70
+    # 当前 O12 固件返回空温度报告。仍发起查询；5秒无结果后依赖已验证的
+    # joint_error_states bit1 过热保护，并在会话记录中明确标注降级。
+    temperature_report_required: false
+    temperature_fallback_after_seconds: 5.0
+    # 现场确认初始保守速度过慢；正式/点动速度提高到2倍，避让另有硬上限。
+    motion_speed_scale: 2.0
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 30.0
+    fixed_base_maximum_corner_drift_px: 2.0
+    fixed_base_movement_confirmation_frames: 5
+    pnp_maximum_reprojection_error_px: 1.5
+    pnp_reprojection_tie_px: 1.5
+    pnp_maximum_pose_jump_deg: 35.0
+    pnp_maximum_translation_jump_m: 0.04
+    pnp_maximum_tag_tilt_deg: 75.0
+    pnp_tracker_reset_seconds: 5.0

src/linkerhand_calibration/config/o6_right_8_tags.yaml

@@ -0,0 +1,59 @@
+/o6_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2]
+      frames: [front_base, thumb_pitch, thumb_ip]
+      sizes: [0.016, 0.016, 0.016]
+
+/o6_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [3, 4, 5]
+      frames: [side_base, pinky_pitch, pinky_dip]
+      sizes: [0.016, 0.016, 0.016]
+
+/o6_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [6, 7]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/linkerhand_calibration/config/o6_right_product.yaml

@@ -0,0 +1,37 @@
+schema_version: 2
+profile_id: O6/right/o6_right_8/v1
+model: O6
+side: right
+tag_layout: o6_right_8
+namespace: /o6_calibration
+serial_number: O6_RIGHT_001
+can_interface: can0
+output_root: calibration_output
+
+cameras:
+  front:
+    serial_number: DB2163742
+    camera_name: hikrobot_front_DB2163742
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
+  side:
+    serial_number: DB2163749
+    camera_name: hikrobot_side_DB2163749
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
+  top:
+    serial_number: DB2163739
+    camera_name: hikrobot_top_DB2163739
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
+
+artifacts:
+  source_urdf: package://linkerhand_calibration/urdf/o6_right/linkerhand_o6_right.urdf
+  source_urdf_sha256: 8f184faad699fbf771e388f109a4e8793b5cb190c33a87b2eba8491a3a37dd62
+  camera_extrinsics: config/o6_three_camera_extrinsics.yaml
+  camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4
+  calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml
+  calibration_config_sha256: ce20d998a4342dfaacb14568513aa9af5063df48566fabd42180acc8da47e4a6
+  tag_config: package://linkerhand_calibration/config/o6_right_8_tags.yaml
+  tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e
+
+release:
+  required_independent_passes: 1
+  static_repeatability_deg: 1.0

src/linkerhand_calibration/config/o6_three_camera_calibration.yaml

@@ -0,0 +1,64 @@
+o6_calibration:
+  ros__parameters:
+    command_topic: /o6/cb_right_hand_control_cmd
+    state_topic: /o6/cb_right_hand_state
+    setting_topic: /o6/cb_hand_setting_cmd
+    front_camera_info_topic: /o6_calibration/front/camera/camera_info
+    front_detections_topic: /o6_calibration/front/apriltag/detections
+    side_camera_info_topic: /o6_calibration/side/camera/camera_info
+    side_detections_topic: /o6_calibration/side/apriltag/detections
+    top_camera_info_topic: /o6_calibration/top/camera/camera_info
+    top_detections_topic: /o6_calibration/top/apriltag/detections
+
+    baseline_command_u8: [255, 255, 255, 255, 255, 255]
+    # O6 has a different speed scale from L6. Motion is still bounded by the
+    # six-second cosine command trajectory; these values are firmware limits.
+    baseline_speed_u8: 80
+    preflight_speed_u8: 60
+    formal_speed_u8: 40
+    speed_settle_seconds: 0.2
+    command_trajectory_full_range_seconds: 6.0
+    torque_u8: 80
+    repetitions: 4
+    preflight_checkpoints_u8: [255, 127, 0]
+    tag_size_m: 0.016
+    tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7]
+    tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+    minimum_detection_rate: 0.95
+    minimum_joint_frame_rate: 0.85
+    minimum_feedback_hz: 25.0
+    maximum_state_image_skew_ms: 50.0
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 30.0
+    pnp_maximum_reprojection_error_px: 1.5
+    pnp_reprojection_tie_px: 1.5
+    pnp_maximum_pose_jump_deg: 35.0
+    pnp_maximum_translation_jump_m: 0.04
+    pnp_maximum_tag_tilt_deg: 75.0
+    pnp_tracker_reset_seconds: 5.0
+
+    minimum_sweep_frames: 40
+    minimum_state_span_u8: 240.0
+    minimum_sweep_bins: 32
+    maximum_bin_gap: 16
+    maximum_monotonic_correction_deg: 2.0
+    passive_maximum_monotonic_correction_deg: 3.0
+    maximum_validation_mae_deg: 1.0
+    maximum_validation_p95_deg: 2.0
+    maximum_validation_error_deg: 3.0
+    mimic_minimum_multiplier: 0.5
+    mimic_maximum_multiplier: 2.2
+    mimic_maximum_cycle_range: 0.03
+    mimic_maximum_residual_p95_deg: 2.0
+
+    endpoint_tolerance_u8: 2.0
+    endpoint_hold_seconds: 1.0
+    motor_stall_timeout_seconds: 2.0
+    position_timeout_seconds: 30.0
+    sweep_timeout_seconds: 90.0
+    automatic_sweep_retry_limit: 2
+    non_target_motion_tolerance_u8: 3.0
+    fixed_base_maximum_corner_drift_px: 2.0
+    fixed_base_movement_confirmation_frames: 5

src/linkerhand_calibration/config/three_camera_calibration.yaml

@@ -0,0 +1,193 @@
+g20_calibration:
+  ros__parameters:
+    command_topic: /g20/cb_left_hand_control_cmd
+    state_topic: /g20/cb_left_hand_state
+    info_topic: /g20/cb_left_hand_info
+    setting_topic: /g20/cb_hand_setting_cmd
+    front_camera_info_topic: /g20_calibration/front/camera/camera_info
+    front_detections_topic: /g20_calibration/front/apriltag/detections
+    side_camera_info_topic: /g20_calibration/side/camera/camera_info
+    side_detections_topic: /g20_calibration/side/apriltag/detections
+    top_camera_info_topic: /g20_calibration/top/camera/camera_info
+    top_detections_topic: /g20_calibration/top/apriltag/detections
+
+    # /start先下发并确认这个20通道基准姿态，稳定后才进入第一条扫描。
+    baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+    normal_calibration_speed: 15
+    index_roll_calibration_speed: 5
+    index_flex_calibration_speed: 10
+    # 19-Tag产品预检仍使用上面保守速度；只有正反预检都留出至少双倍正式分箱余量，
+    # 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束，
+    # 始终保持速度5；任一方向采样余量不足也保持原速度。
+    adaptive_formal_speed_enabled: true
+    adaptive_formal_speed_max_scale: 1.5
+    adaptive_formal_speed_minimum_bins: 64
+    adaptive_formal_speed_maximum_bin_gap: 8
+    speed_setting_settle_seconds: 0.25
+
+    # tag36h11尺寸是检测角点围成的黑色正方形边长，不包含外围白边。
+    # 19张Tag的黑色码区外边长均为16 mm。自定义PnP必须与
+    # apriltag_ros逐ID尺寸一致，禁止用纸张/白边尺寸代替码区尺寸。
+    tag_size_m: 0.016
+    # ROS 2无法从YAML空数组推断整数/浮点数组类型。这四个
+    # 末端Tag仍显式写16 mm，防止节点启动时得到未初始化参数。
+    tag_size_override_ids: [7, 14, 16, 18]
+    tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016]
+    repetitions: 3
+    # 19-Tag产品正式零位使用前三轮训练、最后一轮完全留出；旧11-Tag仍读取repetitions=3。
+    g20_right_19_repetitions: 4
+    preflight_frames: 60
+    minimum_detection_rate: 0.95
+    minimum_detection_hz: 15.0
+    minimum_feedback_hz: 25.0
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 30.0
+
+    pnp_maximum_reprojection_error_px: 1.5
+    pnp_reprojection_tie_px: 1.5
+    pnp_maximum_pose_jump_deg: 35.0
+    pnp_maximum_translation_jump_m: 0.04
+    pnp_maximum_tag_tilt_deg: 75.0
+    pnp_tracker_reset_seconds: 5.0
+    # 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
+    pnp_group_initialization_frames: 8
+    # 侧面当前任务所需Tag在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
+    pnp_group_normal_alignment_scale_deg: 5.0
+    pnp_group_maximum_normal_alignment_deg: 15.0
+    # 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见；
+    # 任一角点相对会话基准漂移超过2 px并连续5帧时，判定标定中基准被移动。
+    fixed_base_maximum_corner_drift_px: 2.0
+    fixed_base_movement_confirmation_frames: 5
+    # 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时，用源URDF mimic
+    # 关系辅助选择IPPE分支；若全部候选超限则退回纯视觉，绝不丢帧，也不生成、
+    # 缩放或替代被动IP的自身Tag实测曲线。
+    thumb_ip_pnp_coupling_multiplier: 1.03
+    thumb_ip_pnp_coupling_scale_deg: 3.0
+    thumb_ip_pnp_maximum_coupling_residual_deg: 7.5
+    top_pnp_invalid_reset_seconds: 1.0
+    # 三维位姿必须与实测20通道状态严格按时间戳配对。
+    maximum_state_image_skew_ms: 50.0
+
+    axis_maximum_plane_rms_m: 0.003
+    # 被动耦合轴只用轨迹确定轴线位置，允许更大的轴向深度噪声；径向和跨轮
+    # 轴线一致性仍沿用严格检查。
+    passive_axis_maximum_plane_rms_m: 0.004
+    axis_maximum_radial_rms_m: 0.003
+    # 整段相对SE(3)运动拟合轴线点；端视关节会投影掉单目PnP光轴深度。
+    axis_maximum_pose_line_rms_m: 0.001
+    # 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴
+    # 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
+    axis_maximum_rotation_circle_difference_deg: 1.0
+    # 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
+    # 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及最终轮留出。
+    active_maximum_rotation_orthogonal_rms_deg: 2.5
+    passive_maximum_rotation_orthogonal_rms_deg: 7.5
+    zero_maximum_axis_cycle_difference_deg: 0.75
+    # 零位无法改变父子轴夹角；超过该值属于CAD/PnP几何错误，不能吸收到零位。
+    zero_maximum_axis_cone_mismatch_deg: 5.0
+    zero_maximum_observability_condition_number: 10000000000.0
+    zero_maximum_offset_deg: 20.0
+    # 四指MCP roll保留严格的装配保护范围。thumb CMC三轴由多轴视觉几何
+    # 求解且不假定电气端点等于CAD上限；thumb_mcp及四指MCP pitch/PIP
+    # 静态零位由实测全行程与CAD机械端点联合求解，不写死为0。
+    zero_finger_maximum_offset_deg: 3.0
+    # 只对实物已确认等同CAD端点的关节使用该限制；CMC电气端点不作此假设。
+    mechanical_endpoint_maximum_offset_deg: 5.0
+
+    endpoint_tolerance_u8: 2.0
+    # 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区，但反馈
+    # 必须已经稳定；大残差仍由机械卡滞保护处理。
+    steady_checkpoint_command_feedback_tolerance_u8: 8.0
+    steady_checkpoint_maximum_feedback_range_u8: 2.0
+    # 电机10在命令0时实测会稳定反馈为4；该0端使用±4。
+    thumb_yaw_zero_endpoint_tolerance_u8: 4.0
+    # 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。
+    right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
+    # 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。
+    pinky_pip_zero_endpoint_tolerance_u8: 5.0
+    endpoint_hold_seconds: 0.5
+    # roll零位127必须从两个方向到位并静止采集，禁止用运动中经过127的帧判回差。
+    baseline_hold_seconds: 0.5
+    minimum_baseline_hold_frames: 10
+    # 19-Tag产品每项正式四轮前先做一次低速往返，端点Tag稳定至少2秒。
+    task_precheck_hold_seconds: 2.0
+    position_timeout_seconds: 30.0
+    sweep_timeout_seconds: 90.0
+    # 启动宽限1秒后，反馈连续2秒没有至少1个u8的进展，按机械卡滞立即暂停；
+    # 这类故障不进入遮挡/超时的三次自动重扫。
+    # 低速5也应持续产生反馈进展；5秒无进展即停，减少机构持续顶死时间。
+    motor_stall_timeout_seconds: 2.0
+    motor_stall_startup_grace_seconds: 1.0
+    motor_stall_minimum_progress_u8: 1.0
+    invalid_timeout_seconds: 3.0
+    minimum_sweep_frames: 40
+    minimum_state_span_u8: 240.0
+    minimum_sweep_bins: 32
+    maximum_bin_gap: 16
+    # 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
+    automatic_sweep_retry_limit: 2
+    # 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
+    automatic_fit_retry_limit: 2
+    automatic_motion_retry_limit: 2
+    # 留空为正式标定；设为pinky/ring/middle/index时只采该指正面+侧面roll，
+    # 即使正面baseline回差失败也继续完成侧面对照，并永久锁定本会话URDF发布。
+    cross_view_roll_diagnostic_finger: ""
+    # 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
+    provisional_warning_ratio: 1.25
+    retry_minimum_speed: 3
+    retry_speed_scales: [0.8, 0.6]
+    retry_endpoint_hold_seconds: [0.75, 1.0]
+
+    trajectory_maximum_plane_rms_m: 0.004
+    trajectory_maximum_radial_rms_m: 0.004
+    trajectory_minimum_radius_m: 0.003
+    trajectory_minimum_arc_deg: 15.0
+    # 以下二维参数只供旧轨迹工具兼容，三机位v4零位不使用二维投影。
+    image_trajectory_maximum_radial_rms_px: 2.0
+    image_trajectory_maximum_radial_p95_px: 3.5
+    image_trajectory_minimum_radius_px: 20.0
+    trajectory_maximum_cycle_travel_difference_deg: 3.0
+    passive_maximum_cycle_travel_difference_deg: 10.0
+    maximum_monotonic_correction_deg: 2.0
+    # 旧布局仍用连续扫描正反程差门限；19-Tag产品的连续运动包含速度相关滞后，
+    # 由方向曲线和最终留出验证建模，不再重复硬判。其绝对正反程门禁使用下面
+    # 的九点稳态command_maximum_direction_gap_deg。
+    maximum_hysteresis_deg: 2.0
+    # 19-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差
+    # 不超过0.5°；四指roll例外：127以255→127为唯一物理零位，反向分支
+    # 保留实测偏差，并改为检查分支间隙上限及跨轮稳定性。
+    baseline_maximum_hysteresis_deg: 0.5
+    directional_zero_maximum_branch_gap_deg: 2.0
+    directional_zero_maximum_branch_gap_range_deg: 0.3
+    cross_view_roll_maximum_branch_gap_difference_deg: 0.3
+    # 正面roll是Tag中心的二维投影角，侧面roll是三维姿态角。允许一个有界的
+    # 固定比例吸收Tag安装倾角/偏置带来的投影缩放，再严格比较两条曲线形状；
+    # 比例过大、方向相反、形状RMS及两视角各自的四轮重复性仍会失败。
+    cross_view_roll_maximum_shape_rms_deg: 1.25
+    cross_view_roll_maximum_projection_scale_ratio: 1.5
+    passive_maximum_monotonic_correction_deg: 3.0
+    passive_maximum_hysteresis_deg: 2.0
+    # 九点姿态先按实测反馈重新对齐，再用此门限检查固有机械回差；请求命令域
+    # 的两条运行曲线仍原样保留固件方向死区，不能把command/feedback差算成回差。
+    command_maximum_direction_gap_deg: 2.0
+
+    # 默认无额外随机动作；19-Tag产品最终一轮始终作为不可关闭的留出验证。
+    validation_enabled: false
+    # 第四轮留出求解后必须再走8个固定安全组合姿态；三机位规定Tag全部可见
+    # 且实测20通道到位才允许发布。只保存Tag位姿，不保存原始图像。
+    # Developer diagnostic only. The formal fourth sweep cycle already gives
+    # every isolated PIP/DIP pair an independent holdout.
+    combination_validation_enabled: false
+    combination_validation_frames: 10
+    combination_maximum_position_p95_m: 0.003
+    combination_maximum_orientation_p95_deg: 2.0
+    validation_command_count: 3
+    validation_frames: 10
+    validation_seed: 20260804
+    validation_timeout_seconds: 20.0
+    maximum_validation_mae_deg: 1.0
+    maximum_validation_p95_deg: 2.0
+    # 19-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。
+    maximum_validation_error_deg: 3.0
+    zero_maximum_confidence_half_width_deg: 1.5

src/linkerhand_calibration/config/three_camera_tags.yaml

@@ -0,0 +1,62 @@
+/g20_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3, 10]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, index_roll]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [4, 5, 6, 7]
+      frames: [side_base, index_mcp, index_pip, index_dip]
+      sizes: [0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [8, 9]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/linkerhand_calibration/config/three_camera_tags_g20_right_15.yaml

@@ -0,0 +1,62 @@
+/g20_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3, 10, 11, 12, 13]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [4, 5, 6, 15, 17]
+      frames: [side_base, ring_pip, pinky_pip, middle_pip, index_pip]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [8, 9]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/linkerhand_calibration/config/three_camera_tags_g20_right_19.yaml

@@ -0,0 +1,63 @@
+/g20_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3, 10, 11, 12, 13]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      # Distal Tags use the same measured 16 mm black-code edge as all others.
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [4, 5, 6, 7, 14, 15, 16, 17, 18]
+      frames: [side_base, ring_pip, pinky_pip, pinky_dip, ring_dip, middle_pip, middle_dip, index_pip, index_dip]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [8, 9]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/linkerhand_calibration/g20_thumb_apriltag_calibration/__init__.py

@@ -0,0 +1,21 @@
+"""One-release compatibility surface for the former Python package name.
+
+New code must import :mod:`linkerhand_calibration`.  Only the documented
+configuration loader is re-exported here; calibration algorithms continue to
+have a single implementation in the renamed package.
+"""
+
+from __future__ import annotations
+
+import warnings
+
+warnings.warn(
+    "g20_thumb_apriltag_calibration is deprecated; "
+    "import linkerhand_calibration instead",
+    DeprecationWarning,
+    stacklevel=2,
+)
+
+from linkerhand_calibration.product import ProductConfig, load_product_config
+
+__all__ = ["ProductConfig", "load_product_config"]

src/linkerhand_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py

@@ -0,0 +1,9 @@
+"""Deprecated forwarding entry point for the runtime joint-state bridge."""
+
+from linkerhand_calibration.calibrated_joint_state_bridge import main
+
+__all__ = ["main"]
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_calibration/g20_thumb_apriltag_calibration/offline_replay.py

@@ -0,0 +1,9 @@
+"""Deprecated forwarding entry point for offline replay."""
+
+from linkerhand_calibration.offline_replay import main
+
+__all__ = ["main"]
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_calibration/g20_thumb_apriltag_calibration/one_command.py

@@ -0,0 +1,9 @@
+"""Deprecated forwarding entry point for the former Python package."""
+
+from linkerhand_calibration.one_command import main
+
+__all__ = ["main"]
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_calibration/launch/calibrated_joint_state_bridge.launch.py

@@ -0,0 +1,37 @@
+"""Publish profile-calibrated URDF angles from raw command/feedback u8 values."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("hand_type", default_value="right"),
+            DeclareLaunchArgument("calibration_file"),
+            DeclareLaunchArgument("input_topic", default_value=""),
+            DeclareLaunchArgument("output_topic", default_value=""),
+            Node(
+                package="linkerhand_calibration",
+                executable="calibrated_joint_state_bridge",
+                name=[
+                    "calibrated_joint_state_bridge_",
+                    LaunchConfiguration("hand_type"),
+                ],
+                output="screen",
+                emulate_tty=True,
+                parameters=[
+                    {
+                        "hand_type": LaunchConfiguration("hand_type"),
+                        "calibration_file": LaunchConfiguration(
+                            "calibration_file"
+                        ),
+                        "input_topic": LaunchConfiguration("input_topic"),
+                        "output_topic": LaunchConfiguration("output_topic"),
+                    }
+                ],
+            ),
+        ]
+    )

src/linkerhand_calibration/launch/front_cmc_pitch_zero.launch.py

@@ -10,12 +10,11 @@ from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
-    IncludeLaunchDescription,
     LogInfo,
     OpaqueFunction,
+    SetEnvironmentVariable,
 )
 from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer, Node
 from launch_ros.descriptions import ComposableNode
@@ -48,29 +47,41 @@ def _launch_stack(context):
 
     tag_config = LaunchConfiguration("tag_config").perform(context)
     zero_config = LaunchConfiguration("zero_config").perform(context)
-    realsense_config = LaunchConfiguration("realsense_config").perform(context)
-    realsense_launch = (
-        Path(get_package_share_directory("realsense2_camera"))
-        / "launch"
-        / "rs_launch.py"
-    )
-
-    camera = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(str(realsense_launch)),
+    camera = Node(
+        package="linkerhand_calibration",
+        executable="hikrobot_camera_node",
+        name="hikrobot_camera",
+        namespace="/camera/camera/color",
+        output="screen",
+        emulate_tty=True,
         condition=IfCondition(LaunchConfiguration("start_camera")),
-        launch_arguments={
-            "camera_name": "camera",
-            "camera_namespace": "camera",
-            "serial_no": LaunchConfiguration("camera_serial_number"),
-            "config_file": realsense_config,
-            "enable_color": "true",
-            "rgb_camera.color_profile": LaunchConfiguration("color_profile"),
-            "depth_module.color_profile": LaunchConfiguration("color_profile"),
-            "enable_depth": "false",
-            "enable_sync": "false",
-            "align_depth.enable": "false",
-            "diagnostics_period": "1.0",
-        }.items(),
+        parameters=[
+            {
+                "serial_number": LaunchConfiguration("camera_serial_number"),
+                "expected_model": LaunchConfiguration("camera_model"),
+                "camera_name": LaunchConfiguration("camera_name"),
+                "frame_id": LaunchConfiguration("camera_frame_id"),
+                "image_width": ParameterValue(
+                    LaunchConfiguration("image_width"), value_type=int
+                ),
+                "image_height": ParameterValue(
+                    LaunchConfiguration("image_height"), value_type=int
+                ),
+                "frame_rate": ParameterValue(
+                    LaunchConfiguration("camera_frame_rate"), value_type=float
+                ),
+                "exposure_time_us": ParameterValue(
+                    LaunchConfiguration("exposure_time_us"), value_type=float
+                ),
+                "gain_db": ParameterValue(
+                    LaunchConfiguration("gain_db"), value_type=float
+                ),
+                "auto_exposure": ParameterValue(
+                    LaunchConfiguration("auto_exposure"), value_type=bool
+                ),
+                "camera_info_url": LaunchConfiguration("camera_info_url"),
+            }
+        ],
     )
 
     raw_topic = "/camera/camera/color/image_raw"
@@ -147,7 +158,7 @@ def _launch_stack(context):
     )
 
     zero_node = Node(
-        package="g20_thumb_apriltag_calibration",
+        package="linkerhand_calibration",
         executable="cmc_pitch_zero_node",
         name="g20_thumb_cmc_pitch_zero",
         output="screen",
@@ -192,14 +203,43 @@ def _launch_stack(context):
 
 def generate_launch_description() -> LaunchDescription:
     package_share = Path(
-        get_package_share_directory("g20_thumb_apriltag_calibration")
+        get_package_share_directory("linkerhand_calibration")
     )
     return LaunchDescription(
         [
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
             DeclareLaunchArgument("serial_number", default_value="UNSET"),
-            DeclareLaunchArgument("camera_serial_number", default_value=""),
             DeclareLaunchArgument(
-                "color_profile", default_value="1280x720x30"
+                "camera_serial_number", default_value="DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_model", default_value="MV-CS020-10UM"
+            ),
+            DeclareLaunchArgument(
+                "camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_frame_id", default_value="camera_color_optical_frame"
+            ),
+            DeclareLaunchArgument("image_width", default_value="1624"),
+            DeclareLaunchArgument("image_height", default_value="1240"),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_info_url",
+                default_value=str(
+                    Path.home()
+                    / ".ros"
+                    / "camera_info"
+                    / "hikrobot_DB2163742.yaml"
+                ),
             ),
             DeclareLaunchArgument("can_interface", default_value="can0"),
             DeclareLaunchArgument("calibration_speed", default_value="15"),
@@ -221,12 +261,6 @@ def generate_launch_description() -> LaunchDescription:
                     package_share / "config" / "cmc_pitch_zero.yaml"
                 ),
             ),
-            DeclareLaunchArgument(
-                "realsense_config",
-                default_value=str(
-                    package_share / "config" / "realsense_color_qos.yaml"
-                ),
-            ),
             DeclareLaunchArgument(
                 "tag_config",
                 default_value=str(

src/linkerhand_calibration/launch/front_cmc_roll_calibration.launch.py

@@ -10,12 +10,11 @@ from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
-    IncludeLaunchDescription,
     LogInfo,
     OpaqueFunction,
+    SetEnvironmentVariable,
 )
 from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer, Node
 from launch_ros.descriptions import ComposableNode
@@ -50,29 +49,41 @@ def _launch_stack(context):
     calibration_config = LaunchConfiguration(
         "calibration_config"
     ).perform(context)
-    realsense_config = LaunchConfiguration("realsense_config").perform(context)
-    realsense_launch = (
-        Path(get_package_share_directory("realsense2_camera"))
-        / "launch"
-        / "rs_launch.py"
-    )
-
-    camera = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(str(realsense_launch)),
+    camera = Node(
+        package="linkerhand_calibration",
+        executable="hikrobot_camera_node",
+        name="hikrobot_camera",
+        namespace="/camera/camera/color",
+        output="screen",
+        emulate_tty=True,
         condition=IfCondition(LaunchConfiguration("start_camera")),
-        launch_arguments={
-            "camera_name": "camera",
-            "camera_namespace": "camera",
-            "serial_no": LaunchConfiguration("camera_serial_number"),
-            "config_file": realsense_config,
-            "enable_color": "true",
-            "rgb_camera.color_profile": LaunchConfiguration("color_profile"),
-            "depth_module.color_profile": LaunchConfiguration("color_profile"),
-            "enable_depth": "false",
-            "enable_sync": "false",
-            "align_depth.enable": "false",
-            "diagnostics_period": "1.0",
-        }.items(),
+        parameters=[
+            {
+                "serial_number": LaunchConfiguration("camera_serial_number"),
+                "expected_model": LaunchConfiguration("camera_model"),
+                "camera_name": LaunchConfiguration("camera_name"),
+                "frame_id": LaunchConfiguration("camera_frame_id"),
+                "image_width": ParameterValue(
+                    LaunchConfiguration("image_width"), value_type=int
+                ),
+                "image_height": ParameterValue(
+                    LaunchConfiguration("image_height"), value_type=int
+                ),
+                "frame_rate": ParameterValue(
+                    LaunchConfiguration("camera_frame_rate"), value_type=float
+                ),
+                "exposure_time_us": ParameterValue(
+                    LaunchConfiguration("exposure_time_us"), value_type=float
+                ),
+                "gain_db": ParameterValue(
+                    LaunchConfiguration("gain_db"), value_type=float
+                ),
+                "auto_exposure": ParameterValue(
+                    LaunchConfiguration("auto_exposure"), value_type=bool
+                ),
+                "camera_info_url": LaunchConfiguration("camera_info_url"),
+            }
+        ],
     )
 
     raw_topic = "/camera/camera/color/image_raw"
@@ -149,7 +160,7 @@ def _launch_stack(context):
     )
 
     calibration_node = Node(
-        package="g20_thumb_apriltag_calibration",
+        package="linkerhand_calibration",
         executable="cmc_roll_calibration_node",
         name="g20_thumb_cmc_roll_calibration",
         output="screen",
@@ -194,14 +205,43 @@ def _launch_stack(context):
 
 def generate_launch_description() -> LaunchDescription:
     package_share = Path(
-        get_package_share_directory("g20_thumb_apriltag_calibration")
+        get_package_share_directory("linkerhand_calibration")
     )
     return LaunchDescription(
         [
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
             DeclareLaunchArgument("serial_number", default_value="UNSET"),
-            DeclareLaunchArgument("camera_serial_number", default_value=""),
             DeclareLaunchArgument(
-                "color_profile", default_value="1280x720x30"
+                "camera_serial_number", default_value="DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_model", default_value="MV-CS020-10UM"
+            ),
+            DeclareLaunchArgument(
+                "camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_frame_id", default_value="camera_color_optical_frame"
+            ),
+            DeclareLaunchArgument("image_width", default_value="1624"),
+            DeclareLaunchArgument("image_height", default_value="1240"),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_info_url",
+                default_value=str(
+                    Path.home()
+                    / ".ros"
+                    / "camera_info"
+                    / "hikrobot_DB2163742.yaml"
+                ),
             ),
             DeclareLaunchArgument("can_interface", default_value="can0"),
             DeclareLaunchArgument("calibration_speed", default_value="15"),
@@ -225,12 +265,6 @@ def generate_launch_description() -> LaunchDescription:
                     / "cmc_roll_zero_travel.yaml"
                 ),
             ),
-            DeclareLaunchArgument(
-                "realsense_config",
-                default_value=str(
-                    package_share / "config" / "realsense_color_qos.yaml"
-                ),
-            ),
             DeclareLaunchArgument(
                 "tag_config",
                 default_value=str(

src/linkerhand_calibration/launch/front_thumb_calibration.launch.py

@@ -11,12 +11,11 @@ from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
     ExecuteProcess,
-    IncludeLaunchDescription,
     LogInfo,
     OpaqueFunction,
+    SetEnvironmentVariable,
 )
 from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer, Node
 from launch_ros.descriptions import ComposableNode
@@ -48,7 +47,6 @@ def _launch_stack(context):
 
     calibration_config = LaunchConfiguration("calibration_config").perform(context)
     tag_config = LaunchConfiguration("tag_config").perform(context)
-    realsense_config = LaunchConfiguration("realsense_config").perform(context)
     use_roi_text = LaunchConfiguration("use_roi").perform(context).strip().lower()
     if use_roi_text not in {"true", "false"}:
         raise RuntimeError("use_roi must be true or false")
@@ -66,47 +64,58 @@ def _launch_stack(context):
     if roi_values["roi_width"] <= 0 or roi_values["roi_height"] <= 0:
         raise RuntimeError("roi_width and roi_height must be positive")
 
-    color_profile = LaunchConfiguration("color_profile").perform(context)
-    profile_match = re.fullmatch(r"(\d+)x(\d+)x(\d+)", color_profile)
-    if use_roi and profile_match is not None:
-        image_width = int(profile_match.group(1))
-        image_height = int(profile_match.group(2))
+    try:
+        image_width = int(LaunchConfiguration("image_width").perform(context))
+        image_height = int(LaunchConfiguration("image_height").perform(context))
+    except ValueError as error:
+        raise RuntimeError("image_width and image_height must be integers") from error
+    if image_width <= 0 or image_height <= 0:
+        raise RuntimeError("image_width and image_height must be positive")
+    if use_roi:
         if (
             roi_values["roi_x"] + roi_values["roi_width"] > image_width
             or roi_values["roi_y"] + roi_values["roi_height"] > image_height
         ):
             raise RuntimeError(
-                "ROI lies outside color_profile "
+                "ROI lies outside camera image "
                 f"{image_width}x{image_height}"
             )
 
-    realsense_launch = Path(
-        get_package_share_directory("realsense2_camera")
-    ) / "launch" / "rs_launch.py"
-
-    camera = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(str(realsense_launch)),
+    camera = Node(
+        package="linkerhand_calibration",
+        executable="hikrobot_camera_node",
+        name="hikrobot_camera",
+        namespace="/camera/camera/color",
+        output="screen",
+        emulate_tty=True,
         condition=IfCondition(LaunchConfiguration("start_camera")),
-        launch_arguments={
-            "camera_name": "camera",
-            "camera_namespace": "camera",
-            "serial_no": LaunchConfiguration("camera_serial_number"),
-            "config_file": realsense_config,
-            "enable_color": "true",
-            # D405 exposes color from the stereo/depth module. Keep the RGB
-            # camera argument as well so the same launch file also works with
-            # D435/D455-class devices.
-            "rgb_camera.color_profile": LaunchConfiguration("color_profile"),
-            "depth_module.color_profile": LaunchConfiguration("color_profile"),
-            "enable_depth": LaunchConfiguration("enable_depth"),
-            "depth_module.depth_profile": LaunchConfiguration("depth_profile"),
-            # Depth is archival/diagnostic only. Synchronising and aligning it
-            # adds significant D405 processing latency without affecting the
-            # relative AprilTag rotation calculation.
-            "enable_sync": "false",
-            "align_depth.enable": "false",
-            "diagnostics_period": "1.0",
-        }.items(),
+        parameters=[
+            {
+                "serial_number": LaunchConfiguration("camera_serial_number"),
+                "expected_model": LaunchConfiguration("camera_model"),
+                "camera_name": LaunchConfiguration("camera_name"),
+                "frame_id": LaunchConfiguration("camera_frame_id"),
+                "image_width": ParameterValue(
+                    LaunchConfiguration("image_width"), value_type=int
+                ),
+                "image_height": ParameterValue(
+                    LaunchConfiguration("image_height"), value_type=int
+                ),
+                "frame_rate": ParameterValue(
+                    LaunchConfiguration("camera_frame_rate"), value_type=float
+                ),
+                "exposure_time_us": ParameterValue(
+                    LaunchConfiguration("exposure_time_us"), value_type=float
+                ),
+                "gain_db": ParameterValue(
+                    LaunchConfiguration("gain_db"), value_type=float
+                ),
+                "auto_exposure": ParameterValue(
+                    LaunchConfiguration("auto_exposure"), value_type=bool
+                ),
+                "camera_info_url": LaunchConfiguration("camera_info_url"),
+            }
+        ],
     )
 
     vision_components = []
@@ -225,7 +234,7 @@ def _launch_stack(context):
     )
 
     calibration = Node(
-        package="g20_thumb_apriltag_calibration",
+        package="linkerhand_calibration",
         executable="calibration_node",
         name="g20_thumb_calibration",
         output="screen",
@@ -275,7 +284,6 @@ def _launch_stack(context):
             str(bag_path),
             processed_image_raw_topic,
             processed_camera_info_topic,
-            "/camera/camera/depth/image_rect_raw",
             "/apriltag/detections",
             "/tf",
             "/g20/cb_left_hand_control_cmd",
@@ -307,20 +315,45 @@ def _launch_stack(context):
 
 def generate_launch_description() -> LaunchDescription:
     package_share = Path(
-        get_package_share_directory("g20_thumb_apriltag_calibration")
+        get_package_share_directory("linkerhand_calibration")
     )
     default_output = str(Path.cwd() / "calibration_output")
     return LaunchDescription(
         [
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
             DeclareLaunchArgument("serial_number", default_value="UNSET"),
-            DeclareLaunchArgument("camera_serial_number", default_value=""),
             DeclareLaunchArgument(
-                "color_profile", default_value="1280x720x30"
+                "camera_serial_number", default_value="DB2163742"
             ),
             DeclareLaunchArgument(
-                "depth_profile", default_value="640x480x30"
+                "camera_model", default_value="MV-CS020-10UM"
+            ),
+            DeclareLaunchArgument(
+                "camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "camera_frame_id", default_value="camera_color_optical_frame"
+            ),
+            DeclareLaunchArgument("image_width", default_value="1624"),
+            DeclareLaunchArgument("image_height", default_value="1240"),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_info_url",
+                default_value=str(
+                    Path.home()
+                    / ".ros"
+                    / "camera_info"
+                    / "hikrobot_DB2163742.yaml"
+                ),
             ),
-            DeclareLaunchArgument("enable_depth", default_value="false"),
             DeclareLaunchArgument(
                 "publish_debug_image", default_value="false"
             ),
@@ -349,12 +382,6 @@ def generate_launch_description() -> LaunchDescription:
                 "calibration_config",
                 default_value=str(package_share / "config" / "calibration.yaml"),
             ),
-            DeclareLaunchArgument(
-                "realsense_config",
-                default_value=str(
-                    package_share / "config" / "realsense_color_qos.yaml"
-                ),
-            ),
             DeclareLaunchArgument(
                 "tag_config",
                 default_value=str(package_share / "config" / "front_tags.yaml"),

src/linkerhand_calibration/launch/three_camera_calibration.launch.py

@@ -0,0 +1,572 @@
+"""Launch three Hikrobot views and one registered hand calibration owner."""
+
+from __future__ import annotations
+
+from datetime import datetime
+import hashlib
+from pathlib import Path
+import re
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    LogInfo,
+    OpaqueFunction,
+    SetEnvironmentVariable,
+)
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+VIEWS = ("front", "side", "top")
+
+
+def _default_source_urdf(model: str, hand_type: str) -> Path:
+    relative = (
+        Path("urdf") / "l6_right" / "linkerhand_l6v3.1_right.urdf"
+        if model.upper() == "L6" and hand_type == "right"
+        else Path("urdf")
+        / f"{model.lower()}_{hand_type}"
+        / f"linkerhand_{model.lower()}_{hand_type}.urdf"
+    )
+    package_source_or_share = Path(__file__).resolve().parents[1] / relative
+    try:
+        installed = (
+            Path(get_package_share_directory("linkerhand_calibration"))
+            / relative
+        )
+    except Exception:
+        installed = package_source_or_share
+    return installed if installed.is_file() else package_source_or_share
+
+
+def _launch_stack(context):
+    from linkerhand_calibration.product import (
+        get_product_calibration_contract,
+    )
+
+    model = LaunchConfiguration("model").perform(context).strip().upper()
+    hand_type = LaunchConfiguration("hand_type").perform(context).lower()
+    if hand_type not in {"left", "right"}:
+        raise RuntimeError("hand_type must be left or right")
+    tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
+    try:
+        contract = get_product_calibration_contract(
+            model, hand_type, tag_layout
+        )
+    except ValueError as error:
+        raise RuntimeError(str(error)) from error
+    requested_tag_config = LaunchConfiguration("tag_config").perform(context)
+    package_share = Path(
+        get_package_share_directory("linkerhand_calibration")
+    )
+    tag_config = (
+        Path(requested_tag_config).expanduser().resolve()
+        if requested_tag_config
+        else package_share
+        / "config"
+        / (
+            "three_camera_tags_g20_right_19.yaml"
+            if tag_layout == "g20_right_19"
+            else "o12_right_16_tags.yaml"
+            if tag_layout == "o12_right_16"
+            else "o6_right_8_tags.yaml"
+            if tag_layout == "o6_right_8"
+            else "l6_right_8_tags.yaml"
+            if tag_layout == "l6_right_8"
+            else "three_camera_tags_g20_right_15.yaml"
+            if tag_layout == "g20_right_15"
+            else "three_camera_tags.yaml"
+        )
+    )
+    if not tag_config.is_file():
+        raise RuntimeError(f"tag config does not exist: {tag_config}")
+    topic_prefix = f"/{model.lower()}"
+    is_o12 = model == "O12"
+    command_topic = (
+        f"{topic_prefix}/{hand_type}/joint_cmd"
+        if is_o12 else f"{topic_prefix}/cb_{hand_type}_hand_control_cmd"
+    )
+    state_topic = (
+        f"{topic_prefix}/{hand_type}/joint_states"
+        if is_o12 else f"{topic_prefix}/cb_{hand_type}_hand_state"
+    )
+    info_topic = f"{topic_prefix}/cb_{hand_type}_hand_info"
+    requested_source = LaunchConfiguration("source_urdf_path").perform(context)
+    source_urdf = (
+        Path(requested_source).expanduser().resolve()
+        if requested_source
+        else _default_source_urdf(model, hand_type).resolve()
+    )
+    if not source_urdf.is_file():
+        raise RuntimeError(f"source URDF does not exist: {source_urdf}")
+    expected_source_hash = LaunchConfiguration(
+        "source_urdf_expected_sha256"
+    ).perform(context).strip().lower()
+    if contract.typed_profile.artifacts.publish_corrected_urdf:
+        if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
+            raise RuntimeError(
+                "this profile requires source_urdf_expected_sha256 confirmed "
+                "by the CAD/hardware owner"
+            )
+        actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
+        if actual_source_hash != expected_source_hash:
+            raise RuntimeError(
+                "source_urdf_expected_sha256 does not match source_urdf_path"
+            )
+
+    hand_serial = LaunchConfiguration("serial_number").perform(context)
+    if (
+        not hand_serial
+        or hand_serial == "UNSET"
+        or re.fullmatch(r"[A-Za-z0-9_.-]+", hand_serial) is None
+        or hand_serial in {".", ".."}
+    ):
+        raise RuntimeError("serial_number must be a safe non-empty hand serial")
+
+    requested_session = LaunchConfiguration("session_dir").perform(context)
+    output_root = Path(
+        LaunchConfiguration("output_root").perform(context)
+    ).expanduser().resolve()
+    if requested_session:
+        session_dir = Path(requested_session).expanduser().resolve()
+    else:
+        session_dir = (
+            output_root
+            / hand_serial
+            / datetime.now().strftime("%Y%m%d_%H%M%S")
+        )
+    session_dir.mkdir(parents=True, exist_ok=True)
+
+    camera_serials = {
+        view: LaunchConfiguration(f"{view}_camera_serial").perform(context)
+        for view in VIEWS
+    }
+    if any(not serial for serial in camera_serials.values()):
+        raise RuntimeError("all three camera serial numbers are required")
+    if len(set(camera_serials.values())) != 3:
+        raise RuntimeError("front/side/top camera serial numbers must be unique")
+
+    cameras = []
+    components = []
+    raw_topics = []
+    info_topics = []
+    detection_topics = []
+    calibration_namespace = contract.typed_profile.namespace
+    for view in VIEWS:
+        namespace = f"{calibration_namespace}/{view}/camera"
+        raw_topic = f"{namespace}/image_raw"
+        info_topic = f"{namespace}/camera_info"
+        rect_topic = f"{namespace}/image_rect"
+        detector_namespace = f"{calibration_namespace}/{view}/apriltag"
+        detection_topic = f"{detector_namespace}/detections"
+        raw_topics.append(raw_topic)
+        info_topics.append(info_topic)
+        detection_topics.append(detection_topic)
+        cameras.append(
+            Node(
+                package="linkerhand_calibration",
+                executable="hikrobot_camera_node",
+                name="hikrobot_camera",
+                namespace=namespace,
+                output="screen",
+                emulate_tty=True,
+                condition=IfCondition(LaunchConfiguration("start_cameras")),
+                parameters=[
+                    {
+                        "serial_number": LaunchConfiguration(
+                            f"{view}_camera_serial"
+                        ),
+                        "expected_model": LaunchConfiguration("camera_model"),
+                        "camera_name": LaunchConfiguration(
+                            f"{view}_camera_name"
+                        ),
+                        "frame_id": (
+                            f"{model.lower()}_calibration_{view}_optical_frame"
+                        ),
+                        "image_width": 1624,
+                        "image_height": 1240,
+                        "frame_rate": ParameterValue(
+                            LaunchConfiguration("camera_frame_rate"),
+                            value_type=float,
+                        ),
+                        "exposure_time_us": ParameterValue(
+                            LaunchConfiguration("exposure_time_us"),
+                            value_type=float,
+                        ),
+                        "gain_db": ParameterValue(
+                            LaunchConfiguration("gain_db"), value_type=float
+                        ),
+                        "auto_exposure": ParameterValue(
+                            LaunchConfiguration("auto_exposure"), value_type=bool
+                        ),
+                        "camera_info_url": LaunchConfiguration(
+                            f"{view}_camera_info_url"
+                        ),
+                    }
+                ],
+            )
+        )
+        components.extend(
+            [
+                ComposableNode(
+                    package="image_proc",
+                    plugin="image_proc::RectifyNode",
+                    name=f"rectify_{view}",
+                    namespace=namespace,
+                    remappings=[
+                        ("image", raw_topic),
+                        ("camera_info", info_topic),
+                        ("image_rect", rect_topic),
+                    ],
+                    parameters=[{"queue_size": 1}],
+                    extra_arguments=[{"use_intra_process_comms": True}],
+                ),
+                ComposableNode(
+                    package="apriltag_ros",
+                    plugin="AprilTagNode",
+                    name="apriltag",
+                    namespace=detector_namespace,
+                    parameters=[
+                        str(tag_config),
+                        {
+                            "detector.decimate": ParameterValue(
+                                LaunchConfiguration("apriltag_decimate"),
+                                value_type=float,
+                            )
+                        },
+                    ],
+                    remappings=[
+                        ("image_rect", rect_topic),
+                        ("camera_info", info_topic),
+                    ],
+                    extra_arguments=[{"use_intra_process_comms": True}],
+                ),
+            ]
+        )
+
+    vision = ComposableNodeContainer(
+        name=f"{model.lower()}_three_camera_vision",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=components,
+        output="screen",
+        emulate_tty=True,
+    )
+    sdk = (
+        Node(
+            package="omnihand_node",
+            executable="omnihand_pro_2025_node",
+            name="omnihand_pro_2025_node",
+            namespace="o12",
+            output="screen",
+            condition=IfCondition(LaunchConfiguration("start_sdk")),
+            parameters=[LaunchConfiguration("vendor_sdk_config")],
+        )
+        if is_o12
+        else Node(
+            package="linker_hand_ros2_sdk",
+            executable="linker_hand_sdk",
+            name="linker_hand_sdk",
+            output="screen",
+            condition=IfCondition(LaunchConfiguration("start_sdk")),
+            parameters=[{
+                "hand_type": hand_type,
+                "hand_joint": model,
+                "can": LaunchConfiguration("can_interface"),
+                "modbus": "None",
+                "topic_prefix": topic_prefix,
+                "move_on_startup": False,
+                "startup_speed": ParameterValue(
+                    LaunchConfiguration("calibration_speed"), value_type=int
+                ),
+                "startup_torque": 80,
+                # Match 30 Hz cameras so state/image p95 skew stays below 50 ms.
+                "state_poll_rate": 30.0,
+                # Calibration does not consume measured joint velocity.  A
+                # G20 velocity read sends another five synchronous CAN
+                # queries, so keep it off the trajectory-critical path.
+                "velocity_poll_rate": 1.0,
+                # G20 sends an endpoint and L6 streams a bounded trajectory.
+                # Keep polling the real motor state during either command path;
+                # otherwise the SDK republishes stale state and creates large
+                # command-unit holes in the trajectory bins.
+                "defer_state_reads_while_commanding": False,
+                "repeat_position_commands": False,
+                "is_touch": False,
+            }],
+        )
+    )
+    calibration = Node(
+        package="linkerhand_calibration",
+        executable="three_camera_calibration_node",
+        name=f"{model.lower()}_calibration",
+        output="screen",
+        emulate_tty=True,
+        arguments=[
+            "--profile-id",
+            contract.typed_profile.key.profile_id,
+        ],
+        parameters=[
+            LaunchConfiguration("calibration_config"),
+            {
+                "serial_number": hand_serial,
+                "model": model,
+                "hand_type": hand_type,
+                "tag_layout": tag_layout,
+                "session_dir": str(session_dir),
+                "resume_raw_samples_path": LaunchConfiguration(
+                    "resume_raw_samples_path"
+                ),
+                "recalibration_scope": LaunchConfiguration(
+                    "recalibration_scope"
+                ),
+                # The SDK performs roughly 25 synchronous CAN queries whenever
+                # cb_<side>_hand_info has a subscriber. Calibration only used
+                # that topic to display a speed diagnostic, while those reads
+                # created 17-33 command-unit holes in position trajectories.
+                "info_topic": f"{calibration_namespace}/disabled_hand_info",
+                "command_topic": command_topic,
+                "state_topic": state_topic,
+                "camera_extrinsics_file": LaunchConfiguration(
+                    "camera_extrinsics_file"
+                ),
+                "source_urdf_path": str(source_urdf),
+                "source_urdf_expected_sha256": LaunchConfiguration(
+                    "source_urdf_expected_sha256"
+                ),
+                "camera_extrinsics_expected_sha256": LaunchConfiguration(
+                    "camera_extrinsics_expected_sha256"
+                ),
+                "calibration_config_expected_sha256": LaunchConfiguration(
+                    "calibration_config_expected_sha256"
+                ),
+                "tag_config_expected_sha256": LaunchConfiguration(
+                    "tag_config_expected_sha256"
+                ),
+                "sdk_config_expected_sha256": LaunchConfiguration(
+                    "sdk_config_expected_sha256"
+                ),
+                "corrected_urdf_output_dir": LaunchConfiguration(
+                    "corrected_urdf_output_dir"
+                ),
+                **{
+                    f"{view}_camera_serial": camera_serials[view]
+                    for view in VIEWS
+                },
+                "commands_enabled": ParameterValue(
+                    LaunchConfiguration("commands_enabled"), value_type=bool
+                ),
+                "normal_calibration_speed": ParameterValue(
+                    LaunchConfiguration("calibration_speed"), value_type=int
+                ),
+                "index_roll_calibration_speed": ParameterValue(
+                    LaunchConfiguration("index_roll_calibration_speed"),
+                    value_type=int,
+                ),
+                "index_flex_calibration_speed": ParameterValue(
+                    LaunchConfiguration("index_flex_calibration_speed"),
+                    value_type=int,
+                ),
+                "adaptive_formal_speed_enabled": ParameterValue(
+                    LaunchConfiguration("adaptive_formal_speed_enabled"),
+                    value_type=bool,
+                ),
+                "cross_view_roll_diagnostic_finger": LaunchConfiguration(
+                    "cross_view_roll_diagnostic_finger"
+                ),
+                "validation_enabled": ParameterValue(
+                    LaunchConfiguration("validation_enabled"), value_type=bool
+                ),
+            },
+        ],
+    )
+    bag = ExecuteProcess(
+        condition=IfCondition(LaunchConfiguration("record_bag")),
+        cmd=[
+            "ros2",
+            "bag",
+            "record",
+            "--storage",
+            "mcap",
+            "--storage-preset-profile",
+            "zstd_fast",
+            "--max-bag-size",
+            "10737418240",
+            "--output",
+            str(session_dir / "rosbag"),
+            *raw_topics,
+            *info_topics,
+            *detection_topics,
+            command_topic,
+            state_topic,
+            info_topic,
+            *(
+                [
+                    "/o12/right/joint_control_mode_states",
+                    "/o12/right/joint_error_states",
+                    "/o12/right/joint_temperature_states",
+                ]
+                if is_o12 else []
+            ),
+            f"{calibration_namespace}/status",
+        ],
+        output="screen",
+    )
+    return [
+        LogInfo(
+            msg=(
+                f"{model} {hand_type} {tag_layout} three-camera session: {session_dir}; "
+                f"source_urdf={source_urdf}"
+            )
+        ),
+        LogInfo(
+            msg=(
+                "Camera mapping: front="
+                f"{camera_serials['front']} side={camera_serials['side']} "
+                f"top={camera_serials['top']}"
+            )
+        ),
+        *cameras,
+        vision,
+        sdk,
+        calibration,
+        bag,
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    package_share = Path(
+        get_package_share_directory("linkerhand_calibration")
+    )
+    info_root = Path.home() / ".ros" / "camera_info"
+    return LaunchDescription(
+        [
+            # Camera processes publish ~2 MB frames across DDS.  Force the
+            # matching RMW and provide both current and legacy profile names
+            # so the configured 64 MB shared-memory segment is actually used.
+            SetEnvironmentVariable(
+                name="RMW_IMPLEMENTATION",
+                value="rmw_fastrtps_cpp",
+            ),
+            SetEnvironmentVariable(
+                name="FASTDDS_DEFAULT_PROFILES_FILE",
+                value=str(package_share / "config" / "fastdds_large_images.xml"),
+            ),
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(package_share / "config" / "fastdds_large_images.xml"),
+            ),
+            DeclareLaunchArgument("model", default_value="G20"),
+            DeclareLaunchArgument("hand_type", default_value="left"),
+            DeclareLaunchArgument("tag_layout", default_value="legacy_11"),
+            DeclareLaunchArgument("serial_number", default_value="UNSET"),
+            DeclareLaunchArgument(
+                "front_camera_serial", default_value="DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "side_camera_serial", default_value="DB2163749"
+            ),
+            DeclareLaunchArgument(
+                "top_camera_serial", default_value="DB2163739"
+            ),
+            DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
+            DeclareLaunchArgument(
+                "front_camera_name", default_value="hikrobot_front_DB2163742"
+            ),
+            DeclareLaunchArgument(
+                "side_camera_name", default_value="hikrobot_side_DB2163749"
+            ),
+            DeclareLaunchArgument(
+                "top_camera_name", default_value="hikrobot_top_DB2163739"
+            ),
+            DeclareLaunchArgument(
+                "front_camera_info_url",
+                default_value=str(info_root / "hikrobot_DB2163742.yaml"),
+            ),
+            DeclareLaunchArgument(
+                "side_camera_info_url",
+                default_value=str(info_root / "hikrobot_DB2163749.yaml"),
+            ),
+            DeclareLaunchArgument(
+                "top_camera_info_url",
+                default_value=str(info_root / "hikrobot_DB2163739.yaml"),
+            ),
+            DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("auto_exposure", default_value="false"),
+            DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
+            DeclareLaunchArgument("can_interface", default_value="can0"),
+            DeclareLaunchArgument("calibration_speed", default_value="15"),
+            DeclareLaunchArgument(
+                "index_roll_calibration_speed", default_value="5"
+            ),
+            DeclareLaunchArgument(
+                "index_flex_calibration_speed", default_value="10"
+            ),
+            DeclareLaunchArgument(
+                "adaptive_formal_speed_enabled", default_value="true"
+            ),
+            DeclareLaunchArgument(
+                "cross_view_roll_diagnostic_finger", default_value=""
+            ),
+            DeclareLaunchArgument("validation_enabled", default_value="false"),
+            DeclareLaunchArgument(
+                "camera_extrinsics_file",
+                default_value=str(
+                    Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "source_urdf_path", default_value=""
+            ),
+            DeclareLaunchArgument(
+                "source_urdf_expected_sha256", default_value=""
+            ),
+            DeclareLaunchArgument(
+                "camera_extrinsics_expected_sha256", default_value=""
+            ),
+            DeclareLaunchArgument(
+                "calibration_config_expected_sha256", default_value=""
+            ),
+            DeclareLaunchArgument(
+                "tag_config_expected_sha256", default_value=""
+            ),
+            DeclareLaunchArgument("vendor_sdk_config", default_value=""),
+            DeclareLaunchArgument(
+                "sdk_config_expected_sha256", default_value=""
+            ),
+            DeclareLaunchArgument(
+                "corrected_urdf_output_dir", default_value=""
+            ),
+            DeclareLaunchArgument("commands_enabled", default_value="true"),
+            DeclareLaunchArgument("start_cameras", default_value="true"),
+            DeclareLaunchArgument("start_sdk", default_value="true"),
+            DeclareLaunchArgument("record_bag", default_value="false"),
+            DeclareLaunchArgument(
+                "output_root",
+                default_value=str(Path.cwd() / "calibration_output"),
+            ),
+            DeclareLaunchArgument("session_dir", default_value=""),
+            DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
+            DeclareLaunchArgument("recalibration_scope", default_value="full"),
+            DeclareLaunchArgument(
+                "calibration_config",
+                default_value=str(
+                    package_share / "config" / "three_camera_calibration.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "tag_config",
+                default_value="",
+            ),
+            OpaqueFunction(function=_launch_stack),
+        ]
+    )

src/linkerhand_calibration/launch/three_camera_extrinsics.launch.py

@@ -0,0 +1,209 @@
+"""Launch three Hikrobot cameras for one-time checkerboard extrinsics."""
+
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+VIEWS = ("front", "side", "top")
+
+
+def _launch(context):
+    cameras = []
+    rectifiers = []
+    serials = {}
+    for view in VIEWS:
+        serial = LaunchConfiguration(f"{view}_camera_serial").perform(context)
+        if not serial:
+            raise RuntimeError(f"{view}_camera_serial is required")
+        serials[view] = serial
+        namespace = f"/g20_extrinsics/{view}/camera"
+        cameras.append(
+            Node(
+                package="linkerhand_calibration",
+                executable="hikrobot_camera_node",
+                name="hikrobot_camera",
+                namespace=namespace,
+                output="screen",
+                emulate_tty=True,
+                parameters=[
+                    {
+                        "serial_number": serial,
+                        "expected_model": LaunchConfiguration("camera_model"),
+                        "camera_name": LaunchConfiguration(
+                            f"{view}_camera_name"
+                        ),
+                        "frame_id": f"g20_extrinsics_{view}_optical_frame",
+                        "image_width": 1624,
+                        "image_height": 1240,
+                        "frame_rate": ParameterValue(
+                            LaunchConfiguration("camera_frame_rate"),
+                            value_type=float,
+                        ),
+                        "exposure_time_us": ParameterValue(
+                            LaunchConfiguration("exposure_time_us"),
+                            value_type=float,
+                        ),
+                        "gain_db": ParameterValue(
+                            LaunchConfiguration("gain_db"), value_type=float
+                        ),
+                        "auto_exposure": False,
+                        "camera_info_url": LaunchConfiguration(
+                            f"{view}_camera_info_url"
+                        ),
+                    }
+                ],
+            )
+        )
+        rectifiers.append(
+            ComposableNode(
+                package="image_proc",
+                plugin="image_proc::RectifyNode",
+                name=f"rectify_{view}",
+                namespace=namespace,
+                remappings=[
+                    ("image", f"{namespace}/image_raw"),
+                    ("camera_info", f"{namespace}/camera_info"),
+                    ("image_rect", f"{namespace}/image_rect"),
+                ],
+                parameters=[{"queue_size": 1}],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            )
+        )
+    container = ComposableNodeContainer(
+        name="g20_extrinsics_vision",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=rectifiers,
+        output="screen",
+    )
+    solver = Node(
+        package="linkerhand_calibration",
+        executable="three_camera_extrinsics_node",
+        name="g20_camera_extrinsics",
+        output="screen",
+        emulate_tty=True,
+        parameters=[
+            {
+                "output_file": LaunchConfiguration("output_file"),
+                "checkerboard_columns": ParameterValue(
+                    LaunchConfiguration("checkerboard_columns"), value_type=int
+                ),
+                "checkerboard_rows": ParameterValue(
+                    LaunchConfiguration("checkerboard_rows"), value_type=int
+                ),
+                "square_size_m": ParameterValue(
+                    LaunchConfiguration("square_size_m"), value_type=float
+                ),
+                "enable_gui": ParameterValue(
+                    LaunchConfiguration("enable_gui"), value_type=bool
+                ),
+                "gui_refresh_hz": ParameterValue(
+                    LaunchConfiguration("gui_refresh_hz"), value_type=float
+                ),
+                "maximum_reprojection_rms_px": ParameterValue(
+                    LaunchConfiguration("maximum_reprojection_rms_px"),
+                    value_type=float,
+                ),
+                "maximum_candidate_pair_reprojection_rms_px": ParameterValue(
+                    LaunchConfiguration(
+                        "maximum_candidate_pair_reprojection_rms_px"
+                    ),
+                    value_type=float,
+                ),
+                "maximum_single_camera_reprojection_rms_px": ParameterValue(
+                    LaunchConfiguration(
+                        "maximum_single_camera_reprojection_rms_px"
+                    ),
+                    value_type=float,
+                ),
+                "auto_capture_default": ParameterValue(
+                    LaunchConfiguration("auto_capture_default"),
+                    value_type=bool,
+                ),
+                "auto_capture_stable_seconds": ParameterValue(
+                    LaunchConfiguration("auto_capture_stable_seconds"),
+                    value_type=float,
+                ),
+                **{
+                    f"{view}_camera_serial": serials[view]
+                    for view in VIEWS
+                },
+            }
+        ],
+    )
+    return [*cameras, container, solver]
+
+
+def generate_launch_description() -> LaunchDescription:
+    package_share = Path(
+        get_package_share_directory("linkerhand_calibration")
+    )
+    camera_info = Path.home() / ".ros" / "camera_info"
+    return LaunchDescription(
+        [
+            # Keep the large-image transport deterministic even when the
+            # calling shell selected another ROS 2 RMW implementation.
+            SetEnvironmentVariable(
+                name="RMW_IMPLEMENTATION",
+                value="rmw_fastrtps_cpp",
+            ),
+            SetEnvironmentVariable(
+                name="FASTDDS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
+            SetEnvironmentVariable(
+                name="FASTRTPS_DEFAULT_PROFILES_FILE",
+                value=str(
+                    package_share / "config" / "fastdds_large_images.xml"
+                ),
+            ),
+            DeclareLaunchArgument("front_camera_serial", default_value="DB2163742"),
+            DeclareLaunchArgument("side_camera_serial", default_value="DB2163749"),
+            DeclareLaunchArgument("top_camera_serial", default_value="DB2163739"),
+            DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
+            DeclareLaunchArgument("front_camera_name", default_value="hikrobot_front_DB2163742"),
+            DeclareLaunchArgument("side_camera_name", default_value="hikrobot_side_DB2163749"),
+            DeclareLaunchArgument("top_camera_name", default_value="hikrobot_top_DB2163739"),
+            DeclareLaunchArgument("front_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163742.yaml")),
+            DeclareLaunchArgument("side_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163749.yaml")),
+            DeclareLaunchArgument("top_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163739.yaml")),
+            DeclareLaunchArgument("camera_frame_rate", default_value="15.0"),
+            DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
+            DeclareLaunchArgument("gain_db", default_value="0.0"),
+            DeclareLaunchArgument("checkerboard_columns", default_value="8"),
+            DeclareLaunchArgument("checkerboard_rows", default_value="5"),
+            DeclareLaunchArgument("square_size_m", default_value="0.027"),
+            DeclareLaunchArgument("enable_gui", default_value="true"),
+            DeclareLaunchArgument("gui_refresh_hz", default_value="2.0"),
+            DeclareLaunchArgument(
+                "maximum_reprojection_rms_px", default_value="1.2"
+            ),
+            DeclareLaunchArgument(
+                "maximum_candidate_pair_reprojection_rms_px",
+                default_value="1.5",
+            ),
+            DeclareLaunchArgument(
+                "maximum_single_camera_reprojection_rms_px",
+                default_value="1.5",
+            ),
+            DeclareLaunchArgument("auto_capture_default", default_value="false"),
+            DeclareLaunchArgument(
+                "auto_capture_stable_seconds", default_value="1.0"
+            ),
+            DeclareLaunchArgument(
+                "output_file",
+                default_value=str(Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"),
+            ),
+            OpaqueFunction(function=_launch),
+        ]
+    )

src/linkerhand_calibration/linkerhand_calibration/__init__.py

@@ -0,0 +1,5 @@
+"""Profile-driven LinkerHand calibration and validated URDF correction."""
+
+from .core import CalibrationProfile, ProfileKey
+
+__all__ = ["CalibrationProfile", "ProfileKey"]

src/linkerhand_calibration/linkerhand_calibration/acquisition.py

@@ -9,7 +9,7 @@ from typing import Any, Mapping, Sequence
 
 import numpy as np
 
-from .core import PAIR_NAMES, robust_rotation_summary
+from .core import robust_rotation_summary
 from .pnp import SquareTagPose
 
 
@@ -18,6 +18,7 @@ TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
     "t3_t4": ("t3", "t4"),
     "t4_t5": ("t4", "t5"),
 }
+PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES)
 
 
 @dataclass(frozen=True)
@@ -51,6 +52,39 @@ def tag_quality_is_valid(
     )
 
 
+def update_pnp_reset_watchdog(
+    *,
+    detection_good: bool,
+    pnp_valid: bool,
+    now: float,
+    invalid_since: float | None,
+    reset_after_seconds: float,
+) -> tuple[float | None, bool]:
+    """Track continuous PnP-only failures and request a throttled reset."""
+    reset_after = float(reset_after_seconds)
+    if reset_after <= 0.0:
+        raise ValueError("reset_after_seconds must be positive")
+    if not detection_good or pnp_valid:
+        return None, False
+    since = float(now) if invalid_since is None else float(invalid_since)
+    if float(now) - since >= reset_after:
+        # Start a new interval so a permanently bad view is not reset on every
+        # frame. The next valid frame clears the interval.
+        return float(now), True
+    return since, False
+
+
+def required_resume_views(active_view: str | None) -> tuple[str, ...]:
+    """Require only the active view on resume; start still checks all views."""
+    all_views = ("front", "side", "top")
+    if active_view is None:
+        return all_views
+    view = str(active_view)
+    if view not in all_views:
+        raise ValueError(f"unknown calibration view: {view}")
+    return (view,)
+
+
 @dataclass(frozen=True)
 class Observation:
     stamp_ns: int
@@ -83,7 +117,7 @@ def interpolate_state_u8(
     *,
     maximum_skew_ns: int,
 ) -> tuple[tuple[float, ...], int] | None:
-    """Interpolate the 20-D hand state at an image timestamp.
+    """Interpolate a profile-sized hand state at an image timestamp.
 
     The SDK publishes state independently from the camera.  Continuous
     calibration must therefore use the image timestamp instead of whichever
@@ -117,7 +151,11 @@ def interpolate_state_u8(
         fraction = before_gap / denominator
         before_values = np.asarray(before.position_u8, dtype=float)
         after_values = np.asarray(after.position_u8, dtype=float)
-        if before_values.shape != (20,) or after_values.shape != (20,):
+        if (
+            before_values.ndim != 1
+            or before_values.size == 0
+            or after_values.shape != before_values.shape
+        ):
             return None
         interpolated = before_values + fraction * (after_values - before_values)
         return (
@@ -129,11 +167,16 @@ def interpolate_state_u8(
     if nearest is None:
         return None
     gap = abs(int(stamp_ns) - int(nearest.stamp_ns))
-    if gap > maximum_skew_ns or len(nearest.position_u8) != 20:
+    if gap > maximum_skew_ns or not nearest.position_u8:
         return None
     return (tuple(float(value) for value in nearest.position_u8), gap)
 
 
+# Physical-angle profiles use the same timestamp interpolation.  Keep the old
+# public name for compatibility and offer a unit-neutral spelling to new code.
+interpolate_state = interpolate_state_u8
+
+
 class ContinuousSweepCollector:
     """Collect timestamp-synchronised observations during one end-to-end move."""
 

src/linkerhand_calibration/linkerhand_calibration/alignment_view.py

@@ -0,0 +1,358 @@
+"""Visual roll-alignment aid for one G20 calibration camera."""
+
+from __future__ import annotations
+
+from collections import deque
+import math
+import time
+from typing import Any, Mapping, Sequence
+
+from apriltag_msgs.msg import AprilTagDetectionArray
+import cv2
+from cv_bridge import CvBridge
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import Image
+
+from .full_hand import VIEW_TAGS
+from .hikrobot_camera import configure_fastdds_large_image_transport
+from .zero_calibration import detect_reference_alignment_line
+
+
+VIEWS = ("front", "side", "top")
+
+
+def summarize_alignment_measurements(
+    measurements: Sequence[Mapping[str, Any] | None],
+) -> dict[str, Any] | None:
+    """Return a median-smoothed physical reference-line measurement."""
+    valid = [measurement for measurement in measurements if measurement]
+    if not valid:
+        return None
+    return {
+        "line_xyxy_px": np.median(
+            np.asarray(
+                [measurement["line_xyxy_px"] for measurement in valid],
+                dtype=float,
+            ),
+            axis=0,
+        ).tolist(),
+        "angle_rad": float(
+            np.median(
+                [float(measurement["angle_rad"]) for measurement in valid]
+            )
+        ),
+        "vertical_offset_px": float(
+            np.median(
+                [
+                    float(measurement["vertical_offset_px"])
+                    for measurement in valid
+                ]
+            )
+        ),
+        "detected_frames": len(valid),
+        "window_frames": len(measurements),
+    }
+
+
+class G20CameraAlignmentView(Node):
+    """Publish a red/blue roll aid based on a physical scene edge."""
+
+    def __init__(self) -> None:
+        """Configure one view without taking ownership of hand commands."""
+        super().__init__("g20_camera_alignment_view")
+        self.declare_parameter("view", "front")
+        view = str(self.get_parameter("view").value).strip().lower()
+        if view not in VIEWS:
+            raise ValueError(f"view must be one of {VIEWS}")
+        self.view = view
+
+        namespace = f"/g20_calibration/{view}"
+        self.required_tag_ids = {
+            int(value) for value in VIEW_TAGS[view].values()
+        }
+        self.declare_parameter("image_topic", f"{namespace}/camera/image_rect")
+        self.declare_parameter(
+            "detections_topic", f"{namespace}/apriltag/detections"
+        )
+        self.declare_parameter("reference_y_ratio", 0.90)
+        self.declare_parameter("roi_y_min_ratio", 0.55)
+        self.declare_parameter("roi_y_max_ratio", 0.98)
+        self.declare_parameter("minimum_line_length_ratio", 0.30)
+        self.declare_parameter("maximum_candidate_angle_deg", 15.0)
+        self.declare_parameter("maximum_alignment_error_deg", 0.5)
+        self.declare_parameter("maximum_vertical_offset_px", 12.0)
+        self.declare_parameter("maximum_hamming", 0)
+        self.declare_parameter("minimum_decision_margin", 20.0)
+        self.declare_parameter("minimum_edge_pixels", 20.0)
+        self.declare_parameter("smoothing_frames", 10)
+        self.declare_parameter("maximum_line_age_seconds", 1.0)
+        self.declare_parameter("maximum_tag_age_seconds", 1.0)
+        self.declare_parameter("maximum_publish_rate_hz", 10.0)
+        self.declare_parameter("output_scale", 0.75)
+
+        def value(name: str) -> Any:
+            return self.get_parameter(name).value
+
+        self.image_topic = str(value("image_topic"))
+        self.detections_topic = str(value("detections_topic"))
+        self.reference_y_ratio = float(value("reference_y_ratio"))
+        self.roi_y_min_ratio = float(value("roi_y_min_ratio"))
+        self.roi_y_max_ratio = float(value("roi_y_max_ratio"))
+        self.minimum_line_length_ratio = float(
+            value("minimum_line_length_ratio")
+        )
+        self.maximum_candidate_angle_rad = math.radians(
+            float(value("maximum_candidate_angle_deg"))
+        )
+        self.maximum_alignment_error_rad = math.radians(
+            float(value("maximum_alignment_error_deg"))
+        )
+        self.maximum_vertical_offset_px = float(
+            value("maximum_vertical_offset_px")
+        )
+        self.maximum_hamming = int(value("maximum_hamming"))
+        self.minimum_decision_margin = float(value("minimum_decision_margin"))
+        self.minimum_edge_pixels = float(value("minimum_edge_pixels"))
+        self.maximum_line_age_seconds = float(
+            value("maximum_line_age_seconds")
+        )
+        self.maximum_tag_age_seconds = float(value("maximum_tag_age_seconds"))
+        self.maximum_publish_rate_hz = float(value("maximum_publish_rate_hz"))
+        self.output_scale = float(value("output_scale"))
+        smoothing_frames = int(value("smoothing_frames"))
+
+        if not (
+            0.0
+            <= self.roi_y_min_ratio
+            < self.reference_y_ratio
+            < self.roi_y_max_ratio
+            <= 1.0
+        ):
+            raise ValueError(
+                "ratios must satisfy 0 <= roi_min < reference < roi_max <= 1"
+            )
+        if not 0.0 < self.minimum_line_length_ratio <= 1.0:
+            raise ValueError("minimum_line_length_ratio must be in (0, 1]")
+        if not (
+            0.0
+            < self.maximum_alignment_error_rad
+            < self.maximum_candidate_angle_rad
+            < math.pi / 2.0
+        ):
+            raise ValueError(
+                "angle limits must satisfy 0 < alignment < candidate < 90"
+            )
+        if self.maximum_vertical_offset_px <= 0.0:
+            raise ValueError("maximum_vertical_offset_px must be positive")
+        if smoothing_frames < 1:
+            raise ValueError("smoothing_frames must be positive")
+        if self.maximum_line_age_seconds <= 0.0:
+            raise ValueError("maximum_line_age_seconds must be positive")
+        if self.maximum_tag_age_seconds <= 0.0:
+            raise ValueError("maximum_tag_age_seconds must be positive")
+        if self.maximum_publish_rate_hz <= 0.0:
+            raise ValueError("maximum_publish_rate_hz must be positive")
+        if not 0.1 <= self.output_scale <= 1.0:
+            raise ValueError("output_scale must be in [0.1, 1.0]")
+
+        self.bridge = CvBridge()
+        self.line_history: deque[dict[str, Any] | None] = deque(
+            maxlen=smoothing_frames
+        )
+        self.last_line_at = 0.0
+        self.latest_tag_corners: dict[int, np.ndarray] = {}
+        self.latest_tag_at: dict[int, float] = {}
+        self.last_publish_at = 0.0
+        self.publisher = self.create_publisher(
+            Image, "~/image", qos_profile_sensor_data
+        )
+        self.create_subscription(
+            AprilTagDetectionArray,
+            self.detections_topic,
+            self._detections_callback,
+            qos_profile_sensor_data,
+        )
+        self.create_subscription(
+            Image,
+            self.image_topic,
+            self._image_callback,
+            qos_profile_sensor_data,
+        )
+        self.get_logger().info(
+            f"{view} alignment view uses physical long-edge detection; "
+            f"Tag orientation is ignored; input={self.image_topic}; "
+            f"output={self.get_name()}/image"
+        )
+
+    def _detections_callback(self, message: AprilTagDetectionArray) -> None:
+        now = time.monotonic()
+        for detection in message.detections:
+            tag_id = int(detection.id)
+            if tag_id not in self.required_tag_ids:
+                continue
+            corners = np.asarray(
+                [
+                    [float(point.x), float(point.y)]
+                    for point in detection.corners
+                ],
+                dtype=float,
+            )
+            if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
+                continue
+            edges = np.linalg.norm(
+                corners - np.roll(corners, -1, axis=0), axis=1
+            )
+            if (
+                int(detection.hamming) > self.maximum_hamming
+                or float(detection.decision_margin)
+                < self.minimum_decision_margin
+                or float(np.mean(edges)) < self.minimum_edge_pixels
+            ):
+                continue
+            self.latest_tag_corners[tag_id] = corners
+            self.latest_tag_at[tag_id] = now
+
+    def _draw_tags(self, image: np.ndarray, now: float) -> None:
+        for tag_id in sorted(self.required_tag_ids):
+            corners = self.latest_tag_corners.get(tag_id)
+            detected_at = self.latest_tag_at.get(tag_id, 0.0)
+            if (
+                corners is None
+                or now - detected_at > self.maximum_tag_age_seconds
+            ):
+                continue
+            points = np.rint(corners * self.output_scale).astype(np.int32)
+            cv2.polylines(image, [points], True, (0, 220, 0), 2)
+            centre = np.rint(np.mean(points, axis=0)).astype(int)
+            cv2.putText(
+                image,
+                f"ID {tag_id}",
+                (int(centre[0]) + 5, int(centre[1]) - 7),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.55,
+                (0, 220, 0),
+                2,
+            )
+
+    def _image_callback(self, message: Image) -> None:
+        # Avoid conversion and Hough work until an image viewer subscribes.
+        if self.publisher.get_subscription_count() < 1:
+            return
+        now = time.monotonic()
+        if now - self.last_publish_at < 1.0 / self.maximum_publish_rate_hz:
+            return
+        self.last_publish_at = now
+        try:
+            image = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
+        except Exception as error:
+            self.get_logger().warning(
+                f"alignment image conversion failed: {error}"
+            )
+            return
+        if self.output_scale != 1.0:
+            image = cv2.resize(
+                image,
+                None,
+                fx=self.output_scale,
+                fy=self.output_scale,
+                interpolation=cv2.INTER_AREA,
+            )
+
+        height, width = image.shape[:2]
+        reference_y = self.reference_y_ratio * float(height - 1)
+        detected = detect_reference_alignment_line(
+            image,
+            reference_y_px=reference_y,
+            roi_y_min_ratio=self.roi_y_min_ratio,
+            roi_y_max_ratio=self.roi_y_max_ratio,
+            minimum_length_ratio=self.minimum_line_length_ratio,
+            maximum_candidate_angle_rad=self.maximum_candidate_angle_rad,
+        )
+        self.line_history.append(detected)
+        if detected is not None:
+            self.last_line_at = now
+        measurement = summarize_alignment_measurements(self.line_history)
+        if now - self.last_line_at > self.maximum_line_age_seconds:
+            measurement = None
+
+        red_y = int(round(reference_y))
+        cv2.line(
+            image,
+            (15, red_y),
+            (max(15, width - 15), red_y),
+            (0, 0, 255),
+            4,
+        )
+        if measurement is not None:
+            line = np.rint(measurement["line_xyxy_px"]).astype(int)
+            blue_ok, blue_start, blue_end = cv2.clipLine(
+                (0, 0, width, height),
+                (int(line[0]), int(line[1])),
+                (int(line[2]), int(line[3])),
+            )
+            if blue_ok:
+                cv2.line(image, blue_start, blue_end, (255, 0, 0), 3)
+            angle_rad = float(measurement["angle_rad"])
+            offset_px = float(measurement["vertical_offset_px"])
+            aligned = bool(
+                abs(angle_rad) <= self.maximum_alignment_error_rad
+                and abs(offset_px) <= self.maximum_vertical_offset_px
+            )
+            status = "ALIGNED" if aligned else "ADJUST CAMERA"
+            status_text = (
+                f"{self.view.upper()}  red-blue "
+                f"{math.degrees(angle_rad):+.2f} deg  "
+                f"dy {offset_px:+.1f}px  {status}"
+            )
+            status_color = (0, 220, 0) if aligned else (0, 165, 255)
+        else:
+            status_text = (
+                f"{self.view.upper()}  PHYSICAL REFERENCE LINE NOT DETECTED"
+            )
+            status_color = (0, 165, 255)
+        cv2.putText(
+            image,
+            status_text,
+            (20, 34),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.72,
+            status_color,
+            2,
+        )
+        self._draw_tags(image, now)
+        cv2.putText(
+            image,
+            "RED=target  BLUE=physical edge  GREEN=Tags (angle ignored)",
+            (20, max(64, height - 24)),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.60,
+            (255, 255, 255),
+            2,
+        )
+        output = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")
+        output.header = message.header
+        self.publisher.publish(output)
+
+
+def main(args: list[str] | None = None) -> None:
+    """Run the single-view alignment helper."""
+    configure_fastdds_large_image_transport()
+    rclpy.init(args=args)
+    node: G20CameraAlignmentView | None = None
+    try:
+        node = G20CameraAlignmentView()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py

@@ -0,0 +1,371 @@
+"""Map model SDK u8 feedback to URDF joint angles using one calibration JSON.
+
+The static encoder-zero corrections in ``zero_angles`` are already baked into
+the corrected URDF joint origins.  This bridge therefore publishes only the
+dynamic ``angle_rad`` values and never adds the static offsets a second time.
+
+Schema-v5 trajectories are fitted against timestamp-synchronised hardware
+feedback, not controller set-points.  They must therefore be queried with the
+SDK ``hand_state`` topic.  The retained schema-v4 path is command-indexed for
+backwards compatibility only.
+"""
+
+from __future__ import annotations
+
+import json
+import math
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+from .full_hand import (
+    get_hand_calibration_profile,
+    infer_compact_payload_layout,
+    validate_compact_payload,
+)
+from .models import get_default_registry, validate_schema_v6_runtime_payload
+from .models.o12.artifacts import validate_o12_runtime_payload
+from .core import ProfileKey
+
+
+G20_COMMAND_NAMES: tuple[str, ...] = (
+    "thumb_cmc_pitch",
+    "index_mcp_pitch",
+    "middle_mcp_pitch",
+    "ring_mcp_pitch",
+    "pinky_mcp_pitch",
+    "thumb_cmc_roll",
+    "index_mcp_roll",
+    "middle_mcp_roll",
+    "ring_mcp_roll",
+    "pinky_mcp_roll",
+    "thumb_cmc_yaw",
+    "reserved_11",
+    "reserved_12",
+    "reserved_13",
+    "reserved_14",
+    "thumb_mcp",
+    "index_pip",
+    "middle_pip",
+    "ring_pip",
+    "pinky_pip",
+)
+
+# Match the stable ordering used by the existing MuJoCo bridge.  JointState
+# consumers must use names, but retaining the ordering also keeps logs and
+# direct comparisons deterministic.
+G20_URDF_JOINT_NAMES: tuple[str, ...] = (
+    "index_dip",
+    "index_mcp_pitch",
+    "index_mcp_roll",
+    "index_pip",
+    "middle_dip",
+    "middle_mcp_pitch",
+    "middle_mcp_roll",
+    "middle_pip",
+    "pinky_dip",
+    "pinky_mcp_pitch",
+    "pinky_mcp_roll",
+    "pinky_pip",
+    "ring_dip",
+    "ring_mcp_pitch",
+    "ring_mcp_roll",
+    "ring_pip",
+    "thumb_cmc_pitch",
+    "thumb_cmc_roll",
+    "thumb_cmc_yaw",
+    "thumb_ip",
+    "thumb_mcp",
+)
+
+
+class CalibratedCommandMapper:
+    """Validated, profile-specific lookup from SDK u8 values to URDF radians."""
+
+    def __init__(
+        self, payload: Mapping[str, Any], *, expected_side: str | None = None
+    ) -> None:
+        schema_version = int(payload["schema_version"])
+        if schema_version == 7:
+            validate_o12_runtime_payload(payload)
+        elif schema_version == 6:
+            validate_schema_v6_runtime_payload(payload)
+        else:
+            validate_compact_payload(payload)
+        side = str(payload["side"]).lower()
+        if expected_side is not None and side != str(expected_side).lower():
+            raise ValueError(
+                f"calibration side {side!r} does not match requested side "
+                f"{str(expected_side).lower()!r}"
+            )
+        quality = payload["quality"]
+        if quality.get("passed") is not True:
+            raise ValueError("calibration quality.passed must be true")
+        layout_id = (
+            str(payload["layout_id"])
+            if schema_version in {6, 7}
+            else infer_compact_payload_layout(payload)
+        )
+        self.side = side
+        self.layout_id = layout_id
+        self.model = str(payload["model"]).upper()
+        self.profile_id = str(
+            payload.get("profile_id", f"G20/{side}/{layout_id}/v1")
+        )
+        self.serial_number = str(payload["serial_number"])
+        self.input_domain = str(
+            payload.get(
+                "curve_input_domain",
+                "command_u8" if schema_version == 4 else "",
+            )
+        )
+        if self.input_domain not in {"command_u8", "feedback_u8", "feedback_rad"}:
+            raise ValueError("calibration curve_input_domain is invalid")
+        if schema_version in {6, 7}:
+            self.command_names = tuple(str(value) for value in payload["command_names"])
+            self.urdf_joint_names = tuple(str(name) for name in payload["joints"])
+            self._motor_by_joint = {
+                name: int(payload["joints"][name]["motor_index"])
+                for name in self.urdf_joint_names
+            }
+            registered = get_default_registry().get(
+                ProfileKey.parse(self.profile_id)
+            )
+            self.feedback_name_aliases = dict(
+                registered.profile.command.feedback_name_aliases
+            )
+            self.feedback_by_index = bool(
+                registered.profile.command.feedback_by_index
+            )
+        else:
+            profile = get_hand_calibration_profile(side, layout_id)
+            self.command_names = G20_COMMAND_NAMES
+            self.urdf_joint_names = G20_URDF_JOINT_NAMES
+            self._motor_by_joint = {
+                name: int(profile.joint_specs[name].motor_index)
+                for name in self.urdf_joint_names
+            }
+            self.feedback_name_aliases = {}
+            self.feedback_by_index = False
+        self._curves = {
+            name: tuple(
+                float(value)
+                for value in payload["joints"][name]["angle_rad"]
+            )
+            for name in self.urdf_joint_names
+        }
+        self._decreasing_curves = {
+            name: tuple(
+                float(value)
+                for value in payload["joints"][name].get(
+                    "decreasing_rad", payload["joints"][name]["angle_rad"]
+                )
+            )
+            for name in self.urdf_joint_names
+        }
+        self._increasing_curves = {
+            name: tuple(
+                float(value)
+                for value in payload["joints"][name].get(
+                    "increasing_rad", payload["joints"][name]["angle_rad"]
+                )
+            )
+            for name in self.urdf_joint_names
+        }
+        self._previous_by_motor: dict[int, float] = {}
+        self._direction_by_motor: dict[int, str] = {}
+        self.direction_deadband_u8 = 0.002 if schema_version == 7 else 0.5
+        self._knots = {
+            name: tuple(float(value) for value in payload["joints"][name].get(
+                "curve_input_knots_rad", ()
+            ))
+            for name in self.urdf_joint_names
+        }
+        self._raw_increasing_branch = {
+            name: str(payload["joints"][name].get(
+                "raw_increasing_curve_branch", "increasing"
+            ))
+            for name in self.urdf_joint_names
+        }
+
+    @staticmethod
+    def _command_index(value: float) -> int:
+        command = float(value)
+        if not math.isfinite(command):
+            raise ValueError("calibrated command positions must be finite")
+        return max(0, min(255, int(math.floor(command + 0.5))))
+
+    def map_positions(
+        self, positions: Sequence[float], names: Sequence[str] = ()
+    ) -> tuple[float, ...]:
+        values = tuple(float(value) for value in positions)
+        if names and not self.feedback_by_index:
+            if len(names) != len(values):
+                raise ValueError(
+                    "JointState names and positions must have equal length"
+                )
+            if len(set(names)) != len(names):
+                raise ValueError("JointState names must be unique")
+            by_name = dict(zip((str(name) for name in names), values))
+            for alias, canonical in self.feedback_name_aliases.items():
+                if alias in by_name and canonical not in by_name:
+                    by_name[canonical] = by_name[alias]
+            missing = [name for name in self.command_names if name not in by_name]
+            if missing:
+                raise ValueError(
+                    f"{self.model} feedback is missing named channels: "
+                    + ",".join(missing)
+                )
+            command = tuple(by_name[name] for name in self.command_names)
+        else:
+            if len(values) != len(self.command_names):
+                raise ValueError(
+                    f"unnamed {self.model} feedback must contain exactly "
+                    f"{len(self.command_names)} positions"
+                )
+            command = values
+        indices = (
+            () if self.input_domain == "feedback_rad"
+            else tuple(self._command_index(value) for value in command)
+        )
+        direction_by_motor: dict[int, str | None] = {}
+        for motor, value in enumerate(command):
+            previous = self._previous_by_motor.get(motor)
+            direction = self._direction_by_motor.get(motor)
+            if previous is not None:
+                if value > previous + self.direction_deadband_u8:
+                    direction = "increasing"
+                elif value < previous - self.direction_deadband_u8:
+                    direction = "decreasing"
+            direction_by_motor[motor] = direction
+        result: list[float] = []
+        for name in self.urdf_joint_names:
+            motor = self._motor_by_joint[name]
+            direction = direction_by_motor[motor]
+            if self.input_domain == "feedback_rad" and direction is not None:
+                raw_increasing = self._raw_increasing_branch[name]
+                direction = (
+                    raw_increasing
+                    if direction == "increasing"
+                    else "increasing" if raw_increasing == "decreasing" else "decreasing"
+                )
+            curves = (
+                self._increasing_curves
+                if direction == "increasing"
+                else self._decreasing_curves
+                if direction == "decreasing"
+                else self._curves
+            )
+            if self.input_domain == "feedback_rad":
+                result.append(float(np.interp(command[motor], self._knots[name], curves[name])))
+            else:
+                result.append(curves[name][indices[motor]])
+        for motor, value in enumerate(command):
+            self._previous_by_motor[motor] = value
+            direction = direction_by_motor[motor]
+            if direction is not None:
+                self._direction_by_motor[motor] = direction
+        return tuple(result)
+
+
+def load_calibrated_command_mapper(
+    calibration_file: str | Path, *, expected_side: str | None = None
+) -> CalibratedCommandMapper:
+    path = Path(calibration_file).expanduser().resolve()
+    if not path.is_file():
+        raise ValueError(f"calibration JSON does not exist: {path}")
+    payload = json.loads(path.read_text(encoding="utf-8"))
+    return CalibratedCommandMapper(payload, expected_side=expected_side)
+
+
+def default_input_topic(
+    hand_type: str, input_domain: str, model: str = "G20"
+) -> str:
+    side = str(hand_type).lower()
+    if side not in {"left", "right"}:
+        raise ValueError("hand_type must be left or right")
+    if input_domain == "feedback_u8":
+        return f"/{str(model).lower()}/cb_{side}_hand_state"
+    if input_domain == "command_u8":
+        return f"/{str(model).lower()}/cb_{side}_hand_control_cmd"
+    if input_domain == "feedback_rad":
+        return f"/{str(model).lower()}/{side}/joint_states"
+    raise ValueError("calibration curve_input_domain is invalid")
+
+
+class CalibratedJointStateBridge(Node):
+    def __init__(self) -> None:
+        super().__init__("calibrated_joint_state_bridge")
+        self.declare_parameter("hand_type", "right")
+        self.declare_parameter("calibration_file", "")
+        self.declare_parameter("input_topic", "")
+        self.declare_parameter("output_topic", "")
+
+        hand_type = str(self.get_parameter("hand_type").value).lower()
+        if hand_type not in {"left", "right"}:
+            raise ValueError("hand_type must be left or right")
+        calibration_file = str(self.get_parameter("calibration_file").value)
+        if not calibration_file:
+            raise ValueError("calibration_file is required")
+        self.mapper = load_calibrated_command_mapper(
+            calibration_file, expected_side=hand_type
+        )
+        input_topic = str(self.get_parameter("input_topic").value).strip()
+        output_topic = str(self.get_parameter("output_topic").value).strip()
+        self.input_topic = input_topic or default_input_topic(
+            hand_type, self.mapper.input_domain, self.mapper.model
+        )
+        self.output_topic = (
+            output_topic
+            or f"/sim/mujoco/{self.mapper.model.lower()}/{hand_type}/joint_state"
+        )
+        self.publisher = self.create_publisher(JointState, self.output_topic, 10)
+        self.subscription = self.create_subscription(
+            JointState, self.input_topic, self._command_callback, 10
+        )
+        self._last_error = ""
+        self.get_logger().info(
+            f"loaded {self.mapper.profile_id} calibration for "
+            f"{self.mapper.serial_number}: "
+            f"{self.input_topic} ({self.mapper.input_domain}) -> "
+            f"{self.output_topic}"
+        )
+
+    def _command_callback(self, command: JointState) -> None:
+        try:
+            positions = self.mapper.map_positions(command.position, command.name)
+        except ValueError as error:
+            message = str(error)
+            if message != self._last_error:
+                self.get_logger().error(message)
+                self._last_error = message
+            return
+        self._last_error = ""
+        result = JointState()
+        result.header = command.header
+        result.name = list(self.mapper.urdf_joint_names)
+        result.position = list(positions)
+        self.publisher.publish(result)
+
+
+def main(args: Sequence[str] | None = None) -> None:
+    rclpy.init(args=args)
+    node: CalibratedJointStateBridge | None = None
+    try:
+        node = CalibratedJointStateBridge()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py

@@ -0,0 +1,21 @@
+"""Compatibility adapters for one-release calibration migrations."""
+
+from .config_v1 import (
+    legacy_default_profile_key,
+    product_profile_key,
+    resolve_legacy_profile_alias,
+)
+from .defaults import (
+    default_product_config_path,
+    default_three_camera_config_path,
+)
+from .paths import resolve_renamed_package_path
+
+__all__ = [
+    "default_product_config_path",
+    "default_three_camera_config_path",
+    "legacy_default_profile_key",
+    "product_profile_key",
+    "resolve_legacy_profile_alias",
+    "resolve_renamed_package_path",
+]

src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py

@@ -0,0 +1,47 @@
+"""Identity migration for deployed product configuration schemas."""
+
+from __future__ import annotations
+
+from typing import Any, Mapping
+
+from ..core import ProfileKey
+
+
+def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey:
+    version = int(raw.get("schema_version", -1))
+    if version in {2, 3}:
+        key = ProfileKey.parse(str(raw.get("profile_id", "")))
+        for field, actual in (
+            ("model", key.model),
+            ("side", key.side),
+            ("tag_layout", key.layout),
+        ):
+            configured = str(raw.get(field, "")).strip()
+            if configured and configured.lower() != actual.lower():
+                raise ValueError(f"{field} differs from profile_id")
+        return key
+    if version != 1:
+        raise ValueError("product config schema_version must be 1, 2 or 3")
+    model = str(raw.get("model", "")).strip().upper()
+    side = str(raw.get("side", "")).strip().lower()
+    layout = str(raw.get("tag_layout", "")).strip().lower()
+    if not layout and (model, side) == ("G20", "right"):
+        layout = "g20_right_19"
+    return ProfileKey(model, side, layout, 1)
+
+
+def legacy_default_profile_key() -> ProfileKey:
+    """Preserve the former no-argument executable for one release."""
+    return ProfileKey("G20", "right", "g20_right_19", 1)
+
+
+def resolve_legacy_profile_alias(key: ProfileKey) -> ProfileKey:
+    """Map retired layout identifiers to their reviewed physical profile."""
+    if (
+        key.model == "G20"
+        and key.side == "right"
+        and key.layout == "g20_right_15"
+        and key.revision == 1
+    ):
+        return ProfileKey("G20", "right", "g20_right_19", 1)
+    return key

src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py

@@ -0,0 +1,16 @@
+"""One-release default selection for invocations without ``--config``."""
+
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+
+
+def default_product_config_path() -> Path:
+    share = Path(get_package_share_directory("linkerhand_calibration"))
+    return share / "config/g20_right_product.yaml"
+
+
+def default_three_camera_config_path() -> Path:
+    """Resolve the installed calibration defaults through the ROS index."""
+    share = Path(get_package_share_directory("linkerhand_calibration"))
+    return share / "config/three_camera_calibration.yaml"

src/linkerhand_calibration/linkerhand_calibration/compat/legacy/__init__.py

@@ -0,0 +1,4 @@
+"""Legacy single-camera algorithms retained for one compatibility release."""
+from .session_v1 import uses_coupled_full_hand_zero_solver
+
+__all__ = ["uses_coupled_full_hand_zero_solver"]

src/linkerhand_calibration/linkerhand_calibration/compat/legacy/session_v1.py

@@ -0,0 +1,24 @@
+"""Version selection for replaying durable pre-v3 hardware sessions."""
+
+from __future__ import annotations
+
+from typing import Any, Mapping
+
+
+def uses_coupled_full_hand_zero_solver(
+    session_start: Mapping[str, Any],
+) -> bool:
+    """Return the solver contract recorded by the legacy session header.
+
+    Capabilities are not consulted by the live runtime.  This adapter reads
+    the durable v1 header only so offline replay can reproduce an artifact
+    created before the independent thumb solver was introduced.
+    """
+    capabilities = {
+        str(value) for value in session_start.get("capabilities", ())
+    }
+    return (
+        int(session_start.get("sample_schema_version", 1)) == 1
+        and "palm_axis_side_channel_v2" in capabilities
+        and "palm_axis_relative_motion_v3" not in capabilities
+    )

src/linkerhand_calibration/linkerhand_calibration/compat/paths.py

@@ -0,0 +1,52 @@
+"""Path compatibility for immutable v1 product configurations."""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+
+_LEGACY_SOURCE_PREFIX = Path("src/g20_thumb_apriltag_calibration")
+_CURRENT_SOURCE_PREFIX = Path("src/linkerhand_calibration")
+
+
+def _resolve_package_uri(value: str, workspace: Path) -> Path | None:
+    prefix = "package://"
+    if not value.startswith(prefix):
+        return None
+    package_name, separator, relative = value[len(prefix) :].partition("/")
+    if not separator or not package_name or not relative:
+        raise ValueError(f"invalid ROS package resource path: {value}")
+    workspace_candidate = (workspace / "src" / package_name / relative).resolve()
+    if workspace_candidate.exists():
+        return workspace_candidate
+    try:
+        from ament_index_python.packages import get_package_share_directory
+
+        package_share = Path(get_package_share_directory(package_name))
+    except Exception:
+        return workspace_candidate
+    return (package_share / relative).resolve()
+
+
+def resolve_renamed_package_path(value: str | Path, workspace: Path) -> Path:
+    """Resolve workspace paths, ROS package resources and the former prefix.
+
+    Deployed v1 product YAML files are kept byte-for-byte stable because the
+    artifact paths participate in operational review.  Existing paths always
+    win; package URIs prefer a source-workspace copy, and the rename mapping
+    is used only when the literal legacy path no longer exists.
+    """
+    text = str(value).strip()
+    package_resource = _resolve_package_uri(text, workspace)
+    if package_resource is not None:
+        return package_resource
+    raw = Path(text).expanduser()
+    candidate = raw if raw.is_absolute() else workspace / raw
+    candidate = candidate.resolve()
+    if candidate.exists() or raw.is_absolute():
+        return candidate
+    try:
+        suffix = raw.relative_to(_LEGACY_SOURCE_PREFIX)
+    except ValueError:
+        return candidate
+    return (workspace / _CURRENT_SOURCE_PREFIX / suffix).resolve()

src/linkerhand_calibration/linkerhand_calibration/core/__init__.py

@@ -0,0 +1,76 @@
+"""Hardware- and model-independent calibration kernel."""
+
+from .domain import (
+    ArtifactPolicy,
+    CalibrationProfile,
+    CommandLayout,
+    MeasurementPolicy,
+    MeasurementSpec,
+    MotionPolicy,
+    ProfileKey,
+    ProfileValidationError,
+    QualityPolicy,
+    SampleRecord,
+    ScopePolicy,
+    TagSpec,
+    TaskSpec,
+    ViewSpec,
+    VisionRigSpec,
+    ZeroSolvePolicy,
+    validate_profile,
+)
+from .domain.task import (
+    DIRECTION_DECREASING,
+    DIRECTION_INCREASING,
+    DIRECTIONS,
+    PHASE_ROOT,
+    PHASE_TIP,
+)
+from .fitting import FitResult, isotonic_nonincreasing
+from .geometry import (
+    delta_rotation_vector,
+    fit_rotation_axis,
+    image_plane_tag_quaternion_xyzw,
+    normalize_quaternion_xyzw,
+    relative_quaternion_xyzw,
+    robust_rotation_summary,
+    rotation_inlier_fraction,
+    rotation_rms_rad,
+    rotation_spread_rad,
+)
+
+__all__ = [
+    "ArtifactPolicy",
+    "CalibrationProfile",
+    "CommandLayout",
+    "DIRECTION_DECREASING",
+    "DIRECTION_INCREASING",
+    "DIRECTIONS",
+    "FitResult",
+    "MeasurementPolicy",
+    "MeasurementSpec",
+    "MotionPolicy",
+    "PHASE_ROOT",
+    "PHASE_TIP",
+    "ProfileKey",
+    "ProfileValidationError",
+    "QualityPolicy",
+    "SampleRecord",
+    "ScopePolicy",
+    "TagSpec",
+    "TaskSpec",
+    "ViewSpec",
+    "VisionRigSpec",
+    "ZeroSolvePolicy",
+    "delta_rotation_vector",
+    "fit_rotation_axis",
+    "image_plane_tag_quaternion_xyzw",
+    "isotonic_nonincreasing",
+    "normalize_quaternion_xyzw",
+    "relative_quaternion_xyzw",
+    "robust_rotation_summary",
+    "rotation_inlier_fraction",
+    "rotation_rms_rad",
+    "rotation_spread_rad",
+    "validate_profile",
+]

src/linkerhand_calibration/linkerhand_calibration/core/artifacts/__init__.py

@@ -0,0 +1,5 @@
+"""Artifact schema and release validation contracts."""
+
+from .release import ReleaseValidation, ReleaseValidator
+
+__all__ = ["ReleaseValidation", "ReleaseValidator"]

src/linkerhand_calibration/linkerhand_calibration/core/artifacts/release.py

@@ -0,0 +1,27 @@
+"""Release validation protocol used before atomic publication."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Mapping, Protocol
+
+from ..domain import CalibrationProfile
+from ..urdf import UrdfCorrectionPlan
+
+
+@dataclass(frozen=True)
+class ReleaseValidation:
+    passed: bool
+    errors: tuple[str, ...] = ()
+    verified_hashes: Mapping[str, str] | None = None
+
+
+class ReleaseValidator(Protocol):
+    def validate_release(
+        self,
+        profile: CalibrationProfile,
+        plan: UrdfCorrectionPlan,
+        calibration_json: Path,
+        corrected_urdf: Path,
+    ) -> ReleaseValidation: ...

src/linkerhand_calibration/linkerhand_calibration/core/artifacts/storage.py

@@ -30,6 +30,24 @@ def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None:
         os.fsync(stream.fileno())
 
 
+def append_jsonl_many(
+    path: str | Path, payloads: Iterable[Mapping[str, Any]]
+) -> None:
+    """Durably append a batch while paying the fsync cost only once."""
+    destination = Path(path)
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    lines = [
+        json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
+        for payload in payloads
+    ]
+    if not lines:
+        return
+    with destination.open("a", encoding="utf-8") as stream:
+        stream.write("\n".join(lines) + "\n")
+        stream.flush()
+        os.fsync(stream.fileno())
+
+
 def load_jsonl(path: str | Path) -> list[dict[str, Any]]:
     source = Path(path)
     if not source.exists():

src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py

@@ -0,0 +1,41 @@
+"""Calibration domain types."""
+
+from .profile import (
+    ArtifactPolicy,
+    CalibrationProfile,
+    CommandLayout,
+    MeasurementPolicy,
+    MeasurementSpec,
+    MotionPolicy,
+    ProfileKey,
+    ProfileValidationError,
+    QualityPolicy,
+    ScopePolicy,
+    TagSpec,
+    TaskSpec,
+    ViewSpec,
+    VisionRigSpec,
+    ZeroSolvePolicy,
+    validate_profile,
+)
+from .sample import SampleRecord
+
+__all__ = [
+    "ArtifactPolicy",
+    "CalibrationProfile",
+    "CommandLayout",
+    "MeasurementPolicy",
+    "MeasurementSpec",
+    "MotionPolicy",
+    "ProfileKey",
+    "ProfileValidationError",
+    "QualityPolicy",
+    "SampleRecord",
+    "ScopePolicy",
+    "TagSpec",
+    "TaskSpec",
+    "ViewSpec",
+    "VisionRigSpec",
+    "ZeroSolvePolicy",
+    "validate_profile",
+]

src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py

@@ -0,0 +1,475 @@
+"""Typed, hardware-independent calibration profile contracts."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+import math
+from pathlib import PurePath
+from typing import Mapping
+
+
+@dataclass(frozen=True, order=True)
+class ProfileKey:
+    """Stable identity for one independently reviewed hand profile."""
+
+    model: str
+    side: str
+    layout: str
+    revision: int = 1
+
+    def __post_init__(self) -> None:
+        object.__setattr__(self, "model", str(self.model).strip().upper())
+        object.__setattr__(self, "side", str(self.side).strip().lower())
+        object.__setattr__(self, "layout", str(self.layout).strip().lower())
+        object.__setattr__(self, "revision", int(self.revision))
+        if not self.model or not self.side or not self.layout:
+            raise ValueError("profile identity fields must be non-empty")
+        if self.side not in {"left", "right"}:
+            raise ValueError("profile side must be left or right")
+        if self.revision < 1:
+            raise ValueError("profile revision must be positive")
+
+    @property
+    def profile_id(self) -> str:
+        return f"{self.model}/{self.side}/{self.layout}/v{self.revision}"
+
+    @classmethod
+    def parse(cls, value: str) -> "ProfileKey":
+        parts = str(value).strip().split("/")
+        if len(parts) != 4 or not parts[3].startswith("v"):
+            raise ValueError(
+                "profile_id must be MODEL/side/layout/vREVISION"
+            )
+        return cls(parts[0], parts[1], parts[2], int(parts[3][1:]))
+
+
+@dataclass(frozen=True)
+class CommandLayout:
+    """Command channels, joint bindings, and the reviewed baseline pose."""
+
+    names: tuple[str, ...]
+    baseline_u8: tuple[int, ...]
+    command_index_by_joint: Mapping[str, int]
+    disabled_indices: frozenset[int] = frozenset()
+    # Calibration names are allowed to stay model-neutral while the source
+    # URDF keeps any vendor/side prefixes (for example ``rh_``).
+    urdf_joint_by_joint: Mapping[str, str] = field(default_factory=dict)
+    # Older SDKs occasionally published a wrong label for a physically stable
+    # channel.  Aliases are accepted only at the declared channel index.
+    feedback_name_aliases: Mapping[str, str] = field(default_factory=dict)
+    # SDK speed commands are not necessarily one value per position channel.
+    # This mapping makes that protocol detail explicit in a profile.
+    speed_slot_by_command_index: Mapping[int, int] = field(default_factory=dict)
+    # Physical-angle SDKs use these fields.  ``baseline_u8`` remains the
+    # deployed compatibility contract for legacy byte-command profiles.
+    unit: str = "u8"
+    baseline: tuple[float, ...] = ()
+    lower_bounds: tuple[float, ...] = ()
+    upper_bounds: tuple[float, ...] = ()
+    feedback_by_index: bool = False
+
+    @property
+    def command_count(self) -> int:
+        return len(self.names)
+
+    @property
+    def baseline_values(self) -> tuple[float, ...]:
+        source = self.baseline if self.baseline else self.baseline_u8
+        return tuple(float(value) for value in source)
+
+    @property
+    def minimum_values(self) -> tuple[float, ...]:
+        return (
+            tuple(float(value) for value in self.lower_bounds)
+            if self.lower_bounds
+            else (0.0,) * self.command_count
+        )
+
+    @property
+    def maximum_values(self) -> tuple[float, ...]:
+        return (
+            tuple(float(value) for value in self.upper_bounds)
+            if self.upper_bounds
+            else (255.0,) * self.command_count
+        )
+
+    def normalize(self, index: int, value: float) -> float:
+        lower = self.minimum_values[int(index)]
+        upper = self.maximum_values[int(index)]
+        if not math.isfinite(float(value)) or upper <= lower:
+            raise ValueError("command value or bounds are invalid")
+        return (float(value) - lower) / (upper - lower)
+
+    def denormalize(self, index: int, progress: float) -> float:
+        lower = self.minimum_values[int(index)]
+        upper = self.maximum_values[int(index)]
+        phase = float(progress)
+        if not math.isfinite(phase) or not 0.0 <= phase <= 1.0:
+            raise ValueError("normalized command progress must be in [0, 1]")
+        return lower + phase * (upper - lower)
+
+
+@dataclass(frozen=True)
+class TagSpec:
+    role: str
+    tag_id: int
+    fixed_reference: bool = False
+
+
+@dataclass(frozen=True)
+class ViewSpec:
+    name: str
+    tags: tuple[TagSpec, ...]
+
+
+@dataclass(frozen=True)
+class VisionRigSpec:
+    """Any number of named views and their Tag roles."""
+
+    views: tuple[ViewSpec, ...]
+    common_frame: str
+    extrinsic_reference_view: str
+    extrinsics_quality_limits: Mapping[str, float] = field(
+        default_factory=dict
+    )
+    minimum_capture_counts: Mapping[str, int] = field(default_factory=dict)
+
+    @property
+    def view_names(self) -> tuple[str, ...]:
+        return tuple(view.name for view in self.views)
+
+    @property
+    def tag_ids(self) -> frozenset[int]:
+        return frozenset(tag.tag_id for view in self.views for tag in view.tags)
+
+
+@dataclass(frozen=True)
+class TaskSpec:
+    key: str
+    view: str
+    command_index: int
+    joints: tuple[str, ...]
+    auxiliary_commands: tuple[tuple[int, float], ...] = ()
+    validation_only: bool = False
+    start_u8: int = 255
+    end_u8: int = 0
+    preflight_speed_u8: int | None = None
+    formal_speed_u8: int | None = None
+    start: float | None = None
+    end: float | None = None
+    preflight_speed: float | None = None
+    formal_speed: float | None = None
+
+    @property
+    def start_value(self) -> float:
+        return float(self.start_u8 if self.start is None else self.start)
+
+    @property
+    def end_value(self) -> float:
+        return float(self.end_u8 if self.end is None else self.end)
+
+
+@dataclass(frozen=True)
+class MotionPolicy:
+    """Reviewed motion tasks and optional safe waypoint sequences."""
+
+    tasks: tuple[TaskSpec, ...]
+    preparation_waypoints_u8: tuple[tuple[int, ...], ...] = ()
+    safe_return_waypoints_u8: tuple[tuple[int, ...], ...] = ()
+    speed_parameters: Mapping[str, float] = field(default_factory=dict)
+    precheck_sweeps: bool = False
+    steady_command_checkpoints: bool = False
+
+
+@dataclass(frozen=True)
+class MeasurementSpec:
+    joint: str
+    kind: str
+    view: str | None
+    parent_role: str | None
+    child_role: str | None
+    validation_source: str | None = None
+    # Some measured trajectories publish only a dynamic curve while their
+    # static URDF zero/axis remains CAD- or mimic-owned.  For those joints a
+    # monocular 3-D axis-line residual is useful diagnostic evidence, but it
+    # must not reject an otherwise clean image/SO(3) trajectory merely because
+    # the hand was placed at a different valid position in the camera view.
+    pose_axis_line_required: bool = True
+
+
+@dataclass(frozen=True)
+class MeasurementPolicy:
+    measurements: Mapping[str, MeasurementSpec]
+    cross_view_sources: Mapping[str, str] = field(default_factory=dict)
+    image_curve_joints: frozenset[str] = frozenset()
+    directional_zero: bool = False
+    cross_view_roll_curve: bool = False
+    stable_cross_view_cone_bias: bool = False
+
+
+@dataclass(frozen=True)
+class ZeroSolvePolicy:
+    active_joints: frozenset[str]
+    passive_joints: frozenset[str]
+    direct_zero_joints: tuple[str, ...]
+    axis_joints: tuple[str, ...]
+    mechanical_endpoint_joints: frozenset[str]
+    post_solve_endpoint_joints: frozenset[str]
+    mimic_source_by_joint: Mapping[str, str]
+    cad_frozen_joints: frozenset[str]
+    # ``upper_at_end`` means TaskSpec.end_u8 is the trusted source-URDF upper
+    # physical endpoint.  The measured travel then defines the electrical
+    # zero and corrected [0, travel] coordinate range.
+    endpoint_anchor_by_joint: Mapping[str, str] = field(default_factory=dict)
+    fitted_mimic_joints: frozenset[str] = frozenset()
+    # Passive coupling is not necessarily representable by the linear URDF
+    # ``mimic`` element.  Profiles must opt in explicitly before a nonlinear
+    # runtime/MuJoCo relation may be published.
+    coupling_model_by_joint: Mapping[str, str] = field(default_factory=dict)
+
+
+@dataclass(frozen=True)
+class QualityPolicy:
+    training_cycles: tuple[int, ...]
+    holdout_cycle: int | None
+    hard_threshold_keys: frozenset[str]
+    retry_metric_scope: Mapping[str, str] = field(default_factory=dict)
+    isolated_holdout: bool = False
+
+
+@dataclass(frozen=True)
+class ScopePolicy:
+    calibrate_joints: Mapping[str, frozenset[str]]
+    frozen_joints: Mapping[str, frozenset[str]]
+    default_scope: str = "full"
+
+    def selected_joints(self, scope: str) -> frozenset[str]:
+        try:
+            return self.calibrate_joints[str(scope)]
+        except KeyError as error:
+            raise ValueError(f"unsupported calibration scope: {scope}") from error
+
+
+@dataclass(frozen=True)
+class ArtifactPolicy:
+    output_schema_version: int
+    calibration_filename: str
+    corrected_urdf_filename: str
+    protected_input_fields: frozenset[str]
+    publication_pointer: str = "latest_passed"
+    session_compatibility_tokens: frozenset[str] = frozenset()
+    publish_corrected_urdf: bool = False
+
+
+@dataclass(frozen=True)
+class CalibrationProfile:
+    key: ProfileKey
+    namespace: str
+    command: CommandLayout
+    vision: VisionRigSpec
+    motion: MotionPolicy
+    measurement: MeasurementPolicy
+    zero: ZeroSolvePolicy
+    quality: QualityPolicy
+    scope: ScopePolicy
+    artifacts: ArtifactPolicy
+    # Per-URDF-joint provenance used by partial calibration artifacts.
+    # Known values are: measured_static_dynamic, measured_dynamic_cad_static,
+    # transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal,
+    # and mimic_nominal.
+    joint_coverage: Mapping[str, str] = field(default_factory=dict)
+
+
+class ProfileValidationError(ValueError):
+    """Raised before hardware startup when a profile is internally unsafe."""
+
+
+def validate_profile(profile: CalibrationProfile) -> None:
+    """Hard-check all cross-policy references before hardware is enabled."""
+    errors: list[str] = []
+    command = profile.command
+    baseline = command.baseline_values
+    if not command.names or len(command.names) != len(baseline):
+        errors.append("command names and baseline must be non-empty and aligned")
+    if len(set(command.names)) != len(command.names):
+        errors.append("command names must be unique")
+    if command.unit not in {"u8", "rad"}:
+        errors.append("command unit must be u8 or rad")
+    if command.unit == "u8" and any(value < 0 or value > 255 for value in baseline):
+        errors.append("baseline command values must be in [0, 255]")
+    if (
+        len(command.minimum_values) != command.command_count
+        or len(command.maximum_values) != command.command_count
+    ):
+        errors.append("command bounds must align with command names")
+    elif any(
+        not math.isfinite(lower)
+        or not math.isfinite(upper)
+        or lower >= upper
+        or not lower <= value <= upper
+        for lower, upper, value in zip(
+            command.minimum_values, command.maximum_values, baseline
+        )
+    ):
+        errors.append("command bounds or baseline values are invalid")
+    indices = set(range(command.command_count))
+    if not set(command.disabled_indices).issubset(indices):
+        errors.append("disabled command index is out of range")
+    if any(index not in indices for index in command.command_index_by_joint.values()):
+        errors.append("joint command index is out of range")
+    if command.urdf_joint_by_joint:
+        if not set(command.command_index_by_joint).issubset(
+            command.urdf_joint_by_joint
+        ):
+            errors.append("every commanded joint must map to a URDF joint")
+        urdf_names = tuple(command.urdf_joint_by_joint.values())
+        if len(set(urdf_names)) != len(urdf_names):
+            errors.append("URDF joint mappings must be unique")
+    if any(
+        index not in indices or slot < 0
+        for index, slot in command.speed_slot_by_command_index.items()
+    ):
+        errors.append("speed-slot mapping is invalid")
+    if any(
+        not str(alias).strip() or canonical not in command.names
+        for alias, canonical in command.feedback_name_aliases.items()
+    ):
+        errors.append("feedback name alias is not part of the command schema")
+
+    view_names = profile.vision.view_names
+    if not view_names or len(set(view_names)) != len(view_names):
+        errors.append("vision views must be non-empty and unique")
+    if profile.vision.extrinsic_reference_view not in view_names:
+        errors.append("extrinsic reference view is not declared")
+    tag_ids = [tag.tag_id for view in profile.vision.views for tag in view.tags]
+    tag_roles = [tag.role for view in profile.vision.views for tag in view.tags]
+    if len(set(tag_ids)) != len(tag_ids):
+        errors.append("Tag IDs must be unique across views")
+    if len(set(tag_roles)) != len(tag_roles):
+        errors.append("Tag roles must be unique across views")
+    if not any(
+        tag.fixed_reference for view in profile.vision.views for tag in view.tags
+    ):
+        errors.append("at least one fixed reference Tag is required")
+
+    task_keys = [task.key for task in profile.motion.tasks]
+    if not task_keys or len(set(task_keys)) != len(task_keys):
+        errors.append("motion task keys must be non-empty and unique")
+    measurement_names = set(profile.measurement.measurements)
+    for task in profile.motion.tasks:
+        if task.view not in view_names:
+            errors.append(f"task {task.key} uses an unknown view")
+        if task.command_index not in indices:
+            errors.append(f"task {task.key} command index is out of range")
+        if not task.joints or not set(task.joints).issubset(measurement_names):
+            errors.append(f"task {task.key} references unknown measurements")
+        if any(index not in indices for index, _ in task.auxiliary_commands):
+            errors.append(f"task {task.key} auxiliary index is out of range")
+        elif any(
+            not command.minimum_values[index] <= float(value) <= command.maximum_values[index]
+            for index, value in task.auxiliary_commands
+        ):
+            errors.append(f"task {task.key} auxiliary command is out of range")
+        lower = command.minimum_values[task.command_index]
+        upper = command.maximum_values[task.command_index]
+        if not lower <= task.start_value <= upper or not lower <= task.end_value <= upper:
+            errors.append(f"task {task.key} sweep endpoint is out of range")
+        if task.start_value == task.end_value:
+            errors.append(f"task {task.key} sweep endpoints must differ")
+        for speed in (task.preflight_speed_u8, task.formal_speed_u8):
+            if speed is not None and not 0 <= speed <= 255:
+                errors.append(f"task {task.key} speed is out of range")
+        if command.unit == "rad" and (
+            task.formal_speed is None or float(task.formal_speed) <= 0.0
+        ):
+            errors.append(f"task {task.key} physical speed must be positive")
+    for name, spec in profile.measurement.measurements.items():
+        if name != spec.joint:
+            errors.append(f"measurement mapping key differs for {name}")
+        if spec.view is not None and spec.view not in view_names:
+            errors.append(f"measurement {name} uses an unknown view")
+    for primary, validation in profile.measurement.cross_view_sources.items():
+        if primary not in measurement_names or validation not in measurement_names:
+            errors.append("cross-view measurement source is unknown")
+
+    zero = profile.zero
+    if zero.active_joints & zero.passive_joints:
+        errors.append("active and passive joints must be disjoint")
+    all_joints = zero.active_joints | zero.passive_joints
+    if not zero.active_joints.issubset(command.command_index_by_joint):
+        errors.append("every active joint must bind to a command channel")
+    if not set(zero.direct_zero_joints).issubset(zero.active_joints):
+        errors.append("direct zero targets must be active joints")
+    if not set(zero.axis_joints).issubset(all_joints):
+        errors.append("axis targets must be known joints")
+    if not zero.mechanical_endpoint_joints.issubset(zero.active_joints):
+        errors.append("mechanical endpoint targets must be active joints")
+    if not zero.post_solve_endpoint_joints.issubset(zero.active_joints):
+        errors.append("post-solve endpoint targets must be active joints")
+    if not set(zero.mimic_source_by_joint).issubset(zero.passive_joints):
+        errors.append("mimic targets must be passive joints")
+    if not set(zero.mimic_source_by_joint.values()).issubset(all_joints):
+        errors.append("mimic sources must be known joints")
+    if not set(zero.endpoint_anchor_by_joint).issubset(zero.active_joints):
+        errors.append("endpoint anchors must target active joints")
+    if not set(zero.endpoint_anchor_by_joint.values()).issubset(
+        {
+            "upper_at_end",
+            "lower_at_start",
+            "zero_at_start",
+            "cad_range_center",
+        }
+    ):
+        errors.append("endpoint anchor policy is unsupported")
+    if not zero.fitted_mimic_joints.issubset(zero.passive_joints):
+        errors.append("fitted mimic targets must be passive joints")
+    if not zero.fitted_mimic_joints.issubset(zero.mimic_source_by_joint):
+        errors.append("fitted mimic target has no source mapping")
+    if not set(zero.coupling_model_by_joint).issubset(
+        zero.mimic_source_by_joint
+    ):
+        errors.append("coupling model target has no source mapping")
+    if not set(zero.coupling_model_by_joint.values()).issubset(
+        {"linear_mimic", "quadratic_runtime"}
+    ):
+        errors.append("coupling model policy is unsupported")
+
+    scopes = set(profile.scope.calibrate_joints)
+    if profile.scope.default_scope not in scopes:
+        errors.append("default scope is not declared")
+    if scopes != set(profile.scope.frozen_joints):
+        errors.append("scope calibration and frozen mappings must align")
+    for name in scopes:
+        selected = profile.scope.calibrate_joints[name]
+        frozen = profile.scope.frozen_joints[name]
+        if selected & frozen or selected | frozen != zero.active_joints:
+            errors.append(f"scope {name} must partition all active joints")
+
+    artifacts = profile.artifacts
+    if artifacts.output_schema_version < 1:
+        errors.append("artifact schema version must be positive")
+    for label, filename in (
+        ("calibration", artifacts.calibration_filename),
+        ("corrected URDF", artifacts.corrected_urdf_filename),
+        ("publication pointer", artifacts.publication_pointer),
+    ):
+        if not filename or PurePath(filename).name != filename:
+            errors.append(f"{label} filename must not contain a directory")
+    if not profile.namespace.startswith("/"):
+        errors.append("runtime namespace must be absolute")
+    if profile.joint_coverage:
+        valid_coverage = {
+            "measured_static_dynamic",
+            "measured_dynamic_cad_static",
+            "transferred_static_dynamic",
+            "transferred_dynamic_cad_static",
+            "cad_nominal",
+            "mimic_nominal",
+        }
+        if set(profile.joint_coverage) != all_joints:
+            errors.append("joint coverage must describe every profile joint")
+        if not set(profile.joint_coverage.values()).issubset(valid_coverage):
+            errors.append("joint coverage contains an unsupported status")
+
+    if errors:
+        raise ProfileValidationError("; ".join(errors))

src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py

@@ -0,0 +1,39 @@
+"""Normalized records shared by online evaluation and offline replay."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+import math
+from typing import Any, Mapping
+
+
+@dataclass(frozen=True)
+class SampleRecord:
+    task_key: str
+    measurement: str
+    view: str
+    cycle: int
+    direction: str
+    command_u8: int
+    timestamp_ns: int
+    values: Mapping[str, Any]
+    quality: Mapping[str, float] = field(default_factory=dict)
+    command: float | None = None
+    command_unit: str = "u8"
+    progress_01: float | None = None
+
+    def __post_init__(self) -> None:
+        if not self.task_key or not self.measurement or not self.view:
+            raise ValueError("sample task, measurement, and view are required")
+        if self.cycle < 0:
+            raise ValueError("sample cycle or command is out of range")
+        if self.command_unit == "u8" and not 0 <= self.command_u8 <= 255:
+            raise ValueError("sample cycle or command is out of range")
+        if self.command_unit not in {"u8", "rad"}:
+            raise ValueError("sample command unit must be u8 or rad")
+        if self.command is not None and not math.isfinite(float(self.command)):
+            raise ValueError("sample command must be finite")
+        if self.progress_01 is not None and not 0.0 <= float(self.progress_01) <= 1.0:
+            raise ValueError("sample progress must be in [0, 1]")
+        if self.timestamp_ns < 0:
+            raise ValueError("sample timestamp must be non-negative")

src/linkerhand_calibration/linkerhand_calibration/core/domain/sample_schema.py

@@ -0,0 +1,192 @@
+"""Canonical command/feedback schema for calibration observations.
+
+The hand command and its measured motor feedback are different physical
+domains. Durable samples always retain both. Fitting code may still use the
+historical ``command_u8`` key, but it is created only as an explicit projection
+of a canonical record at the fitting boundary.
+"""
+
+from __future__ import annotations
+
+import math
+from typing import Any, Iterable, Literal, Mapping
+
+import numpy as np
+
+
+SAMPLE_KINDS = frozenset(
+    {
+        'sample',
+        'baseline_hold_sample',
+        'steady_command_sample',
+        'palm_axis_sample',
+    }
+)
+
+FitDomain = Literal['default', 'requested', 'feedback']
+
+
+class SampleDataContractError(ValueError):
+    """A calibration observation mixes or omits command domains."""
+
+
+def _finite_u8(value: Any, field: str, *, integral: bool) -> int | float:
+    try:
+        number = float(value)
+    except (TypeError, ValueError) as error:
+        raise SampleDataContractError(
+            f'DATA-CONTRACT-701:{field} must be numeric'
+        ) from error
+    if not math.isfinite(number) or not 0.0 <= number <= 255.0:
+        raise SampleDataContractError(
+            f'DATA-CONTRACT-701:{field} must be finite and in [0, 255]'
+        )
+    if integral:
+        rounded = int(round(number))
+        if not math.isclose(number, rounded, rel_tol=0.0, abs_tol=1.0e-9):
+            raise SampleDataContractError(
+                f'DATA-CONTRACT-701:{field} must be an integer command'
+            )
+        return rounded
+    return number
+
+
+def explicit_domain_value(
+    source: Mapping[str, Any], domain: Literal['requested', 'feedback']
+) -> int | float:
+    """Read and validate one explicitly named domain from any observation."""
+    field = (
+        'requested_command_u8' if domain == 'requested' else 'feedback_u8'
+    )
+    if field not in source or source[field] is None:
+        raise SampleDataContractError(
+            f'DATA-CONTRACT-701:observation is missing explicit {field}'
+        )
+    return _finite_u8(source[field], field, integral=domain == 'requested')
+
+
+def canonical_sample_record(
+    source: Mapping[str, Any],
+    *,
+    allow_legacy_command: bool = False,
+) -> dict[str, Any]:
+    """Return one durable, unambiguous calibration observation.
+
+    ``allow_legacy_command`` is restricted to importing historical sessions
+    and unit fixtures. New online observations must provide both explicit
+    fields and therefore cannot silently reinterpret ``command_u8``.
+    """
+    record = dict(source)
+    kind = str(record.get('kind', ''))
+    if not kind and allow_legacy_command:
+        # Old in-memory steady-curve fixtures predate durable sample kinds.
+        # This adapter is never enabled by the new online/import contract.
+        kind = 'steady_command_sample'
+        record['kind'] = kind
+    if kind not in SAMPLE_KINDS:
+        raise SampleDataContractError(
+            f'DATA-CONTRACT-701:unsupported calibration sample kind {kind!r}'
+        )
+
+    requested = record.get('requested_command_u8')
+    feedback = record.get('feedback_u8')
+    legacy = record.get('command_u8')
+    if requested is None or feedback is None:
+        if not allow_legacy_command or legacy is None:
+            missing = [
+                name
+                for name, value in (
+                    ('requested_command_u8', requested),
+                    ('feedback_u8', feedback),
+                )
+                if value is None
+            ]
+            raise SampleDataContractError(
+                'DATA-CONTRACT-701:'
+                f'{kind} is missing explicit {",".join(missing)}'
+            )
+        # Historical in-memory records used requested commands for settled
+        # checkpoints and feedback bins for dense/baseline/palm observations.
+        if requested is None:
+            requested = legacy
+        if feedback is None:
+            feedback = legacy
+
+    record.pop('command_u8', None)
+    record['requested_command_u8'] = explicit_domain_value(
+        {'requested_command_u8': requested}, 'requested'
+    )
+    record['feedback_u8'] = explicit_domain_value(
+        {'feedback_u8': feedback}, 'feedback'
+    )
+    return record
+
+
+def fitting_sample_record(
+    source: Mapping[str, Any],
+    *,
+    domain: FitDomain = 'default',
+    allow_legacy_command: bool = False,
+    snap_requested_endpoints: bool = False,
+) -> dict[str, Any]:
+    """Project a canonical sample into the legacy curve-fitter interface."""
+    record = canonical_sample_record(
+        source, allow_legacy_command=allow_legacy_command
+    )
+    kind = str(record['kind'])
+    selected = domain
+    if selected == 'default':
+        selected = (
+            'requested' if kind == 'steady_command_sample' else 'feedback'
+        )
+    if selected not in {'requested', 'feedback'}:
+        raise SampleDataContractError(
+            f'DATA-CONTRACT-701:unsupported fitting domain {domain!r}'
+        )
+    requested = int(record['requested_command_u8'])
+    if selected == 'requested' or (
+        snap_requested_endpoints and requested in {0, 255}
+    ):
+        index = requested
+    else:
+        index = int(
+            np.clip(np.rint(float(record['feedback_u8'])), 0, 255)
+        )
+    record['command_u8'] = index
+    return record
+
+
+def fitting_sample_records(
+    records: Iterable[Mapping[str, Any]],
+    *,
+    domain: FitDomain = 'default',
+    allow_legacy_command: bool = False,
+    snap_requested_endpoints: bool = False,
+) -> list[dict[str, Any]]:
+    """Project several canonical samples into one explicit fitting domain."""
+    return [
+        fitting_sample_record(
+            record,
+            domain=domain,
+            allow_legacy_command=allow_legacy_command,
+            snap_requested_endpoints=snap_requested_endpoints,
+        )
+        for record in records
+    ]
+
+
+def validate_sample_records(
+    records: Iterable[Mapping[str, Any]],
+    *,
+    allow_legacy_command: bool = False,
+) -> None:
+    """Validate a collection without changing its representation."""
+    for index, record in enumerate(records):
+        try:
+            canonical_sample_record(
+                record, allow_legacy_command=allow_legacy_command
+            )
+        except SampleDataContractError as error:
+            raise SampleDataContractError(
+                f'{error};record_index={index}'
+            ) from error

src/linkerhand_calibration/linkerhand_calibration/core/domain/task.py

@@ -0,0 +1,19 @@
+"""Shared task direction vocabulary."""
+
+DIRECTION_DECREASING = "decreasing"
+DIRECTION_INCREASING = "increasing"
+DIRECTIONS: tuple[str, ...] = (
+    DIRECTION_DECREASING,
+    DIRECTION_INCREASING,
+)
+
+PHASE_ROOT = "root"
+PHASE_TIP = "tip"
+
+__all__ = [
+    "DIRECTION_DECREASING",
+    "DIRECTION_INCREASING",
+    "DIRECTIONS",
+    "PHASE_ROOT",
+    "PHASE_TIP",
+]

src/linkerhand_calibration/linkerhand_calibration/core/fitting/__init__.py

@@ -0,0 +1,5 @@
+"""Pure curve and axis fitting."""
+
+from .curve import FitResult, isotonic_nonincreasing
+
+__all__ = ["FitResult", "isotonic_nonincreasing"]

src/linkerhand_calibration/linkerhand_calibration/core/fitting/curve.py

@@ -0,0 +1,70 @@
+"""Model-independent curve fitting result and monotonic projection."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Any, Sequence
+
+import numpy as np
+
+from ..geometry import delta_rotation_vector
+
+
+def isotonic_nonincreasing(values: Sequence[float]) -> np.ndarray:
+    """Unweighted PAVA projection onto non-increasing values."""
+    original = np.asarray(values, dtype=float)
+    if original.ndim != 1 or not np.all(np.isfinite(original)):
+        raise ValueError("values must be a finite vector")
+    negated = -original
+    levels: list[float] = []
+    weights: list[int] = []
+    starts: list[int] = []
+    for index, value in enumerate(negated):
+        levels.append(float(value))
+        weights.append(1)
+        starts.append(index)
+        while len(levels) >= 2 and levels[-2] > levels[-1]:
+            total_weight = weights[-2] + weights[-1]
+            merged = (
+                levels[-2] * weights[-2] + levels[-1] * weights[-1]
+            ) / total_weight
+            levels[-2:] = [merged]
+            weights[-2:] = [total_weight]
+            starts.pop()
+    projected = np.empty_like(original)
+    for block_index, (level, start) in enumerate(zip(levels, starts)):
+        end = (
+            starts[block_index + 1]
+            if block_index + 1 < len(starts)
+            else len(original)
+        )
+        projected[start:end] = -level
+    return projected
+
+
+@dataclass(frozen=True)
+class FitResult:
+    joints: dict[str, dict[str, Any]]
+    axes: dict[str, tuple[float, float, float]]
+    references: dict[str, tuple[float, float, float, float]]
+    ip_coupling: dict[str, float]
+    max_monotonic_correction_rad: float
+    max_hysteresis_rad: float
+    measurement_mode: str = "rotation"
+    trajectory_models: dict[str, Any] = field(default_factory=dict)
+    trajectory_quality: dict[str, Any] = field(default_factory=dict)
+
+    def measure_from_reference(
+        self,
+        joint_name: str,
+        observed_quaternion_xyzw: Sequence[float],
+        reference_quaternion_xyzw: Sequence[float] | None = None,
+    ) -> float:
+        reference = (
+            reference_quaternion_xyzw
+            if reference_quaternion_xyzw is not None
+            else self.references[joint_name]
+        )
+        vector = delta_rotation_vector(reference, observed_quaternion_xyzw)
+        axis = np.asarray(self.axes[joint_name], dtype=float)
+        return float(vector @ axis)

src/linkerhand_calibration/linkerhand_calibration/core/geometry/__init__.py

@@ -0,0 +1,41 @@
+"""Pure geometry used by online and offline calibration."""
+
+from .extrinsics import (
+    CameraCalibrationIdentity,
+    CameraExtrinsics,
+    camera_info_fingerprint,
+    load_camera_extrinsics,
+    matrix_payload,
+    transform_matrix,
+    validate_camera_extrinsics_payload,
+)
+from .rotation import (
+    delta_rotation_vector,
+    fit_rotation_axis,
+    image_plane_tag_quaternion_xyzw,
+    normalize_quaternion_xyzw,
+    relative_quaternion_xyzw,
+    robust_rotation_summary,
+    rotation_inlier_fraction,
+    rotation_rms_rad,
+    rotation_spread_rad,
+)
+
+__all__ = [
+    "CameraCalibrationIdentity",
+    "CameraExtrinsics",
+    "camera_info_fingerprint",
+    "delta_rotation_vector",
+    "fit_rotation_axis",
+    "image_plane_tag_quaternion_xyzw",
+    "load_camera_extrinsics",
+    "matrix_payload",
+    "normalize_quaternion_xyzw",
+    "relative_quaternion_xyzw",
+    "robust_rotation_summary",
+    "rotation_inlier_fraction",
+    "rotation_rms_rad",
+    "rotation_spread_rad",
+    "transform_matrix",
+    "validate_camera_extrinsics_payload",
+]

src/linkerhand_calibration/linkerhand_calibration/core/geometry/extrinsics.py

@@ -0,0 +1,212 @@
+"""Camera-extrinsic data model shared by calibration and runtime nodes."""
+
+from __future__ import annotations
+
+import hashlib
+import json
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+import yaml
+from scipy.spatial.transform import Rotation
+
+
+def camera_info_fingerprint(
+    *,
+    width: int,
+    height: int,
+    camera_matrix: Sequence[Sequence[float]] | Sequence[float],
+    distortion: Sequence[float] = (),
+    rectification: Sequence[float] = (),
+    projection: Sequence[float] = (),
+) -> str:
+    """Return a stable fingerprint for rectified image geometry."""
+    matrix = np.asarray(camera_matrix, dtype=float).reshape(3, 3)
+    payload = {
+        "width": int(width),
+        "height": int(height),
+        "camera_matrix": [round(float(value), 12) for value in matrix.flat],
+        "distortion": [round(float(value), 12) for value in distortion],
+        "rectification": [round(float(value), 12) for value in rectification],
+        "projection": [round(float(value), 12) for value in projection],
+    }
+    encoded = json.dumps(
+        payload, sort_keys=True, separators=(",", ":")
+    ).encode("utf-8")
+    return hashlib.sha256(encoded).hexdigest()
+
+
+def transform_matrix(
+    translation_xyz_m: Sequence[float],
+    quaternion_xyzw: Sequence[float],
+) -> np.ndarray:
+    translation = np.asarray(translation_xyz_m, dtype=float)
+    quaternion = np.asarray(quaternion_xyzw, dtype=float)
+    if translation.shape != (3,) or not np.all(np.isfinite(translation)):
+        raise ValueError("translation_xyz_m must contain three finite values")
+    if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
+        raise ValueError("quaternion_xyzw must contain four finite values")
+    norm = float(np.linalg.norm(quaternion))
+    if norm < 1.0e-12:
+        raise ValueError("quaternion_xyzw has zero norm")
+    result = np.eye(4, dtype=float)
+    result[:3, :3] = Rotation.from_quat(quaternion / norm).as_matrix()
+    result[:3, 3] = translation
+    return result
+
+
+def matrix_payload(matrix: Sequence[Sequence[float]]) -> dict[str, list[float]]:
+    value = np.asarray(matrix, dtype=float)
+    if value.shape != (4, 4) or not np.all(np.isfinite(value)):
+        raise ValueError("transform must be a finite 4x4 matrix")
+    return {
+        "translation_xyz_m": [float(item) for item in value[:3, 3]],
+        "quaternion_xyzw": [
+            float(item) for item in Rotation.from_matrix(value[:3, :3]).as_quat()
+        ],
+    }
+
+
+@dataclass(frozen=True)
+class CameraCalibrationIdentity:
+    serial_number: str
+    width: int
+    height: int
+    intrinsics_sha256: str
+
+
+@dataclass(frozen=True)
+class CameraExtrinsics:
+    """Transforms every declared camera into one Profile-selected reference."""
+
+    cameras: Mapping[str, CameraCalibrationIdentity]
+    reference_view: str
+    reference_from_view: Mapping[str, np.ndarray]
+    quality: Mapping[str, float]
+
+    def transform(self, view: str) -> np.ndarray:
+        if view not in self.reference_from_view:
+            raise KeyError(f"extrinsics do not contain view {view}")
+        return np.asarray(self.reference_from_view[view], dtype=float).copy()
+
+
+def validate_camera_extrinsics_payload(
+    payload: Mapping[str, Any],
+    *,
+    required_views: Sequence[str],
+    reference_view: str,
+    quality_limits: Mapping[str, float] | None = None,
+    minimum_capture_counts: Mapping[str, int] | None = None,
+) -> None:
+    if int(payload.get("schema_version", -1)) != 1:
+        raise ValueError("camera extrinsics schema_version must be 1")
+    reference = str(reference_view)
+    if payload.get("reference_view") != reference:
+        raise ValueError(
+            "camera extrinsics reference_view differs from the Profile"
+        )
+    cameras = payload.get("cameras")
+    transforms = payload.get(f"{reference}_from_view")
+    quality = payload.get("quality")
+    views = tuple(str(view) for view in required_views)
+    if not views or len(set(views)) != len(views):
+        raise ValueError("required extrinsic views must be non-empty and unique")
+    if reference not in views:
+        raise ValueError("extrinsic reference view is not required")
+    if not isinstance(cameras, Mapping) or set(cameras) != set(views):
+        raise ValueError("camera extrinsics differ from the Profile views")
+    if not isinstance(transforms, Mapping) or set(transforms) != set(views):
+        raise ValueError("camera transforms differ from the Profile views")
+    if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
+        raise ValueError("camera extrinsics quality is not passed")
+    for key, limit in dict(quality_limits or {}).items():
+        value = float(quality.get(key, float("inf")))
+        if not np.isfinite(value) or value > limit:
+            raise ValueError(
+                f"camera extrinsics {key}={value} exceeds {limit}"
+            )
+    for key, minimum in dict(minimum_capture_counts or {}).items():
+        if int(quality.get(key, 0)) < int(minimum):
+            raise ValueError(
+                f"camera extrinsics {key} must be at least {minimum}"
+            )
+    for view in views:
+        identity = cameras[view]
+        if not isinstance(identity, Mapping):
+            raise ValueError(f"{view} camera identity must be an object")
+        if not str(identity.get("serial_number", "")):
+            raise ValueError(f"{view} camera serial_number is missing")
+        if int(identity.get("width", 0)) <= 0 or int(identity.get("height", 0)) <= 0:
+            raise ValueError(f"{view} camera image dimensions are invalid")
+        fingerprint = str(identity.get("intrinsics_sha256", ""))
+        if len(fingerprint) != 64:
+            raise ValueError(f"{view} camera intrinsics fingerprint is invalid")
+        transform = transforms[view]
+        if not isinstance(transform, Mapping):
+            raise ValueError(f"{view} transform must be an object")
+        matrix = transform_matrix(
+            transform.get("translation_xyz_m", ()),
+            transform.get("quaternion_xyzw", ()),
+        )
+        if view == reference and not np.allclose(
+            matrix, np.eye(4), atol=1.0e-9
+        ):
+            raise ValueError("reference-view transform must be identity")
+    serials = [str(cameras[view]["serial_number"]) for view in views]
+    if len(set(serials)) != len(views):
+        raise ValueError("camera extrinsics serial numbers must be unique")
+
+
+def load_camera_extrinsics(
+    path: str | Path,
+    *,
+    required_views: Sequence[str],
+    reference_view: str,
+    quality_limits: Mapping[str, float] | None = None,
+    minimum_capture_counts: Mapping[str, int] | None = None,
+) -> CameraExtrinsics:
+    source = Path(path).expanduser().resolve()
+    if not source.is_file():
+        raise ValueError(f"camera extrinsics file does not exist: {source}")
+    with source.open("r", encoding="utf-8") as stream:
+        payload = yaml.safe_load(stream)
+    if not isinstance(payload, Mapping):
+        raise ValueError("camera extrinsics file must contain an object")
+    validate_camera_extrinsics_payload(
+        payload,
+        required_views=required_views,
+        reference_view=reference_view,
+        quality_limits=quality_limits,
+        minimum_capture_counts=minimum_capture_counts,
+    )
+    views = tuple(str(view) for view in required_views)
+    transform_key = f"{reference_view}_from_view"
+    cameras = {
+        view: CameraCalibrationIdentity(
+            serial_number=str(payload["cameras"][view]["serial_number"]),
+            width=int(payload["cameras"][view]["width"]),
+            height=int(payload["cameras"][view]["height"]),
+            intrinsics_sha256=str(
+                payload["cameras"][view]["intrinsics_sha256"]
+            ),
+        )
+        for view in views
+    }
+    transforms = {
+        view: transform_matrix(
+            payload[transform_key][view]["translation_xyz_m"],
+            payload[transform_key][view]["quaternion_xyzw"],
+        )
+        for view in views
+    }
+    return CameraExtrinsics(
+        cameras=cameras,
+        reference_view=str(reference_view),
+        reference_from_view=transforms,
+        quality={
+            str(key): float(value) if isinstance(value, (int, float)) else value
+            for key, value in payload["quality"].items()
+        },
+    )

src/linkerhand_calibration/linkerhand_calibration/core/geometry/rotation.py

@@ -0,0 +1,157 @@
+"""Pure quaternion summaries and rotation-axis fitting."""
+
+from __future__ import annotations
+
+import math
+from typing import Sequence
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+def normalize_quaternion_xyzw(values: Sequence[float]) -> np.ndarray:
+    quaternion = np.asarray(values, dtype=float)
+    if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
+        raise ValueError("quaternion must contain four finite xyzw values")
+    norm = float(np.linalg.norm(quaternion))
+    if norm < 1e-12:
+        raise ValueError("quaternion norm is zero")
+    return quaternion / norm
+
+
+def relative_quaternion_xyzw(
+    parent_camera_quaternion: Sequence[float],
+    child_camera_quaternion: Sequence[float],
+) -> tuple[float, float, float, float]:
+    """Compute parent-to-child orientation from two camera-to-Tag rotations."""
+    parent = Rotation.from_quat(
+        normalize_quaternion_xyzw(parent_camera_quaternion)
+    )
+    child = Rotation.from_quat(
+        normalize_quaternion_xyzw(child_camera_quaternion)
+    )
+    quaternion = (parent.inv() * child).as_quat()
+    return tuple(float(value) for value in quaternion)
+
+
+def image_plane_tag_quaternion_xyzw(
+    corners_xy: Sequence[Sequence[float]],
+) -> tuple[float, float, float, float]:
+    """Estimate Tag orientation about the optical axis from ordered corners."""
+    corners = np.asarray(corners_xy, dtype=float)
+    if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
+        raise ValueError("corners_xy must contain four finite xy points")
+    x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3])
+    if float(np.linalg.norm(x_axis)) < 1e-9:
+        raise ValueError("tag x-axis is degenerate")
+    angle = -math.atan2(float(x_axis[1]), float(x_axis[0]))
+    quaternion = Rotation.from_rotvec([0.0, 0.0, angle]).as_quat()
+    return tuple(float(value) for value in quaternion)
+
+
+def robust_rotation_summary(
+    quaternions_xyzw: Sequence[Sequence[float]],
+) -> tuple[tuple[float, float, float, float], float]:
+    """Return a robust orientation and maximum angular residual in radians."""
+    if not quaternions_xyzw:
+        raise ValueError("at least one quaternion is required")
+    rotations = Rotation.from_quat(
+        np.asarray(
+            [normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
+            dtype=float,
+        )
+    )
+    reference = rotations[0]
+    delta_vectors = (reference.inv() * rotations).as_rotvec()
+    median_delta = np.median(delta_vectors, axis=0)
+    robust = reference * Rotation.from_rotvec(median_delta)
+    residuals = (robust.inv() * rotations).magnitude()
+    maximum = float(np.max(residuals)) if residuals.size else 0.0
+    return tuple(float(value) for value in robust.as_quat()), maximum
+
+
+def rotation_spread_rad(
+    quaternions_xyzw: Sequence[Sequence[float]],
+) -> float:
+    """Return the maximum geodesic residual around a robust orientation."""
+    _, spread = robust_rotation_summary(quaternions_xyzw)
+    return spread
+
+
+def rotation_rms_rad(
+    quaternions_xyzw: Sequence[Sequence[float]],
+    *,
+    outlier_threshold_rad: float | None = None,
+) -> float:
+    """Return RMS geodesic noise around a robust orientation."""
+    robust, _ = robust_rotation_summary(quaternions_xyzw)
+    reference = Rotation.from_quat(robust)
+    rotations = Rotation.from_quat(
+        np.asarray(
+            [normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
+            dtype=float,
+        )
+    )
+    residuals = (reference.inv() * rotations).magnitude()
+    if outlier_threshold_rad is not None:
+        threshold = float(outlier_threshold_rad)
+        if threshold <= 0.0:
+            raise ValueError("outlier_threshold_rad must be positive")
+        residuals = residuals[residuals <= threshold]
+        if residuals.size == 0:
+            return float("inf")
+    return float(np.sqrt(np.mean(np.square(residuals))))
+
+
+def rotation_inlier_fraction(
+    quaternions_xyzw: Sequence[Sequence[float]],
+    *,
+    outlier_threshold_rad: float,
+) -> float:
+    """Return the fraction close to the robust orientation."""
+    threshold = float(outlier_threshold_rad)
+    if threshold <= 0.0:
+        raise ValueError("outlier_threshold_rad must be positive")
+    robust, _ = robust_rotation_summary(quaternions_xyzw)
+    reference = Rotation.from_quat(robust)
+    rotations = Rotation.from_quat(
+        np.asarray(
+            [normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
+            dtype=float,
+        )
+    )
+    residuals = (reference.inv() * rotations).magnitude()
+    return float(np.mean(residuals <= threshold))
+
+
+def delta_rotation_vector(
+    reference_xyzw: Sequence[float],
+    observed_xyzw: Sequence[float],
+) -> np.ndarray:
+    reference = Rotation.from_quat(normalize_quaternion_xyzw(reference_xyzw))
+    observed = Rotation.from_quat(normalize_quaternion_xyzw(observed_xyzw))
+    return (reference.inv() * observed).as_rotvec()
+
+
+def fit_rotation_axis(
+    vectors: Sequence[Sequence[float]],
+    commands: Sequence[int],
+) -> np.ndarray:
+    """Fit and orient the single rotational axis used by one command sweep."""
+    matrix = np.asarray(vectors, dtype=float)
+    command_values = np.asarray(commands, dtype=int)
+    if matrix.ndim != 2 or matrix.shape[1] != 3:
+        raise ValueError("vectors must have shape (N, 3)")
+    if command_values.shape != (matrix.shape[0],):
+        raise ValueError("commands must match vectors")
+    useful = np.linalg.norm(matrix, axis=1) > 1e-6
+    if int(np.count_nonzero(useful)) < 3:
+        raise ValueError("insufficient non-zero rotations to fit an axis")
+    _, _, vh = np.linalg.svd(matrix[useful], full_matrices=False)
+    axis = vh[0]
+    projections = matrix @ axis
+    low = projections[command_values <= 16]
+    high = projections[command_values >= 239]
+    if low.size and high.size and float(np.median(low)) < float(np.median(high)):
+        axis = -axis
+    return axis / np.linalg.norm(axis)

src/linkerhand_calibration/linkerhand_calibration/core/solver/__init__.py

@@ -0,0 +1,15 @@
+"""Task acceptance and final-session solver contracts."""
+
+from .interfaces import (
+    SessionSolution,
+    SessionSolver,
+    TaskEvaluation,
+    TaskEvaluator,
+)
+
+__all__ = [
+    "SessionSolution",
+    "SessionSolver",
+    "TaskEvaluation",
+    "TaskEvaluator",
+]

src/linkerhand_calibration/linkerhand_calibration/core/solver/interfaces.py

@@ -0,0 +1,42 @@
+"""Shared evaluator and final-solver interfaces."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Any, Mapping, Protocol, Sequence
+
+from ..domain import CalibrationProfile, SampleRecord, TaskSpec
+
+
+@dataclass(frozen=True)
+class TaskEvaluation:
+    accepted: bool
+    failures: tuple[Mapping[str, Any], ...] = ()
+    rescan_measurements: frozenset[str] = frozenset()
+    rescan_cycles: frozenset[int] = frozenset()
+
+
+@dataclass(frozen=True)
+class SessionSolution:
+    passed: bool
+    calibration: Mapping[str, Any]
+    zero_offsets_rad: Mapping[str, float]
+    failures: tuple[Mapping[str, Any], ...] = ()
+    diagnostics: Mapping[str, Any] = field(default_factory=dict)
+
+
+class TaskEvaluator(Protocol):
+    def evaluate_task(
+        self,
+        profile: CalibrationProfile,
+        task: TaskSpec,
+        samples: Sequence[SampleRecord],
+    ) -> TaskEvaluation: ...
+
+
+class SessionSolver(Protocol):
+    def solve_session(
+        self,
+        profile: CalibrationProfile,
+        samples: Sequence[SampleRecord],
+    ) -> SessionSolution: ...

src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py

@@ -0,0 +1,22 @@
+"""URDF correction authorization and validation types."""
+
+from .plan import UrdfCorrectionPlan, build_correction_plan
+from .patch import (
+    MujocoEqualityPatch,
+    UrdfJointPatch,
+    UrdfPatchSet,
+    apply_urdf_patch_text,
+    materialize_relative_mesh_assets,
+    write_urdf_patches,
+)
+
+__all__ = [
+    "MujocoEqualityPatch",
+    "UrdfCorrectionPlan",
+    "UrdfJointPatch",
+    "UrdfPatchSet",
+    "apply_urdf_patch_text",
+    "build_correction_plan",
+    "materialize_relative_mesh_assets",
+    "write_urdf_patches",
+]

src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py

@@ -0,0 +1,315 @@
+"""Byte-preserving, declarative URDF patch application.
+
+Model profiles decide *what* values are authorized.  This module owns the
+shared mechanics of locating those fields in the original XML text, changing
+only the declared attributes, materializing mesh resources and atomically
+publishing a new file.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+import os
+from pathlib import Path
+import re
+import shutil
+from typing import Mapping, Sequence
+import xml.etree.ElementTree as ET
+
+
+@dataclass(frozen=True)
+class UrdfJointPatch:
+    """Authorized attribute replacements inside one top-level URDF joint."""
+
+    origin_rpy: str | None = None
+    limit_lower: str | None = None
+    limit_upper: str | None = None
+    mimic_multiplier: str | None = None
+    mimic_offset: str | None = None
+
+    def replacements(self) -> tuple[tuple[str, str, str], ...]:
+        values = (
+            ("origin", "rpy", self.origin_rpy),
+            ("limit", "lower", self.limit_lower),
+            ("limit", "upper", self.limit_upper),
+            ("mimic", "multiplier", self.mimic_multiplier),
+            ("mimic", "offset", self.mimic_offset),
+        )
+        return tuple(
+            (element, attribute, str(value))
+            for element, attribute, value in values
+            if value is not None
+        )
+
+
+
+@dataclass(frozen=True)
+class MujocoEqualityPatch:
+    """Replacement and optional topology assertion for one equality joint."""
+
+    polycoef: str
+    expected_joint1: str | None = None
+    expected_joint2: str | None = None
+
+
+@dataclass(frozen=True)
+class UrdfPatchSet:
+    """Complete declarative edit set for one generated URDF."""
+
+    joints: Mapping[str, UrdfJointPatch]
+    mujoco_equalities: Mapping[str, MujocoEqualityPatch] = field(
+        default_factory=dict
+    )
+
+    def __post_init__(self) -> None:
+        empty = [
+            name for name, patch in self.joints.items() if not patch.replacements()
+        ]
+        if empty:
+            raise ValueError(
+                "URDF joint patch contains no replacements: "
+                + ",".join(sorted(empty))
+            )
+
+
+def _replace_attribute(
+    block: str, element: str, attribute: str, value: str
+) -> str:
+    pattern = re.compile(
+        rf"(<{element}\b[^>]*\b{attribute}\s*=\s*)([\"'])"
+        rf"(?P<value>[^\"']*)\2",
+        re.DOTALL,
+    )
+    match = pattern.search(block)
+    if match is None:
+        raise ValueError(f"{element} has no {attribute} attribute")
+    start, end = match.span("value")
+    return block[:start] + str(value) + block[end:]
+
+
+def apply_urdf_patch_text(
+    original_text: str,
+    root: ET.Element,
+    patches: UrdfPatchSet,
+) -> str:
+    """Apply declared patches without serializing unaffected XML."""
+    top_level_joints = {
+        str(joint.get("name")): joint for joint in root.findall("joint")
+    }
+    missing_joints = set(patches.joints) - set(top_level_joints)
+    if missing_joints:
+        raise ValueError(
+            "source URDF is missing target joints: "
+            + ",".join(sorted(missing_joints))
+        )
+
+    equality_nodes = {
+        str(joint.get("name")): joint
+        for joint in root.findall("./mujoco/equality/joint")
+    }
+    missing_equalities = set(patches.mujoco_equalities) - set(equality_nodes)
+    if missing_equalities:
+        raise ValueError(
+            "source URDF is missing MuJoCo equalities: "
+            + ",".join(sorted(missing_equalities))
+        )
+    for name, patch in patches.mujoco_equalities.items():
+        node = equality_nodes[name]
+        if (
+            patch.expected_joint1 is not None
+            and node.get("joint1") != patch.expected_joint1
+        ):
+            raise ValueError(f"MuJoCo equality joint1 differs for {name}")
+        if (
+            patch.expected_joint2 is not None
+            and node.get("joint2") != patch.expected_joint2
+        ):
+            raise ValueError(f"MuJoCo equality joint2 differs for {name}")
+
+    # Requiring the URDF ``type`` attribute excludes transmission and MuJoCo
+    # elements which also use the tag name ``joint``.
+    joint_pattern = re.compile(
+        r"<joint\b(?=[^>]*\btype\s*=)[^>]*\bname\s*=\s*"
+        r"([\"'])(?P<name>[^\"']+)\1[^>]*>"
+        r".*?</joint>",
+        re.DOTALL,
+    )
+    applied_joints: set[str] = set()
+
+    def replace_joint(match: re.Match[str]) -> str:
+        name = match.group("name")
+        patch = patches.joints.get(name)
+        if patch is None:
+            return match.group(0)
+        if name in applied_joints:
+            raise ValueError(f"duplicate top-level URDF joint text: {name}")
+        block = match.group(0)
+        for element, attribute, value in patch.replacements():
+            block = _replace_attribute(block, element, attribute, value)
+        applied_joints.add(name)
+        return block
+
+    corrected = joint_pattern.sub(replace_joint, original_text)
+    if applied_joints != set(patches.joints):
+        missing = set(patches.joints) - applied_joints
+        raise ValueError(
+            "could not locate every target joint in source URDF text: "
+            + ",".join(sorted(missing))
+        )
+
+    applied_equalities: set[str] = set()
+    for name, patch in patches.mujoco_equalities.items():
+        equality_pattern = re.compile(
+            rf"(<joint\b[^>]*\bname\s*=\s*([\"']))"
+            rf"{re.escape(name)}\2[^>]*>",
+            re.DOTALL,
+        )
+        matches = list(equality_pattern.finditer(corrected))
+        if len(matches) != 1:
+            raise ValueError(f"could not uniquely locate MuJoCo equality {name}")
+        match = matches[0]
+        replacement = _replace_attribute(
+            match.group(0), "joint", "polycoef", patch.polycoef
+        )
+        corrected = corrected[: match.start()] + replacement + corrected[match.end() :]
+        applied_equalities.add(name)
+    if applied_equalities != set(patches.mujoco_equalities):
+        raise ValueError("could not apply every MuJoCo equality patch")
+    return corrected
+
+
+def _files_have_identical_contents(left: Path, right: Path) -> bool:
+    if left.stat().st_size != right.stat().st_size:
+        return False
+    with left.open("rb") as left_stream, right.open("rb") as right_stream:
+        while True:
+            left_chunk = left_stream.read(1024 * 1024)
+            right_chunk = right_stream.read(1024 * 1024)
+            if left_chunk != right_chunk:
+                return False
+            if not left_chunk:
+                return True
+
+
+def materialize_relative_mesh_assets(
+    *, source: Path, output: Path, urdf_root: ET.Element
+) -> tuple[Path, ...]:
+    """Copy safe relative mesh resources beside the generated URDF."""
+    filenames = sorted(
+        {
+            str(mesh.get("filename", "")).strip()
+            for mesh in urdf_root.findall(".//mesh")
+            if str(mesh.get("filename", "")).strip()
+        }
+    )
+    materialized: list[Path] = []
+    for filename in filenames:
+        if "://" in filename or filename.startswith("package:"):
+            continue
+        relative = Path(filename)
+        if relative.is_absolute() or ".." in relative.parts:
+            raise ValueError(
+                f"URDF mesh path must be a safe relative path or URI: {filename}"
+            )
+        source_asset = (source.parent / relative).resolve()
+        if not source_asset.is_file():
+            raise ValueError(f"URDF mesh resource does not exist: {source_asset}")
+        destination_asset = (output / relative).resolve()
+        try:
+            destination_asset.relative_to(output)
+        except ValueError as error:
+            raise ValueError(
+                f"URDF mesh destination escapes output directory: {filename}"
+            ) from error
+        if destination_asset == source_asset:
+            materialized.append(destination_asset)
+            continue
+        destination_asset.parent.mkdir(parents=True, exist_ok=True)
+        if destination_asset.exists():
+            if not destination_asset.is_file() or not _files_have_identical_contents(
+                source_asset, destination_asset
+            ):
+                raise ValueError(
+                    "refusing to overwrite a different mesh resource: "
+                    f"{destination_asset}"
+                )
+            materialized.append(destination_asset)
+            continue
+        temporary_asset = destination_asset.with_name(
+            f".{destination_asset.name}.{os.getpid()}.tmp"
+        )
+        if temporary_asset.exists():
+            raise ValueError(f"temporary mesh path is occupied: {temporary_asset}")
+        try:
+            shutil.copy2(source_asset, temporary_asset)
+            os.replace(temporary_asset, destination_asset)
+        finally:
+            if temporary_asset.exists():
+                temporary_asset.unlink()
+        materialized.append(destination_asset)
+    return tuple(materialized)
+
+
+def _copy_complete_mesh_directory(source: Path, output: Path) -> None:
+    source_meshes = source.parent / "meshes"
+    if not source_meshes.is_dir():
+        return
+    destination_meshes = output / "meshes"
+    destination_meshes.mkdir(parents=True, exist_ok=True)
+    for mesh in source_meshes.iterdir():
+        if mesh.is_file():
+            shutil.copy2(mesh, destination_meshes / mesh.name)
+
+
+def write_urdf_patches(
+    *,
+    source_urdf: str | Path,
+    destination_urdf: str | Path,
+    patches: UrdfPatchSet,
+    forbidden_source_stem_patterns: Sequence[str] = (),
+    copy_complete_mesh_directory: bool = False,
+) -> Path:
+    """Validate and atomically materialize one patched URDF."""
+    source = Path(source_urdf).expanduser().resolve()
+    destination = Path(destination_urdf).expanduser().resolve()
+    if not source.is_file():
+        raise ValueError(f"source URDF does not exist: {source}")
+    for pattern in forbidden_source_stem_patterns:
+        if re.search(str(pattern), source.stem, re.IGNORECASE):
+            raise ValueError(
+                "source URDF must be the immutable original CAD URDF"
+            )
+    if destination == source or destination.exists():
+        raise ValueError(f"refusing to overwrite URDF: {destination}")
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    tree = ET.parse(source)
+    root = tree.getroot()
+    corrected = apply_urdf_patch_text(
+        source.read_text(encoding="utf-8"), root, patches
+    )
+    materialize_relative_mesh_assets(
+        source=source, output=destination.parent, urdf_root=root
+    )
+    if copy_complete_mesh_directory:
+        _copy_complete_mesh_directory(source, destination.parent)
+    temporary = destination.with_suffix(destination.suffix + ".tmp")
+    try:
+        with temporary.open("w", encoding="utf-8") as stream:
+            stream.write(corrected)
+            stream.flush()
+            os.fsync(stream.fileno())
+        os.replace(temporary, destination)
+    finally:
+        if temporary.exists():
+            temporary.unlink()
+    return destination
+
+
+__all__ = [
+    "MujocoEqualityPatch",
+    "UrdfJointPatch",
+    "UrdfPatchSet",
+    "apply_urdf_patch_text",
+    "materialize_relative_mesh_assets",
+    "write_urdf_patches",
+]

src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py

@@ -0,0 +1,104 @@
+"""One authorization plan shared by URDF writers and validators."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+import hashlib
+from pathlib import Path
+from typing import Mapping
+
+from ..domain import CalibrationProfile
+
+
+@dataclass(frozen=True)
+class UrdfCorrectionPlan:
+    source_sha256: str
+    allowed_active_joints: frozenset[str]
+    endpoint_limit_joints: frozenset[str]
+    mimic_source_by_joint: Mapping[str, str]
+    frozen_joints: frozenset[str]
+    frozen_offsets_rad: Mapping[str, float] = field(default_factory=dict)
+    forbid_calibrated_source: bool = True
+    forbid_overwrite: bool = True
+    preserve_passive_joints: bool = True
+
+    def __post_init__(self) -> None:
+        if len(self.source_sha256) != 64 or any(
+            character not in "0123456789abcdef"
+            for character in self.source_sha256.lower()
+        ):
+            raise ValueError("source URDF SHA-256 is invalid")
+        if self.allowed_active_joints & self.frozen_joints:
+            raise ValueError("allowed and frozen URDF joints overlap")
+        applied = self.allowed_active_joints | set(self.frozen_offsets_rad)
+        if not set(self.frozen_offsets_rad).issubset(self.frozen_joints):
+            raise ValueError("frozen offsets must belong to frozen joints")
+        if not self.endpoint_limit_joints.issubset(applied):
+            raise ValueError("endpoint limit joint is not an applied active joint")
+        if set(self.mimic_source_by_joint) & self.allowed_active_joints:
+            raise ValueError("dependent mimic joints cannot be active edit targets")
+
+    def authorize_offsets(self, offsets_rad: Mapping[str, float]) -> None:
+        required = self.allowed_active_joints | set(self.frozen_offsets_rad)
+        unexpected = set(offsets_rad) - required
+        if unexpected:
+            raise ValueError(
+                "URDF correction contains unauthorized joints: "
+                + ", ".join(sorted(unexpected))
+            )
+        missing = required - set(offsets_rad)
+        if missing:
+            raise ValueError(
+                "URDF correction is missing active joints: "
+                + ", ".join(sorted(missing))
+            )
+        changed_frozen = {
+            name
+            for name, expected in self.frozen_offsets_rad.items()
+            if abs(float(offsets_rad[name]) - float(expected)) > 1.0e-12
+        }
+        if changed_frozen:
+            raise ValueError(
+                "URDF correction changed frozen offsets: "
+                + ", ".join(sorted(changed_frozen))
+            )
+
+    def verify_source(self, source_urdf: str | Path) -> None:
+        digest = hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest()
+        if digest != self.source_sha256.lower():
+            raise ValueError("source URDF SHA-256 differs from correction plan")
+
+
+def build_correction_plan(
+    profile: CalibrationProfile,
+    *,
+    source_sha256: str,
+    scope: str,
+    frozen_offsets_rad: Mapping[str, float] | None = None,
+) -> UrdfCorrectionPlan:
+    """Build one scope-aware edit authorization from typed policies."""
+    selected = profile.scope.selected_joints(scope)
+    frozen = profile.scope.frozen_joints[str(scope)]
+    expected_frozen = {
+        str(name): float(value)
+        for name, value in dict(frozen_offsets_rad or {}).items()
+    }
+    if set(expected_frozen) != set(frozen):
+        missing = set(frozen) - set(expected_frozen)
+        extra = set(expected_frozen) - set(frozen)
+        raise ValueError(
+            "frozen URDF offset state differs from scope policy: "
+            f"missing={','.join(sorted(missing)) or '-'};"
+            f"extra={','.join(sorted(extra)) or '-'}"
+        )
+    applied = selected | frozen
+    return UrdfCorrectionPlan(
+        source_sha256=source_sha256,
+        allowed_active_joints=selected,
+        endpoint_limit_joints=(
+            profile.zero.mechanical_endpoint_joints & applied
+        ),
+        mimic_source_by_joint=profile.zero.mimic_source_by_joint,
+        frozen_joints=frozen | profile.zero.cad_frozen_joints,
+        frozen_offsets_rad=expected_frozen,
+    )

src/linkerhand_calibration/linkerhand_calibration/extrinsics.py

@@ -0,0 +1,150 @@
+"""Three-view compatibility policy over generic camera extrinsics."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+import yaml
+
+from .core.geometry.extrinsics import (
+    CameraCalibrationIdentity,
+    CameraExtrinsics,
+    camera_info_fingerprint,
+    load_camera_extrinsics,
+    matrix_payload,
+    transform_matrix,
+    validate_camera_extrinsics_payload,
+)
+
+
+VIEWS: tuple[str, ...] = ("front", "side", "top")
+_QUALITY_LIMITS = {
+    "reprojection_rms_px": 1.2,
+    "maximum_rotation_repeatability_deg": 0.3,
+    "maximum_translation_repeatability_m": 0.0015,
+}
+_MINIMUM_CAPTURE_COUNTS = {
+    "front_side_captures": 15,
+    "front_top_captures": 15,
+}
+
+
+@dataclass(frozen=True)
+class ThreeCameraExtrinsics:
+    """Transforms points from each camera optical frame into front optical."""
+
+    cameras: Mapping[str, CameraCalibrationIdentity]
+    front_from_view: Mapping[str, np.ndarray]
+    quality: Mapping[str, float]
+
+    def transform(self, view: str) -> np.ndarray:
+        if view not in self.front_from_view:
+            raise KeyError(f"extrinsics do not contain view {view}")
+        return np.asarray(self.front_from_view[view], dtype=float).copy()
+
+    def camera_matches(
+        self,
+        view: str,
+        *,
+        serial_number: str,
+        width: int,
+        height: int,
+        intrinsics_sha256: str,
+    ) -> bool:
+        expected = self.cameras.get(view)
+        return bool(
+            expected is not None
+            and expected.serial_number == str(serial_number)
+            and expected.width == int(width)
+            and expected.height == int(height)
+            and expected.intrinsics_sha256 == str(intrinsics_sha256)
+        )
+
+
+def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
+    validate_camera_extrinsics_payload(
+        payload,
+        required_views=VIEWS,
+        reference_view="front",
+        quality_limits=_QUALITY_LIMITS,
+        minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
+    )
+
+
+def load_three_camera_extrinsics(
+    path: str | Path,
+    *,
+    quality_limits: Mapping[str, float] | None = None,
+    minimum_capture_counts: Mapping[str, int] | None = None,
+) -> ThreeCameraExtrinsics:
+    generic = load_camera_extrinsics(
+        path,
+        required_views=VIEWS,
+        reference_view="front",
+        quality_limits=(
+            _QUALITY_LIMITS if quality_limits is None else quality_limits
+        ),
+        minimum_capture_counts=(
+            _MINIMUM_CAPTURE_COUNTS
+            if minimum_capture_counts is None
+            else minimum_capture_counts
+        ),
+    )
+    return ThreeCameraExtrinsics(
+        cameras=generic.cameras,
+        front_from_view=generic.reference_from_view,
+        quality=generic.quality,
+    )
+
+
+def dump_three_camera_extrinsics(
+    path: str | Path,
+    *,
+    cameras: Mapping[str, Mapping[str, Any]],
+    front_from_view: Mapping[str, Sequence[Sequence[float]]],
+    quality: Mapping[str, Any],
+    quality_limits: Mapping[str, float] | None = None,
+) -> None:
+    payload = {
+        "schema_version": 1,
+        "reference_view": "front",
+        "cameras": {view: dict(cameras[view]) for view in VIEWS},
+        "front_from_view": {
+            view: matrix_payload(front_from_view[view]) for view in VIEWS
+        },
+        "quality": dict(quality),
+    }
+    validate_camera_extrinsics_payload(
+        payload,
+        required_views=VIEWS,
+        reference_view="front",
+        quality_limits=(
+            _QUALITY_LIMITS if quality_limits is None else quality_limits
+        ),
+        minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
+    )
+    destination = Path(path).expanduser().resolve()
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    temporary = destination.with_suffix(destination.suffix + ".tmp")
+    with temporary.open("w", encoding="utf-8") as stream:
+        yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
+    temporary.replace(destination)
+
+
+__all__ = [
+    "CameraCalibrationIdentity",
+    "CameraExtrinsics",
+    "ThreeCameraExtrinsics",
+    "VIEWS",
+    "camera_info_fingerprint",
+    "dump_three_camera_extrinsics",
+    "load_camera_extrinsics",
+    "load_three_camera_extrinsics",
+    "matrix_payload",
+    "transform_matrix",
+    "validate_camera_extrinsics_payload",
+    "validate_extrinsics_payload",
+]

src/linkerhand_calibration/linkerhand_calibration/full_hand.py

@@ -0,0 +1,7 @@
+"""One-release module alias for the relocated model profile implementation."""
+
+import sys
+
+from .models.g20 import profile as _implementation
+
+sys.modules[__name__] = _implementation

src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py

@@ -0,0 +1,644 @@
+"""ROS 2 image publisher for Hikrobot MVS USB3 Vision cameras."""
+
+from __future__ import annotations
+
+from ctypes import byref, cast, memset, POINTER, sizeof, string_at
+from dataclasses import dataclass
+import importlib
+import os
+from pathlib import Path
+import sys
+from threading import Event, Lock, Thread
+from typing import Any, Sequence
+from urllib.parse import unquote, urlparse
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import CameraInfo, Image
+from sensor_msgs.srv import SetCameraInfo
+import yaml
+
+
+DEFAULT_MVS_PYTHON_PATH = "/opt/MVS/Samples/64/Python/MvImport"
+DEFAULT_MVS_RUNTIME_PATH = "/opt/MVS/lib"
+
+
+@dataclass(frozen=True)
+class DeviceDescriptor:
+    """Stable identity fields for one enumerated USB3 Vision camera."""
+
+    index: int
+    model: str
+    serial: str
+    guid: str
+
+
+@dataclass(frozen=True)
+class CameraCalibration:
+    """Validated subset of a standard ROS camera calibration YAML file."""
+
+    camera_name: str
+    width: int
+    height: int
+    distortion_model: str
+    d: tuple[float, ...]
+    k: tuple[float, ...]
+    r: tuple[float, ...]
+    p: tuple[float, ...]
+
+
+def decode_c_string(value: Any) -> str:
+    """Decode a null-terminated MVS SDK character array."""
+    raw = bytes(value).split(b"\0", 1)[0]
+    for encoding in ("utf-8", "gbk", "latin-1"):
+        try:
+            return raw.decode(encoding)
+        except UnicodeDecodeError:
+            continue
+    return raw.decode("latin-1", errors="replace")
+
+
+def select_device(
+    devices: Sequence[DeviceDescriptor],
+    selector: str,
+    expected_model: str,
+) -> DeviceDescriptor:
+    """Select exactly one camera by serial or GUID and check its model."""
+    if not devices:
+        raise RuntimeError("MVS did not enumerate any USB3 Vision cameras")
+    selector = selector.strip()
+    if selector:
+        matches = [
+            device
+            for device in devices
+            if selector in {device.serial, device.guid}
+        ]
+        if not matches:
+            available = ", ".join(
+                f"{device.model}:{device.serial}:{device.guid}"
+                for device in devices
+            )
+            raise RuntimeError(
+                f"camera selector {selector!r} was not found; "
+                f"available cameras: {available}"
+            )
+    elif len(devices) == 1:
+        matches = [devices[0]]
+    else:
+        available = ", ".join(device.serial for device in devices)
+        raise RuntimeError(
+            "camera selector is required when multiple cameras are present; "
+            f"available serials: {available}"
+        )
+    if len(matches) != 1:
+        raise RuntimeError(f"camera selector {selector!r} is not unique")
+    selected = matches[0]
+    if expected_model and expected_model not in selected.model:
+        raise RuntimeError(
+            f"camera {selected.serial} is {selected.model!r}, expected a model "
+            f"containing {expected_model!r}"
+        )
+    return selected
+
+
+def resolve_camera_info_path(url_or_path: str) -> Path | None:
+    """Resolve a plain path or file:// URL used for CameraInfo persistence."""
+    value = url_or_path.strip()
+    if not value:
+        return None
+    parsed = urlparse(value)
+    if parsed.scheme not in {"", "file"}:
+        raise ValueError(
+            "camera_info_url must be a filesystem path or file:// URL"
+        )
+    if parsed.scheme == "file":
+        if parsed.netloc not in {"", "localhost"}:
+            raise ValueError("remote file:// camera_info_url is not supported")
+        value = unquote(parsed.path)
+    return Path(value).expanduser().resolve()
+
+
+def _matrix_data(mapping: dict[str, Any], name: str, length: int) -> tuple[float, ...]:
+    section = mapping.get(name)
+    if not isinstance(section, dict):
+        raise ValueError(f"camera calibration is missing {name}")
+    data = section.get("data")
+    if not isinstance(data, list) or len(data) != length:
+        raise ValueError(f"{name}.data must contain exactly {length} values")
+    result = tuple(float(value) for value in data)
+    if not all(value == value and abs(value) != float("inf") for value in result):
+        raise ValueError(f"{name}.data contains a non-finite value")
+    return result
+
+
+def load_camera_calibration(path: Path) -> CameraCalibration:
+    """Load and validate a standard ROS camera calibration YAML file."""
+    mapping = yaml.safe_load(path.read_text(encoding="utf-8"))
+    if not isinstance(mapping, dict):
+        raise ValueError("camera calibration YAML root must be a mapping")
+    width = int(mapping.get("image_width", 0))
+    height = int(mapping.get("image_height", 0))
+    if width <= 0 or height <= 0:
+        raise ValueError("camera calibration image dimensions must be positive")
+    distortion = mapping.get("distortion_coefficients")
+    if not isinstance(distortion, dict) or not isinstance(
+        distortion.get("data"), list
+    ):
+        raise ValueError(
+            "camera calibration is missing distortion_coefficients.data"
+        )
+    d = tuple(float(value) for value in distortion["data"])
+    calibration = CameraCalibration(
+        camera_name=str(mapping.get("camera_name", "hikrobot_camera")),
+        width=width,
+        height=height,
+        distortion_model=str(mapping.get("distortion_model", "plumb_bob")),
+        d=d,
+        k=_matrix_data(mapping, "camera_matrix", 9),
+        r=_matrix_data(mapping, "rectification_matrix", 9),
+        p=_matrix_data(mapping, "projection_matrix", 12),
+    )
+    if calibration.k[0] <= 0.0 or calibration.k[4] <= 0.0:
+        raise ValueError("camera calibration focal lengths must be positive")
+    if calibration.p[0] <= 0.0 or calibration.p[5] <= 0.0:
+        raise ValueError("camera calibration projection focal lengths must be positive")
+    return calibration
+
+
+def calibration_to_mapping(
+    camera_name: str, camera_info: CameraInfo
+) -> dict[str, Any]:
+    """Convert CameraInfo into the standard ROS YAML representation."""
+    return {
+        "image_width": int(camera_info.width),
+        "image_height": int(camera_info.height),
+        "camera_name": camera_name,
+        "camera_matrix": {
+            "rows": 3,
+            "cols": 3,
+            "data": [float(value) for value in camera_info.k],
+        },
+        "distortion_model": camera_info.distortion_model,
+        "distortion_coefficients": {
+            "rows": 1,
+            "cols": len(camera_info.d),
+            "data": [float(value) for value in camera_info.d],
+        },
+        "rectification_matrix": {
+            "rows": 3,
+            "cols": 3,
+            "data": [float(value) for value in camera_info.r],
+        },
+        "projection_matrix": {
+            "rows": 3,
+            "cols": 4,
+            "data": [float(value) for value in camera_info.p],
+        },
+    }
+
+
+def configure_fastdds_large_image_transport() -> Path:
+    """Select the package's large-image SHM profile before creating ROS nodes."""
+    from ament_index_python.packages import get_package_share_directory
+
+    profile = (
+        Path(get_package_share_directory("linkerhand_calibration"))
+        / "config"
+        / "fastdds_large_images.xml"
+    )
+    if not profile.is_file():
+        raise RuntimeError(f"Fast DDS large-image profile not found: {profile}")
+    os.environ.setdefault("FASTRTPS_DEFAULT_PROFILES_FILE", str(profile))
+    return profile
+
+
+class HikrobotCameraNode(Node):
+    """Publish synchronized Mono8 Image and CameraInfo messages from MVS."""
+
+    def __init__(self) -> None:
+        super().__init__("hikrobot_camera")
+        self._declare_parameters()
+        self._stop_event = Event()
+        self._camera_info_lock = Lock()
+        self._grab_thread: Thread | None = None
+        self._camera: Any | None = None
+        self._mvs: Any | None = None
+        self._sdk_initialized = False
+        self._handle_created = False
+        self._camera_opened = False
+        self._grabbing = False
+        self._closed = False
+        self._frame_count = 0
+        self._timeout_count = 0
+
+        self.selector = str(self.get_parameter("serial_number").value)
+        self.expected_model = str(self.get_parameter("expected_model").value)
+        self.frame_id = str(self.get_parameter("frame_id").value)
+        self.camera_name = str(self.get_parameter("camera_name").value)
+        self.image_width = int(self.get_parameter("image_width").value)
+        self.image_height = int(self.get_parameter("image_height").value)
+        if self.image_width <= 0 or self.image_height <= 0:
+            raise ValueError("image_width and image_height must be positive")
+        if float(self.get_parameter("frame_rate").value) <= 0.0:
+            raise ValueError("frame_rate must be positive")
+        if int(self.get_parameter("grab_timeout_ms").value) <= 0:
+            raise ValueError("grab_timeout_ms must be positive")
+        self.camera_info_path = resolve_camera_info_path(
+            str(self.get_parameter("camera_info_url").value)
+        )
+        self._calibration = self._load_calibration()
+
+        self.image_publisher = self.create_publisher(
+            Image, "image_raw", qos_profile_sensor_data
+        )
+        self.camera_info_publisher = self.create_publisher(
+            CameraInfo, "camera_info", qos_profile_sensor_data
+        )
+        self.create_service(
+            SetCameraInfo, "set_camera_info", self._set_camera_info_callback
+        )
+
+        try:
+            self._open_camera()
+            self._grab_thread = Thread(
+                target=self._grab_loop,
+                name="hikrobot_mvs_grab",
+                daemon=True,
+            )
+            self._grab_thread.start()
+        except Exception:
+            self.close()
+            raise
+
+    def _declare_parameters(self) -> None:
+        self.declare_parameter("serial_number", "")
+        self.declare_parameter("expected_model", "MV-CS020-10UM")
+        self.declare_parameter("camera_name", "hikrobot_front")
+        self.declare_parameter("frame_id", "camera_color_optical_frame")
+        self.declare_parameter("image_width", 1624)
+        self.declare_parameter("image_height", 1240)
+        self.declare_parameter("frame_rate", 30.0)
+        self.declare_parameter("exposure_time_us", 5000.0)
+        self.declare_parameter("gain_db", 0.0)
+        self.declare_parameter("auto_exposure", False)
+        self.declare_parameter("camera_info_url", "")
+        self.declare_parameter("mvs_python_path", DEFAULT_MVS_PYTHON_PATH)
+        self.declare_parameter("mvs_runtime_path", DEFAULT_MVS_RUNTIME_PATH)
+        self.declare_parameter("grab_timeout_ms", 1000)
+
+    def _load_calibration(self) -> CameraCalibration | None:
+        if self.camera_info_path is None or not self.camera_info_path.exists():
+            target = self.camera_info_path or "an unset camera_info_url"
+            self.get_logger().warning(
+                f"No camera calibration found at {target}; publishing "
+                "uncalibrated CameraInfo. Trajectory calibration will remain "
+                "locked until valid intrinsics are installed."
+            )
+            return None
+        calibration = load_camera_calibration(self.camera_info_path)
+        self._validate_calibration_dimensions(calibration)
+        self.get_logger().info(
+            f"Loaded camera intrinsics from {self.camera_info_path}"
+        )
+        return calibration
+
+    def _validate_calibration_dimensions(
+        self, calibration: CameraCalibration
+    ) -> None:
+        if (
+            calibration.width != self.image_width
+            or calibration.height != self.image_height
+        ):
+            raise ValueError(
+                "camera calibration dimensions "
+                f"{calibration.width}x{calibration.height} do not match "
+                f"configured stream {self.image_width}x{self.image_height}"
+            )
+
+    def _import_mvs(self) -> Any:
+        python_path = Path(
+            str(self.get_parameter("mvs_python_path").value)
+        ).expanduser()
+        runtime_path = Path(
+            str(self.get_parameter("mvs_runtime_path").value)
+        ).expanduser()
+        if not python_path.is_dir():
+            raise RuntimeError(f"MVS Python bindings not found: {python_path}")
+        if not runtime_path.is_dir():
+            raise RuntimeError(f"MVS runtime not found: {runtime_path}")
+        os.environ.setdefault("MVCAM_COMMON_RUNENV", str(runtime_path))
+        if str(python_path) not in sys.path:
+            sys.path.insert(0, str(python_path))
+        return importlib.import_module("MvCameraControl_class")
+
+    def _enumerate_usb_cameras(
+        self, mvs: Any
+    ) -> tuple[Any, list[DeviceDescriptor]]:
+        device_list = mvs.MV_CC_DEVICE_INFO_LIST()
+        result = mvs.MvCamera.MV_CC_EnumDevices(
+            mvs.MV_USB_DEVICE, device_list
+        )
+        self._require_ok("enumerate USB3 Vision devices", result)
+        descriptors: list[DeviceDescriptor] = []
+        for index in range(device_list.nDeviceNum):
+            device_info = cast(
+                device_list.pDeviceInfo[index], POINTER(mvs.MV_CC_DEVICE_INFO)
+            ).contents
+            usb_info = device_info.SpecialInfo.stUsb3VInfo
+            descriptors.append(
+                DeviceDescriptor(
+                    index=index,
+                    model=decode_c_string(usb_info.chModelName),
+                    serial=decode_c_string(usb_info.chSerialNumber),
+                    guid=decode_c_string(usb_info.chDeviceGUID),
+                )
+            )
+        return device_list, descriptors
+
+    def _open_camera(self) -> None:
+        mvs = self._import_mvs()
+        self._mvs = mvs
+        self._require_ok("initialize MVS SDK", mvs.MvCamera.MV_CC_Initialize())
+        self._sdk_initialized = True
+        device_list, descriptors = self._enumerate_usb_cameras(mvs)
+        selected = select_device(descriptors, self.selector, self.expected_model)
+        device_info = cast(
+            device_list.pDeviceInfo[selected.index],
+            POINTER(mvs.MV_CC_DEVICE_INFO),
+        ).contents
+
+        camera = mvs.MvCamera()
+        self._require_ok("create camera handle", camera.MV_CC_CreateHandle(device_info))
+        self._camera = camera
+        self._handle_created = True
+        self._require_ok(
+            "open camera",
+            camera.MV_CC_OpenDevice(mvs.MV_ACCESS_Exclusive, 0),
+        )
+        self._camera_opened = True
+
+        self._set_required_enum("AcquisitionMode", "Continuous")
+        self._set_required_enum("TriggerMode", "Off")
+        self._set_required_enum("PixelFormat", "Mono8")
+        self._set_required_integer("OffsetX", 0)
+        self._set_required_integer("OffsetY", 0)
+        self._set_required_integer("Width", self.image_width)
+        self._set_required_integer("Height", self.image_height)
+        self._set_optional_bool("AcquisitionFrameRateEnable", True)
+        self._set_optional_float(
+            "AcquisitionFrameRate",
+            float(self.get_parameter("frame_rate").value),
+        )
+        if bool(self.get_parameter("auto_exposure").value):
+            self._set_required_enum("ExposureAuto", "Continuous")
+        else:
+            self._set_required_enum("ExposureAuto", "Off")
+            self._set_optional_float(
+                "ExposureTime",
+                float(self.get_parameter("exposure_time_us").value),
+            )
+            self._set_required_enum("GainAuto", "Off")
+            self._set_optional_float(
+                "Gain", float(self.get_parameter("gain_db").value)
+            )
+
+        self._require_ok(
+            "set latest-frame grab strategy",
+            camera.MV_CC_SetGrabStrategy(mvs.MV_GrabStrategy_LatestImagesOnly),
+        )
+        self._require_ok("start image acquisition", camera.MV_CC_StartGrabbing())
+        self._grabbing = True
+        self.get_logger().info(
+            "Opened Hikrobot camera "
+            f"model={selected.model} serial={selected.serial} "
+            f"guid={selected.guid} stream={self.image_width}x{self.image_height} "
+            "encoding=mono8"
+        )
+
+    def _set_required_enum(self, name: str, value: str) -> None:
+        assert self._camera is not None
+        self._require_ok(
+            f"set {name}={value}",
+            self._camera.MV_CC_SetEnumValueByString(name, value),
+        )
+
+    def _set_required_integer(self, name: str, value: int) -> None:
+        assert self._camera is not None
+        self._require_ok(
+            f"set {name}={value}",
+            self._camera.MV_CC_SetIntValueEx(name, value),
+        )
+
+    def _set_optional_bool(self, name: str, value: bool) -> None:
+        assert self._camera is not None
+        result = self._camera.MV_CC_SetBoolValue(name, value)
+        if result != 0:
+            self.get_logger().warning(
+                f"MVS could not set {name}={value}: 0x{result:08x}"
+            )
+
+    def _set_optional_float(self, name: str, value: float) -> None:
+        assert self._camera is not None
+        result = self._camera.MV_CC_SetFloatValue(name, value)
+        if result != 0:
+            self.get_logger().warning(
+                f"MVS could not set {name}={value}: 0x{result:08x}"
+            )
+
+    @staticmethod
+    def _require_ok(operation: str, result: int) -> None:
+        if result != 0:
+            hint = ""
+            if result == 0x80000203:
+                hint = (
+                    "; access denied: stop acquisition and close the MVS "
+                    "viewer or any other process using this camera"
+                )
+            raise RuntimeError(
+                f"MVS failed to {operation}: 0x{result:08x}{hint}"
+            )
+
+    def _grab_loop(self) -> None:
+        assert self._camera is not None
+        assert self._mvs is not None
+        timeout_ms = int(self.get_parameter("grab_timeout_ms").value)
+        while not self._stop_event.is_set():
+            frame = self._mvs.MV_FRAME_OUT()
+            memset(byref(frame), 0, sizeof(frame))
+            result = self._camera.MV_CC_GetImageBuffer(frame, timeout_ms)
+            if result != 0:
+                self._timeout_count += 1
+                if self._timeout_count == 1 or self._timeout_count % 10 == 0:
+                    self.get_logger().warning(
+                        "MVS image acquisition timed out or failed: "
+                        f"0x{result:08x}; consecutive={self._timeout_count}"
+                    )
+                continue
+            try:
+                self._timeout_count = 0
+                self._publish_frame(frame)
+            except Exception as error:
+                self.get_logger().error(f"Failed to publish MVS frame: {error}")
+            finally:
+                free_result = self._camera.MV_CC_FreeImageBuffer(frame)
+                if free_result != 0:
+                    self.get_logger().error(
+                        f"MVS failed to release frame: 0x{free_result:08x}"
+                    )
+
+    def _publish_frame(self, frame: Any) -> None:
+        assert self._mvs is not None
+        info = frame.stFrameInfo
+        width = int(info.nWidth)
+        height = int(info.nHeight)
+        if width != self.image_width or height != self.image_height:
+            raise RuntimeError(
+                f"received {width}x{height}, expected "
+                f"{self.image_width}x{self.image_height}"
+            )
+        if int(info.enPixelType) != int(self._mvs.PixelType_Gvsp_Mono8):
+            raise RuntimeError(
+                f"received pixel type 0x{int(info.enPixelType):x}, expected Mono8"
+            )
+        expected_size = width * height
+        if int(info.nFrameLen) < expected_size:
+            raise RuntimeError(
+                f"frame contains {info.nFrameLen} bytes, expected {expected_size}"
+            )
+        stamp = self.get_clock().now().to_msg()
+        image = Image()
+        image.header.stamp = stamp
+        image.header.frame_id = self.frame_id
+        image.height = height
+        image.width = width
+        image.encoding = "mono8"
+        image.is_bigendian = 0
+        image.step = width
+        image.data = string_at(frame.pBufAddr, expected_size)
+
+        camera_info = self._make_camera_info(width, height)
+        camera_info.header.stamp = stamp
+        camera_info.header.frame_id = self.frame_id
+        self.image_publisher.publish(image)
+        self.camera_info_publisher.publish(camera_info)
+        self._frame_count += 1
+
+    def _make_camera_info(self, width: int, height: int) -> CameraInfo:
+        message = CameraInfo()
+        message.width = width
+        message.height = height
+        with self._camera_info_lock:
+            calibration = self._calibration
+        if calibration is None:
+            message.distortion_model = "plumb_bob"
+            message.d = []
+            message.k = [0.0] * 9
+            message.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+            message.p = [0.0] * 12
+            return message
+        message.distortion_model = calibration.distortion_model
+        message.d = list(calibration.d)
+        message.k = list(calibration.k)
+        message.r = list(calibration.r)
+        message.p = list(calibration.p)
+        return message
+
+    def _set_camera_info_callback(
+        self,
+        request: SetCameraInfo.Request,
+        response: SetCameraInfo.Response,
+    ) -> SetCameraInfo.Response:
+        try:
+            if self.camera_info_path is None:
+                raise ValueError(
+                    "camera_info_url is empty; set it before saving calibration"
+                )
+            message = request.camera_info
+            if (
+                int(message.width) != self.image_width
+                or int(message.height) != self.image_height
+            ):
+                raise ValueError(
+                    f"calibration is {message.width}x{message.height}, expected "
+                    f"{self.image_width}x{self.image_height}"
+                )
+            mapping = calibration_to_mapping(self.camera_name, message)
+            self.camera_info_path.parent.mkdir(parents=True, exist_ok=True)
+            temporary_path = self.camera_info_path.with_suffix(
+                self.camera_info_path.suffix + ".tmp"
+            )
+            temporary_path.write_text(
+                yaml.safe_dump(mapping, sort_keys=False), encoding="utf-8"
+            )
+            temporary_path.replace(self.camera_info_path)
+            calibration = load_camera_calibration(self.camera_info_path)
+            self._validate_calibration_dimensions(calibration)
+            with self._camera_info_lock:
+                self._calibration = calibration
+            response.success = True
+            response.status_message = (
+                f"saved camera calibration to {self.camera_info_path}"
+            )
+            self.get_logger().info(response.status_message)
+        except Exception as error:
+            response.success = False
+            response.status_message = str(error)
+            self.get_logger().error(
+                f"Rejected camera calibration: {response.status_message}"
+            )
+        return response
+
+    def close(self) -> None:
+        if self._closed:
+            return
+        self._closed = True
+        self._stop_event.set()
+        if self._grab_thread is not None:
+            self._grab_thread.join(timeout=2.0)
+        if self._camera is not None and self._camera_opened:
+            if self._grabbing:
+                result = self._camera.MV_CC_StopGrabbing()
+                if result != 0:
+                    self.get_logger().warning(
+                        f"MVS failed to stop grabbing: 0x{result:08x}"
+                    )
+                self._grabbing = False
+            result = self._camera.MV_CC_CloseDevice()
+            if result != 0:
+                self.get_logger().warning(
+                    f"MVS failed to close camera: 0x{result:08x}"
+                )
+            self._camera.MV_CC_DestroyHandle()
+        elif self._camera is not None and self._handle_created:
+            self._camera.MV_CC_DestroyHandle()
+        self._camera = None
+        self._handle_created = False
+        self._camera_opened = False
+        if self._sdk_initialized and self._mvs is not None:
+            self._mvs.MvCamera.MV_CC_Finalize()
+            self._sdk_initialized = False
+
+    def destroy_node(self) -> None:
+        self.close()
+        super().destroy_node()
+
+
+def main(args: list[str] | None = None) -> None:
+    configure_fastdds_large_image_transport()
+    rclpy.init(args=args)
+    node: HikrobotCameraNode | None = None
+    try:
+        node = HikrobotCameraNode()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_calibration/linkerhand_calibration/models/__init__.py

@@ -0,0 +1,17 @@
+"""Model- and side-specific calibration policies."""
+
+from .registry import (
+    EngineBindings,
+    ProfileRegistry,
+    RegisteredProfile,
+    get_default_registry,
+)
+from .runtime_schema import validate_schema_v6_runtime_payload
+
+__all__ = [
+    "EngineBindings",
+    "ProfileRegistry",
+    "RegisteredProfile",
+    "get_default_registry",
+    "validate_schema_v6_runtime_payload",
+]

src/linkerhand_calibration/linkerhand_calibration/models/g20/__init__.py

@@ -0,0 +1,15 @@
+"""Registered profiles for this hand family."""
+
+from ..registry import ProfileRegistry
+
+
+def register_profiles(registry: ProfileRegistry) -> None:
+    from .legacy_11 import build_left_profile, build_right_profile
+    from .right_19 import build_profile
+
+    registry.register(build_profile())
+    registry.register(build_left_profile())
+    registry.register(build_right_profile())
+
+
+__all__ = ["register_profiles"]

src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py

@@ -0,0 +1,219 @@
+"""Adapt reviewed family profiles to the shared typed contract."""
+
+from __future__ import annotations
+
+from ...core import (
+    CalibrationProfile,
+    CommandLayout,
+    MeasurementPolicy,
+    MeasurementSpec,
+    MotionPolicy,
+    ProfileKey,
+    QualityPolicy,
+    ScopePolicy,
+    TagSpec,
+    TaskSpec,
+    ViewSpec,
+    VisionRigSpec,
+    ZeroSolvePolicy,
+)
+from .profile import MIMIC_DERIVED_FINGER_DIPS
+from ..registry import EngineBindings, RegisteredProfile
+from .artifacts import build_artifact_policy
+from .motion import (
+    build_calibration_motion_command,
+    build_calibration_preparation_waypoints,
+    build_calibration_return_waypoints,
+)
+
+
+_HARD_THRESHOLD_KEYS = frozenset(
+    {
+        "minimum_detection_rate",
+        "maximum_reprojection_error_px",
+        "maximum_axis_cycle_difference_rad",
+        "maximum_pose_line_rms_m",
+        "maximum_hysteresis_rad",
+        "maximum_validation_error_rad",
+    }
+)
+
+
+def _run_cli(args: list[str] | None = None) -> None:
+    from .runner import main
+
+    main(args)
+
+
+def _run_node(args: list[str] | None = None) -> None:
+    from .node import main
+
+    main(args)
+
+
+def adapt_profile(
+    *,
+    key: ProfileKey,
+    namespace: str,
+    hand_profile,
+    zero_profile,
+    mechanical_endpoint_joints: frozenset[str] = frozenset(),
+    post_solve_endpoint_joints: frozenset[str] = frozenset(),
+) -> RegisteredProfile:
+    fixed_by_view = {
+        view: frozenset(roles)
+        for view, roles in hand_profile.preflight_view_roles.items()
+    }
+    views = tuple(
+        ViewSpec(
+            name=view,
+            tags=tuple(
+                TagSpec(
+                    role=role,
+                    tag_id=int(tag_id),
+                    fixed_reference=role in fixed_by_view.get(view, frozenset()),
+                )
+                for role, tag_id in roles.items()
+            ),
+        )
+        for view, roles in hand_profile.view_tags.items()
+    )
+    record_specs = hand_profile.record_specs
+    command_index_by_joint = {
+        name: int(spec.motor_index) for name, spec in record_specs.items()
+    }
+    command_index_by_joint.update(
+        {
+            name: int(spec.motor_index)
+            for name, spec in hand_profile.joint_specs.items()
+        }
+    )
+    measurements = {
+        name: MeasurementSpec(
+            joint=name,
+            kind=str(spec.zero_kind or "curve"),
+            view=spec.view,
+            parent_role=spec.parent_role,
+            child_role=spec.child_role,
+            validation_source=(hand_profile.axis_validation_sources or {}).get(
+                name
+            ),
+            pose_axis_line_required=bool(
+                spec.pose_axis_line_required
+            ),
+        )
+        for name, spec in record_specs.items()
+    }
+    tasks = tuple(
+        TaskSpec(
+            key=spec.key,
+            view=spec.view,
+            command_index=int(spec.motor_index),
+            joints=tuple(spec.joints),
+            auxiliary_commands=tuple(spec.auxiliary_commands),
+            validation_only=bool(spec.validation_only),
+        )
+        for spec in hand_profile.sweep_specs
+    )
+    active = frozenset(hand_profile.active_joints)
+    passive = frozenset(hand_profile.passive_joints)
+    thumb = frozenset(name for name in active if name.startswith("thumb_"))
+    fingers = active - thumb
+    typed = CalibrationProfile(
+        key=key,
+        namespace=namespace,
+        command=CommandLayout(
+            names=tuple(hand_profile.command_names),
+            baseline_u8=tuple(int(value) for value in hand_profile.baseline_command),
+            command_index_by_joint=command_index_by_joint,
+            disabled_indices=frozenset(
+                index
+                for index, name in enumerate(hand_profile.command_names)
+                if name.startswith("reserved_")
+            ),
+        ),
+        vision=VisionRigSpec(
+            views=views,
+            common_frame="calibration_common",
+            extrinsic_reference_view=views[0].name,
+            extrinsics_quality_limits={
+                "reprojection_rms_px": 1.2,
+                "maximum_rotation_repeatability_deg": 0.3,
+                "maximum_translation_repeatability_m": 0.0015,
+            },
+            minimum_capture_counts={
+                "front_side_captures": 15,
+                "front_top_captures": 15,
+            },
+        ),
+        motion=MotionPolicy(
+            tasks=tasks,
+            precheck_sweeps=bool(hand_profile.precheck_sweeps),
+            steady_command_checkpoints=bool(
+                hand_profile.steady_command_checkpoints
+            ),
+        ),
+        measurement=MeasurementPolicy(
+            measurements=measurements,
+            cross_view_sources=dict(
+                hand_profile.axis_validation_sources or {}
+            ),
+            image_curve_joints=frozenset(
+                hand_profile.image_trajectory_joints
+            ),
+            directional_zero=bool(hand_profile.directional_zero),
+            cross_view_roll_curve=bool(hand_profile.cross_view_roll_curve),
+            stable_cross_view_cone_bias=bool(
+                hand_profile.stable_cross_view_cone_bias
+            ),
+        ),
+        zero=ZeroSolvePolicy(
+            active_joints=active,
+            passive_joints=passive,
+            direct_zero_joints=tuple(zero_profile.direct_zero_joints),
+            axis_joints=tuple(zero_profile.axis_joints),
+            mechanical_endpoint_joints=mechanical_endpoint_joints,
+            post_solve_endpoint_joints=post_solve_endpoint_joints,
+            mimic_source_by_joint={
+                target: source
+                for target, source in MIMIC_DERIVED_FINGER_DIPS.items()
+                if target in passive and source in active
+            },
+            cad_frozen_joints=frozenset(
+                passive - set(zero_profile.static_output_zero_offsets_rad)
+            ),
+        ),
+        quality=QualityPolicy(
+            training_cycles=(0, 1, 2),
+            holdout_cycle=3 if key.layout != "legacy_11" else None,
+            hard_threshold_keys=_HARD_THRESHOLD_KEYS,
+            isolated_holdout=bool(hand_profile.isolated_holdout),
+        ),
+        scope=ScopePolicy(
+            calibrate_joints={
+                "full": active,
+                "thumb": thumb,
+                "fingers": fingers,
+            },
+            frozen_joints={
+                "full": frozenset(),
+                "thumb": fingers,
+                "fingers": thumb,
+            },
+        ),
+        artifacts=build_artifact_policy(
+            frozenset(hand_profile.capabilities)
+        ),
+    )
+    return RegisteredProfile(
+        profile=typed,
+        engine=EngineBindings(
+            hand_profile=hand_profile,
+            zero_profile=zero_profile,
+            motion_command=build_calibration_motion_command,
+            preparation_waypoints=build_calibration_preparation_waypoints,
+            return_waypoints=build_calibration_return_waypoints,
+            cli_main=_run_cli,
+            node_main=_run_node,
+        ),
+    )

src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py

@@ -0,0 +1,30 @@
+"""Runtime JSON and corrected-URDF naming policy."""
+
+from ...core import ArtifactPolicy
+
+
+def build_artifact_policy(
+    compatibility_tokens: frozenset[str],
+) -> ArtifactPolicy:
+    return ArtifactPolicy(
+        output_schema_version=4,
+        calibration_filename="g20_{side}_{serial_number}_calibration.json",
+        corrected_urdf_filename=(
+            "linkerhand_g20_{side}_{serial_number}_zero_calibrated.urdf"
+        ),
+        protected_input_fields=frozenset(
+            {
+                "source_urdf_sha256",
+                "camera_extrinsics_sha256",
+                "calibration_config_sha256",
+                "tag_config_sha256",
+            }
+        ),
+        session_compatibility_tokens=frozenset(compatibility_tokens),
+        publish_corrected_urdf=(
+            "urdf_zero_publication" in compatibility_tokens
+        ),
+    )
+
+
+__all__ = ["build_artifact_policy"]

src/linkerhand_calibration/linkerhand_calibration/models/g20/command_layout.py

@@ -0,0 +1,26 @@
+"""Reviewed command-channel layouts for this hand family."""
+
+G20_COMMAND_NAMES: tuple[str, ...] = (
+    "thumb_cmc_pitch",
+    "index_mcp_pitch",
+    "middle_mcp_pitch",
+    "ring_mcp_pitch",
+    "pinky_mcp_pitch",
+    "thumb_cmc_roll",
+    "index_mcp_roll",
+    "middle_mcp_roll",
+    "ring_mcp_roll",
+    "pinky_mcp_roll",
+    "thumb_cmc_yaw",
+    "reserved_11",
+    "reserved_12",
+    "reserved_13",
+    "reserved_14",
+    "thumb_mcp",
+    "index_pip",
+    "middle_pip",
+    "ring_pip",
+    "pinky_pip",
+)
+
+__all__ = ["G20_COMMAND_NAMES"]

src/linkerhand_calibration/linkerhand_calibration/models/g20/golden_regression.py

@@ -0,0 +1,144 @@
+"""Read-only regression checks for the reviewed hardware sessions."""
+
+from __future__ import annotations
+
+import argparse
+import hashlib
+import json
+from pathlib import Path
+from typing import Any
+
+_PASS_SESSIONS = {
+    "20260830_181154": "full",
+    "20260831_141123": "thumb",
+    "20260831_163843": "thumb",
+}
+_FAIL_SESSIONS = {
+    "20260831_111837": ("FIT-MODEL-401", "joint_fit_check_failed"),
+    "20260831_142322": ("VAL-QUALITY-501", "zero_model_validation_failed"),
+}
+
+
+def _sha256_file(path: Path) -> str:
+    digest = hashlib.sha256()
+    with path.open("rb") as stream:
+        for chunk in iter(lambda: stream.read(1024 * 1024), b""):
+            digest.update(chunk)
+    return digest.hexdigest()
+
+
+def _summary(session: Path) -> dict[str, Any]:
+    value = json.loads(
+        (session / "calibration_summary_zh.json").read_text(encoding="utf-8")
+    )
+    if not isinstance(value, dict):
+        raise ValueError(f"invalid golden summary: {session}")
+    return value
+
+
+def validate_golden_sessions(
+    serial_root: str | Path,
+    *,
+    replay_full_session: bool = True,
+) -> dict[str, Any]:
+    """Prove PASS/FAIL, hashes, partial freezes, and failure non-publication."""
+    root = Path(serial_root).expanduser().resolve()
+    results: dict[str, Any] = {}
+    for session_id, scope in _PASS_SESSIONS.items():
+        session = root / session_id
+        summary = _summary(session)
+        if summary.get("result") != "PASS":
+            raise ValueError(f"golden PASS changed: {session_id}")
+        if summary.get("calibration_scope") != scope:
+            raise ValueError(f"golden scope changed: {session_id}")
+        artifacts = summary["artifacts"]
+        hashes = summary["hashes"]
+        calibration_json = session / artifacts["json"]
+        corrected_urdf = session / artifacts["urdf"]
+        actual_json_hash = _sha256_file(calibration_json)
+        actual_urdf_hash = _sha256_file(corrected_urdf)
+        if actual_json_hash != hashes["calibration_json_sha256"]:
+            raise ValueError(f"golden JSON hash changed: {session_id}")
+        if actual_urdf_hash != hashes["corrected_urdf_sha256"]:
+            raise ValueError(f"golden URDF hash changed: {session_id}")
+        if session_id == "20260831_141123" and len(
+            summary.get("preserved_certified_zero_joints", ())
+        ) != 12:
+            raise ValueError("merged thumb no longer freezes all finger zeros")
+        if session_id == "20260831_163843":
+            payload = json.loads(calibration_json.read_text(encoding="utf-8"))
+            if payload.get("artifact_type") != (
+                "g20_right_standalone_thumb_calibration"
+            ):
+                raise ValueError("standalone thumb artifact type changed")
+        results[session_id] = {
+            "result": "PASS",
+            "json_sha256": actual_json_hash,
+            "urdf_sha256": actual_urdf_hash,
+        }
+
+    for session_id, (error_code, reason) in _FAIL_SESSIONS.items():
+        session = root / session_id
+        summary = _summary(session)
+        if (
+            summary.get("result") != "FAIL"
+            or summary.get("error_code") != error_code
+            or summary.get("reason") != reason
+        ):
+            raise ValueError(f"golden failure decision changed: {session_id}")
+        results[session_id] = {
+            "result": "FAIL",
+            "error_code": error_code,
+            "reason": reason,
+        }
+
+    failure_ids = set(_FAIL_SESSIONS)
+    for pointer_name in ("latest_passed", "latest_thumb_passed"):
+        pointer = root / pointer_name
+        if pointer.exists() and pointer.resolve().name in failure_ids:
+            raise ValueError(f"failure session was published through {pointer_name}")
+
+    if replay_full_session:
+        from .offline_replay import replay_session
+
+        session_id = "20260830_181154"
+        replay = replay_session(root / session_id, write_outputs=False)
+        expected = results[session_id]
+        if replay["computed_final_json_sha256"] != expected["json_sha256"]:
+            raise ValueError(
+                "full-session replay JSON is not byte-identical: "
+                f"computed={replay['computed_final_json_sha256']} "
+                f"expected={expected['json_sha256']}"
+            )
+        if replay["corrected_urdf_sha256"] != expected["urdf_sha256"]:
+            raise ValueError(
+                "full-session replay URDF is not byte-identical: "
+                f"computed={replay['corrected_urdf_sha256']} "
+                f"expected={expected['urdf_sha256']}"
+            )
+        results[session_id]["offline_replay"] = "byte_identical"
+    return results
+
+
+def main(args: list[str] | None = None) -> None:
+    parser = argparse.ArgumentParser(
+        description="Validate the five reviewed calibration sessions"
+    )
+    parser.add_argument("serial_root")
+    parser.add_argument("--no-replay", action="store_true")
+    selected = parser.parse_args(args)
+    print(
+        json.dumps(
+            validate_golden_sessions(
+                selected.serial_root,
+                replay_full_session=not selected.no_replay,
+            ),
+            ensure_ascii=False,
+            indent=2,
+            sort_keys=True,
+        )
+    )
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py

@@ -0,0 +1,35 @@
+"""One-release typed wrappers for the legacy 11-Tag layouts."""
+
+from ...core import ProfileKey
+from .profile import get_hand_calibration_profile
+from ...urdf_zero import get_zero_calibration_profile
+from ..registry import RegisteredProfile
+from ._adapter import adapt_profile
+
+
+LEFT_KEY = ProfileKey("G20", "left", "legacy_11", 1)
+RIGHT_KEY = ProfileKey("G20", "right", "legacy_11", 1)
+
+
+def build_left_profile() -> RegisteredProfile:
+    hand = get_hand_calibration_profile(LEFT_KEY.side, LEFT_KEY.layout)
+    return adapt_profile(
+        key=LEFT_KEY,
+        namespace="/g20_calibration",
+        hand_profile=hand,
+        zero_profile=get_zero_calibration_profile(
+            LEFT_KEY.side, LEFT_KEY.layout
+        ),
+    )
+
+
+def build_right_profile() -> RegisteredProfile:
+    hand = get_hand_calibration_profile(RIGHT_KEY.side, RIGHT_KEY.layout)
+    return adapt_profile(
+        key=RIGHT_KEY,
+        namespace="/g20_calibration",
+        hand_profile=hand,
+        zero_profile=get_zero_calibration_profile(
+            RIGHT_KEY.side, RIGHT_KEY.layout
+        ),
+    )

src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py

@@ -0,0 +1,15 @@
+"""Reviewed motion and safe-waypoint strategy exports."""
+
+from .profile import (
+    build_calibration_motion_command,
+    build_calibration_preparation_waypoints,
+    build_calibration_return_waypoints,
+    build_calibration_speed_profile,
+)
+
+__all__ = [
+    "build_calibration_motion_command",
+    "build_calibration_preparation_waypoints",
+    "build_calibration_return_waypoints",
+    "build_calibration_speed_profile",
+]