diff --git a/src/linkerhand_range_calibration/README.md b/src/linkerhand_range_calibration/README.md index e1fdcf2..443ea83 100644 --- a/src/linkerhand_range_calibration/README.md +++ b/src/linkerhand_range_calibration/README.md @@ -1,6 +1,6 @@ # LinkerHand 有效指令行程标定 -本包通过指尖 AprilTag 的图像运动,测量机械手实际有效的控制指令范围。首个型号为 O30 右手。 +本包通过指尖 AprilTag 的图像运动,测量机械手实际有效的控制指令范围,支持 O30 左手和右手。 输出的是 `0~255` 中的有效指令上下界,单位为 `u8`。没有角度反解、URDF 修改或三维融合。 **O30 SDK 源码、launch 和配置均保持原样。** 新包启动既有驱动可执行程序,并使用其原有话题与参数。 @@ -35,6 +35,9 @@ source install/range_calibration/setup.bash # 仅加载和检查配置,不初始化 ROS 或硬件。 ros2 run linkerhand_range_calibration calibrate_range --profile o30_right --validate-only +# 左手使用镜像配置。 +ros2 run linkerhand_range_calibration calibrate_range --profile o30_left --validate-only + # 假SDK + 虚拟时间,自动扫描全部17个任务;结果写在 SIMULATED_HAND 目录。 ros2 run linkerhand_range_calibration calibrate_range --profile o30_right --simulate \ --output /tmp/linkerhand_range_simulation @@ -62,6 +65,9 @@ ros2 launch linkerhand_range_calibration calibrate.launch.py profile:=o30_right ```bash ros2 launch linkerhand_range_calibration calibrate.launch.py profile:=o30_right +# 左手 +ros2 launch linkerhand_range_calibration calibrate.launch.py profile:=o30_left + # 使用自己的工位配置 ros2 launch linkerhand_range_calibration calibrate.launch.py \ profile:=o30_right station:=/absolute/path/to/station.yaml @@ -77,7 +83,7 @@ ros2 launch linkerhand_range_calibration calibrate.launch.py \ 4. 必要时为单独任务保存“当前任务准备姿态覆盖”。避让值优先于覆盖值,防止覆盖已设置的避让。四指侧摆固定使用基础姿态,禁止覆盖。 5. 先试标定一个任务,检查可见性和运动;之后执行全手标定。 -O30 右手所有避让均使用固定指令,无需手动示教:`thumb_cmc_roll` 任务的食指侧摆固定为0,`thumb_mcp` 任务的拇指 `thumb_cmc_yaw` 固定为80,小指、无名指、中指的弯曲避让固定为255。 +O30 两侧手的所有避让均使用固定指令,无需手动示教:`thumb_cmc_roll` 任务的食指侧摆固定为0,`thumb_mcp` 任务的拇指 `thumb_cmc_yaw` 固定为80。右手侧面标定依次折叠小指、无名指、中指;左手镜像为依次折叠食指、中指、无名指,弯曲避让值均为255。 示教下拉框只保留全手基础姿态和可选的任务准备姿态覆盖。 关节、SDK 下标、目标滑块、实际反馈、min、max 和状态合并在同一张表中,共用一个滚动条,同一关节始终在同一行。 @@ -93,22 +99,24 @@ SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK 示教配置保存在 `~/.ros/linkerhand_range_calibration//teaching.yaml`。 不同设备 UID 不会自动复用示教。 已有示教文件可继续使用,只需已有全手基础姿态,无需补存避让项。 -旧文件中的 `index_roll_for_thumb`、`pinky_fold`、`ring_fold`、`middle_fold` 记录可以保留;运行时采用型号配置中的固定避让值,食指侧摆为0,三指弯曲为255。 +旧文件中的避让记录可以保留;运行时采用当前左右手型号配置中的固定避让值,食指侧摆为0,三指弯曲为255。左右手示教文件通过 `side` 字段严格隔离,不会交叉复用。 ## 4. 任务与 Tag -| 机位 / ID | 标定关节 | SDK 数组下标(从0开始) | +| 机位 / ID | O30 右手 | O30 左手(镜像) | |---|---|---| -| 正面 / 0 | thumb_cmc_roll、thumb_mcp、thumb_ip | 0、6、15 | -| 正面 / 4 | index_mcp_roll | 2 | -| 正面 / 3 | middle_mcp_roll | 3 | -| 正面 / 2 | ring_mcp_roll | 4 | -| 正面 / 1 | pinky_mcp_roll | 5 | -| 侧面 / 5 | pinky_mcp_pitch、pinky_pip、pinky_dip | 10、14、19 | -| 侧面 / 6 | ring_mcp_pitch、ring_pip、ring_dip | 9、13、18 | -| 侧面 / 7 | middle_mcp_pitch、middle_pip、middle_dip | 8、12、17 | -| 侧面 / 8 | index_mcp_pitch、index_pip、index_dip | 7、11、16 | -| 顶部 / 9 | thumb_cmc_yaw | 1 | +| 正面 / 10 | thumb_cmc_roll、thumb_mcp、thumb_ip | thumb_cmc_roll、thumb_mcp、thumb_ip | +| 正面 / 11 | pinky_mcp_roll | index_mcp_roll | +| 正面 / 12 | ring_mcp_roll | middle_mcp_roll | +| 正面 / 13 | middle_mcp_roll | ring_mcp_roll | +| 正面 / 14 | index_mcp_roll | pinky_mcp_roll | +| 侧面 / 15 | pinky_mcp_pitch、pinky_pip、pinky_dip | index_mcp_pitch、index_pip、index_dip | +| 侧面 / 16 | ring_mcp_pitch、ring_pip、ring_dip | middle_mcp_pitch、middle_pip、middle_dip | +| 侧面 / 17 | middle_mcp_pitch、middle_pip、middle_dip | ring_mcp_pitch、ring_pip、ring_dip | +| 侧面 / 18 | index_mcp_pitch、index_pip、index_dip | pinky_mcp_pitch、pinky_pip、pinky_dip | +| 顶部 / 19 | thumb_cmc_yaw | thumb_cmc_yaw | + +SDK 数组下标两侧一致:`thumb_cmc_roll=0`、`thumb_cmc_yaw=1`、四指侧摆为2~5、`thumb_mcp=6`、四指 mcp_pitch 为7~10、pip 为11~14、`thumb_ip=15`、四指 dip 为16~19。 任务顺序:正面拇指3项 → 四指侧摆分阶段1项 → 侧面弯曲12项 → 顶部拇指1项。 @@ -143,7 +151,7 @@ SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK 三指每次改变1,中指每次改变0或1,始终在同一条消息中更新。每个 Tag 独立判断;中指相同指令的重复观测不重复参与边界搜索。 正向确认三指下限后,可直接沿限速轨迹到255再测上限;反向确认四指上限后,仍会先到达“三指0、中指80”并等待新反馈稳定,再只让中指回零。 -中指单独扫描只要求正面 ID3 有效,其他三指保持0。若到80仍未检测到运动,就进入联动继续阶段; +中指单独扫描只要求该型号配置中 `middle_mcp_roll` 绑定的正面 Tag 有效,其他三指保持0。若到80仍未检测到运动,就进入联动继续阶段; 从0采集的参考和噪声全部保留,不在80重置,也不重复计算80。首次检测到运动即结束该轮,不再扫描剩余内部行程。 同步及联动继续阶段要求四个 Tag 有效,任一丢失整组停止推进。 @@ -151,23 +159,22 @@ SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK 其他关节扫描到对端仍无法确认运动也记录失败,不为已失败关节重复测量。 复测时重做上述完整顺序;暂停后继续会重做整个四指任务。 -配置位于 `config/profiles/o30_right.yaml` 的 `four_finger_roll.deferred_lower`: +配置位于 `config/profiles/o30_right.yaml` 和 `config/profiles/o30_left.yaml` 的 `four_finger_roll.deferred_lower`: `joint: middle_mcp_roll` 指定延后测下限的通道,`split: 80` 指定分段值。 该流程不依赖 O30 的关节名称,可复用于其他型号的多通道任务。没有该配置的任务沿用普通两端搜索。 -侧面按小指→无名指→中指→食指测量,每指依次测 mcp_pitch、pip、dip。 -测无名指时小指弯曲;测中指时小指和无名指弯曲;测食指时前三指弯曲。 +右手侧面按小指→无名指→中指→食指测量;左手按镜像顺序食指→中指→无名指→小指测量,每指依次测 mcp_pitch、pip、dip。 +右手测后一根手指时,依次累积折叠小指、无名指、中指;左手依次累积折叠食指、中指、无名指。 参与避让的每根手指,其 `mcp_pitch`、`pip`、`dip` 三个关节目标均固定为255: -| 避让组 | 固定目标 | +| 侧别 | 避让链 | |---|---| -| `pinky_fold` | `pinky_mcp_pitch=255`、`pinky_pip=255`、`pinky_dip=255` | -| `ring_fold` | `ring_mcp_pitch=255`、`ring_pip=255`、`ring_dip=255` | -| `middle_fold` | `middle_mcp_pitch=255`、`middle_pip=255`、`middle_dip=255` | +| 右手 | `pinky_fold` → `ring_fold` → `middle_fold` | +| 左手 | `index_fold` → `middle_fold` → `ring_fold` | 当前手指非目标关节的目标指令保持准备姿态,切换任务时重新构建完整姿态。 扫描只使用当前任务绑定的 Tag,其他关节的连带运动、反馈变化及无关 Tag 的移动或丢失不会中断扫描。 -例如标定 `pinky_pip` 时,只使用侧面 ID5;正面 ID1 的连带运动和 `pinky_mcp_roll` 的反馈变化不参与边界判定。 +例如右手标定 `pinky_pip` 时,只使用侧面 ID15;正面 ID11 的连带运动和 `pinky_mcp_roll` 的反馈变化不参与边界判定。左手标定 `index_pip` 时同样只使用侧面 ID15。 ## 5. 算法与暂停 @@ -213,7 +220,7 @@ SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK 新日志的 `sampling_method` 为 `separate_noise_drift_v2`。样本分别记录 `jitter_px`、`trend_px`、`trend_allowance_px`、`drift_px` 及两项门槛。 `sampling_unstable` 和 `sampling_timeout` 额外保存当时的逐帧时间戳、四角点(`window_samples`),便于离线检查。旧日志只有汇总值时无法补回随机抖动与持续漂移的区别。 -超时提示会列出当前受阻的机位、Tag ID、关节和原因,例如 `正面 Tag ID0(thumb_cmc_roll):随机抖动 0.620 > 0.500 像素` 或 `持续漂移 0.180 > 0.100 像素`。 +超时提示会列出当前受阻的机位、Tag ID、关节和原因,例如 `正面 Tag ID10(thumb_cmc_roll):随机抖动 0.620 > 0.500 像素` 或 `持续漂移 0.180 > 0.100 像素`。 原因包括未检出、检测质量不足、观测过期、新图像数量或观察时长不足、随机抖动/持续漂移过大;反馈不足或未稳定会单独说明,不归因于 Tag。 四指任务只列出当前受阻的观测;相机检测流断流时列出该机位当前任务所需的 Tag。 同一机位至少3个 Tag 的角点在相同时间呈现高度一致的平移时,超时提示会补充“多个 Tag 出现共同平移”及幅度,日志记录 `shared_motion`。这只能提示共同变化,无法仅凭指尖 Tag 判定是相机、手掌还是支架在移动,也不会据此扣除图像位移或放行采样。 @@ -251,7 +258,7 @@ SDK 诊断只进入日志,保留标定关节名、SDK 原名和具体内容, ## 6. 文件与离线复算 -结果位置:`range_calibration_output//<时间>/o30_right_ranges.json`。 +结果位置:`range_calibration_output//<时间>/o30__ranges.json`,其中 `` 为 `left` 或 `right`。 仅包含 `schema_version`、`model`、`side`、`device_uid`、`command_unit`、`joints`。 20个关节始终存在,每项只有min/max;失败或未完成均为null。 每个任务结束、暂停或取消时原子保存。再次点击开始/重测创建新会话,不覆盖历史文件。 diff --git a/src/linkerhand_range_calibration/config/profiles/o30_left.yaml b/src/linkerhand_range_calibration/config/profiles/o30_left.yaml new file mode 100644 index 0000000..6e8833c --- /dev/null +++ b/src/linkerhand_range_calibration/config/profiles/o30_left.yaml @@ -0,0 +1,67 @@ +schema_version: 1 +model: O30 +side: left +command_unit: u8 +adapter: o30_ros +command: {minimum: 0, maximum: 255, resolution: 1} +sdk: + package: linker_hand_o30_ros2_sdk + executable: linker_hand_o30_ros2_sdk + node: /linkerhand_range_sdk + command_topic: /cb_left_hand_control_cmd + feedback_topic: /cb_left_hand_state + setting_topic: /cb_left_hand_setting_cmd + info_topic: /cb_left_hand_info +joints: + - {name: thumb_cmc_roll, sdk_name: thumb_roll, index: 0, view: front, tag_id: 10} + - {name: thumb_cmc_yaw, sdk_name: thumb_yaw, index: 1, view: top, tag_id: 19} + - {name: index_mcp_roll, sdk_name: index_yaw, index: 2, view: front, tag_id: 11} + - {name: middle_mcp_roll, sdk_name: middle_yaw, index: 3, view: front, tag_id: 12} + - {name: ring_mcp_roll, sdk_name: ring_yaw, index: 4, view: front, tag_id: 13} + - {name: pinky_mcp_roll, sdk_name: little_yaw, index: 5, view: front, tag_id: 14} + - {name: thumb_mcp, sdk_name: thumb_root1, index: 6, view: front, tag_id: 10} + - {name: index_mcp_pitch, sdk_name: index_root1, index: 7, view: side, tag_id: 15} + - {name: middle_mcp_pitch, sdk_name: middle_root1, index: 8, view: side, tag_id: 16} + - {name: ring_mcp_pitch, sdk_name: ring_root1, index: 9, view: side, tag_id: 17} + - {name: pinky_mcp_pitch, sdk_name: little_root1, index: 10, view: side, tag_id: 18} + - {name: index_pip, sdk_name: index_root2, index: 11, view: side, tag_id: 15} + - {name: middle_pip, sdk_name: middle_root2, index: 12, view: side, tag_id: 16} + - {name: ring_pip, sdk_name: ring_root2, index: 13, view: side, tag_id: 17} + - {name: pinky_pip, sdk_name: little_root2, index: 14, view: side, tag_id: 18} + - {name: thumb_ip, sdk_name: thumb_tip, index: 15, view: front, tag_id: 10} + - {name: index_dip, sdk_name: index_tip, index: 16, view: side, tag_id: 15} + - {name: middle_dip, sdk_name: middle_tip, index: 17, view: side, tag_id: 16} + - {name: ring_dip, sdk_name: ring_tip, index: 18, view: side, tag_id: 17} + - {name: pinky_dip, sdk_name: little_tip, index: 19, view: side, tag_id: 18} +clearances: + index_roll_for_thumb: + targets: {index_mcp_roll: 0} + thumb_yaw_for_mcp: + targets: {thumb_cmc_yaw: 80} + index_fold: + targets: {index_mcp_pitch: 255, index_pip: 255, index_dip: 255} + middle_fold: + targets: {middle_mcp_pitch: 255, middle_pip: 255, middle_dip: 255} + ring_fold: + targets: {ring_mcp_pitch: 255, ring_pip: 255, ring_dip: 255} +tasks: + - {name: thumb_cmc_roll, joints: [thumb_cmc_roll], clearances: [index_roll_for_thumb], restore_after: [thumb_cmc_roll, index_mcp_roll]} + - {name: thumb_mcp, joints: [thumb_mcp], clearances: [thumb_yaw_for_mcp]} + - {name: thumb_ip, joints: [thumb_ip]} + - name: four_finger_roll + joints: [index_mcp_roll, middle_mcp_roll, ring_mcp_roll, pinky_mcp_roll] + allow_override: false + deferred_lower: {joint: middle_mcp_roll, split: 80} + - {name: index_mcp_pitch, joints: [index_mcp_pitch]} + - {name: index_pip, joints: [index_pip]} + - {name: index_dip, joints: [index_dip]} + - {name: middle_mcp_pitch, joints: [middle_mcp_pitch], clearances: [index_fold]} + - {name: middle_pip, joints: [middle_pip], clearances: [index_fold]} + - {name: middle_dip, joints: [middle_dip], clearances: [index_fold]} + - {name: ring_mcp_pitch, joints: [ring_mcp_pitch], clearances: [index_fold, middle_fold]} + - {name: ring_pip, joints: [ring_pip], clearances: [index_fold, middle_fold]} + - {name: ring_dip, joints: [ring_dip], clearances: [index_fold, middle_fold]} + - {name: pinky_mcp_pitch, joints: [pinky_mcp_pitch], clearances: [index_fold, middle_fold, ring_fold]} + - {name: pinky_pip, joints: [pinky_pip], clearances: [index_fold, middle_fold, ring_fold]} + - {name: pinky_dip, joints: [pinky_dip], clearances: [index_fold, middle_fold, ring_fold]} + - {name: thumb_cmc_yaw, joints: [thumb_cmc_yaw]} diff --git a/src/linkerhand_range_calibration/config/profiles/o30_right.yaml b/src/linkerhand_range_calibration/config/profiles/o30_right.yaml index a136723..70fa4af 100644 --- a/src/linkerhand_range_calibration/config/profiles/o30_right.yaml +++ b/src/linkerhand_range_calibration/config/profiles/o30_right.yaml @@ -13,26 +13,26 @@ sdk: setting_topic: /cb_right_hand_setting_cmd info_topic: /cb_right_hand_info joints: - - {name: thumb_cmc_roll, sdk_name: thumb_roll, index: 0, view: front, tag_id: 0} - - {name: thumb_cmc_yaw, sdk_name: thumb_yaw, index: 1, view: top, tag_id: 9} - - {name: index_mcp_roll, sdk_name: index_yaw, index: 2, view: front, tag_id: 4} - - {name: middle_mcp_roll, sdk_name: middle_yaw, index: 3, view: front, tag_id: 3} - - {name: ring_mcp_roll, sdk_name: ring_yaw, index: 4, view: front, tag_id: 2} - - {name: pinky_mcp_roll, sdk_name: little_yaw, index: 5, view: front, tag_id: 1} - - {name: thumb_mcp, sdk_name: thumb_root1, index: 6, view: front, tag_id: 0} - - {name: index_mcp_pitch, sdk_name: index_root1, index: 7, view: side, tag_id: 8} - - {name: middle_mcp_pitch, sdk_name: middle_root1, index: 8, view: side, tag_id: 7} - - {name: ring_mcp_pitch, sdk_name: ring_root1, index: 9, view: side, tag_id: 6} - - {name: pinky_mcp_pitch, sdk_name: little_root1, index: 10, view: side, tag_id: 5} - - {name: index_pip, sdk_name: index_root2, index: 11, view: side, tag_id: 8} - - {name: middle_pip, sdk_name: middle_root2, index: 12, view: side, tag_id: 7} - - {name: ring_pip, sdk_name: ring_root2, index: 13, view: side, tag_id: 6} - - {name: pinky_pip, sdk_name: little_root2, index: 14, view: side, tag_id: 5} - - {name: thumb_ip, sdk_name: thumb_tip, index: 15, view: front, tag_id: 0} - - {name: index_dip, sdk_name: index_tip, index: 16, view: side, tag_id: 8} - - {name: middle_dip, sdk_name: middle_tip, index: 17, view: side, tag_id: 7} - - {name: ring_dip, sdk_name: ring_tip, index: 18, view: side, tag_id: 6} - - {name: pinky_dip, sdk_name: little_tip, index: 19, view: side, tag_id: 5} + - {name: thumb_cmc_roll, sdk_name: thumb_roll, index: 0, view: front, tag_id: 10} + - {name: thumb_cmc_yaw, sdk_name: thumb_yaw, index: 1, view: top, tag_id: 19} + - {name: index_mcp_roll, sdk_name: index_yaw, index: 2, view: front, tag_id: 14} + - {name: middle_mcp_roll, sdk_name: middle_yaw, index: 3, view: front, tag_id: 13} + - {name: ring_mcp_roll, sdk_name: ring_yaw, index: 4, view: front, tag_id: 12} + - {name: pinky_mcp_roll, sdk_name: little_yaw, index: 5, view: front, tag_id: 11} + - {name: thumb_mcp, sdk_name: thumb_root1, index: 6, view: front, tag_id: 10} + - {name: index_mcp_pitch, sdk_name: index_root1, index: 7, view: side, tag_id: 18} + - {name: middle_mcp_pitch, sdk_name: middle_root1, index: 8, view: side, tag_id: 17} + - {name: ring_mcp_pitch, sdk_name: ring_root1, index: 9, view: side, tag_id: 16} + - {name: pinky_mcp_pitch, sdk_name: little_root1, index: 10, view: side, tag_id: 15} + - {name: index_pip, sdk_name: index_root2, index: 11, view: side, tag_id: 18} + - {name: middle_pip, sdk_name: middle_root2, index: 12, view: side, tag_id: 17} + - {name: ring_pip, sdk_name: ring_root2, index: 13, view: side, tag_id: 16} + - {name: pinky_pip, sdk_name: little_root2, index: 14, view: side, tag_id: 15} + - {name: thumb_ip, sdk_name: thumb_tip, index: 15, view: front, tag_id: 10} + - {name: index_dip, sdk_name: index_tip, index: 16, view: side, tag_id: 18} + - {name: middle_dip, sdk_name: middle_tip, index: 17, view: side, tag_id: 17} + - {name: ring_dip, sdk_name: ring_tip, index: 18, view: side, tag_id: 16} + - {name: pinky_dip, sdk_name: little_tip, index: 19, view: side, tag_id: 15} clearances: index_roll_for_thumb: targets: {index_mcp_roll: 0} diff --git a/src/linkerhand_range_calibration/test/fixtures/group_upper_reference_drift.json b/src/linkerhand_range_calibration/test/fixtures/group_upper_reference_drift.json index e45653c..d5d6185 100644 --- a/src/linkerhand_range_calibration/test/fixtures/group_upper_reference_drift.json +++ b/src/linkerhand_range_calibration/test/fixtures/group_upper_reference_drift.json @@ -11,7 +11,7 @@ "issues": [ { "joint": "middle_mcp_roll", - "tag_id": 3, + "tag_id": 13, "window_samples": [ { "stamp_ns": 1789646741618200400, @@ -838,7 +838,7 @@ }, { "joint": "ring_mcp_roll", - "tag_id": 2, + "tag_id": 12, "window_samples": [ { "stamp_ns": 1789646741618200400, diff --git a/src/linkerhand_range_calibration/test/fixtures/shared_translation_timeout.json b/src/linkerhand_range_calibration/test/fixtures/shared_translation_timeout.json index 599d696..16be3e4 100644 --- a/src/linkerhand_range_calibration/test/fixtures/shared_translation_timeout.json +++ b/src/linkerhand_range_calibration/test/fixtures/shared_translation_timeout.json @@ -6,7 +6,7 @@ "source": "tag", "joint": "index_mcp_roll", "view": "front", - "tag_id": 4, + "tag_id": 14, "window_samples": [ { "stamp_ns": 1789626527738607441, @@ -280,7 +280,7 @@ "source": "tag", "joint": "middle_mcp_roll", "view": "front", - "tag_id": 3, + "tag_id": 13, "window_samples": [ { "stamp_ns": 1789626527738607441, @@ -554,7 +554,7 @@ "source": "tag", "joint": "ring_mcp_roll", "view": "front", - "tag_id": 2, + "tag_id": 12, "window_samples": [ { "stamp_ns": 1789626527738607441, @@ -828,7 +828,7 @@ "source": "tag", "joint": "pinky_mcp_roll", "view": "front", - "tag_id": 1, + "tag_id": 11, "window_samples": [ { "stamp_ns": 1789626527738607441, diff --git a/src/linkerhand_range_calibration/test/test_candidate_direction.py b/src/linkerhand_range_calibration/test/test_candidate_direction.py index 0ae15b1..a578103 100644 --- a/src/linkerhand_range_calibration/test/test_candidate_direction.py +++ b/src/linkerhand_range_calibration/test/test_candidate_direction.py @@ -85,7 +85,7 @@ def test_recorded_departure_stops_live_single_joint_scan_and_replays(tmp_path): def frames(self): frames = super().frames() if self.target[10] in trace: - frames['side'][5] = trace[self.target[10]] + frames['side'][15] = trace[self.target[10]] return frames profile = load_profile() diff --git a/src/linkerhand_range_calibration/test/test_core.py b/src/linkerhand_range_calibration/test/test_core.py index 19766ab..d8be72c 100644 --- a/src/linkerhand_range_calibration/test/test_core.py +++ b/src/linkerhand_range_calibration/test/test_core.py @@ -98,6 +98,7 @@ def test_rotation_and_small_cumulative_movement(): def test_profile_coverage_and_clearance(): p = load_profile() assert len(p.joints)==20 and len(p.tasks)==17 + assert {joint.tag_id for joint in p.joints} == set(range(10, 20)) group=next(t for t in p.tasks if t.name=='four_finger_roll') assert [p.by_name[n].index for n in group.joints]==[2,3,4,5] teach=simulated_teaching(p,'TEST') @@ -116,6 +117,47 @@ def test_profile_coverage_and_clearance(): with pytest.raises(ValueError): profile_from_dict(bad) +def test_o30_left_profile_mirrors_tags_task_order_and_clearances(): + profile = load_profile('o30_left') + assert profile.side == 'left' + assert profile.sdk == { + 'package': 'linker_hand_o30_ros2_sdk', + 'executable': 'linker_hand_o30_ros2_sdk', + 'node': '/linkerhand_range_sdk', + 'command_topic': '/cb_left_hand_control_cmd', + 'feedback_topic': '/cb_left_hand_state', + 'setting_topic': '/cb_left_hand_setting_cmd', + 'info_topic': '/cb_left_hand_info', + } + assert {joint.tag_id for joint in profile.joints} == set(range(10, 20)) + assert {name: profile.by_name[name].tag_id for name in ( + 'index_mcp_roll', 'middle_mcp_roll', 'ring_mcp_roll', 'pinky_mcp_roll') + } == {'index_mcp_roll': 11, 'middle_mcp_roll': 12, + 'ring_mcp_roll': 13, 'pinky_mcp_roll': 14} + assert {name: profile.by_name[name].tag_id for name in ( + 'index_pip', 'middle_pip', 'ring_pip', 'pinky_pip') + } == {'index_pip': 15, 'middle_pip': 16, 'ring_pip': 17, 'pinky_pip': 18} + + side_tasks = [task for task in profile.tasks if task.joints[0].endswith(('mcp_pitch', 'pip', 'dip'))] + assert [task.joints[0] for task in side_tasks] == [ + f'{finger}_{joint}' + for finger in ('index', 'middle', 'ring', 'pinky') + for joint in ('mcp_pitch', 'pip', 'dip') + ] + expected_clearances = { + 'index': (), + 'middle': ('index_fold',), + 'ring': ('index_fold', 'middle_fold'), + 'pinky': ('index_fold', 'middle_fold', 'ring_fold'), + } + for task in side_tasks: + finger = task.joints[0].split('_', 1)[0] + assert task.clearances == expected_clearances[finger] + for finger in ('index', 'middle', 'ring'): + assert profile.clearances[f'{finger}_fold'].targets == { + f'{finger}_mcp_pitch': 255, f'{finger}_pip': 255, f'{finger}_dip': 255} + + def test_missing_and_foreign_teaching(): p=load_profile(); teach=Teaching(p,'TEST') assert '全手基础姿态' in teach.missing(p.tasks) @@ -519,7 +561,7 @@ def test_group_holds_and_restart_preserves_completed_results(): before=len(a.sent) for _ in range(5): now+=.1;stamp=round(now*1e9);a.advance(now,stamp) - frame=a.frames()['front'];frame.pop(4) + frame=a.frames()['front'];frame.pop(14) e.observe('front',stamp,frame,now);e.tick(now,stamp) assert len(a.sent)==before e.pause('测试暂停');a.advance(now,stamp);e.resume(now,stamp) @@ -530,8 +572,9 @@ def test_group_holds_and_restart_preserves_completed_results(): assert len(a.sent)==before -def test_complete_simulation_and_replay(tmp_path): - p=load_profile() +@pytest.mark.parametrize('profile_name', ['o30_right', 'o30_left']) +def test_complete_simulation_and_replay(tmp_path, profile_name): + p=load_profile(profile_name) output=simulate(p,Settings(),tmp_path) document=json.loads(output.read_text()) assert len(document['joints'])==20 @@ -570,7 +613,7 @@ def test_noise_and_stale_samples(): if e.state=='SCANNING':break before=e.view_stamps['front'] e.observe('front',before-1,{},now) - assert e.view_stamps['front']==before and ('front',0) in e.latest + assert e.view_stamps['front']==before and ('front',10) in e.latest @pytest.mark.parametrize('scenario', ['moving_front', 'missing_front', 'missing_side', 'moving_active_feedback']) @@ -588,7 +631,7 @@ def test_pinky_pip_uses_its_own_tag_and_feedback_despite_coupled_motion(scenario now = 1.0 adapter.advance(now, round(now * 1e9)) engine.start(teaching, now, round(now * 1e9), ['pinky_pip']) - assert engine.keys == {'pinky_pip': ('side', 5)} + assert engine.keys == {'pinky_pip': ('side', 15)} for step in range(2000): now += .1 stamp = round(now * 1e9) @@ -601,13 +644,13 @@ def test_pinky_pip_uses_its_own_tag_and_feedback_despite_coupled_motion(scenario positions[14] = 0 if step % 2 else 255 adapter.feedback = replace(adapter.feedback, positions=tuple(positions)) if scenario == 'missing_side': - frames['side'].pop(5) + frames['side'].pop(15) if scenario == 'missing_front': frames['front'] = {} else: - frames['front'][1] = (np.asarray(frames['front'][1]) + [step * 10, 0]).tolist() + frames['front'][11] = (np.asarray(frames['front'][11]) + [step * 10, 0]).tolist() # Other side-camera Tags can be absent throughout this single-joint task. - frames['side'] = {tag: points for tag, points in frames['side'].items() if tag == 5} + frames['side'] = {tag: points for tag, points in frames['side'].items() if tag == 15} for view, frame in frames.items(): engine.observe(view, stamp, frame, now) engine.tick(now, stamp) @@ -618,7 +661,7 @@ def test_pinky_pip_uses_its_own_tag_and_feedback_despite_coupled_motion(scenario for event in commands) if scenario in ('missing_side', 'moving_active_feedback'): assert engine.state == 'PAUSED' - assert ('侧面 Tag ID5' if scenario == 'missing_side' + assert ('侧面 Tag ID15' if scenario == 'missing_side' else '关节 pinky_pip:反馈未稳定') in engine.reason assert not engine.samples else: diff --git a/src/linkerhand_range_calibration/test/test_endpoints.py b/src/linkerhand_range_calibration/test/test_endpoints.py index fd2ea92..7feb8ef 100644 --- a/src/linkerhand_range_calibration/test/test_endpoints.py +++ b/src/linkerhand_range_calibration/test/test_endpoints.py @@ -269,7 +269,7 @@ def test_middle_alone_ignores_other_tags_and_extension_requires_group_again(): frame = adapter.frames()['front'] if engine.segment and engine.segment.name == 'deferred_lower': solo_seen = True - frame = {3: frame[3]} + frame = {13: frame[13]} engine.observe('front', stamp, frame, now) engine.tick(now, stamp) assert engine.state != 'PAUSED', engine.reason @@ -285,7 +285,7 @@ def test_middle_alone_ignores_other_tags_and_extension_requires_group_again(): stamp = round(now * 1e9) adapter.advance(now, stamp) frame = adapter.frames()['front'] - frame.pop(1) + frame.pop(11) engine.observe('front', stamp, frame, now) engine.tick(now, stamp) assert engine.state == 'PAUSED' and len(adapter.sent) == before diff --git a/src/linkerhand_range_calibration/test/test_gui.py b/src/linkerhand_range_calibration/test/test_gui.py index 3bb830a..7d5d3f7 100644 --- a/src/linkerhand_range_calibration/test/test_gui.py +++ b/src/linkerhand_range_calibration/test/test_gui.py @@ -219,14 +219,14 @@ node.camera_info['front']=(1624,1240) message=NS(header=NS(stamp=node.get_clock().now().to_msg()),detections=[]) for tag,points in node.adapter.frames()['front'].items(): message.detections.append(NS(id=tag,family='36h11',hamming=0, - decision_margin=0 if tag==2 else 100,corners=[NS(x=x,y=y) for x,y in points])) + decision_margin=0 if tag==12 else 100,corners=[NS(x=x,y=y) for x,y in points])) node._detections('front',message) node.engine._pause_sampling_timeout(time.monotonic()) node._tick();window.refresh() -assert '正面 Tag ID2(ring_mcp_roll):Tag 解码质量不足' in window.message.text() +assert '正面 Tag ID12(ring_mcp_roll):Tag 解码质量不足' in window.message.text() events=[json.loads(line) for line in (node.session.directory/'samples.jsonl').read_text().splitlines()] failure=next(event for event in reversed(events) if event['kind']=='sampling_timeout') -assert any(issue.get('tag_id')==2 and issue['reason']=='Tag 解码质量不足' +assert any(issue.get('tag_id')==12 and issue['reason']=='Tag 解码质量不足' for issue in failure['issues']) # Stream-level failures likewise identify affected tags from the current requirements. @@ -234,8 +234,8 @@ node.demo=False now=time.monotonic() node.camera_received['front']=now node.image_times['front']=now-2 -error=node._vision_error({'ring_mcp_roll':('front',2)},now) -assert '正面 Tag ID2(ring_mcp_roll)' in error and '检测流未收到或断流' in error +error=node._vision_error({'ring_mcp_roll':('front',12)},now) +assert '正面 Tag ID12(ring_mcp_roll)' in error and '检测流未收到或断流' in error node.demo=True window.cancel.click();until(lambda:node.engine.state=='CANCELLED') window.close();node.close_session();node.destroy_node();rclpy.shutdown() diff --git a/src/linkerhand_range_calibration/test/test_reference_platform.py b/src/linkerhand_range_calibration/test/test_reference_platform.py index 1efbf80..80a401d 100644 --- a/src/linkerhand_range_calibration/test/test_reference_platform.py +++ b/src/linkerhand_range_calibration/test/test_reference_platform.py @@ -100,7 +100,7 @@ def test_live_group_uses_its_original_middle_reference(tmp_path): frames = super().frames() command = self.target[3] x = 0 if command == 0 else .65 if command < 5 else -.5 - 1.15 * ((command - 5) // 4) - frames['front'][3] = observation(x + .1 * (-1) ** self.tick)['corners'] + frames['front'][13] = observation(x + .1 * (-1) ** self.tick)['corners'] return frames profile = load_profile() diff --git a/src/linkerhand_range_calibration/test/test_ros_interface.py b/src/linkerhand_range_calibration/test/test_ros_interface.py index 4d1fe46..40e52f7 100644 --- a/src/linkerhand_range_calibration/test/test_ros_interface.py +++ b/src/linkerhand_range_calibration/test/test_ros_interface.py @@ -6,10 +6,13 @@ import subprocess import sys import textwrap +import pytest -def test_ros_adapter_in_isolated_process(): + +@pytest.mark.parametrize('profile_name', ['o30_right', 'o30_left']) +def test_ros_adapter_in_isolated_process(profile_name): program = r''' -import json,time +import json,sys,time import rclpy from rclpy.node import Node from rclpy.executors import SingleThreadedExecutor @@ -21,8 +24,9 @@ from linkerhand_range_calibration.core.engine import Engine from linkerhand_range_calibration.simulation import simulated_teaching rclpy.init() -p=load_profile();sdk=Node('linkerhand_range_sdk');owner=Node('range_test_owner') -for key,value in {'hand_type':'right','hand_joint':'O30','auto_init_pose':False, +p=load_profile(sys.argv[1]);hand_side=p.side +sdk=Node('linkerhand_range_sdk');owner=Node('range_test_owner') +for key,value in {'hand_type':hand_side,'hand_joint':'O30','auto_init_pose':False, 'joint_limit_min':[0]*20,'joint_limit_max':[255]*20,'cmd_timeout':0.0}.items(): sdk.declare_parameter(key,value) positions=[20.0]*20; commands=[]; settings=[] @@ -37,7 +41,7 @@ info_pub=sdk.create_publisher(String,p.sdk['info_topic'],10) def publish(): m=JointState();m.header.stamp=sdk.get_clock().now().to_msg();m.name=[j.sdk_name for j in p.joints];m.position=list(positions) state_pub.publish(m) - info_pub.publish(String(data=json.dumps({'uid':'FAKE_O30_TEST','model':'O30','side':'RIGHT','hand_type':'right', + info_pub.publish(String(data=json.dumps({'uid':'FAKE_O30_TEST','model':'O30','side':hand_side.upper(),'hand_type':hand_side, 'joint_names':m.name,**diagnostics}))) timer=sdk.create_timer(.03,publish) a=O30RosAdapter(owner,p,Settings());ex=SingleThreadedExecutor();ex.add_node(sdk);ex.add_node(owner) @@ -57,7 +61,7 @@ until(lambda:len(commands)==1 and len(settings)==2) assert list(commands[0].position)==target assert len(commands[0].position)==20 and not commands[0].velocity and not commands[0].effort assert {s['setting_cmd'] for s in settings}=={'set_speed','set_max_torque_limits'} -assert all(s['params']['hand_type']=='right' for s in settings) +assert all(s['params']['hand_type']==hand_side for s in settings) assert all(len(s['params'].get('speed',s['params'].get('torque')))==20 for s in settings) # Both SDK stall flags permit starting and advancing an automatic scan. @@ -111,7 +115,7 @@ assert a.control_error(time.monotonic()) is None # SDK startup checks are still enabled; identity and the sole command owner are required. parameters=a.launch_parameters(p,{'sdk':{}}) assert parameters['strict_device_check'] and not parameters['ignore_joint_faults'] -diagnostics['side']='LEFT' +diagnostics['side']='LEFT' if hand_side=='right' else 'RIGHT' until(lambda:'身份不符' in (a.control_error(time.monotonic()) or '')) diagnostics.pop('side') until(lambda:a.control_error(time.monotonic()) is None) @@ -131,7 +135,7 @@ print('isolated ROS adapter passed') ''' env = dict(os.environ, ROS_DOMAIN_ID='198', ROS_LOCALHOST_ONLY='1') env['PYTHONPATH'] = str(Path(__file__).resolve().parents[1]) + os.pathsep + env.get('PYTHONPATH','') - result = subprocess.run([sys.executable,'-c',textwrap.dedent(program)],env=env, + result = subprocess.run([sys.executable,'-c',textwrap.dedent(program),profile_name],env=env, capture_output=True,text=True,timeout=25) assert result.returncode==0, result.stdout+result.stderr diff --git a/src/linkerhand_range_calibration/test/test_sampling_diagnostics.py b/src/linkerhand_range_calibration/test/test_sampling_diagnostics.py index 2653c19..372078a 100644 --- a/src/linkerhand_range_calibration/test/test_sampling_diagnostics.py +++ b/src/linkerhand_range_calibration/test/test_sampling_diagnostics.py @@ -55,9 +55,9 @@ class SamplingRun: @pytest.mark.parametrize('task,noisy,expected', [ - ('thumb_cmc_roll', [0], {'thumb_cmc_roll'}), - ('four_finger_roll', [2], {'ring_mcp_roll'}), - ('four_finger_roll', [2, 4], {'ring_mcp_roll', 'index_mcp_roll'}), + ('thumb_cmc_roll', [10], {'thumb_cmc_roll'}), + ('four_finger_roll', [12], {'ring_mcp_roll'}), + ('four_finger_roll', [12, 14], {'ring_mcp_roll', 'index_mcp_roll'}), ]) def test_current_unstable_metrics_identify_only_affected_tags(task, noisy, expected): run = SamplingRun(task) @@ -90,7 +90,7 @@ def test_four_finger_sampling_advances_with_random_jitter_above_old_limit(): def jitter(frames, rejected): offset = .25 * (-1) ** round(run.now * 20) - for tag in (1, 2, 3, 4): + for tag in (11, 12, 13, 14): frames['front'][tag] = (np.asarray(frames['front'][tag]) + [offset, 0]).tolist() for _ in range(15): @@ -116,7 +116,7 @@ def test_waits_for_transient_vibration_without_advancing_or_relaxing_limits(dead # supplies a known recovery after four seconds to exercise the deadline. if settles_at is None or run.now - started < settles_at: offset = .85 * (-1) ** round(run.now * 20) - for tag in (1, 2, 3, 4): + for tag in (11, 12, 13, 14): frames['front'][tag] = (np.asarray(frames['front'][tag]) + [offset, 0]).tolist() for _ in range(140): @@ -130,7 +130,7 @@ def test_waits_for_transient_vibration_without_advancing_or_relaxing_limits(dead assert deadline < run.now - started <= deadline + .1 timeout = next(event for event in reversed(run.events) if event['kind'] == 'sampling_timeout') assert '共同平移' in run.engine.reason - assert timeout['shared_motion'][0]['tag_ids'] == [1, 2, 3, 4] + assert timeout['shared_motion'][0]['tag_ids'] == [11, 12, 13, 14] else: assert samples and run.engine.state == 'SCANNING' assert settles_at <= run.now - started < settles_at + 1 @@ -144,7 +144,7 @@ def test_recorded_upper_reference_drift_is_still_rejected(): settings = Settings() assert recorded['target'] == [255] * 4 assert settings.point_timeout < recorded['held_seconds'] < settings.reference_timeout - assert {issue['tag_id'] for issue in recorded['issues']} == {2, 3} + assert {issue['tag_id'] for issue in recorded['issues']} == {12, 13} for issue in recorded['issues']: window = ReferenceWindow(settings) for sample in issue['window_samples']: @@ -177,7 +177,7 @@ def test_endpoint_holds_until_stable_or_reference_deadline(direction, settles_at elapsed = run.now - started # Middle/ring drift independently of stable encoder feedback. travel = elapsed if settles_at is None else min(elapsed, settles_at) - for tag, rate in [(3, .3), (2, .6)]: + for tag, rate in [(13, .3), (12, .6)]: frames['front'][tag] = (np.asarray(frames['front'][tag]) + [travel * rate, 0]).tolist() for _ in range(round((settings.reference_timeout + 1) / .05)): @@ -193,7 +193,7 @@ def test_endpoint_holds_until_stable_or_reference_deadline(direction, settles_at assert timeout['kind'] == 'sampling_timeout' assert timeout['timeout_seconds'] == settings.reference_timeout assert settings.reference_timeout < timeout['elapsed_seconds'] <= settings.reference_timeout + .1 - assert {issue['tag_id'] for issue in timeout['issues']} == {2, 3} + assert {issue['tag_id'] for issue in timeout['issues']} == {12, 13} else: assert len(samples) == 1 and run.engine.state == 'SCANNING' assert settles_at <= run.now - started < settles_at + 2 @@ -214,7 +214,7 @@ def test_ordinary_command_does_not_inherit_reference_deadline(): started, sent = run.engine.arrived, len(run.adapter.sent) def creep(frames, rejected): - frames['front'][2] = (np.asarray(frames['front'][2]) + [run.now - started, 0]).tolist() + frames['front'][12] = (np.asarray(frames['front'][12]) + [run.now - started, 0]).tolist() for _ in range(130): run.step(creep) @@ -233,16 +233,16 @@ def test_missing_or_rejected_tag_does_not_blame_other_group_tags(phase, reason): run = SamplingRun('four_finger_roll', phase) def lose_ring(frames, rejected): - frames['front'].pop(2) + frames['front'].pop(12) if reason: - rejected['front'] = {2: reason} + rejected['front'] = {12: reason} event = run.timeout(lose_ring) assert len(event['issues']) == 1 assert event['issues'][0]['joint'] == 'ring_mcp_roll' assert event['issues'][0]['reason'] == (reason or '未检出有效 Tag') - assert '正面 Tag ID2(ring_mcp_roll)' in run.engine.reason - assert 'ID3' not in run.engine.reason and 'ID4' not in run.engine.reason + assert '正面 Tag ID12(ring_mcp_roll)' in run.engine.reason + assert 'ID13' not in run.engine.reason and 'ID14' not in run.engine.reason def test_feedback_instability_names_the_joint_without_accusing_tags(): @@ -265,14 +265,14 @@ def test_stale_tag_reports_correct_camera_and_id(): run = SamplingRun('pinky_pip') event = run.timeout(lambda frames, rejected: frames.pop('side')) assert len(event['issues']) == 1 - assert '侧面 Tag ID5(pinky_pip)' in run.engine.reason + assert '侧面 Tag ID15(pinky_pip)' in run.engine.reason assert '有效观测已过期' in run.engine.reason def test_rejection_is_replaced_only_by_a_new_detector_frame(): run = SamplingRun() stamp = round((run.now + .01) * 1e9) - run.engine.observe('front', stamp, {}, run.now, {0: 'Tag 解码质量不足'}) + run.engine.observe('front', stamp, {}, run.now, {10: 'Tag 解码质量不足'}) run.engine.observe('front', stamp - 1, run.adapter.frames()['front'], run.now) assert run.engine._visibility_issues(run.now)[0]['reason'] == 'Tag 解码质量不足' run.engine.observe('front', stamp + 1, run.adapter.frames()['front'], run.now) diff --git a/src/linkerhand_range_calibration/test/test_settling.py b/src/linkerhand_range_calibration/test/test_settling.py index cd7d562..98cbbbb 100644 --- a/src/linkerhand_range_calibration/test/test_settling.py +++ b/src/linkerhand_range_calibration/test/test_settling.py @@ -98,7 +98,7 @@ class RingRelaxation(FakeAdapter): else: last = np.asarray(self.trace[-1]['observation']['corners']) corners = last + [-(command - 13) * .8, 0] - frames['front'][2] = corners.tolist() + frames['front'][12] = corners.tolist() return frames diff --git a/src/linkerhand_range_calibration/test/test_shared_motion.py b/src/linkerhand_range_calibration/test/test_shared_motion.py index 3d7cca2..29a0564 100644 --- a/src/linkerhand_range_calibration/test/test_shared_motion.py +++ b/src/linkerhand_range_calibration/test/test_shared_motion.py @@ -19,7 +19,7 @@ def test_recorded_shared_vibration_is_explained_but_still_rejected(): issues = recorded_issues() original = copy.deepcopy(issues) report, = shared_translation(issues) - assert report['view'] == 'front' and report['tag_ids'] == [1, 2, 3, 4] + assert report['view'] == 'front' and report['tag_ids'] == [11, 12, 13, 14] assert report['shared_fraction'] > .99 assert 1.9 < report['horizontal_span_px'] < 2.1 assert '共同平移' in report['reason'] diff --git a/src/linkerhand_range_calibration/test/test_transient_motion.py b/src/linkerhand_range_calibration/test/test_transient_motion.py index 1a02ad3..a8ddccb 100644 --- a/src/linkerhand_range_calibration/test/test_transient_motion.py +++ b/src/linkerhand_range_calibration/test/test_transient_motion.py @@ -110,7 +110,7 @@ def test_live_scan_remembers_motion_before_settling_and_replays(tmp_path, pulse_ elapsed_frames = round((self.clock - self.pulse_start) * 30) moved = (self.target[10] == 253 or (self.target[10] == 254 and 1 <= elapsed_frames <= pulse_frames)) - frames['side'][5] = (BASE + [2 if moved else 0, 0]).tolist() + frames['side'][15] = (BASE + [2 if moved else 0, 0]).tolist() return frames profile, settings = load_profile(), Settings(rate=rate)