From a8ddcc629646d325bbb8139dac1db76869bbbc32 Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Fri, 4 Sep 2026 18:35:04 +0800 Subject: [PATCH] =?UTF-8?q?O6gui=E9=A2=84=E8=AE=BE=E5=8A=A8=E4=BD=9C?= =?UTF-8?q?=E4=BF=AE=E6=94=B9=E3=80=81O12=E6=A0=87=E5=AE=9A=E5=88=9D?= =?UTF-8?q?=E5=A7=8B=E5=8C=96=E6=8F=90=E4=BA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../gui_control/config/constants.py | 21 +- src/gui_control/gui_control/gui_control.py | 16 +- src/gui_control/test/test_o6_config.py | 18 + src/linkerhand_calibration/README.md | 30 ++ .../config/o12_right_16_tags.yaml | 41 ++ .../config/o12_right_product.yaml | 43 ++ .../config/o12_three_camera_calibration.yaml | 50 +++ .../launch/three_camera_calibration.launch.py | 58 ++- .../linkerhand_calibration/acquisition.py | 7 +- .../calibrated_joint_state_bridge.py | 51 ++- .../compat/config_v1.py | 4 +- .../core/domain/profile.py | 95 ++++- .../core/domain/sample.py | 14 +- .../linkerhand_calibration/models/l6/node.py | 357 ++++++++++++----- .../models/l6/runner.py | 34 +- .../models/o12/__init__.py | 11 + .../models/o12/artifacts.py | 313 +++++++++++++++ .../models/o12/fitting.py | 181 +++++++++ .../models/o12/motion.py | 76 ++++ .../linkerhand_calibration/models/o12/node.py | 369 ++++++++++++++++++ .../models/o12/pipeline.py | 145 +++++++ .../models/o12/profile.py | 344 ++++++++++++++++ .../models/o12/runner.py | 256 ++++++++++++ .../linkerhand_calibration/models/o12/urdf.py | 140 +++++++ .../linkerhand_calibration/models/registry.py | 2 + .../linkerhand_calibration/product.py | 46 ++- .../test/test_architecture.py | 2 +- .../test/test_o12_right_profile.py | 284 ++++++++++++++ 28 files changed, 2862 insertions(+), 146 deletions(-) create mode 100644 src/gui_control/test/test_o6_config.py create mode 100644 src/linkerhand_calibration/config/o12_right_16_tags.yaml create mode 100644 src/linkerhand_calibration/config/o12_right_product.yaml create mode 100644 src/linkerhand_calibration/config/o12_three_camera_calibration.yaml create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/node.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py create mode 100644 src/linkerhand_calibration/test/test_o12_right_profile.py diff --git a/src/gui_control/gui_control/config/constants.py b/src/gui_control/gui_control/config/constants.py index bb55b38..ece3355 100644 --- a/src/gui_control/gui_control/config/constants.py +++ b/src/gui_control/gui_control/config/constants.py @@ -9,11 +9,21 @@ 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 + # ------------------------------------------------------------------ # 常量字典（仅构建一次） # ------------------------------------------------------------------ @@ -228,10 +238,17 @@ _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_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"], diff --git a/src/gui_control/gui_control/gui_control.py b/src/gui_control/gui_control/gui_control.py index fc169b4..447bdcc 100644 --- a/src/gui_control/gui_control/gui_control.py +++ b/src/gui_control/gui_control/gui_control.py @@ -363,6 +363,7 @@ class HandControlGUI(QWidget): 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. @@ -636,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( @@ -889,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 @@ -912,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) @@ -949,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} diff --git a/src/gui_control/test/test_o6_config.py b/src/gui_control/test/test_o6_config.py new file mode 100644 index 0000000..74361a0 --- /dev/null +++ b/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, + ] diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index f447e22..a6950f6 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -1,5 +1,35 @@ # LinkerHand 专业标定包 +## O12 右手 16-Tag 完整标定 + +O12 使用 vendor `omnihand_pro_2025_node` 的标准弧度接口，固定订阅 +`/o12/right/joint_states`、发布 `/o12/right/joint_cmd`。标定程序不会使用 +0–2000 混合控制接口，也不会按可能错位的 `JointState.name[]` 重排数据；12 路 +`position[]` 顺序及 `thumb_roll → thumb_cmc_roll`、 +`thumb_abad → thumb_cmc_yaw` 映射由产品 Profile 固定。 + +把 [o12_right_product.yaml](config/o12_right_product.yaml) 中的 +`serial_number` 改成实物串号后，只需运行： + +```bash +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/o12_right_product.yaml +``` + +runner 会自动加载仓库内 vendor Jazzy overlay，启动 HCAN device 0/channel 0 +节点、三相机、AprilTag 与标定节点；READY 后自动开始。启动前会确认 12 路 +POSITION 模式、错误码、温度和反馈，并以不超过 3° 的低速点动执行固定通道预检。 +正式扫描以 20 Hz 发布端点速度为零的弧度余弦轨迹；中止、堵转或质量失败时保持 +当前位置。结果发布到 `calibration_output//latest_passed`，其中 JSON +为 schema v7 弧度 knots，不生成 256 点 u8 表。 + +只验证配置、16 张 16 mm Tag、相机/外参、源 URDF 和 SDK 配置哈希而不运动： + +```bash +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/o12_right_product.yaml --validate-only +``` + ## O6 右手局部标定（o6_right_8/v1） O6 使用与 L6 相同的三机位八 Tag 观测拓扑，但保留 O6 自己的六通道协议与 diff --git a/src/linkerhand_calibration/config/o12_right_16_tags.yaml b/src/linkerhand_calibration/config/o12_right_16_tags.yaml new file mode 100644 index 0000000..8b19198 --- /dev/null +++ b/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] diff --git a/src/linkerhand_calibration/config/o12_right_product.yaml b/src/linkerhand_calibration/config/o12_right_product.yaml new file mode 100644 index 0000000..66ebbf7 --- /dev/null +++ b/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 diff --git a/src/linkerhand_calibration/config/o12_three_camera_calibration.yaml b/src/linkerhand_calibration/config/o12_three_camera_calibration.yaml new file mode 100644 index 0000000..399745c --- /dev/null +++ b/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 diff --git a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py index 3fb36dd..579032b 100644 --- a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py +++ b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py @@ -73,6 +73,8 @@ def _launch_stack(context): / ( "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" @@ -85,8 +87,15 @@ def _launch_stack(context): if not tag_config.is_file(): raise RuntimeError(f"tag config does not exist: {tag_config}") topic_prefix = f"/{model.lower()}" - command_topic = f"{topic_prefix}/cb_{hand_type}_hand_control_cmd" - state_topic = f"{topic_prefix}/cb_{hand_type}_hand_state" + 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 = ( @@ -250,14 +259,24 @@ def _launch_stack(context): output="screen", emulate_tty=True, ) - sdk = Node( - package="linker_hand_ros2_sdk", - executable="linker_hand_sdk", - name="linker_hand_sdk", - output="screen", - condition=IfCondition(LaunchConfiguration("start_sdk")), - parameters=[ - { + 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"), @@ -281,8 +300,8 @@ def _launch_stack(context): "defer_state_reads_while_commanding": False, "repeat_position_commands": False, "is_touch": False, - } - ], + }], + ) ) calibration = Node( package="linkerhand_calibration", @@ -331,6 +350,9 @@ def _launch_stack(context): "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" ), @@ -385,6 +407,14 @@ def _launch_stack(context): 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", @@ -509,6 +539,10 @@ def generate_launch_description() -> LaunchDescription: 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="" ), diff --git a/src/linkerhand_calibration/linkerhand_calibration/acquisition.py b/src/linkerhand_calibration/linkerhand_calibration/acquisition.py index 1cc0b8b..188a9bc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/acquisition.py +++ b/src/linkerhand_calibration/linkerhand_calibration/acquisition.py @@ -167,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.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py b/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py index 13ad306..be87ad9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py +++ b/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py @@ -17,6 +17,7 @@ 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 @@ -27,6 +28,7 @@ from .full_hand import ( 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 @@ -88,7 +90,9 @@ class CalibratedCommandMapper: self, payload: Mapping[str, Any], *, expected_side: str | None = None ) -> None: schema_version = int(payload["schema_version"]) - if schema_version == 6: + if schema_version == 7: + validate_o12_runtime_payload(payload) + elif schema_version == 6: validate_schema_v6_runtime_payload(payload) else: validate_compact_payload(payload) @@ -103,7 +107,7 @@ class CalibratedCommandMapper: raise ValueError("calibration quality.passed must be true") layout_id = ( str(payload["layout_id"]) - if schema_version == 6 + if schema_version in {6, 7} else infer_compact_payload_layout(payload) ) self.side = side @@ -119,9 +123,9 @@ class CalibratedCommandMapper: "command_u8" if schema_version == 4 else "", ) ) - if self.input_domain not in {"command_u8", "feedback_u8"}: + if self.input_domain not in {"command_u8", "feedback_u8", "feedback_rad"}: raise ValueError("calibration curve_input_domain is invalid") - if schema_version == 6: + 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 = { @@ -134,6 +138,9 @@ class CalibratedCommandMapper: 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 @@ -143,6 +150,7 @@ class CalibratedCommandMapper: for name in self.urdf_joint_names } self.feedback_name_aliases = {} + self.feedback_by_index = False self._curves = { name: tuple( float(value) @@ -170,7 +178,19 @@ class CalibratedCommandMapper: } self._previous_by_motor: dict[int, float] = {} self._direction_by_motor: dict[int, str] = {} - self.direction_deadband_u8 = 0.5 + 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: @@ -183,7 +203,7 @@ class CalibratedCommandMapper: self, positions: Sequence[float], names: Sequence[str] = () ) -> tuple[float, ...]: values = tuple(float(value) for value in positions) - if names: + if names and not self.feedback_by_index: if len(names) != len(values): raise ValueError( "JointState names and positions must have equal length" @@ -208,7 +228,10 @@ class CalibratedCommandMapper: f"{len(self.command_names)} positions" ) command = values - indices = tuple(self._command_index(value) for value in command) + 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) @@ -223,6 +246,13 @@ class CalibratedCommandMapper: 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" @@ -230,7 +260,10 @@ class CalibratedCommandMapper: if direction == "decreasing" else self._curves ) - result.append(curves[name][indices[motor]]) + 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] @@ -259,6 +292,8 @@ def default_input_topic( 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") diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py b/src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py index a54cffc..7699908 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py @@ -9,7 +9,7 @@ from ..core import ProfileKey def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey: version = int(raw.get("schema_version", -1)) - if version == 2: + if version in {2, 3}: key = ProfileKey.parse(str(raw.get("profile_id", ""))) for field, actual in ( ("model", key.model), @@ -21,7 +21,7 @@ def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey: raise ValueError(f"{field} differs from profile_id") return key if version != 1: - raise ValueError("product config schema_version must be 1 or 2") + 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() diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py index 84d7ea9..42ff94c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py @@ -3,6 +3,7 @@ from __future__ import annotations from dataclasses import dataclass, field +import math from pathlib import PurePath from typing import Mapping @@ -59,11 +60,54 @@ class CommandLayout: # 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: @@ -105,12 +149,24 @@ class TaskSpec: view: str command_index: int joints: tuple[str, ...] - auxiliary_commands: tuple[tuple[int, int], ...] = () + 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) @@ -232,12 +288,30 @@ def validate_profile(profile: CalibrationProfile) -> None: """Hard-check all cross-policy references before hardware is enabled.""" errors: list[str] = [] command = profile.command - if not command.names or len(command.names) != len(command.baseline_u8): + 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 any(value < 0 or value > 255 for value in command.baseline_u8): + 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") @@ -291,13 +365,24 @@ def validate_profile(profile: CalibrationProfile) -> None: 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") - if not 0 <= task.start_u8 <= 255 or not 0 <= task.end_u8 <= 255: + 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_u8 == task.end_u8: + 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}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py index a1b7d04..fa00099 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py @@ -3,6 +3,7 @@ from __future__ import annotations from dataclasses import dataclass, field +import math from typing import Any, Mapping @@ -17,11 +18,22 @@ class SampleRecord: 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 or not 0 <= self.command_u8 <= 255: + 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") diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py index b2b60b7..6db9eef 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py @@ -39,11 +39,12 @@ class MotionStep: phase: str task_key: str | None command_index: int | None - target_u8: int - speed_u8: int + target_u8: float + speed_u8: float cycle: int | None = None direction: str | None = None attempt: int = 1 + target_command: tuple[float, ...] | None = None @property def recording(self) -> bool: @@ -67,7 +68,11 @@ class L6ThreeCameraCalibrationNode(Node): self.profile = build_typed_profile() if profile is None else profile self.model_name = self.profile.key.model self.command_names = tuple(self.profile.command.names) - self.baseline_command = tuple(self.profile.command.baseline_u8) + self.command_count = self.profile.command.command_count + self.command_unit = self.profile.command.unit + self.baseline_command = tuple(self.profile.command.baseline_values) + self.command_lower = tuple(self.profile.command.minimum_values) + self.command_upper = tuple(self.profile.command.maximum_values) self.sample_kind = str(sample_kind) self.sweep_quality_kind = f"{self.model_name.lower()}_sweep_observation_quality" self.finalize_session = finalizer or finalize_l6_session @@ -80,10 +85,10 @@ class L6ThreeCameraCalibrationNode(Node): self.raw_path, { "kind": "session_start", - "sample_schema_version": 6, + "sample_schema_version": self.profile.artifacts.output_schema_version, "profile_id": self.profile.key.profile_id, "serial_number": self.serial_number, - "curve_input_domain": "feedback_u8", + "curve_input_domain": f"feedback_{self.command_unit}", }, ) @@ -159,6 +164,7 @@ class L6ThreeCameraCalibrationNode(Node): self.step_last_command_u8: tuple[int, ...] | None = None self.step_trajectory_phase = 0.0 self.step_trajectory_duration_seconds = 0.0 + self.step_moving_indices: frozenset[int] = frozenset() self.step_valid_frames = 0 self.step_total_frames = 0 self.step_required_roles: tuple[str, ...] = () @@ -175,7 +181,11 @@ class L6ThreeCameraCalibrationNode(Node): self.started = False self.commanded_speed: int | None = None self.retry_counts: dict[tuple[str, int, str], int] = {} - self.create_timer(0.01, self._tick) + self.create_timer( + 1.0 / float(self.command_rate_hz) + if self.command_unit == "rad" else 0.01, + self._tick, + ) self.create_timer(0.5, self._publish_status) def _declare_parameters(self) -> None: @@ -191,6 +201,7 @@ class L6ThreeCameraCalibrationNode(Node): "camera_extrinsics_expected_sha256": "", "calibration_config_expected_sha256": "", "tag_config_expected_sha256": "", + "sdk_config_expected_sha256": "", "command_topic": f"/{model}/cb_right_hand_control_cmd", "state_topic": f"/{model}/cb_right_hand_state", "setting_topic": f"/{model}/cb_hand_setting_cmd", @@ -245,6 +256,10 @@ class L6ThreeCameraCalibrationNode(Node): "pnp_maximum_translation_jump_m": 0.04, "pnp_maximum_tag_tilt_deg": 75.0, "pnp_tracker_reset_seconds": 5.0, + "command_rate_hz": float(speed_parameters.get("command_rate_hz", 100.0)), + "endpoint_tolerance_rad": 0.01, + "non_target_motion_tolerance_rad": 0.015, + "minimum_state_span_fraction": 0.90, } for name, default in defaults.items(): self.declare_parameter(name, default) @@ -280,8 +295,16 @@ class L6ThreeCameraCalibrationNode(Node): ), "tag_config_sha256": str(value("tag_config_expected_sha256")), } - if any(len(item) != 64 for item in self.protected_inputs.values()): - raise ValueError("all four immutable input SHA-256 values are required") + if "sdk_config_sha256" in self.profile.artifacts.protected_input_fields: + self.protected_inputs["sdk_config_sha256"] = str( + value("sdk_config_expected_sha256") + ) + if ( + set(self.protected_inputs) + != self.profile.artifacts.protected_input_fields + or any(len(item) != 64 for item in self.protected_inputs.values()) + ): + raise ValueError("all profile-protected SHA-256 values are required") self.command_topic = str(value("command_topic")) self.state_topic = str(value("state_topic")) self.setting_topic = str(value("setting_topic")) @@ -311,16 +334,31 @@ class L6ThreeCameraCalibrationNode(Node): "pnp_maximum_reprojection_error_px", "pnp_reprojection_tie_px", "pnp_maximum_pose_jump_deg", "pnp_maximum_translation_jump_m", "pnp_maximum_tag_tilt_deg", "pnp_tracker_reset_seconds", + "command_rate_hz", "endpoint_tolerance_rad", + "non_target_motion_tolerance_rad", "minimum_state_span_fraction", ): setattr(self, name, value(name)) self.maximum_state_image_skew_ns = int( float(self.maximum_state_image_skew_ms) * 1_000_000 ) - for name in ( - "baseline_speed_u8", "preflight_speed_u8", "formal_speed_u8" - ): - if not 1 <= int(getattr(self, name)) <= 255: - raise ValueError(f"{name} must be in [1, 255]") + if self.command_unit == "u8": + for name in ( + "baseline_speed_u8", "preflight_speed_u8", "formal_speed_u8" + ): + if not 1 <= int(getattr(self, name)) <= 255: + raise ValueError(f"{name} must be in [1, 255]") + else: + self.endpoint_tolerance_u8 = float(self.endpoint_tolerance_rad) + self.non_target_motion_tolerance_u8 = float( + self.non_target_motion_tolerance_rad + ) + self.minimum_state_span_u8 = float(self.minimum_state_span_fraction) + if not 1.0 <= float(self.command_rate_hz) <= 100.0: + raise ValueError("command_rate_hz must be in [1, 100]") + if not 0.0 < float(self.endpoint_tolerance_u8) <= 0.1: + raise ValueError("endpoint_tolerance_rad must be in (0, 0.1]") + if not 0.0 < float(self.minimum_state_span_u8) <= 1.0: + raise ValueError("minimum_state_span_fraction must be in (0, 1]") if not 0.0 <= float(self.speed_settle_seconds) <= 5.0: raise ValueError("speed_settle_seconds must be in [0, 5]") if not 2.0 <= float(self.command_trajectory_full_range_seconds) <= 30.0: @@ -333,27 +371,38 @@ class L6ThreeCameraCalibrationNode(Node): raise ValueError("minimum_joint_frame_rate must be in (0, 1]") def _build_steps(self) -> list[MotionStep]: + command_unit = getattr(self, "command_unit", "u8") steps = [ MotionStep( "baseline", None, None, 255, int(self.baseline_speed_u8) ) ] for task in self.profile.motion.tasks: - midpoint = int(round(0.5 * (task.start_u8 + task.end_u8))) - for target in (task.start_u8, midpoint, task.end_u8, task.start_u8): + midpoint = 0.5 * (task.start_value + task.end_value) + preflight_speed = ( + task.preflight_speed + if command_unit == "rad" + else task.preflight_speed_u8 or self.preflight_speed_u8 + ) + formal_speed = ( + task.formal_speed + if command_unit == "rad" + else self.formal_speed_u8 + ) + for target in (task.start_value, midpoint, task.end_value, task.start_value): steps.append( MotionStep( "preflight", task.key, task.command_index, target, - int(task.preflight_speed_u8 or self.preflight_speed_u8), + float(preflight_speed), ) ) for cycle in (0, 1, 2, 3): steps.extend( [ - MotionStep("prepare", task.key, task.command_index, task.start_u8, int(self.formal_speed_u8), cycle), - MotionStep("sweep", task.key, task.command_index, task.end_u8, int(self.formal_speed_u8), cycle, "decreasing"), - MotionStep("prepare", task.key, task.command_index, task.end_u8, int(self.formal_speed_u8), cycle), - MotionStep("sweep", task.key, task.command_index, task.start_u8, int(self.formal_speed_u8), cycle, "increasing"), + MotionStep("prepare", task.key, task.command_index, task.start_value, float(formal_speed), cycle), + MotionStep("sweep", task.key, task.command_index, task.end_value, float(formal_speed), cycle, "decreasing"), + MotionStep("prepare", task.key, task.command_index, task.end_value, float(formal_speed), cycle), + MotionStep("sweep", task.key, task.command_index, task.start_value, float(formal_speed), cycle, "increasing"), ] ) return steps @@ -378,9 +427,18 @@ class L6ThreeCameraCalibrationNode(Node): return response def _abort(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: - self._publish_command(list(self.baseline_command)) + hold = ( + list(self.latest_state_u8) + if self.command_unit == "rad" and len(self.latest_state_u8) == self.command_count + else list(self.baseline_command) + ) + self._publish_command(hold) self.state = "ABORTED" - self.reason = "operator_abort_returning_to_open_baseline" + self.reason = ( + "operator_abort_holding_current_position" + if self.command_unit == "rad" + else "operator_abort_returning_to_open_baseline" + ) response.success = True response.message = self.reason return response @@ -392,9 +450,9 @@ class L6ThreeCameraCalibrationNode(Node): self.image_sizes[view] = (int(message.width), int(message.height)) def _state_callback(self, message: JointState) -> None: - if len(message.position) != 6: + if len(message.position) != self.command_count: return - if message.name: + if message.name and not self.profile.command.feedback_by_index: by_name = dict(zip((str(name) for name in message.name), message.position)) for alias, canonical in self.profile.command.feedback_name_aliases.items(): if alias in by_name and canonical not in by_name: @@ -406,6 +464,13 @@ class L6ThreeCameraCalibrationNode(Node): state = tuple(float(value) for value in message.position) if not all(math.isfinite(value) for value in state): return + if any( + value < self.command_lower[index] - 1.0e-6 + or value > self.command_upper[index] + 1.0e-6 + for index, value in enumerate(state) + ): + self._pause("feedback_outside_registered_command_domain") + return stamp = _stamp_ns(message.header.stamp) if stamp <= 0: stamp = int(self.get_clock().now().nanoseconds) @@ -416,9 +481,15 @@ class L6ThreeCameraCalibrationNode(Node): step = self._current_step() if step is not None and step.task_key is not None: for index, actual in enumerate(state): - if index == step.command_index: + if index == step.command_index or index in self.step_moving_indices: continue - if abs(actual - self.baseline_command[index]) > float(self.non_target_motion_tolerance_u8): + expected = ( + self.step_last_command_u8[index] + if self.step_last_command_u8 is not None + else self.baseline_command[index] + ) + tolerance = float(self.non_target_motion_tolerance_u8) + if abs(actual - expected) > tolerance: self._pause( f"non_target_motor_moved:channel={index}:feedback={actual:.2f}" ) @@ -666,6 +737,9 @@ class L6ThreeCameraCalibrationNode(Node): if recording_this_view: self.step_pnp_valid_frames += 1 self.last_view_valid_at[view] = time.monotonic() + preflight_hook = getattr(self, "_observe_preflight_pose", None) + if preflight_hook is not None and step is not None and step.phase == "preflight": + preflight_hook(view, selected, step) if step is None or not step.recording or self._task(step.task_key).view != view: return matched = interpolate_state_u8( @@ -680,9 +754,12 @@ class L6ThreeCameraCalibrationNode(Node): self.step_state_sync_frames += 1 task = self._task(step.task_key) feedback = float(state_u8[task.command_index]) - if not 0.0 <= feedback <= 255.0: - self._count_step_rejection("feedback:outside_u8_domain") + lower = self.command_lower[task.command_index] + upper = self.command_upper[task.command_index] + if not lower <= feedback <= upper: + self._count_step_rejection("feedback:outside_registered_domain") return + progress = self.profile.command.normalize(task.command_index, feedback) for joint in task.joints: measurement = self.profile.measurement.measurements[joint] parent = selected[str(measurement.parent_role)] @@ -718,10 +795,6 @@ class L6ThreeCameraCalibrationNode(Node): # Keep the established schema contract: this field labels the # requested sweep endpoint. The live shaped set-point is a # separate diagnostic and is never used as the fit domain. - "requested_command_u8": int(step.target_u8), - "trajectory_command_u8": round(self.step_requested_u8, 6), - "feedback_u8": round(feedback, 6), - "state_u8": [round(float(value), 6) for value in state_u8], "state_image_sync_error_ms": round(abs(skew_ns) / 1_000_000.0, 6), "relative_quaternion_xyzw": [ float(value) for value in relative.as_quat() @@ -749,6 +822,22 @@ class L6ThreeCameraCalibrationNode(Node): ), "image_stamp_ns": stamp, } + if self.command_unit == "u8": + record.update({ + "requested_command_u8": int(step.target_u8), + "trajectory_command_u8": round(self.step_requested_u8, 6), + "feedback_u8": round(feedback, 6), + "state_u8": [round(float(value), 6) for value in state_u8], + }) + else: + record.update({ + "requested_command_rad": round(float(step.target_u8), 9), + "trajectory_command_rad": round(self.step_requested_u8, 9), + "command_rad": round(self.step_requested_u8, 9), + "feedback_rad": round(feedback, 9), + "state_rad": [round(float(value), 9) for value in state_u8], + "progress_01": round(progress, 9), + }) self.raw_records.append(record) append_jsonl(self.raw_path, record) self.step_valid_frames += 1 @@ -760,7 +849,7 @@ class L6ThreeCameraCalibrationNode(Node): return 0.0 if elapsed <= 0 else (len(self.state_receive_times) - 1) / elapsed def _publish_torque(self) -> None: - if not self.commands_enabled: + if not self.commands_enabled or self.command_unit != "u8": return message = String() message.data = json.dumps( @@ -771,7 +860,10 @@ class L6ThreeCameraCalibrationNode(Node): ) self.setting_publisher.publish(message) - def _publish_speed(self, speed: int) -> None: + def _publish_speed(self, speed: float) -> None: + if self.command_unit != "u8": + self.commanded_speed = float(speed) + return if not self.commands_enabled or self.commanded_speed == int(speed): return message = String() @@ -784,47 +876,70 @@ class L6ThreeCameraCalibrationNode(Node): self.setting_publisher.publish(message) self.commanded_speed = int(speed) - def _publish_command(self, values: list[int]) -> None: + def _publish_command(self, values: list[float]) -> None: if not self.commands_enabled: return + if len(values) != self.command_count: + raise ValueError("command has the wrong channel count") + bounded = [ + float(np.clip(value, self.command_lower[index], self.command_upper[index])) + for index, value in enumerate(values) + ] message = JointState() message.header.stamp = self.get_clock().now().to_msg() - message.name = list(self.command_names) - message.position = [float(value) for value in values] + message.name = ( + [] if self.profile.command.feedback_by_index else list(self.command_names) + ) + message.position = bounded self.command_publisher.publish(message) + def _target_command(self, step: MotionStep) -> tuple[float, ...]: + if step.target_command is not None: + if len(step.target_command) != self.command_count: + raise ValueError("motion-step target has the wrong channel count") + return tuple(float(value) for value in step.target_command) + target = [float(value) for value in self.baseline_command] + if step.task_key is not None: + for index, value in self._task(step.task_key).auxiliary_commands: + target[int(index)] = float(value) + if step.command_index is not None: + target[step.command_index] = float(step.target_u8) + return tuple(target) + def _begin_step(self, step: MotionStep) -> None: now = time.monotonic() - if self.commanded_speed != int(step.speed_u8): + if self.commanded_speed != step.speed_u8: self._publish_speed(step.speed_u8) self.step_speed_ready_at = now + float(self.speed_settle_seconds) self.reason = f"setting_speed:{step.speed_u8}" return if now < self.step_speed_ready_at: return - if len(self.latest_state_u8) != 6: + if len(self.latest_state_u8) != self.command_count: return self.step_start_state_u8 = tuple(float(value) for value in self.latest_state_u8) self.step_started_at = now self.step_last_progress_at = now self.step_last_feedback = ( float(self.latest_state_u8[step.command_index]) - if step.command_index is not None and len(self.latest_state_u8) == 6 + if step.command_index is not None and len(self.latest_state_u8) == self.command_count else ( float(np.mean(self.latest_state_u8)) - if len(self.latest_state_u8) == 6 + if len(self.latest_state_u8) == self.command_count else float("nan") ) ) self.step_initial_feedback = self.step_last_feedback self.step_requested_u8 = self.step_initial_feedback - target = [float(value) for value in self.baseline_command] - if step.command_index is not None: - target[step.command_index] = float(step.target_u8) + target = list(self._target_command(step)) errors = [ abs(end - start) for start, end in zip(self.step_start_state_u8, target) ] + self.step_moving_indices = frozenset( + index for index, error in enumerate(errors) + if error > float(self.non_target_motion_tolerance_u8) + ) self.step_last_distance_u8 = ( float(sum(errors)) if step.command_index is None @@ -832,14 +947,16 @@ class L6ThreeCameraCalibrationNode(Node): ) self.step_initial_distance_u8 = self.step_last_distance_u8 maximum_distance = max(errors) - _, _, self.step_trajectory_duration_seconds = ( - cosine_position_trajectory_u8( - 0.0, - maximum_distance, - 0.0, + if self.command_unit == "rad": + self.step_trajectory_duration_seconds = ( + 0.0 if maximum_distance <= 0.0 else + math.pi * maximum_distance / (2.0 * float(step.speed_u8)) + ) + else: + _, _, self.step_trajectory_duration_seconds = cosine_position_trajectory_u8( + 0.0, maximum_distance, 0.0, float(self.command_trajectory_full_range_seconds), ) - ) self.step_trajectory_phase = 0.0 self.step_last_command_u8 = None self.step_hold_since = None @@ -855,32 +972,34 @@ class L6ThreeCameraCalibrationNode(Node): ) def _advance_step_trajectory(self, step: MotionStep, now: float) -> None: - baseline = tuple(getattr(self, "baseline_command", (255,) * 6)) + if hasattr(self, "_target_command"): + target_values = self._target_command(step) + else: + # Compatibility for the isolated legacy trajectory unit harness. + target = [255.0] * len(self.step_start_state_u8) + if step.command_index is not None: + target[step.command_index] = float(step.target_u8) + target_values = tuple(target) elapsed = max(0.0, float(now) - self.step_started_at) - _, phase, _ = cosine_position_trajectory_u8( - 0.0, - max( - ( - abs(float(baseline[index]) - value) - if step.command_index is None - else abs(float(step.target_u8) - value) - if index == step.command_index - else abs(float(baseline[index]) - value) - ) - for index, value in enumerate(self.step_start_state_u8) - ), - elapsed, - float(self.command_trajectory_full_range_seconds), + duration = float( + getattr( + self, + "step_trajectory_duration_seconds", + self.command_trajectory_full_range_seconds, + ) ) + phase = 1.0 if duration <= 0.0 else min(1.0, elapsed / duration) blend = 0.5 - 0.5 * math.cos(math.pi * phase) - targets = [float(value) for value in baseline] - if step.command_index is not None: - targets[step.command_index] = float(step.target_u8) values = [ start + (target - start) * blend - for start, target in zip(self.step_start_state_u8, targets) + for start, target in zip(self.step_start_state_u8, target_values) ] - command = tuple(int(np.clip(round(value), 0, 255)) for value in values) + command = tuple( + int(np.clip(round(value), 0, 255)) + if getattr(self, "command_unit", "u8") == "u8" + else float(np.clip(value, self.command_lower[index], self.command_upper[index])) + for index, value in enumerate(values) + ) self.step_trajectory_phase = phase self.step_requested_u8 = ( float(np.mean(values)) @@ -901,20 +1020,41 @@ class L6ThreeCameraCalibrationNode(Node): and int(row["attempt"]) == step.attempt and row["joint"] == task.joints[0] ] - feedback = np.asarray([float(row["feedback_u8"]) for row in rows]) - bins = sorted(set(int(round(value)) for value in feedback)) - gap = max((right - left for left, right in zip(bins, bins[1:])), default=256) + command_unit = getattr(self, "command_unit", "u8") + feedback_field = "feedback_u8" if command_unit == "u8" else "feedback_rad" + feedback = np.asarray([float(row[feedback_field]) for row in rows]) + if command_unit == "u8": + bins = sorted(set(int(round(value)) for value in feedback)) + span = float(np.ptp(feedback)) if feedback.size else 0.0 + required_span = float(self.minimum_state_span_u8) + gap = max((right - left for left, right in zip(bins, bins[1:])), default=256) + maximum_gap = float(self.maximum_bin_gap) + else: + normalized = sorted( + set( + min(31, max(0, int(self.profile.command.normalize(task.command_index, value) * 32.0))) + for value in feedback + ) + ) + bins = normalized + span = ( + float(np.ptp([self.profile.command.normalize(task.command_index, value) for value in feedback])) + if feedback.size else 0.0 + ) + required_span = float(self.minimum_state_span_u8) + gap = max((right - left for left, right in zip(bins, bins[1:])), default=32) + maximum_gap = float(self.maximum_bin_gap) observation = self._step_observation_metrics() detection_rate = float(observation["tag_detection_rate"]) joint_frame_rate = float(observation["joint_frame_rate"]) failures = [] if len(rows) < int(self.minimum_sweep_frames): failures.append(f"frames={len(rows)}") - if feedback.size == 0 or float(np.ptp(feedback)) < float(self.minimum_state_span_u8): + if feedback.size == 0 or span < required_span: failures.append("feedback_span") if len(bins) < int(self.minimum_sweep_bins): failures.append(f"bins={len(bins)}") - if gap > int(self.maximum_bin_gap): + if gap > maximum_gap: failures.append(f"maximum_gap={gap}") if detection_rate < float(self.minimum_detection_rate): role_rates = observation["tag_detection_rate_by_role"] @@ -964,10 +1104,11 @@ class L6ThreeCameraCalibrationNode(Node): attempt = retries + 2 self.retry_counts[key] = retries + 1 task = self._task(str(step.task_key)) - start = task.start_u8 if step.direction == "decreasing" else task.end_u8 + start = task.start_value if step.direction == "decreasing" else task.end_value + retry_speed = float(step.speed_u8) * (0.75 if attempt == 2 else 0.5 / 0.75) replacement = [ - MotionStep("retry_prepare", step.task_key, step.command_index, start, step.speed_u8, step.cycle, attempt=attempt), - MotionStep("sweep", step.task_key, step.command_index, step.target_u8, step.speed_u8, step.cycle, step.direction, attempt), + MotionStep("retry_prepare", step.task_key, step.command_index, start, retry_speed, step.cycle, attempt=attempt), + MotionStep("sweep", step.task_key, step.command_index, step.target_u8, retry_speed, step.cycle, step.direction, attempt), ] self.steps[self.step_index + 1:self.step_index + 1] = replacement append_jsonl( @@ -1013,7 +1154,7 @@ class L6ThreeCameraCalibrationNode(Node): ) return if not self.started: - if len(self.latest_state_u8) == 6 and set(self.camera_matrices) == set( + if len(self.latest_state_u8) == self.command_count and set(self.camera_matrices) == set( self.profile.vision.view_names ): self.state = "READY" @@ -1031,11 +1172,9 @@ class L6ThreeCameraCalibrationNode(Node): if now - self.step_started_at > float(timeout): self._pause(f"motion_timeout:{self.reason}") return - if len(self.latest_state_u8) != 6: + if len(self.latest_state_u8) != self.command_count: return - target_state = [float(value) for value in self.baseline_command] - if step.command_index is not None: - target_state[step.command_index] = float(step.target_u8) + target_state = list(self._target_command(step)) endpoint_errors = [ abs(value - target_state[index]) for index, value in enumerate(self.latest_state_u8) @@ -1062,7 +1201,9 @@ class L6ThreeCameraCalibrationNode(Node): # final target also rejects movement in the wrong direction. if ( not math.isfinite(self.step_last_distance_u8) - or progress_distance <= self.step_last_distance_u8 - 0.5 + or progress_distance <= self.step_last_distance_u8 - ( + 0.5 if self.command_unit == "u8" else 0.002 + ) ): self.step_last_distance_u8 = progress_distance self.step_last_progress_at = now @@ -1113,7 +1254,11 @@ class L6ThreeCameraCalibrationNode(Node): return self._publish_command(list(self.baseline_command)) self.state = "PASSED" - self.reason = "partial_calibration_passed" + self.reason = ( + "partial_calibration_passed" + if self.profile.scope.default_scope != "full" + else "full_calibration_passed" + ) self.final_json = str( self.session_dir / self.profile.artifacts.calibration_filename.format( @@ -1130,16 +1275,20 @@ class L6ThreeCameraCalibrationNode(Node): self.reason = str(reason) # Holding the latest position is safer than issuing an automatic move # after a stall, unexpected motor motion, or moved palm reference. - if len(self.latest_state_u8) == 6: + if len(self.latest_state_u8) == self.command_count: self._publish_command( - [int(np.clip(round(value), 0, 255)) for value in self.latest_state_u8] + [ + int(np.clip(round(value), 0, 255)) + if self.command_unit == "u8" else float(value) + for value in self.latest_state_u8 + ] ) append_jsonl(self.raw_path, {"kind": "paused", "reason": self.reason}) def _status(self) -> dict[str, Any]: step = self._current_step() feedback = float("nan") - if len(self.latest_state_u8) == 6: + if len(self.latest_state_u8) == self.command_count: feedback = ( float(np.mean(self.latest_state_u8)) if step is None or step.command_index is None @@ -1148,10 +1297,8 @@ class L6ThreeCameraCalibrationNode(Node): target_state: list[float] = [] channel_errors: list[float] = [] if step is not None: - target_state = [float(value) for value in self.baseline_command] - if step.command_index is not None: - target_state[step.command_index] = float(step.target_u8) - if len(self.latest_state_u8) == 6: + target_state = list(self._target_command(step)) + if len(self.latest_state_u8) == self.command_count: channel_errors = [ abs(value - target_state[index]) for index, value in enumerate(self.latest_state_u8) @@ -1186,8 +1333,19 @@ class L6ThreeCameraCalibrationNode(Node): "direction": None if step is None else step.direction, "attempt": None if step is None else step.attempt, "motor_index": None if step is None else step.command_index, - "speed_u8": None if step is None else int(step.speed_u8), - "target_u8": None if step is None else step.target_u8, + "command_unit": self.command_unit, + "speed_u8": ( + None if step is None or self.command_unit != "u8" else int(step.speed_u8) + ), + "speed_rad_s": ( + None if step is None or self.command_unit != "rad" else round(float(step.speed_u8), 6) + ), + "target_u8": ( + None if step is None or self.command_unit != "u8" else step.target_u8 + ), + "target_rad": ( + None if step is None or self.command_unit != "rad" else round(float(step.target_u8), 9) + ), "current_command_u8": ( None if step is None or not math.isfinite(self.step_requested_u8) @@ -1235,23 +1393,24 @@ class L6ThreeCameraCalibrationNode(Node): self.fixed_base_maximum_corner_drift_px ), "feedback_hz": round(self._feedback_hz(), 3), + "camera_info_views": sorted(self.camera_matrices), "state_publisher_count": self.count_publishers(self.state_topic), "command_publisher_count": self.count_publishers(self.command_topic), "command_names": list(self.command_names), - "latest_state_u8": list(self.latest_state_u8), - "target_state_u8": [round(value, 3) for value in target_state], - "current_command_state_u8": ( + f"latest_state_{self.command_unit}": list(self.latest_state_u8), + f"target_state_{self.command_unit}": [round(value, 6) for value in target_state], + f"current_command_state_{self.command_unit}": ( [] if self.step_last_command_u8 is None else list(self.step_last_command_u8) ), - "channel_errors_u8": [round(value, 3) for value in channel_errors], + f"channel_errors_{self.command_unit}": [round(value, 6) for value in channel_errors], "maximum_error_channel": ( None if not channel_errors else self.command_names[int(np.argmax(channel_errors))] ), - "maximum_error_u8": ( + f"maximum_error_{self.command_unit}": ( None if not channel_errors else round(max(channel_errors), 3) ), "final_json": getattr(self, "final_json", ""), diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py index ad5a053..043626f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py @@ -48,6 +48,12 @@ _PHASE_LABELS = { "prepare": "扫描起点准备", "retry_prepare": "自动重扫起点准备", "sweep": "正式扫描", + "clearance": "手指避让", + "clearance_outer": "小指/无名指避让", + "clearance_splay": "侧摆避让", + "return_splay_zero": "侧摆回零", + "return_middle_open": "展开中指", + "return_outer_open": "展开小指/无名指", } _DIRECTION_LABELS = { "decreasing": "递减", @@ -207,10 +213,15 @@ def render_six_channel_progress_zh( cycle_text = "-" if cycle is None else str(int(cycle) + 1) direction = status.get("direction") direction_text = _DIRECTION_LABELS.get(str(direction), "-") - target = status.get("target_u8") + command_unit = str(status.get("command_unit", "u8")) + target = status.get("target_rad") if command_unit == "rad" else status.get("target_u8") requested = status.get("current_command_u8", target) actual = status.get("actual_u8") - actual_text = "未知" if actual is None else f"{float(actual):.1f}" + if command_unit == "rad": + requested = "未知" if requested is None else f"{float(requested):.3f} rad" + actual_text = "未知" if actual is None else f"{float(actual):.3f} rad" + else: + actual_text = "未知" if actual is None else f"{float(actual):.1f}" valid = int(status.get("valid_frames", 0) or 0) total = int(status.get("total_frames", 0) or 0) rate = float(status.get("tag_detection_rate", 0.0) or 0.0) @@ -257,6 +268,15 @@ def render_six_channel_progress_zh( lines.append( f"运动：速度档 {int(speed_u8)}；全行程 {trajectory_seconds:.1f} 秒余弦轨迹" ) + speed_rad_s = status.get("speed_rad_s") + if speed_rad_s is not None: + trajectory_seconds = float( + status.get("command_trajectory_duration_seconds", 0.0) or 0.0 + ) + lines.append( + f"运动：峰值 {float(speed_rad_s):.3f} rad/s；" + f"本段 {trajectory_seconds:.1f} 秒余弦轨迹" + ) latest_state = status.get("latest_state_u8", []) command_names = status.get("command_names", []) if ( @@ -353,7 +373,6 @@ def _launch_command( "hand_type": config.side, "tag_layout": config.tag_layout, "serial_number": config.serial_number, - "can_interface": config.can_interface, "source_urdf_path": str(config.source_urdf), "source_urdf_expected_sha256": config.source_urdf_sha256, "camera_extrinsics_file": str(config.camera_extrinsics), @@ -362,16 +381,23 @@ def _launch_command( "calibration_config_expected_sha256": config.calibration_config_sha256, "tag_config": str(config.tag_config), "tag_config_expected_sha256": config.tag_config_sha256, + "vendor_sdk_config": "" if config.sdk_config is None else str(config.sdk_config), + "sdk_config_expected_sha256": config.sdk_config_sha256, "output_root": str(config.output_root), "session_dir": str(session), "corrected_urdf_output_dir": str(session), - "recalibration_scope": "partial", + "recalibration_scope": config.calibration_contract.typed_profile.scope.default_scope, "calibration_speed": str(int(sdk_startup_speed_u8)), "index_roll_calibration_speed": "1", "index_flex_calibration_speed": "1", "commands_enabled": str(commands_enabled).lower(), "record_bag": str(record_bag).lower(), } + # HCAN/ZLG vendor products do not have a Linux SocketCAN interface. + # Omitting the launch override also avoids the invalid token + # ``can_interface:=`` when the reviewed product value is intentionally empty. + if config.can_interface: + arguments["can_interface"] = config.can_interface for view, camera in config.cameras.items(): arguments[f"{view}_camera_serial"] = camera["serial_number"] arguments[f"{view}_camera_name"] = camera["camera_name"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/__init__.py new file mode 100644 index 0000000..26fbdf3 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/__init__.py @@ -0,0 +1,11 @@ +"""Registered O12 calibration profiles.""" + +from ..registry import ProfileRegistry + + +def register_profiles(registry: ProfileRegistry) -> None: + from .profile import build_profile + registry.register(build_profile()) + + +__all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py new file mode 100644 index 0000000..b5edc00 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py @@ -0,0 +1,313 @@ +"""Schema-v7 radian-knot runtime artifact for O12.""" + +from __future__ import annotations + +import math +from pathlib import Path +from typing import Any, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np + +from .fitting import O12FitResult, curve_input_knots_rad, curve_values_at_rad +from .profile import ( + ACTIVE_JOINTS, + COMMAND_INDEX_BY_JOINT, + COMMAND_NAMES, + KEY, + MEASURED_PASSIVE_JOINTS, + MIMIC_SOURCE_BY_JOINT, + PASSIVE_JOINTS, + SAFE_LOWER_RAD, + SAFE_UPPER_RAD, + SDK_LOWER_RAD, + SDK_TO_URDF_JOINT, + SDK_UPPER_RAD, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + build_typed_profile, +) + + +ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS) + + +def _motor_index(joint: str) -> int: + source = str(joint) + while source not in COMMAND_INDEX_BY_JOINT: + source = MIMIC_SOURCE_BY_JOINT[source] + return COMMAND_INDEX_BY_JOINT[source] + + +def _task_for_joint(joint: str): + target = str(joint) + while True: + if target in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT: + target = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT[target] + elif target not in COMMAND_INDEX_BY_JOINT: + target = MIMIC_SOURCE_BY_JOINT[target] + else: + break + return next( + task for task in build_typed_profile().motion.tasks + if target in task.joints + ) + + +def _source_joint_metadata(source_urdf: str | Path) -> dict[str, dict[str, Any]]: + root = ET.parse(Path(source_urdf)).getroot() + result: dict[str, dict[str, Any]] = {} + for node in root.findall("joint"): + name = str(node.get("name", "")) + if name not in ALL_REVOLUTE_JOINTS: + continue + limit = node.find("limit") + if node.get("type") != "revolute" or limit is None: + raise ValueError(f"invalid O12 revolute joint: {name}") + item: dict[str, Any] = { + "lower": float(limit.get("lower", "nan")), + "upper": float(limit.get("upper", "nan")), + } + mimic = node.find("mimic") + if mimic is not None: + item.update({ + "source_joint": str(mimic.get("joint", "")), + "multiplier": float(mimic.get("multiplier", "nan")), + "offset": float(mimic.get("offset", "0")), + }) + result[name] = item + if set(result) != ALL_REVOLUTE_JOINTS: + raise ValueError("source URDF must contain all 19 O12 revolute joints") + return result + + +def _round(values: Sequence[float], digits: int = 10) -> list[float]: + array = np.asarray(values, dtype=float) + if array.shape != (65,) or not np.all(np.isfinite(array)): + raise ValueError("O12 knot curve must contain 65 finite values") + return [round(float(value), digits) for value in array] + + +def _zeroed(values: Sequence[float], inputs: Sequence[float]) -> np.ndarray: + curve = np.asarray(values, dtype=float) + baseline = float(np.interp(0.0, np.asarray(inputs, dtype=float), curve)) + return curve - baseline + + +def build_o12_runtime_payload( + *, + serial_number: str, + source_urdf: str | Path, + result: O12FitResult, + protected_inputs: Mapping[str, str], + passed: bool, +) -> dict[str, Any]: + profile = build_typed_profile() + source = _source_joint_metadata(source_urdf) + curves: dict[str, tuple[np.ndarray, np.ndarray, np.ndarray, list[float]]] = {} + joints: dict[str, dict[str, Any]] = {} + + for name in ACTIVE_JOINTS: + donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name) + fit = result.curves[donor] + inputs = list(curve_input_knots_rad(donor)) + decreasing = _zeroed( + curve_values_at_rad(donor, fit, inputs, "decreasing_rad"), inputs + ) + increasing = _zeroed( + curve_values_at_rad(donor, fit, inputs, "increasing_rad"), inputs + ) + angle = 0.5 * (decreasing + increasing) + curves[name] = (angle, decreasing, increasing, inputs) + motor = COMMAND_INDEX_BY_JOINT[name] + joint: dict[str, Any] = { + "urdf_joint": name, + "sdk_channel": COMMAND_NAMES[motor], + "motor_index": motor, + "passive": False, + "calibration_status": profile.joint_coverage[name], + "curve_input_knots_rad": _round(inputs), + "angle_rad": _round(angle), + "decreasing_rad": _round(decreasing), + "increasing_rad": _round(increasing), + "zero_feedback_rad": 0.0, + "urdf_sign_adapter": ( + "sdk_negative_to_urdf_positive" + if SDK_UPPER_RAD[motor] <= 0.0 and SDK_LOWER_RAD[motor] < 0.0 + else "identity" + ), + "raw_increasing_curve_branch": ( + "decreasing" + if _task_for_joint(donor).end_value > _task_for_joint(donor).start_value + else "increasing" + ), + } + if donor != name: + joint["transferred_from_joint"] = donor + joint["transfer_policy"] = "pinky_feedback_correction_on_ring_cad" + joints[name] = joint + + for name in PASSIVE_JOINTS: + metadata = source[name] + source_name = MIMIC_SOURCE_BY_JOINT[name] + motor = _motor_index(source_name) + if name in MEASURED_PASSIVE_JOINTS: + fit = result.curves[name] + inputs = list(curve_input_knots_rad(name)) + decreasing = _zeroed( + curve_values_at_rad(name, fit, inputs, "decreasing_rad"), inputs + ) + float(metadata["offset"]) + increasing = _zeroed( + curve_values_at_rad(name, fit, inputs, "increasing_rad"), inputs + ) + float(metadata["offset"]) + angle = 0.5 * (decreasing + increasing) + coupling = result.mimic_fits[name] + coefficients = [float(metadata["offset"]), *coupling.coefficients] + coefficients.extend([0.0] * (6 - len(coefficients))) + coupling_model = coupling.model + multiplier = coupling.urdf_mimic_multiplier + policy = coupling.urdf_mimic_policy + status = profile.joint_coverage[name] + else: + source_curve, source_dec, source_inc, inputs = curves[source_name] + multiplier = float(metadata["multiplier"]) + offset = float(metadata["offset"]) + angle = offset + multiplier * source_curve + decreasing = offset + multiplier * source_dec + increasing = offset + multiplier * source_inc + coefficients = [offset, multiplier, 0.0, 0.0, 0.0, 0.0] + coupling_model = "linear_mimic" + policy = "cad_nominal_preserved" + status = profile.joint_coverage[name] + curves[name] = (angle, decreasing, increasing, list(inputs)) + joints[name] = { + "urdf_joint": name, + "sdk_channel": COMMAND_NAMES[motor], + "motor_index": motor, + "passive": True, + "source_joint": source_name, + "calibration_status": status, + "static_zero_policy": "cad_preserved_tag_mount_ambiguous", + "curve_input_knots_rad": _round(inputs), + "angle_rad": _round(angle), + "decreasing_rad": _round(decreasing), + "increasing_rad": _round(increasing), + "coupling_model": coupling_model, + "coupling_coefficients": [round(float(v), 10) for v in coefficients], + "mimic_multiplier": round(float(multiplier), 10), + "urdf_mimic_policy": policy, + "raw_increasing_curve_branch": ( + "decreasing" + if _task_for_joint(name).end_value > _task_for_joint(name).start_value + else "increasing" + ), + } + + errors = np.abs(np.concatenate([ + np.asarray(values, dtype=float) + for values in result.holdout_errors_rad.values() + ])) + payload: dict[str, Any] = { + "schema_version": 7, + "profile_id": KEY.profile_id, + "layout_id": KEY.layout, + "model": "O12", + "side": "right", + "serial_number": str(serial_number), + "calibration_scope": "full", + "publication_pointer": "latest_passed", + "angle_unit": "rad", + "curve_input_domain": "feedback_rad", + "runtime_curve_policy": "direction_aware_knots", + "command_names": list(COMMAND_NAMES), + "command_lower_rad": list(SAFE_LOWER_RAD), + "command_upper_rad": list(SAFE_UPPER_RAD), + "baseline_command_rad": [0.0] * 12, + "sdk_mapping": [ + { + "motor_index": i, + "sdk_channel": command, + "urdf_joint": urdf, + "sdk_lower_rad": SDK_LOWER_RAD[i], + "sdk_upper_rad": SDK_UPPER_RAD[i], + "calibration_lower_rad": SAFE_LOWER_RAD[i], + "calibration_upper_rad": SAFE_UPPER_RAD[i], + } + for i, (command, urdf) in enumerate(zip(COMMAND_NAMES, SDK_TO_URDF_JOINT)) + ], + "protected_inputs": dict(protected_inputs), + "joints": joints, + "quality": { + "passed": bool(passed), + "training_cycles": [0, 1, 2], + "holdout_cycle": 3, + "validation_mae_rad": round(float(np.mean(errors)), 10), + "validation_p95_rad": round(float(np.percentile(errors, 95.0)), 10), + "validation_max_rad": round(float(np.max(errors)), 10), + "ring_transfer_source": "pinky_mcp_pitch", + "ring_cad_geometry_and_mimic_preserved": True, + }, + } + validate_o12_runtime_payload(payload) + return payload + + +def validate_o12_runtime_payload(payload: Mapping[str, Any]) -> None: + required = { + "schema_version", "profile_id", "layout_id", "model", "side", + "serial_number", "calibration_scope", "publication_pointer", + "angle_unit", "curve_input_domain", "runtime_curve_policy", + "command_names", "command_lower_rad", "command_upper_rad", + "baseline_command_rad", "sdk_mapping", "protected_inputs", "joints", + "quality", + } + if set(payload) != required: + raise ValueError("schema v7 O12 artifact has unexpected top-level fields") + if ( + payload["schema_version"] != 7 + or payload["profile_id"] != KEY.profile_id + or payload["model"] != "O12" + or payload["side"] != "right" + or payload["angle_unit"] != "rad" + or payload["curve_input_domain"] != "feedback_rad" + or payload["runtime_curve_policy"] != "direction_aware_knots" + ): + raise ValueError("schema v7 O12 artifact identity/domain is invalid") + if tuple(payload["command_names"]) != COMMAND_NAMES: + raise ValueError("O12 SDK channel order differs from fixed contract") + if len(payload["sdk_mapping"]) != 12 or any( + int(row.get("motor_index", -1)) != i + or row.get("sdk_channel") != COMMAND_NAMES[i] + or row.get("urdf_joint") != SDK_TO_URDF_JOINT[i] + or float(row.get("sdk_lower_rad", math.nan)) != SDK_LOWER_RAD[i] + or float(row.get("sdk_upper_rad", math.nan)) != SDK_UPPER_RAD[i] + or float(row.get("calibration_lower_rad", math.nan)) != SAFE_LOWER_RAD[i] + or float(row.get("calibration_upper_rad", math.nan)) != SAFE_UPPER_RAD[i] + for i, row in enumerate(payload["sdk_mapping"]) + ): + raise ValueError("O12 SDK/URDF mapping differs from fixed contract") + protected = payload["protected_inputs"] + expected = build_typed_profile().artifacts.protected_input_fields + if not isinstance(protected, Mapping) or set(protected) != expected: + raise ValueError("O12 protected input hashes are incomplete") + if any(len(str(v)) != 64 or any(c not in "0123456789abcdef" for c in str(v)) for v in protected.values()): + raise ValueError("O12 protected input hash is invalid") + joints = payload["joints"] + if not isinstance(joints, Mapping) or set(joints) != ALL_REVOLUTE_JOINTS: + raise ValueError("schema v7 must contain all 19 O12 revolute joints") + for name, joint in joints.items(): + inputs = np.asarray(joint.get("curve_input_knots_rad"), dtype=float) + if inputs.shape != (65,) or not np.all(np.isfinite(inputs)) or np.any(np.diff(inputs) <= 0.0): + raise ValueError(f"{name} has invalid radian knots") + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + values = np.asarray(joint.get(field), dtype=float) + if values.shape != (65,) or not np.all(np.isfinite(values)): + raise ValueError(f"{name}.{field} must contain 65 finite values") + if int(joint.get("motor_index", -1)) != _motor_index(name): + raise ValueError(f"{name} has invalid O12 motor binding") + if joint.get("raw_increasing_curve_branch") not in {"decreasing", "increasing"}: + raise ValueError(f"{name} has invalid O12 direction binding") + if not bool(payload["quality"].get("passed")): + raise ValueError("failed O12 calibration cannot be published") + + +__all__ = ["ALL_REVOLUTE_JOINTS", "build_o12_runtime_payload", "validate_o12_runtime_payload"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py new file mode 100644 index 0000000..0586a85 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py @@ -0,0 +1,181 @@ +"""Direction-aware fitting for O12 feedback expressed in radians.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from pathlib import Path +from typing import Any, Mapping, Sequence + +import numpy as np + +from ..g20.profile import JointCurveFit +from ..g20.zero_solver import fit_rotation_joint_curve, rotation_curve_holdout_errors +from ..l6.fitting import MimicFit, fit_coupling_model +from .profile import ( + CALIBRATED_ACTIVE_JOINTS, + COMMAND_INDEX_BY_JOINT, + MEASURED_PASSIVE_JOINTS, + MIMIC_SOURCE_BY_JOINT, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + build_typed_profile, +) + + +@dataclass(frozen=True) +class O12FitResult: + curves: Mapping[str, JointCurveFit] + zero_offsets_rad: Mapping[str, float] + travels_rad: Mapping[str, float] + mimic_fits: Mapping[str, MimicFit] + holdout_errors_rad: Mapping[str, tuple[float, ...]] + + +def _task_by_joint() -> dict[str, Any]: + return { + joint: task + for task in build_typed_profile().motion.tasks + for joint in task.joints + } + + +def feedback_rad_to_curve_index(joint: str, feedback_rad: float) -> float: + """Map a physical feedback angle to the fitter's normalized 255..0 axis.""" + task = _task_by_joint()[str(joint)] + denominator = task.end_value - task.start_value + if abs(denominator) <= 1.0e-12: + raise ValueError(f"O12 task {task.key} has a degenerate range") + phase = (float(feedback_rad) - task.start_value) / denominator + return 255.0 * (1.0 - float(np.clip(phase, 0.0, 1.0))) + + +def curve_input_knots_rad(joint: str, count: int = 65) -> tuple[float, ...]: + task = _task_by_joint()[str(joint)] + return tuple( + float(value) + for value in np.linspace( + min(task.start_value, task.end_value), + max(task.start_value, task.end_value), + int(count), + ) + ) + + +def curve_values_at_rad( + joint: str, fit: JointCurveFit, inputs_rad: Sequence[float], branch: str +) -> tuple[float, ...]: + values = np.asarray(getattr(fit, branch), dtype=float) + indices = np.arange(256, dtype=float) + return tuple( + float(np.interp(feedback_rad_to_curve_index(joint, value), indices, values)) + for value in inputs_rad + ) + + +def _virtual_records( + joint: str, rows: Sequence[Mapping[str, Any]] +) -> list[dict[str, Any]]: + result = [] + for source in rows: + row = dict(source) + curve_index = feedback_rad_to_curve_index(joint, float(row["feedback_rad"])) + row["command_u8"] = int(np.clip(round(curve_index), 0, 255)) + result.append(row) + return result + + +def _travel(fit: JointCurveFit) -> float: + return 0.5 * ( + float(fit.decreasing_rad[0] - fit.decreasing_rad[255]) + + float(fit.increasing_rad[0] - fit.increasing_rad[255]) + ) + + +def fit_o12_session( + source_urdf: str | Path, + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], +) -> O12FitResult: + del source_urdf # topology is validated by the immutable product loader + expected = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS + if set(records_by_joint) != expected: + missing = sorted(expected - set(records_by_joint)) + extra = sorted(set(records_by_joint) - expected) + raise ValueError(f"O12 records differ from profile: missing={missing} extra={extra}") + curves: dict[str, JointCurveFit] = {} + holdout: dict[str, tuple[float, ...]] = {} + cycle_curves: dict[str, dict[int, JointCurveFit]] = {} + for joint in sorted(expected): + rows = _virtual_records(joint, records_by_joint[joint]) + training = [row for row in rows if int(row["cycle"]) in {0, 1, 2}] + validation = [row for row in rows if int(row["cycle"]) == 3] + if not training or not validation: + raise ValueError(f"{joint} is missing training or holdout records") + fit = fit_rotation_joint_curve( + training, zero_command_u8=255, canonical_zero_direction="decreasing" + ) + errors = rotation_curve_holdout_errors(fit, validation, zero_command_u8=255) + absolute = np.abs(np.asarray(errors, dtype=float)) + if ( + float(np.mean(absolute)) > math.radians(1.0) + or float(np.percentile(absolute, 95.0)) > math.radians(2.0) + or float(np.max(absolute)) > math.radians(3.0) + ): + raise ValueError(f"{joint} isolated holdout failed") + if fit.maximum_hysteresis_rad > math.radians(2.0): + raise ValueError(f"{joint} hysteresis exceeds 2 degrees") + curves[joint] = fit + holdout[joint] = tuple(float(value) for value in errors) + cycle_curves[joint] = { + cycle: fit_rotation_joint_curve( + [row for row in training if int(row["cycle"]) == cycle], + zero_command_u8=255, + canonical_zero_direction="decreasing", + ) + for cycle in (0, 1, 2) + } + + mimic_fits: dict[str, MimicFit] = {} + for target in sorted(MEASURED_PASSIVE_JOINTS): + source = MIMIC_SOURCE_BY_JOINT[target] + cycle_pairs = [ + ( + tuple(cycle_curves[source][cycle].decreasing_rad) + + tuple(cycle_curves[source][cycle].increasing_rad), + tuple(cycle_curves[target][cycle].decreasing_rad) + + tuple(cycle_curves[target][cycle].increasing_rad), + ) + for cycle in (0, 1, 2) + ] + mimic_fits[target] = fit_coupling_model( + source, + target, + curves[source], + curves[target], + model="quadratic_runtime", + cycle_curve_pairs=cycle_pairs, + minimum_multiplier=0.5, + maximum_multiplier=2.2, + ) + + travels = { + joint: _travel(curves[joint]) for joint in CALIBRATED_ACTIVE_JOINTS + } + for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items(): + travels[target] = travels[donor] + # O12 active feedback is already a physical angle with open/centred zero. + # Calibration publishes the visual transfer curves and measured travel; + # it must not invent a static offset from an arbitrary Tag mounting angle. + offsets = {joint: 0.0 for joint in travels} + return O12FitResult( + curves=curves, + zero_offsets_rad=offsets, + travels_rad=travels, + mimic_fits=mimic_fits, + holdout_errors_rad=holdout, + ) + + +__all__ = [ + "O12FitResult", "curve_input_knots_rad", "curve_values_at_rad", + "feedback_rad_to_curve_index", "fit_o12_session", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py new file mode 100644 index 0000000..8eec1f5 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py @@ -0,0 +1,76 @@ +"""Physical-radian motion helpers for O12 right calibration.""" + +from __future__ import annotations + +import math +from typing import Sequence + +from ...core import CalibrationProfile, TaskSpec + + +def cosine_position_trajectory_rad( + start_rad: float, + target_rad: float, + elapsed_seconds: float, + maximum_speed_rad_s: float, +) -> tuple[float, float, float]: + """Cosine trajectory whose peak velocity does not exceed the limit.""" + speed = float(maximum_speed_rad_s) + if not math.isfinite(speed) or speed <= 0.0: + raise ValueError("maximum_speed_rad_s must be positive") + distance = abs(float(target_rad) - float(start_rad)) + if distance <= 0.0: + return float(target_rad), 1.0, 0.0 + duration = math.pi * distance / (2.0 * speed) + phase = min(1.0, max(0.0, float(elapsed_seconds) / duration)) + blend = 0.5 - 0.5 * math.cos(math.pi * phase) + return ( + float(start_rad) + (float(target_rad) - float(start_rad)) * blend, + phase, + duration, + ) + + +def build_calibration_motion_command( + task: TaskSpec, + command_rad: float, + *, + profile: CalibrationProfile, +) -> list[float]: + values = list(profile.command.baseline_values) + for index, value in task.auxiliary_commands: + values[int(index)] = float(value) + values[int(task.command_index)] = float(command_rad) + return values + + +def build_calibration_preparation_waypoints( + task: TaskSpec, + *, + profile: CalibrationProfile, + current_command: Sequence[float] | None = None, +) -> tuple[tuple[float, ...], ...]: + del current_command + return (tuple(build_calibration_motion_command(task, task.start_value, profile=profile)),) + + +def build_calibration_return_waypoints( + target_command: Sequence[float] | None = None, + *, + profile: CalibrationProfile, + current_command: Sequence[float] | None = None, + **_: object, +) -> tuple[tuple[float, ...], ...]: + del current_command + target = tuple( + profile.command.baseline_values if target_command is None else target_command + ) + if len(target) != profile.command.command_count: + raise ValueError("O12 return command has the wrong channel count") + return (tuple(float(value) for value in target),) + + +__all__ = [ + "build_calibration_motion_command", "build_calibration_preparation_waypoints", + "build_calibration_return_waypoints", "cosine_position_trajectory_rad", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/node.py new file mode 100644 index 0000000..2e75b62 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/node.py @@ -0,0 +1,369 @@ +"""Safety-gated online acquisition node for O12 right.""" + +from __future__ import annotations + +import math +import time + +import numpy as np +import rclpy +from scipy.spatial.transform import Rotation +from std_msgs.msg import Empty, Int8MultiArray, Int16MultiArray + +from ...storage import append_jsonl +from ..l6.node import L6ThreeCameraCalibrationNode, MotionStep +from .pipeline import finalize_o12_session +from .profile import ( + INDEX_CLEARANCE_RAD, + PARK_FINGER_RAD, + PARK_MIDDLE_MCP_RAD, + PARK_MIDDLE_PIP_RAD, + build_typed_profile, +) + + +POSITION_MODE = 0 + + +class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): + def __init__(self) -> None: + super().__init__( + profile=build_typed_profile(), + finalizer=finalize_o12_session, + sample_kind="o12_joint_sample", + ) + self.mode_verified = False + self.error_verified = False + self.temperature_verified = False + self.temperature_fallback_active = False + self.health_check_started_at = time.monotonic() + self.latest_errors: tuple[int, ...] = () + self.latest_temperatures: tuple[int, ...] = () + self.last_health_query_at = 0.0 + self.preflight_reference_by_task: dict[str, Rotation] = {} + self.preflight_maximum_rotation_by_task: dict[str, float] = {} + self.measured_direction_axis_by_task: dict[str, tuple[float, float, float]] = {} + self.control_mode_publisher = self.create_publisher( + Int8MultiArray, "/o12/right/joint_control_mode_cmd", 10 + ) + self.error_query_publisher = self.create_publisher( + Empty, "/o12/right/joint_error_cmd", 10 + ) + self.temperature_query_publisher = self.create_publisher( + Empty, "/o12/right/joint_temperature_cmd", 10 + ) + self.create_subscription( + Int8MultiArray, + "/o12/right/joint_control_mode_states", + self._mode_callback, + 10, + ) + self.create_subscription( + Int16MultiArray, + "/o12/right/joint_error_states", + self._error_callback, + 10, + ) + self.create_subscription( + Int16MultiArray, + "/o12/right/joint_temperature_states", + self._temperature_callback, + 10, + ) + + def _declare_parameters(self) -> None: + super()._declare_parameters() + self.declare_parameter("maximum_temperature_c", 70) + self.declare_parameter("temperature_report_required", False) + self.declare_parameter("temperature_fallback_after_seconds", 5.0) + self.declare_parameter("motion_speed_scale", 1.0) + + def _load_parameters(self) -> None: + super()._load_parameters() + self.maximum_temperature_c = int( + self.get_parameter("maximum_temperature_c").value + ) + self.temperature_report_required = bool( + self.get_parameter("temperature_report_required").value + ) + self.temperature_fallback_after_seconds = float( + self.get_parameter("temperature_fallback_after_seconds").value + ) + self.motion_speed_scale = float( + self.get_parameter("motion_speed_scale").value + ) + if not 40 <= self.maximum_temperature_c <= 90: + raise ValueError("maximum_temperature_c must be in [40, 90]") + if not 1.0 <= self.temperature_fallback_after_seconds <= 30.0: + raise ValueError("temperature_fallback_after_seconds must be in [1, 30]") + if not 0.5 <= self.motion_speed_scale <= 2.0: + raise ValueError("motion_speed_scale must be in [0.5, 2.0]") + + def _temperature_ready(self) -> bool: + return self.temperature_verified or self.temperature_fallback_active + + def _activate_temperature_fallback_if_allowed(self, now: float) -> None: + if ( + self.temperature_report_required + or self.temperature_verified + or self.temperature_fallback_active + or not self.error_verified + or now - self.health_check_started_at + < self.temperature_fallback_after_seconds + ): + return + self.temperature_fallback_active = True + append_jsonl(self.raw_path, { + "kind": "o12_temperature_capability_fallback", + "temperature_report_received": False, + "fallback_protection": "joint_error_states_bit1_overheat", + "sdk_config_sha256": self.protected_inputs["sdk_config_sha256"], + }) + self.get_logger().warning( + "O12 temperature report unavailable; continuing with error-code " + "bit1 overheat protection" + ) + + def _mode_callback(self, message: Int8MultiArray) -> None: + values = tuple(int(value) for value in message.data) + if len(values) != 12 or any(value != POSITION_MODE for value in values): + self._pause("control_mode_is_not_position") + return + self.mode_verified = True + + def _error_callback(self, message: Int16MultiArray) -> None: + self.latest_errors = tuple(int(value) for value in message.data) + if len(self.latest_errors) != 12: + self._pause("invalid_error_report_length") + elif any(self.latest_errors): + self._pause("o12_error_report_nonzero") + else: + self.error_verified = True + + def _temperature_callback(self, message: Int16MultiArray) -> None: + self.latest_temperatures = tuple(int(value) for value in message.data) + if len(self.latest_temperatures) != 12: + self._pause("invalid_temperature_report_length") + elif any(value >= self.maximum_temperature_c for value in self.latest_temperatures): + self._pause("o12_over_temperature") + else: + self.temperature_verified = True + self.temperature_fallback_active = False + + def _query_health(self) -> None: + if not self.commands_enabled: + return + mode = Int8MultiArray() + mode.data = [POSITION_MODE] * 12 + self.control_mode_publisher.publish(mode) + self.error_query_publisher.publish(Empty()) + self.temperature_query_publisher.publish(Empty()) + self.last_health_query_at = time.monotonic() + + @staticmethod + def _full_target(**values: float) -> tuple[float, ...]: + index = { + "thumb_roll": 0, "thumb_abad": 1, "thumb_mcp": 2, + "thumb_pip": 3, "index_abad": 4, "index_mcp": 5, + "index_pip": 6, "middle_abad": 7, "middle_mcp": 8, + "middle_pip": 9, "ring_mcp": 10, "pinky_mcp": 11, + } + target = [0.0] * 12 + for name, value in values.items(): + target[index[name]] = float(value) + return tuple(target) + + def _build_steps(self) -> list[MotionStep]: + def scaled(speed: float, maximum: float | None = None) -> float: + value = float(speed) * self.motion_speed_scale + return value if maximum is None else min(value, maximum) + + steps: list[MotionStep] = [ + MotionStep( + "baseline", None, None, 0.0, scaled(0.10, 0.15), + target_command=self._full_target(), + ) + ] + previous_group = "thumb" + for task in self.profile.motion.tasks: + group = task.key.split("_", 1)[0] + if group != previous_group: + if group == "middle": + outer = self._full_target( + ring_mcp=PARK_FINGER_RAD, + pinky_mcp=PARK_FINGER_RAD, + ) + steps.append(MotionStep("clearance_outer", None, None, 0.0, scaled(0.10, 0.15), target_command=outer)) + target = self._full_target( + index_abad=INDEX_CLEARANCE_RAD, + ring_mcp=PARK_FINGER_RAD, + pinky_mcp=PARK_FINGER_RAD, + ) + steps.append(MotionStep("clearance_splay", None, None, 0.0, scaled(0.04, 0.08), target_command=target)) + elif group == "index": + splay_zero = self._full_target( + ring_mcp=PARK_FINGER_RAD, + pinky_mcp=PARK_FINGER_RAD, + ) + steps.append(MotionStep("clearance_splay", None, None, 0.0, scaled(0.04, 0.08), target_command=splay_zero)) + target = self._full_target( + middle_mcp=PARK_MIDDLE_MCP_RAD, + middle_pip=PARK_MIDDLE_PIP_RAD, + ring_mcp=PARK_FINGER_RAD, + pinky_mcp=PARK_FINGER_RAD, + ) + steps.append(MotionStep("clearance", None, None, 0.0, scaled(0.10, 0.15), target_command=target)) + previous_group = group + probe_delta = math.copysign( + min(abs(task.end_value - task.start_value), math.radians(3.0)), + task.end_value - task.start_value, + ) + for target in (task.start_value, task.start_value + probe_delta, task.start_value): + steps.append(MotionStep( + "preflight", task.key, task.command_index, target, + scaled(float(task.preflight_speed or 0.02), 0.04), + )) + for cycle in (0, 1, 2, 3): + formal_speed = scaled(float(task.formal_speed)) + steps.extend(( + MotionStep("prepare", task.key, task.command_index, task.start_value, formal_speed, cycle), + MotionStep("sweep", task.key, task.command_index, task.end_value, formal_speed, cycle, "decreasing"), + MotionStep("prepare", task.key, task.command_index, task.end_value, formal_speed, cycle), + MotionStep("sweep", task.key, task.command_index, task.start_value, formal_speed, cycle, "increasing"), + )) + # Collision-aware return: side-swing neutral, middle open, then outer pair. + middle_and_outer_parked = self._full_target( + middle_mcp=PARK_MIDDLE_MCP_RAD, + middle_pip=PARK_MIDDLE_PIP_RAD, + ring_mcp=PARK_FINGER_RAD, pinky_mcp=PARK_FINGER_RAD + ) + outer_parked = self._full_target( + ring_mcp=PARK_FINGER_RAD, pinky_mcp=PARK_FINGER_RAD + ) + steps.append(MotionStep("return_splay_zero", None, None, 0.0, scaled(0.04, 0.08), target_command=middle_and_outer_parked)) + steps.append(MotionStep("return_middle_open", None, None, 0.0, scaled(0.10, 0.15), target_command=outer_parked)) + steps.append(MotionStep("return_outer_open", None, None, 0.0, scaled(0.10, 0.15), target_command=self._full_target())) + return steps + + def _start(self, request, response): + if not (self.mode_verified and self.error_verified and self._temperature_ready()): + response.success = False + response.message = "O12 POSITION mode/error/temperature precheck is incomplete" + return response + return super()._start(request, response) + + def _observe_preflight_pose(self, view, selected, step: MotionStep) -> None: + task = self._task(str(step.task_key)) + if task.view != view: + return + measurement = self.profile.measurement.measurements[task.joints[0]] + parent = Rotation.from_quat( + selected[str(measurement.parent_role)].quaternion_xyzw + ) + child = Rotation.from_quat( + selected[str(measurement.child_role)].quaternion_xyzw + ) + relative = parent.inv() * child + if abs(float(step.target_u8) - task.start_value) <= 1.0e-9: + self.preflight_reference_by_task.setdefault(task.key, relative) + return + reference = self.preflight_reference_by_task.get(task.key) + if reference is None: + return + vector = (reference.inv() * relative).as_rotvec() + magnitude = float(np.linalg.norm(vector)) + if magnitude > self.preflight_maximum_rotation_by_task.get(task.key, 0.0): + self.preflight_maximum_rotation_by_task[task.key] = magnitude + if magnitude > 1.0e-9: + axis = vector / magnitude + self.measured_direction_axis_by_task[task.key] = tuple( + float(value) for value in axis + ) + + def _finish_step(self, step: MotionStep) -> None: + previous_task = step.task_key + if step.phase == "preflight" and step.task_key is not None: + task = self._task(step.task_key) + is_probe = abs(float(step.target_u8) - task.start_value) > 1.0e-9 + if is_probe: + rotation = self.preflight_maximum_rotation_by_task.get(task.key, 0.0) + if rotation < math.radians(0.25): + self._pause(f"mapping_preflight_no_target_motion:{task.key}") + return + append_jsonl(self.raw_path, { + "kind": "o12_fixed_mapping_preflight", + "task_name": task.key, + "motor_index": task.command_index, + "sdk_channel": self.command_names[task.command_index], + "urdf_joint": task.joints[0], + "command_delta_rad": round(float(step.target_u8) - task.start_value, 9), + "observed_rotation_rad": round(rotation, 9), + "observed_axis_tag_frame_xyz": list( + self.measured_direction_axis_by_task[task.key] + ), + "channel_exchange_allowed": False, + }) + super()._finish_step(step) + next_step = self._current_step() + if self.state == "RUNNING" and ( + next_step is None + or next_step.phase.startswith("clearance") + or (previous_task and next_step.task_key and previous_task.split("_", 1)[0] != next_step.task_key.split("_", 1)[0]) + ): + self._query_health() + + def _tick(self) -> None: + now = time.monotonic() + self._activate_temperature_fallback_if_allowed(now) + if not self.started and self.state not in {"PAUSED", "ABORTED", "PASSED"}: + if now - self.last_health_query_at >= 1.0: + self._query_health() + # The SDK's joint state is command-triggered; a 20 Hz zero command + # supplies the feedback stream used by READY and frequency checks. + self._publish_command(list(self.baseline_command)) + super()._tick() + if ( + not self.started + and self.state == "READY" + and not (self.mode_verified and self.error_verified and self._temperature_ready()) + ): + self.state = "WAIT_DEVICES" + self.reason = "waiting_for_o12_position_mode_and_health_reports" + + def _status(self): + value = super()._status() + value.update({ + "position_mode_verified": self.mode_verified, + "error_report_verified": self.error_verified, + "temperature_report_verified": self.temperature_verified, + "temperature_fallback_active": self.temperature_fallback_active, + "temperature_safety_policy": ( + "direct_temperature_report" + if self.temperature_verified + else "error_bitmask_bit1_overheat" + if self.temperature_fallback_active + else "waiting_for_temperature_report" + ), + "latest_error_codes": list(self.latest_errors), + "latest_temperatures_c": list(self.latest_temperatures), + "motion_speed_scale": self.motion_speed_scale, + }) + return value + + +def main(args: list[str] | None = None) -> None: + rclpy.init(args=args) + node: O12ThreeCameraCalibrationNode | None = None + try: + node = O12ThreeCameraCalibrationNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +__all__ = ["O12ThreeCameraCalibrationNode", "POSITION_MODE", "main"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py new file mode 100644 index 0000000..a33f2a1 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py @@ -0,0 +1,145 @@ +"""Shared online/offline O12 session finalization.""" + +from __future__ import annotations + +from datetime import datetime +import json +import os +from pathlib import Path +from typing import Any, Mapping, Sequence + +from ...product import sha256_file +from ...storage import atomic_write_json +from .artifacts import build_o12_runtime_payload +from .fitting import O12FitResult, fit_o12_session +from .profile import CALIBRATED_ACTIVE_JOINTS, MEASURED_PASSIVE_JOINTS +from .urdf import O12UrdfCorrection, write_o12_corrected_urdf + + +MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS + + +def accepted_records_by_joint( + records: Sequence[Mapping[str, Any]], +) -> dict[str, list[dict[str, Any]]]: + samples = [ + dict(row) for row in records + if row.get("kind") == "o12_joint_sample" + and str(row.get("joint", "")) in MEASURED_JOINTS + ] + latest: dict[tuple[str, int, str], int] = {} + for row in samples: + key = (str(row["task_name"]), int(row["cycle"]), str(row["direction"])) + latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1))) + result = {name: [] for name in MEASURED_JOINTS} + for row in samples: + key = (str(row["task_name"]), int(row["cycle"]), str(row["direction"])) + if int(row.get("attempt", 1)) != latest[key]: + continue + accepted: dict[str, Any] = { + "cycle": int(row["cycle"]), + "direction": str(row["direction"]), + "feedback_rad": float(row["feedback_rad"]), + "relative_quaternion_xyzw": list(row["relative_quaternion_xyzw"]), + } + for field in ( + "relative_translation_xyz_m", "parent_pose_common", + "child_pose_common", "view_normal_common_xyz", + "camera_center_common_xyz_m", "state_rad", + ): + if field in row: + value = row[field] + accepted[field] = dict(value) if isinstance(value, Mapping) else list(value) + result[str(row["joint"])].append(accepted) + return result + + +def load_o12_raw_samples(path: str | Path) -> list[dict[str, Any]]: + source = Path(path).expanduser().resolve() + if not source.is_file(): + raise ValueError(f"raw O12 sample file does not exist: {source}") + rows: list[dict[str, Any]] = [] + with source.open("r", encoding="utf-8") as stream: + for line_number, line in enumerate(stream, 1): + if not line.strip(): + continue + try: + item = json.loads(line) + except json.JSONDecodeError as error: + raise ValueError(f"invalid O12 JSONL record at line {line_number}") from error + if not isinstance(item, Mapping): + raise ValueError(f"O12 JSONL line {line_number} is not an object") + rows.append(dict(item)) + return rows + + +def _publish(serial_root: Path, session: Path) -> Path: + if session.parent != serial_root or not session.is_dir(): + raise ValueError("O12 session must be a direct existing child") + destination = serial_root / "latest_passed" + temporary = serial_root / f".latest_passed.{os.getpid()}.tmp" + if temporary.exists() or temporary.is_symlink(): + temporary.unlink() + os.symlink(session.name, temporary, target_is_directory=True) + os.replace(temporary, destination) + return destination + + +def finalize_o12_session( + *, + session_dir: str | Path, + serial_number: str, + source_urdf: str | Path, + protected_inputs: Mapping[str, str], + records: Sequence[Mapping[str, Any]], + publish: bool = True, + timestamp: str | None = None, +) -> tuple[dict[str, Any], O12FitResult, O12UrdfCorrection]: + directory = Path(session_dir).expanduser().resolve() + directory.mkdir(parents=True, exist_ok=True) + result = fit_o12_session(source_urdf, accepted_records_by_joint(records)) + payload = build_o12_runtime_payload( + serial_number=serial_number, + source_urdf=source_urdf, + result=result, + protected_inputs=protected_inputs, + passed=True, + ) + correction = write_o12_corrected_urdf( + source_urdf=source_urdf, + output_directory=directory, + serial_number=serial_number, + result=result, + timestamp=timestamp or datetime.now().strftime("%Y%m%d_%H%M%S"), + ) + json_path = directory / f"o12_right_{serial_number}_calibration.json" + atomic_write_json(json_path, payload) + summary = { + "schema_version": 1, + "profile_id": "O12/right/o12_right_16/v1", + "serial_number": str(serial_number), + "result": "PASS", + "measured_active_joints": sorted(CALIBRATED_ACTIVE_JOINTS), + "measured_passive_joints": sorted(MEASURED_PASSIVE_JOINTS), + "ring_transfer": { + "source": "pinky_mcp_pitch", + "target": "ring_mcp_pitch", + "preserved_fields": list(correction.preserved_ring_fields), + }, + "artifacts": { + "json": json_path.name, + "urdf": correction.path.name, + "calibration_json_sha256": sha256_file(json_path), + "corrected_urdf_sha256": sha256_file(correction.path), + }, + } + atomic_write_json(directory / "calibration_summary_zh.json", summary) + if publish: + _publish(directory.parent, directory) + return payload, result, correction + + +__all__ = [ + "MEASURED_JOINTS", "accepted_records_by_joint", "finalize_o12_session", + "load_o12_raw_samples", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py new file mode 100644 index 0000000..3395f55 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py @@ -0,0 +1,344 @@ +"""Reviewed O12 right-hand 16-Tag calibration profile.""" + +from __future__ import annotations + +import math + +from ...core import ( + ArtifactPolicy, + CalibrationProfile, + CommandLayout, + MeasurementPolicy, + MeasurementSpec, + MotionPolicy, + ProfileKey, + QualityPolicy, + ScopePolicy, + TagSpec, + TaskSpec, + ViewSpec, + VisionRigSpec, + ZeroSolvePolicy, +) +from ..registry import EngineBindings, RegisteredProfile +from .motion import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, +) + + +KEY = ProfileKey("O12", "right", "o12_right_16", 1) + +# sensor_msgs/JointState.position active-angle order from API_CPP_O12.md. +COMMAND_NAMES: tuple[str, ...] = ( + "thumb_roll", + "thumb_abad", + "thumb_mcp", + "thumb_pip", + "index_abad", + "index_mcp", + "index_pip", + "middle_abad", + "middle_mcp", + "middle_pip", + "ring_mcp", + "pinky_mcp", +) + +SDK_LOWER_RAD = ( + 0.0, -1.387536755335492, -0.8272860654453121, -1.2915436464758039, + -0.2617993877991494, 0.0, 0.0, -0.2617993877991494, + 0.0, 0.0, 0.0, 0.0, +) +SDK_UPPER_RAD = ( + 0.9424777960769379, 0.0, 0.0, 0.0, + 0.2617993877991494, 1.3526301702956054, 1.530653753999027, + 0.2617993877991494, 1.3578661580515883, 1.8151424220741028, + 1.53588974175501, 1.53588974175501, +) + +# Source-URDF-safe intersections. The SDK exposes a larger range on several +# axes, but calibration must never command outside the immutable CAD model. +SAFE_LOWER_RAD = ( + 0.0, -0.94, -0.8272860654453121, -1.29, + -0.26, 0.0, 0.0, -0.26, 0.0, 0.0, 0.0, 0.0, +) +SAFE_UPPER_RAD = ( + 0.73, 0.0, 0.0, 0.0, + 0.26, 1.33, 1.48, 0.26, 1.33, 1.71, 1.38, 1.38, +) + +SDK_TO_URDF_JOINT: tuple[str, ...] = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "index_mcp_roll", + "index_mcp_pitch", + "index_pip", + "middle_mcp_roll", + "middle_mcp_pitch", + "middle_pip", + "ring_mcp_pitch", + "pinky_mcp_pitch", +) + +ACTIVE_JOINTS = SDK_TO_URDF_JOINT +PASSIVE_JOINTS: tuple[str, ...] = ( + "thumb_dip", + "index_dip", + "middle_dip", + "ring_pip", + "ring_dip", + "pinky_pip", + "pinky_dip", +) +CALIBRATED_ACTIVE_JOINTS = frozenset(ACTIVE_JOINTS) - {"ring_mcp_pitch"} +MEASURED_PASSIVE_JOINTS = frozenset( + {"thumb_dip", "index_dip", "middle_dip", "pinky_pip", "pinky_dip"} +) +TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = { + "ring_mcp_pitch": "pinky_mcp_pitch", +} +MIMIC_SOURCE_BY_JOINT = { + "thumb_dip": "thumb_mcp", + "index_dip": "index_pip", + "middle_dip": "middle_pip", + "ring_pip": "ring_mcp_pitch", + "ring_dip": "ring_pip", + "pinky_pip": "pinky_mcp_pitch", + "pinky_dip": "pinky_pip", +} +COMMAND_INDEX_BY_JOINT = { + joint: index for index, joint in enumerate(SDK_TO_URDF_JOINT) +} + +PARK_FINGER_RAD = 0.65 * 1.38 +PARK_MIDDLE_MCP_RAD = 0.65 * 1.33 +PARK_MIDDLE_PIP_RAD = 0.65 * 1.71 +INDEX_CLEARANCE_RAD = -math.radians(10.0) + + +def _task( + key: str, + view: str, + index: int, + joints: tuple[str, ...], + *, + start: float, + end: float, + speed: float, + auxiliary: tuple[tuple[int, float], ...] = (), +) -> TaskSpec: + return TaskSpec( + key, + view, + index, + joints, + auxiliary_commands=auxiliary, + start=start, + end=end, + preflight_speed=min(speed, 0.02), + formal_speed=speed, + ) + + +def build_typed_profile() -> CalibrationProfile: + middle_clearance = ( + (4, INDEX_CLEARANCE_RAD), + (10, PARK_FINGER_RAD), + (11, PARK_FINGER_RAD), + ) + index_clearance = ( + (7, 0.0), + (8, PARK_MIDDLE_MCP_RAD), + (9, PARK_MIDDLE_PIP_RAD), + (10, PARK_FINGER_RAD), + (11, PARK_FINGER_RAD), + ) + tasks = ( + _task("thumb_pitch_front", "front", 2, ("thumb_cmc_pitch",), start=0.0, end=SAFE_LOWER_RAD[2], speed=0.03), + _task("thumb_roll_front", "front", 0, ("thumb_cmc_roll",), start=0.0, end=SAFE_UPPER_RAD[0], speed=0.04), + _task("thumb_mcp_dip_front", "front", 3, ("thumb_mcp", "thumb_dip"), start=0.0, end=SAFE_LOWER_RAD[3], speed=0.08), + _task("thumb_yaw_top", "top", 1, ("thumb_cmc_yaw",), start=0.0, end=SAFE_LOWER_RAD[1], speed=0.04), + _task("pinky_chain_side", "side", 11, ("pinky_mcp_pitch", "pinky_pip", "pinky_dip"), start=0.0, end=SAFE_UPPER_RAD[11], speed=0.08), + _task("middle_roll_front", "front", 7, ("middle_mcp_roll",), start=SAFE_UPPER_RAD[7], end=SAFE_LOWER_RAD[7], speed=0.04, auxiliary=middle_clearance), + _task("middle_mcp_side", "side", 8, ("middle_mcp_pitch",), start=0.0, end=SAFE_UPPER_RAD[8], speed=0.08, auxiliary=middle_clearance), + _task("middle_pip_dip_side", "side", 9, ("middle_pip", "middle_dip"), start=0.0, end=SAFE_UPPER_RAD[9], speed=0.08, auxiliary=middle_clearance), + _task("index_roll_front", "front", 4, ("index_mcp_roll",), start=SAFE_UPPER_RAD[4], end=SAFE_LOWER_RAD[4], speed=0.04, auxiliary=index_clearance), + _task("index_mcp_side", "side", 5, ("index_mcp_pitch",), start=0.0, end=SAFE_UPPER_RAD[5], speed=0.08, auxiliary=index_clearance), + _task("index_pip_dip_side", "side", 6, ("index_pip", "index_dip"), start=0.0, end=SAFE_UPPER_RAD[6], speed=0.08, auxiliary=index_clearance), + ) + measurements = { + "thumb_cmc_pitch": MeasurementSpec("thumb_cmc_pitch", "relative_rotation", "front", "front_base", "thumb_cmc"), + "thumb_cmc_roll": MeasurementSpec("thumb_cmc_roll", "relative_rotation", "front", "front_base", "thumb_cmc"), + "thumb_mcp": MeasurementSpec("thumb_mcp", "relative_rotation", "front", "thumb_cmc", "thumb_mcp"), + "thumb_dip": MeasurementSpec("thumb_dip", "relative_rotation", "front", "thumb_mcp", "thumb_dip", pose_axis_line_required=False), + "thumb_cmc_yaw": MeasurementSpec("thumb_cmc_yaw", "relative_rotation", "top", "top_base", "thumb_yaw"), + "pinky_mcp_pitch": MeasurementSpec("pinky_mcp_pitch", "relative_rotation", "side", "side_base", "pinky_mcp"), + "pinky_pip": MeasurementSpec("pinky_pip", "relative_rotation", "side", "pinky_mcp", "pinky_pip", pose_axis_line_required=False), + "pinky_dip": MeasurementSpec("pinky_dip", "relative_rotation", "side", "pinky_pip", "pinky_dip", pose_axis_line_required=False), + "middle_mcp_roll": MeasurementSpec("middle_mcp_roll", "relative_rotation", "front", "front_base", "middle_roll"), + "middle_mcp_pitch": MeasurementSpec("middle_mcp_pitch", "relative_rotation", "side", "side_base", "middle_pip"), + "middle_pip": MeasurementSpec("middle_pip", "relative_rotation", "side", "side_base", "middle_pip"), + "middle_dip": MeasurementSpec("middle_dip", "relative_rotation", "side", "middle_pip", "middle_dip", pose_axis_line_required=False), + "index_mcp_roll": MeasurementSpec("index_mcp_roll", "relative_rotation", "front", "front_base", "index_roll"), + "index_mcp_pitch": MeasurementSpec("index_mcp_pitch", "relative_rotation", "side", "side_base", "index_pip"), + "index_pip": MeasurementSpec("index_pip", "relative_rotation", "side", "side_base", "index_pip"), + "index_dip": MeasurementSpec("index_dip", "relative_rotation", "side", "index_pip", "index_dip", pose_axis_line_required=False), + } + active = frozenset(ACTIVE_JOINTS) + passive = frozenset(PASSIVE_JOINTS) + coverage = { + **{ + name: ( + "transferred_static_dynamic" + if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT + else "measured_static_dynamic" + ) + for name in active + }, + **{ + name: ( + "measured_dynamic_cad_static" + if name in MEASURED_PASSIVE_JOINTS + else "mimic_nominal" + ) + for name in passive + }, + } + return CalibrationProfile( + key=KEY, + namespace="/o12_calibration", + command=CommandLayout( + names=COMMAND_NAMES, + baseline_u8=(), + baseline=(0.0,) * 12, + # The public command domain is the reviewed SDK/CAD intersection. + lower_bounds=SAFE_LOWER_RAD, + upper_bounds=SAFE_UPPER_RAD, + unit="rad", + feedback_by_index=True, + command_index_by_joint=COMMAND_INDEX_BY_JOINT, + urdf_joint_by_joint={name: name for name in ACTIVE_JOINTS}, + ), + vision=VisionRigSpec( + views=( + ViewSpec("front", ( + TagSpec("front_base", 0, True), TagSpec("thumb_cmc", 1), + TagSpec("thumb_mcp", 2), TagSpec("thumb_dip", 3), + TagSpec("middle_roll", 12), TagSpec("index_roll", 13), + )), + ViewSpec("side", ( + TagSpec("side_base", 4, True), TagSpec("pinky_mcp", 5), + TagSpec("pinky_pip", 6), TagSpec("pinky_dip", 7), + TagSpec("middle_pip", 8), TagSpec("middle_dip", 9), + TagSpec("index_pip", 10), TagSpec("index_dip", 11), + )), + ViewSpec("top", ( + TagSpec("top_base", 14, True), TagSpec("thumb_yaw", 15), + )), + ), + common_frame="calibration_common", + extrinsic_reference_view="front", + 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=True, + steady_command_checkpoints=True, + speed_parameters={ + "command_rate_hz": 20.0, + "clearance_flex_rad_s": 0.10, + "clearance_splay_rad_s": 0.04, + "probe_travel_rad": math.radians(3.0), + "probe_speed_rad_s": 0.02, + "endpoint_hold_seconds": 1.0, + "stall_timeout_seconds": 2.0, + }, + ), + measurement=MeasurementPolicy(measurements=measurements, directional_zero=True), + zero=ZeroSolvePolicy( + active_joints=active, + passive_joints=passive, + direct_zero_joints=tuple(sorted(CALIBRATED_ACTIVE_JOINTS)), + axis_joints=tuple(sorted(CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS)), + mechanical_endpoint_joints=frozenset(CALIBRATED_ACTIVE_JOINTS), + post_solve_endpoint_joints=frozenset(), + mimic_source_by_joint=MIMIC_SOURCE_BY_JOINT, + cad_frozen_joints=passive, + endpoint_anchor_by_joint={name: "lower_at_start" for name in CALIBRATED_ACTIVE_JOINTS}, + fitted_mimic_joints=MEASURED_PASSIVE_JOINTS, + coupling_model_by_joint={name: "quadratic_runtime" for name in MEASURED_PASSIVE_JOINTS}, + ), + quality=QualityPolicy( + training_cycles=(0, 1, 2), + holdout_cycle=3, + hard_threshold_keys=frozenset({ + "minimum_detection_rate", "maximum_state_image_skew_ms", + "maximum_hysteresis_rad", "maximum_validation_error_rad", + "maximum_mimic_residual_rad", + }), + isolated_holdout=True, + ), + scope=ScopePolicy( + calibrate_joints={"full": active}, + frozen_joints={"full": frozenset()}, + default_scope="full", + ), + artifacts=ArtifactPolicy( + output_schema_version=7, + calibration_filename="o12_right_{serial_number}_calibration.json", + corrected_urdf_filename="linkerhand_o12_right_{serial_number}_zero_calibrated.urdf", + protected_input_fields=frozenset({ + "source_urdf_sha256", "camera_extrinsics_sha256", + "calibration_config_sha256", "tag_config_sha256", "sdk_config_sha256", + }), + publication_pointer="latest_passed", + session_compatibility_tokens=frozenset({"o12_right_16_v1", "feedback_rad_v1"}), + publish_corrected_urdf=True, + ), + joint_coverage=coverage, + ) + + +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 build_profile() -> RegisteredProfile: + typed = build_typed_profile() + return RegisteredProfile( + typed, + EngineBindings( + hand_profile=typed, + zero_profile=typed.zero, + 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, + ), + ) + + +__all__ = [ + "ACTIVE_JOINTS", "CALIBRATED_ACTIVE_JOINTS", "COMMAND_INDEX_BY_JOINT", + "COMMAND_NAMES", "INDEX_CLEARANCE_RAD", "KEY", "MEASURED_PASSIVE_JOINTS", + "MIMIC_SOURCE_BY_JOINT", "PARK_FINGER_RAD", "PARK_MIDDLE_MCP_RAD", + "PARK_MIDDLE_PIP_RAD", "PASSIVE_JOINTS", "SAFE_LOWER_RAD", "SAFE_UPPER_RAD", + "SDK_LOWER_RAD", "SDK_TO_URDF_JOINT", "SDK_UPPER_RAD", + "TRANSFERRED_ACTIVE_SOURCE_BY_JOINT", "build_profile", "build_typed_profile", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py new file mode 100644 index 0000000..deed742 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py @@ -0,0 +1,256 @@ +"""One-command O12 right runner with vendor Jazzy overlay loading.""" + +from __future__ import annotations + +import argparse +from datetime import datetime +import json +import os +from pathlib import Path +import subprocess +import time +from typing import Any + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from ...product import ProductConfig, load_product_config +from ..l6.runner import ( + _ProgressConsole, + _l6_reason_zh, + _launch_command, + _stop_stack, + _wait_until, + render_six_channel_progress_zh, +) +from .pipeline import finalize_o12_session, load_o12_raw_samples + + +_TASK_LABELS = { + "thumb_pitch_front": "拇指 CMC pitch（正面 ID0→ID1）", + "thumb_roll_front": "拇指 CMC roll（正面 ID0→ID1）", + "thumb_mcp_dip_front": "拇指 MCP / 被动 DIP（正面 ID1→ID2→ID3）", + "thumb_yaw_top": "拇指 CMC yaw（顶部 ID14→ID15）", + "pinky_chain_side": "小指 MCP / 被动 PIP/DIP（侧面 ID4→ID5→ID6→ID7）", + "middle_roll_front": "中指 MCP roll（正面 ID0→ID12）", + "middle_mcp_side": "中指 MCP pitch（侧面 ID4→ID8）", + "middle_pip_dip_side": "中指 PIP / 被动 DIP（侧面 ID8→ID9）", + "index_roll_front": "食指 MCP roll（正面 ID0→ID13）", + "index_mcp_side": "食指 MCP pitch（侧面 ID4→ID10）", + "index_pip_dip_side": "食指 PIP / 被动 DIP（侧面 ID10→ID11）", +} + + +def _o12_reason_zh(status): + reason = str(status.get("reason", "")) + if reason.startswith("mapping_preflight_no_target_motion:"): + task = reason.split(":", 1)[1] + return ( + "O12-MAPPING-301", + f"固定 SDK 映射点动时没有观察到目标关节运动：{task}。", + "检查对应 Tag、通道映射和机械连接；不要交换通道后强行继续。", + ) + if reason.startswith("o12_error_report_nonzero"): + return ( + "O12-HEALTH-201", + f"O12 返回非零错误码：{status.get('latest_error_codes', [])}。", + "检查堵转、过流、过热或通信异常，排除后重新启动。", + ) + return _l6_reason_zh(status, model_name="O12") + + +def render_o12_progress_zh(status, estimator=None) -> str: + """Render O12 in the same operator-oriented layout as G20 and L6.""" + text = render_six_channel_progress_zh( + status, + task_labels=_TASK_LABELS, + reason_renderer=_o12_reason_zh, + estimator=estimator, + ) + temperature = ( + "直接温度回读" + if status.get("temperature_report_verified") + else "错误码 bit1 过热保护" + if status.get("temperature_fallback_active") + else "等待温度能力确认" + ) + return text + ( + "\nO12安全：POSITION=" + f"{bool(status.get('position_mode_verified'))}；" + f"错误码通道={bool(status.get('error_report_verified'))}；" + f"温度策略={temperature}；速度倍率=" + f"{float(status.get('motion_speed_scale', 1.0)):.1f}x" + ) + + +class _Monitor(Node): + def __init__(self, progress: _ProgressConsole) -> None: + super().__init__("o12_calibration_runner") + self.status: dict[str, Any] = {} + self.progress = progress + self.create_subscription(String, "/o12_calibration/status", self._status, 10) + self.start_client = self.create_client(Trigger, "/o12_calibration/start") + self.abort_client = self.create_client(Trigger, "/o12_calibration/abort") + + def _status(self, message: String) -> None: + try: + value = json.loads(message.data) + except json.JSONDecodeError: + return + if isinstance(value, dict): + self.status = value + self.progress.update(value) + + +def _overlay_environment(setup: Path) -> dict[str, str]: + completed = subprocess.run( + ["bash", "-c", 'source "$1" >/dev/null 2>&1; env -0', "bash", str(setup)], + check=True, + stdout=subprocess.PIPE, + ) + environment = dict(os.environ) + for item in completed.stdout.split(b"\0"): + if b"=" in item: + key, value = item.split(b"=", 1) + environment[key.decode()] = value.decode(errors="surrogateescape") + return environment + + +def _run_online( + config: ProductConfig, *, record_bag: bool, commands_enabled: bool +) -> int: + if config.sdk_setup is None or config.sdk_config is None: + raise ValueError("O12 vendor SDK overlay/config are required") + session = config.session_root / datetime.now().strftime("%Y%m%d_%H%M%S") + while session.exists(): + time.sleep(1.0) + session = config.session_root / datetime.now().strftime("%Y%m%d_%H%M%S") + session.mkdir(parents=True) + log_path = session / "calibration.log" + log_stream = log_path.open("a", encoding="utf-8", buffering=1) + print(f"O12 标定环境正在启动；日志：{log_path}", flush=True) + process = subprocess.Popen( + _launch_command( + config, session, record_bag=record_bag, + commands_enabled=commands_enabled, + ), + cwd=config.workspace, + env=_overlay_environment(config.sdk_setup), + stdout=log_stream, + stderr=subprocess.STDOUT, + text=True, + start_new_session=True, + ) + rclpy.init() + monitor = _Monitor(_ProgressConsole(renderer=render_o12_progress_zh)) + try: + ready = _wait_until( + monitor, process, + lambda status: status.get("state") in {"READY", "PAUSED", "ABORTED"}, + timeout=120.0, + ) + if not ready or monitor.status.get("state") != "READY": + print( + "O12 启动预检失败：请检查 HCAN、POSITION 模式、错误/温度回读、" + f"12路反馈和三相机。日志：{log_path}", flush=True, + ) + return 2 + if not monitor.start_client.wait_for_service(timeout_sec=10.0): + print("O12 标定 /start 服务不可用。", flush=True) + return 2 + future = monitor.start_client.call_async(Trigger.Request()) + while rclpy.ok() and not future.done(): + rclpy.spin_once(monitor, timeout_sec=0.2) + response = future.result() + if response is None or not response.success: + print(f"O12 标定未启动：{getattr(response, 'message', '')}", flush=True) + return 2 + print("O12 标定已自动开始：POSITION，20 Hz，弧度余弦轨迹。", flush=True) + finished = _wait_until( + monitor, process, + lambda status: status.get("state") in {"PASSED", "PAUSED", "ABORTED"}, + timeout=None, + ) + if not finished or monitor.status.get("state") != "PASSED": + print( + "O12 标定已安全停止并保持当前位置：" + + str(monitor.status.get("reason", "process_exit")), + flush=True, + ) + return 3 + print("\n".join(( + "PASS：O12 右手 11 个实测任务和独立 holdout 已通过。", + f"发布结果：{config.session_root / 'latest_passed'}", + f"JSON：{monitor.status.get('final_json')}", + f"URDF：{monitor.status.get('final_urdf')}", + )), flush=True) + return 0 + except KeyboardInterrupt: + if monitor.abort_client.wait_for_service(timeout_sec=2.0): + monitor.abort_client.call_async(Trigger.Request()) + rclpy.spin_once(monitor, timeout_sec=1.0) + return 130 + finally: + monitor.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + _stop_stack(process) + log_stream.flush() + os.fsync(log_stream.fileno()) + log_stream.close() + + +def main(args: list[str] | None = None) -> None: + parser = argparse.ArgumentParser(description="O12 right 16-Tag calibration") + parser.add_argument("--config", required=True) + parser.add_argument("--workspace", default=None) + parser.add_argument("--record-bag", action="store_true") + parser.add_argument("--commands-disabled", action="store_true") + parser.add_argument("--validate-only", action="store_true") + parser.add_argument("--offline-raw", default="") + parser.add_argument("--offline-output", default="") + parser.add_argument("--publish-offline", action="store_true") + selected = parser.parse_args(args) + config = load_product_config( + selected.config, + workspace=selected.workspace, + check_can=False, + ) + if config.profile_key.profile_id != "O12/right/o12_right_16/v1": + raise ValueError("O12 runner received a different product profile") + if selected.validate_only: + print(f"配置有效：{config.profile_key.profile_id}，SDK {config.sdk_config_sha256}") + return + if selected.offline_raw: + output = ( + Path(selected.offline_output).expanduser().resolve() + if selected.offline_output else config.session_root / + (datetime.now().strftime("%Y%m%d_%H%M%S") + "_offline") + ) + output.mkdir(parents=True, exist_ok=False) + payload, _fit, correction = finalize_o12_session( + session_dir=output, + serial_number=config.serial_number, + source_urdf=config.source_urdf, + protected_inputs={ + "source_urdf_sha256": config.source_urdf_sha256, + "camera_extrinsics_sha256": config.camera_extrinsics_sha256, + "calibration_config_sha256": config.calibration_config_sha256, + "tag_config_sha256": config.tag_config_sha256, + "sdk_config_sha256": config.sdk_config_sha256, + }, + records=load_o12_raw_samples(selected.offline_raw), + publish=selected.publish_offline, + ) + print(f"离线回放PASS：schema {payload['schema_version']}，URDF {correction.path}") + return + raise SystemExit(_run_online( + config, + record_bag=selected.record_bag, + commands_enabled=not selected.commands_disabled, + )) + + +__all__ = ["main", "render_o12_progress_zh"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py new file mode 100644 index 0000000..e81079d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py @@ -0,0 +1,140 @@ +"""Auditable O12 URDF correction from radian-domain calibration results.""" + +from __future__ import annotations + +from dataclasses import dataclass +from datetime import datetime +import math +from pathlib import Path +import re +from typing import Mapping +import xml.etree.ElementTree as ET + +from ...core.urdf import UrdfJointPatch, UrdfPatchSet, write_urdf_patches +from .fitting import O12FitResult +from .profile import ( + CALIBRATED_ACTIVE_JOINTS, + MEASURED_PASSIVE_JOINTS, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, +) + + +SPLAY_JOINTS = frozenset( + {"index_mcp_roll", "middle_mcp_roll"} +) + + +@dataclass(frozen=True) +class O12UrdfCorrection: + path: Path + corrected_limits_rad: Mapping[str, tuple[float, float]] + mimic_multipliers: Mapping[str, float] + preserved_ring_fields: tuple[str, ...] + + +def write_o12_corrected_urdf( + *, + source_urdf: str | Path, + output_directory: str | Path, + serial_number: str, + result: O12FitResult, + timestamp: str | None = None, +) -> O12UrdfCorrection: + """Patch only measured travel and dynamic mimic terms. + + Tag mounting angle cannot be separated from a static passive-joint zero, + so origins and passive limits stay byte-for-byte CAD. The unobserved ring + keeps its own origin, limits and both CAD mimic ratios. + """ + source = Path(source_urdf).expanduser().resolve() + if not source.is_file(): + raise ValueError(f"source URDF does not exist: {source}") + if "calibrated" in source.stem.lower(): + raise ValueError("O12 source URDF must be immutable original CAD") + if set(result.travels_rad) != ( + CALIBRATED_ACTIVE_JOINTS | set(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT) + ): + raise ValueError("O12 travel result has the wrong active joint set") + if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS: + raise ValueError("O12 mimic result has the wrong passive joint set") + + root = ET.parse(source).getroot() + joints = { + str(node.get("name")): node + for node in root.findall("joint") + if node.get("type") == "revolute" + } + required = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS | { + "ring_mcp_pitch", "ring_pip", "ring_dip" + } + if missing := required - set(joints): + raise ValueError("source URDF is missing O12 joints: " + ",".join(sorted(missing))) + + corrected_limits: dict[str, tuple[float, float]] = {} + patches: dict[str, UrdfJointPatch] = {} + for name in sorted(CALIBRATED_ACTIVE_JOINTS): + limit = joints[name].find("limit") + if limit is None: + raise ValueError(f"O12 source active joint has no limit: {name}") + cad_lower = float(limit.get("lower", "nan")) + cad_upper = float(limit.get("upper", "nan")) + travel = abs(float(result.travels_rad[name])) + if not math.isfinite(travel) or travel <= math.radians(2.0): + raise ValueError(f"invalid measured O12 travel: {name}") + if name in SPLAY_JOINTS: + half = min(0.5 * travel, abs(cad_lower), abs(cad_upper)) + lower, upper = -half, half + else: + lower, upper = max(0.0, cad_lower), min(cad_upper, travel) + if not lower < upper: + raise ValueError(f"invalid corrected O12 limits: {name}") + corrected_limits[name] = (lower, upper) + patches[name] = UrdfJointPatch( + limit_lower=f"{lower:.15g}", limit_upper=f"{upper:.15g}" + ) + + mimic_multipliers: dict[str, float] = {} + for name in sorted(MEASURED_PASSIVE_JOINTS): + mimic = joints[name].find("mimic") + if mimic is None: + raise ValueError(f"O12 passive joint has no mimic element: {name}") + multiplier = float(result.mimic_fits[name].urdf_mimic_multiplier) + if not math.isfinite(multiplier) or not 0.5 <= multiplier <= 2.2: + raise ValueError(f"invalid O12 mimic multiplier: {name}") + mimic_multipliers[name] = multiplier + patches[name] = UrdfJointPatch( + mimic_multiplier=f"{multiplier:.15g}" + ) + + stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") + if re.fullmatch(r"\d{8}_\d{6}", stamp) is None: + raise ValueError("O12 URDF timestamp must use YYYYMMDD_HHMMSS") + safe_serial = "".join( + char if char.isalnum() or char in "_.-" else "_" + for char in str(serial_number) + ) + if not safe_serial: + raise ValueError("serial number must not be empty") + output = Path(output_directory).expanduser().resolve() + output.mkdir(parents=True, exist_ok=True) + destination = output / f"{source.stem}_calibrated_{safe_serial}_{stamp}.urdf" + write_urdf_patches( + source_urdf=source, + destination_urdf=destination, + patches=UrdfPatchSet(joints=patches), + forbidden_source_stem_patterns=(r"calibrated",), + copy_complete_mesh_directory=True, + ) + return O12UrdfCorrection( + path=destination, + corrected_limits_rad=corrected_limits, + mimic_multipliers=mimic_multipliers, + preserved_ring_fields=( + "ring_mcp_pitch.origin", "ring_mcp_pitch.limit", + "ring_pip.origin", "ring_pip.limit", "ring_pip.mimic", + "ring_dip.origin", "ring_dip.limit", "ring_dip.mimic", + ), + ) + + +__all__ = ["O12UrdfCorrection", "write_o12_corrected_urdf"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py index c035548..79e020e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py @@ -79,10 +79,12 @@ def get_default_registry() -> ProfileRegistry: from .g20 import register_profiles from .l6 import register_profiles as register_l6_profiles from .o6 import register_profiles as register_o6_profiles + from .o12 import register_profiles as register_o12_profiles registry = ProfileRegistry() register_profiles(registry) register_l6_profiles(registry) register_o6_profiles(registry) + register_o12_profiles(registry) _DEFAULT_REGISTRY = registry return _DEFAULT_REGISTRY diff --git a/src/linkerhand_calibration/linkerhand_calibration/product.py b/src/linkerhand_calibration/linkerhand_calibration/product.py index 77d8934..3865cb9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/product.py +++ b/src/linkerhand_calibration/linkerhand_calibration/product.py @@ -213,6 +213,11 @@ class ProductConfig: cameras: Mapping[str, Mapping[str, str]] required_independent_passes: int static_repeatability_rad: float + sdk_driver: str = "linker_hand_sdk" + sdk_transport: str = "socketcan" + sdk_setup: Path | None = None + sdk_config: Path | None = None + sdk_config_sha256: str = "" @property def session_root(self) -> Path: @@ -247,11 +252,39 @@ def load_product_config( ).strip() if not namespace.startswith("/") or namespace.endswith("/"): raise ValueError("namespace must be an absolute ROS namespace") + sdk_raw = _mapping(raw.get("sdk", {}), "sdk") + sdk_driver = str(sdk_raw.get("driver", "linker_hand_sdk")).strip() + sdk_transport = str(sdk_raw.get("transport", "socketcan")).strip().lower() can_interface = str(raw.get("can_interface", "")).strip() - if not can_interface: - raise ValueError("can_interface is required") - if check_can and not (Path("/sys/class/net") / can_interface).exists(): - raise ValueError(f"CAN interface does not exist: {can_interface}") + if sdk_transport == "socketcan": + if not can_interface: + raise ValueError("can_interface is required for SocketCAN products") + if check_can and not (Path("/sys/class/net") / can_interface).exists(): + raise ValueError(f"CAN interface does not exist: {can_interface}") + elif sdk_transport != "hcan": + raise ValueError("sdk.transport must be socketcan or hcan") + + sdk_setup: Path | None = None + sdk_config: Path | None = None + sdk_config_hash = "" + if sdk_transport == "hcan": + sdk_setup = _resolve_path( + sdk_raw.get("setup"), workspace=root, name="sdk.setup" + ) + sdk_config = _resolve_path( + sdk_raw.get("config"), workspace=root, name="sdk.config" + ) + if not sdk_setup.is_file() or not sdk_config.is_file(): + raise ValueError("vendor SDK overlay and config must exist") + sdk_config_hash = str(sdk_raw.get("config_sha256", "")).lower() + if re.fullmatch(r"[0-9a-f]{64}", sdk_config_hash) is None: + raise ValueError("SDK config expected SHA-256 is invalid") + actual_sdk_hash = sha256_file(sdk_config) + if actual_sdk_hash != sdk_config_hash: + raise ValueError( + "SDK config SHA-256 mismatch: " + f"expected={sdk_config_hash} actual={actual_sdk_hash}" + ) artifacts = _mapping(raw.get("artifacts"), "artifacts") source_urdf = _resolve_path( @@ -389,4 +422,9 @@ def load_product_config( cameras=cameras, required_independent_passes=passes, static_repeatability_rad=repeatability_deg * 3.141592653589793 / 180.0, + sdk_driver=sdk_driver, + sdk_transport=sdk_transport, + sdk_setup=sdk_setup, + sdk_config=sdk_config, + sdk_config_sha256=sdk_config_hash, ) diff --git a/src/linkerhand_calibration/test/test_architecture.py b/src/linkerhand_calibration/test/test_architecture.py index 048183e..56bd721 100644 --- a/src/linkerhand_calibration/test/test_architecture.py +++ b/src/linkerhand_calibration/test/test_architecture.py @@ -145,7 +145,7 @@ def test_runtime_has_no_concrete_model_or_view_assumption() -> None: def test_every_registered_profile_passes_static_integrity_checks() -> None: registry = get_default_registry() - assert len(registry) == 7 + assert len(registry) == 8 for registered in registry: validate_profile(registered.profile) assert registered.profile.zero.active_joints diff --git a/src/linkerhand_calibration/test/test_o12_right_profile.py b/src/linkerhand_calibration/test/test_o12_right_profile.py new file mode 100644 index 0000000..309eec1 --- /dev/null +++ b/src/linkerhand_calibration/test/test_o12_right_profile.py @@ -0,0 +1,284 @@ +from __future__ import annotations + +import math +from pathlib import Path +from types import SimpleNamespace +import xml.etree.ElementTree as ET + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.calibrated_joint_state_bridge import CalibratedCommandMapper +from linkerhand_calibration.models.o12.artifacts import build_o12_runtime_payload +from linkerhand_calibration.models.o12.fitting import fit_o12_session +from linkerhand_calibration.models.o12.motion import cosine_position_trajectory_rad +from linkerhand_calibration.models.o12.node import O12ThreeCameraCalibrationNode +from linkerhand_calibration.models.o12.runner import render_o12_progress_zh +from linkerhand_calibration.models.l6.runner import _launch_command +from linkerhand_calibration.models.o12.profile import ( + CALIBRATED_ACTIVE_JOINTS, + COMMAND_NAMES, + MEASURED_PASSIVE_JOINTS, + PARK_FINGER_RAD, + SDK_TO_URDF_JOINT, + build_typed_profile, +) +from linkerhand_calibration.models.o12.urdf import write_o12_corrected_urdf + + +ROOT = Path(__file__).resolve().parents[1] +SOURCE_URDF = ROOT / "urdf/o12_right/linkerhand_o12_t3_right-0703.urdf" + + +def _synthetic_records(): + profile = build_typed_profile() + task_by_joint = { + joint: task for task in profile.motion.tasks for joint in task.joints + } + ratios = { + "thumb_dip": 1.01, + "index_dip": 0.88, + "middle_dip": 0.77, + "pinky_pip": 1.11, + "pinky_dip": 0.95, + } + source = { + "thumb_dip": "thumb_mcp", + "index_dip": "index_pip", + "middle_dip": "middle_pip", + "pinky_pip": "pinky_mcp_pitch", + "pinky_dip": "pinky_pip", + } + travel: dict[str, float] = {} + for name in CALIBRATED_ACTIVE_JOINTS: + task = task_by_joint[name] + travel[name] = abs(task.end_value - task.start_value) + travel["pinky_pip"] = ratios["pinky_pip"] * travel["pinky_mcp_pitch"] + travel["pinky_dip"] = ratios["pinky_dip"] * travel["pinky_pip"] + for name in ("thumb_dip", "index_dip", "middle_dip"): + travel[name] = ratios[name] * travel[source[name]] + result = {name: [] for name in CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS} + for name, rows in result.items(): + task = task_by_joint[name] + for cycle in range(4): + for direction, phases in ( + ("decreasing", np.linspace(0.0, 1.0, 65)), + ("increasing", np.linspace(1.0, 0.0, 65)), + ): + for phase in phases: + feedback = task.start_value + phase * (task.end_value - task.start_value) + angle = phase * travel[name] + rows.append({ + "cycle": cycle, + "direction": direction, + "feedback_rad": float(feedback), + "relative_quaternion_xyzw": Rotation.from_rotvec( + [0.0, angle, 0.0] + ).as_quat().tolist(), + }) + return result + + +def test_o12_fixed_mapping_tags_and_radian_domain() -> None: + profile = build_typed_profile() + assert profile.key.profile_id == "O12/right/o12_right_16/v1" + assert profile.command.unit == "rad" + assert profile.command.feedback_by_index is True + assert profile.command.names == COMMAND_NAMES + assert SDK_TO_URDF_JOINT[:4] == ( + "thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch", "thumb_mcp" + ) + assert profile.vision.tag_ids == frozenset(range(16)) + assert len(profile.motion.tasks) == 11 + assert [task.formal_speed for task in profile.motion.tasks] == [ + 0.03, 0.04, 0.08, 0.04, 0.08, 0.04, 0.08, 0.08, 0.04, 0.08, 0.08 + ] + + +def test_o12_cosine_speed_is_bounded() -> None: + speed = 0.03 + _, _, duration = cosine_position_trajectory_rad(0.0, -0.8, 0.0, speed) + samples = np.asarray([ + cosine_position_trajectory_rad(0.0, -0.8, t, speed)[0] + for t in np.linspace(0.0, duration, 2001) + ]) + measured = np.max(np.abs(np.diff(samples))) / (duration / 2000.0) + assert measured <= speed + 1.0e-6 + + +def test_o12_hcan_launch_does_not_emit_empty_socketcan_argument(tmp_path: Path) -> None: + profile = build_typed_profile() + config = SimpleNamespace( + model="O12", side="right", tag_layout="o12_right_16", + serial_number="O12_TEST", can_interface="", source_urdf=SOURCE_URDF, + source_urdf_sha256="a" * 64, + camera_extrinsics=tmp_path / "extrinsics.yaml", + camera_extrinsics_sha256="b" * 64, + calibration_config=tmp_path / "calibration.yaml", + calibration_config_sha256="c" * 64, + tag_config=tmp_path / "tags.yaml", tag_config_sha256="d" * 64, + sdk_config=tmp_path / "sdk.yaml", sdk_config_sha256="e" * 64, + output_root=tmp_path, cameras={ + view: { + "serial_number": f"camera-{view}", + "camera_name": f"camera_{view}", + "camera_info": str(tmp_path / f"{view}.yaml"), + } + for view in ("front", "side", "top") + }, + calibration_contract=SimpleNamespace(typed_profile=profile), + ) + command = _launch_command( + config, tmp_path / "session", record_bag=False, commands_enabled=False + ) + assert not any(item.startswith("can_interface:=") for item in command) + + +def test_o12_vendor_node_is_launched_in_documented_namespace() -> None: + launch_text = (ROOT / "launch/three_camera_calibration.launch.py").read_text() + vendor = launch_text.split('package="omnihand_node"', 1)[1].split(")", 1)[0] + assert 'executable="omnihand_pro_2025_node"' in vendor + assert 'namespace="o12"' in vendor + + +def test_o12_temperature_fallback_requires_verified_error_channel(tmp_path: Path) -> None: + warnings: list[str] = [] + fake = SimpleNamespace( + temperature_report_required=False, + temperature_verified=False, + temperature_fallback_active=False, + error_verified=False, + health_check_started_at=10.0, + temperature_fallback_after_seconds=5.0, + raw_path=tmp_path / "raw.jsonl", + protected_inputs={"sdk_config_sha256": "a" * 64}, + get_logger=lambda: SimpleNamespace(warning=warnings.append), + ) + + O12ThreeCameraCalibrationNode._activate_temperature_fallback_if_allowed( + fake, 16.0 + ) + assert not fake.temperature_fallback_active + + fake.error_verified = True + O12ThreeCameraCalibrationNode._activate_temperature_fallback_if_allowed( + fake, 16.0 + ) + assert fake.temperature_fallback_active + assert warnings == [ + "O12 temperature report unavailable; continuing with error-code " + "bit1 overheat protection" + ] + raw = (tmp_path / "raw.jsonl").read_text() + assert '"fallback_protection":"joint_error_states_bit1_overheat"' in raw + + +def test_o12_required_temperature_report_disables_fallback(tmp_path: Path) -> None: + fake = SimpleNamespace( + temperature_report_required=True, + temperature_verified=False, + temperature_fallback_active=False, + error_verified=True, + health_check_started_at=10.0, + temperature_fallback_after_seconds=5.0, + ) + O12ThreeCameraCalibrationNode._activate_temperature_fallback_if_allowed( + fake, 30.0 + ) + assert not fake.temperature_fallback_active + + +def test_o12_motion_plan_has_collision_clearance_and_safe_return() -> None: + profile = build_typed_profile() + fake = SimpleNamespace( + profile=profile, + motion_speed_scale=2.0, + _full_target=O12ThreeCameraCalibrationNode._full_target, + ) + steps = O12ThreeCameraCalibrationNode._build_steps(fake) + clearance = [step for step in steps if step.phase.startswith("clearance")] + assert clearance[0].target_command[10:] == (PARK_FINGER_RAD, PARK_FINGER_RAD) + assert clearance[0].speed_u8 == 0.15 + assert clearance[1].target_command[4] == pytest.approx(-math.radians(10.0)) + assert clearance[1].speed_u8 == 0.08 + assert [step.phase for step in steps[-3:]] == [ + "return_splay_zero", "return_middle_open", "return_outer_open" + ] + for task in profile.motion.tasks: + probes = [step for step in steps if step.task_key == task.key and step.phase == "preflight"] + assert max(abs(step.target_u8 - task.start_value) for step in probes) <= math.radians(3.0) + 1e-12 + sweeps = [step for step in steps if step.task_key == task.key and step.phase == "sweep"] + assert {step.speed_u8 for step in sweeps} == {2.0 * task.formal_speed} + + +def test_o12_operator_progress_uses_unified_layout_and_radians() -> None: + text = render_o12_progress_zh({ + "state": "RUNNING", + "serial_number": "O12_TEST", + "step_index": 20, + "step_count": 190, + "step_fraction": 0.5, + "task_name": "thumb_pitch_front", + "phase": "sweep", + "cycle": 1, + "direction": "decreasing", + "attempt": 1, + "command_unit": "rad", + "target_rad": -0.827, + "current_command_u8": -0.400, + "actual_u8": -0.395, + "speed_rad_s": 0.060, + "command_trajectory_duration_seconds": 21.7, + "valid_frames": 80, + "total_frames": 82, + "tag_detection_rate": 80 / 82, + "joint_frame_rate": 80 / 82, + "recognized_tag_ids": [0, 1], + "unrecognized_tag_ids": [], + "feedback_hz": 20.0, + "position_mode_verified": True, + "error_report_verified": True, + "temperature_fallback_active": True, + "motion_speed_scale": 2.0, + }) + assert "[O12_TEST] 标定中" in text + assert "拇指 CMC pitch（正面 ID0→ID1）" in text + assert "阶段：正式扫描（第 2/4 轮）" in text + assert "命令/反馈：-0.400 rad/-0.395 rad" in text + assert "峰值 0.060 rad/s；本段 21.7 秒余弦轨迹" in text + assert "温度策略=错误码 bit1 过热保护；速度倍率=2.0x" in text + + +def test_o12_synthetic_fit_schema7_bridge_and_ring_preservation(tmp_path: Path) -> None: + result = fit_o12_session(SOURCE_URDF, _synthetic_records()) + hashes = {name: "a" * 64 for name in build_typed_profile().artifacts.protected_input_fields} + payload = build_o12_runtime_payload( + serial_number="O12_TEST", + source_urdf=SOURCE_URDF, + result=result, + protected_inputs=hashes, + passed=True, + ) + assert payload["schema_version"] == 7 + assert len(payload["joints"]["thumb_cmc_yaw"]["angle_rad"]) == 65 + assert payload["joints"]["thumb_cmc_roll"]["raw_increasing_curve_branch"] == "decreasing" + assert payload["joints"]["thumb_cmc_yaw"]["raw_increasing_curve_branch"] == "increasing" + assert "command_range" not in payload + assert payload["joints"]["ring_mcp_pitch"]["transferred_from_joint"] == "pinky_mcp_pitch" + mapper = CalibratedCommandMapper(payload) + assert len(mapper.map_positions([0.0] * 12, ["wrong"] * 12)) == 19 + + correction = write_o12_corrected_urdf( + source_urdf=SOURCE_URDF, + output_directory=tmp_path, + serial_number="O12_TEST", + result=result, + timestamp="20260904_120000", + ) + before = ET.parse(SOURCE_URDF).getroot() + after = ET.parse(correction.path).getroot() + by_name_before = {node.get("name"): node for node in before.findall("joint")} + by_name_after = {node.get("name"): node for node in after.findall("joint")} + for name in ("ring_mcp_pitch", "ring_pip", "ring_dip"): + assert ET.tostring(by_name_before[name]) == ET.tostring(by_name_after[name])