diff --git a/.gitignore b/.gitignore index 4d6c492..390ae2a 100644 --- a/.gitignore +++ b/.gitignore @@ -69,6 +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 /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/gui_control/gui_control/config/constants.py b/src/gui_control/gui_control/config/constants.py index a80e5ae..3049fa5 100644 --- a/src/gui_control/gui_control/config/constants.py +++ b/src/gui_control/gui_control/config/constants.py @@ -230,7 +230,7 @@ _HAND_CONFIGS: Dict[str, HandConfig] = { } ), "L6": HandConfig( - joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"], + joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"], joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"], init_pos=[250] * 6, preset_actions={ diff --git a/src/gui_control/gui_control/gui_control.py b/src/gui_control/gui_control/gui_control.py index bc24ce1..9e8dae0 100644 --- a/src/gui_control/gui_control/gui_control.py +++ b/src/gui_control/gui_control/gui_control.py @@ -33,6 +33,10 @@ _CANONICAL_COMMAND_NAMES = { "thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", ], + "L6": [ + "thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", + "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", + ], } _CANONICAL_COMMAND_BOUNDS = { @@ -43,6 +47,7 @@ _CANONICAL_COMMAND_BOUNDS = { *[(0, 255)] * 5, ], "O6": [(0, 255)] * 6, + "L6": [(0, 255)] * 6, } diff --git a/src/gui_control/test/test_l6_config.py b/src/gui_control/test/test_l6_config.py new file mode 100644 index 0000000..dd526f9 --- /dev/null +++ b/src/gui_control/test/test_l6_config.py @@ -0,0 +1,12 @@ +from gui_control.config.constants import HAND_CONFIGS + + +def test_l6_gui_uses_the_sdk_channel_order() -> None: + assert HAND_CONFIGS["L6"].joint_names_en == [ + "thumb_cmc_pitch", + "thumb_cmc_roll", + "index_mcp_pitch", + "middle_mcp_pitch", + "ring_mcp_pitch", + "pinky_mcp_pitch", + ] diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py index d5cca5d..de94aed 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py @@ -1,3 +1,5 @@ +from collections import deque + import can import time, sys import threading @@ -57,6 +59,14 @@ class LinkerHandL6Can: self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)] self.is_lock = False self.version = None + # L6 replies to a six-byte 0x01 position command with an immediate + # byte-for-byte echo on the same CAN ID. A zero-payload 0x01 state + # query also replies on that ID, but with the measured positions. + # Keep the two transactions distinct so command echoes never enter + # the published feedback stream used by calibration. + self._position_echo_lock = threading.Lock() + self._pending_position_echoes = deque(maxlen=32) + self._position_echo_timeout_seconds = 0.02 # Start the receiving thread self.running = True self.receive_thread = threading.Thread(target=self.receive_response) @@ -111,6 +121,11 @@ class LinkerHandL6Can: frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property data = [frame_property_value] + [int(val) for val in data_list] msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False) + if frame_property_value == 0x01 and len(data_list) == 6: + with self._position_echo_lock: + self._pending_position_echoes.append( + (time.monotonic(), tuple(int(value) for value in data_list)) + ) try: self.bus.send(msg) except can.CanError as e: @@ -201,7 +216,23 @@ class LinkerHandL6Can: except: return if frame_type == 0x01: # 0x01 - self.x01 = list(response_data) + response = tuple(int(value) for value in response_data) + now = time.monotonic() + is_position_echo = False + with self._position_echo_lock: + while ( + self._pending_position_echoes + and now - self._pending_position_echoes[0][0] + > self._position_echo_timeout_seconds + ): + self._pending_position_echoes.popleft() + for pending in tuple(self._pending_position_echoes): + if pending[1] == response: + self._pending_position_echoes.remove(pending) + is_position_echo = True + break + if not is_position_echo: + self.x01 = list(response) elif frame_type == 0x02: # 0x02 self.x02 = list(response_data) elif frame_type == 0x05: # Set speed @@ -391,7 +422,10 @@ class LinkerHandL6Can: return self.x35 def get_finger_order(self): - return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"] + # L6 channel 1 is the physical CMC roll actuator. Older SDK releases + # exposed the channel as ``thumb_cmc_yaw`` even though the wire order + # and mechanism have always been roll. + return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"] def show_fun_table(self): pass diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py index 81db0e2..b604bc4 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py +++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py @@ -372,7 +372,7 @@ class LinkerHandL6RS485: return [0] * 6 def get_finger_order(self): - return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"] + return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"] # -------------------------------------------------- # 便捷方法 diff --git a/src/linker_hand_ros2_sdk/test/test_l6_channel_order.py b/src/linker_hand_ros2_sdk/test/test_l6_channel_order.py new file mode 100644 index 0000000..15c8cd8 --- /dev/null +++ b/src/linker_hand_ros2_sdk/test/test_l6_channel_order.py @@ -0,0 +1,72 @@ +import ast +from collections import deque +from pathlib import Path +import sys +import threading +import time +from types import SimpleNamespace + + +PACKAGE = Path(__file__).resolve().parents[1] / "linker_hand_ros2_sdk/LinkerHand/core" +EXPECTED = [ + "thumb_cmc_pitch", + "thumb_cmc_roll", + "index_mcp_pitch", + "middle_mcp_pitch", + "ring_mcp_pitch", + "pinky_mcp_pitch", +] + + +def _finger_order(path: Path, class_name: str) -> list[str]: + module = ast.parse(path.read_text(encoding="utf-8")) + selected = next( + item + for item in module.body + if isinstance(item, ast.ClassDef) and item.name == class_name + ) + method = next( + item + for item in selected.body + if isinstance(item, ast.FunctionDef) and item.name == "get_finger_order" + ) + returned = next(item for item in method.body if isinstance(item, ast.Return)) + return ast.literal_eval(returned.value) + + +def test_l6_can_and_rs485_publish_the_same_physical_channel_order() -> None: + assert _finger_order(PACKAGE / "can/linker_hand_l6_can.py", "LinkerHandL6Can") == EXPECTED + assert _finger_order( + PACKAGE / "rs485/linker_hand_l6_rs485.py", "LinkerHandL6RS485" + ) == EXPECTED + + +def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None: + linker_hand_root = PACKAGE.parent + sys.path.insert(0, str(linker_hand_root)) + try: + from core.can.linker_hand_l6_can import LinkerHandL6Can + finally: + sys.path.remove(str(linker_hand_root)) + + hand = LinkerHandL6Can.__new__(LinkerHandL6Can) + hand.can_id = 0x27 + hand.x01 = [10, 20, 30, 40, 50, 60] + hand._position_echo_lock = threading.Lock() + command = (255, 2, 253, 253, 253, 253) + hand._pending_position_echoes = deque( + [(time.monotonic(), command)], maxlen=32 + ) + hand._position_echo_timeout_seconds = 0.02 + + hand.process_response( + SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command))) + ) + assert hand.x01 == [10, 20, 30, 40, 50, 60] + assert not hand._pending_position_echoes + + measured = (250, 3, 252, 252, 252, 252) + hand.process_response( + SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured))) + ) + assert hand.x01 == list(measured) diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index d90dbd1..c5e3672 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -1,5 +1,79 @@ # LinkerHand 专业标定包 +## L6 右手局部标定（l6_right_8/v1） + +本版只发布 `rh_thumb_cmc_pitch`、`rh_thumb_cmc_roll`、 +`rh_pinky_mcp_pitch` 的静态零位与动态曲线，以及 `rh_thumb_dip`、 +`rh_pinky_dip` 的视觉动态曲线。拇指 DIP 通过线性 mimic；小指 DIP 使用实测 +双方向运行曲线和 MuJoCo 二次 equality，因为它的传动比会随屈曲角变化。生成的 +修正 URDF 保留 `rh_pinky_dip` 的 ``，因此在普通 URDF/RViz 中仍会跟随 +MCP 运动；该线性回退精确对齐实测零位和闭合端点。中间行程的准确非线性轨迹由 +MuJoCo equality 或下述标定桥提供。根据 L6 四指同机构的实机确认，食指、中指、 +无名指的 MCP 零偏、行程、双向反馈曲线及 DIP 耦合从小指迁移；每指自己的 +`origin.xyz`、axis、mesh 和惯量保持 CAD，不把迁移结果标成 Tag 实测。四指 MCP +以反馈 255 的展开端作为 CAD lower/zero 锚点；实测行程不会再被强制压回较短的 +CAD upper，因此不会向四指 origin 写入系统性的负零偏。 +结果指针为 `latest_partial_passed`，不会被当作六路主动关节的完整标定。 + +拇指 pitch/roll 根据两组已记录六路反馈值的实机/仿真姿态对比，以反馈 255 +保持源 CAD joint zero，不叠加端点推断的静态偏置；小指及三指迁移仍以反馈 0 +机械闭合姿态对齐源 CAD upper。该策略只改变坐标锚点，不改变视觉实测的双方向 +行程曲线。现场姿态对比必须同时记录对应的六路反馈值。 + +先启动 SDK 和 GUI 做手动检查时使用： + +```bash +ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \ + -p hand_type:=right -p hand_joint:=L6 -p can:=can0 -p topic_prefix:=/l6 + +ros2 run gui_control gui_control +``` + +正式一键标定由 runner 自己启动 SDK、三相机、AprilTag 和标定节点，不要同时 +运行上面的 SDK/GUI 控制命令： + +```bash +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/l6_right_product.yaml +``` + +只检查 Profile、8 张 16 mm Tag、相机/外参哈希和只读源 URDF： + +```bash +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/l6_right_product.yaml --validate-only +``` + +离线回放与在线发布使用同一拟合/URDF写回路径： + +```bash +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/l6_right_product.yaml \ + --offline-raw calibration_output/L6_RIGHT_001/<时间戳>/raw_samples.jsonl +``` + +运行前将产品 YAML 中的 `serial_number` 改成实物串号。通道顺序固定为 pitch、 +roll、index、middle、ring、pinky；旧 SDK 反馈中的 `thumb_cmc_yaw` 仅作为第二路 +兼容别名读取，新产物和运行桥始终输出 `thumb_cmc_roll` / `rh_*` URDF 名。 +预检和正式扫描均使用 L6 硬件速度 `1` 作为上限，并由 100 Hz 余弦缓入缓出 +轨迹把完整 `255↔0` 行程固定为 `6 s`；短行程按距离同比缩短。SDK 会过滤 L6 +在同一 CAN ID 回送的位置命令回显，标定只使用状态查询返回的真实反馈。标定 +启动时还会检查重复 SDK/GUI 发布者，避免两个进程同时访问同一只手。 + +标定完成后，用生成的 JSON 将六路硬件反馈转换成包括小指 DIP 在内的 11 关节 +`JointState`： + +```bash +ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \ + hand_type:=right \ + calibration_file:=$PWD/calibration_output/L6_RIGHT_001/latest_partial_passed/l6_right_L6_RIGHT_001_partial_calibration.json +``` + +schema v6 默认订阅 `/l6/cb_right_hand_state`，发布 +`/sim/mujoco/l6/right/joint_state`。标准 URDF 的 `` 本身只支持线性关系， +所以只查看 URDF 时小指 DIP 中间行程是端点对齐的近似；需要实测轨迹时使用该桥 +或修正 URDF 内的 MuJoCo equality。 + ## G20右手正式一键标定 固定三相机和19张Tag安装完成后，用户只运行： diff --git a/src/linkerhand_calibration/config/l6_right_8_tags.yaml b/src/linkerhand_calibration/config/l6_right_8_tags.yaml new file mode 100644 index 0000000..bd295ef --- /dev/null +++ b/src/linkerhand_calibration/config/l6_right_8_tags.yaml @@ -0,0 +1,59 @@ +/l6_calibration/front/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + max_hamming: 0 + detector: + threads: 4 + decimate: 1.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [0, 1, 2] + frames: [front_base, thumb_pitch, thumb_dip] + sizes: [0.016, 0.016, 0.016] + +/l6_calibration/side/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + max_hamming: 0 + detector: + threads: 4 + decimate: 1.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [3, 4, 5] + frames: [side_base, pinky_pitch, pinky_dip] + sizes: [0.016, 0.016, 0.016] + +/l6_calibration/top/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + max_hamming: 0 + detector: + threads: 4 + decimate: 1.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [6, 7] + frames: [top_base, thumb_roll] + sizes: [0.016, 0.016] diff --git a/src/linkerhand_calibration/config/l6_right_product.yaml b/src/linkerhand_calibration/config/l6_right_product.yaml new file mode 100644 index 0000000..09a6e29 --- /dev/null +++ b/src/linkerhand_calibration/config/l6_right_product.yaml @@ -0,0 +1,37 @@ +schema_version: 2 +profile_id: L6/right/l6_right_8/v1 +model: L6 +side: right +tag_layout: l6_right_8 +namespace: /l6_calibration +serial_number: L6_RIGHT_001 +can_interface: can0 +output_root: calibration_output + +cameras: + front: + serial_number: DB2163742 + camera_name: hikrobot_front_DB2163742 + camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml + side: + serial_number: DB2163749 + camera_name: hikrobot_side_DB2163749 + camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml + top: + serial_number: DB2163739 + camera_name: hikrobot_top_DB2163739 + camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml + +artifacts: + source_urdf: package://linkerhand_calibration/urdf/l6_right/linkerhand_l6v3.1_right.urdf + source_urdf_sha256: 298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666 + camera_extrinsics: config/g20_three_camera_extrinsics.yaml + camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80 + calibration_config: package://linkerhand_calibration/config/l6_three_camera_calibration.yaml + calibration_config_sha256: 0934699c8225891e748deefef6791eb28355821b89aeadd1f7ff0b7f7b4d265f + tag_config: package://linkerhand_calibration/config/l6_right_8_tags.yaml + tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5 + +release: + required_independent_passes: 1 + static_repeatability_deg: 1.0 diff --git a/src/linkerhand_calibration/config/l6_three_camera_calibration.yaml b/src/linkerhand_calibration/config/l6_three_camera_calibration.yaml new file mode 100644 index 0000000..b43f030 --- /dev/null +++ b/src/linkerhand_calibration/config/l6_three_camera_calibration.yaml @@ -0,0 +1,67 @@ +l6_calibration: + ros__parameters: + command_topic: /l6/cb_right_hand_control_cmd + state_topic: /l6/cb_right_hand_state + setting_topic: /l6/cb_hand_setting_cmd + front_camera_info_topic: /l6_calibration/front/camera/camera_info + front_detections_topic: /l6_calibration/front/apriltag/detections + side_camera_info_topic: /l6_calibration/side/camera/camera_info + side_detections_topic: /l6_calibration/side/apriltag/detections + top_camera_info_topic: /l6_calibration/top/camera/camera_info + top_detections_topic: /l6_calibration/top/apriltag/detections + + baseline_command_u8: [255, 255, 255, 255, 255, 255] + # L6_RIGHT_001 measured a 250->5 travel of only ~0.9 s at speed 10, + # which left fewer than 32 useful feedback bins. Speed 1 is still only a + # firmware ceiling: different L6 motors complete a full stroke in 0.7-1.3 s. + # A 100 Hz cosine trajectory therefore sets the actual, model-level pace. + preflight_speed_u8: 1 + formal_speed_u8: 1 + speed_settle_seconds: 0.2 + command_trajectory_full_range_seconds: 6.0 + torque_u8: 80 + repetitions: 4 + preflight_checkpoints_u8: [255, 127, 0] + tag_size_m: 0.016 + tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7] + tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016] + + minimum_detection_rate: 0.95 + # Per-Tag quality remains >=95%. With three independently detected Tags, + # the fully joined frame rate may be 0.95^3 ~= 85.7%. + minimum_joint_frame_rate: 0.85 + minimum_feedback_hz: 25.0 + maximum_state_image_skew_ms: 50.0 + maximum_hamming: 0 + minimum_decision_margin: 30.0 + minimum_edge_pixels: 30.0 + pnp_maximum_reprojection_error_px: 1.5 + pnp_reprojection_tie_px: 1.5 + pnp_maximum_pose_jump_deg: 35.0 + pnp_maximum_translation_jump_m: 0.04 + pnp_maximum_tag_tilt_deg: 75.0 + pnp_tracker_reset_seconds: 5.0 + + minimum_sweep_frames: 40 + minimum_state_span_u8: 240.0 + minimum_sweep_bins: 32 + maximum_bin_gap: 16 + maximum_monotonic_correction_deg: 2.0 + passive_maximum_monotonic_correction_deg: 3.0 + maximum_validation_mae_deg: 1.0 + maximum_validation_p95_deg: 2.0 + maximum_validation_error_deg: 3.0 + mimic_minimum_multiplier: 0.5 + mimic_maximum_multiplier: 1.5 + mimic_maximum_cycle_range: 0.03 + mimic_maximum_residual_p95_deg: 2.0 + + endpoint_tolerance_u8: 2.0 + endpoint_hold_seconds: 1.0 + motor_stall_timeout_seconds: 2.0 + position_timeout_seconds: 30.0 + sweep_timeout_seconds: 90.0 + automatic_sweep_retry_limit: 2 + non_target_motion_tolerance_u8: 3.0 + fixed_base_maximum_corner_drift_px: 2.0 + fixed_base_movement_confirmation_frames: 5 diff --git a/src/linkerhand_calibration/launch/calibrated_joint_state_bridge.launch.py b/src/linkerhand_calibration/launch/calibrated_joint_state_bridge.launch.py index c6a19d6..e026039 100644 --- a/src/linkerhand_calibration/launch/calibrated_joint_state_bridge.launch.py +++ b/src/linkerhand_calibration/launch/calibrated_joint_state_bridge.launch.py @@ -1,4 +1,4 @@ -"""Publish calibrated G20 URDF angles from raw command/feedback u8 values.""" +"""Publish profile-calibrated URDF angles from raw command/feedback u8 values.""" from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -17,7 +17,7 @@ def generate_launch_description() -> LaunchDescription: package="linkerhand_calibration", executable="calibrated_joint_state_bridge", name=[ - "g20_calibrated_joint_state_bridge_", + "calibrated_joint_state_bridge_", LaunchConfiguration("hand_type"), ], output="screen", diff --git a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py index 3c3ef52..ca82c31 100644 --- a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py +++ b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py @@ -1,4 +1,4 @@ -"""Launch three Hikrobot views and one complete-G20 calibration owner.""" +"""Launch three Hikrobot views and one registered hand calibration owner.""" from __future__ import annotations @@ -26,11 +26,13 @@ from launch_ros.parameter_descriptions import ParameterValue VIEWS = ("front", "side", "top") -def _default_source_urdf(hand_type: str) -> Path: +def _default_source_urdf(model: str, hand_type: str) -> Path: relative = ( - Path("urdf") - / f"g20_{hand_type}" - / f"linkerhand_g20_{hand_type}.urdf" + Path("urdf") / "l6_right" / "linkerhand_l6v3.1_right.urdf" + if model.upper() == "L6" and hand_type == "right" + else Path("urdf") + / f"{model.lower()}_{hand_type}" + / f"linkerhand_{model.lower()}_{hand_type}.urdf" ) package_source_or_share = Path(__file__).resolve().parents[1] / relative try: @@ -71,6 +73,8 @@ def _launch_stack(context): / ( "three_camera_tags_g20_right_19.yaml" if tag_layout == "g20_right_19" + else "l6_right_8_tags.yaml" + if tag_layout == "l6_right_8" else "three_camera_tags_g20_right_15.yaml" if tag_layout == "g20_right_15" else "three_camera_tags.yaml" @@ -86,7 +90,7 @@ def _launch_stack(context): source_urdf = ( Path(requested_source).expanduser().resolve() if requested_source - else _default_source_urdf(hand_type).resolve() + else _default_source_urdf(model, hand_type).resolve() ) if not source_urdf.is_file(): raise RuntimeError(f"source URDF does not exist: {source_urdf}") @@ -96,7 +100,7 @@ def _launch_stack(context): if contract.typed_profile.artifacts.publish_corrected_urdf: if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None: raise RuntimeError( - "G20 right product layout requires source_urdf_expected_sha256 confirmed " + "this profile requires source_urdf_expected_sha256 confirmed " "by the CAD/hardware owner" ) actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest() @@ -142,12 +146,13 @@ def _launch_stack(context): raw_topics = [] info_topics = [] detection_topics = [] + calibration_namespace = contract.typed_profile.namespace for view in VIEWS: - namespace = f"/g20_calibration/{view}/camera" + namespace = f"{calibration_namespace}/{view}/camera" raw_topic = f"{namespace}/image_raw" info_topic = f"{namespace}/camera_info" rect_topic = f"{namespace}/image_rect" - detector_namespace = f"/g20_calibration/{view}/apriltag" + detector_namespace = f"{calibration_namespace}/{view}/apriltag" detection_topic = f"{detector_namespace}/detections" raw_topics.append(raw_topic) info_topics.append(info_topic) @@ -170,7 +175,9 @@ def _launch_stack(context): "camera_name": LaunchConfiguration( f"{view}_camera_name" ), - "frame_id": f"g20_calibration_{view}_optical_frame", + "frame_id": ( + f"{model.lower()}_calibration_{view}_optical_frame" + ), "image_width": 1624, "image_height": 1240, "frame_rate": ParameterValue( @@ -233,7 +240,7 @@ def _launch_stack(context): ) vision = ComposableNodeContainer( - name="g20_three_camera_vision", + name=f"{model.lower()}_three_camera_vision", namespace="/", package="rclcpp_components", executable="component_container_mt", @@ -265,10 +272,10 @@ def _launch_stack(context): # G20 velocity read sends another five synchronous CAN # queries, so keep it off the trajectory-critical path. "velocity_poll_rate": 1.0, - # Calibration sends one endpoint command per sweep. Keep - # polling the real motor state during the following motion; - # otherwise the SDK republishes a stale state for 0.2 s and - # creates 17-27 command-unit holes in the trajectory bins. + # G20 sends an endpoint and L6 streams a bounded trajectory. + # Keep polling the real motor state during either command path; + # otherwise the SDK republishes stale state and creates large + # command-unit holes in the trajectory bins. "defer_state_reads_while_commanding": False, "repeat_position_commands": False, "is_touch": False, @@ -278,7 +285,7 @@ def _launch_stack(context): calibration = Node( package="linkerhand_calibration", executable="three_camera_calibration_node", - name="g20_calibration", + name=f"{model.lower()}_calibration", output="screen", emulate_tty=True, arguments=[ @@ -303,7 +310,7 @@ def _launch_stack(context): # cb__hand_info has a subscriber. Calibration only used # that topic to display a speed diagnostic, while those reads # created 17-33 command-unit holes in position trajectories. - "info_topic": "/g20_calibration/disabled_hand_info", + "info_topic": f"{calibration_namespace}/disabled_hand_info", "command_topic": command_topic, "state_topic": state_topic, "camera_extrinsics_file": LaunchConfiguration( @@ -313,6 +320,15 @@ def _launch_stack(context): "source_urdf_expected_sha256": LaunchConfiguration( "source_urdf_expected_sha256" ), + "camera_extrinsics_expected_sha256": LaunchConfiguration( + "camera_extrinsics_expected_sha256" + ), + "calibration_config_expected_sha256": LaunchConfiguration( + "calibration_config_expected_sha256" + ), + "tag_config_expected_sha256": LaunchConfiguration( + "tag_config_expected_sha256" + ), "corrected_urdf_output_dir": LaunchConfiguration( "corrected_urdf_output_dir" ), @@ -367,14 +383,14 @@ def _launch_stack(context): command_topic, state_topic, info_topic, - "/g20_calibration/status", + f"{calibration_namespace}/status", ], output="screen", ) return [ LogInfo( msg=( - f"G20 {hand_type} {tag_layout} three-camera session: {session_dir}; " + f"{model} {hand_type} {tag_layout} three-camera session: {session_dir}; " f"source_urdf={source_urdf}" ) ), @@ -482,6 +498,15 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument( "source_urdf_expected_sha256", default_value="" ), + DeclareLaunchArgument( + "camera_extrinsics_expected_sha256", default_value="" + ), + DeclareLaunchArgument( + "calibration_config_expected_sha256", default_value="" + ), + DeclareLaunchArgument( + "tag_config_expected_sha256", default_value="" + ), DeclareLaunchArgument( "corrected_urdf_output_dir", default_value="" ), diff --git a/src/linkerhand_calibration/linkerhand_calibration/acquisition.py b/src/linkerhand_calibration/linkerhand_calibration/acquisition.py index 0e40a6c..1cc0b8b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/acquisition.py +++ b/src/linkerhand_calibration/linkerhand_calibration/acquisition.py @@ -117,7 +117,7 @@ def interpolate_state_u8( *, maximum_skew_ns: int, ) -> tuple[tuple[float, ...], int] | None: - """Interpolate the 20-D hand state at an image timestamp. + """Interpolate a profile-sized hand state at an image timestamp. The SDK publishes state independently from the camera. Continuous calibration must therefore use the image timestamp instead of whichever @@ -151,7 +151,11 @@ def interpolate_state_u8( fraction = before_gap / denominator before_values = np.asarray(before.position_u8, dtype=float) after_values = np.asarray(after.position_u8, dtype=float) - if before_values.shape != (20,) or after_values.shape != (20,): + if ( + before_values.ndim != 1 + or before_values.size == 0 + or after_values.shape != before_values.shape + ): return None interpolated = before_values + fraction * (after_values - before_values) return ( 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 1347746..c41fbea 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py +++ b/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py @@ -1,4 +1,4 @@ -"""Map G20 u8 feedback to URDF joint angles using one calibration JSON. +"""Map model SDK u8 feedback to URDF joint angles using one calibration JSON. The static encoder-zero corrections in ``zero_angles`` are already baked into the corrected URDF joint origins. This bridge therefore publishes only the @@ -26,6 +26,7 @@ from .full_hand import ( infer_compact_payload_layout, validate_compact_payload, ) +from .models.l6.artifacts import validate_l6_runtime_payload G20_COMMAND_NAMES: tuple[str, ...] = ( @@ -80,12 +81,16 @@ G20_URDF_JOINT_NAMES: tuple[str, ...] = ( class CalibratedCommandMapper: - """Validated, side-specific lookup from G20 u8 values to URDF radians.""" + """Validated, profile-specific lookup from SDK u8 values to URDF radians.""" def __init__( self, payload: Mapping[str, Any], *, expected_side: str | None = None ) -> None: - validate_compact_payload(payload) + schema_version = int(payload["schema_version"]) + if schema_version == 6: + validate_l6_runtime_payload(payload) + else: + validate_compact_payload(payload) side = str(payload["side"]).lower() if expected_side is not None and side != str(expected_side).lower(): raise ValueError( @@ -95,12 +100,18 @@ class CalibratedCommandMapper: quality = payload["quality"] if quality.get("passed") is not True: raise ValueError("calibration quality.passed must be true") - layout_id = infer_compact_payload_layout(payload) - profile = get_hand_calibration_profile(side, layout_id) + layout_id = ( + str(payload["layout_id"]) + if schema_version == 6 + else infer_compact_payload_layout(payload) + ) self.side = side self.layout_id = layout_id + self.model = str(payload["model"]).upper() + self.profile_id = str( + payload.get("profile_id", f"G20/{side}/{layout_id}/v1") + ) self.serial_number = str(payload["serial_number"]) - schema_version = int(payload["schema_version"]) self.input_domain = str( payload.get( "curve_input_domain", @@ -109,16 +120,29 @@ class CalibratedCommandMapper: ) if self.input_domain not in {"command_u8", "feedback_u8"}: raise ValueError("calibration curve_input_domain is invalid") - self._motor_by_joint = { - name: int(profile.joint_specs[name].motor_index) - for name in G20_URDF_JOINT_NAMES - } + if schema_version == 6: + self.command_names = tuple(str(value) for value in payload["command_names"]) + self.urdf_joint_names = tuple(str(name) for name in payload["joints"]) + self._motor_by_joint = { + name: int(payload["joints"][name]["motor_index"]) + for name in self.urdf_joint_names + } + self.feedback_name_aliases = {"thumb_cmc_yaw": "thumb_cmc_roll"} + else: + profile = get_hand_calibration_profile(side, layout_id) + self.command_names = G20_COMMAND_NAMES + self.urdf_joint_names = G20_URDF_JOINT_NAMES + self._motor_by_joint = { + name: int(profile.joint_specs[name].motor_index) + for name in self.urdf_joint_names + } + self.feedback_name_aliases = {} self._curves = { name: tuple( float(value) for value in payload["joints"][name]["angle_rad"] ) - for name in G20_URDF_JOINT_NAMES + for name in self.urdf_joint_names } self._decreasing_curves = { name: tuple( @@ -127,7 +151,7 @@ class CalibratedCommandMapper: "decreasing_rad", payload["joints"][name]["angle_rad"] ) ) - for name in G20_URDF_JOINT_NAMES + for name in self.urdf_joint_names } self._increasing_curves = { name: tuple( @@ -136,7 +160,7 @@ class CalibratedCommandMapper: "increasing_rad", payload["joints"][name]["angle_rad"] ) ) - for name in G20_URDF_JOINT_NAMES + for name in self.urdf_joint_names } self._previous_by_motor: dict[int, float] = {} self._direction_by_motor: dict[int, str] = {} @@ -161,16 +185,21 @@ class CalibratedCommandMapper: if len(set(names)) != len(names): raise ValueError("JointState names must be unique") by_name = dict(zip((str(name) for name in names), values)) - missing = [name for name in G20_COMMAND_NAMES if name not in by_name] + for alias, canonical in self.feedback_name_aliases.items(): + if alias in by_name and canonical not in by_name: + by_name[canonical] = by_name[alias] + missing = [name for name in self.command_names if name not in by_name] if missing: raise ValueError( - "G20 command is missing named channels: " + ",".join(missing) + f"{self.model} feedback is missing named channels: " + + ",".join(missing) ) - command = tuple(by_name[name] for name in G20_COMMAND_NAMES) + command = tuple(by_name[name] for name in self.command_names) else: - if len(values) != len(G20_COMMAND_NAMES): + if len(values) != len(self.command_names): raise ValueError( - "unnamed G20 command must contain exactly 20 positions" + f"unnamed {self.model} feedback must contain exactly " + f"{len(self.command_names)} positions" ) command = values indices = tuple(self._command_index(value) for value in command) @@ -185,7 +214,7 @@ class CalibratedCommandMapper: direction = "decreasing" direction_by_motor[motor] = direction result: list[float] = [] - for name in G20_URDF_JOINT_NAMES: + for name in self.urdf_joint_names: motor = self._motor_by_joint[name] direction = direction_by_motor[motor] curves = ( @@ -214,20 +243,22 @@ def load_calibrated_command_mapper( return CalibratedCommandMapper(payload, expected_side=expected_side) -def default_input_topic(hand_type: str, input_domain: str) -> str: +def default_input_topic( + hand_type: str, input_domain: str, model: str = "G20" +) -> str: side = str(hand_type).lower() if side not in {"left", "right"}: raise ValueError("hand_type must be left or right") if input_domain == "feedback_u8": - return f"/g20/cb_{side}_hand_state" + return f"/{str(model).lower()}/cb_{side}_hand_state" if input_domain == "command_u8": - return f"/g20/cb_{side}_hand_control_cmd" + return f"/{str(model).lower()}/cb_{side}_hand_control_cmd" raise ValueError("calibration curve_input_domain is invalid") class CalibratedJointStateBridge(Node): def __init__(self) -> None: - super().__init__("g20_calibrated_joint_state_bridge") + super().__init__("calibrated_joint_state_bridge") self.declare_parameter("hand_type", "right") self.declare_parameter("calibration_file", "") self.declare_parameter("input_topic", "") @@ -245,10 +276,11 @@ class CalibratedJointStateBridge(Node): input_topic = str(self.get_parameter("input_topic").value).strip() output_topic = str(self.get_parameter("output_topic").value).strip() self.input_topic = input_topic or default_input_topic( - hand_type, self.mapper.input_domain + hand_type, self.mapper.input_domain, self.mapper.model ) self.output_topic = ( - output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state" + output_topic + or f"/sim/mujoco/{self.mapper.model.lower()}/{hand_type}/joint_state" ) self.publisher = self.create_publisher(JointState, self.output_topic, 10) self.subscription = self.create_subscription( @@ -256,7 +288,8 @@ class CalibratedJointStateBridge(Node): ) self._last_error = "" self.get_logger().info( - f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: " + f"loaded {self.mapper.profile_id} calibration for " + f"{self.mapper.serial_number}: " f"{self.input_topic} ({self.mapper.input_domain}) -> " f"{self.output_topic}" ) @@ -273,7 +306,7 @@ class CalibratedJointStateBridge(Node): self._last_error = "" result = JointState() result.header = command.header - result.name = list(G20_URDF_JOINT_NAMES) + result.name = list(self.mapper.urdf_joint_names) result.position = list(positions) self.publisher.publish(result) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py index cd74a67..84d7ea9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py @@ -50,6 +50,15 @@ class CommandLayout: baseline_u8: tuple[int, ...] command_index_by_joint: Mapping[str, int] disabled_indices: frozenset[int] = frozenset() + # Calibration names are allowed to stay model-neutral while the source + # URDF keeps any vendor/side prefixes (for example ``rh_``). + urdf_joint_by_joint: Mapping[str, str] = field(default_factory=dict) + # Older SDKs occasionally published a wrong label for a physically stable + # channel. Aliases are accepted only at the declared channel index. + feedback_name_aliases: Mapping[str, str] = field(default_factory=dict) + # SDK speed commands are not necessarily one value per position channel. + # This mapping makes that protocol detail explicit in a profile. + speed_slot_by_command_index: Mapping[int, int] = field(default_factory=dict) @property def command_count(self) -> int: @@ -98,6 +107,10 @@ class TaskSpec: joints: tuple[str, ...] auxiliary_commands: tuple[tuple[int, int], ...] = () validation_only: bool = False + start_u8: int = 255 + end_u8: int = 0 + preflight_speed_u8: int | None = None + formal_speed_u8: int | None = None @dataclass(frozen=True) @@ -148,6 +161,15 @@ class ZeroSolvePolicy: post_solve_endpoint_joints: frozenset[str] mimic_source_by_joint: Mapping[str, str] cad_frozen_joints: frozenset[str] + # ``upper_at_end`` means TaskSpec.end_u8 is the trusted source-URDF upper + # physical endpoint. The measured travel then defines the electrical + # zero and corrected [0, travel] coordinate range. + endpoint_anchor_by_joint: Mapping[str, str] = field(default_factory=dict) + fitted_mimic_joints: frozenset[str] = frozenset() + # Passive coupling is not necessarily representable by the linear URDF + # ``mimic`` element. Profiles must opt in explicitly before a nonlinear + # runtime/MuJoCo relation may be published. + coupling_model_by_joint: Mapping[str, str] = field(default_factory=dict) @dataclass(frozen=True) @@ -195,6 +217,11 @@ class CalibrationProfile: quality: QualityPolicy scope: ScopePolicy artifacts: ArtifactPolicy + # Per-URDF-joint provenance used by partial calibration artifacts. + # Known values are: measured_static_dynamic, measured_dynamic_cad_static, + # transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal, + # and mimic_nominal. + joint_coverage: Mapping[str, str] = field(default_factory=dict) class ProfileValidationError(ValueError): @@ -216,6 +243,24 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append("disabled command index is out of range") if any(index not in indices for index in command.command_index_by_joint.values()): errors.append("joint command index is out of range") + if command.urdf_joint_by_joint: + if not set(command.command_index_by_joint).issubset( + command.urdf_joint_by_joint + ): + errors.append("every commanded joint must map to a URDF joint") + urdf_names = tuple(command.urdf_joint_by_joint.values()) + if len(set(urdf_names)) != len(urdf_names): + errors.append("URDF joint mappings must be unique") + if any( + index not in indices or slot < 0 + for index, slot in command.speed_slot_by_command_index.items() + ): + errors.append("speed-slot mapping is invalid") + if any( + not str(alias).strip() or canonical not in command.names + for alias, canonical in command.feedback_name_aliases.items() + ): + errors.append("feedback name alias is not part of the command schema") view_names = profile.vision.view_names if not view_names or len(set(view_names)) != len(view_names): @@ -246,6 +291,13 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append(f"task {task.key} references unknown measurements") if any(index not in indices for index, _ in task.auxiliary_commands): errors.append(f"task {task.key} auxiliary index is out of range") + if not 0 <= task.start_u8 <= 255 or not 0 <= task.end_u8 <= 255: + errors.append(f"task {task.key} sweep endpoint is out of range") + if task.start_u8 == task.end_u8: + errors.append(f"task {task.key} sweep endpoints must differ") + for speed in (task.preflight_speed_u8, task.formal_speed_u8): + if speed is not None and not 0 <= speed <= 255: + errors.append(f"task {task.key} speed is out of range") for name, spec in profile.measurement.measurements.items(): if name != spec.joint: errors.append(f"measurement mapping key differs for {name}") @@ -273,6 +325,29 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append("mimic targets must be passive joints") if not set(zero.mimic_source_by_joint.values()).issubset(all_joints): errors.append("mimic sources must be known joints") + if not set(zero.endpoint_anchor_by_joint).issubset(zero.active_joints): + errors.append("endpoint anchors must target active joints") + if not set(zero.endpoint_anchor_by_joint.values()).issubset( + { + "upper_at_end", + "lower_at_start", + "zero_at_start", + "cad_range_center", + } + ): + errors.append("endpoint anchor policy is unsupported") + if not zero.fitted_mimic_joints.issubset(zero.passive_joints): + errors.append("fitted mimic targets must be passive joints") + if not zero.fitted_mimic_joints.issubset(zero.mimic_source_by_joint): + errors.append("fitted mimic target has no source mapping") + if not set(zero.coupling_model_by_joint).issubset( + zero.mimic_source_by_joint + ): + errors.append("coupling model target has no source mapping") + if not set(zero.coupling_model_by_joint.values()).issubset( + {"linear_mimic", "quadratic_runtime"} + ): + errors.append("coupling model policy is unsupported") scopes = set(profile.scope.calibrate_joints) if profile.scope.default_scope not in scopes: @@ -297,6 +372,19 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append(f"{label} filename must not contain a directory") if not profile.namespace.startswith("/"): errors.append("runtime namespace must be absolute") + if profile.joint_coverage: + valid_coverage = { + "measured_static_dynamic", + "measured_dynamic_cad_static", + "transferred_static_dynamic", + "transferred_dynamic_cad_static", + "cad_nominal", + "mimic_nominal", + } + if set(profile.joint_coverage) != all_joints: + errors.append("joint coverage must describe every profile joint") + if not set(profile.joint_coverage.values()).issubset(valid_coverage): + errors.append("joint coverage contains an unsupported status") if errors: raise ProfileValidationError("; ".join(errors)) 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 be258b8..e4f1b5a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py @@ -67,6 +67,11 @@ class ZeroCalibrationProfile: # thumb kernel instead uses only the serial thumb chain, so its nuisance # palm pose cannot be influenced by finger observations. base_pose_strategy: str = "full_hand" + # A serial-chain model may use a separate palm-root joint axis to fix the + # otherwise free rotation about its primary root axis. G20 retains its + # 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 @property def reference_finger(self) -> str: @@ -3238,7 +3243,7 @@ def solve_urdf_zero_offsets( np.ptp(np.asarray(curves[name].angle_rad)) ), ) - orientation_anchor = ( + orientation_anchor = profile.orientation_anchor_joint or ( "thumb_cmc_pitch" if profile.base_pose_strategy == "thumb_serial" else f"{profile.reference_finger}_mcp_pitch" diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py index 680495c..adffc48 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py @@ -1 +1,12 @@ -"""Future profiles live here; no publishable profile is registered yet.""" +"""Registered L6 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/l6/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/artifacts.py new file mode 100644 index 0000000..3a4f0f7 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/artifacts.py @@ -0,0 +1,524 @@ +"""Schema-v6 runtime artifact and atomic partial-result publication for L6.""" + +from __future__ import annotations + +import json +import hashlib +import math +import os +from pathlib import Path +from typing import Any, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np + +from .fitting import L6FitResult +from .profile import ( + ACTIVE_JOINTS, + CALIBRATED_ACTIVE_JOINTS, + COMMAND_INDEX_BY_JOINT, + COMMAND_NAMES, + 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_typed_profile, +) + + +ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS) + + +def _sha256_file(path: str | Path) -> str: + digest = hashlib.sha256() + with Path(path).open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _source_joint_metadata( + source_urdf: str | Path, +) -> dict[str, dict[str, float | str]]: + root = ET.parse(Path(source_urdf)).getroot() + result: dict[str, dict[str, float | str]] = {} + for joint in root.findall("joint"): + name = str(joint.get("name", "")) + if name not in ALL_REVOLUTE_JOINTS: + continue + if joint.get("type") != "revolute": + raise ValueError(f"L6 profile joint is not revolute: {name}") + limit = joint.find("limit") + if limit is None: + raise ValueError(f"L6 source joint has no limit: {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 L6 revolute joints") + return result + + +def _rounded_curve(values: Sequence[float]) -> list[float]: + curve = np.asarray(values, dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError("runtime curve must contain 256 finite values") + return [round(float(value), 8) for value in curve] + + +def _linear_curve(lower: float, upper: float) -> np.ndarray: + # feedback 255 is the open/lower endpoint and feedback 0 is upper/closed. + return np.linspace(float(upper), float(lower), 256, dtype=float) + + +def _validate_transfer_topology( + source: Mapping[str, Mapping[str, float | str]], +) -> None: + 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(source[target][field]) - float(source[donor][field]) + ) > 1.0e-8: + raise ValueError( + f"L6 transfer {target} differs from {donor} {field}" + ) + + +def build_l6_runtime_payload( + *, + serial_number: str, + source_urdf: str | Path, + result: L6FitResult, + protected_inputs: Mapping[str, str], + passed: bool = True, +) -> dict[str, Any]: + """Build all eleven L6 curves with explicit measured/frozen provenance.""" + profile = build_typed_profile() + source = _source_joint_metadata(source_urdf) + _validate_transfer_topology(source) + joints: dict[str, dict[str, Any]] = {} + curves: dict[str, np.ndarray] = {} + + for name in ACTIVE_JOINTS: + metadata = source[name] + motor_index = COMMAND_INDEX_BY_JOINT[name] + measurement_source = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get( + name, name + ) + if ( + name in CALIBRATED_ACTIVE_JOINTS + or name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT + ): + fit = result.curves[measurement_source] + decreasing = np.asarray(fit.decreasing_rad, dtype=float) + increasing = np.asarray(fit.increasing_rad, dtype=float) + curve = 0.5 * (decreasing + increasing) + curve -= float(curve[255]) + decreasing = decreasing - float(decreasing[255]) + increasing = increasing - float(increasing[255]) + zero_method = result.zero_method_by_joint[measurement_source] + zero_angles: dict[str, Any] = { + "policy": zero_method, + "measured_travel_rad": round( + float(result.travels_rad[measurement_source]), 8 + ), + "urdf_origin_offset_rad": round( + float(result.zero_offsets_rad[measurement_source]), 8 + ), + } + if measurement_source in ENDPOINT_ANCHOR_BY_JOINT: + anchor = ENDPOINT_ANCHOR_BY_JOINT[measurement_source] + if anchor == "cad_range_center": + zero_angles.update( + { + "source_lower_rad": round( + float(metadata["lower"]), 8 + ), + "source_upper_rad": round( + float(metadata["upper"]), 8 + ), + } + ) + else: + endpoint_field = { + "lower_at_start": "source_lower_rad", + "upper_at_end": "source_upper_rad", + "zero_at_start": "source_joint_zero_rad", + }[anchor] + endpoint_value = { + "lower_at_start": metadata["lower"], + "upper_at_end": metadata["upper"], + "zero_at_start": 0.0, + }[anchor] + zero_angles[endpoint_field] = round( + float(endpoint_value), 8 + ) + if measurement_source in result.zero_fallback_reason_by_joint: + zero_angles["geometry_fallback_reason"] = ( + result.zero_fallback_reason_by_joint[measurement_source] + ) + if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT: + zero_angles.update( + { + "policy": "transferred_from_pinky", + "transferred_from_joint": measurement_source, + } + ) + else: + curve = _linear_curve( + float(metadata["lower"]), float(metadata["upper"]) + ) + decreasing = curve.copy() + increasing = curve.copy() + zero_angles = { + "policy": "cad_nominal", + "source_lower_rad": round(float(metadata["lower"]), 8), + "source_upper_rad": round(float(metadata["upper"]), 8), + } + curves[name] = curve + joint_payload = { + "urdf_joint": name, + "sdk_channel": COMMAND_NAMES[motor_index], + "motor_index": motor_index, + "passive": False, + "calibration_status": profile.joint_coverage[name], + "zero_command_u8": 255, + "zero_angles": zero_angles, + "angle_rad": _rounded_curve(curve), + "decreasing_rad": _rounded_curve(decreasing), + "increasing_rad": _rounded_curve(increasing), + } + if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT: + joint_payload["transferred_from_joint"] = measurement_source + joints[name] = joint_payload + + for name in PASSIVE_JOINTS: + metadata = source[name] + source_name = MIMIC_SOURCE_BY_JOINT[name] + motor_index = COMMAND_INDEX_BY_JOINT[source_name] + measurement_source = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get( + name, name + ) + if ( + name in MEASURED_PASSIVE_JOINTS + or name in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT + ): + fit = result.curves[measurement_source] + decreasing = np.asarray(fit.decreasing_rad, dtype=float) + increasing = np.asarray(fit.increasing_rad, dtype=float) + curve = 0.5 * (decreasing + increasing) + curve -= float(curve[255]) + decreasing = decreasing - float(decreasing[255]) + increasing = increasing - float(increasing[255]) + offset = float(metadata["offset"]) + curve += offset + decreasing += offset + increasing += offset + coupling = result.mimic_fits[measurement_source] + coupling_model = coupling.model + coefficients = list(coupling.mujoco_polycoef) + coefficients[0] = offset + urdf_mimic_multiplier = coupling.urdf_mimic_multiplier + urdf_mimic_policy = coupling.urdf_mimic_policy + else: + multiplier = float(metadata["multiplier"]) + offset = float(metadata["offset"]) + curve = offset + multiplier * curves[source_name] + decreasing = curve.copy() + increasing = curve.copy() + coupling_model = "linear_mimic" + coefficients = [offset, multiplier, 0.0, 0.0, 0.0, 0.0] + urdf_mimic_multiplier = multiplier + urdf_mimic_policy = "cad_nominal" + joint_payload = { + "urdf_joint": name, + "sdk_channel": COMMAND_NAMES[motor_index], + "motor_index": motor_index, + "passive": True, + "source_joint": source_name, + "mimic_offset_rad": round(float(metadata["offset"]), 8), + "coupling_model": coupling_model, + "coupling_coefficients": [ + round(float(value), 10) for value in coefficients + ], + "urdf_mimic_enabled": True, + "urdf_mimic_policy": urdf_mimic_policy, + "mimic_multiplier": round(float(urdf_mimic_multiplier), 8), + "calibration_status": profile.joint_coverage[name], + "zero_command_u8": 255, + "zero_angles": {"policy": "cad_static"}, + "angle_rad": _rounded_curve(curve), + "decreasing_rad": _rounded_curve(decreasing), + "increasing_rad": _rounded_curve(increasing), + } + joints[name] = joint_payload + if name in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT: + joint_payload["transferred_from_joint"] = measurement_source + + 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": "L6", + "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", + "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": ( + "hybrid_axis_geometry_cad_range_center" + if result.zero_fallback_reason_by_joint + else "g20_serial_axis_geometry" + ), + "passed": bool(result.thumb_zero_result.passed), + "geometry_fallback_reasons": dict( + sorted(result.zero_fallback_reason_by_joint.items()) + ), + "axis_line_rms_m": round( + float(result.thumb_zero_result.axis_line_rms_m), 10 + ), + "offsets_rad": { + name: round(float(value), 10) + for name, value in sorted( + result.thumb_zero_result.direct_offsets_rad.items() + ) + }, + "cycle_offsets_rad": { + name: [round(float(value), 10) for value in values] + for name, values in sorted( + result.thumb_zero_result.cycle_offsets_rad.items() + ) + }, + "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() + ) + }, + } + ), + }, + } + validate_l6_runtime_payload(payload) + return payload + + +def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: + required_top = { + "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_top: + raise ValueError("schema v6 calibration has unexpected top-level fields") + if ( + payload["schema_version"] != 6 + or payload["profile_id"] != KEY.profile_id + or payload["model"] != "L6" + or payload["side"] != "right" + or payload["calibration_scope"] != "partial" + ): + raise ValueError("schema v6 identity is invalid") + if payload["publication_pointer"] != "latest_partial_passed": + raise ValueError("L6 partial result has the wrong publication pointer") + if payload["curve_input_domain"] != "feedback_u8": + raise ValueError("schema v6 must be indexed by feedback_u8") + if payload["runtime_curve_policy"] != "direction_aware": + raise ValueError("schema v6 must retain both motion directions") + if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]: + raise ValueError("schema v6 units are invalid") + if tuple(payload["command_names"]) != COMMAND_NAMES: + raise ValueError("schema v6 command channel order is invalid") + if payload["baseline_command_u8"] != [255] * 6: + raise ValueError("schema v6 baseline must be six open commands") + protected = payload["protected_inputs"] + expected_hashes = { + "source_urdf_sha256", "camera_extrinsics_sha256", + "calibration_config_sha256", "tag_config_sha256", + } + if not isinstance(protected, Mapping) or set(protected) != expected_hashes: + raise ValueError("schema v6 protected inputs are incomplete") + if any( + len(str(value)) != 64 + or any(char not in "0123456789abcdef" for char in str(value)) + for value in protected.values() + ): + raise ValueError("schema v6 protected input hash is invalid") + joints = payload["joints"] + if not isinstance(joints, Mapping) or set(joints) != ALL_REVOLUTE_JOINTS: + raise ValueError("schema v6 must contain all 11 L6 revolute joints") + profile = build_typed_profile() + for name in ACTIVE_JOINTS + PASSIVE_JOINTS: + joint = joints[name] + motor = COMMAND_INDEX_BY_JOINT[ + MIMIC_SOURCE_BY_JOINT.get(name, name) + ] + if joint.get("urdf_joint") != name or int(joint.get("motor_index", -1)) != motor: + raise ValueError(f"{name} has an invalid URDF/SDK mapping") + if joint.get("sdk_channel") != COMMAND_NAMES[motor]: + raise ValueError(f"{name} has an invalid SDK channel") + if joint.get("calibration_status") != profile.joint_coverage[name]: + raise ValueError(f"{name} has an invalid coverage status") + if joint.get("passive") is not (name in PASSIVE_JOINTS): + raise ValueError(f"{name} passive flag is invalid") + if joint.get("zero_command_u8") != 255: + raise ValueError(f"{name} zero command must be 255") + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + curve = np.asarray(joint.get(field), dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"{name}.{field} must contain 256 finite values") + if np.any(np.diff(curve) > 1.0e-7): + raise ValueError(f"{name}.{field} must be non-increasing") + if name in PASSIVE_JOINTS: + if joint.get("source_joint") != MIMIC_SOURCE_BY_JOINT[name]: + raise ValueError(f"{name} mimic source is invalid") + model = str(joint.get("coupling_model", "")) + expected_model = COUPLING_MODEL_BY_JOINT.get( + name, "linear_mimic" + ) + if model != expected_model: + raise ValueError(f"{name} coupling model is invalid") + coefficients = np.asarray( + joint.get("coupling_coefficients"), dtype=float + ) + if coefficients.shape != (6,) or not np.all( + np.isfinite(coefficients) + ): + raise ValueError(f"{name} coupling coefficients are invalid") + enabled = joint.get("urdf_mimic_enabled") + if enabled is not True: + raise ValueError(f"{name} URDF mimic policy is invalid") + expected_policy = ( + "endpoint_linear_fallback" + if model == "quadratic_runtime" + else "exact_linear" + if name in MEASURED_PASSIVE_JOINTS + else "cad_nominal" + ) + # Early schema-v6 linear artifacts predate the explicit policy + # label; their unambiguous model/coverage combination remains + # readable. New writers always materialize the field. + policy = str( + joint.get("urdf_mimic_policy", expected_policy) + ) + if policy != expected_policy: + raise ValueError(f"{name} URDF mimic fallback is invalid") + multiplier = float(joint.get("mimic_multiplier", "nan")) + if not math.isfinite(multiplier) or multiplier <= 0.0: + raise ValueError(f"{name} mimic multiplier is invalid") + if model == "linear_mimic": + if abs(multiplier - coefficients[1]) > 1.0e-7: + raise ValueError(f"{name} mimic multiplier is inconsistent") + if np.any(np.abs(coefficients[2:]) > 1.0e-10): + raise ValueError(f"{name} linear mimic is not linear") + if abs(float(coefficients[0]) - float( + joint.get("mimic_offset_rad", "nan") + )) > 1.0e-7: + raise ValueError(f"{name} coupling offset is inconsistent") + transferred_from = ( + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name) + or TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name) + ) + if transferred_from is not None: + if joint.get("transferred_from_joint") != transferred_from: + raise ValueError(f"{name} transfer provenance is invalid") + donor = joints[transferred_from] + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + if joint[field] != donor[field]: + raise ValueError(f"{name} transfer curve differs from donor") + quality = payload["quality"] + if not isinstance(quality, Mapping) or quality.get("scope") != "partial": + raise ValueError("schema v6 quality scope must be partial") + if quality.get("passed") is not True: + raise ValueError("schema v6 quality.passed must be true") + + +def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> Path: + destination = Path(path).resolve() + destination.parent.mkdir(parents=True, exist_ok=True) + temporary = destination.with_name(f".{destination.name}.{os.getpid()}.tmp") + try: + with temporary.open("w", encoding="utf-8") as stream: + json.dump(payload, stream, ensure_ascii=False, indent=2, sort_keys=True) + stream.write("\n") + stream.flush() + os.fsync(stream.fileno()) + os.replace(temporary, destination) + finally: + if temporary.exists(): + temporary.unlink() + return destination + + +def publish_partial_session(serial_root: str | Path, session: str | Path) -> Path: + parent = Path(serial_root).resolve() + target = Path(session).resolve() + if target.parent != parent or not target.is_dir(): + raise ValueError("partial session must be a direct existing child") + destination = parent / "latest_partial_passed" + temporary = parent / f".latest_partial_passed.{os.getpid()}.tmp" + if temporary.exists() or temporary.is_symlink(): + temporary.unlink() + os.symlink(target.name, temporary, target_is_directory=True) + os.replace(temporary, destination) + return destination + + +def artifact_hashes(json_path: str | Path, urdf_path: str | Path) -> dict[str, str]: + return { + "calibration_json_sha256": _sha256_file(json_path), + "corrected_urdf_sha256": _sha256_file(urdf_path), + } + + +__all__ = [ + "ALL_REVOLUTE_JOINTS", + "artifact_hashes", + "atomic_write_json", + "build_l6_runtime_payload", + "publish_partial_session", + "validate_l6_runtime_payload", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/fitting.py new file mode 100644 index 0000000..5196821 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/fitting.py @@ -0,0 +1,708 @@ +"""L6 curve, endpoint-zero, holdout, and mimic fitting.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from pathlib import Path +from typing import Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.optimize import least_squares + +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 .profile import ( + CALIBRATED_ACTIVE_JOINTS, + COMMAND_INDEX_BY_JOINT, + COMMAND_NAMES, + COUPLING_MODEL_BY_JOINT, + ENDPOINT_ANCHOR_BY_JOINT, + KEY, + MEASURED_PASSIVE_JOINTS, + MIMIC_SOURCE_BY_JOINT, +) + + +@dataclass(frozen=True) +class MimicFit: + source_joint: str + target_joint: str + model: str + coefficients: tuple[float, ...] + urdf_mimic_multiplier: float + urdf_mimic_policy: str + cycle_coefficients: tuple[tuple[float, ...], ...] + maximum_cycle_range: float + maximum_cycle_prediction_range_rad: float + residual_rms_rad: float + residual_p95_rad: float + residual_max_rad: float + + @property + def multiplier(self) -> float: + """Linear term retained for compatible diagnostics and artifacts.""" + return float(self.coefficients[0]) + + @property + def cycle_multipliers(self) -> tuple[float, ...]: + return tuple(float(values[0]) for values in self.cycle_coefficients) + + @property + def mujoco_polycoef(self) -> tuple[float, ...]: + """MuJoCo q_target = p0 + p1*q_source + ... coefficients.""" + return (0.0, *self.coefficients, *(0.0,) * (5 - len(self.coefficients))) + + +@dataclass(frozen=True) +class L6FitResult: + curves: Mapping[str, JointCurveFit] + zero_offsets_rad: Mapping[str, float] + travels_rad: Mapping[str, float] + mimic_fits: Mapping[str, MimicFit] + holdout_errors_rad: Mapping[str, tuple[float, ...]] + zero_method_by_joint: Mapping[str, str] + zero_fallback_reason_by_joint: Mapping[str, str] + thumb_zero_result: ZeroSolveResult | None = None + + +L6_THUMB_AXIS_JOINTS: tuple[str, ...] = ( + "rh_thumb_cmc_roll", + "rh_thumb_cmc_pitch", + "rh_thumb_dip", + "rh_pinky_mcp_pitch", +) + + +def _l6_thumb_zero_profile() -> ZeroCalibrationProfile: + """Describe the observable L6 thumb/palm axis graph to the G20 solver.""" + joint_specs = { + "rh_thumb_cmc_roll": JointSpec( + "rh_thumb_cmc_roll", 1, True, "top", "top_base", "thumb_roll" + ), + "rh_thumb_cmc_pitch": JointSpec( + "rh_thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_pitch" + ), + "rh_thumb_dip": JointSpec( + "rh_thumb_dip", 0, False, "front", "thumb_pitch", "thumb_dip" + ), + # This root axis fixes the palm-frame phase around the thumb roll axis. + # Its electrical zero is irrelevant because rotating a revolute joint + # does not change its own physical screw axis. + "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=joint_specs, + sweep_specs=(), + image_trajectory_joints=frozenset(), + roll_clearance_commands={}, + thumb_pitch_clearance_commands={}, + layout_id=KEY.layout, + model="L6", + command_names=COMMAND_NAMES, + baseline_command=(255,) * 6, + directional_zero=True, + isolated_holdout=True, + ) + return ZeroCalibrationProfile( + hand=hand, + direct_zero_joints=( + "rh_thumb_cmc_roll", + "rh_thumb_cmc_pitch", + ), + axis_joints=L6_THUMB_AXIS_JOINTS, + inherited_zero_joints={}, + inherited_static_zero_joints={}, + constrained_circle_joints=frozenset(L6_THUMB_AXIS_JOINTS), + root_anchor_joints=frozenset({"rh_thumb_cmc_roll"}), + axis_parent_joint={ + "rh_thumb_cmc_pitch": "rh_thumb_cmc_roll", + }, + phase_parent_joint={ + "rh_thumb_dip": "rh_thumb_cmc_pitch", + }, + offset_observer_joint={ + "rh_thumb_cmc_roll": "rh_thumb_cmc_pitch", + "rh_thumb_cmc_pitch": "rh_thumb_dip", + }, + 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", + ) + + +def curve_travel_rad(fit: JointCurveFit) -> float: + decreasing = float(fit.decreasing_rad[0] - fit.decreasing_rad[255]) + increasing = float(fit.increasing_rad[0] - fit.increasing_rad[255]) + travel = 0.5 * (decreasing + increasing) + if not math.isfinite(travel) or travel <= 0.0: + raise ValueError("L6 fitted travel must be finite and positive") + return travel + + +def derive_endpoint_zero_offsets( + source_urdf: str | Path, + curves: Mapping[str, JointCurveFit], + *, + endpoint_anchor_by_joint: Mapping[str, str] | None = None, + maximum_offset_rad: float = math.radians(15.0), +) -> tuple[dict[str, float], dict[str, float]]: + """Anchor each measured joint to its profile-selected CAD endpoint. + + Every published dynamic curve is zero at feedback 255. ``upper_at_end`` + rotates the static frame by ``source_upper - measured_travel`` so feedback + 0 lands on the CAD upper endpoint. ``lower_at_start`` rotates it by the + source lower value so feedback 255 lands on the CAD lower endpoint. + ``cad_range_center`` splits a source-vs-measured travel discrepancy equally + between the two endpoints when neither source endpoint is a trusted datum. + ``zero_at_start`` keeps the CAD joint frame itself at feedback 255. All + policies publish the normalized corrected coordinate [0, measured travel]. + """ + 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] = {} + anchors = endpoint_anchor_by_joint or dict(ENDPOINT_ANCHOR_BY_JOINT) + if not anchors or not set(anchors).issubset(CALIBRATED_ACTIVE_JOINTS): + raise ValueError("L6 endpoint anchors must target measured active joints") + for name in sorted(CALIBRATED_ACTIVE_JOINTS): + if name not in curves: + raise ValueError(f"missing measured L6 curve: {name}") + joint = joints.get(name) + limit = None if joint is None else joint.find("limit") + if ( + limit is None + or limit.get("upper") is None + or limit.get("lower") is None + ): + raise ValueError(f"source URDF joint has incomplete limits: {name}") + travel = curve_travel_rad(curves[name]) + travels[name] = travel + if name not in anchors: + continue + policy = str(anchors[name]) + if policy == "upper_at_end": + offset = float(limit.get("upper")) - travel + elif policy == "lower_at_start": + offset = float(limit.get("lower")) + elif policy == "cad_range_center": + offset = 0.5 * ( + float(limit.get("lower")) + + float(limit.get("upper")) + - travel + ) + elif policy == "zero_at_start": + offset = 0.0 + else: + raise ValueError(f"unsupported L6 endpoint anchor: {policy}") + if not math.isfinite(offset) or abs(offset) > maximum_offset_rad: + raise ValueError( + f"{name} endpoint zero offset exceeds 15 degrees: " + f"{math.degrees(offset):.3f}" + ) + offsets[name] = offset + return offsets, travels + + +def _has_complete_axis_geometry( + records_by_joint: 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 L6_THUMB_AXIS_JOINTS + for rows in (records_by_joint.get(name, ()),) + ) + + +def _fit_l6_thumb_axis_zero( + source_urdf: str | Path, + records_by_joint: Mapping[str, Sequence[Mapping[str, object]]], + curves: Mapping[str, JointCurveFit], + *, + fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None, + require_passed: bool = True, +) -> ZeroSolveResult: + """Recover thumb roll/pitch zeros from four physical screw axes.""" + profile = _l6_thumb_zero_profile() + measurements = [] + by_key: dict[tuple[str, int], object] = {} + for cycle in range(4): + # Fit the two root/reference axes before the serial passive observer. + for name in ( + "rh_thumb_cmc_roll", + "rh_thumb_cmc_pitch", + "rh_pinky_mcp_pitch", + ): + rows = records_by_joint[name] + view_normal = rows[0]["view_normal_common_xyz"] + measurement = fit_joint_axis_measurement( + name, + rows, + cycle=cycle, + zero_command_u8=255, + constrained_circle_joints=profile.constrained_circle_joints, + view_normal_common_xyz=view_normal, + 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 + + dip_rows = records_by_joint["rh_thumb_dip"] + pitch_axis = by_key[("rh_thumb_cmc_pitch", cycle)] + dip = fit_joint_axis_measurement( + "rh_thumb_dip", + dip_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=dip_rows[0]["view_normal_common_xyz"], + canonical_zero_direction="decreasing", + ) + dip = with_depth_free_axis_projection( + dip, + dip_rows[0]["camera_center_common_xyz_m"], + ) + measurements.append(dip) + + 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 L6_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_roll": 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, + fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets_rad, + zero_profile=profile, + ) + if require_passed and not result.passed: + details = ",".join( + f"{name}={reason}" + for name, reason in sorted(result.failure_reasons.items()) + ) + raise ValueError("L6 thumb axis zero solve failed:" + details) + return result + + +def _coupling_regression( + active: Sequence[float], passive: Sequence[float], *, degree: int +) -> tuple[tuple[float, ...], np.ndarray]: + x = np.asarray(active, dtype=float) + y = np.asarray(passive, dtype=float) + if x.shape != y.shape or x.ndim != 1 or x.size < 16: + raise ValueError("mimic curves must be aligned finite vectors") + if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)): + raise ValueError("mimic curves must be finite") + if degree not in {1, 2}: + raise ValueError("L6 coupling degree must be one or two") + if float(x @ x) <= 1.0e-9: + raise ValueError("active mimic source has insufficient travel") + design = np.column_stack([x ** power for power in range(1, degree + 1)]) + initial = np.linalg.lstsq(design, y, rcond=None)[0] + scale = max( + math.radians(0.25), + float(np.median(np.abs(y - design @ initial))), + ) + fitted = least_squares( + lambda value: y - design @ value, + np.asarray(initial, dtype=float), + loss="soft_l1", + f_scale=scale, + ) + if not fitted.success or not np.all(np.isfinite(fitted.x)): + raise ValueError("robust through-origin mimic regression failed") + coefficients = tuple(float(value) for value in fitted.x) + grid = np.linspace(0.0, float(np.max(x)), 256) + derivative = np.full_like(grid, coefficients[0]) + if degree == 2: + derivative += 2.0 * coefficients[1] * grid + if float(np.min(derivative)) < -1.0e-7: + raise ValueError("L6 coupling model is not monotonic") + return coefficients, y - design @ fitted.x + + +def fit_coupling_model( + source_joint: str, + target_joint: str, + active_fit: JointCurveFit, + passive_fit: JointCurveFit, + *, + model: str, + cycle_curve_pairs: Sequence[ + tuple[Sequence[float], Sequence[float]] + ] = (), + minimum_multiplier: float = 0.5, + maximum_multiplier: float = 1.5, + maximum_cycle_range: float = 0.03, + maximum_cycle_prediction_range_rad: float = math.radians(1.0), + maximum_residual_p95_rad: float = math.radians(2.0), + maximum_residual_rad: float = math.radians(3.0), +) -> MimicFit: + if model not in {"linear_mimic", "quadratic_runtime"}: + raise ValueError(f"unsupported L6 coupling model: {model}") + degree = 1 if model == "linear_mimic" else 2 + active = np.concatenate( + ( + np.asarray(active_fit.decreasing_rad, dtype=float), + np.asarray(active_fit.increasing_rad, dtype=float), + ) + ) + passive = np.concatenate( + ( + np.asarray(passive_fit.decreasing_rad, dtype=float), + np.asarray(passive_fit.increasing_rad, dtype=float), + ) + ) + coefficients, residual = _coupling_regression( + active, passive, degree=degree + ) + multiplier = coefficients[0] + if not minimum_multiplier <= multiplier <= maximum_multiplier: + raise ValueError( + f"{target_joint} coupling linear term is outside [0.5, 1.5]" + ) + cycle_coefficients = tuple( + _coupling_regression( + active_cycle, passive_cycle, degree=degree + )[0] + for active_cycle, passive_cycle in cycle_curve_pairs + ) + cycle_multipliers = tuple(values[0] for values in cycle_coefficients) + cycle_range = ( + 0.0 + if len(cycle_multipliers) < 2 + else float(max(cycle_multipliers) - min(cycle_multipliers)) + ) + if cycle_range > maximum_cycle_range: + raise ValueError( + f"{target_joint} coupling linear-term cycle range exceeds 0.03" + ) + cycle_prediction_range = 0.0 + if len(cycle_coefficients) >= 2: + grid = np.linspace(0.0, float(np.max(active)), 256) + predictions = np.asarray( + [ + sum(value * grid ** (index + 1) for index, value in enumerate(values)) + for values in cycle_coefficients + ] + ) + cycle_prediction_range = float( + np.max(np.ptp(predictions, axis=0)) + ) + if cycle_prediction_range > maximum_cycle_prediction_range_rad: + raise ValueError( + f"{target_joint} coupling cycle prediction range exceeds 1 degree" + ) + absolute = np.abs(residual) + rms = float(np.sqrt(np.mean(np.square(residual)))) + p95 = float(np.percentile(absolute, 95.0)) + maximum = float(np.max(absolute)) + if p95 > maximum_residual_p95_rad or maximum > maximum_residual_rad: + raise ValueError( + "coupling_residual_exceeds:" + f"joint={target_joint}:model={model}:" + f"linear_term={multiplier:.6f}:" + f"p95_deg={math.degrees(p95):.3f}:" + f"maximum_deg={math.degrees(maximum):.3f}:" + f"p95_limit_deg={math.degrees(maximum_residual_p95_rad):.3f}:" + f"maximum_limit_deg={math.degrees(maximum_residual_rad):.3f}" + ) + return MimicFit( + source_joint=source_joint, + target_joint=target_joint, + model=model, + coefficients=coefficients, + # Standard URDF has only a linear mimic. For a nonlinear coupling, + # preserve the familiar editor/RViz linkage with a fallback line that + # is exact at both the open zero and measured closed endpoint. The + # direction-aware runtime curves and MuJoCo polynomial remain exact in + # between those endpoints. + urdf_mimic_multiplier=( + multiplier + if model == "linear_mimic" + else curve_travel_rad(passive_fit) / curve_travel_rad(active_fit) + ), + urdf_mimic_policy=( + "exact_linear" + if model == "linear_mimic" + else "endpoint_linear_fallback" + ), + cycle_coefficients=cycle_coefficients, + maximum_cycle_range=cycle_range, + maximum_cycle_prediction_range_rad=cycle_prediction_range, + residual_rms_rad=rms, + residual_p95_rad=p95, + residual_max_rad=maximum, + ) + + +def fit_mimic_multiplier( + source_joint: str, + target_joint: str, + active_fit: JointCurveFit, + passive_fit: JointCurveFit, + *, + cycle_curve_pairs: Sequence[ + tuple[Sequence[float], Sequence[float]] + ] = (), + minimum_multiplier: float = 0.5, + maximum_multiplier: float = 1.5, + maximum_cycle_range: float = 0.03, + maximum_residual_p95_rad: float = math.radians(2.0), + maximum_residual_rad: float = math.radians(3.0), +) -> MimicFit: + return fit_coupling_model( + source_joint, + target_joint, + active_fit, + passive_fit, + model="linear_mimic", + cycle_curve_pairs=cycle_curve_pairs, + minimum_multiplier=minimum_multiplier, + maximum_multiplier=maximum_multiplier, + maximum_cycle_range=maximum_cycle_range, + maximum_residual_p95_rad=maximum_residual_p95_rad, + maximum_residual_rad=maximum_residual_rad, + ) + + +def fit_l6_session( + source_urdf: str | Path, + records_by_joint: Mapping[str, Sequence[Mapping[str, object]]], + *, + require_thumb_axis_zero: bool = False, +) -> L6FitResult: + """Fit three training cycles and validate the isolated fourth cycle.""" + expected = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS + if set(records_by_joint) != expected: + raise ValueError("L6 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(2.0): + raise ValueError(f"{name} hysteresis exceeds 2 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) + } + endpoint_offsets, travels = derive_endpoint_zero_offsets( + source_urdf, + curves, + endpoint_anchor_by_joint=ENDPOINT_ANCHOR_BY_JOINT, + ) + thumb_zero_result: ZeroSolveResult | None = None + offsets = { + "rh_thumb_cmc_roll": 0.0, + "rh_thumb_cmc_pitch": endpoint_offsets["rh_thumb_cmc_pitch"], + **endpoint_offsets, + } + pinky_endpoint_method = { + "upper_at_end": "mechanical_upper_endpoint", + "lower_at_start": "mechanical_lower_endpoint", + "cad_range_center": "cad_range_center", + "zero_at_start": "source_joint_zero", + }[ENDPOINT_ANCHOR_BY_JOINT["rh_pinky_mcp_pitch"]] + zero_methods = { + "rh_thumb_cmc_roll": "source_joint_zero_unpublished", + "rh_thumb_cmc_pitch": "cad_range_center_unpublished", + "rh_pinky_mcp_pitch": pinky_endpoint_method, + } + zero_fallback_reasons: dict[str, str] = {} + has_axis_geometry = _has_complete_axis_geometry(records_by_joint) + if require_thumb_axis_zero and not has_axis_geometry: + raise ValueError( + "L6 thumb absolute zero requires G20-compatible common-frame " + "Tag pose trajectories; this session must be reacquired" + ) + if has_axis_geometry: + geometric_result = _fit_l6_thumb_axis_zero( + source_urdf, + records_by_joint, + curves, + require_passed=False, + ) + thumb_zero_result = geometric_result + pitch_failure = geometric_result.failure_reasons.get( + "rh_thumb_cmc_pitch" + ) + endpoint_fallback_reasons = { + "zero_offset_exceeds_configured_limit", + "zero_offset_reached_diagnostic_bound", + } + if ( + not geometric_result.passed + and set(geometric_result.failure_reasons) + == {"rh_thumb_cmc_pitch"} + and pitch_failure in endpoint_fallback_reasons + ): + # L6_RIGHT_001 demonstrated a stable pitch-to-DIP axis-line phase + # beyond the diagnostic search bound. That phase includes + # physical link geometry and is not a safe encoder-zero observation + # when it contradicts both the reviewed endpoint and the +/-15 deg + # write limit. Keep roll geometric, but freeze pitch to its + # independent measured/CAD range-centre datum. Any roll, + # holdout, cone, confidence, or multi-joint failure remains a hard + # rejection. + zero_fallback_reasons["rh_thumb_cmc_pitch"] = str(pitch_failure) + thumb_zero_result = _fit_l6_thumb_axis_zero( + source_urdf, + records_by_joint, + curves, + fixed_direct_zero_offsets_rad={ + "rh_thumb_cmc_pitch": endpoint_offsets[ + "rh_thumb_cmc_pitch" + ] + }, + ) + elif not geometric_result.passed: + details = ",".join( + f"{name}={reason}" + for name, reason in sorted( + geometric_result.failure_reasons.items() + ) + ) + raise ValueError("L6 thumb axis zero solve failed:" + details) + offsets.update( + { + name: float(thumb_zero_result.direct_offsets_rad[name]) + for name in ( + "rh_thumb_cmc_roll", + "rh_thumb_cmc_pitch", + ) + } + ) + zero_methods.update( + { + "rh_thumb_cmc_roll": "urdf_serial_axis_geometry", + "rh_thumb_cmc_pitch": ( + "cad_range_center_after_geometry_rejection" + if "rh_thumb_cmc_pitch" in zero_fallback_reasons + else "urdf_serial_axis_geometry" + ), + } + ) + mimic_fits: dict[str, MimicFit] = {} + for target in sorted(MEASURED_PASSIVE_JOINTS): + source = MIMIC_SOURCE_BY_JOINT[target] + cycle_pairs = [] + for cycle in (0, 1, 2): + active = cycle_curves[source][cycle] + passive = cycle_curves[target][cycle] + 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=cycle_pairs, + ) + return L6FitResult( + 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=zero_fallback_reasons, + thumb_zero_result=thumb_zero_result, + ) + + +__all__ = [ + "L6FitResult", + "MimicFit", + "curve_travel_rad", + "derive_endpoint_zero_offsets", + "L6_THUMB_AXIS_JOINTS", + "fit_coupling_model", + "fit_l6_session", + "fit_mimic_multiplier", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/motion.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/motion.py new file mode 100644 index 0000000..f648ba0 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/motion.py @@ -0,0 +1,82 @@ +"""Safe six-channel motion helpers for the partial L6 profile.""" + +from __future__ import annotations + +import math +from typing import Sequence + +from ...core import CalibrationProfile, TaskSpec + + +def cosine_position_trajectory_u8( + start_u8: float, + target_u8: float, + elapsed_seconds: float, + full_range_duration_seconds: float, +) -> tuple[float, float, float]: + """Return a bounded, zero-end-velocity L6 command trajectory sample.""" + duration = ( + float(full_range_duration_seconds) + * abs(float(target_u8) - float(start_u8)) + / 255.0 + ) + if full_range_duration_seconds <= 0.0: + raise ValueError("full_range_duration_seconds must be positive") + if duration <= 0.0: + return float(target_u8), 1.0, 0.0 + phase = min(1.0, max(0.0, float(elapsed_seconds) / duration)) + blend = 0.5 - 0.5 * math.cos(math.pi * phase) + value = float(start_u8) + (float(target_u8) - float(start_u8)) * blend + return value, phase, duration + + +def build_calibration_motion_command( + task: TaskSpec, + command_u8: int, + *, + profile: CalibrationProfile, +) -> list[int]: + values = list(profile.command.baseline_u8) + for index, value in task.auxiliary_commands: + values[int(index)] = int(value) + values[int(task.command_index)] = int(command_u8) + return values + + +def build_calibration_preparation_waypoints( + task: TaskSpec, + *, + profile: CalibrationProfile, + current_command: Sequence[int] | None = None, +) -> tuple[tuple[int, ...], ...]: + del current_command + start = build_calibration_motion_command( + task, task.start_u8, profile=profile + ) + return (tuple(start),) + + +def build_calibration_return_waypoints( + target_command: Sequence[int] | None = None, + *, + profile: CalibrationProfile, + current_command: Sequence[int] | None = None, + **_: object, +) -> tuple[tuple[int, ...], ...]: + del current_command + target = ( + tuple(int(value) for value in target_command) + if target_command is not None + else tuple(profile.command.baseline_u8) + ) + if len(target) != profile.command.command_count: + raise ValueError("L6 return command has the wrong channel count") + return (target,) + + +__all__ = [ + "build_calibration_motion_command", + "build_calibration_preparation_waypoints", + "build_calibration_return_waypoints", + "cosine_position_trajectory_u8", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py new file mode 100644 index 0000000..10c7bb7 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py @@ -0,0 +1,1174 @@ +"""Online three-view acquisition node for L6/right/l6_right_8/v1.""" + +from __future__ import annotations + +from collections import deque +from dataclasses import dataclass +import json +import math +from pathlib import Path +import time +import traceback +from typing import Any + +import numpy as np +import rclpy +from apriltag_msgs.msg import AprilTagDetectionArray +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from scipy.spatial.transform import Rotation +from sensor_msgs.msg import CameraInfo, JointState +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from ...acquisition import StateSample, interpolate_state_u8 +from ...extrinsics import ( + load_three_camera_extrinsics, + matrix_payload, + transform_matrix, +) +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 + + +@dataclass(frozen=True) +class MotionStep: + phase: str + task_key: str | None + command_index: int | None + target_u8: int + speed_u8: int + cycle: int | None = None + direction: str | None = None + attempt: int = 1 + + @property + def recording(self) -> bool: + return self.direction in {"decreasing", "increasing"} + + +def _stamp_ns(stamp: Any) -> int: + return int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) + + +class L6ThreeCameraCalibrationNode(Node): + """Own the L6 commands and publish only a reviewed partial result.""" + + def __init__(self) -> None: + super().__init__("l6_calibration") + self.profile = build_typed_profile() + self._declare_parameters() + self._load_parameters() + self.session_dir.mkdir(parents=True, exist_ok=True) + self.raw_path = self.session_dir / "raw_samples.jsonl" + append_jsonl( + self.raw_path, + { + "kind": "session_start", + "sample_schema_version": 6, + "profile_id": self.profile.key.profile_id, + "serial_number": self.serial_number, + "curve_input_domain": "feedback_u8", + }, + ) + + self.command_publisher = self.create_publisher( + JointState, self.command_topic, 10 + ) + self.setting_publisher = self.create_publisher( + String, self.setting_topic, 10 + ) + self.status_publisher = self.create_publisher( + String, "/l6_calibration/status", 10 + ) + self.create_subscription( + JointState, self.state_topic, self._state_callback, 30 + ) + self.camera_matrices: dict[str, np.ndarray] = {} + self.image_sizes: dict[str, tuple[int, int]] = {} + self.last_view_valid_at: dict[str, float] = {} + self.base_corner_observations: dict[str, deque[np.ndarray]] = { + view: deque(maxlen=30) for view in self.profile.vision.view_names + } + self.base_corner_reference: dict[str, np.ndarray] = {} + self.base_drift_counts: dict[str, int] = {} + self.latest_base_drift_px: dict[str, float] = {} + self.trackers = { + view: SquareTagPoseTracker( + maximum_reprojection_error_px=self.pnp_maximum_reprojection_error_px, + reprojection_tie_px=self.pnp_reprojection_tie_px, + maximum_pose_jump_rad=math.radians(self.pnp_maximum_pose_jump_deg), + maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, + maximum_tag_tilt_rad=math.radians(self.pnp_maximum_tag_tilt_deg), + reset_after_seconds=self.pnp_tracker_reset_seconds, + ) + for view in self.profile.vision.view_names + } + for view in self.profile.vision.view_names: + self.create_subscription( + CameraInfo, + self.camera_info_topics[view], + lambda message, selected=view: self._camera_info_callback( + selected, message + ), + qos_profile_sensor_data, + ) + self.create_subscription( + AprilTagDetectionArray, + self.detection_topics[view], + lambda message, selected=view: self._detections_callback( + selected, message + ), + qos_profile_sensor_data, + ) + self.create_service(Trigger, "/l6_calibration/start", self._start) + self.create_service(Trigger, "/l6_calibration/abort", self._abort) + + self.latest_state_u8: tuple[float, ...] = () + self.state_history: deque[StateSample] = deque(maxlen=2000) + self.state_receive_times: deque[float] = deque(maxlen=300) + self.raw_records: list[dict[str, Any]] = [] + self.steps: list[MotionStep] = [] + self.step_index = -1 + self.step_started_at = 0.0 + self.step_hold_since: float | None = None + self.step_command_sent = False + self.step_last_progress_at = 0.0 + self.step_last_feedback = float("nan") + self.step_initial_feedback = float("nan") + self.step_speed_ready_at = 0.0 + self.step_requested_u8 = float("nan") + self.step_start_state_u8: tuple[float, ...] = () + self.step_last_command_u8: tuple[int, ...] | None = None + self.step_trajectory_phase = 0.0 + self.step_trajectory_duration_seconds = 0.0 + self.step_valid_frames = 0 + self.step_total_frames = 0 + self.step_required_roles: tuple[str, ...] = () + self.step_tag_seen_frames: dict[str, int] = {} + self.step_tag_quality_frames: dict[str, int] = {} + self.step_all_tags_quality_frames = 0 + self.step_pnp_valid_frames = 0 + self.step_state_sync_frames = 0 + self.step_rejection_counts: dict[str, int] = {} + self.latest_recognized_tag_ids: tuple[int, ...] = () + self.latest_unrecognized_tag_ids: tuple[int, ...] = () + self.state = "WAIT_DEVICES" + self.reason = "waiting_for_six_channel_feedback_and_three_camera_info" + self.started = False + self.commanded_speed: int | None = None + self.retry_counts: dict[tuple[str, int, str], int] = {} + self.create_timer(0.01, self._tick) + self.create_timer(0.5, self._publish_status) + + def _declare_parameters(self) -> None: + defaults: dict[str, Any] = { + "serial_number": "UNSET", + "session_dir": "", + "source_urdf_path": "", + "source_urdf_expected_sha256": "", + "camera_extrinsics_file": "", + "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", + "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, + "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, + "minimum_sweep_frames": 40, + "minimum_state_span_u8": 240.0, + "minimum_sweep_bins": 32, + "maximum_bin_gap": 16, + "minimum_detection_rate": 0.95, + # Three independently detected Tags at 95% each can only yield + # 0.95**3 ~= 85.7% fully joined frames. Keep the per-Tag gate at + # 95%, and gate the downstream joined PnP/state samples separately. + "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, + "fixed_base_maximum_corner_drift_px": 2.0, + "fixed_base_movement_confirmation_frames": 5, + "tag_size_m": 0.016, + "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, + } + for name, default in defaults.items(): + self.declare_parameter(name, default) + + def _load_parameters(self) -> None: + value = lambda name: self.get_parameter(name).value + self.serial_number = str(value("serial_number")) + self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() + self.source_urdf = Path(str(value("source_urdf_path"))).expanduser().resolve() + self.camera_extrinsics_file = Path( + str(value("camera_extrinsics_file")) + ).expanduser().resolve() + if self.serial_number in {"", "UNSET"}: + raise ValueError("serial_number is required") + 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") + self.extrinsics = load_three_camera_extrinsics( + self.camera_extrinsics_file + ) + self.protected_inputs = { + "source_urdf_sha256": str(value("source_urdf_expected_sha256")), + "camera_extrinsics_sha256": str( + value("camera_extrinsics_expected_sha256") + ), + "calibration_config_sha256": str( + value("calibration_config_expected_sha256") + ), + "tag_config_sha256": str(value("tag_config_expected_sha256")), + } + if any(len(item) != 64 for item in self.protected_inputs.values()): + raise ValueError("all four immutable input SHA-256 values are required") + self.command_topic = str(value("command_topic")) + self.state_topic = str(value("state_topic")) + self.setting_topic = str(value("setting_topic")) + self.camera_info_topics = { + view: str(value(f"{view}_camera_info_topic")) + for view in self.profile.vision.view_names + } + self.detection_topics = { + view: str(value(f"{view}_detections_topic")) + for view in self.profile.vision.view_names + } + for name in ( + "commands_enabled", "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", + "sweep_timeout_seconds", "automatic_sweep_retry_limit", + "non_target_motion_tolerance_u8", "minimum_sweep_frames", + "minimum_state_span_u8", "minimum_sweep_bins", "maximum_bin_gap", + "minimum_detection_rate", "minimum_joint_frame_rate", + "minimum_feedback_hz", + "maximum_state_image_skew_ms", "maximum_hamming", + "minimum_decision_margin", "minimum_edge_pixels", + "fixed_base_maximum_corner_drift_px", + "fixed_base_movement_confirmation_frames", "tag_size_m", + "pnp_maximum_reprojection_error_px", "pnp_reprojection_tie_px", + "pnp_maximum_pose_jump_deg", "pnp_maximum_translation_jump_m", + "pnp_maximum_tag_tilt_deg", "pnp_tracker_reset_seconds", + ): + setattr(self, name, value(name)) + self.maximum_state_image_skew_ns = int( + float(self.maximum_state_image_skew_ms) * 1_000_000 + ) + 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: + raise ValueError( + "command_trajectory_full_range_seconds must be in [2, 30]" + ) + if not 0.0 < float(self.minimum_detection_rate) <= 1.0: + raise ValueError("minimum_detection_rate must be in (0, 1]") + if not 0.0 < float(self.minimum_joint_frame_rate) <= 1.0: + 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))] + for task in self.profile.motion.tasks: + for target in (255, 127, 0, 255): + steps.append( + MotionStep( + "preflight", task.key, task.command_index, target, + int(task.preflight_speed_u8 or self.preflight_speed_u8), + ) + ) + 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"), + ] + ) + return steps + + def _start(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: + if self.started: + response.success = False + response.message = "L6 calibration has already started" + return response + if self.state != "READY": + response.success = False + response.message = self.reason + return response + self.steps = self._build_steps() + self.step_index = 0 + self.started = True + self.state = "RUNNING" + self.reason = "moving_to_open_baseline" + self._publish_torque() + response.success = True + response.message = "L6 partial calibration started" + return response + + def _abort(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: + self._publish_command([255] * 6) + self.state = "ABORTED" + self.reason = "operator_abort_returning_to_open_baseline" + response.success = True + response.message = self.reason + return response + + def _camera_info_callback(self, view: str, message: CameraInfo) -> None: + matrix = np.asarray(message.k, dtype=float).reshape(3, 3) + if np.all(np.isfinite(matrix)) and matrix[0, 0] > 0 and matrix[1, 1] > 0: + self.camera_matrices[view] = matrix + self.image_sizes[view] = (int(message.width), int(message.height)) + + def _state_callback(self, message: JointState) -> None: + if len(message.position) != 6: + 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): + return + state = tuple(float(by_name[name]) for name in COMMAND_NAMES) + else: + state = tuple(float(value) for value in message.position) + if not all(math.isfinite(value) for value in state): + return + stamp = _stamp_ns(message.header.stamp) + if stamp <= 0: + stamp = int(self.get_clock().now().nanoseconds) + if not self.state_history or stamp > self.state_history[-1].stamp_ns: + self.state_history.append(StateSample(stamp, state)) + self.latest_state_u8 = state + self.state_receive_times.append(time.monotonic()) + step = self._current_step() + if step is not None and step.task_key is not None: + for index, actual in enumerate(state): + if index == step.command_index: + continue + if abs(actual - 255.0) > float(self.non_target_motion_tolerance_u8): + self._pause( + f"non_target_motor_moved:channel={index}:feedback={actual:.2f}" + ) + return + + def _task(self, key: str): + return next(task for task in self.profile.motion.tasks if task.key == key) + + def _view(self, name: str): + return next(view for view in self.profile.vision.views if view.name == name) + + def _current_step(self) -> MotionStep | None: + if 0 <= self.step_index < len(self.steps): + return self.steps[self.step_index] + return None + + def _required_roles(self, view: str) -> tuple[str, ...]: + step = self._current_step() + if step is None or step.task_key is None: + return tuple(tag.role for tag in self._view(view).tags) + task = self._task(step.task_key) + if task.view != view: + return () + roles: list[str] = [] + for joint in task.joints: + measurement = self.profile.measurement.measurements[joint] + roles.extend((str(measurement.parent_role), str(measurement.child_role))) + return tuple(dict.fromkeys(roles)) + + def _reset_step_observation_counters( + self, roles: tuple[str, ...] = () + ) -> None: + self.step_required_roles = tuple(roles) + self.step_valid_frames = 0 + self.step_total_frames = 0 + self.step_tag_seen_frames = {role: 0 for role in roles} + self.step_tag_quality_frames = {role: 0 for role in roles} + self.step_all_tags_quality_frames = 0 + self.step_pnp_valid_frames = 0 + self.step_state_sync_frames = 0 + self.step_rejection_counts = {} + required_ids = tuple( + int(tag.tag_id) + for view in self.profile.vision.views + for tag in view.tags + if tag.role in roles + ) + self.latest_recognized_tag_ids = () + self.latest_unrecognized_tag_ids = tuple(sorted(required_ids)) + + def _count_step_rejection(self, reason: str) -> None: + self.step_rejection_counts[reason] = ( + self.step_rejection_counts.get(reason, 0) + 1 + ) + + def _step_observation_metrics(self) -> dict[str, Any]: + total = int(self.step_total_frames) + + def rate(count: int) -> float: + return float(count) / total if total else 0.0 + + seen_by_role = { + role: rate(self.step_tag_seen_frames.get(role, 0)) + for role in self.step_required_roles + } + quality_by_role = { + role: rate(self.step_tag_quality_frames.get(role, 0)) + for role in self.step_required_roles + } + return { + "tag_seen_rate_by_role": seen_by_role, + "tag_detection_rate_by_role": quality_by_role, + # The formal >=95% Tag gate applies to every required Tag, not to + # the product of several independent per-Tag probabilities. + "tag_detection_rate": min(quality_by_role.values(), default=0.0), + "all_tags_quality_rate": rate(self.step_all_tags_quality_frames), + "pnp_valid_rate": rate(self.step_pnp_valid_frames), + "state_sync_rate": rate(self.step_state_sync_frames), + "joint_frame_rate": rate(self.step_valid_frames), + "rejection_counts": dict(sorted(self.step_rejection_counts.items())), + } + + def _detections_callback(self, view: str, message: AprilTagDetectionArray) -> None: + roles = self._required_roles(view) + if not roles or view not in self.camera_matrices: + return + role_by_id = {tag.tag_id: tag.role for tag in self._view(view).tags} + corners: dict[str, np.ndarray] = {} + good: dict[str, bool] = {} + detected_roles: set[str] = set() + quality_failures: dict[str, tuple[str, ...]] = {} + width, height = self.image_sizes.get(view, (0, 0)) + for detection in message.detections: + role = role_by_id.get(int(detection.id)) + if role is None: + continue + detected_roles.add(role) + points = np.asarray( + [[float(point.x), float(point.y)] for point in detection.corners], + dtype=float, + ) + if points.shape != (4, 2): + quality_failures[role] = ("malformed_corners",) + continue + edges = np.linalg.norm(points - np.roll(points, -1, axis=0), axis=1) + full_border = bool( + width > 0 and height > 0 + and np.min(points[:, 0]) >= 2.0 + and np.max(points[:, 0]) <= width - 3.0 + and np.min(points[:, 1]) >= 2.0 + and np.max(points[:, 1]) <= height - 3.0 + ) + failures: list[str] = [] + if int(detection.hamming) > int(self.maximum_hamming): + failures.append("hamming") + if float(detection.decision_margin) < float(self.minimum_decision_margin): + failures.append("decision_margin") + if float(np.min(edges)) < float(self.minimum_edge_pixels): + failures.append("edge_pixels") + if not full_border: + failures.append("image_border") + candidate_good = not failures + # Prefer a good detection if a detector ever emits a duplicate ID. + if role not in corners or candidate_good or not good.get(role, False): + corners[role] = points + good[role] = candidate_good + quality_failures[role] = tuple(failures) + step = self._current_step() + recording_this_view = bool( + step is not None + and step.recording + and self._task(str(step.task_key)).view == view + ) + active_this_view = bool( + step is not None + and step.task_key is not None + and self._task(str(step.task_key)).view == view + ) + if active_this_view: + id_by_role = { + tag.role: int(tag.tag_id) for tag in self._view(view).tags + } + self.latest_recognized_tag_ids = tuple( + sorted(id_by_role[role] for role in roles if good.get(role, False)) + ) + self.latest_unrecognized_tag_ids = tuple( + sorted( + id_by_role[role] + for role in roles + if not good.get(role, False) + ) + ) + if recording_this_view: + self.step_total_frames += 1 + for role in roles: + if role in detected_roles: + self.step_tag_seen_frames[role] = ( + self.step_tag_seen_frames.get(role, 0) + 1 + ) + else: + self._count_step_rejection(f"tag:{role}:missing") + if good.get(role, False): + self.step_tag_quality_frames[role] = ( + self.step_tag_quality_frames.get(role, 0) + 1 + ) + else: + reasons = quality_failures.get( + role, + ("malformed_corners",) if role in detected_roles else (), + ) + for reason in reasons: + self._count_step_rejection(f"tag:{role}:{reason}") + if all(role in corners and good.get(role, False) for role in roles): + self.step_all_tags_quality_frames += 1 + base_role = next( + tag.role for tag in self._view(view).tags if tag.fixed_reference + ) + if base_role in corners and good.get(base_role, False): + if not recording_this_view: + observations = self.base_corner_observations[view] + observations.append(corners[base_role].copy()) + if len(observations) >= 5: + self.base_corner_reference[view] = np.median( + np.asarray(observations, dtype=float), axis=0 + ) + self.base_drift_counts[view] = 0 + self.latest_base_drift_px[view] = 0.0 + elif view in self.base_corner_reference: + reference = self.base_corner_reference[view] + drift = float( + np.max( + np.linalg.norm(corners[base_role] - reference, axis=1) + ) + ) + self.latest_base_drift_px[view] = drift + self.base_drift_counts[view] = ( + self.base_drift_counts.get(view, 0) + 1 + if drift > float(self.fixed_base_maximum_corner_drift_px) + else 0 + ) + if self.base_drift_counts[view] >= int( + self.fixed_base_movement_confirmation_frames + ): + append_jsonl( + self.raw_path, + { + "kind": "fixed_base_reference_moved", + "view": view, + "tag_id": next( + tag.tag_id + for tag in self._view(view).tags + if tag.fixed_reference + ), + "corner_drift_px": round(drift, 6), + "maximum_corner_drift_px": float( + self.fixed_base_maximum_corner_drift_px + ), + "confirmation_frames": self.base_drift_counts[view], + "task_name": step.task_key, + }, + ) + self._pause( + f"fixed_base_tag_moved:{view}:drift_px={drift:.3f}" + ) + return + if not all(role in corners and good.get(role, False) for role in roles): + return + stamp = _stamp_ns(message.header.stamp) + selected: dict[str, SquareTagPose] = {} + for role in roles: + pose, pnp_reason = self.trackers[view].estimate( + role, + corners[role], + tag_size_m=float(self.tag_size_m), + camera_matrix=self.camera_matrices[view], + stamp_ns=stamp, + ) + if pose is None: + if recording_this_view: + self._count_step_rejection( + f"pnp:{role}:{pnp_reason or 'rejected'}" + ) + return + selected[role] = pose + if recording_this_view: + self.step_pnp_valid_frames += 1 + self.last_view_valid_at[view] = time.monotonic() + if step is None or not step.recording or self._task(step.task_key).view != view: + return + matched = interpolate_state_u8( + list(self.state_history), + stamp, + maximum_skew_ns=self.maximum_state_image_skew_ns, + ) + if matched is None: + self._count_step_rejection("state_sync:no_sample_within_limit") + return + state_u8, skew_ns = matched + self.step_state_sync_frames += 1 + task = self._task(step.task_key) + feedback = float(state_u8[task.command_index]) + if not 0.0 <= feedback <= 255.0: + self._count_step_rejection("feedback:outside_u8_domain") + return + for joint in task.joints: + measurement = self.profile.measurement.measurements[joint] + parent = selected[str(measurement.parent_role)] + child = selected[str(measurement.child_role)] + relative_translation = Rotation.from_quat( + parent.quaternion_xyzw + ).inv().apply( + np.asarray(child.translation_xyz_m, dtype=float) + - np.asarray(parent.translation_xyz_m, dtype=float) + ) + common_from_view = self.extrinsics.transform(view) + parent_matrix = common_from_view @ transform_matrix( + parent.translation_xyz_m, parent.quaternion_xyzw + ) + child_matrix = common_from_view @ transform_matrix( + child.translation_xyz_m, child.quaternion_xyzw + ) + relative = ( + Rotation.from_matrix(parent_matrix[:3, :3]).inv() + * Rotation.from_matrix(child_matrix[:3, :3]) + ) + record = { + "kind": "l6_joint_sample", + "profile_id": self.profile.key.profile_id, + "task_name": task.key, + "view": view, + "joint": joint, + "sdk_channel": COMMAND_NAMES[task.command_index], + "motor_index": task.command_index, + "cycle": int(step.cycle), + "direction": str(step.direction), + "attempt": int(step.attempt), + # Keep the established schema contract: this field labels the + # requested sweep endpoint. The live shaped set-point is a + # separate diagnostic and is never used as the fit domain. + "requested_command_u8": int(step.target_u8), + "trajectory_command_u8": round(self.step_requested_u8, 6), + "feedback_u8": round(feedback, 6), + "state_u8": [round(float(value), 6) for value in state_u8], + "state_image_sync_error_ms": round(abs(skew_ns) / 1_000_000.0, 6), + "relative_quaternion_xyzw": [ + float(value) for value in relative.as_quat() + ], + # G20-compatible geometric observations. The fitted screw + # axes, rather than an electrical endpoint assumption, define + # the two thumb CMC static zeros. + "relative_translation_xyz_m": [ + float(value) for value in relative_translation + ], + "parent_pose_common": matrix_payload(parent_matrix), + "child_pose_common": matrix_payload(child_matrix), + "view_normal_common_xyz": [ + float(value) + for value in ( + common_from_view[:3, :3] + @ np.asarray([0.0, 0.0, 1.0], dtype=float) + ) + ], + "camera_center_common_xyz_m": [ + float(value) for value in common_from_view[:3, 3] + ], + "pnp_reprojection_error_px": round( + max(parent.reprojection_error_px, child.reprojection_error_px), 6 + ), + "image_stamp_ns": stamp, + } + self.raw_records.append(record) + append_jsonl(self.raw_path, record) + self.step_valid_frames += 1 + + def _feedback_hz(self) -> float: + if len(self.state_receive_times) < 2: + return 0.0 + elapsed = self.state_receive_times[-1] - self.state_receive_times[0] + return 0.0 if elapsed <= 0 else (len(self.state_receive_times) - 1) / elapsed + + def _publish_torque(self) -> None: + if not self.commands_enabled: + return + message = String() + message.data = json.dumps( + { + "setting_cmd": "set_max_torque_limits", + "params": {"hand_type": "right", "torque": [int(self.torque_u8)] * 6}, + } + ) + self.setting_publisher.publish(message) + + def _publish_speed(self, speed: int) -> None: + if not self.commands_enabled or self.commanded_speed == int(speed): + return + message = String() + message.data = json.dumps( + { + "setting_cmd": "set_speed", + "params": {"hand_type": "right", "speed": [int(speed)] * 6}, + } + ) + self.setting_publisher.publish(message) + self.commanded_speed = int(speed) + + def _publish_command(self, values: list[int]) -> None: + if not self.commands_enabled: + return + message = JointState() + message.header.stamp = self.get_clock().now().to_msg() + message.name = list(COMMAND_NAMES) + message.position = [float(value) for value in values] + self.command_publisher.publish(message) + + def _begin_step(self, step: MotionStep) -> None: + now = time.monotonic() + if self.commanded_speed != int(step.speed_u8): + self._publish_speed(step.speed_u8) + self.step_speed_ready_at = now + float(self.speed_settle_seconds) + self.reason = f"setting_speed:{step.speed_u8}" + return + if now < self.step_speed_ready_at: + return + if len(self.latest_state_u8) != 6: + return + self.step_start_state_u8 = tuple(float(value) for value in self.latest_state_u8) + self.step_started_at = now + self.step_last_progress_at = now + self.step_last_feedback = ( + float(self.latest_state_u8[step.command_index]) + if step.command_index is not None and len(self.latest_state_u8) == 6 + else ( + float(np.mean(self.latest_state_u8)) + if len(self.latest_state_u8) == 6 + else float("nan") + ) + ) + self.step_initial_feedback = self.step_last_feedback + self.step_requested_u8 = self.step_initial_feedback + target = [255.0] * 6 + if step.command_index is not None: + target[step.command_index] = float(step.target_u8) + maximum_distance = max( + abs(end - start) + for start, end in zip(self.step_start_state_u8, target) + ) + _, _, self.step_trajectory_duration_seconds = ( + cosine_position_trajectory_u8( + 0.0, + maximum_distance, + 0.0, + float(self.command_trajectory_full_range_seconds), + ) + ) + self.step_trajectory_phase = 0.0 + self.step_last_command_u8 = None + self.step_hold_since = None + roles: tuple[str, ...] = () + if step.recording and step.task_key is not None: + roles = self._required_roles(self._task(step.task_key).view) + self._reset_step_observation_counters(roles) + self.step_command_sent = True + self._advance_step_trajectory(step, now) + self.reason = ( + f"{step.phase}:{step.task_key or 'all'}:cycle={step.cycle}:" + f"direction={step.direction}:target={step.target_u8}:attempt={step.attempt}" + ) + + def _advance_step_trajectory(self, step: MotionStep, now: float) -> None: + elapsed = max(0.0, float(now) - self.step_started_at) + _, phase, _ = cosine_position_trajectory_u8( + 0.0, + max( + ( + abs(255.0 - value) + if step.command_index is None + else abs(float(step.target_u8) - value) + if index == step.command_index + else abs(255.0 - value) + ) + for index, value in enumerate(self.step_start_state_u8) + ), + elapsed, + float(self.command_trajectory_full_range_seconds), + ) + blend = 0.5 - 0.5 * math.cos(math.pi * phase) + targets = [255.0] * 6 + if step.command_index is not None: + targets[step.command_index] = float(step.target_u8) + values = [ + start + (target - start) * blend + for start, target in zip(self.step_start_state_u8, targets) + ] + command = tuple(int(np.clip(round(value), 0, 255)) for value in values) + self.step_trajectory_phase = phase + self.step_requested_u8 = ( + float(np.mean(values)) + if step.command_index is None + else float(values[step.command_index]) + ) + if command != self.step_last_command_u8: + self._publish_command(list(command)) + self.step_last_command_u8 = command + + def _qualify_recording_step(self, step: MotionStep) -> None: + task = self._task(str(step.task_key)) + rows = [ + row for row in self.raw_records + if row["task_name"] == task.key + and int(row["cycle"]) == int(step.cycle) + and row["direction"] == step.direction + and int(row["attempt"]) == step.attempt + and row["joint"] == task.joints[0] + ] + feedback = np.asarray([float(row["feedback_u8"]) for row in rows]) + bins = sorted(set(int(round(value)) for value in feedback)) + gap = max((right - left for left, right in zip(bins, bins[1:])), default=256) + observation = self._step_observation_metrics() + detection_rate = float(observation["tag_detection_rate"]) + joint_frame_rate = float(observation["joint_frame_rate"]) + failures = [] + if len(rows) < int(self.minimum_sweep_frames): + failures.append(f"frames={len(rows)}") + if feedback.size == 0 or float(np.ptp(feedback)) < float(self.minimum_state_span_u8): + failures.append("feedback_span") + if len(bins) < int(self.minimum_sweep_bins): + failures.append(f"bins={len(bins)}") + if gap > int(self.maximum_bin_gap): + failures.append(f"maximum_gap={gap}") + if detection_rate < float(self.minimum_detection_rate): + role_rates = observation["tag_detection_rate_by_role"] + worst_role = min(role_rates, key=role_rates.get, default="unknown") + tag_id = next( + ( + tag.tag_id + for view in self.profile.vision.views + for tag in view.tags + if tag.role == worst_role + ), + "?", + ) + failures.append( + f"tag_rate[ID{tag_id}/{worst_role}]={detection_rate:.3f}" + ) + if joint_frame_rate < float(self.minimum_joint_frame_rate): + failures.append(f"joint_frame_rate={joint_frame_rate:.3f}") + if self._feedback_hz() < float(self.minimum_feedback_hz): + failures.append(f"feedback_hz={self._feedback_hz():.2f}") + append_jsonl( + self.raw_path, + { + "kind": "l6_sweep_observation_quality", + "task_name": step.task_key, + "cycle": step.cycle, + "direction": step.direction, + "attempt": step.attempt, + "valid_frames": len(rows), + "total_frames": self.step_total_frames, + "feedback_bins": len(bins), + "maximum_bin_gap": gap, + **observation, + "failures": failures, + }, + ) + if failures: + raise ValueError(",".join(failures)) + + def _retry_step(self, step: MotionStep, reason: str) -> bool: + key = (str(step.task_key), int(step.cycle), str(step.direction)) + retries = self.retry_counts.get(key, 0) + if retries >= int(self.automatic_sweep_retry_limit): + return False + attempt = retries + 2 + self.retry_counts[key] = retries + 1 + start = 255 if step.direction == "decreasing" else 0 + 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), + ] + self.steps[self.step_index + 1:self.step_index + 1] = replacement + append_jsonl( + self.raw_path, + { + "kind": "automatic_rescan", + "task_name": step.task_key, + "cycle": step.cycle, + "direction": step.direction, + "failed_attempt": step.attempt, + "next_attempt": attempt, + "reason": reason, + }, + ) + return True + + def _finish_step(self, step: MotionStep) -> None: + if step.recording: + try: + self._qualify_recording_step(step) + except ValueError as error: + if not self._retry_step(step, str(error)): + self._pause(f"sweep_quality_failed:{step.task_key}:{error}") + return + self.step_index += 1 + self.step_command_sent = False + if self.step_index >= len(self.steps): + self._finalize() + + def _tick(self) -> None: + if self.state in {"PASSED", "PAUSED", "ABORTED"}: + return + state_publisher_count = self.count_publishers(self.state_topic) + if state_publisher_count > 1: + self._pause( + f"multiple_state_publishers:count={state_publisher_count}" + ) + return + command_publisher_count = self.count_publishers(self.command_topic) + if command_publisher_count > 1: + self._pause( + f"multiple_command_publishers:count={command_publisher_count}" + ) + return + if not self.started: + if len(self.latest_state_u8) == 6 and set(self.camera_matrices) == set( + self.profile.vision.view_names + ): + self.state = "READY" + self.reason = "ready_for_operator_start" + return + step = self._current_step() + if step is None: + return + if not self.step_command_sent: + self._begin_step(step) + return + now = time.monotonic() + self._advance_step_trajectory(step, now) + timeout = self.sweep_timeout_seconds if step.recording else self.position_timeout_seconds + if now - self.step_started_at > float(timeout): + self._pause(f"motion_timeout:{self.reason}") + return + if len(self.latest_state_u8) != 6: + return + actual = ( + max(abs(value - 255.0) for value in self.latest_state_u8) + if step.command_index is None + else abs(self.latest_state_u8[step.command_index] - step.target_u8) + ) + 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_progress_at = now + if ( + actual > float(self.endpoint_tolerance_u8) + and now - self.step_started_at > 1.0 + and now - self.step_last_progress_at > float(self.motor_stall_timeout_seconds) + ): + self._pause(f"mechanical_stall:{self.reason}") + return + if actual > float(self.endpoint_tolerance_u8): + self.step_hold_since = None + return + if self.step_trajectory_phase < 1.0: + self.step_hold_since = None + return + task_view = None if step.task_key is None else self._task(step.task_key).view + if task_view is not None and now - self.last_view_valid_at.get(task_view, 0.0) > 0.25: + self.step_hold_since = None + return + if self.step_hold_since is None: + self.step_hold_since = now + return + if now - self.step_hold_since >= float(self.endpoint_hold_seconds): + self._finish_step(step) + + def _finalize(self) -> None: + try: + payload, _fit, correction = finalize_l6_session( + session_dir=self.session_dir, + serial_number=self.serial_number, + source_urdf=self.source_urdf, + protected_inputs=self.protected_inputs, + records=self.raw_records, + publish=True, + ) + except BaseException as error: + atomic_write_json( + self.session_dir / "failure_diagnostic.json", + { + "profile_id": self.profile.key.profile_id, + "reason": str(error), + "traceback": traceback.format_exc(), + "source_urdf_unchanged": True, + }, + ) + self._pause(f"fit_or_publication_failed:{error}") + return + self._publish_command([255] * 6) + 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.final_urdf = str(correction.path) + self.final_quality = payload["quality"] + + def _pause(self, reason: str) -> None: + if self.state in {"PAUSED", "ABORTED", "PASSED"}: + return + self.state = "PAUSED" + self.reason = str(reason) + # Holding the latest position is safer than issuing an automatic move + # after a stall, unexpected motor motion, or moved palm reference. + if len(self.latest_state_u8) == 6: + self._publish_command( + [int(np.clip(round(value), 0, 255)) for value in self.latest_state_u8] + ) + append_jsonl(self.raw_path, {"kind": "paused", "reason": self.reason}) + + def _status(self) -> dict[str, Any]: + step = self._current_step() + feedback = float("nan") + if len(self.latest_state_u8) == 6: + feedback = ( + float(np.mean(self.latest_state_u8)) + if step is None or step.command_index is None + else float(self.latest_state_u8[step.command_index]) + ) + step_fraction = 0.0 + if step is not None and math.isfinite(feedback): + initial_distance = abs(self.step_initial_feedback - step.target_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 + step_fraction = float(np.clip(step_fraction, 0.0, 1.0)) + elif self.state == "PASSED": + step_fraction = 1.0 + observation = self._step_observation_metrics() + return { + "profile_id": self.profile.key.profile_id, + "state": self.state, + "reason": self.reason, + "serial_number": self.serial_number, + "session_dir": str(self.session_dir), + "step_index": self.step_index, + "step_count": len(self.steps), + "task_name": None if step is None else step.task_key, + "phase": None if step is None else step.phase, + "cycle": None if step is None else step.cycle, + "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, + "target_u8": None if step is None else step.target_u8, + "current_command_u8": ( + None + if step is None or not math.isfinite(self.step_requested_u8) + else round(self.step_requested_u8, 3) + ), + "actual_u8": None if not math.isfinite(feedback) else round(feedback, 3), + "step_fraction": round(step_fraction, 6), + "valid_frames": self.step_valid_frames, + "total_frames": self.step_total_frames, + "tag_detection_rate": round( + float(observation["tag_detection_rate"]), 6 + ), + "tag_detection_rate_by_role": { + role: round(float(rate), 6) + for role, rate in observation["tag_detection_rate_by_role"].items() + }, + "tag_seen_rate_by_role": { + role: round(float(rate), 6) + for role, rate in observation["tag_seen_rate_by_role"].items() + }, + "recognized_tag_ids": list(self.latest_recognized_tag_ids), + "unrecognized_tag_ids": list(self.latest_unrecognized_tag_ids), + "all_tags_quality_rate": round( + float(observation["all_tags_quality_rate"]), 6 + ), + "pnp_valid_rate": round(float(observation["pnp_valid_rate"]), 6), + "state_sync_rate": round(float(observation["state_sync_rate"]), 6), + "joint_frame_rate": round(float(observation["joint_frame_rate"]), 6), + "observation_rejection_counts": observation["rejection_counts"], + "preflight_speed_u8": int(self.preflight_speed_u8), + "formal_speed_u8": int(self.formal_speed_u8), + "command_trajectory_full_range_seconds": float( + self.command_trajectory_full_range_seconds + ), + "command_trajectory_phase": round(self.step_trajectory_phase, 6), + "command_trajectory_duration_seconds": round( + self.step_trajectory_duration_seconds, 6 + ), + "base_corner_drift_px": { + view: round(value, 4) + for view, value in self.latest_base_drift_px.items() + }, + "base_corner_drift_limit_px": float( + self.fixed_base_maximum_corner_drift_px + ), + "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), + "latest_state_u8": list(self.latest_state_u8), + "final_json": getattr(self, "final_json", ""), + "final_urdf": getattr(self, "final_urdf", ""), + "quality": getattr(self, "final_quality", {}), + } + + def _publish_status(self) -> None: + message = String() + message.data = json.dumps(self._status(), ensure_ascii=False) + self.status_publisher.publish(message) + + +def main(args: list[str] | None = None) -> None: + rclpy.init(args=args) + node: L6ThreeCameraCalibrationNode | None = None + try: + node = L6ThreeCameraCalibrationNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +__all__ = ["L6ThreeCameraCalibrationNode", "MotionStep", "main"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/pipeline.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/pipeline.py new file mode 100644 index 0000000..2d23012 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/pipeline.py @@ -0,0 +1,267 @@ +"""Shared online/offline finalization path for one L6 partial session.""" + +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_l6_runtime_payload, + publish_partial_session, +) +from .fitting import L6FitResult, fit_l6_session +from .profile import ( + CALIBRATED_ACTIVE_JOINTS, + CORRECTED_PASSIVE_JOINTS, + MEASURED_PASSIVE_JOINTS, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, +) +from .urdf import L6UrdfCorrection, write_l6_corrected_urdf + + +MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS +ENDPOINT_SNAP_TOLERANCE_U8 = 2 + + +def canonical_feedback_command_u8(feedback_u8: float) -> int: + """Map a reached L6 feedback endpoint onto the curve's 0/255 domain.""" + value = int(round(float(feedback_u8))) + if value <= ENDPOINT_SNAP_TOLERANCE_U8: + return 0 + if value >= 255 - ENDPOINT_SNAP_TOLERANCE_U8: + return 255 + return value + + +def accepted_records_by_joint( + records: Sequence[Mapping[str, Any]], +) -> dict[str, list[dict[str, Any]]]: + """Select the newest attempt for every task/cycle/direction.""" + samples = [ + dict(row) + for row in records + if row.get("kind") == "l6_joint_sample" + and str(row.get("joint", "")) in MEASURED_JOINTS + ] + latest_attempt: dict[tuple[str, int, str], int] = {} + for row in samples: + key = ( + str(row["task_name"]), + int(row["cycle"]), + str(row["direction"]), + ) + latest_attempt[key] = max( + latest_attempt.get(key, 0), int(row.get("attempt", 1)) + ) + result = {name: [] for name in MEASURED_JOINTS} + for row in samples: + key = ( + str(row["task_name"]), + int(row["cycle"]), + str(row["direction"]), + ) + if int(row.get("attempt", 1)) != latest_attempt[key]: + continue + accepted = { + "cycle": int(row["cycle"]), + "direction": str(row["direction"]), + # The reusable fitter calls the independent variable + # command_u8; schema v6 deliberately supplies measured SDK + # feedback here, never the requested controller set-point. + "command_u8": canonical_feedback_command_u8( + float(row["feedback_u8"]) + ), + "feedback_u8": float(row["feedback_u8"]), + "relative_quaternion_xyzw": list( + row["relative_quaternion_xyzw"] + ), + } + # Schema-v6.1 adds the G20-compatible pose trajectory required for + # absolute thumb CMC zero recovery. Keep the projection here so the + # online and offline finalizers consume byte-equivalent fitting rows. + geometric_fields = ( + "relative_translation_xyz_m", + "parent_pose_common", + "child_pose_common", + "view_normal_common_xyz", + "camera_center_common_xyz_m", + "state_u8", + ) + if any(field in row for field in geometric_fields): + missing = [field for field in geometric_fields if field not in row] + if missing: + raise ValueError( + "L6 geometric sample is incomplete: " + ",".join(missing) + ) + for field in geometric_fields: + value = row[field] + accepted[field] = ( + dict(value) if isinstance(value, Mapping) else list(value) + ) + result[str(row["joint"])].append(accepted) + return result + + +def load_l6_raw_samples(path: str | Path) -> list[dict[str, Any]]: + source = Path(path).expanduser().resolve() + if not source.is_file(): + raise ValueError(f"raw L6 sample file does not exist: {source}") + rows: list[dict[str, Any]] = [] + with source.open("r", encoding="utf-8") as stream: + for line_number, line in enumerate(stream, 1): + if not line.strip(): + continue + try: + value = json.loads(line) + except json.JSONDecodeError as error: + raise ValueError( + f"invalid L6 JSONL record at line {line_number}" + ) from error + if not isinstance(value, Mapping): + raise ValueError(f"L6 JSONL line {line_number} is not an object") + rows.append(dict(value)) + return rows + + +def finalize_l6_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], L6FitResult, L6UrdfCorrection]: + directory = Path(session_dir).expanduser().resolve() + directory.mkdir(parents=True, exist_ok=True) + result = fit_l6_session( + source_urdf, + accepted_records_by_joint(records), + require_thumb_axis_zero=True, + ) + # Validate the complete runtime schema before materializing any corrected + # URDF. A fit/schema rejection therefore leaves only the node's failure + # diagnostic and the immutable raw samples. + payload = build_l6_runtime_payload( + serial_number=serial_number, + source_urdf=source_urdf, + result=result, + protected_inputs=protected_inputs, + passed=True, + ) + stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") + correction = write_l6_corrected_urdf( + source_urdf=source_urdf, + output_directory=directory, + serial_number=serial_number, + result=result, + timestamp=stamp, + ) + json_path = directory / f"l6_right_{serial_number}_partial_calibration.json" + atomic_write_json(json_path, payload) + summary = { + "schema_version": 1, + "profile_id": "L6/right/l6_right_8/v1", + "serial_number": str(serial_number), + "result": "PARTIAL_PASS", + "publication_pointer": "latest_partial_passed", + "calibrated_active_joints": sorted(CALIBRATED_ACTIVE_JOINTS), + "active_zero_methods": dict(sorted(result.zero_method_by_joint.items())), + "active_zero_fallback_reasons": dict( + sorted(result.zero_fallback_reason_by_joint.items()) + ), + "thumb_axis_zero": ( + None + if result.thumb_zero_result is None + else { + "offsets_rad": { + name: round(float(value), 10) + for name, value in sorted( + result.thumb_zero_result.direct_offsets_rad.items() + ) + }, + "axis_line_rms_m": round( + float(result.thumb_zero_result.axis_line_rms_m), 10 + ), + "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() + ) + }, + } + ), + "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": fit.model, + "transferred_from_joint": ( + donor if donor != name else None + ), + "coefficients": [ + round(float(value), 10) for value in fit.coefficients + ], + "urdf_mimic_enabled": True, + "urdf_mimic_multiplier": round( + float(fit.urdf_mimic_multiplier), 10 + ), + "urdf_mimic_policy": fit.urdf_mimic_policy, + "residual_p95_deg": round( + math.degrees(float(fit.residual_p95_rad)), 6 + ), + "residual_max_deg": round( + math.degrees(float(fit.residual_max_rad)), 6 + ), + "cycle_prediction_range_deg": round( + math.degrees( + float(fit.maximum_cycle_prediction_range_rad) + ), + 6, + ), + } + for name, donor in sorted( + { + target: TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get( + target, target + ) + for target in CORRECTED_PASSIVE_JOINTS + }.items() + ) + for fit in (result.mimic_fits[donor],) + }, + "explicit_runtime_joints": sorted( + correction.explicit_runtime_joints + ), + "artifacts": { + "json": json_path.name, + "urdf": correction.path.name, + **artifact_hashes(json_path, correction.path), + }, + } + atomic_write_json(directory / "calibration_summary_zh.json", summary) + if publish: + publish_partial_session(directory.parent, directory) + return payload, result, correction + + +__all__ = [ + "ENDPOINT_SNAP_TOLERANCE_U8", + "MEASURED_JOINTS", + "accepted_records_by_joint", + "canonical_feedback_command_u8", + "finalize_l6_session", + "load_l6_raw_samples", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/profile.py new file mode 100644 index 0000000..10d2770 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/profile.py @@ -0,0 +1,392 @@ +"""Reviewed partial-calibration profile for the right L6 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 ..registry import EngineBindings, RegisteredProfile +from .motion import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, +) + + +KEY = ProfileKey("L6", "right", "l6_right_8", 1) + +COMMAND_NAMES: tuple[str, ...] = ( + "thumb_cmc_pitch", + "thumb_cmc_roll", + "index_mcp_pitch", + "middle_mcp_pitch", + "ring_mcp_pitch", + "pinky_mcp_pitch", +) + +ACTIVE_JOINTS: tuple[str, ...] = ( + "rh_thumb_cmc_pitch", + "rh_thumb_cmc_roll", + "rh_index_mcp_pitch", + "rh_middle_mcp_pitch", + "rh_ring_mcp_pitch", + "rh_pinky_mcp_pitch", +) +PASSIVE_JOINTS: tuple[str, ...] = ( + "rh_thumb_dip", + "rh_index_dip", + "rh_middle_dip", + "rh_ring_dip", + "rh_pinky_dip", +) +CALIBRATED_ACTIVE_JOINTS = frozenset( + { + "rh_thumb_cmc_pitch", + "rh_thumb_cmc_roll", + "rh_pinky_mcp_pitch", + } +) +MEASURED_PASSIVE_JOINTS = frozenset( + {"rh_thumb_dip", "rh_pinky_dip"} +) + +# The four L6 fingers use the same six-channel mechanism. This profile has +# visual Tags only on the pinky, so the remaining three fingers deliberately +# inherit the pinky's measured travel, feedback curves, and passive coupling +# while retaining their own CAD frames and geometry. The shared MCP zero is +# anchored at the observed open endpoint; it is not inferred by forcing the +# measured closed travel back onto the shorter CAD upper limit. +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 = { + # Thumb pitch normally uses serial-axis geometry. If that observation is + # incompatible with the source link geometry, align the centre of the + # measured physical range with the centre of the source CAD range. Real + # endpoint comparison showed lower_at_start slightly under-corrected and + # upper_at_end over-corrected this serial, so neither endpoint is an + # independently trustworthy absolute datum. + "rh_thumb_cmc_pitch": "cad_range_center", + # Feedback 255 is the repeatable open/lower endpoint. The measured travel + # is about 70.55 deg while the source CAD upper is 65 deg. Anchoring the + # closed endpoint therefore introduced a -5.55 deg offset into all four + # fingers and made intermediate pinch poses systematically under-flexed. + "rh_pinky_mcp_pitch": "lower_at_start", +} + +COMMAND_INDEX_BY_JOINT = { + "rh_thumb_cmc_pitch": 0, + "rh_thumb_cmc_roll": 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_dip": "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 = { + "rh_thumb_dip": "linear_mimic", + # The L6 pinky transmission has a repeatable changing ratio over its + # travel. Keep the exact direction-aware lookup at runtime and use the + # quadratic centre relation only for MuJoCo's equality constraint. + "rh_pinky_dip": "quadratic_runtime", + "rh_index_dip": "quadratic_runtime", + "rh_middle_dip": "quadratic_runtime", + "rh_ring_dip": "quadratic_runtime", +} + + +def build_typed_profile() -> CalibrationProfile: + active = frozenset(ACTIVE_JOINTS) + passive = frozenset(PASSIVE_JOINTS) + frozen_active = active - CALIBRATED_ACTIVE_JOINTS + measurements = { + "rh_thumb_cmc_roll": MeasurementSpec( + "rh_thumb_cmc_roll", + "relative_rotation", + "top", + "top_base", + "thumb_roll", + ), + "rh_thumb_cmc_pitch": MeasurementSpec( + "rh_thumb_cmc_pitch", + "relative_rotation", + "front", + "front_base", + "thumb_pitch", + ), + "rh_thumb_dip": MeasurementSpec( + "rh_thumb_dip", + "relative_rotation", + "front", + "thumb_pitch", + "thumb_dip", + ), + "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_roll_top", + "top", + 1, + ("rh_thumb_cmc_roll",), + auxiliary_commands=((0, 255),), + preflight_speed_u8=1, + formal_speed_u8=1, + ), + TaskSpec( + "thumb_pitch_dip_front", + "front", + 0, + ("rh_thumb_cmc_pitch", "rh_thumb_dip"), + auxiliary_commands=((1, 255),), + preflight_speed_u8=1, + formal_speed_u8=1, + ), + TaskSpec( + "pinky_pitch_dip_side", + "side", + 5, + ("rh_pinky_mcp_pitch", "rh_pinky_dip"), + preflight_speed_u8=1, + formal_speed_u8=1, + ), + ) + coverage = { + **{ + name: ( + "measured_static_dynamic" + if name in CALIBRATED_ACTIVE_JOINTS + else "transferred_static_dynamic" + if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT + else "cad_nominal" + ) + for name in active + }, + **{ + name: ( + "measured_dynamic_cad_static" + if name in MEASURED_PASSIVE_JOINTS + else "transferred_dynamic_cad_static" + if name in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT + else "mimic_nominal" + ) + for name in passive + }, + } + return CalibrationProfile( + key=KEY, + namespace="/l6_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}, + feedback_name_aliases={"thumb_cmc_yaw": "thumb_cmc_roll"}, + 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_dip", 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_roll", 7), + ), + ), + ), + common_frame="calibration_common", + extrinsic_reference_view="front", + extrinsics_quality_limits={ + "reprojection_rms_px": 1.2, + "maximum_rotation_repeatability_deg": 0.3, + "maximum_translation_repeatability_m": 0.0015, + }, + minimum_capture_counts={ + "front_side_captures": 15, + "front_top_captures": 15, + }, + ), + motion=MotionPolicy( + tasks=tasks, + precheck_sweeps=True, + steady_command_checkpoints=True, + speed_parameters={ + "preflight_u8": 1, + "formal_u8": 1, + "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={ + name: ENDPOINT_ANCHOR_BY_JOINT[name] + for name in 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": frozen_active}, + default_scope="partial", + ), + artifacts=ArtifactPolicy( + output_schema_version=6, + calibration_filename=( + "l6_right_{serial_number}_partial_calibration.json" + ), + corrected_urdf_filename=( + "linkerhand_l6_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( + {"l6_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", + "KEY", + "MEASURED_PASSIVE_JOINTS", + "MIMIC_SOURCE_BY_JOINT", + "PASSIVE_JOINTS", + "build_profile", + "build_typed_profile", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py new file mode 100644 index 0000000..d30252b --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py @@ -0,0 +1,622 @@ +"""One-command online runner and deterministic offline replay for L6 right.""" + +from __future__ import annotations + +import argparse +from datetime import datetime +import hashlib +import json +import os +from pathlib import Path +import re +import signal +import subprocess +import sys +import time +from typing import Any, Mapping + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from ...operator_report import ( + ProgressEstimator, + render_compact_progress_header_zh, +) +from ...product import ProductConfig, load_product_config +from ...storage import atomic_write_json +from .pipeline import finalize_l6_session, load_l6_raw_samples + + +_STATE_LABELS = { + "WAIT_DEVICES": "等待六通道反馈和三相机内参", + "READY": "设备就绪", + "RUNNING": "标定中", + "PASSED": "通过", + "PAUSED": "已暂停", + "ABORTED": "已中止", +} +_TASK_LABELS = { + "thumb_roll_top": "拇指 CMC roll（上方机位，ID6→ID7）", + "thumb_pitch_dip_front": "拇指 CMC pitch / DIP（正面机位，ID0→ID1→ID2）", + "pinky_pitch_dip_side": "小指 MCP pitch / DIP（侧面机位，ID3→ID4→ID5）", +} +_PHASE_LABELS = { + "baseline": "安全恢复基准形态", + "preflight": "任务运动预检", + "prepare": "扫描起点准备", + "retry_prepare": "自动重扫起点准备", + "sweep": "正式扫描", +} +_DIRECTION_LABELS = { + "decreasing": "递减", + "increasing": "递增", +} + + +def _progress_fraction(status: Mapping[str, Any]) -> tuple[float, int, int]: + state = str(status.get("state", "")) + step_count = max(0, int(status.get("step_count", 0) or 0)) + step_index = int(status.get("step_index", -1) or 0) + step_fraction = float(status.get("step_fraction", 0.0) or 0.0) + if state == "PASSED": + overall = 1.0 + elif step_count and step_index >= 0: + overall = min(1.0, max(0.0, (step_index + step_fraction) / step_count)) + else: + overall = 0.0 + current_step = min(step_count, max(0, step_index + 1)) if step_count else 0 + return overall, current_step, step_count + + +def _duration_zh(seconds: float | None) -> str: + if seconds is None or seconds < 0.0: + return "计算中" + value = int(round(seconds)) + return f"{value // 60}分{value % 60:02d}秒" + + +def _l6_reason_zh(status: Mapping[str, Any]) -> tuple[str, str, str]: + reason = str(status.get("reason", "unknown")) + if reason.startswith("fixed_base_tag_moved:"): + fields = reason.split(":") + view = fields[1] if len(fields) > 1 else "unknown" + labels = {"front": "正面", "side": "侧面", "top": "上方"} + match = re.search(r"drift_px=([0-9.]+)", reason) + drift = match.group(1) if match else "未知" + limit = float(status.get("base_corner_drift_limit_px", 2.0) or 2.0) + return ( + "OBS-BASE-DRIFT-105", + f"{labels.get(view, view)}机位的掌心固定基准 Tag 相对本方向扫描前" + f"锁定位置连续漂移，最大角点位移 {drift} px，超过 {limit:g} px。", + "检查手掌支架、相机和掌心基准 Tag 是否松动或被碰触；固定后重新开始。" + "程序已禁止发布本次结果。", + ) + if reason.startswith("sweep_quality_failed:"): + fields = reason.split(":", 2) + details = fields[2] if len(fields) > 2 else "未提供明细" + return ( + "OBS-SWEEP-QUALITY-104", + "当前方向经过自动重扫后仍未满足采集门限；具体未通过项：" + f"{details}。", + "查看会话诊断中的具体 frames/bins/maximum_gap/tag_rate；先处理遮挡或" + "反馈采样问题，再重新开始。程序已禁止发布本次结果。", + ) + if reason.startswith("non_target_motor_moved:"): + return ( + "MOTION-NONTARGET-304", + "扫描期间检测到非目标电机离开保持位置。", + "停止其他控制节点并检查机械耦合或反馈通道顺序，确认后重新开始。", + ) + if reason.startswith("multiple_state_publishers:"): + count = status.get("state_publisher_count", "?") + return ( + "DEVICE-DUPLICATE-SDK-203", + f"检测到 {count} 个 L6 状态发布者；这通常表示已有 SDK/GUI 未退出。", + "先停止单独启动的 linker_hand_sdk 和 GUI，只保留本标定命令自动拉起的 SDK，" + "再重新开始。程序已禁止同时控制同一只手。", + ) + if reason.startswith("multiple_command_publishers:"): + count = status.get("command_publisher_count", "?") + return ( + "DEVICE-COMMAND-CONFLICT-204", + f"检测到 {count} 个 L6 控制命令发布者，标定节点之外还有程序在控制手。", + "停止 GUI、手动控制节点或其他标定进程，只保留当前标定命令后重新开始。", + ) + if reason.startswith("mechanical_stall:"): + return ( + "MOTION-STALL-303", + "目标电机连续两秒没有向目标推进，程序已保持当前位置。", + "检查碰撞、摩擦和机械端点；不要连续重启强推。", + ) + if reason.startswith("motion_timeout:"): + return ( + "MOTION-TIMEOUT-302", + "当前运动在规定时间内没有到达目标位置。", + "检查 CAN 反馈、电机状态和机械阻挡后重新开始。", + ) + if reason.startswith("fit_or_publication_failed:"): + if ( + "mimic_residual_exceeds:" in reason + or "coupling_residual_exceeds:" in reason + ): + joint_match = re.search(r"joint=([^:]+)", reason) + model_match = re.search(r"model=([^:]+)", reason) + multiplier_match = re.search( + r"(?:multiplier|linear_term)=([0-9.]+)", reason + ) + p95_match = re.search(r"p95_deg=([0-9.]+)", reason) + maximum_match = re.search(r"maximum_deg=([0-9.]+)", reason) + joint = joint_match.group(1) if joint_match else "未知关节" + model = model_match.group(1) if model_match else "linear_mimic" + multiplier = multiplier_match.group(1) if multiplier_match else "未知" + p95 = p95_match.group(1) if p95_match else "未知" + maximum = maximum_match.group(1) if maximum_match else "未知" + return ( + "FIT-MIMIC-503", + f"{joint} 的 {model} 耦合模型未达到精度门限：" + f"线性项 {multiplier}，" + f"残差 P95={p95}°、最大={maximum}°。", + "原始视觉曲线已保留；不要放宽门限或发布错误 URDF。请检查 Tag 刚性、" + "遮挡与机械重复性后重新采集。", + ) + detail = reason.split(":", 1)[1] if ":" in reason else "未知" + return ( + "FIT-PUBLISH-501", + f"采集完成后的拟合、质量验证或 URDF 安全写回失败：{detail}。", + "保留本会话，不要修改源 URDF；复制下方诊断块给开发者。", + ) + if reason.startswith("operator_abort"): + return "OPERATOR-ABORT-001", "操作员主动中止了本次标定。", "排除现场问题后重新开始。" + return ( + "L6-CAL-500", + "L6 标定因未分类保护条件停止。", + "保留会话目录和运行日志，并复制下方诊断块给开发者。", + ) + + +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.""" + 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)) + elif phase == "baseline": + task_text = "全手基准姿态" + elif state == "WAIT_DEVICES": + task_text = "等待设备连接" + elif state == "READY": + task_text = "等待开始" + else: + task_text = "无" + eta = _duration_zh(estimator.remaining(overall) if estimator else None) + cycle = status.get("cycle") + cycle_text = "-" if cycle is None else str(int(cycle) + 1) + direction = status.get("direction") + direction_text = _DIRECTION_LABELS.get(str(direction), "-") + target = status.get("target_u8") + requested = status.get("current_command_u8", target) + actual = status.get("actual_u8") + actual_text = "未知" if actual is None else f"{float(actual):.1f}" + valid = int(status.get("valid_frames", 0) or 0) + total = int(status.get("total_frames", 0) or 0) + rate = float(status.get("tag_detection_rate", 0.0) or 0.0) + joint_rate = float( + status.get( + "joint_frame_rate", + (float(valid) / total) if total else 0.0, + ) + or 0.0 + ) + recognized = [int(value) for value in status.get("recognized_tag_ids", [])] + unrecognized = [ + int(value) for value in status.get("unrecognized_tag_ids", []) + ] + recognized_text = "/".join(f"ID{value}" for value in recognized) or "无" + unrecognized_text = "/".join(f"ID{value}" for value in unrecognized) or "无" + attempt = int(status.get("attempt", 1) or 1) + lines = render_compact_progress_header_zh( + serial_number=str(status.get("serial_number", "?")), + progress=overall, + eta=eta, + stage=_PHASE_LABELS.get(str(phase), _STATE_LABELS.get(state, state)), + cycle=cycle_text, + repetitions=4, + task=task_text, + requested=requested, + actual=actual_text, + direction=direction_text, + tag_status=( + f"已识别 {recognized_text}；未识别/不合格 {unrecognized_text}；" + f"本方向各Tag最低 {rate:.1%}；联合 {valid}/{total} 帧" + f"（{joint_rate:.1%}）" + ), + ready_cameras=3, + feedback_hz=float(status.get("feedback_hz", 0.0) or 0.0), + valid_frames=valid, + automatic_retry_count=max(0, attempt - 1), + ) + if state in {"PAUSED", "ABORTED"}: + _code, problem, suggestion = _l6_reason_zh(status) + lines.extend((f"原因：{problem}", f"建议：{suggestion}")) + return "\n".join(lines) + + +class _ProgressConsole: + def __init__(self) -> None: + self.last_text = "" + self.estimator = ProgressEstimator.start() + + def update(self, status: Mapping[str, Any]) -> None: + text = render_l6_progress_zh(status, self.estimator) + if text == self.last_text: + return + self.last_text = text + if sys.stdout.isatty(): + sys.stdout.write("\x1b[2J\x1b[H" + text + "\n") + sys.stdout.flush() + else: + print(text, flush=True) + + +class _Monitor(Node): + def __init__(self, progress: _ProgressConsole) -> None: + super().__init__("l6_calibration_runner") + self.status: dict[str, Any] = {} + self.progress = progress + self.create_subscription( + String, "/l6_calibration/status", self._status_callback, 10 + ) + self.start_client = self.create_client(Trigger, "/l6_calibration/start") + self.abort_client = self.create_client(Trigger, "/l6_calibration/abort") + + def _status_callback(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 _launch_command( + config: ProductConfig, + session: Path, + *, + record_bag: bool, + commands_enabled: bool, +) -> list[str]: + arguments = { + "model": config.model, + "hand_type": config.side, + "tag_layout": config.tag_layout, + "serial_number": config.serial_number, + "can_interface": config.can_interface, + "source_urdf_path": str(config.source_urdf), + "source_urdf_expected_sha256": config.source_urdf_sha256, + "camera_extrinsics_file": str(config.camera_extrinsics), + "camera_extrinsics_expected_sha256": config.camera_extrinsics_sha256, + "calibration_config": str(config.calibration_config), + "calibration_config_expected_sha256": config.calibration_config_sha256, + "tag_config": str(config.tag_config), + "tag_config_expected_sha256": config.tag_config_sha256, + "output_root": str(config.output_root), + "session_dir": str(session), + "corrected_urdf_output_dir": str(session), + "recalibration_scope": "partial", + "calibration_speed": "1", + "index_roll_calibration_speed": "1", + "index_flex_calibration_speed": "1", + "commands_enabled": str(commands_enabled).lower(), + "record_bag": str(record_bag).lower(), + } + for view, camera in config.cameras.items(): + arguments[f"{view}_camera_serial"] = camera["serial_number"] + arguments[f"{view}_camera_name"] = camera["camera_name"] + arguments[f"{view}_camera_info_url"] = camera["camera_info"] + return [ + "ros2", "launch", "linkerhand_calibration", + "three_camera_calibration.launch.py", + *(f"{name}:={value}" for name, value in arguments.items()), + ] + + +def _wait_until( + monitor: _Monitor, + process: subprocess.Popen[Any], + predicate, + *, + timeout: float | None, +) -> bool: + started = time.monotonic() + while rclpy.ok(): + if process.poll() is not None: + return False + rclpy.spin_once(monitor, timeout_sec=0.2) + if predicate(monitor.status): + return True + if timeout is not None and time.monotonic() - started > timeout: + return False + return False + + +def _stop_stack(process: subprocess.Popen[Any]) -> None: + """Stop the whole launch process group without leaking child-node noise.""" + if process.poll() is not None: + return + try: + os.killpg(process.pid, signal.SIGINT) + except ProcessLookupError: + return + try: + process.wait(timeout=15.0) + except subprocess.TimeoutExpired: + try: + os.killpg(process.pid, signal.SIGTERM) + except ProcessLookupError: + return + try: + process.wait(timeout=5.0) + except subprocess.TimeoutExpired: + try: + os.killpg(process.pid, signal.SIGKILL) + except ProcessLookupError: + return + process.wait(timeout=5.0) + + +def _l6_failure_report( + config: ProductConfig, + session: Path, + status: Mapping[str, Any], + log_path: Path, +) -> str: + code, problem, suggestion = _l6_reason_zh(status) + task_key = status.get("task_name") + task = "无" if task_key is None else _TASK_LABELS.get(str(task_key), str(task_key)) + metrics = { + "task": task, + "cycle": status.get("cycle"), + "direction": status.get("direction"), + "valid_frames": status.get("valid_frames", 0), + "detection_frames": status.get("total_frames", 0), + "detection_rate": status.get("tag_detection_rate", 0.0), + "detection_rate_by_role": status.get("tag_detection_rate_by_role", {}), + "tag_seen_rate_by_role": status.get("tag_seen_rate_by_role", {}), + "recognized_tag_ids": status.get("recognized_tag_ids", []), + "unrecognized_tag_ids": status.get("unrecognized_tag_ids", []), + "joint_frame_rate": status.get("joint_frame_rate", 0.0), + "all_tags_quality_rate": status.get("all_tags_quality_rate", 0.0), + "pnp_valid_rate": status.get("pnp_valid_rate", 0.0), + "state_sync_rate": status.get("state_sync_rate", 0.0), + "observation_rejection_counts": status.get( + "observation_rejection_counts", {} + ), + "actual_u8": status.get("actual_u8"), + "current_command_u8": status.get("current_command_u8"), + "base_corner_drift_px": status.get("base_corner_drift_px", {}), + "state_publisher_count": status.get("state_publisher_count"), + "command_publisher_count": status.get("command_publisher_count"), + "failure_reason": status.get("reason"), + } + payload = { + "schema_version": 1, + "profile_id": status.get("profile_id"), + "serial_number": config.serial_number, + "result": "FAIL", + "error_code": code, + "stage": status.get("phase") + or ("fit" if str(status.get("reason", "")).startswith( + "fit_or_publication_failed:" + ) else status.get("state", "unknown")), + "reason": status.get("reason", "unknown"), + "problem_zh": problem, + "automatic_action_zh": "已停止运动、保持当前位置并禁止发布标定 JSON/URDF", + "suggestion_zh": suggestion, + "metrics": metrics, + "feedback_hz": status.get("feedback_hz", 0.0), + "hashes": { + "product_config_sha256": hashlib.sha256( + config.path.read_bytes() + ).hexdigest(), + "camera_extrinsics_sha256": config.camera_extrinsics_sha256, + "calibration_config_sha256": config.calibration_config_sha256, + "source_urdf_sha256": config.source_urdf_sha256, + }, + "session_dir": str(session), + "log_path": str(log_path), + "quality": {"passed": False}, + } + atomic_write_json(session / "calibration_summary_zh.json", payload) + return "\n".join( + [ + "========== 请复制以下内容给开发者 ==========", + f"会话编号：{config.serial_number}_{session.name}", + "结果：FAIL", + f"错误代码：{code}", + f"失败阶段：{payload['stage']}", + f"问题：{problem}", + f"自动处理：{payload['automatic_action_zh']}", + "关键指标：" + + json.dumps(metrics, ensure_ascii=False, separators=(",", ":")), + f"反馈状态：{float(payload['feedback_hz'] or 0.0):.1f} Hz", + f"配置哈希：{payload['hashes']['product_config_sha256']}", + f"外参哈希：{config.camera_extrinsics_sha256}", + f"源 URDF 哈希：{config.source_urdf_sha256}", + f"会话目录：{session}", + f"运行日志：{log_path}", + f"建议：{suggestion}", + "========== 复制结束 ==========", + ] + ) + + +def _run_online( + config: ProductConfig, + *, + record_bag: bool, + commands_enabled: bool, +) -> int: + stamp = datetime.now().strftime("%Y%m%d_%H%M%S") + session = config.session_root / stamp + while session.exists(): + time.sleep(1.0) + stamp = datetime.now().strftime("%Y%m%d_%H%M%S") + session = config.session_root / stamp + session.mkdir(parents=True) + command = _launch_command( + config, session, record_bag=record_bag, commands_enabled=commands_enabled + ) + log_path = session / "calibration.log" + log_stream = log_path.open("a", encoding="utf-8", buffering=1) + process = subprocess.Popen( + command, + cwd=config.workspace, + stdout=log_stream, + stderr=subprocess.STDOUT, + text=True, + start_new_session=True, + ) + rclpy.init() + monitor = _Monitor(_ProgressConsole()) + try: + ready = _wait_until( + monitor, + process, + lambda status: status.get("state") + in {"READY", "PAUSED", "ABORTED"}, + timeout=120.0, + ) + if ready and monitor.status.get("state") in {"PAUSED", "ABORTED"}: + print( + _l6_failure_report(config, session, monitor.status, log_path), + flush=True, + ) + return 3 + if not ready: + print("L6 启动失败：120秒内未收到六通道反馈和三相机内参。", flush=True) + return 2 + if not monitor.start_client.wait_for_service(timeout_sec=10.0): + print("L6 标定 /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"L6 标定未启动：{getattr(response, 'message', '')}", flush=True) + return 2 + print( + "L6右手标定已启动：通道0/1/5，预检/正式速度1，" + "全行程6秒余弦缓入缓出，torque 80。", + flush=True, + ) + finished = _wait_until( + monitor, + process, + lambda status: status.get("state") in {"PASSED", "PAUSED", "ABORTED"}, + timeout=None, + ) + if not finished: + print("L6 标定进程意外退出。", flush=True) + return 2 + status = monitor.status + if status.get("state") != "PASSED": + print(_l6_failure_report(config, session, status, log_path), flush=True) + return 3 + print( + "\n".join( + [ + "PASS：L6右手三主动关节与两条DIP实测通过；" + "小指结果已迁移到食指、中指和无名指。", + f"部分结果：{config.session_root / 'latest_partial_passed'}", + f"JSON：{status.get('final_json')}", + f"URDF：{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="L6 right partial three-camera 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 " + f"{config.source_urdf_sha256}", + flush=True, + ) + 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_l6_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_l6_raw_samples(selected.offline_raw), + publish=selected.publish_offline, + ) + print( + f"离线回放PASS：schema {payload['schema_version']}，URDF {correction.path}", + flush=True, + ) + return + raise SystemExit( + _run_online( + config, + record_bag=selected.record_bag, + commands_enabled=not selected.commands_disabled, + ) + ) + + +__all__ = ["main", "render_l6_progress_zh"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py new file mode 100644 index 0000000..d505f23 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py @@ -0,0 +1,342 @@ +"""Minimal, auditable URDF correction for the partial L6 profile.""" + +from __future__ import annotations + +from dataclasses import dataclass +from datetime import datetime +import math +import os +from pathlib import Path +import re +import shutil +from typing import Mapping +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + +from ..g20.zero_solver import _materialize_relative_mesh_assets +from .fitting import L6FitResult +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, +) + + +@dataclass(frozen=True) +class L6UrdfCorrection: + path: Path + origin_offsets_rad: Mapping[str, float] + corrected_limits_rad: Mapping[str, tuple[float, float]] + mimic_multipliers: Mapping[str, float] + coupling_polycoef: Mapping[str, tuple[float, ...]] + explicit_runtime_joints: frozenset[str] + + +def _triplet(value: str) -> np.ndarray: + result = np.asarray([float(item) for item in str(value).split()], dtype=float) + if result.shape != (3,) or not np.all(np.isfinite(result)): + raise ValueError(f"invalid URDF vector: {value}") + return result + + +def _replace_attribute( + block: str, element: str, attribute: str, value: str +) -> str: + pattern = re.compile( + rf"(<{element}\b[^>]*\b{attribute}\s*=\s*)([\"'])" + rf"(?P[^\"']*)\2", + re.DOTALL, + ) + match = pattern.search(block) + if match is None: + raise ValueError(f"{element} has no {attribute} attribute") + start, end = match.span("value") + return block[:start] + value + block[end:] + + +def _corrected_origin_rpy(joint: ET.Element, offset: float) -> str: + origin = joint.find("origin") + if origin is None or origin.get("rpy") is None: + raise ValueError(f"joint {joint.get('name')} has no origin.rpy") + # Preserve the reviewed CAD spelling exactly when the calibrated open + # endpoint is the source joint zero. Apart from avoiding Euler round-off, + # this makes it explicit that a zero correction must not rotate the frame. + if abs(float(offset)) <= 1.0e-12: + return str(origin.get("rpy")) + axis_node = joint.find("axis") + axis = _triplet( + "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") + ) + norm = float(np.linalg.norm(axis)) + if norm <= 1.0e-12: + raise ValueError(f"joint {joint.get('name')} has a degenerate axis") + source = Rotation.from_euler("xyz", _triplet(origin.get("rpy", "0 0 0"))) + corrected = source * Rotation.from_rotvec(axis / norm * float(offset)) + return " ".join( + f"{float(value):.15g}" for value in corrected.as_euler("xyz") + ) + + +def _validate_passive_ranges( + joints: Mapping[str, ET.Element], result: L6FitResult +) -> None: + for target in sorted(CORRECTED_PASSIVE_JOINTS): + source = MIMIC_SOURCE_BY_JOINT[target] + measured_source = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get( + source, source + ) + measured_target = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get( + target, target + ) + joint = joints[target] + mimic = joint.find("mimic") + limit = joint.find("limit") + if mimic is None or limit is None: + raise ValueError(f"passive joint is missing mimic or limit: {target}") + if mimic.get("joint") != source: + raise ValueError(f"passive source differs from profile: {target}") + offset = float(mimic.get("offset", "0")) + coupling = result.mimic_fits[measured_target] + travel = float(result.travels_rad[measured_source]) + grid = np.linspace(0.0, travel, 256) + predicted = np.full_like(grid, offset) + for power, coefficient in enumerate(coupling.coefficients, 1): + predicted += float(coefficient) * grid ** power + lower = float(limit.get("lower", "-inf")) + upper = float(limit.get("upper", "inf")) + if ( + float(np.min(predicted)) < lower - 1.0e-8 + or float(np.max(predicted)) > upper + 1.0e-8 + ): + raise ValueError( + f"{target} fitted coupling range exceeds preserved passive limit" + ) + measured = result.curves[measured_target] + observed = np.concatenate( + ( + np.asarray(measured.decreasing_rad, dtype=float), + np.asarray(measured.increasing_rad, dtype=float), + ) + ) + offset + if float(np.min(observed)) < lower - 1.0e-8 or float( + np.max(observed) + ) > upper + 1.0e-8: + raise ValueError( + f"{target} measured curve exceeds preserved passive limit" + ) + + +def write_l6_corrected_urdf( + *, + source_urdf: str | Path, + output_directory: str | Path, + serial_number: str, + result: L6FitResult, + timestamp: str | None = None, +) -> L6UrdfCorrection: + source = Path(source_urdf).expanduser().resolve() + output = Path(output_directory).expanduser().resolve() + if not source.is_file(): + raise ValueError(f"source URDF does not exist: {source}") + if "calibrated" in source.stem.lower(): + raise ValueError("source URDF must be the immutable original CAD file") + if set(result.zero_offsets_rad) != CALIBRATED_ACTIVE_JOINTS: + raise ValueError("L6 zero result has the wrong active joint set") + if set(result.travels_rad) != CALIBRATED_ACTIVE_JOINTS: + raise ValueError("L6 travel result has the wrong active joint set") + if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS: + raise ValueError("L6 mimic result has the wrong passive joint set") + + tree = ET.parse(source) + root = tree.getroot() + joints = { + str(joint.get("name")): joint for joint in root.findall("joint") + } + required = CORRECTED_ACTIVE_JOINTS | CORRECTED_PASSIVE_JOINTS + missing = required - set(joints) + if missing: + raise ValueError("source URDF is missing L6 targets: " + ",".join(sorted(missing))) + _validate_passive_ranges(joints, result) + + active_replacements: dict[str, tuple[str, str, str]] = {} + for name in sorted(CORRECTED_ACTIVE_JOINTS): + measured_source = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name) + offset = float(result.zero_offsets_rad[measured_source]) + travel = float(result.travels_rad[measured_source]) + if not math.isfinite(offset) or abs(offset) > math.radians(15.0): + raise ValueError(f"invalid L6 zero offset: {name}") + if not math.isfinite(travel) or not 0.0 < travel < math.pi: + raise ValueError(f"invalid L6 travel: {name}") + active_replacements[name] = ( + _corrected_origin_rpy(joints[name], offset), + "0", + f"{travel:.15g}", + ) + + mimic_replacements = { + name: f"{float(result.mimic_fits[donor].urdf_mimic_multiplier):.15g}" + for name, donor in sorted( + { + target: TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get( + target, target + ) + for target in CORRECTED_PASSIVE_JOINTS + }.items() + ) + } + explicit_runtime_joints = frozenset( + name for name in CORRECTED_PASSIVE_JOINTS + if result.mimic_fits[ + TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name) + ].model == "quadratic_runtime" + ) + coupling_polycoef = { + name: tuple( + float(value) + for value in result.mimic_fits[ + TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name) + ].mujoco_polycoef + ) + for name in sorted(CORRECTED_PASSIVE_JOINTS) + } + equality_replacements: dict[str, str] = {} + for equality in root.findall("./mujoco/equality/joint"): + target = str(equality.get("joint1", "")) + source_name = str(equality.get("joint2", "")) + if target not in coupling_polycoef: + continue + if MIMIC_SOURCE_BY_JOINT[target] != source_name: + raise ValueError(f"MuJoCo equality source differs for {target}") + coefficients = [ + float(value) for value in str(equality.get("polycoef", "")).split() + ] + if len(coefficients) != 6: + raise ValueError(f"MuJoCo equality polycoef is invalid for {target}") + offset = float(joints[target].find("mimic").get("offset", "0")) + coefficients = list(coupling_polycoef[target]) + coefficients[0] = offset + equality_name = str(equality.get("name", "")) + if not equality_name: + raise ValueError(f"MuJoCo equality has no name for {target}") + equality_replacements[equality_name] = " ".join( + f"{value:.15g}" for value in coefficients + ) + if len(equality_replacements) != len(coupling_polycoef): + raise ValueError("source URDF lacks a MuJoCo equality for fitted coupling") + + original = source.read_text(encoding="utf-8") + # The file also contains MuJoCo/transmission ```` elements. Requiring + # a URDF ``type`` attribute keeps the surgical block replacement confined + # to the eleven kinematic joints. + joint_pattern = re.compile( + r"]*\btype\s*=)[^>]*\bname\s*=\s*" + r"([\"'])(?P[^\"']+)\1[^>]*>" + r".*?", + re.DOTALL, + ) + + def replace_joint(match: re.Match[str]) -> str: + name = match.group("name") + block = match.group(0) + if name in active_replacements: + rpy, lower, upper = active_replacements[name] + block = _replace_attribute(block, "origin", "rpy", rpy) + block = _replace_attribute(block, "limit", "lower", lower) + block = _replace_attribute(block, "limit", "upper", upper) + if name in mimic_replacements: + block = _replace_attribute( + block, "mimic", "multiplier", mimic_replacements[name] + ) + return block + + corrected = joint_pattern.sub(replace_joint, original) + for equality_name, polycoef in equality_replacements.items(): + equality_pattern = re.compile( + rf"(]*\bname\s*=\s*([\"'])" + rf"{re.escape(equality_name)}\2[^>]*>)", + re.DOTALL, + ) + match = equality_pattern.search(corrected) + if match is None: + raise ValueError(f"could not locate MuJoCo equality {equality_name}") + replacement = _replace_attribute( + match.group(0), "joint", "polycoef", polycoef + ) + corrected = corrected[: match.start()] + replacement + corrected[match.end() :] + + stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") + if re.fullmatch(r"\d{8}_\d{6}", stamp) is None: + raise ValueError("URDF zero timestamp must use YYYYMMDD_HHMMSS") + safe_serial = "".join( + char if char.isalnum() or char in "_.-" else "_" + for char in str(serial_number) + ) + if not safe_serial: + raise ValueError("serial number must not be empty") + output.mkdir(parents=True, exist_ok=True) + destination = output / ( + f"{source.stem}_partial_zero_calibrated_{safe_serial}_{stamp}.urdf" + ) + if destination.exists() or destination == source: + raise ValueError(f"refusing to overwrite URDF: {destination}") + _materialize_relative_mesh_assets(source=source, output=output, urdf_root=root) + # Keep the complete vendor mesh bundle beside the generated URDF, including + # auxiliary meshes not referenced by this particular XML revision. + source_meshes = source.parent / "meshes" + if source_meshes.is_dir(): + destination_meshes = output / "meshes" + destination_meshes.mkdir(parents=True, exist_ok=True) + for mesh in source_meshes.iterdir(): + if mesh.is_file(): + shutil.copy2(mesh, destination_meshes / mesh.name) + temporary = destination.with_suffix(".urdf.tmp") + try: + with temporary.open("w", encoding="utf-8") as stream: + stream.write(corrected) + stream.flush() + os.fsync(stream.fileno()) + os.replace(temporary, destination) + finally: + if temporary.exists(): + temporary.unlink() + return L6UrdfCorrection( + 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) + }, + mimic_multipliers={ + name: float( + result.mimic_fits[ + TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name) + ].urdf_mimic_multiplier + ) + for name in sorted(mimic_replacements) + }, + coupling_polycoef=coupling_polycoef, + explicit_runtime_joints=explicit_runtime_joints, + ) + + +__all__ = ["L6UrdfCorrection", "write_l6_corrected_urdf"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py index 7944db5..f2b53b0 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py @@ -77,8 +77,10 @@ def get_default_registry() -> ProfileRegistry: global _DEFAULT_REGISTRY if _DEFAULT_REGISTRY is None: from .g20 import register_profiles + from .l6 import register_profiles as register_l6_profiles registry = ProfileRegistry() register_profiles(registry) + register_l6_profiles(registry) _DEFAULT_REGISTRY = registry return _DEFAULT_REGISTRY diff --git a/src/linkerhand_calibration/linkerhand_calibration/operator_report.py b/src/linkerhand_calibration/linkerhand_calibration/operator_report.py index 6b181e3..be89347 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/operator_report.py +++ b/src/linkerhand_calibration/linkerhand_calibration/operator_report.py @@ -21,6 +21,37 @@ def _duration(seconds: float | None) -> str: return f"{value // 60}分{value % 60:02d}秒" +def render_compact_progress_header_zh( + *, + serial_number: str, + progress: float, + eta: str, + stage: str, + cycle: object, + repetitions: object, + task: str, + requested: object, + actual: object, + direction: str, + tag_status: str, + ready_cameras: int, + feedback_hz: float, + valid_frames: int, + automatic_retry_count: int, +) -> list[str]: + """Render the model-independent five-line operator progress header.""" + return [ + f"[{serial_number}] 标定中 {float(progress) * 100:5.1f}% " + f"预计剩余 {eta}", + f"阶段：{stage}（第 {cycle}/{repetitions} 轮）", + f"任务：{task} 命令/反馈：{requested}/{actual} 方向：{direction}", + f"Tag：{tag_status} 相机：{int(ready_cameras)}/3 正常 " + f"反馈：{float(feedback_hz):.1f} Hz", + f"质量：有效帧 {int(valid_frames)} 已自动重扫 " + f"{int(automatic_retry_count)} 次", + ] + + def _task_tag_id_status(views: Mapping[str, Any]) -> str: """Render every currently required Tag ID with its live visibility.""" labels = {"front": "正面", "side": "侧面", "top": "顶部"} @@ -273,13 +304,23 @@ def render_progress_zh( == "waiting_for_task_tags_at_sweep_start" else _duration(estimator.remaining(progress)) ) - lines = [ - f"[{serial_number}] 标定中 {progress * 100:5.1f}% 预计剩余 {eta}", - f"阶段：{stage}（第 {cycle}/{repetitions} 轮）", - f"任务：{joint} 命令/反馈：{requested}/{actual} 方向：{direction}", - f"Tag：{tag_status}{task_tag_ids} 相机：{ready_cameras}/3 正常 反馈：{feedback_hz:.1f} Hz", - f"质量：有效帧 {active.get('valid_frames', 0)} 已自动重扫 {retry} 次", - ] + lines = render_compact_progress_header_zh( + serial_number=serial_number, + progress=progress, + eta=eta, + stage=stage, + cycle=cycle, + repetitions=repetitions, + task=joint, + requested=requested, + actual=actual, + direction=direction, + tag_status=tag_status + task_tag_ids, + ready_cameras=ready_cameras, + feedback_hz=feedback_hz, + valid_frames=int(active.get("valid_frames", 0)), + automatic_retry_count=retry, + ) if str(status.get("reason", "")) == "waiting_for_task_tags_at_sweep_start": pnp_wait = _pnp_wait_status(views) if pnp_wait: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py index 774eb31..07db0e5 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py @@ -27,13 +27,19 @@ def build_session_controller( def main(args: list[str] | None = None) -> None: """Select the profile first, then delegate to its reviewed CLI strategy.""" selector = argparse.ArgumentParser(add_help=False) - selector.add_argument( - "--config", default=str(default_product_config_path()) - ) + # Resolve the installed default lazily. A caller that supplies --config + # must also work directly from a source workspace before the package has + # been installed. + selector.add_argument("--config", default=None) selector.add_argument("--workspace", default=None) selected, _ = selector.parse_known_args(args) + config_path = ( + str(default_product_config_path()) + if selected.config is None + else selected.config + ) product = load_product_config( - selected.config, + config_path, workspace=selected.workspace, check_can=False, ) diff --git a/src/linkerhand_calibration/test/test_architecture.py b/src/linkerhand_calibration/test/test_architecture.py index 032c573..787e7c4 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) == 3 + assert len(registry) == 4 for registered in registry: validate_profile(registered.profile) assert registered.profile.zero.active_joints diff --git a/src/linkerhand_calibration/test/test_l6_right_profile.py b/src/linkerhand_calibration/test/test_l6_right_profile.py new file mode 100644 index 0000000..6eca122 --- /dev/null +++ b/src/linkerhand_calibration/test/test_l6_right_profile.py @@ -0,0 +1,828 @@ +from __future__ import annotations + +from pathlib import Path +import re +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, + default_input_topic, +) +from linkerhand_calibration.core import validate_profile +from linkerhand_calibration.models.l6.artifacts import ( + build_l6_runtime_payload, + validate_l6_runtime_payload, +) +from linkerhand_calibration.models.l6.fitting import fit_l6_session +from linkerhand_calibration.models.l6.motion import cosine_position_trajectory_u8 +from linkerhand_calibration.models.l6.node import ( + L6ThreeCameraCalibrationNode, + MotionStep, +) +from linkerhand_calibration.models.l6.pipeline import ( + accepted_records_by_joint, + canonical_feedback_command_u8, + finalize_l6_session, +) +from linkerhand_calibration.models.l6.profile import ( + ACTIVE_JOINTS, + CALIBRATED_ACTIVE_JOINTS, + COMMAND_NAMES, + ENDPOINT_ANCHOR_BY_JOINT, + KEY, + PASSIVE_JOINTS, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, + build_typed_profile, +) +from linkerhand_calibration.models.l6.runner import render_l6_progress_zh +from linkerhand_calibration.models.l6.urdf import write_l6_corrected_urdf +from linkerhand_calibration.extrinsics import matrix_payload, transform_matrix +from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel +from linkerhand_calibration.product import load_product_config, sha256_file + + +PACKAGE = Path(__file__).resolve().parents[1] +SOURCE = PACKAGE / "urdf/l6_right/linkerhand_l6v3.1_right.urdf" +PRODUCT = PACKAGE / "config/l6_right_product.yaml" + +TRAVELS = { + "rh_thumb_cmc_roll": 1.34, + "rh_thumb_cmc_pitch": 0.78, + "rh_pinky_mcp_pitch": 1.10, +} +MULTIPLIERS = {"rh_thumb_dip": 1.20, "rh_pinky_dip": 1.10} +THUMB_ZERO_OFFSETS = { + "rh_thumb_cmc_roll": 0.04, + "rh_thumb_cmc_pitch": -0.05, +} +KINEMATIC_MODEL = UrdfKinematicModel(SOURCE) + + +def _pose(xyz: list[float], rpy: list[float]) -> np.ndarray: + return transform_matrix(xyz, Rotation.from_euler("xyz", rpy).as_quat()) + + +def _geometric_observation( + joint: str, + feedback: int, + *, + thumb_zero_offsets: dict[str, float] | None = None, +) -> dict[str, object]: + model = KINEMATIC_MODEL + state = [255.0] * 6 + motor = { + "rh_thumb_cmc_pitch": 0, + "rh_thumb_dip": 0, + "rh_thumb_cmc_roll": 1, + "rh_pinky_mcp_pitch": 5, + "rh_pinky_dip": 5, + }[joint] + state[motor] = float(feedback) + active_angles = { + name: travel * (255.0 - state[index]) / 255.0 + for name, travel, index in ( + ("rh_thumb_cmc_pitch", TRAVELS["rh_thumb_cmc_pitch"], 0), + ("rh_thumb_cmc_roll", TRAVELS["rh_thumb_cmc_roll"], 1), + ("rh_pinky_mcp_pitch", TRAVELS["rh_pinky_mcp_pitch"], 5), + ) + } + joint_angles = { + **active_angles, + "rh_thumb_dip": active_angles["rh_thumb_cmc_pitch"] + * MULTIPLIERS["rh_thumb_dip"], + "rh_pinky_dip": active_angles["rh_pinky_mcp_pitch"] + * MULTIPLIERS["rh_pinky_dip"], + } + zero_offsets = { + **( + THUMB_ZERO_OFFSETS + if thumb_zero_offsets is None + else thumb_zero_offsets + ), + "rh_pinky_mcp_pitch": 0.0, + } + base_common = _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_roll": _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_dip": _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]), + } + view = { + "rh_thumb_cmc_roll": "top", + "rh_thumb_cmc_pitch": "front", + "rh_thumb_dip": "front", + "rh_pinky_mcp_pitch": "side", + "rh_pinky_dip": "side", + }[joint] + if joint in {"rh_thumb_dip", "rh_pinky_dip"}: + parent_joint = { + "rh_thumb_dip": "rh_thumb_cmc_pitch", + "rh_pinky_dip": "rh_pinky_mcp_pitch", + }[joint] + parent = base_common @ model.link_transform( + parent_joint, + zero_offsets=zero_offsets, + joint_angles=joint_angles, + independent_mimic_angles=True, + ) @ link_mounts[parent_joint] + else: + parent = base_common @ base_mounts[view] + child = base_common @ model.link_transform( + joint, + zero_offsets=zero_offsets, + joint_angles=joint_angles, + independent_mimic_angles=True, + ) @ link_mounts[joint] + relative = np.linalg.inv(parent) @ child + return { + "state_u8": state, + "relative_translation_xyz_m": relative[:3, 3].tolist(), + "relative_quaternion_xyzw": Rotation.from_matrix( + relative[:3, :3] + ).as_quat().tolist(), + "parent_pose_common": matrix_payload(parent), + "child_pose_common": matrix_payload(child), + "view_normal_common_xyz": [0.577350269, 0.577350269, 0.577350269], + "camera_center_common_xyz_m": [0.0, 0.0, -0.5], + } + + +def _synthetic_records( + *, thumb_zero_offsets: dict[str, float] | None = None +) -> list[dict]: + result = [] + task_by_joint = { + "rh_thumb_cmc_roll": "thumb_roll_top", + "rh_thumb_cmc_pitch": "thumb_pitch_dip_front", + "rh_thumb_dip": "thumb_pitch_dip_front", + "rh_pinky_mcp_pitch": "pinky_pitch_dip_side", + "rh_pinky_dip": "pinky_pitch_dip_side", + } + source_by_passive = { + "rh_thumb_dip": "rh_thumb_cmc_pitch", + "rh_pinky_dip": "rh_pinky_mcp_pitch", + } + for name, task in task_by_joint.items(): + travel = ( + TRAVELS[name] + if name in TRAVELS + else TRAVELS[source_by_passive[name]] * MULTIPLIERS[name] + ) + for cycle in range(4): + for direction, commands in ( + ("decreasing", range(255, -1, -4)), + ("increasing", range(0, 256, 4)), + ): + values = list(commands) + if values[-1] not in {0, 255}: + values.append(0 if direction == "decreasing" else 255) + for feedback in values: + angle = travel * (255 - feedback) / 255.0 + geometry = _geometric_observation( + name, + feedback, + thumb_zero_offsets=thumb_zero_offsets, + ) + result.append( + { + "kind": "l6_joint_sample", + "task_name": task, + "joint": name, + "cycle": cycle, + "direction": direction, + "attempt": 1, + "feedback_u8": float(feedback), + **geometry, + } + ) + return result + + +def _synthetic_nonlinear_pinky_records() -> list[dict]: + records = _synthetic_records() + mounted = Rotation.from_euler("xyz", [0.2, -0.1, 0.3]) + for row in records: + if row["joint"] != "rh_pinky_dip": + continue + active = TRAVELS["rh_pinky_mcp_pitch"] * ( + 255.0 - float(row["feedback_u8"]) + ) / 255.0 + angle = 1.2 * active - 0.2 * active * active + observed = mounted * Rotation.from_rotvec([angle, 0.0, 0.0]) + row["relative_quaternion_xyzw"] = observed.as_quat().tolist() + return records + + +def _joint_block(text: str, name: str) -> str: + pattern = re.compile( + rf"]*\btype\s*=)[^>]*\bname=\"{re.escape(name)}\"" + rf"[^>]*>.*?", + re.DOTALL, + ) + match = pattern.search(text) + assert match is not None + return match.group(0) + + +def test_l6_profile_declares_six_channels_eight_tags_and_partial_scope() -> None: + profile = build_typed_profile() + validate_profile(profile) + assert profile.key == KEY + assert profile.namespace == "/l6_calibration" + assert profile.command.names == COMMAND_NAMES + assert profile.command.baseline_u8 == (255,) * 6 + assert profile.command.feedback_name_aliases == { + "thumb_cmc_yaw": "thumb_cmc_roll" + } + assert profile.motion.speed_parameters["preflight_u8"] == 1 + assert profile.motion.speed_parameters["formal_u8"] == 1 + assert profile.motion.speed_parameters["speed_settle_seconds"] == 0.2 + assert ( + profile.motion.speed_parameters["command_trajectory_full_range_seconds"] + == 6.0 + ) + assert all(task.preflight_speed_u8 == 1 for task in profile.motion.tasks) + assert all(task.formal_speed_u8 == 1 for task in profile.motion.tasks) + assert profile.vision.tag_ids == frozenset(range(8)) + assert profile.scope.selected_joints("partial") == CALIBRATED_ACTIVE_JOINTS + assert len(profile.joint_coverage) == 11 + assert profile.artifacts.publication_pointer == "latest_partial_passed" + assert profile.zero.coupling_model_by_joint == { + "rh_thumb_dip": "linear_mimic", + "rh_pinky_dip": "quadratic_runtime", + "rh_index_dip": "quadratic_runtime", + "rh_middle_dip": "quadratic_runtime", + "rh_ring_dip": "quadratic_runtime", + } + assert profile.zero.endpoint_anchor_by_joint == ENDPOINT_ANCHOR_BY_JOINT + + +def test_l6_operator_progress_names_current_task_round_and_direction() -> None: + text = render_l6_progress_zh( + { + "state": "RUNNING", + "serial_number": "L6_TEST", + "step_index": 42, + "step_count": 61, + "step_fraction": 0.5, + "task_name": "pinky_pitch_dip_side", + "phase": "sweep", + "cycle": 3, + "direction": "decreasing", + "attempt": 2, + "target_u8": 0, + "current_command_u8": 130.0, + "actual_u8": 127.5, + "valid_frames": 50, + "total_frames": 52, + "tag_detection_rate": 50 / 52, + "joint_frame_rate": 50 / 52, + "recognized_tag_ids": [3, 4], + "unrecognized_tag_ids": [5], + "feedback_hz": 29.8, + "formal_speed_u8": 1, + } + ) + assert "[L6_TEST] 标定中" in text + assert "小指 MCP pitch / DIP（侧面机位，ID3→ID4→ID5）" in text + assert "阶段：正式扫描（第 4/4 轮）" in text + assert "方向：递减" in text + assert "已识别 ID3/ID4" in text + assert "未识别/不合格 ID5" in text + assert "已自动重扫 1 次" in text + assert "步骤" not in text + + +def test_l6_six_second_cosine_trajectory_is_monotonic_and_smooth() -> None: + start, start_phase, duration = cosine_position_trajectory_u8( + 255.0, 0.0, 0.0, 6.0 + ) + middle, middle_phase, _ = cosine_position_trajectory_u8( + 255.0, 0.0, 3.0, 6.0 + ) + end, end_phase, _ = cosine_position_trajectory_u8( + 255.0, 0.0, 6.0, 6.0 + ) + assert (start, start_phase, duration) == pytest.approx((255.0, 0.0, 6.0)) + assert (middle, middle_phase) == pytest.approx((127.5, 0.5)) + assert (end, end_phase) == pytest.approx((0.0, 1.0)) + + commands = [ + round(cosine_position_trajectory_u8(255.0, 0.0, tick / 100.0, 6.0)[0]) + for tick in range(601) + ] + assert all(right <= left for left, right in zip(commands, commands[1:])) + assert max(abs(right - left) for left, right in zip(commands, commands[1:])) <= 1 + assert cosine_position_trajectory_u8(255.0, 127.5, 1.5, 6.0)[2] == pytest.approx(3.0) + + +def test_l6_node_streams_the_six_second_trajectory_without_command_jumps() -> None: + published: list[list[int]] = [] + fake = SimpleNamespace( + 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_pitch_dip_front", 0, 0, 1, 0, "decreasing") + for tick in range(601): + L6ThreeCameraCalibrationNode._advance_step_trajectory( + fake, step, tick / 100.0 + ) + + channel = [command[0] for command in published] + assert channel[0] == 255 + assert channel[-1] == 0 + assert max(abs(right - left) for left, right in zip(channel, channel[1:])) <= 1 + assert all(command[1:] == [255] * 5 for command in published) + + +def _l6_sweep_quality_fake(tmp_path: Path) -> tuple[SimpleNamespace, MotionStep]: + task_key = "pinky_pitch_dip_side" + feedback = np.linspace(255.0, 0.0, 188) + fake = SimpleNamespace( + raw_records=[ + { + "task_name": task_key, + "cycle": 3, + "direction": "decreasing", + "attempt": 3, + "joint": "rh_pinky_mcp_pitch", + "feedback_u8": float(value), + } + for value in feedback + ], + raw_path=tmp_path / "raw_samples.jsonl", + step_required_roles=("side_base", "pinky_pitch", "pinky_dip"), + step_total_frames=211, + step_tag_seen_frames={ + "side_base": 211, + "pinky_pitch": 210, + "pinky_dip": 208, + }, + # Every Tag independently clears 95%, while the fully joined frames + # match the real 20260901_181435 sweep at about 89.1%. + step_tag_quality_frames={ + "side_base": 209, + "pinky_pitch": 205, + "pinky_dip": 202, + }, + step_all_tags_quality_frames=190, + step_pnp_valid_frames=189, + step_state_sync_frames=188, + step_valid_frames=188, + step_rejection_counts={}, + minimum_sweep_frames=40, + minimum_state_span_u8=240.0, + minimum_sweep_bins=32, + maximum_bin_gap=16, + minimum_detection_rate=0.95, + minimum_joint_frame_rate=0.85, + minimum_feedback_hz=25.0, + profile=build_typed_profile(), + _task=lambda _key: SimpleNamespace( + key=task_key, + joints=("rh_pinky_mcp_pitch", "rh_pinky_dip"), + ), + _feedback_hz=lambda: 58.9, + ) + fake._step_observation_metrics = lambda: ( + L6ThreeCameraCalibrationNode._step_observation_metrics(fake) + ) + step = MotionStep( + "sweep", task_key, 5, 0, 1, 3, "decreasing", attempt=3 + ) + return fake, step + + +def test_l6_quality_gates_each_tag_separately_from_joined_frames( + tmp_path: Path, +) -> None: + fake, step = _l6_sweep_quality_fake(tmp_path) + metrics = L6ThreeCameraCalibrationNode._step_observation_metrics(fake) + assert metrics["tag_detection_rate"] == pytest.approx(202 / 211) + assert metrics["joint_frame_rate"] == pytest.approx(188 / 211) + + # This is a valid sweep: every individual Tag is >=95%, the joined frame + # rate is >=85%, and all existing trajectory coverage gates still pass. + L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step) + + +def test_l6_quality_failure_names_the_specific_tag_id(tmp_path: Path) -> None: + fake, step = _l6_sweep_quality_fake(tmp_path) + fake.step_tag_quality_frames["pinky_dip"] = 190 + with pytest.raises(ValueError, match=r"tag_rate\[ID5/pinky_dip\]=0\.900"): + L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step) + + +def test_l6_operator_progress_shows_exact_failed_sweep_metric() -> None: + text = render_l6_progress_zh( + { + "state": "PAUSED", + "serial_number": "L6_TEST", + "reason": "sweep_quality_failed:thumb_roll_top:maximum_gap=22", + "phase": "sweep", + } + ) + assert "具体未通过项：maximum_gap=22" in text + + +def test_l6_operator_progress_explains_nonlinear_mimic_failure() -> None: + text = render_l6_progress_zh( + { + "state": "PAUSED", + "serial_number": "L6_TEST", + "reason": ( + "fit_or_publication_failed:mimic_residual_exceeds:" + "joint=rh_pinky_dip:multiplier=1.008793:" + "p95_deg=4.715:maximum_deg=6.476" + ), + } + ) + assert "rh_pinky_dip 的 linear_mimic 耦合模型未达到精度门限" in text + assert "P95=4.715°、最大=6.476°" in text + + +def test_l6_product_config_and_immutable_source_hash_are_valid() -> None: + product = load_product_config(PRODUCT, workspace=PACKAGE.parents[1], check_can=False) + assert product.profile_key == KEY + assert product.source_urdf == SOURCE.resolve() + assert sha256_file(SOURCE) == ( + "298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666" + ) + assert product.camera_extrinsics_sha256 == ( + "dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80" + ) + + +def test_l6_synthetic_fit_recovers_travel_zero_and_mimic() -> None: + records = accepted_records_by_joint(_synthetic_records()) + result = fit_l6_session(SOURCE, records) + for name, expected in TRAVELS.items(): + assert result.travels_rad[name] == pytest.approx(expected, abs=2.0e-4) + source_limits = { + joint.get("name"): { + field: float(joint.find("limit").get(field)) + for field in ("lower", "upper") + } + for joint in ET.parse(SOURCE).getroot().findall("joint") + if joint.find("limit") is not None + } + for name, expected_offset in THUMB_ZERO_OFFSETS.items(): + assert result.zero_offsets_rad[name] == pytest.approx( + expected_offset, abs=2.0e-4 + ) + assert result.zero_method_by_joint[name] == ( + "urdf_serial_axis_geometry" + ) + assert result.zero_fallback_reason_by_joint == {} + assert result.zero_offsets_rad["rh_pinky_mcp_pitch"] == pytest.approx( + source_limits["rh_pinky_mcp_pitch"]["lower"], + abs=2.0e-4, + ) + assert result.zero_method_by_joint["rh_pinky_mcp_pitch"] == ( + "mechanical_lower_endpoint" + ) + for name, expected in MULTIPLIERS.items(): + assert result.mimic_fits[name].multiplier == pytest.approx(expected, abs=2.0e-4) + assert result.mimic_fits[name].maximum_cycle_range < 1.0e-6 + + +def test_l6_pitch_geometry_bound_centres_measured_range_in_cad_range() -> None: + records = accepted_records_by_joint( + _synthetic_records( + thumb_zero_offsets={ + "rh_thumb_cmc_roll": THUMB_ZERO_OFFSETS[ + "rh_thumb_cmc_roll" + ], + "rh_thumb_cmc_pitch": 1.2, + } + ) + ) + result = fit_l6_session(SOURCE, records) + source_pitch = next( + joint + for joint in ET.parse(SOURCE).getroot().findall("joint") + if joint.get("name") == "rh_thumb_cmc_pitch" + ) + expected = 0.5 * ( + float(source_pitch.find("limit").get("lower")) + + float(source_pitch.find("limit").get("upper")) + - TRAVELS["rh_thumb_cmc_pitch"] + ) + assert result.zero_offsets_rad["rh_thumb_cmc_pitch"] == pytest.approx( + expected, abs=2.0e-4 + ) + assert result.zero_method_by_joint["rh_thumb_cmc_pitch"] == ( + "cad_range_center_after_geometry_rejection" + ) + assert result.zero_fallback_reason_by_joint == { + "rh_thumb_cmc_pitch": "zero_offset_reached_diagnostic_bound" + } + # A pitch-only fallback must not replace the independently observed roll. + assert result.zero_offsets_rad["rh_thumb_cmc_roll"] == pytest.approx( + THUMB_ZERO_OFFSETS["rh_thumb_cmc_roll"], abs=2.0e-4 + ) + + +def test_l6_fit_recovers_stable_nonlinear_pinky_coupling( + tmp_path: Path, +) -> None: + result = fit_l6_session( + SOURCE, accepted_records_by_joint(_synthetic_nonlinear_pinky_records()) + ) + coupling = result.mimic_fits["rh_pinky_dip"] + assert coupling.model == "quadratic_runtime" + assert coupling.coefficients == pytest.approx((1.2, -0.2), abs=2.0e-4) + assert coupling.maximum_cycle_prediction_range_rad < 1.0e-6 + assert coupling.residual_p95_rad < 2.0e-5 + correction = write_l6_corrected_urdf( + source_urdf=SOURCE, + output_directory=tmp_path, + serial_number="L6_NONLINEAR_TEST", + result=result, + timestamp="20260901_120002", + ) + root = ET.parse(correction.path).getroot() + pinky = next( + joint + for joint in root.findall("joint") + if joint.get("name") == "rh_pinky_dip" + ) + pinky_mimic = pinky.find("mimic") + assert pinky_mimic is not None + assert float(pinky_mimic.get("multiplier")) == pytest.approx( + (1.2 * TRAVELS["rh_pinky_mcp_pitch"] + - 0.2 * TRAVELS["rh_pinky_mcp_pitch"] ** 2) + / TRAVELS["rh_pinky_mcp_pitch"], + abs=2.0e-4, + ) + equality = next( + joint + for joint in root.findall("./mujoco/equality/joint") + if joint.get("joint1") == "rh_pinky_dip" + ) + assert [float(value) for value in equality.get("polycoef").split()] == ( + pytest.approx([0.0, 1.2, -0.2, 0.0, 0.0, 0.0], abs=2.0e-4) + ) + for finger in ("index", "middle", "ring"): + transferred = next( + joint + for joint in root.findall("./mujoco/equality/joint") + if joint.get("joint1") == f"rh_{finger}_dip" + ) + assert [ + float(value) for value in transferred.get("polycoef").split() + ] == pytest.approx( + [0.0, 1.2, -0.2, 0.0, 0.0, 0.0], abs=2.0e-4 + ) + + +def test_l6_feedback_endpoint_deadband_is_canonicalized_for_fitting() -> None: + assert [canonical_feedback_command_u8(value) for value in (0, 1, 2)] == [0] * 3 + assert canonical_feedback_command_u8(3) == 3 + assert canonical_feedback_command_u8(252) == 252 + assert [canonical_feedback_command_u8(value) for value in (253, 254, 255)] == [255] * 3 + + records = _synthetic_records() + for row in records: + if row["feedback_u8"] == 0.0: + row["feedback_u8"] = 1.0 + elif row["feedback_u8"] == 255.0: + row["feedback_u8"] = 254.0 + result = fit_l6_session(SOURCE, accepted_records_by_joint(records)) + for name, expected in TRAVELS.items(): + assert result.travels_rad[name] == pytest.approx(expected, abs=2.0e-4) + + +def test_l6_urdf_writer_changes_only_authorized_joint_fields(tmp_path: Path) -> None: + result = fit_l6_session(SOURCE, accepted_records_by_joint(_synthetic_records())) + correction = write_l6_corrected_urdf( + source_urdf=SOURCE, + output_directory=tmp_path, + serial_number="L6_TEST", + result=result, + timestamp="20260901_120000", + ) + original = SOURCE.read_text(encoding="utf-8") + corrected = correction.path.read_text(encoding="utf-8") + original_root = ET.parse(SOURCE).getroot() + original_joints = { + joint.get("name"): joint for joint in original_root.findall("joint") + } + root = ET.parse(correction.path).getroot() + joints = {joint.get("name"): joint for joint in root.findall("joint")} + # The already reviewed DIP geometry and limits stay byte-equivalent; only + # the measured mimic multiplier and matching MuJoCo equality may change. + for passive_name in PASSIVE_JOINTS: + passive = joints[passive_name] + source_passive = original_joints[passive_name] + for element in ("origin", "axis", "limit", "parent", "child"): + assert passive.find(element).attrib == source_passive.find(element).attrib + assert passive.find("mimic").get("offset") == ( + source_passive.find("mimic").get("offset") + ) + for finger in ("index", "middle", "ring"): + active_name = f"rh_{finger}_mcp_pitch" + passive_name = f"rh_{finger}_dip" + active = joints[active_name] + source_active = original_joints[active_name] + assert active.find("origin").get("xyz") == source_active.find("origin").get("xyz") + assert active.find("origin").get("rpy") == source_active.find("origin").get("rpy") + assert active.find("axis").attrib == source_active.find("axis").attrib + assert active.find("parent").attrib == source_active.find("parent").attrib + assert active.find("child").attrib == source_active.find("child").attrib + assert float(active.find("limit").get("lower")) == 0.0 + assert float(active.find("limit").get("upper")) == pytest.approx( + TRAVELS["rh_pinky_mcp_pitch"], abs=2.0e-4 + ) + for field in ("effort", "velocity"): + assert active.find("limit").get(field) == source_active.find("limit").get(field) + passive = joints[passive_name] + source_passive = original_joints[passive_name] + for element in ("origin", "axis", "limit", "parent", "child"): + assert passive.find(element).attrib == source_passive.find(element).attrib + assert passive.find("mimic").get("joint") == active_name + assert float(passive.find("mimic").get("multiplier")) == pytest.approx( + MULTIPLIERS["rh_pinky_dip"], abs=2.0e-4 + ) + assert correction.origin_offsets_rad[active_name] == pytest.approx( + correction.origin_offsets_rad["rh_pinky_mcp_pitch"] + ) + assert correction.origin_offsets_rad[active_name] == pytest.approx(0.0) + pinky = joints["rh_pinky_mcp_pitch"] + source_pinky = original_joints["rh_pinky_mcp_pitch"] + assert pinky.find("origin").get("rpy") == source_pinky.find("origin").get("rpy") + assert correction.origin_offsets_rad["rh_pinky_mcp_pitch"] == pytest.approx(0.0) + assert result.zero_method_by_joint["rh_pinky_mcp_pitch"] == ( + "mechanical_lower_endpoint" + ) + roll = joints["rh_thumb_cmc_roll"] + assert float(roll.find("limit").get("lower")) == 0.0 + assert float(roll.find("limit").get("upper")) == pytest.approx(1.34, abs=2.0e-4) + assert correction.origin_offsets_rad["rh_thumb_cmc_roll"] == pytest.approx( + THUMB_ZERO_OFFSETS["rh_thumb_cmc_roll"], abs=2.0e-4 + ) + assert correction.origin_offsets_rad["rh_thumb_cmc_pitch"] == pytest.approx( + THUMB_ZERO_OFFSETS["rh_thumb_cmc_pitch"], abs=2.0e-4 + ) + assert roll.find("origin").get("xyz") == "0.0078133 0.030812 0.025678" + assert float( + joints["rh_thumb_dip"].find("mimic").get("multiplier") + ) == pytest.approx(MULTIPLIERS["rh_thumb_dip"]) + assert joints["rh_pinky_dip"].find("mimic") is not None + assert correction.explicit_runtime_joints == frozenset( + {"rh_index_dip", "rh_middle_dip", "rh_ring_dip", "rh_pinky_dip"} + ) + equalities = { + joint.get("joint1"): joint + for joint in root.findall("./mujoco/equality/joint") + } + assert float( + equalities["rh_thumb_dip"].get("polycoef").split()[1] + ) == pytest.approx(MULTIPLIERS["rh_thumb_dip"]) + pinky_polycoef = [ + float(value) + for value in equalities["rh_pinky_dip"].get("polycoef").split() + ] + assert pinky_polycoef == pytest.approx( + [0.0, MULTIPLIERS["rh_pinky_dip"], 0.0, 0.0, 0.0, 0.0], + abs=2.0e-4, + ) + assert len(list((tmp_path / "meshes").iterdir())) == 13 + assert sha256_file(SOURCE) == ( + "298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666" + ) + + +def test_l6_schema_v6_bridge_uses_feedback_and_rh_joint_names() -> None: + result = fit_l6_session(SOURCE, accepted_records_by_joint(_synthetic_records())) + hashes = { + "source_urdf_sha256": "0" * 64, + "camera_extrinsics_sha256": "1" * 64, + "calibration_config_sha256": "2" * 64, + "tag_config_sha256": "3" * 64, + } + payload = build_l6_runtime_payload( + serial_number="L6_TEST", + source_urdf=SOURCE, + result=result, + protected_inputs=hashes, + ) + validate_l6_runtime_payload(payload) + assert set(payload["joints"]) == set(ACTIVE_JOINTS + PASSIVE_JOINTS) + assert payload["joints"]["rh_thumb_dip"]["urdf_mimic_enabled"] is True + assert payload["joints"]["rh_pinky_dip"]["urdf_mimic_enabled"] is True + assert payload["joints"]["rh_pinky_dip"]["coupling_model"] == ( + "quadratic_runtime" + ) + assert payload["joints"]["rh_pinky_dip"]["urdf_mimic_policy"] == ( + "endpoint_linear_fallback" + ) + assert payload["joints"]["rh_pinky_dip"]["mimic_multiplier"] == ( + pytest.approx(MULTIPLIERS["rh_pinky_dip"], abs=2.0e-4) + ) + for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items(): + assert payload["joints"][target]["calibration_status"] == ( + "transferred_static_dynamic" + ) + assert payload["joints"][target]["transferred_from_joint"] == donor + assert payload["joints"][target]["angle_rad"] == payload["joints"][donor]["angle_rad"] + for target, donor in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items(): + assert payload["joints"][target]["calibration_status"] == ( + "transferred_dynamic_cad_static" + ) + assert payload["joints"][target]["transferred_from_joint"] == donor + assert payload["joints"][target]["angle_rad"] == payload["joints"][donor]["angle_rad"] + mapper = CalibratedCommandMapper(payload, expected_side="right") + assert mapper.profile_id == KEY.profile_id + assert mapper.input_domain == "feedback_u8" + mapped = dict(zip(mapper.urdf_joint_names, mapper.map_positions([0] * 6))) + assert mapped["rh_thumb_cmc_roll"] == pytest.approx(TRAVELS["rh_thumb_cmc_roll"], abs=2.0e-4) + # Accept one-release feedback from an older SDK that mislabeled channel 2. + names = list(COMMAND_NAMES) + names[1] = "thumb_cmc_yaw" + assert mapper.map_positions([255] * 6, names) == mapper.map_positions([255] * 6) + assert default_input_topic("right", "feedback_u8", "L6") == ( + "/l6/cb_right_hand_state" + ) + + +def test_l6_online_and_offline_finalization_are_identical(tmp_path: Path) -> None: + records = _synthetic_records() + hashes = { + "source_urdf_sha256": "0" * 64, + "camera_extrinsics_sha256": "1" * 64, + "calibration_config_sha256": "2" * 64, + "tag_config_sha256": "3" * 64, + } + online = tmp_path / "online" + offline = tmp_path / "offline" + first, _, first_urdf = finalize_l6_session( + session_dir=online, + serial_number="L6_TEST", + source_urdf=SOURCE, + protected_inputs=hashes, + records=records, + publish=False, + timestamp="20260901_120001", + ) + second, _, second_urdf = finalize_l6_session( + session_dir=offline, + serial_number="L6_TEST", + source_urdf=SOURCE, + protected_inputs=hashes, + records=records, + publish=False, + timestamp="20260901_120001", + ) + assert first == second + assert first_urdf.path.read_bytes() == second_urdf.path.read_bytes() + + +def test_l6_legacy_relative_only_session_cannot_publish_thumb_zero( + tmp_path: Path, +) -> None: + geometric_fields = { + "relative_translation_xyz_m", + "parent_pose_common", + "child_pose_common", + "view_normal_common_xyz", + "camera_center_common_xyz_m", + "state_u8", + } + records = [ + {key: value for key, value in row.items() if key not in geometric_fields} + for row in _synthetic_records() + ] + with pytest.raises(ValueError, match="session must be reacquired"): + finalize_l6_session( + session_dir=tmp_path / "legacy", + serial_number="L6_LEGACY", + source_urdf=SOURCE, + protected_inputs={ + "source_urdf_sha256": "0" * 64, + "camera_extrinsics_sha256": "1" * 64, + "calibration_config_sha256": "2" * 64, + "tag_config_sha256": "3" * 64, + }, + records=records, + publish=False, + timestamp="20260901_120003", + )