From 8de69c34a1a38fc481874ecbb1bfac2ab276bcb0 Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Thu, 3 Sep 2026 10:11:13 +0800 Subject: [PATCH] =?UTF-8?q?o6=E5=8F=B3=E6=89=8B=E6=A0=87=E5=AE=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 4 +- src/linkerhand_calibration/README.md | 52 +- .../config/o6_right_8_tags.yaml | 59 +++ .../config/o6_right_product.yaml | 37 ++ .../config/o6_three_camera_calibration.yaml | 64 +++ .../launch/three_camera_calibration.launch.py | 2 + .../launch/three_camera_extrinsics.launch.py | 10 + .../calibrated_joint_state_bridge.py | 14 +- .../linkerhand_calibration/core/urdf/patch.py | 5 +- .../linkerhand_calibration/extrinsics.py | 28 +- .../linkerhand_calibration/extrinsics_node.py | 41 +- .../linkerhand_calibration/models/__init__.py | 2 + .../models/g20/zero_solver.py | 17 +- .../models/l6/motion.py | 2 +- .../linkerhand_calibration/models/l6/node.py | 227 ++++++--- .../models/l6/runner.py | 85 +++- .../models/o6/__init__.py | 12 + .../models/o6/artifacts.py | 450 +++++++++++++++++ .../models/o6/fitting.py | 339 +++++++++++++ .../linkerhand_calibration/models/o6/node.py | 38 ++ .../models/o6/pipeline.py | 179 +++++++ .../models/o6/profile.py | 322 ++++++++++++ .../models/o6/runner.py | 225 +++++++++ .../linkerhand_calibration/models/o6/urdf.py | 172 +++++++ .../linkerhand_calibration/models/registry.py | 2 + .../models/runtime_schema.py | 25 + .../test/test_architecture.py | 2 +- .../test/test_o6_right_profile.py | 477 ++++++++++++++++++ .../test/test_urdf_zero.py | 28 + 29 files changed, 2819 insertions(+), 101 deletions(-) create mode 100644 src/linkerhand_calibration/config/o6_right_8_tags.yaml create mode 100644 src/linkerhand_calibration/config/o6_right_product.yaml create mode 100644 src/linkerhand_calibration/config/o6_three_camera_calibration.yaml create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o6/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o6/artifacts.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o6/fitting.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o6/node.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o6/pipeline.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o6/profile.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o6/runner.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o6/urdf.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/runtime_schema.py create mode 100644 src/linkerhand_calibration/test/test_o6_right_profile.py diff --git a/.gitignore b/.gitignore index 390ae2a..d28b1cd 100644 --- a/.gitignore +++ b/.gitignore @@ -69,8 +69,8 @@ Thumbs.db *_mapping_quality.json # Device-specific robot descriptions derived from local calibration runs -# Includes both full and partial timestamped zero-calibration outputs. -/src/linkerhand_calibration/urdf/*/*_zero_calibrated_*.urdf +# Includes full/partial zero-calibration outputs and local copies. +/src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index 2bfba48..f447e22 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -1,5 +1,51 @@ # LinkerHand 专业标定包 +## O6 右手局部标定（o6_right_8/v1） + +O6 使用与 L6 相同的三机位八 Tag 观测拓扑，但保留 O6 自己的六通道协议与 +URDF 关节名：正面 ID0/1/2 标定 `rh_thumb_cmc_pitch` 与 +`rh_thumb_ip`，侧面 ID3/4/5 标定 `rh_pinky_mcp_pitch` 与 +`rh_pinky_dip`，上面 ID6/7 标定 `rh_thumb_cmc_yaw`。六路 baseline 均为 +`255`；每次只扫描通道 0、1 或 5，其他通道保持 255。经硬件确认，食指、中指、 +无名指与小指同机构，因此小指 MCP/DIP 的实测结果会以迁移来源标记后用于其余 +三指。O6 实测 IP/DIP 均存在稳定的非线性，因此使用通过独立 holdout 的双向 +运行曲线和二次耦合模型。标准 URDF 无法表示二次 mimic，因此修正 URDF 与 L6 +一样保留端点对齐的线性 ``，使普通 URDF/RViz 中五个被动关节能正常联动， +同时把被动关节 limit 更新为实测范围；中间行程的精确双向非线性轨迹由下述标定桥 +发布。 + +```bash +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/o6_right_product.yaml --validate-only + +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/o6_right_product.yaml +``` + +由于 O6 的 FRONT/TOP 光轴接近正交，同一平面棋盘的同步视角天然更倾斜。O6 外参 +允许最终批次 RMS 不超过 1.5 px，但仍保持 0.3° 旋转、1.5 mm 平移稳定性门限， +并在标定发布前额外用 20 mm 跨机位实体轴线 RMS 粗差门限拦截移动相机等明显错误： + +```bash +ros2 launch linkerhand_calibration three_camera_extrinsics.launch.py \ + output_file:=$PWD/config/o6_three_camera_extrinsics.yaml \ + checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027 \ + maximum_reprojection_rms_px:=1.5 +``` + +结果发布到 `calibration_output//latest_partial_passed`，运行时 JSON、 +修正 URDF 与 correction-input JSON 均使用 `o6_right_` 前缀。产品 YAML 内的实物 +串号、相机身份、外参和四项输入哈希必须在启动硬件前通过校验。 + +标定完成后，以修正 URDF 启动 robot state publisher，并用同一会话中的 JSON 把 +O6 六路反馈转换为 11 个 URDF 关节： + +```bash +ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \ + hand_type:=right \ + calibration_file:=$PWD/calibration_output/O6_RIGHT_001/latest_partial_passed/o6_right_O6_RIGHT_001_partial_calibration.json +``` + ## L6 右手局部标定（l6_right_8/v1） 本版只发布 `rh_thumb_cmc_pitch`、`rh_thumb_cmc_roll`、 @@ -408,11 +454,11 @@ PnP的最终外参偏差锁死采集。点击 `AUTO ON` 后，棋盘稳定1秒 外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中，每改变一次位置和倾角添加 一个候选；随后以相同方法采集正面/上面。程序使用固定内参的 `stereoCalibrate` 联合优化唯一旋转/平移。采集准入和最终验收分离：FRONT和 -配对相机的单帧RMS分别不得超过1.5 px，同时组合RMS不得超过1.2 px；界面中 +配对相机的单帧RMS分别不得超过1.5 px，候选组合RMS不得超过1.5 px；界面中 单相机1.2 px以内显示绿色、1.2～1.5 px显示黄色且仍可采集、超过1.5 px显示红色。 新姿态会与全部已采姿态比较，避免在少数姿态间反复采集。拟合先剔除粗大异常组， -再在不低于15个内点的前提下有界裁剪联合误差最高的候选，最终1.2 px门限不会被 -放宽。两组均得到 +再在不低于15个内点的前提下有界裁剪联合误差最高的候选，只有最终批次组合RMS +不超过1.2 px才允许保存。两组均得到 至少15个内点且联合RMS、三折稳定性合格后 `SAVE` 才变绿。 ```bash diff --git a/src/linkerhand_calibration/config/o6_right_8_tags.yaml b/src/linkerhand_calibration/config/o6_right_8_tags.yaml new file mode 100644 index 0000000..5d06f3e --- /dev/null +++ b/src/linkerhand_calibration/config/o6_right_8_tags.yaml @@ -0,0 +1,59 @@ +/o6_calibration/front/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + max_hamming: 0 + detector: + threads: 4 + decimate: 1.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [0, 1, 2] + frames: [front_base, thumb_pitch, thumb_ip] + sizes: [0.016, 0.016, 0.016] + +/o6_calibration/side/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + max_hamming: 0 + detector: + threads: 4 + decimate: 1.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [3, 4, 5] + frames: [side_base, pinky_pitch, pinky_dip] + sizes: [0.016, 0.016, 0.016] + +/o6_calibration/top/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + max_hamming: 0 + detector: + threads: 4 + decimate: 1.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [6, 7] + frames: [top_base, thumb_yaw] + sizes: [0.016, 0.016] diff --git a/src/linkerhand_calibration/config/o6_right_product.yaml b/src/linkerhand_calibration/config/o6_right_product.yaml new file mode 100644 index 0000000..46b1284 --- /dev/null +++ b/src/linkerhand_calibration/config/o6_right_product.yaml @@ -0,0 +1,37 @@ +schema_version: 2 +profile_id: O6/right/o6_right_8/v1 +model: O6 +side: right +tag_layout: o6_right_8 +namespace: /o6_calibration +serial_number: O6_RIGHT_001 +can_interface: can0 +output_root: calibration_output + +cameras: + front: + serial_number: DB2163742 + camera_name: hikrobot_front_DB2163742 + camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml + side: + serial_number: DB2163749 + camera_name: hikrobot_side_DB2163749 + camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml + top: + serial_number: DB2163739 + camera_name: hikrobot_top_DB2163739 + camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml + +artifacts: + source_urdf: package://linkerhand_calibration/urdf/o6_right/linkerhand_o6_right.urdf + source_urdf_sha256: 8f184faad699fbf771e388f109a4e8793b5cb190c33a87b2eba8491a3a37dd62 + camera_extrinsics: config/o6_three_camera_extrinsics.yaml + camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4 + calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml + calibration_config_sha256: ce20d998a4342dfaacb14568513aa9af5063df48566fabd42180acc8da47e4a6 + tag_config: package://linkerhand_calibration/config/o6_right_8_tags.yaml + tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e + +release: + required_independent_passes: 1 + static_repeatability_deg: 1.0 diff --git a/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml b/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml new file mode 100644 index 0000000..85cef95 --- /dev/null +++ b/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml @@ -0,0 +1,64 @@ +o6_calibration: + ros__parameters: + command_topic: /o6/cb_right_hand_control_cmd + state_topic: /o6/cb_right_hand_state + setting_topic: /o6/cb_hand_setting_cmd + front_camera_info_topic: /o6_calibration/front/camera/camera_info + front_detections_topic: /o6_calibration/front/apriltag/detections + side_camera_info_topic: /o6_calibration/side/camera/camera_info + side_detections_topic: /o6_calibration/side/apriltag/detections + top_camera_info_topic: /o6_calibration/top/camera/camera_info + top_detections_topic: /o6_calibration/top/apriltag/detections + + baseline_command_u8: [255, 255, 255, 255, 255, 255] + # O6 has a different speed scale from L6. Motion is still bounded by the + # six-second cosine command trajectory; these values are firmware limits. + baseline_speed_u8: 80 + preflight_speed_u8: 60 + formal_speed_u8: 40 + speed_settle_seconds: 0.2 + command_trajectory_full_range_seconds: 6.0 + torque_u8: 80 + repetitions: 4 + preflight_checkpoints_u8: [255, 127, 0] + tag_size_m: 0.016 + tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7] + tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016] + + minimum_detection_rate: 0.95 + minimum_joint_frame_rate: 0.85 + minimum_feedback_hz: 25.0 + maximum_state_image_skew_ms: 50.0 + maximum_hamming: 0 + minimum_decision_margin: 30.0 + minimum_edge_pixels: 30.0 + pnp_maximum_reprojection_error_px: 1.5 + pnp_reprojection_tie_px: 1.5 + pnp_maximum_pose_jump_deg: 35.0 + pnp_maximum_translation_jump_m: 0.04 + pnp_maximum_tag_tilt_deg: 75.0 + pnp_tracker_reset_seconds: 5.0 + + minimum_sweep_frames: 40 + minimum_state_span_u8: 240.0 + minimum_sweep_bins: 32 + maximum_bin_gap: 16 + maximum_monotonic_correction_deg: 2.0 + passive_maximum_monotonic_correction_deg: 3.0 + maximum_validation_mae_deg: 1.0 + maximum_validation_p95_deg: 2.0 + maximum_validation_error_deg: 3.0 + mimic_minimum_multiplier: 0.5 + mimic_maximum_multiplier: 2.2 + mimic_maximum_cycle_range: 0.03 + mimic_maximum_residual_p95_deg: 2.0 + + endpoint_tolerance_u8: 2.0 + endpoint_hold_seconds: 1.0 + motor_stall_timeout_seconds: 2.0 + position_timeout_seconds: 30.0 + sweep_timeout_seconds: 90.0 + automatic_sweep_retry_limit: 2 + non_target_motion_tolerance_u8: 3.0 + fixed_base_maximum_corner_drift_px: 2.0 + fixed_base_movement_confirmation_frames: 5 diff --git a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py index ca82c31..3fb36dd 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 "o6_right_8_tags.yaml" + if tag_layout == "o6_right_8" else "l6_right_8_tags.yaml" if tag_layout == "l6_right_8" else "three_camera_tags_g20_right_15.yaml" diff --git a/src/linkerhand_calibration/launch/three_camera_extrinsics.launch.py b/src/linkerhand_calibration/launch/three_camera_extrinsics.launch.py index 4371795..fe01a4c 100644 --- a/src/linkerhand_calibration/launch/three_camera_extrinsics.launch.py +++ b/src/linkerhand_calibration/launch/three_camera_extrinsics.launch.py @@ -112,6 +112,12 @@ def _launch(context): LaunchConfiguration("maximum_reprojection_rms_px"), value_type=float, ), + "maximum_candidate_pair_reprojection_rms_px": ParameterValue( + LaunchConfiguration( + "maximum_candidate_pair_reprojection_rms_px" + ), + value_type=float, + ), "maximum_single_camera_reprojection_rms_px": ParameterValue( LaunchConfiguration( "maximum_single_camera_reprojection_rms_px" @@ -182,6 +188,10 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument( "maximum_reprojection_rms_px", default_value="1.2" ), + DeclareLaunchArgument( + "maximum_candidate_pair_reprojection_rms_px", + default_value="1.5", + ), DeclareLaunchArgument( "maximum_single_camera_reprojection_rms_px", default_value="1.5", 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 c41fbea..13ad306 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py +++ b/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py @@ -26,7 +26,8 @@ from .full_hand import ( infer_compact_payload_layout, validate_compact_payload, ) -from .models.l6.artifacts import validate_l6_runtime_payload +from .models import get_default_registry, validate_schema_v6_runtime_payload +from .core import ProfileKey G20_COMMAND_NAMES: tuple[str, ...] = ( @@ -88,7 +89,7 @@ class CalibratedCommandMapper: ) -> None: schema_version = int(payload["schema_version"]) if schema_version == 6: - validate_l6_runtime_payload(payload) + validate_schema_v6_runtime_payload(payload) else: validate_compact_payload(payload) side = str(payload["side"]).lower() @@ -127,7 +128,12 @@ class CalibratedCommandMapper: name: int(payload["joints"][name]["motor_index"]) for name in self.urdf_joint_names } - self.feedback_name_aliases = {"thumb_cmc_yaw": "thumb_cmc_roll"} + registered = get_default_registry().get( + ProfileKey.parse(self.profile_id) + ) + self.feedback_name_aliases = dict( + registered.profile.command.feedback_name_aliases + ) else: profile = get_hand_calibration_profile(side, layout_id) self.command_names = G20_COMMAND_NAMES @@ -170,7 +176,7 @@ class CalibratedCommandMapper: def _command_index(value: float) -> int: command = float(value) if not math.isfinite(command): - raise ValueError("G20 command positions must be finite") + raise ValueError("calibrated command positions must be finite") return max(0, min(255, int(math.floor(command + 0.5)))) def map_positions( diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py index 8c66ef0..c505dc1 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py @@ -42,6 +42,7 @@ class UrdfJointPatch: ) + @dataclass(frozen=True) class MujocoEqualityPatch: """Replacement and optional topology assertion for one equality joint.""" @@ -61,7 +62,9 @@ class UrdfPatchSet: ) def __post_init__(self) -> None: - empty = [name for name, patch in self.joints.items() if not patch.replacements()] + empty = [ + name for name, patch in self.joints.items() if not patch.replacements() + ] if empty: raise ValueError( "URDF joint patch contains no replacements: " diff --git a/src/linkerhand_calibration/linkerhand_calibration/extrinsics.py b/src/linkerhand_calibration/linkerhand_calibration/extrinsics.py index 7003789..71ad5e5 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/extrinsics.py +++ b/src/linkerhand_calibration/linkerhand_calibration/extrinsics.py @@ -74,13 +74,24 @@ def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None: ) -def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics: +def load_three_camera_extrinsics( + path: str | Path, + *, + quality_limits: Mapping[str, float] | None = None, + minimum_capture_counts: Mapping[str, int] | None = None, +) -> ThreeCameraExtrinsics: generic = load_camera_extrinsics( path, required_views=VIEWS, reference_view="front", - quality_limits=_QUALITY_LIMITS, - minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS, + quality_limits=( + _QUALITY_LIMITS if quality_limits is None else quality_limits + ), + minimum_capture_counts=( + _MINIMUM_CAPTURE_COUNTS + if minimum_capture_counts is None + else minimum_capture_counts + ), ) return ThreeCameraExtrinsics( cameras=generic.cameras, @@ -95,6 +106,7 @@ def dump_three_camera_extrinsics( cameras: Mapping[str, Mapping[str, Any]], front_from_view: Mapping[str, Sequence[Sequence[float]]], quality: Mapping[str, Any], + quality_limits: Mapping[str, float] | None = None, ) -> None: payload = { "schema_version": 1, @@ -105,7 +117,15 @@ def dump_three_camera_extrinsics( }, "quality": dict(quality), } - validate_extrinsics_payload(payload) + validate_camera_extrinsics_payload( + payload, + required_views=VIEWS, + reference_view="front", + quality_limits=( + _QUALITY_LIMITS if quality_limits is None else quality_limits + ), + minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS, + ) destination = Path(path).expanduser().resolve() destination.parent.mkdir(parents=True, exist_ok=True) temporary = destination.with_suffix(destination.suffix + ".tmp") diff --git a/src/linkerhand_calibration/linkerhand_calibration/extrinsics_node.py b/src/linkerhand_calibration/linkerhand_calibration/extrinsics_node.py index 1d05330..25d7d39 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/extrinsics_node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/extrinsics_node.py @@ -605,6 +605,9 @@ class ThreeCameraExtrinsicsNode(Node): self.declare_parameter("minimum_captures_per_pair", 15) self.declare_parameter("maximum_pair_skew_ms", 100.0) self.declare_parameter("maximum_reprojection_rms_px", 1.2) + self.declare_parameter( + "maximum_candidate_pair_reprojection_rms_px", 1.5 + ) self.declare_parameter( "maximum_single_camera_reprojection_rms_px", 1.5 ) @@ -640,6 +643,9 @@ class ThreeCameraExtrinsicsNode(Node): self.maximum_reprojection_rms_px = float( value("maximum_reprojection_rms_px") ) + self.maximum_candidate_pair_reprojection_rms_px = float( + value("maximum_candidate_pair_reprojection_rms_px") + ) self.maximum_single_camera_reprojection_rms_px = float( value("maximum_single_camera_reprojection_rms_px") ) @@ -671,13 +677,21 @@ class ThreeCameraExtrinsicsNode(Node): if self.maximum_reprojection_rms_px <= 0.0: raise ValueError("maximum_reprojection_rms_px must be positive") if ( - self.maximum_single_camera_reprojection_rms_px + self.maximum_candidate_pair_reprojection_rms_px < self.maximum_reprojection_rms_px ): raise ValueError( - "maximum_single_camera_reprojection_rms_px must be at " + "maximum_candidate_pair_reprojection_rms_px must be at " "least maximum_reprojection_rms_px" ) + if ( + self.maximum_single_camera_reprojection_rms_px + < self.maximum_candidate_pair_reprojection_rms_px + ): + raise ValueError( + "maximum_single_camera_reprojection_rms_px must be at " + "least maximum_candidate_pair_reprojection_rms_px" + ) if self.gui_refresh_hz <= 0.0 or self.gui_refresh_hz > 10.0: raise ValueError("gui_refresh_hz must be in (0, 10]") if self.auto_capture_stable_seconds < 0.5: @@ -869,14 +883,20 @@ class ThreeCameraExtrinsicsNode(Node): selected.reprojection_rms_px, self.maximum_single_camera_reprojection_rms_px, ) - if camera_rms_failed or pair_rms > self.maximum_reprojection_rms_px: + if ( + camera_rms_failed + or pair_rms + > self.maximum_candidate_pair_reprojection_rms_px + ): return PairAssessment( ready=False, reason_code="reprojection_rms_too_large", message=( f"当前组未通过采集门限，未计入。单相机上限=" f"{self.maximum_single_camera_reprojection_rms_px:.3f}px，" - f"组合上限={self.maximum_reprojection_rms_px:.3f}px；" + "候选组合上限=" + f"{self.maximum_candidate_pair_reprojection_rms_px:.3f}px，" + f"最终批次上限={self.maximum_reprojection_rms_px:.3f}px；" f"组合RMS={pair_rms:.3f}px，" f"front={front.reprojection_rms_px:.3f}px，" f"{other}={selected.reprojection_rms_px:.3f}px；" @@ -1352,7 +1372,9 @@ class ThreeCameraExtrinsicsNode(Node): ) cv2.putText( canvas, - f"Pair RMS {pair_rms}/{self.maximum_reprojection_rms_px:.3f}px " + "Pair RMS " + f"{pair_rms}/" + f"{self.maximum_candidate_pair_reprojection_rms_px:.3f}px " f"Skew {skew}/{self.maximum_pair_skew_ns / 1e6:.1f}ms " f"Batch RMS {batch_rms}/{self.maximum_reprojection_rms_px:.3f}px " f"stability {rotation}/" @@ -1526,6 +1548,15 @@ class ThreeCameraExtrinsicsNode(Node): cameras=self.camera_identities, front_from_view=transforms, quality=quality, + quality_limits={ + "reprojection_rms_px": self.maximum_reprojection_rms_px, + "maximum_rotation_repeatability_deg": math.degrees( + self.maximum_rotation_repeatability_rad + ), + "maximum_translation_repeatability_m": ( + self.maximum_translation_repeatability_m + ), + }, ) response.success = True response.message = f"外参已保存：{self.output_file}；{quality}" diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/__init__.py index 1c6e914..a86bee6 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/__init__.py @@ -6,10 +6,12 @@ from .registry import ( RegisteredProfile, get_default_registry, ) +from .runtime_schema import validate_schema_v6_runtime_payload __all__ = [ "EngineBindings", "ProfileRegistry", "RegisteredProfile", "get_default_registry", + "validate_schema_v6_runtime_payload", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py index 0fe09a3..6e42166 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py @@ -76,6 +76,10 @@ class ZeroCalibrationProfile: # historical defaults; other model profiles can name the physical anchor # explicitly without introducing model-specific branches in the solver. orientation_anchor_joint: str | None = None + # A model whose feedback direction is mechanically reviewed may use the + # signed rotation axes to disambiguate the otherwise mirrored palm-frame + # branches. The default remains undirected for legacy G20/L6 profiles. + directed_base_axis_joints: frozenset[str] = frozenset() @property def reference_finger(self) -> str: @@ -3314,14 +3318,13 @@ def solve_urdf_zero_offsets( predicted_axis, _ = predicted_local(item, pose_zero_offsets) predicted_axis = rotation.apply(predicted_axis) observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + alignment = float( + np.clip(predicted_axis @ observed_axis, -1.0, 1.0) + ) error = math.acos( - abs( - float( - np.clip( - predicted_axis @ observed_axis, -1.0, 1.0 - ) - ) - ) + alignment + if item.joint in profile.directed_base_axis_joints + else abs(alignment) ) errors_by_joint.setdefault(item.joint, []).append(error) branch_score = sum( diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/motion.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/motion.py index f648ba0..21c194f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/motion.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/motion.py @@ -70,7 +70,7 @@ def build_calibration_return_waypoints( else tuple(profile.command.baseline_u8) ) if len(target) != profile.command.command_count: - raise ValueError("L6 return command has the wrong channel count") + raise ValueError("six-channel return command has the wrong channel count") return (target,) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py index 10c7bb7..b2b60b7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py @@ -31,7 +31,7 @@ from ...pnp import SquareTagPose, SquareTagPoseTracker from ...storage import append_jsonl, atomic_write_json from .motion import cosine_position_trajectory_u8 from .pipeline import finalize_l6_session -from .profile import COMMAND_NAMES, build_typed_profile +from .profile import build_typed_profile @dataclass(frozen=True) @@ -55,11 +55,23 @@ def _stamp_ns(stamp: Any) -> int: class L6ThreeCameraCalibrationNode(Node): - """Own the L6 commands and publish only a reviewed partial result.""" + """Own one reviewed six-channel partial-calibration session.""" - def __init__(self) -> None: - super().__init__("l6_calibration") - self.profile = build_typed_profile() + def __init__( + self, + *, + profile=None, + finalizer=None, + sample_kind: str = "l6_joint_sample", + ) -> None: + 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.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 + super().__init__(f"{self.model_name.lower()}_calibration") self._declare_parameters() self._load_parameters() self.session_dir.mkdir(parents=True, exist_ok=True) @@ -82,7 +94,7 @@ class L6ThreeCameraCalibrationNode(Node): String, self.setting_topic, 10 ) self.status_publisher = self.create_publisher( - String, "/l6_calibration/status", 10 + String, f"{self.profile.namespace}/status", 10 ) self.create_subscription( JointState, self.state_topic, self._state_callback, 30 @@ -124,8 +136,8 @@ class L6ThreeCameraCalibrationNode(Node): ), qos_profile_sensor_data, ) - self.create_service(Trigger, "/l6_calibration/start", self._start) - self.create_service(Trigger, "/l6_calibration/abort", self._abort) + self.create_service(Trigger, f"{self.profile.namespace}/start", self._start) + self.create_service(Trigger, f"{self.profile.namespace}/abort", self._abort) self.latest_state_u8: tuple[float, ...] = () self.state_history: deque[StateSample] = deque(maxlen=2000) @@ -138,6 +150,8 @@ class L6ThreeCameraCalibrationNode(Node): self.step_command_sent = False self.step_last_progress_at = 0.0 self.step_last_feedback = float("nan") + self.step_last_distance_u8 = float("nan") + self.step_initial_distance_u8 = float("nan") self.step_initial_feedback = float("nan") self.step_speed_ready_at = 0.0 self.step_requested_u8 = float("nan") @@ -165,6 +179,9 @@ class L6ThreeCameraCalibrationNode(Node): self.create_timer(0.5, self._publish_status) def _declare_parameters(self) -> None: + model = self.model_name.lower() + namespace = self.profile.namespace + speed_parameters = self.profile.motion.speed_parameters defaults: dict[str, Any] = { "serial_number": "UNSET", "session_dir": "", @@ -174,21 +191,30 @@ class L6ThreeCameraCalibrationNode(Node): "camera_extrinsics_expected_sha256": "", "calibration_config_expected_sha256": "", "tag_config_expected_sha256": "", - "command_topic": "/l6/cb_right_hand_control_cmd", - "state_topic": "/l6/cb_right_hand_state", - "setting_topic": "/l6/cb_hand_setting_cmd", - "front_camera_info_topic": "/l6_calibration/front/camera/camera_info", - "front_detections_topic": "/l6_calibration/front/apriltag/detections", - "side_camera_info_topic": "/l6_calibration/side/camera/camera_info", - "side_detections_topic": "/l6_calibration/side/apriltag/detections", - "top_camera_info_topic": "/l6_calibration/top/camera/camera_info", - "top_detections_topic": "/l6_calibration/top/apriltag/detections", + "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", + "front_camera_info_topic": f"{namespace}/front/camera/camera_info", + "front_detections_topic": f"{namespace}/front/apriltag/detections", + "side_camera_info_topic": f"{namespace}/side/camera/camera_info", + "side_detections_topic": f"{namespace}/side/apriltag/detections", + "top_camera_info_topic": f"{namespace}/top/camera/camera_info", + "top_detections_topic": f"{namespace}/top/apriltag/detections", "commands_enabled": True, - "preflight_speed_u8": 1, - "formal_speed_u8": 1, - "speed_settle_seconds": 0.2, - "command_trajectory_full_range_seconds": 6.0, - "torque_u8": 80, + "baseline_speed_u8": int( + speed_parameters.get( + "baseline_u8", speed_parameters.get("preflight_u8", 1) + ) + ), + "preflight_speed_u8": int(speed_parameters.get("preflight_u8", 1)), + "formal_speed_u8": int(speed_parameters.get("formal_u8", 1)), + "speed_settle_seconds": float( + speed_parameters.get("speed_settle_seconds", 0.2) + ), + "command_trajectory_full_range_seconds": float( + speed_parameters.get("command_trajectory_full_range_seconds", 6.0) + ), + "torque_u8": int(speed_parameters.get("torque_u8", 80)), "endpoint_tolerance_u8": 2.0, "endpoint_hold_seconds": 1.0, "motor_stall_timeout_seconds": 2.0, @@ -236,9 +262,13 @@ class L6ThreeCameraCalibrationNode(Node): if not str(value("session_dir")) or not self.source_urdf.is_file(): raise ValueError("session_dir and immutable source_urdf_path are required") if not self.camera_extrinsics_file.is_file(): - raise ValueError("camera_extrinsics_file is required for L6 zero solve") + raise ValueError( + f"camera_extrinsics_file is required for {self.model_name} zero solve" + ) self.extrinsics = load_three_camera_extrinsics( - self.camera_extrinsics_file + self.camera_extrinsics_file, + quality_limits=self.profile.vision.extrinsics_quality_limits, + minimum_capture_counts=self.profile.vision.minimum_capture_counts, ) self.protected_inputs = { "source_urdf_sha256": str(value("source_urdf_expected_sha256")), @@ -264,7 +294,8 @@ class L6ThreeCameraCalibrationNode(Node): for view in self.profile.vision.view_names } for name in ( - "commands_enabled", "preflight_speed_u8", "formal_speed_u8", + "commands_enabled", "baseline_speed_u8", "preflight_speed_u8", + "formal_speed_u8", "speed_settle_seconds", "command_trajectory_full_range_seconds", "torque_u8", "endpoint_tolerance_u8", "endpoint_hold_seconds", "motor_stall_timeout_seconds", "position_timeout_seconds", @@ -285,6 +316,11 @@ class L6ThreeCameraCalibrationNode(Node): 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 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: @@ -297,9 +333,14 @@ class L6ThreeCameraCalibrationNode(Node): raise ValueError("minimum_joint_frame_rate must be in (0, 1]") def _build_steps(self) -> list[MotionStep]: - steps = [MotionStep("baseline", None, None, 255, int(self.preflight_speed_u8))] + steps = [ + MotionStep( + "baseline", None, None, 255, int(self.baseline_speed_u8) + ) + ] for task in self.profile.motion.tasks: - for target in (255, 127, 0, 255): + midpoint = int(round(0.5 * (task.start_u8 + task.end_u8))) + for target in (task.start_u8, midpoint, task.end_u8, task.start_u8): steps.append( MotionStep( "preflight", task.key, task.command_index, target, @@ -309,10 +350,10 @@ class L6ThreeCameraCalibrationNode(Node): for cycle in (0, 1, 2, 3): steps.extend( [ - MotionStep("prepare", task.key, task.command_index, 255, int(self.formal_speed_u8), cycle), - MotionStep("sweep", task.key, task.command_index, 0, int(self.formal_speed_u8), cycle, "decreasing"), - MotionStep("prepare", task.key, task.command_index, 0, int(self.formal_speed_u8), cycle), - MotionStep("sweep", task.key, task.command_index, 255, int(self.formal_speed_u8), cycle, "increasing"), + 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"), ] ) return steps @@ -320,7 +361,7 @@ class L6ThreeCameraCalibrationNode(Node): def _start(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: if self.started: response.success = False - response.message = "L6 calibration has already started" + response.message = f"{self.model_name} calibration has already started" return response if self.state != "READY": response.success = False @@ -333,11 +374,11 @@ class L6ThreeCameraCalibrationNode(Node): self.reason = "moving_to_open_baseline" self._publish_torque() response.success = True - response.message = "L6 partial calibration started" + response.message = f"{self.model_name} partial calibration started" return response def _abort(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: - self._publish_command([255] * 6) + self._publish_command(list(self.baseline_command)) self.state = "ABORTED" self.reason = "operator_abort_returning_to_open_baseline" response.success = True @@ -355,11 +396,12 @@ class L6ThreeCameraCalibrationNode(Node): return if message.name: by_name = dict(zip((str(name) for name in message.name), message.position)) - if "thumb_cmc_yaw" in by_name and "thumb_cmc_roll" not in by_name: - by_name["thumb_cmc_roll"] = by_name["thumb_cmc_yaw"] - if any(name not in by_name for name in COMMAND_NAMES): + for alias, canonical in self.profile.command.feedback_name_aliases.items(): + if alias in by_name and canonical not in by_name: + by_name[canonical] = by_name[alias] + if any(name not in by_name for name in self.command_names): return - state = tuple(float(by_name[name]) for name in COMMAND_NAMES) + state = tuple(float(by_name[name]) for name in self.command_names) else: state = tuple(float(value) for value in message.position) if not all(math.isfinite(value) for value in state): @@ -376,7 +418,7 @@ class L6ThreeCameraCalibrationNode(Node): for index, actual in enumerate(state): if index == step.command_index: continue - if abs(actual - 255.0) > float(self.non_target_motion_tolerance_u8): + if abs(actual - self.baseline_command[index]) > float(self.non_target_motion_tolerance_u8): self._pause( f"non_target_motor_moved:channel={index}:feedback={actual:.2f}" ) @@ -663,12 +705,12 @@ class L6ThreeCameraCalibrationNode(Node): * Rotation.from_matrix(child_matrix[:3, :3]) ) record = { - "kind": "l6_joint_sample", + "kind": self.sample_kind, "profile_id": self.profile.key.profile_id, "task_name": task.key, "view": view, "joint": joint, - "sdk_channel": COMMAND_NAMES[task.command_index], + "sdk_channel": self.command_names[task.command_index], "motor_index": task.command_index, "cycle": int(step.cycle), "direction": str(step.direction), @@ -747,7 +789,7 @@ class L6ThreeCameraCalibrationNode(Node): return message = JointState() message.header.stamp = self.get_clock().now().to_msg() - message.name = list(COMMAND_NAMES) + message.name = list(self.command_names) message.position = [float(value) for value in values] self.command_publisher.publish(message) @@ -776,13 +818,20 @@ class L6ThreeCameraCalibrationNode(Node): ) self.step_initial_feedback = self.step_last_feedback self.step_requested_u8 = self.step_initial_feedback - target = [255.0] * 6 + target = [float(value) for value in self.baseline_command] if step.command_index is not None: target[step.command_index] = float(step.target_u8) - maximum_distance = max( + errors = [ abs(end - start) for start, end in zip(self.step_start_state_u8, target) + ] + self.step_last_distance_u8 = ( + float(sum(errors)) + if step.command_index is None + else float(errors[step.command_index]) ) + 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, @@ -806,16 +855,17 @@ class L6ThreeCameraCalibrationNode(Node): ) def _advance_step_trajectory(self, step: MotionStep, now: float) -> None: + baseline = tuple(getattr(self, "baseline_command", (255,) * 6)) elapsed = max(0.0, float(now) - self.step_started_at) _, phase, _ = cosine_position_trajectory_u8( 0.0, max( ( - abs(255.0 - value) + 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(255.0 - value) + else abs(float(baseline[index]) - value) ) for index, value in enumerate(self.step_start_state_u8) ), @@ -823,7 +873,7 @@ class L6ThreeCameraCalibrationNode(Node): float(self.command_trajectory_full_range_seconds), ) blend = 0.5 - 0.5 * math.cos(math.pi * phase) - targets = [255.0] * 6 + targets = [float(value) for value in baseline] if step.command_index is not None: targets[step.command_index] = float(step.target_u8) values = [ @@ -888,7 +938,9 @@ class L6ThreeCameraCalibrationNode(Node): append_jsonl( self.raw_path, { - "kind": "l6_sweep_observation_quality", + "kind": getattr( + self, "sweep_quality_kind", "l6_sweep_observation_quality" + ), "task_name": step.task_key, "cycle": step.cycle, "direction": step.direction, @@ -911,7 +963,8 @@ class L6ThreeCameraCalibrationNode(Node): return False attempt = retries + 2 self.retry_counts[key] = retries + 1 - start = 255 if step.direction == "decreasing" else 0 + task = self._task(str(step.task_key)) + start = task.start_u8 if step.direction == "decreasing" else task.end_u8 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), @@ -980,18 +1033,38 @@ class L6ThreeCameraCalibrationNode(Node): return if len(self.latest_state_u8) != 6: 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) + endpoint_errors = [ + abs(value - target_state[index]) + for index, value in enumerate(self.latest_state_u8) + ] actual = ( - max(abs(value - 255.0) for value in self.latest_state_u8) + max(endpoint_errors) if step.command_index is None - else abs(self.latest_state_u8[step.command_index] - step.target_u8) + else endpoint_errors[step.command_index] + ) + progress_distance = ( + float(sum(endpoint_errors)) + if step.command_index is None + else float(endpoint_errors[step.command_index]) ) feedback = ( float(np.mean(self.latest_state_u8)) if step.command_index is None else float(self.latest_state_u8[step.command_index]) ) - if not math.isfinite(self.step_last_feedback) or abs(feedback - self.step_last_feedback) >= 1.0: - self.step_last_feedback = feedback + self.step_last_feedback = feedback + # Baseline recovery moves all six channels at once. Using their mean + # as the progress signal divides a one-count move by six and can turn + # slow, valid O6 motion into a false two-second stall. Distance to the + # 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 + ): + self.step_last_distance_u8 = progress_distance self.step_last_progress_at = now if ( actual > float(self.endpoint_tolerance_u8) @@ -1018,7 +1091,7 @@ class L6ThreeCameraCalibrationNode(Node): def _finalize(self) -> None: try: - payload, _fit, correction = finalize_l6_session( + payload, _fit, correction = self.finalize_session( session_dir=self.session_dir, serial_number=self.serial_number, source_urdf=self.source_urdf, @@ -1038,12 +1111,14 @@ class L6ThreeCameraCalibrationNode(Node): ) self._pause(f"fit_or_publication_failed:{error}") return - self._publish_command([255] * 6) + self._publish_command(list(self.baseline_command)) self.state = "PASSED" self.reason = "partial_calibration_passed" self.final_json = str( self.session_dir - / f"l6_right_{self.serial_number}_partial_calibration.json" + / self.profile.artifacts.calibration_filename.format( + serial_number=self.serial_number + ) ) self.final_urdf = str(correction.path) self.final_quality = payload["quality"] @@ -1070,12 +1145,28 @@ class L6ThreeCameraCalibrationNode(Node): if step is None or step.command_index is None else float(self.latest_state_u8[step.command_index]) ) + 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: + channel_errors = [ + abs(value - target_state[index]) + for index, value in enumerate(self.latest_state_u8) + ] step_fraction = 0.0 if step is not None and math.isfinite(feedback): - initial_distance = abs(self.step_initial_feedback - step.target_u8) + current_distance = ( + float(sum(channel_errors)) + if step.command_index is None + else channel_errors[step.command_index] + ) + initial_distance = self.step_initial_distance_u8 if math.isfinite(initial_distance) and initial_distance > 0.0: - step_fraction = 1.0 - abs(feedback - step.target_u8) / initial_distance - elif abs(feedback - step.target_u8) <= float(self.endpoint_tolerance_u8): + step_fraction = 1.0 - current_distance / initial_distance + elif current_distance <= float(self.endpoint_tolerance_u8): step_fraction = 1.0 step_fraction = float(np.clip(step_fraction, 0.0, 1.0)) elif self.state == "PASSED": @@ -1095,6 +1186,7 @@ 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, "current_command_u8": ( None @@ -1126,6 +1218,7 @@ class L6ThreeCameraCalibrationNode(Node): "joint_frame_rate": round(float(observation["joint_frame_rate"]), 6), "observation_rejection_counts": observation["rejection_counts"], "preflight_speed_u8": int(self.preflight_speed_u8), + "baseline_speed_u8": int(self.baseline_speed_u8), "formal_speed_u8": int(self.formal_speed_u8), "command_trajectory_full_range_seconds": float( self.command_trajectory_full_range_seconds @@ -1144,7 +1237,23 @@ class L6ThreeCameraCalibrationNode(Node): "feedback_hz": round(self._feedback_hz(), 3), "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": ( + [] + 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], + "maximum_error_channel": ( + None + if not channel_errors + else self.command_names[int(np.argmax(channel_errors))] + ), + "maximum_error_u8": ( + None if not channel_errors else round(max(channel_errors), 3) + ), "final_json": getattr(self, "final_json", ""), "final_urdf": getattr(self, "final_urdf", ""), "quality": getattr(self, "final_quality", {}), diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py index d30252b..ad5a053 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py @@ -13,7 +13,7 @@ import signal import subprocess import sys import time -from typing import Any, Mapping +from typing import Any, Callable, Mapping import rclpy from rclpy.node import Node @@ -77,7 +77,9 @@ def _duration_zh(seconds: float | None) -> str: return f"{value // 60}分{value % 60:02d}秒" -def _l6_reason_zh(status: Mapping[str, Any]) -> tuple[str, str, str]: +def _l6_reason_zh( + status: Mapping[str, Any], *, model_name: str = "L6" +) -> tuple[str, str, str]: reason = str(status.get("reason", "unknown")) if reason.startswith("fixed_base_tag_moved:"): fields = reason.split(":") @@ -113,7 +115,7 @@ def _l6_reason_zh(status: Mapping[str, Any]) -> tuple[str, str, str]: count = status.get("state_publisher_count", "?") return ( "DEVICE-DUPLICATE-SDK-203", - f"检测到 {count} 个 L6 状态发布者；这通常表示已有 SDK/GUI 未退出。", + f"检测到 {count} 个 {model_name} 状态发布者；这通常表示已有 SDK/GUI 未退出。", "先停止单独启动的 linker_hand_sdk 和 GUI，只保留本标定命令自动拉起的 SDK，" "再重新开始。程序已禁止同时控制同一只手。", ) @@ -121,7 +123,7 @@ def _l6_reason_zh(status: Mapping[str, Any]) -> tuple[str, str, str]: count = status.get("command_publisher_count", "?") return ( "DEVICE-COMMAND-CONFLICT-204", - f"检测到 {count} 个 L6 控制命令发布者，标定节点之外还有程序在控制手。", + f"检测到 {count} 个 {model_name} 控制命令发布者，标定节点之外还有程序在控制手。", "停止 GUI、手动控制节点或其他标定进程，只保留当前标定命令后重新开始。", ) if reason.startswith("mechanical_stall:"): @@ -170,23 +172,28 @@ def _l6_reason_zh(status: Mapping[str, Any]) -> tuple[str, str, str]: if reason.startswith("operator_abort"): return "OPERATOR-ABORT-001", "操作员主动中止了本次标定。", "排除现场问题后重新开始。" return ( - "L6-CAL-500", - "L6 标定因未分类保护条件停止。", + f"{model_name}-CAL-500", + f"{model_name} 标定因未分类保护条件停止。", "保留会话目录和运行日志，并复制下方诊断块给开发者。", ) -def render_l6_progress_zh( +def render_six_channel_progress_zh( status: Mapping[str, Any], + *, + task_labels: Mapping[str, str], + reason_renderer: Callable[ + [Mapping[str, Any]], tuple[str, str, str] + ], estimator: ProgressEstimator | None = None, ) -> str: - """Render L6 progress in the same operator-oriented layout as G20.""" + """Render one six-channel profile in the operator-oriented G20 layout.""" state = str(status.get("state", "")) overall, _current_step, _step_count = _progress_fraction(status) task = status.get("task_name") phase = status.get("phase") if task: - task_text = _TASK_LABELS.get(str(task), str(task)) + task_text = task_labels.get(str(task), str(task)) elif phase == "baseline": task_text = "全手基准姿态" elif state == "WAIT_DEVICES": @@ -242,19 +249,66 @@ def render_l6_progress_zh( valid_frames=valid, automatic_retry_count=max(0, attempt - 1), ) + speed_u8 = status.get("speed_u8") + if speed_u8 is not None: + trajectory_seconds = float( + status.get("command_trajectory_full_range_seconds", 0.0) or 0.0 + ) + lines.append( + f"运动：速度档 {int(speed_u8)}；全行程 {trajectory_seconds:.1f} 秒余弦轨迹" + ) + latest_state = status.get("latest_state_u8", []) + command_names = status.get("command_names", []) + if ( + isinstance(latest_state, (list, tuple)) + and isinstance(command_names, (list, tuple)) + and len(latest_state) == len(command_names) == 6 + and (phase == "baseline" or state in {"PAUSED", "ABORTED"}) + ): + feedback_text = ", ".join( + f"{name}={float(value):.1f}" + for name, value in zip(command_names, latest_state) + ) + maximum_error_channel = status.get("maximum_error_channel") + maximum_error = status.get("maximum_error_u8") + error_text = ( + "未知" + if maximum_error_channel is None or maximum_error is None + else f"{maximum_error_channel}={float(maximum_error):.1f}" + ) + lines.append(f"六路反馈：{feedback_text} 最大偏差：{error_text}") if state in {"PAUSED", "ABORTED"}: - _code, problem, suggestion = _l6_reason_zh(status) + _code, problem, suggestion = reason_renderer(status) lines.extend((f"原因：{problem}", f"建议：{suggestion}")) return "\n".join(lines) +def render_l6_progress_zh( + status: Mapping[str, Any], + estimator: ProgressEstimator | None = None, +) -> str: + """Render L6 progress in the same operator-oriented layout as G20.""" + return render_six_channel_progress_zh( + status, + task_labels=_TASK_LABELS, + reason_renderer=_l6_reason_zh, + estimator=estimator, + ) + + class _ProgressConsole: - def __init__(self) -> None: + def __init__( + self, + renderer: Callable[ + [Mapping[str, Any], ProgressEstimator | None], str + ] = render_l6_progress_zh, + ) -> None: self.last_text = "" self.estimator = ProgressEstimator.start() + self.renderer = renderer def update(self, status: Mapping[str, Any]) -> None: - text = render_l6_progress_zh(status, self.estimator) + text = self.renderer(status, self.estimator) if text == self.last_text: return self.last_text = text @@ -292,6 +346,7 @@ def _launch_command( *, record_bag: bool, commands_enabled: bool, + sdk_startup_speed_u8: int = 1, ) -> list[str]: arguments = { "model": config.model, @@ -311,7 +366,7 @@ def _launch_command( "session_dir": str(session), "corrected_urdf_output_dir": str(session), "recalibration_scope": "partial", - "calibration_speed": "1", + "calibration_speed": str(int(sdk_startup_speed_u8)), "index_roll_calibration_speed": "1", "index_flex_calibration_speed": "1", "commands_enabled": str(commands_enabled).lower(), @@ -619,4 +674,6 @@ def main(args: list[str] | None = None) -> None: ) -__all__ = ["main", "render_l6_progress_zh"] +__all__ = [ + "main", "render_l6_progress_zh", "render_six_channel_progress_zh" +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/__init__.py new file mode 100644 index 0000000..622724b --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/__init__.py @@ -0,0 +1,12 @@ +"""Registered O6 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/o6/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/artifacts.py new file mode 100644 index 0000000..d6c3bb3 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/artifacts.py @@ -0,0 +1,450 @@ +"""Schema-v6 runtime and correction-input artifacts for O6 right.""" + +from __future__ import annotations + +import hashlib +import json +import math +from pathlib import Path +from typing import Any, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np + +from ..g20.profile import JointCurveFit +from ..l6.artifacts import artifact_hashes, atomic_write_json, publish_partial_session +from .fitting import MimicFit, O6FitResult +from .profile import ( + ACTIVE_JOINTS, + CALIBRATED_ACTIVE_JOINTS, + COMMAND_INDEX_BY_JOINT, + COMMAND_NAMES, + COUPLING_MODEL_BY_JOINT, + KEY, + MEASURED_PASSIVE_JOINTS, + MIMIC_SOURCE_BY_JOINT, + MAXIMUM_HYSTERESIS_DEG, + PASSIVE_JOINTS, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, + build_typed_profile, +) + + +ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS) +O6_URDF_INPUT_SCHEMA_VERSION = 2 +O6_URDF_INPUT_ARTIFACT_TYPE = "linkerhand_o6_urdf_correction_input" + + +def _sha256(path: str | Path) -> str: + return hashlib.sha256(Path(path).read_bytes()).hexdigest() + + +def _source_metadata(source_urdf: str | Path) -> dict[str, dict[str, float | str]]: + result: dict[str, dict[str, float | str]] = {} + for joint in ET.parse(Path(source_urdf)).getroot().findall("joint"): + name = str(joint.get("name", "")) + if name not in ALL_REVOLUTE_JOINTS: + continue + limit = joint.find("limit") + if joint.get("type") != "revolute" or limit is None: + raise ValueError(f"O6 profile joint is invalid: {name}") + item: dict[str, float | str] = { + "lower": float(limit.get("lower", "nan")), + "upper": float(limit.get("upper", "nan")), + } + mimic = joint.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 does not contain exactly 11 O6 revolute joints") + for transfers in ( + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, + ): + for target, donor in transfers.items(): + for field in ("lower", "upper"): + if abs(float(result[target][field]) - float(result[donor][field])) > 1e-8: + raise ValueError(f"O6 transfer {target} differs from {donor} {field}") + return result + + +def _curve(values: Sequence[float]) -> list[float]: + array = np.asarray(values, dtype=float) + if array.shape != (256,) or not np.all(np.isfinite(array)): + raise ValueError("O6 runtime curve must contain 256 finite values") + return [round(float(value), 8) for value in array] + + +def build_o6_runtime_payload( + *, + serial_number: str, + source_urdf: str | Path, + result: O6FitResult, + protected_inputs: Mapping[str, str], + passed: bool = True, +) -> dict[str, Any]: + profile = build_typed_profile() + source = _source_metadata(source_urdf) + joints: dict[str, dict[str, Any]] = {} + for name in ACTIVE_JOINTS: + donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name) + motor = COMMAND_INDEX_BY_JOINT[name] + fit = result.curves[donor] + decreasing = np.asarray(fit.decreasing_rad, dtype=float) + increasing = np.asarray(fit.increasing_rad, dtype=float) + decreasing -= float(decreasing[255]) + increasing -= float(increasing[255]) + average = 0.5 * (decreasing + increasing) + zero = { + "policy": result.zero_method_by_joint[donor], + "source_lower_rad": round(float(source[name]["lower"]), 8), + "measured_travel_rad": round(float(result.travels_rad[donor]), 8), + "urdf_origin_offset_rad": round(float(result.zero_offsets_rad[donor]), 8), + } + if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT: + zero = { + **zero, + "policy": "transferred_from_pinky", + "transferred_from_joint": donor, + } + item = { + "urdf_joint": name, + "sdk_channel": COMMAND_NAMES[motor], + "motor_index": motor, + "passive": False, + "calibration_status": profile.joint_coverage[name], + "zero_command_u8": 255, + "zero_angles": zero, + "angle_rad": _curve(average), + "decreasing_rad": _curve(decreasing), + "increasing_rad": _curve(increasing), + } + if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT: + item["transferred_from_joint"] = donor + joints[name] = item + for name in PASSIVE_JOINTS: + source_name = MIMIC_SOURCE_BY_JOINT[name] + donor = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name) + motor = COMMAND_INDEX_BY_JOINT[source_name] + fit = result.curves[donor] + decreasing = np.asarray(fit.decreasing_rad, dtype=float) + increasing = np.asarray(fit.increasing_rad, dtype=float) + decreasing -= float(decreasing[255]) + increasing -= float(increasing[255]) + offset = float(source[name]["offset"]) + decreasing += offset + increasing += offset + average = 0.5 * (decreasing + increasing) + coupling = result.mimic_fits[donor] + coefficients = [ + offset, + *coupling.coefficients, + *(0.0 for _ in range(5 - len(coupling.coefficients))), + ] + item = { + "urdf_joint": name, + "sdk_channel": COMMAND_NAMES[motor], + "motor_index": motor, + "passive": True, + "source_joint": source_name, + "mimic_offset_rad": round(offset, 8), + "coupling_model": coupling.model, + "coupling_coefficients": [round(float(v), 10) for v in coefficients], + "urdf_mimic_enabled": True, + "urdf_mimic_policy": coupling.urdf_mimic_policy, + "mimic_multiplier": round( + float(coupling.urdf_mimic_multiplier), 8 + ), + "calibration_status": profile.joint_coverage[name], + "zero_command_u8": 255, + "zero_angles": {"policy": "cad_static"}, + "angle_rad": _curve(average), + "decreasing_rad": _curve(decreasing), + "increasing_rad": _curve(increasing), + } + if name in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT: + item["transferred_from_joint"] = donor + joints[name] = item + errors = np.abs(np.concatenate([ + np.asarray(values, dtype=float) for values in result.holdout_errors_rad.values() + ])) + payload: dict[str, Any] = { + "schema_version": 6, + "profile_id": KEY.profile_id, + "layout_id": KEY.layout, + "model": "O6", + "side": "right", + "serial_number": str(serial_number), + "calibration_scope": "partial", + "publication_pointer": "latest_partial_passed", + "angle_unit": "rad", + "command_range": [0, 255], + "curve_input_domain": "feedback_u8", + "runtime_curve_policy": "direction_aware", + "command_names": list(COMMAND_NAMES), + "baseline_command_u8": [255] * 6, + "protected_inputs": dict(protected_inputs), + "joints": joints, + "quality": { + "passed": bool(passed), + "scope": "partial", + "maximum_hysteresis_limit_rad": round( + math.radians(MAXIMUM_HYSTERESIS_DEG), 8 + ), + "maximum_hysteresis_by_joint_rad": { + name: round(float(fit.maximum_hysteresis_rad), 8) + for name, fit in sorted(result.curves.items()) + }, + "validation_mae_rad": round(float(np.mean(errors)), 8), + "validation_p95_rad": round(float(np.percentile(errors, 95.0)), 8), + "validation_max_rad": round(float(np.max(errors)), 8), + "thumb_axis_zero": None if result.thumb_zero_result is None else { + "method": "yaw_geometry_pitch_cad_endpoint", + "passed": bool(result.thumb_zero_result.passed), + "axis_line_rms_m": round(float(result.thumb_zero_result.axis_line_rms_m), 10), + "offsets_rad": { + name: round(float(result.zero_offsets_rad[name]), 10) + for name in ("rh_thumb_cmc_pitch", "rh_thumb_cmc_yaw") + }, + "candidate_geometry_offsets_rad": { + name: round(float(value), 10) + for name, value in sorted( + result.thumb_zero_result.direct_offsets_rad.items() + ) + }, + "policy_by_joint": { + name: result.zero_method_by_joint[name] + for name in ("rh_thumb_cmc_pitch", "rh_thumb_cmc_yaw") + }, + "validation_error_by_joint_rad": { + name: round(float(value), 10) + for name, value in sorted(result.thumb_zero_result.validation_error_by_joint_rad.items()) + }, + "validation_line_error_by_joint_m": { + name: round(float(value), 10) + for name, value in sorted( + result.thumb_zero_result.validation_line_error_by_joint_m.items() + ) + }, + }, + }, + } + validate_o6_runtime_payload(payload) + return payload + + +def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: + required = { + "schema_version", "profile_id", "layout_id", "model", "side", + "serial_number", "calibration_scope", "publication_pointer", "angle_unit", + "command_range", "curve_input_domain", "runtime_curve_policy", "command_names", + "baseline_command_u8", "protected_inputs", "joints", "quality", + } + if set(payload) != required: + raise ValueError("O6 schema v6 has unexpected top-level fields") + if ( + payload["schema_version"] != 6 or payload["profile_id"] != KEY.profile_id + or payload["model"] != "O6" or payload["side"] != "right" + or payload["layout_id"] != KEY.layout + or payload["calibration_scope"] != "partial" + or payload["publication_pointer"] != "latest_partial_passed" + or payload["curve_input_domain"] != "feedback_u8" + or payload["runtime_curve_policy"] != "direction_aware" + or payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255] + or tuple(payload["command_names"]) != COMMAND_NAMES + or payload["baseline_command_u8"] != [255] * 6 + ): + raise ValueError("O6 schema v6 identity or command contract is invalid") + protected = payload["protected_inputs"] + if not isinstance(protected, Mapping) or set(protected) != { + "source_urdf_sha256", "camera_extrinsics_sha256", + "calibration_config_sha256", "tag_config_sha256", + } or any(len(str(value)) != 64 for value in protected.values()): + raise ValueError("O6 protected inputs are invalid") + joints = payload["joints"] + if not isinstance(joints, Mapping) or set(joints) != ALL_REVOLUTE_JOINTS: + raise ValueError("O6 schema v6 must contain all 11 revolute joints") + profile = build_typed_profile() + for name in ACTIVE_JOINTS + PASSIVE_JOINTS: + item = joints[name] + motor = COMMAND_INDEX_BY_JOINT[MIMIC_SOURCE_BY_JOINT.get(name, name)] + if ( + item.get("urdf_joint") != name or item.get("motor_index") != motor + or item.get("sdk_channel") != COMMAND_NAMES[motor] + or item.get("calibration_status") != profile.joint_coverage[name] + or item.get("passive") is not (name in PASSIVE_JOINTS) + or item.get("zero_command_u8") != 255 + ): + raise ValueError(f"O6 joint contract is invalid: {name}") + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + values = np.asarray(item.get(field), dtype=float) + if values.shape != (256,) or not np.all(np.isfinite(values)): + raise ValueError(f"{name}.{field} must contain 256 finite values") + if np.any(np.diff(values) > 1e-7): + raise ValueError(f"{name}.{field} must be non-increasing") + if name in PASSIVE_JOINTS: + if ( + item.get("source_joint") != MIMIC_SOURCE_BY_JOINT[name] + or item.get("coupling_model") != COUPLING_MODEL_BY_JOINT[name] + or item.get("urdf_mimic_enabled") is not True + or item.get("urdf_mimic_policy") != "endpoint_linear_fallback" + ): + raise ValueError(f"O6 passive coupling is invalid: {name}") + coefficients = np.asarray(item.get("coupling_coefficients"), dtype=float) + if ( + coefficients.shape != (6,) + or not np.all(np.isfinite(coefficients)) + or np.any(np.abs(coefficients[3:]) > 1e-10) + ): + raise ValueError(f"O6 passive quadratic coupling is invalid: {name}") + if abs(float(coefficients[0]) - float( + item.get("mimic_offset_rad", "nan") + )) > 1e-7: + raise ValueError(f"O6 passive coupling offset differs: {name}") + multiplier = float(item.get("mimic_multiplier", "nan")) + if not math.isfinite(multiplier) or multiplier <= 0.0: + raise ValueError(f"O6 passive mimic multiplier is invalid: {name}") + donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name) or TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name) + if donor is not None: + if item.get("transferred_from_joint") != donor: + raise ValueError(f"O6 transfer provenance is invalid: {name}") + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + if item[field] != joints[donor][field]: + raise ValueError(f"O6 transferred curve differs: {name}") + if payload["quality"].get("passed") is not True: + raise ValueError("O6 quality.passed must be true") + + +def build_o6_urdf_input_payload( + *, serial_number: str, source_urdf: str | Path, result: O6FitResult +) -> dict[str, Any]: + measured = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS + payload = { + "schema_version": O6_URDF_INPUT_SCHEMA_VERSION, + "artifact_type": O6_URDF_INPUT_ARTIFACT_TYPE, + "profile_id": KEY.profile_id, + "model": "O6", + "side": "right", + "serial_number": str(serial_number), + "source_urdf_sha256": _sha256(source_urdf), + "zero_offsets_rad": {name: float(result.zero_offsets_rad[name]) for name in sorted(CALIBRATED_ACTIVE_JOINTS)}, + "travels_rad": {name: float(result.travels_rad[name]) for name in sorted(CALIBRATED_ACTIVE_JOINTS)}, + "measured_curves": { + name: { + field: [float(value) for value in getattr(result.curves[name], field)] + for field in ("angle_rad", "decreasing_rad", "increasing_rad") + } + for name in sorted(measured) + }, + "mimic_fits": { + name: { + "source_joint": result.mimic_fits[name].source_joint, + "target_joint": name, + "model": result.mimic_fits[name].model, + "coefficients": [ + float(value) for value in result.mimic_fits[name].coefficients + ], + "urdf_mimic_multiplier": float( + result.mimic_fits[name].urdf_mimic_multiplier + ), + "urdf_mimic_enabled": True, + "urdf_mimic_policy": result.mimic_fits[name].urdf_mimic_policy, + } + for name in sorted(MEASURED_PASSIVE_JOINTS) + }, + } + validate_o6_urdf_input_payload(payload) + return payload + + +def validate_o6_urdf_input_payload(payload: Mapping[str, Any]) -> None: + required = { + "schema_version", "artifact_type", "profile_id", "model", "side", + "serial_number", "source_urdf_sha256", "zero_offsets_rad", "travels_rad", + "measured_curves", "mimic_fits", + } + measured = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS + if set(payload) != required or ( + payload.get("schema_version") != O6_URDF_INPUT_SCHEMA_VERSION + or payload.get("artifact_type") != O6_URDF_INPUT_ARTIFACT_TYPE + or payload.get("profile_id") != KEY.profile_id + or payload.get("model") != "O6" or payload.get("side") != "right" + ): + raise ValueError("O6 URDF correction input identity is invalid") + if set(payload["zero_offsets_rad"]) != CALIBRATED_ACTIVE_JOINTS or set(payload["travels_rad"]) != CALIBRATED_ACTIVE_JOINTS: + raise ValueError("O6 URDF correction input active sets are incomplete") + if set(payload["measured_curves"]) != measured or set(payload["mimic_fits"]) != MEASURED_PASSIVE_JOINTS: + raise ValueError("O6 URDF correction input measured sets are incomplete") + if len(str(payload["source_urdf_sha256"])) != 64: + raise ValueError("O6 source hash is invalid") + for name, item in payload["mimic_fits"].items(): + coefficients = np.asarray(item.get("coefficients"), dtype=float) + if ( + item.get("source_joint") != MIMIC_SOURCE_BY_JOINT[name] + or item.get("target_joint") != name + or item.get("model") != "quadratic_runtime" + or item.get("urdf_mimic_enabled") is not True + or item.get("urdf_mimic_policy") != "endpoint_linear_fallback" + or coefficients.shape != (2,) + or not np.all(np.isfinite(coefficients)) + or not math.isfinite(float(item.get("urdf_mimic_multiplier", "nan"))) + or float(item.get("urdf_mimic_multiplier", 0.0)) <= 0.0 + ): + raise ValueError(f"O6 URDF input coupling is invalid: {name}") + + +def load_o6_urdf_input( + path: str | Path, *, source_urdf: str | Path, serial_number: str +) -> O6FitResult: + try: + payload = json.loads(Path(path).read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError) as error: + raise ValueError("could not read O6 URDF correction input") from error + validate_o6_urdf_input_payload(payload) + if str(payload["serial_number"]) != str(serial_number) or payload["source_urdf_sha256"] != _sha256(source_urdf): + raise ValueError("O6 correction input identity or source hash differs") + curves = { + name: JointCurveFit( + angle_rad=tuple(float(v) for v in item["angle_rad"]), + decreasing_rad=tuple(float(v) for v in item["decreasing_rad"]), + increasing_rad=tuple(float(v) for v in item["increasing_rad"]), + circle={}, maximum_monotonic_correction_rad=0.0, + maximum_hysteresis_rad=0.0, quality={}, + ) + for name, item in payload["measured_curves"].items() + } + mimic = { + name: MimicFit( + source_joint=item["source_joint"], target_joint=name, + model=str(item["model"]), + coefficients=tuple(float(value) for value in item["coefficients"]), + urdf_mimic_multiplier=float(item["urdf_mimic_multiplier"]), + urdf_mimic_policy=str(item["urdf_mimic_policy"]), + cycle_coefficients=(), + maximum_cycle_range=0.0, maximum_cycle_prediction_range_rad=0.0, + residual_rms_rad=0.0, residual_p95_rad=0.0, residual_max_rad=0.0, + ) + for name, item in payload["mimic_fits"].items() + } + return O6FitResult( + curves=curves, + zero_offsets_rad={name: float(v) for name, v in payload["zero_offsets_rad"].items()}, + travels_rad={name: float(v) for name, v in payload["travels_rad"].items()}, + mimic_fits=mimic, + holdout_errors_rad={}, zero_method_by_joint={}, + zero_fallback_reason_by_joint={}, thumb_zero_result=None, + ) + + +__all__ = [ + "ALL_REVOLUTE_JOINTS", "artifact_hashes", "atomic_write_json", + "build_o6_runtime_payload", "build_o6_urdf_input_payload", "load_o6_urdf_input", + "publish_partial_session", "validate_o6_runtime_payload", + "validate_o6_urdf_input_payload", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/fitting.py new file mode 100644 index 0000000..42c7c70 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/fitting.py @@ -0,0 +1,339 @@ +"""O6 curve, geometric-zero, holdout, and linear mimic fitting.""" + +from __future__ import annotations + +import math +from pathlib import Path +from typing import Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np + +from ..g20.profile import HandCalibrationProfile, JointCurveFit, JointSpec +from ..g20.zero_solver import ( + ZeroCalibrationProfile, + ZeroSolveResult, + fit_joint_axis_measurement, + fit_rotation_joint_curve, + rotation_curve_holdout_errors, + solve_urdf_zero_offsets, + with_depth_free_axis_projection, +) +from ..l6.fitting import L6FitResult, MimicFit, fit_coupling_model +from .profile import ( + CALIBRATED_ACTIVE_JOINTS, + GEOMETRIC_ZERO_JOINTS, + COMMAND_INDEX_BY_JOINT, + COMMAND_NAMES, + COUPLING_MODEL_BY_JOINT, + COUPLING_RESIDUAL_MAX_DEG, + COUPLING_RESIDUAL_P95_DEG, + ENDPOINT_ANCHOR_BY_JOINT, + KEY, + MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M, + MAXIMUM_HYSTERESIS_DEG, + MEASURED_PASSIVE_JOINTS, + MIMIC_SOURCE_BY_JOINT, +) + + +O6FitResult = L6FitResult +O6_THUMB_AXIS_JOINTS: tuple[str, ...] = ( + "rh_thumb_cmc_yaw", + "rh_thumb_cmc_pitch", + "rh_thumb_ip", + "rh_pinky_mcp_pitch", +) + + +def _zero_profile() -> ZeroCalibrationProfile: + specs = { + "rh_thumb_cmc_yaw": JointSpec( + "rh_thumb_cmc_yaw", 1, True, "top", "top_base", "thumb_yaw" + ), + "rh_thumb_cmc_pitch": JointSpec( + "rh_thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_pitch" + ), + "rh_thumb_ip": JointSpec( + "rh_thumb_ip", 0, False, "front", "thumb_pitch", "thumb_ip" + ), + "rh_pinky_mcp_pitch": JointSpec( + "rh_pinky_mcp_pitch", 5, True, "side", "side_base", "pinky_pitch" + ), + } + hand = HandCalibrationProfile( + side="right", + reference_finger="pinky", + view_tags={}, + preflight_view_roles={}, + joint_specs=specs, + sweep_specs=(), + image_trajectory_joints=frozenset(), + roll_clearance_commands={}, + thumb_pitch_clearance_commands={}, + layout_id=KEY.layout, + model="O6", + command_names=COMMAND_NAMES, + baseline_command=(255,) * 6, + directional_zero=True, + isolated_holdout=True, + ) + return ZeroCalibrationProfile( + hand=hand, + direct_zero_joints=("rh_thumb_cmc_yaw", "rh_thumb_cmc_pitch"), + axis_joints=O6_THUMB_AXIS_JOINTS, + inherited_zero_joints={}, + inherited_static_zero_joints={}, + constrained_circle_joints=frozenset(O6_THUMB_AXIS_JOINTS), + root_anchor_joints=frozenset({"rh_thumb_cmc_yaw"}), + axis_parent_joint={"rh_thumb_cmc_pitch": "rh_thumb_cmc_yaw"}, + phase_parent_joint={"rh_thumb_ip": "rh_thumb_cmc_pitch"}, + offset_observer_joint={ + "rh_thumb_cmc_yaw": "rh_thumb_cmc_pitch", + "rh_thumb_cmc_pitch": "rh_thumb_ip", + }, + same_view_axis_pair_by_offset={}, + fixed_direct_zero_offsets_rad={}, + static_output_zero_offsets_rad={}, + base_pose_strategy="thumb_serial", + orientation_anchor_joint="rh_pinky_mcp_pitch", + directed_base_axis_joints=frozenset({ + "rh_thumb_cmc_yaw", "rh_pinky_mcp_pitch" + }), + ) + + +def _travel(fit: JointCurveFit) -> float: + value = 0.5 * ( + float(fit.decreasing_rad[0] - fit.decreasing_rad[255]) + + float(fit.increasing_rad[0] - fit.increasing_rad[255]) + ) + if not math.isfinite(value) or value <= 0.0: + raise ValueError("O6 fitted travel must be finite and positive") + return value + + +def _endpoint_offsets( + source_urdf: str | Path, + curves: Mapping[str, JointCurveFit], +) -> tuple[dict[str, float], dict[str, float]]: + joints = { + str(joint.get("name")): joint + for joint in ET.parse(Path(source_urdf)).getroot().findall("joint") + } + offsets: dict[str, float] = {} + travels: dict[str, float] = {} + for name in sorted(CALIBRATED_ACTIVE_JOINTS): + joint = joints.get(name) + limit = None if joint is None else joint.find("limit") + if limit is None or limit.get("lower") is None: + raise ValueError(f"O6 source joint has incomplete limits: {name}") + travel = _travel(curves[name]) + offset = float(limit.get("lower")) + if ENDPOINT_ANCHOR_BY_JOINT[name] != "lower_at_start": + raise ValueError(f"unsupported O6 endpoint anchor: {name}") + if not math.isfinite(offset) or abs(offset) > math.radians(15.0): + raise ValueError(f"O6 endpoint zero offset exceeds 15 degrees: {name}") + offsets[name] = offset + travels[name] = travel + return offsets, travels + + +def _has_complete_geometry( + records: Mapping[str, Sequence[Mapping[str, object]]], +) -> bool: + required = { + "relative_translation_xyz_m", "parent_pose_common", "child_pose_common", + "view_normal_common_xyz", "camera_center_common_xyz_m", "state_u8", + } + return all( + rows and all(required.issubset(row) for row in rows) + for name in O6_THUMB_AXIS_JOINTS + for rows in (records.get(name, ()),) + ) + + +def _fit_thumb_zero( + source_urdf: str | Path, + records: Mapping[str, Sequence[Mapping[str, object]]], + curves: Mapping[str, JointCurveFit], +) -> ZeroSolveResult: + profile = _zero_profile() + measurements = [] + by_key: dict[tuple[str, int], object] = {} + for cycle in range(4): + for name in ( + "rh_thumb_cmc_yaw", "rh_thumb_cmc_pitch", "rh_pinky_mcp_pitch" + ): + rows = records[name] + measurement = fit_joint_axis_measurement( + name, + rows, + cycle=cycle, + zero_command_u8=255, + constrained_circle_joints=profile.constrained_circle_joints, + view_normal_common_xyz=rows[0]["view_normal_common_xyz"], + canonical_zero_direction="decreasing", + ) + measurement = with_depth_free_axis_projection( + measurement, rows[0]["camera_center_common_xyz_m"] + ) + measurements.append(measurement) + by_key[(name, cycle)] = measurement + rows = records["rh_thumb_ip"] + pitch_axis = by_key[("rh_thumb_cmc_pitch", cycle)] + measurement = fit_joint_axis_measurement( + "rh_thumb_ip", + rows, + cycle=cycle, + zero_command_u8=255, + axis_common_constraint=pitch_axis.axis_common_xyz, + constrained_circle_joints=profile.constrained_circle_joints, + view_normal_common_xyz=rows[0]["view_normal_common_xyz"], + canonical_zero_direction="decreasing", + ) + measurements.append(with_depth_free_axis_projection( + measurement, rows[0]["camera_center_common_xyz_m"] + )) + result = solve_urdf_zero_offsets( + source_urdf=source_urdf, + measurements=measurements, + curves=curves, + motor_by_joint={ + name: COMMAND_INDEX_BY_JOINT[MIMIC_SOURCE_BY_JOINT.get(name, name)] + for name in O6_THUMB_AXIS_JOINTS + }, + training_cycles=(0, 1, 2), + validation_cycle=3, + maximum_offset_rad=math.radians(15.0), + finger_maximum_offset_rad=math.radians(15.0), + joint_maximum_offset_rad={ + "rh_thumb_cmc_yaw": math.radians(15.0), + "rh_thumb_cmc_pitch": math.radians(15.0), + }, + maximum_cycle_difference_rad=math.radians(0.75), + minimum_applied_offset_rad=math.radians(0.1), + maximum_validation_mae_rad=math.radians(1.0), + maximum_validation_p95_rad=math.radians(2.0), + maximum_validation_error_rad=math.radians(3.0), + maximum_confidence_half_width_rad=math.radians(1.0), + maximum_pose_axis_line_rms_m=0.0015, + hand_type="right", + tag_layout=KEY.layout, + zero_profile=profile, + ) + if not result.passed: + details = ",".join( + f"{name}={reason}" for name, reason in sorted(result.failure_reasons.items()) + ) + raise ValueError("O6 thumb axis zero solve failed:" + details) + if ( + not math.isfinite(result.axis_line_rms_m) + or result.axis_line_rms_m > MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M + ): + raise ValueError( + "O6 thumb cross-view axis line RMS exceeds " + f"{MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M * 1000.0:.1f} mm: " + f"actual={result.axis_line_rms_m * 1000.0:.3f} mm; " + "recalibrate camera extrinsics or check the rigid checkerboard" + ) + return result + + +def fit_o6_session( + source_urdf: str | Path, + records_by_joint: Mapping[str, Sequence[Mapping[str, object]]], + *, + require_thumb_axis_zero: bool = False, +) -> O6FitResult: + expected = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS + if set(records_by_joint) != expected: + raise ValueError("O6 records must contain exactly five measured joints") + curves: dict[str, JointCurveFit] = {} + holdout: dict[str, tuple[float, ...]] = {} + cycle_curves: dict[str, dict[int, JointCurveFit]] = {} + for name in sorted(expected): + rows = [dict(row) for row in records_by_joint[name]] + 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"{name} is missing training or holdout records") + fit = fit_rotation_joint_curve(training, zero_command_u8=255) + 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"{name} isolated holdout failed") + correction_limit = math.radians( + 3.0 if name in MEASURED_PASSIVE_JOINTS else 2.0 + ) + if fit.maximum_monotonic_correction_rad > correction_limit: + raise ValueError(f"{name} monotonic correction exceeds limit") + if fit.maximum_hysteresis_rad > math.radians(MAXIMUM_HYSTERESIS_DEG): + raise ValueError( + f"{name} hysteresis {math.degrees(fit.maximum_hysteresis_rad):.3f} " + f"degrees exceeds O6 limit {MAXIMUM_HYSTERESIS_DEG:.3f} degrees" + ) + curves[name] = fit + holdout[name] = errors + cycle_curves[name] = { + cycle: fit_rotation_joint_curve( + [row for row in training if int(row["cycle"]) == cycle], + zero_command_u8=255, + ) + for cycle in (0, 1, 2) + } + offsets, travels = _endpoint_offsets(source_urdf, curves) + if require_thumb_axis_zero and not _has_complete_geometry(records_by_joint): + raise ValueError( + "O6 thumb absolute zero requires common-frame Tag pose trajectories" + ) + thumb_result = None + zero_methods = { + name: "mechanical_lower_endpoint" for name in CALIBRATED_ACTIVE_JOINTS + } + if _has_complete_geometry(records_by_joint): + thumb_result = _fit_thumb_zero(source_urdf, records_by_joint, curves) + for name in GEOMETRIC_ZERO_JOINTS: + offsets[name] = float(thumb_result.direct_offsets_rad[name]) + zero_methods[name] = "urdf_serial_axis_geometry" + mimic_fits: dict[str, MimicFit] = {} + for target in sorted(MEASURED_PASSIVE_JOINTS): + source = MIMIC_SOURCE_BY_JOINT[target] + pairs = [] + for cycle in (0, 1, 2): + active = cycle_curves[source][cycle] + passive = cycle_curves[target][cycle] + pairs.append(( + tuple(active.decreasing_rad) + tuple(active.increasing_rad), + tuple(passive.decreasing_rad) + tuple(passive.increasing_rad), + )) + mimic_fits[target] = fit_coupling_model( + source, + target, + curves[source], + curves[target], + model=COUPLING_MODEL_BY_JOINT[target], + cycle_curve_pairs=pairs, + maximum_multiplier=2.2, + maximum_residual_p95_rad=math.radians( + COUPLING_RESIDUAL_P95_DEG + ), + maximum_residual_rad=math.radians(COUPLING_RESIDUAL_MAX_DEG), + ) + return O6FitResult( + curves=curves, + zero_offsets_rad=offsets, + travels_rad=travels, + mimic_fits=mimic_fits, + holdout_errors_rad=holdout, + zero_method_by_joint=zero_methods, + zero_fallback_reason_by_joint={}, + thumb_zero_result=thumb_result, + ) + + +__all__ = ["MimicFit", "O6FitResult", "O6_THUMB_AXIS_JOINTS", "fit_o6_session"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/node.py new file mode 100644 index 0000000..95e9219 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/node.py @@ -0,0 +1,38 @@ +"""Online three-view acquisition node for O6/right/o6_right_8/v1.""" + +from __future__ import annotations + +import rclpy + +from ..l6.node import L6ThreeCameraCalibrationNode, MotionStep +from .pipeline import finalize_o6_session +from .profile import build_typed_profile + + +class O6ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): + """O6 specialization of the shared six-channel acquisition node.""" + + def __init__(self) -> None: + super().__init__( + profile=build_typed_profile(), + finalizer=finalize_o6_session, + sample_kind="o6_joint_sample", + ) + + +def main(args: list[str] | None = None) -> None: + rclpy.init(args=args) + node: O6ThreeCameraCalibrationNode | None = None + try: + node = O6ThreeCameraCalibrationNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +__all__ = ["MotionStep", "O6ThreeCameraCalibrationNode", "main"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/pipeline.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/pipeline.py new file mode 100644 index 0000000..e0445b4 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/pipeline.py @@ -0,0 +1,179 @@ +"""Shared online/offline finalization path for O6 right.""" + +from __future__ import annotations + +from datetime import datetime +import json +import math +from pathlib import Path +from typing import Any, Mapping, Sequence + +from .artifacts import ( + artifact_hashes, + atomic_write_json, + build_o6_runtime_payload, + build_o6_urdf_input_payload, + load_o6_urdf_input, + publish_partial_session, +) +from .fitting import O6FitResult, fit_o6_session +from .profile import ( + CALIBRATED_ACTIVE_JOINTS, + CORRECTED_PASSIVE_JOINTS, + KEY, + MEASURED_PASSIVE_JOINTS, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, +) +from .urdf import O6UrdfCorrection, write_o6_corrected_urdf + + +MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS + + +def _canonical_feedback_u8(value: float) -> int: + command = int(round(float(value))) + if command <= 2: + return 0 + if command >= 253: + return 255 + return command + + +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") == "o6_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} + geometric = ( + "relative_translation_xyz_m", "parent_pose_common", "child_pose_common", + "view_normal_common_xyz", "camera_center_common_xyz_m", "state_u8", + ) + 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"]), + "command_u8": _canonical_feedback_u8(float(row["feedback_u8"])), + "feedback_u8": float(row["feedback_u8"]), + "relative_quaternion_xyzw": list(row["relative_quaternion_xyzw"]), + } + if any(field in row for field in geometric): + missing = [field for field in geometric if field not in row] + if missing: + raise ValueError("O6 geometric sample is incomplete: " + ",".join(missing)) + for field in geometric: + accepted[field] = dict(row[field]) if isinstance(row[field], Mapping) else list(row[field]) + result[str(row["joint"])].append(accepted) + return result + + +def load_o6_raw_samples(path: str | Path) -> list[dict[str, Any]]: + source = Path(path).expanduser().resolve() + if not source.is_file(): + raise ValueError(f"raw O6 sample file does not exist: {source}") + rows = [] + for number, line in enumerate(source.read_text(encoding="utf-8").splitlines(), 1): + if not line.strip(): + continue + try: + value = json.loads(line) + except json.JSONDecodeError as error: + raise ValueError(f"invalid O6 JSONL record at line {number}") from error + if not isinstance(value, Mapping): + raise ValueError(f"O6 JSONL line {number} is not an object") + rows.append(dict(value)) + return rows + + +def finalize_o6_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], O6FitResult, O6UrdfCorrection]: + directory = Path(session_dir).expanduser().resolve() + directory.mkdir(parents=True, exist_ok=True) + result = fit_o6_session( + source_urdf, accepted_records_by_joint(records), require_thumb_axis_zero=True + ) + payload = build_o6_runtime_payload( + serial_number=serial_number, + source_urdf=source_urdf, + result=result, + protected_inputs=protected_inputs, + passed=True, + ) + correction_input = directory / f"o6_right_{serial_number}_urdf_correction_input.json" + atomic_write_json(correction_input, build_o6_urdf_input_payload( + serial_number=serial_number, source_urdf=source_urdf, result=result + )) + authenticated = load_o6_urdf_input( + correction_input, source_urdf=source_urdf, serial_number=serial_number + ) + correction = write_o6_corrected_urdf( + source_urdf=source_urdf, + output_directory=directory, + serial_number=serial_number, + result=authenticated, + timestamp=timestamp or datetime.now().strftime("%Y%m%d_%H%M%S"), + ) + json_path = directory / f"o6_right_{serial_number}_partial_calibration.json" + atomic_write_json(json_path, payload) + summary = { + "schema_version": 1, + "profile_id": KEY.profile_id, + "serial_number": str(serial_number), + "result": "PARTIAL_PASS", + "publication_pointer": "latest_partial_passed", + "calibrated_active_joints": sorted(CALIBRATED_ACTIVE_JOINTS), + "measured_passive_joints": sorted(MEASURED_PASSIVE_JOINTS), + "transferred_active_joints": dict(sorted(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items())), + "transferred_passive_joints": dict(sorted(TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items())), + "passive_coupling": { + name: { + "model": result.mimic_fits[donor].model, + "coefficients": [ + round(float(value), 10) + for value in result.mimic_fits[donor].coefficients + ], + "urdf_mimic_enabled": True, + "urdf_mimic_policy": result.mimic_fits[donor].urdf_mimic_policy, + "mimic_multiplier": round( + float(result.mimic_fits[donor].urdf_mimic_multiplier), 10 + ), + "residual_p95_deg": round(math.degrees(float(result.mimic_fits[donor].residual_p95_rad)), 6), + "residual_max_deg": round(math.degrees(float(result.mimic_fits[donor].residual_max_rad)), 6), + } + for name, donor in sorted({ + target: TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(target, target) + for target in CORRECTED_PASSIVE_JOINTS + }.items()) + }, + "explicit_runtime_joints": sorted(correction.explicit_runtime_joints), + "artifacts": artifact_hashes(json_path, correction.path), + "quality": {"passed": True}, + } + atomic_write_json(directory / "calibration_summary.json", summary) + if publish: + publish_partial_session(directory.parent, directory) + return payload, result, correction + + +__all__ = [ + "accepted_records_by_joint", "finalize_o6_session", "load_o6_raw_samples" +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/profile.py new file mode 100644 index 0000000..127eded --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/profile.py @@ -0,0 +1,322 @@ +"""Reviewed partial-calibration profile for the right O6 eight-Tag rig.""" + +from __future__ import annotations + +from ...core import ( + ArtifactPolicy, + CalibrationProfile, + CommandLayout, + MeasurementPolicy, + MeasurementSpec, + MotionPolicy, + ProfileKey, + QualityPolicy, + ScopePolicy, + TagSpec, + TaskSpec, + ViewSpec, + VisionRigSpec, + ZeroSolvePolicy, +) +from ..l6.motion import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, +) +from ..registry import EngineBindings, RegisteredProfile + + +KEY = ProfileKey("O6", "right", "o6_right_8", 1) + +BASELINE_SPEED_U8 = 80 +PREFLIGHT_SPEED_U8 = 60 +FORMAL_SPEED_U8 = 40 +MAXIMUM_HYSTERESIS_DEG = 3.5 +COUPLING_RESIDUAL_P95_DEG = 2.2 +COUPLING_RESIDUAL_MAX_DEG = 3.0 +MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX = 1.5 +MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M = 0.020 + +COMMAND_NAMES: tuple[str, ...] = ( + "thumb_cmc_pitch", + "thumb_cmc_yaw", + "index_mcp_pitch", + "middle_mcp_pitch", + "ring_mcp_pitch", + "pinky_mcp_pitch", +) + +ACTIVE_JOINTS: tuple[str, ...] = ( + "rh_thumb_cmc_pitch", + "rh_thumb_cmc_yaw", + "rh_index_mcp_pitch", + "rh_middle_mcp_pitch", + "rh_ring_mcp_pitch", + "rh_pinky_mcp_pitch", +) +PASSIVE_JOINTS: tuple[str, ...] = ( + "rh_thumb_ip", + "rh_index_dip", + "rh_middle_dip", + "rh_ring_dip", + "rh_pinky_dip", +) +CALIBRATED_ACTIVE_JOINTS = frozenset( + {"rh_thumb_cmc_pitch", "rh_thumb_cmc_yaw", "rh_pinky_mcp_pitch"} +) +# Field validation on O6_RIGHT_001 showed that the yaw zero is reliably +# observable from cross-view axis directions, while the pitch phase depends +# on two monocular axis-line centres and can acquire a repeatable systematic +# bias. Keep the mechanically established CAD lower endpoint for pitch. +GEOMETRIC_ZERO_JOINTS = frozenset({"rh_thumb_cmc_yaw"}) +CAD_ENDPOINT_ZERO_JOINTS = frozenset( + {"rh_thumb_cmc_pitch", "rh_pinky_mcp_pitch"} +) +MEASURED_PASSIVE_JOINTS = frozenset({"rh_thumb_ip", "rh_pinky_dip"}) + +TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = { + "rh_index_mcp_pitch": "rh_pinky_mcp_pitch", + "rh_middle_mcp_pitch": "rh_pinky_mcp_pitch", + "rh_ring_mcp_pitch": "rh_pinky_mcp_pitch", +} +TRANSFERRED_PASSIVE_SOURCE_BY_JOINT = { + "rh_index_dip": "rh_pinky_dip", + "rh_middle_dip": "rh_pinky_dip", + "rh_ring_dip": "rh_pinky_dip", +} +CORRECTED_ACTIVE_JOINTS = frozenset( + CALIBRATED_ACTIVE_JOINTS | TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.keys() +) +CORRECTED_PASSIVE_JOINTS = frozenset( + MEASURED_PASSIVE_JOINTS | TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.keys() +) + +ENDPOINT_ANCHOR_BY_JOINT = { + "rh_thumb_cmc_pitch": "lower_at_start", + "rh_thumb_cmc_yaw": "lower_at_start", + "rh_pinky_mcp_pitch": "lower_at_start", +} + +COMMAND_INDEX_BY_JOINT = { + "rh_thumb_cmc_pitch": 0, + "rh_thumb_cmc_yaw": 1, + "rh_index_mcp_pitch": 2, + "rh_middle_mcp_pitch": 3, + "rh_ring_mcp_pitch": 4, + "rh_pinky_mcp_pitch": 5, +} + +MIMIC_SOURCE_BY_JOINT = { + "rh_thumb_ip": "rh_thumb_cmc_pitch", + "rh_index_dip": "rh_index_mcp_pitch", + "rh_middle_dip": "rh_middle_mcp_pitch", + "rh_ring_dip": "rh_ring_mcp_pitch", + "rh_pinky_dip": "rh_pinky_mcp_pitch", +} + +COUPLING_MODEL_BY_JOINT = { + name: "quadratic_runtime" for name in PASSIVE_JOINTS +} + + +def build_typed_profile() -> CalibrationProfile: + active = frozenset(ACTIVE_JOINTS) + passive = frozenset(PASSIVE_JOINTS) + measurements = { + "rh_thumb_cmc_yaw": MeasurementSpec( + "rh_thumb_cmc_yaw", "relative_rotation", "top", "top_base", "thumb_yaw" + ), + "rh_thumb_cmc_pitch": MeasurementSpec( + "rh_thumb_cmc_pitch", "relative_rotation", "front", "front_base", "thumb_pitch" + ), + "rh_thumb_ip": MeasurementSpec( + "rh_thumb_ip", "relative_rotation", "front", "thumb_pitch", "thumb_ip" + ), + "rh_pinky_mcp_pitch": MeasurementSpec( + "rh_pinky_mcp_pitch", "relative_rotation", "side", "side_base", "pinky_pitch" + ), + "rh_pinky_dip": MeasurementSpec( + "rh_pinky_dip", "relative_rotation", "side", "pinky_pitch", "pinky_dip" + ), + } + tasks = ( + TaskSpec( + "thumb_yaw_top", "top", 1, ("rh_thumb_cmc_yaw",), + preflight_speed_u8=PREFLIGHT_SPEED_U8, + formal_speed_u8=FORMAL_SPEED_U8, + ), + TaskSpec( + "thumb_pitch_ip_front", "front", 0, + ("rh_thumb_cmc_pitch", "rh_thumb_ip"), + preflight_speed_u8=PREFLIGHT_SPEED_U8, + formal_speed_u8=FORMAL_SPEED_U8, + ), + TaskSpec( + "pinky_pitch_dip_side", "side", 5, + ("rh_pinky_mcp_pitch", "rh_pinky_dip"), + preflight_speed_u8=PREFLIGHT_SPEED_U8, + formal_speed_u8=FORMAL_SPEED_U8, + ), + ) + coverage = { + **{ + name: ( + "measured_static_dynamic" + if name in CALIBRATED_ACTIVE_JOINTS + else "transferred_static_dynamic" + ) + for name in active + }, + **{ + name: ( + "measured_dynamic_cad_static" + if name in MEASURED_PASSIVE_JOINTS + else "transferred_dynamic_cad_static" + ) + for name in passive + }, + } + return CalibrationProfile( + key=KEY, + namespace="/o6_calibration", + command=CommandLayout( + names=COMMAND_NAMES, + baseline_u8=(255,) * 6, + command_index_by_joint=COMMAND_INDEX_BY_JOINT, + urdf_joint_by_joint={name: name for name in ACTIVE_JOINTS}, + speed_slot_by_command_index={index: index for index in range(6)}, + ), + vision=VisionRigSpec( + views=( + ViewSpec("front", ( + TagSpec("front_base", 0, fixed_reference=True), + TagSpec("thumb_pitch", 1), + TagSpec("thumb_ip", 2), + )), + ViewSpec("side", ( + TagSpec("side_base", 3, fixed_reference=True), + TagSpec("pinky_pitch", 4), + TagSpec("pinky_dip", 5), + )), + ViewSpec("top", ( + TagSpec("top_base", 6, fixed_reference=True), + TagSpec("thumb_yaw", 7), + )), + ), + common_frame="calibration_common", + extrinsic_reference_view="front", + extrinsics_quality_limits={ + "reprojection_rms_px": MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX, + "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={ + "baseline_u8": BASELINE_SPEED_U8, + "preflight_u8": PREFLIGHT_SPEED_U8, + "formal_u8": FORMAL_SPEED_U8, + "speed_settle_seconds": 0.2, + "command_trajectory_full_range_seconds": 6.0, + "torque_u8": 80, + "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(ENDPOINT_ANCHOR_BY_JOINT), + post_solve_endpoint_joints=frozenset(), + mimic_source_by_joint=MIMIC_SOURCE_BY_JOINT, + cad_frozen_joints=passive, + endpoint_anchor_by_joint=ENDPOINT_ANCHOR_BY_JOINT, + fitted_mimic_joints=MEASURED_PASSIVE_JOINTS, + coupling_model_by_joint=COUPLING_MODEL_BY_JOINT, + ), + 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={"partial": CALIBRATED_ACTIVE_JOINTS}, + frozen_joints={"partial": active - CALIBRATED_ACTIVE_JOINTS}, + default_scope="partial", + ), + artifacts=ArtifactPolicy( + output_schema_version=6, + calibration_filename="o6_right_{serial_number}_partial_calibration.json", + corrected_urdf_filename="linkerhand_o6_right_{serial_number}_partial_zero_calibrated.urdf", + protected_input_fields=frozenset({ + "source_urdf_sha256", + "camera_extrinsics_sha256", + "calibration_config_sha256", + "tag_config_sha256", + }), + publication_pointer="latest_partial_passed", + session_compatibility_tokens=frozenset({ + "o6_partial_v1", "feedback_curves_v6" + }), + 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( + profile=typed, + engine=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_NAMES", + "CORRECTED_ACTIVE_JOINTS", "CORRECTED_PASSIVE_JOINTS", + "COUPLING_MODEL_BY_JOINT", "ENDPOINT_ANCHOR_BY_JOINT", "KEY", + "MEASURED_PASSIVE_JOINTS", "MIMIC_SOURCE_BY_JOINT", "PASSIVE_JOINTS", + "TRANSFERRED_ACTIVE_SOURCE_BY_JOINT", + "TRANSFERRED_PASSIVE_SOURCE_BY_JOINT", "build_profile", "build_typed_profile", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/runner.py new file mode 100644 index 0000000..8c96a6d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/runner.py @@ -0,0 +1,225 @@ +"""One-command online runner and deterministic offline replay for O6 right.""" + +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 ...operator_report import ProgressEstimator +from .pipeline import finalize_o6_session, load_o6_raw_samples +from .profile import BASELINE_SPEED_U8 + + +_TASK_LABELS = { + "thumb_yaw_top": "拇指 CMC yaw（上面机位，ID6→ID7）", + "thumb_pitch_ip_front": "拇指 CMC pitch / IP（正面机位，ID0→ID1→ID2）", + "pinky_pitch_dip_side": "小指 MCP pitch / DIP（侧面机位，ID3→ID4→ID5）", +} + + +def render_o6_progress_zh( + status: dict[str, Any], + estimator: ProgressEstimator | None = None, +) -> str: + return render_six_channel_progress_zh( + status, + task_labels=_TASK_LABELS, + reason_renderer=lambda value: _l6_reason_zh( + value, model_name="O6" + ), + estimator=estimator, + ) + + +class _Monitor(Node): + def __init__(self) -> None: + super().__init__("o6_calibration_runner") + self.status: dict[str, Any] = {} + self.progress = _ProgressConsole(render_o6_progress_zh) + self.create_subscription(String, "/o6_calibration/status", self._status, 10) + self.start_client = self.create_client(Trigger, "/o6_calibration/start") + self.abort_client = self.create_client(Trigger, "/o6_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 _run_online( + config: ProductConfig, *, record_bag: bool, commands_enabled: bool +) -> int: + 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"O6 标定环境正在启动（相机、SDK、标定节点）；日志：{log_path}", + flush=True, + ) + process = subprocess.Popen( + _launch_command( + config, + session, + record_bag=record_bag, + commands_enabled=commands_enabled, + sdk_startup_speed_u8=BASELINE_SPEED_U8, + ), + cwd=config.workspace, + stdout=log_stream, + stderr=subprocess.STDOUT, + text=True, + start_new_session=True, + ) + rclpy.init() + monitor = _Monitor() + 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": + detail = "" + try: + lines = log_path.read_text( + encoding="utf-8", errors="replace" + ).splitlines() + significant = [ + line for line in lines + if "[ERROR]" in line + or "Traceback" in line + or "ValueError:" in line + or "RuntimeError:" in line + ] + if significant: + detail = "\n启动日志摘要：\n" + "\n".join(significant[-6:]) + except OSError: + pass + print( + "O6 启动失败；请检查六通道反馈、三相机内参和状态信息。" + + detail + + f"\n完整日志：{log_path}", + flush=True, + ) + return 2 + if not monitor.start_client.wait_for_service(timeout_sec=10.0): + print("O6 标定 /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"O6 标定未启动：{getattr(response, 'message', '')}", flush=True) + return 2 + 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("O6 标定失败：" + str(monitor.status.get("reason", "process_exit")), flush=True) + return 3 + print( + "\n".join([ + "PASS：O6 右手三主动关节与两条被动关节实测通过；" + "小指结果已迁移到食指、中指和无名指。", + f"部分结果：{config.session_root / 'latest_partial_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="O6 right partial 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=not bool(selected.validate_only or selected.offline_raw), + ) + if selected.validate_only: + print(f"配置有效：{config.profile_key.profile_id}，源URDF {config.source_urdf_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_o6_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, + }, + records=load_o6_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_o6_progress_zh"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/urdf.py new file mode 100644 index 0000000..f8c9baa --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/urdf.py @@ -0,0 +1,172 @@ +"""Minimal, auditable URDF correction for the partial O6 profile.""" + +from __future__ import annotations + +import math +from pathlib import Path +import re +from typing import Mapping +import xml.etree.ElementTree as ET + +import numpy as np + +from ...core.urdf import UrdfJointPatch, UrdfPatchSet, write_urdf_patches +from ..l6.urdf import L6UrdfCorrection, _corrected_origin_rpy +from .fitting import O6FitResult +from .profile import ( + CALIBRATED_ACTIVE_JOINTS, + CORRECTED_ACTIVE_JOINTS, + CORRECTED_PASSIVE_JOINTS, + MEASURED_PASSIVE_JOINTS, + MIMIC_SOURCE_BY_JOINT, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, +) + + +O6UrdfCorrection = L6UrdfCorrection + + +def _validate_passive_ranges( + joints: Mapping[str, ET.Element], result: O6FitResult +) -> dict[str, tuple[float, float]]: + limits: dict[str, tuple[float, float]] = {} + for target in sorted(CORRECTED_PASSIVE_JOINTS): + source = MIMIC_SOURCE_BY_JOINT[target] + active_donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(source, source) + passive_donor = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(target, target) + mimic = joints[target].find("mimic") + limit = joints[target].find("limit") + if mimic is None or limit is None or mimic.get("joint") != source: + raise ValueError(f"O6 passive topology differs from profile: {target}") + offset = float(mimic.get("offset", "0")) + fit = result.mimic_fits[passive_donor] + if fit.model != "quadratic_runtime" or len(fit.coefficients) != 2: + raise ValueError(f"O6 passive coupling must be quadratic: {target}") + grid = np.linspace(0.0, float(result.travels_rad[active_donor]), 256) + predicted = np.full_like(grid, offset) + for power, coefficient in enumerate(fit.coefficients, 1): + predicted += float(coefficient) * grid ** power + observed = np.concatenate(( + np.asarray(result.curves[passive_donor].decreasing_rad, dtype=float), + np.asarray(result.curves[passive_donor].increasing_rad, dtype=float), + )) + offset + values = np.concatenate((predicted, observed)) + if not np.all(np.isfinite(values)): + raise ValueError(f"O6 passive coupling is not finite: {target}") + measured_lower = float(np.min(values)) + measured_upper = float(np.max(values)) + # The open endpoint is the CAD zero. Tiny negative values can arise + # from circle fitting noise; a material negative range indicates a + # wrong axis/phase branch and must not be hidden by widening the URDF. + if measured_lower < offset - math.radians(1.0): + raise ValueError(f"O6 passive coupling goes below CAD zero: {target}") + lower = min(offset, measured_lower) + upper = max(offset, measured_upper) + if not lower > -math.pi or not upper < math.pi or upper <= lower: + raise ValueError(f"O6 passive fitted range is implausible: {target}") + limits[target] = (lower, upper) + return limits + + +def write_o6_corrected_urdf( + *, + source_urdf: str | Path, + output_directory: str | Path, + serial_number: str, + result: O6FitResult, + timestamp: str | None = None, +) -> O6UrdfCorrection: + del timestamp + source = Path(source_urdf).expanduser().resolve() + output = Path(output_directory).expanduser().resolve() + if not source.is_file() or "calibrated" in source.stem.lower(): + raise ValueError("source URDF must be the immutable original O6 CAD file") + if set(result.zero_offsets_rad) != CALIBRATED_ACTIVE_JOINTS: + raise ValueError("O6 zero result has the wrong active joint set") + if set(result.travels_rad) != CALIBRATED_ACTIVE_JOINTS: + raise ValueError("O6 travel result has the wrong active joint set") + if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS: + raise ValueError("O6 mimic result has the wrong passive joint set") + root = ET.parse(source).getroot() + joints = {str(joint.get("name")): joint for joint in root.findall("joint")} + required = CORRECTED_ACTIVE_JOINTS | CORRECTED_PASSIVE_JOINTS + if required - set(joints): + raise ValueError("source URDF is missing O6 calibration targets") + passive_limits = _validate_passive_ranges(joints, result) + + patches: dict[str, UrdfJointPatch] = {} + for name in sorted(CORRECTED_ACTIVE_JOINTS): + donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name) + offset = float(result.zero_offsets_rad[donor]) + travel = float(result.travels_rad[donor]) + if not math.isfinite(offset) or abs(offset) > math.radians(15.0): + raise ValueError(f"invalid O6 zero offset: {name}") + if not math.isfinite(travel) or not 0.0 < travel < math.pi: + raise ValueError(f"invalid O6 travel: {name}") + patches[name] = UrdfJointPatch( + origin_rpy=_corrected_origin_rpy(joints[name], offset), + limit_lower="0", + limit_upper=f"{travel:.15g}", + ) + multipliers: dict[str, float] = {} + polycoef: dict[str, tuple[float, ...]] = {} + for name in sorted(CORRECTED_PASSIVE_JOINTS): + donor = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name) + fit = result.mimic_fits[donor] + offset = float(joints[name].find("mimic").get("offset", "0")) + multiplier = float(fit.urdf_mimic_multiplier) + if not math.isfinite(multiplier) or multiplier <= 0.0: + raise ValueError(f"invalid O6 URDF mimic multiplier: {name}") + multipliers[name] = multiplier + coefficients = tuple(float(value) for value in fit.coefficients) + polycoef[name] = ( + offset, + *coefficients, + *(0.0 for _ in range(5 - len(coefficients))), + ) + lower, upper = passive_limits[name] + patches[name] = UrdfJointPatch( + limit_lower=f"{lower:.15g}", + limit_upper=f"{upper:.15g}", + # Match L6: generic URDF consumers retain a linkage that is exact + # at the open and closed endpoints. The schema-v6 bridge keeps + # the measured direction-aware nonlinear trajectory in between. + mimic_multiplier=f"{multiplier:.15g}", + ) + + safe_serial = str(serial_number) + if re.fullmatch(r"[A-Za-z0-9_.-]+", safe_serial) is None: + raise ValueError("serial number is invalid") + output.mkdir(parents=True, exist_ok=True) + destination = output / ( + f"linkerhand_o6_right_{safe_serial}_partial_zero_calibrated.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 O6UrdfCorrection( + path=destination, + origin_offsets_rad={ + name: float(result.zero_offsets_rad[ + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name) + ]) + for name in sorted(CORRECTED_ACTIVE_JOINTS) + }, + corrected_limits_rad={ + name: (0.0, float(result.travels_rad[ + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name) + ])) + for name in sorted(CORRECTED_ACTIVE_JOINTS) + } | passive_limits, + mimic_multipliers=multipliers, + coupling_polycoef=polycoef, + explicit_runtime_joints=frozenset(CORRECTED_PASSIVE_JOINTS), + ) + + +__all__ = ["O6UrdfCorrection", "write_o6_corrected_urdf"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py index f2b53b0..c035548 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py @@ -78,9 +78,11 @@ def get_default_registry() -> ProfileRegistry: if _DEFAULT_REGISTRY is None: from .g20 import register_profiles from .l6 import register_profiles as register_l6_profiles + from .o6 import register_profiles as register_o6_profiles registry = ProfileRegistry() register_profiles(registry) register_l6_profiles(registry) + register_o6_profiles(registry) _DEFAULT_REGISTRY = registry return _DEFAULT_REGISTRY diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/runtime_schema.py b/src/linkerhand_calibration/linkerhand_calibration/models/runtime_schema.py new file mode 100644 index 0000000..1b42c27 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/runtime_schema.py @@ -0,0 +1,25 @@ +"""Dispatch model-specific runtime artifact validation by profile identity.""" + +from __future__ import annotations + +from typing import Any, Mapping + +from ..core import ProfileKey + + +def validate_schema_v6_runtime_payload(payload: Mapping[str, Any]) -> None: + key = ProfileKey.parse(str(payload.get("profile_id", ""))) + if key.model == "L6": + from .l6.artifacts import validate_l6_runtime_payload + + validate_l6_runtime_payload(payload) + return + if key.model == "O6": + from .o6.artifacts import validate_o6_runtime_payload + + validate_o6_runtime_payload(payload) + return + raise ValueError(f"schema v6 is unsupported for {key.profile_id}") + + +__all__ = ["validate_schema_v6_runtime_payload"] diff --git a/src/linkerhand_calibration/test/test_architecture.py b/src/linkerhand_calibration/test/test_architecture.py index 787e7c4..d465df8 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) == 4 + assert len(registry) == 5 for registered in registry: validate_profile(registered.profile) assert registered.profile.zero.active_joints diff --git a/src/linkerhand_calibration/test/test_o6_right_profile.py b/src/linkerhand_calibration/test/test_o6_right_profile.py new file mode 100644 index 0000000..887a2cb --- /dev/null +++ b/src/linkerhand_calibration/test/test_o6_right_profile.py @@ -0,0 +1,477 @@ +from __future__ import annotations + +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.core import validate_profile +from linkerhand_calibration.extrinsics import matrix_payload, transform_matrix +from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel +from linkerhand_calibration.models.l6.node import MotionStep +from linkerhand_calibration.models.o6.artifacts import ( + build_o6_runtime_payload, + validate_o6_runtime_payload, +) +from linkerhand_calibration.models.o6.fitting import fit_o6_session +from linkerhand_calibration.models.o6.fitting import _zero_profile +from linkerhand_calibration.models.o6.node import O6ThreeCameraCalibrationNode +from linkerhand_calibration.models.o6.profile import ( + ACTIVE_JOINTS, + BASELINE_SPEED_U8, + COMMAND_NAMES, + FORMAL_SPEED_U8, + GEOMETRIC_ZERO_JOINTS, + KEY, + MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M, + MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX, + MAXIMUM_HYSTERESIS_DEG, + MIMIC_SOURCE_BY_JOINT, + PASSIVE_JOINTS, + PREFLIGHT_SPEED_U8, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, + build_typed_profile, +) +from linkerhand_calibration.models.o6.runner import render_o6_progress_zh +from linkerhand_calibration.models.o6.urdf import write_o6_corrected_urdf + + +PACKAGE = Path(__file__).resolve().parents[1] +SOURCE = PACKAGE / "urdf/o6_right/linkerhand_o6_right.urdf" +KINEMATIC_MODEL = UrdfKinematicModel(SOURCE) + + +def test_o6_profile_declares_reviewed_six_channel_contract() -> None: + profile = build_typed_profile() + validate_profile(profile) + assert profile.key == KEY + assert profile.namespace == "/o6_calibration" + assert profile.command.names == COMMAND_NAMES + assert profile.command.baseline_u8 == (255,) * 6 + assert profile.command.feedback_name_aliases == {} + assert profile.motion.speed_parameters["baseline_u8"] == BASELINE_SPEED_U8 == 80 + assert profile.motion.speed_parameters["preflight_u8"] == PREFLIGHT_SPEED_U8 == 60 + assert profile.motion.speed_parameters["formal_u8"] == FORMAL_SPEED_U8 == 40 + assert MAXIMUM_HYSTERESIS_DEG == 3.5 + assert MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX == 1.5 + assert MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M == 0.020 + assert GEOMETRIC_ZERO_JOINTS == frozenset({"rh_thumb_cmc_yaw"}) + assert profile.vision.extrinsics_quality_limits[ + "reprojection_rms_px" + ] == 1.5 + assert _zero_profile().directed_base_axis_joints == frozenset({ + "rh_thumb_cmc_yaw", "rh_pinky_mcp_pitch" + }) + assert all( + task.preflight_speed_u8 == PREFLIGHT_SPEED_U8 + and task.formal_speed_u8 == FORMAL_SPEED_U8 + for task in profile.motion.tasks + ) + assert profile.vision.tag_ids == frozenset(range(8)) + assert [task.key for task in profile.motion.tasks] == [ + "thumb_yaw_top", "thumb_pitch_ip_front", "pinky_pitch_dip_side" + ] + assert profile.zero.coupling_model_by_joint == { + name: "quadratic_runtime" for name in PASSIVE_JOINTS + } + + +def test_o6_profile_matches_source_urdf_topology() -> None: + root = ET.parse(SOURCE).getroot() + joints = { + str(joint.get("name")): joint + for joint in root.findall("joint") + if joint.get("type") == "revolute" + } + assert set(joints) == set(ACTIVE_JOINTS + PASSIVE_JOINTS) + for target, source in MIMIC_SOURCE_BY_JOINT.items(): + assert joints[target].find("mimic").get("joint") == source + assert all( + joints[target].find("limit").get("upper") + == joints[source].find("limit").get("upper") + for target, source in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items() + ) + assert all( + joints[target].find("limit").get("upper") + == joints[source].find("limit").get("upper") + for target, source in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items() + ) + + +def test_o6_progress_uses_the_shared_operator_layout() -> None: + text = render_o6_progress_zh({ + "state": "RUNNING", + "serial_number": "O6_TEST", + "step_index": 12, + "step_count": 61, + "step_fraction": 0.5, + "task_name": "thumb_yaw_top", + "phase": "sweep", + "cycle": 2, + "direction": "decreasing", + "attempt": 1, + "target_u8": 0, + "current_command_u8": 127.0, + "actual_u8": 128.0, + "valid_frames": 45, + "total_frames": 48, + "tag_detection_rate": 0.96, + "joint_frame_rate": 0.9375, + "recognized_tag_ids": [6, 7], + "unrecognized_tag_ids": [], + "feedback_hz": 29.8, + }) + assert "[O6_TEST] 标定中" in text + assert "拇指 CMC yaw（上面机位，ID6→ID7）" in text + assert "阶段：正式扫描（第 3/4 轮）" in text + assert "方向：递减" in text + assert "已识别 ID6/ID7" in text + + +def test_o6_baseline_failure_identifies_the_slowest_channel() -> None: + text = render_o6_progress_zh({ + "state": "PAUSED", + "reason": "mechanical_stall:baseline:all", + "serial_number": "O6_TEST", + "step_index": 0, + "step_count": 61, + "step_fraction": 0.25, + "phase": "baseline", + "cycle": None, + "direction": None, + "attempt": 1, + "target_u8": 255, + "speed_u8": BASELINE_SPEED_U8, + "command_trajectory_full_range_seconds": 6.0, + "current_command_u8": 249.381, + "actual_u8": 233.3, + "valid_frames": 0, + "total_frames": 0, + "feedback_hz": 58.9, + "command_names": list(COMMAND_NAMES), + "latest_state_u8": [255, 125, 255, 255, 255, 255], + "maximum_error_channel": "thumb_cmc_yaw", + "maximum_error_u8": 130.0, + }) + assert "六路反馈：" in text + assert "运动：速度档 80；全行程 6.0 秒余弦轨迹" in text + assert "thumb_cmc_yaw=125.0" in text + assert "最大偏差：thumb_cmc_yaw=130.0" in text + assert "原因：目标电机连续两秒没有向目标推进" in text + + +def test_o6_motion_keeps_all_non_target_channels_at_255() -> None: + published: list[list[int]] = [] + fake = SimpleNamespace( + baseline_command=(255,) * 6, + step_started_at=0.0, + step_start_state_u8=(255.0,) * 6, + command_trajectory_full_range_seconds=6.0, + step_last_command_u8=None, + step_trajectory_phase=0.0, + step_requested_u8=255.0, + _publish_command=lambda values: published.append(values), + ) + step = MotionStep("sweep", "thumb_yaw_top", 1, 0, 1, 0, "decreasing") + for tick in range(601): + O6ThreeCameraCalibrationNode._advance_step_trajectory(fake, step, tick / 100.0) + assert published[0] == [255] * 6 + assert published[-1] == [255, 0, 255, 255, 255, 255] + assert all(command[0] == 255 and command[2:] == [255] * 4 for command in published) + + +def test_o6_motion_plan_uses_o6_specific_speed_tiers() -> None: + fake = SimpleNamespace( + profile=build_typed_profile(), + baseline_speed_u8=BASELINE_SPEED_U8, + preflight_speed_u8=PREFLIGHT_SPEED_U8, + formal_speed_u8=FORMAL_SPEED_U8, + ) + steps = O6ThreeCameraCalibrationNode._build_steps(fake) + assert steps[0].phase == "baseline" + assert steps[0].speed_u8 == 80 + assert {step.speed_u8 for step in steps if step.phase == "preflight"} == {60} + assert { + step.speed_u8 + for step in steps + if step.phase in {"prepare", "sweep"} + } == {40} + + +def _valid_payload() -> dict: + profile = build_typed_profile() + curves = list(np.linspace(1.0, 0.0, 256)) + joints = {} + for name in ACTIVE_JOINTS + PASSIVE_JOINTS: + source = MIMIC_SOURCE_BY_JOINT.get(name, name) + motor = profile.command.command_index_by_joint[source] + passive = name in PASSIVE_JOINTS + item = { + "urdf_joint": name, + "sdk_channel": COMMAND_NAMES[motor], + "motor_index": motor, + "passive": passive, + "calibration_status": profile.joint_coverage[name], + "zero_command_u8": 255, + "zero_angles": {"policy": "test"}, + "angle_rad": curves, + "decreasing_rad": curves, + "increasing_rad": curves, + } + if passive: + item.update({ + "source_joint": source, + "mimic_offset_rad": 0.0, + "coupling_model": "quadratic_runtime", + "coupling_coefficients": [0.0, 1.0, 0.0, 0.0, 0.0, 0.0], + "urdf_mimic_enabled": True, + "urdf_mimic_policy": "endpoint_linear_fallback", + "mimic_multiplier": 1.0, + }) + donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name) or TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name) + if donor: + item["transferred_from_joint"] = donor + joints[name] = item + return { + "schema_version": 6, + "profile_id": KEY.profile_id, + "layout_id": KEY.layout, + "model": "O6", + "side": "right", + "serial_number": "O6_TEST", + "calibration_scope": "partial", + "publication_pointer": "latest_partial_passed", + "angle_unit": "rad", + "command_range": [0, 255], + "curve_input_domain": "feedback_u8", + "runtime_curve_policy": "direction_aware", + "command_names": list(COMMAND_NAMES), + "baseline_command_u8": [255] * 6, + "protected_inputs": { + "source_urdf_sha256": "0" * 64, + "camera_extrinsics_sha256": "1" * 64, + "calibration_config_sha256": "2" * 64, + "tag_config_sha256": "3" * 64, + }, + "joints": joints, + "quality": {"passed": True, "scope": "partial"}, + } + + +def test_o6_schema_v6_and_bridge_use_native_yaw_channel() -> None: + payload = _valid_payload() + validate_o6_runtime_payload(payload) + mapper = CalibratedCommandMapper(payload, expected_side="right") + assert mapper.feedback_name_aliases == {} + assert mapper.command_names == COMMAND_NAMES + assert len(mapper.map_positions([255] * 6, COMMAND_NAMES)) == 11 + mismatched = {**payload, "model": "L6"} + with pytest.raises(ValueError, match="identity or command contract"): + CalibratedCommandMapper(mismatched) + + +def _curve_records(travel: float) -> list[dict]: + rows = [] + for cycle in range(4): + for direction, commands in ( + ("decreasing", range(255, -1, -4)), + ("increasing", range(0, 256, 4)), + ): + values = list(commands) + endpoint = 0 if direction == "decreasing" else 255 + if values[-1] != endpoint: + values.append(endpoint) + for feedback in values: + angle = travel * (255.0 - feedback) / 255.0 + rows.append({ + "cycle": cycle, + "direction": direction, + "command_u8": feedback, + "feedback_u8": float(feedback), + "relative_quaternion_xyzw": Rotation.from_rotvec( + [angle, 0.0, 0.0] + ).as_quat().tolist(), + }) + return rows + + +def test_o6_fit_artifact_bridge_and_urdf_patch_end_to_end(tmp_path: Path) -> None: + records = { + "rh_thumb_cmc_pitch": _curve_records(0.5), + "rh_thumb_cmc_yaw": _curve_records(1.2), + "rh_thumb_ip": _curve_records(0.5 * 1.86), + "rh_pinky_mcp_pitch": _curve_records(1.2), + "rh_pinky_dip": _curve_records(1.2 * 0.89), + } + result = fit_o6_session(SOURCE, records) + assert result.mimic_fits["rh_thumb_ip"].multiplier == pytest.approx(1.86) + assert result.mimic_fits["rh_pinky_dip"].multiplier == pytest.approx(0.89) + payload = build_o6_runtime_payload( + serial_number="O6_TEST", + source_urdf=SOURCE, + result=result, + protected_inputs={ + "source_urdf_sha256": "0" * 64, + "camera_extrinsics_sha256": "1" * 64, + "calibration_config_sha256": "2" * 64, + "tag_config_sha256": "3" * 64, + }, + ) + mapper = CalibratedCommandMapper(payload, expected_side="right") + assert payload["quality"]["maximum_hysteresis_limit_rad"] == pytest.approx( + np.deg2rad(3.5), abs=1e-8 + ) + assert set(payload["quality"]["maximum_hysteresis_by_joint_rad"]) == set( + ACTIVE_JOINTS[:2] + (ACTIVE_JOINTS[-1],) + PASSIVE_JOINTS[::4] + ) + mapped = dict(zip(mapper.urdf_joint_names, mapper.map_positions([0] * 6))) + assert mapped["rh_thumb_ip"] == pytest.approx(0.93, abs=2e-4) + assert mapped["rh_index_mcp_pitch"] == pytest.approx(1.2, abs=2e-4) + correction = write_o6_corrected_urdf( + source_urdf=SOURCE, + output_directory=tmp_path, + serial_number="O6_TEST", + result=result, + ) + corrected = ET.parse(correction.path).getroot() + joints = {joint.get("name"): joint for joint in corrected.findall("joint")} + assert result.mimic_fits["rh_thumb_ip"].model == "quadratic_runtime" + assert result.mimic_fits["rh_pinky_dip"].model == "quadratic_runtime" + assert float(joints["rh_thumb_ip"].find("mimic").get("multiplier")) == pytest.approx(1.86) + assert float(joints["rh_pinky_dip"].find("mimic").get("multiplier")) == pytest.approx(0.89) + assert set(correction.mimic_multipliers) == set(PASSIVE_JOINTS) + assert correction.explicit_runtime_joints == frozenset(PASSIVE_JOINTS) + assert float(joints["rh_thumb_ip"].find("limit").get("upper")) == pytest.approx(0.93, abs=2e-4) + assert float(joints["rh_index_mcp_pitch"].find("limit").get("upper")) == pytest.approx(1.2) + + +def _pose(xyz: list[float], rpy: list[float]) -> np.ndarray: + return transform_matrix(xyz, Rotation.from_euler("xyz", rpy).as_quat()) + + +def _geometric_records() -> dict[str, list[dict]]: + model = KINEMATIC_MODEL + travels = { + "rh_thumb_cmc_pitch": 0.5, + "rh_thumb_cmc_yaw": 1.2, + "rh_pinky_mcp_pitch": 1.2, + } + offsets = { + "rh_thumb_cmc_pitch": -0.02, + "rh_thumb_cmc_yaw": 0.03, + "rh_pinky_mcp_pitch": 0.0, + } + views = { + "rh_thumb_cmc_pitch": "front", + "rh_thumb_cmc_yaw": "top", + "rh_thumb_ip": "front", + "rh_pinky_mcp_pitch": "side", + "rh_pinky_dip": "side", + } + motors = { + "rh_thumb_cmc_pitch": 0, + "rh_thumb_cmc_yaw": 1, + "rh_thumb_ip": 0, + "rh_pinky_mcp_pitch": 5, + "rh_pinky_dip": 5, + } + passive_sources = { + "rh_thumb_ip": "rh_thumb_cmc_pitch", + "rh_pinky_dip": "rh_pinky_mcp_pitch", + } + base = _pose([0.11, -0.04, 0.72], [0.11, -0.08, 0.17]) + base_mounts = { + "front": _pose([0.0, 0.008, 0.055], [0.2, -0.1, 0.3]), + "side": _pose([-0.01, 0.002, 0.045], [-0.1, 0.2, -0.2]), + "top": _pose([0.012, -0.006, 0.05], [0.1, 0.3, 0.15]), + } + link_mounts = { + "rh_thumb_cmc_yaw": _pose([0.012, 0.019, 0.006], [0.2, 0.1, -0.1]), + "rh_thumb_cmc_pitch": _pose([0.009, -0.028, 0.011], [-0.2, 0.1, 0.25]), + "rh_thumb_ip": _pose([0.006, -0.025, 0.018], [0.15, -0.1, 0.2]), + "rh_pinky_mcp_pitch": _pose([0.006, 0.004, 0.021], [-0.1, 0.2, 0.1]), + "rh_pinky_dip": _pose([0.004, 0.003, 0.019], [0.12, 0.08, -0.2]), + } + result = {name: [] for name in views} + for name, view in views.items(): + for cycle in range(4): + for direction, commands in ( + ("decreasing", range(255, -1, -8)), + ("increasing", range(0, 256, 8)), + ): + values = list(commands) + endpoint = 0 if direction == "decreasing" else 255 + if values[-1] != endpoint: + values.append(endpoint) + for feedback in values: + state = [255.0] * 6 + state[motors[name]] = float(feedback) + active = { + joint: travel * (255.0 - state[motor]) / 255.0 + for joint, travel, motor in ( + ("rh_thumb_cmc_pitch", 0.5, 0), + ("rh_thumb_cmc_yaw", 1.2, 1), + ("rh_pinky_mcp_pitch", 1.2, 5), + ) + } + angles = { + **active, + "rh_thumb_ip": active["rh_thumb_cmc_pitch"] * 1.86, + "rh_pinky_dip": active["rh_pinky_mcp_pitch"] * 0.89, + } + if name in passive_sources: + parent_name = passive_sources[name] + parent = base @ model.link_transform( + parent_name, + zero_offsets=offsets, + joint_angles=angles, + independent_mimic_angles=True, + ) @ link_mounts[parent_name] + else: + parent = base @ base_mounts[view] + child = base @ model.link_transform( + name, + zero_offsets=offsets, + joint_angles=angles, + independent_mimic_angles=True, + ) @ link_mounts[name] + relative = np.linalg.inv(parent) @ child + result[name].append({ + "cycle": cycle, + "direction": direction, + "command_u8": feedback, + "feedback_u8": float(feedback), + "relative_quaternion_xyzw": Rotation.from_matrix( + relative[:3, :3] + ).as_quat().tolist(), + "relative_translation_xyz_m": relative[:3, 3].tolist(), + "parent_pose_common": matrix_payload(parent), + "child_pose_common": matrix_payload(child), + "view_normal_common_xyz": [0.577350269] * 3, + "camera_center_common_xyz_m": [0.0, 0.0, -0.5], + "state_u8": state, + }) + return result + + +def test_o6_uses_geometric_yaw_and_cad_endpoint_pitch() -> None: + result = fit_o6_session( + SOURCE, _geometric_records(), require_thumb_axis_zero=True + ) + assert result.zero_offsets_rad["rh_thumb_cmc_yaw"] == pytest.approx(0.03, abs=1e-6) + assert result.zero_offsets_rad["rh_thumb_cmc_pitch"] == pytest.approx(0.0) + assert result.zero_method_by_joint["rh_thumb_cmc_yaw"] == ( + "urdf_serial_axis_geometry" + ) + assert result.zero_method_by_joint["rh_thumb_cmc_pitch"] == ( + "mechanical_lower_endpoint" + ) + assert result.zero_fallback_reason_by_joint == {} + assert result.thumb_zero_result.direct_offsets_rad[ + "rh_thumb_cmc_pitch" + ] == pytest.approx(-0.02, abs=1e-6) diff --git a/src/linkerhand_calibration/test/test_urdf_zero.py b/src/linkerhand_calibration/test/test_urdf_zero.py index 016a7fb..8830c16 100644 --- a/src/linkerhand_calibration/test/test_urdf_zero.py +++ b/src/linkerhand_calibration/test/test_urdf_zero.py @@ -483,6 +483,34 @@ def test_extrinsics_round_trip_keeps_camera_identity(tmp_path: Path) -> None: intrinsics_sha256=cameras["front"]["intrinsics_sha256"], ) + relaxed_destination = tmp_path / "relaxed_extrinsics.yaml" + relaxed_quality_limits = { + "reprojection_rms_px": 1.5, + "maximum_rotation_repeatability_deg": 0.3, + "maximum_translation_repeatability_m": 0.0015, + } + dump_three_camera_extrinsics( + relaxed_destination, + cameras=cameras, + front_from_view=transforms, + quality={ + "passed": True, + "reprojection_rms_px": 1.4, + "maximum_rotation_repeatability_deg": 0.2, + "maximum_translation_repeatability_m": 0.001, + "front_side_captures": 15, + "front_top_captures": 15, + }, + quality_limits=relaxed_quality_limits, + ) + with pytest.raises(ValueError, match="exceeds 1.2"): + load_three_camera_extrinsics(relaxed_destination) + relaxed = load_three_camera_extrinsics( + relaxed_destination, + quality_limits=relaxed_quality_limits, + ) + assert relaxed.quality["reprojection_rms_px"] == pytest.approx(1.4) + def _joint_origin(path: Path, name: str) -> tuple[np.ndarray, np.ndarray]: joint = next(