"""Independent pinhole projections exercise the real PnP/capture boundary.""" from dataclasses import replace from types import SimpleNamespace import numpy as np from scipy.spatial.transform import Rotation import pytest from linkerhand_calibration.profiles import load_bundled_hand_profile from linkerhand_calibration.runtime.capture import CaptureFrame, ObservationCapture from linkerhand_calibration.runtime.reference_lock import ReferenceLock from linkerhand_calibration.runtime.motion_execution import MotionCommand def projected_square(size, translation, rpy=(.1, -.15, .03)): # IPPE square order: top-left, top-right, bottom-right, bottom-left. s = size/2 points = np.array([[-s, s, 0], [s, s, 0], [s, -s, 0], [-s, -s, 0]]) xyz = points @ Rotation.from_euler("xyz", rpy).as_matrix().T + translation return (xyz[:, :2]/xyz[:, 2:])*1200 + [640, 480] def make_capture(): profile = load_bundled_hand_profile("l6_right_8") front = profile.vision.views[0] tags = tuple(replace(tag, size_m=.024 if tag.role == "thumb_pitch" else .016) for tag in front.tags) profile = replace(profile, vision=replace(profile.vision, views=(replace(front, tags=tags),)+profile.vision.views[1:])) lock = ReferenceLock({v.name: next(t.tag_id for t in v.tags if t.fixed_reference) for v in profile.vision.views}) capture = ObservationCapture(profile, reference_lock=lock, extrinsics=SimpleNamespace(transform=lambda _: np.eye(4))) return profile, lock, capture def frame(profile, stamp, *, omit=(), shift=0, feedback=True): roles = {"front_base": (.0, .0, .45), "thumb_pitch": (.05, .03, .45), "thumb_dip": (.08, .05, .45)} sizes = {tag.role: tag.size_m for tag in profile.vision.views[0].tags} corners = {role: projected_square(sizes[role], xyz)+[shift, 0] for role, xyz in roles.items() if role not in omit} values = tuple(map(float, profile.command.baseline_values)) return CaptureFrame("front", stamp, np.array([[1200, 0, 640], [0, 1200, 480], [0, 0, 1]]), corners, values if feedback else None, values, command_directions=("increasing",)*len(values)) def test_only_ten_unique_post_start_valid_frames_lock_and_mounts_keep_per_tag_size(): profile, lock, capture = make_capture() for index in range(20): capture.consume(frame(profile, index+1)) assert not lock.references # Preview is not a reference. lock.begin_session() capture.reset() lock.start_locking() for _ in range(20): capture.consume(frame(profile, 100), collect_installations=True) assert not lock.references for index in range(9): capture.consume(frame(profile, 101+index), collect_installations=True) fixed, moving = capture.fingerprint_poses() assert "front" in fixed np.testing.assert_allclose(fixed["front"]["translation_xyz_m"], [0, 0, .45], atol=1e-7) assert "front:thumb_pitch" in moving assert len(capture.installations["front:thumb_pitch"]) == 10 def test_short_visual_loss_discards_frames_cached_base_is_not_fake_image_data(): profile, lock, capture = make_capture() lock.begin_session() lock.start_locking() for i in range(10): capture.consume(frame(profile, i+1), collect_installations=True) motion = MotionCommand("sweep", tuple(profile.command.baseline_values), 1, "thumb_pitch_dip_front", 0, 0, "decreasing") rows, moved = capture.consume(frame(profile, 20, omit=("front_base",)), motion) assert moved is None and len(rows) == 2 assert rows[0]["locked_reference_roles"] == ["front_base"] assert "image_relative_xy_px" not in rows[0] assert capture.consume(frame(profile, 21, feedback=False), motion)[0] == () assert capture.consume(frame(profile, 22, omit=("thumb_pitch",)), motion)[0] == () assert len(capture.consume(frame(profile, 23), motion)[0]) == 2 assert len(capture.installations["front:thumb_pitch"]) == 10 # Frozen during motion. @pytest.mark.parametrize("drift", [6, 200]) def test_fixed_reference_displacement_requires_ten_confirming_images(drift): profile, lock, capture = make_capture() lock.begin_session() lock.start_locking() for i in range(10): capture.consume(frame(profile, i+1)) for i in range(9): assert capture.consume(frame(profile, 20+i, shift=drift))[1] is None assert capture.consume(frame(profile, 29, shift=drift))[1] is not None def test_joint_zero_only_requires_its_own_parent_child_tags(): from linkerhand_calibration.core.domain.reference import build_joint_zero_reference profile, lock, capture = make_capture() lock.begin_session() lock.start_locking() for i in range(10): capture.consume(frame(profile, i+1)) name = "rh_thumb_cmc_pitch" spec = profile.motion.joint_zero_references[name] motion = MotionCommand("joint_zero", spec.command, 1, task_key=spec.task_key, command_index=0, cycle=0, zero_joints=(name,)) rows = [] for i in range(10): samples, movement = capture.consume(frame(profile, 20+i, omit=("thumb_dip",)), motion) assert movement is None rows.extend(samples) assert len(rows) == 10 and all(row["kind"] == "joint_zero_sample" for row in rows) reference = build_joint_zero_reference(profile, name, rows, session_epoch=1, motion_version=2) assert reference.joint == name and len(reference.samples) == 10 def test_articulated_capture_discards_partial_moving_tags_and_records_cached_base(): profile, lock, capture = make_capture() profile = replace(profile, measurement=replace(profile.measurement, candidate_selection_tasks=frozenset({"thumb_pitch_dip_front"}))) capture.profile = profile lock.begin_session() lock.start_locking() for i in range(10): capture.consume(frame(profile, i+1)) motion = MotionCommand("sweep", profile.command.baseline_values, 1, "thumb_pitch_dip_front", 0, 0, "decreasing") assert capture.consume(frame(profile, 20, omit=("thumb_dip",)), motion)[0] == () samples, _ = capture.consume(frame(profile, 21, omit=("front_base",)), motion) candidate = next(r for r in samples if r["kind"] == "pnp_candidate_frame") assert candidate["roles"]["front_base"]["locked_reference"] is True assert candidate["roles"]["front_base"]["corners_xy"] is None assert candidate["roles"]["front_base"]["selected"]