added motion simulator feature

apps/autocal/tests/test_finger_panel.py

@@ -0,0 +1,629 @@
+"""The finger debugger's panel: picking, refusing, and what a row contains.
+
+The bundle is the real export on this machine. It is skipped rather than failed
+when absent, because the export lives in a sibling design repo: a machine without
+it has not broken AutoCal.
+"""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+import numpy as np
+import pytest
+from PySide6.QtCore import QPointF
+
+from orisys_autocal.motion.kinematics import PlatformStack
+from orisys_autocal.sensor import (
+    DEFAULT_FLAT_SIZE,
+    load_bundle,
+    rasterise,
+    to_platform,
+    to_sensor,
+)
+from orisys_autocal.ui.sensor_panel import FingerPanel
+
+BUNDLE = Path(
+    r"D:\orisys_ws\orisys-sensor-design\build\export\finger\r01\compute"
+)
+
+pytestmark = pytest.mark.skipif(
+    not BUNDLE.is_dir(), reason=f"no sensor export at {BUNDLE}"
+)
+
+
+@pytest.fixture(scope="module")
+def bundle():
+    return load_bundle(BUNDLE)
+
+
+@pytest.fixture(scope="module")
+def flat(bundle):
+    return rasterise(bundle, size_hw=DEFAULT_FLAT_SIZE)
+
+
+@pytest.fixture
+def panel(qt_app, bundle, flat):
+    widget = FingerPanel(stack=PlatformStack())
+    widget.set_bundle(bundle, flat)
+    yield widget
+    widget.deleteLater()
+
+
+def _first_surface_pixel(flat) -> tuple[int, int]:
+    """A pixel that carries surface, found rather than assumed."""
+    for y in range(0, flat.normals.shape[0], 7):
+        for x in range(0, flat.normals.shape[1], 7):
+            if flat.sample(x, y) is not None:
+                return x, y
+    raise AssertionError("the flat raster has no surface pixels at all")
+
+
+def _shrink_raster(monkeypatch, window_mod, size=(256, 256)):
+    """Force a small flat raster for a test that is not about the raster.
+
+    ``DEFAULT_FLAT_SIZE`` is bound as a default argument, so patching the module
+    attribute would not reach it; wrapping the function does.
+    """
+    real = window_mod.rasterise
+
+    def small(bundle, flat_size=None, **kwargs):
+        return real(bundle, size, **kwargs)
+
+    monkeypatch.setattr(window_mod, "rasterise", small)
+
+
+def test_the_window_loads_the_remembered_bundle_at_startup(qt_app, monkeypatch):
+    """A filled-in path over two empty views reads as a broken screen."""
+    import shutil
+
+    from orisys_app_shared import paths as paths_mod
+    from orisys_app_shared import prefs as prefs_pkg
+    from orisys_app_shared.i18n import set_locale
+
+    from orisys_autocal import preferences as prefs_mod
+    from orisys_autocal.ui import window as window_mod
+    from orisys_autocal.ui.window import AutoCalMainWindow
+
+    scratch = Path(__file__).resolve().parents[1] / "outputs" / "_test_scratch" / "autoload"
+    scratch.mkdir(parents=True, exist_ok=True)
+    prefs_file = scratch / "preference.json"
+    monkeypatch.setattr(paths_mod, "preference_path", lambda: prefs_file)
+    monkeypatch.setattr(prefs_pkg.store, "preference_path", lambda: prefs_file)
+    monkeypatch.setattr(prefs_mod, "preference_path", lambda: prefs_file)
+    _shrink_raster(monkeypatch, window_mod)
+
+    prefs = prefs_mod.default_preferences()
+    prefs["finger_bundle_path"] = str(BUNDLE)
+    set_locale(str(prefs.get("language") or "zh_CN"))
+
+    window = AutoCalMainWindow(prefs=prefs)
+    try:
+        assert window.finger_panel.view.flat is not None, "the startup load did not happen"
+        pixmap = window.finger_panel.view.pixmap()
+        assert pixmap is not None and not pixmap.isNull()
+    finally:
+        window.poller.shutdown()
+        window.close()
+        shutil.rmtree(scratch, ignore_errors=True)
+
+
+def test_a_missing_remembered_bundle_is_survivable(qt_app, monkeypatch, tmp_dir):
+    """The normal case on a machine that has never exported one."""
+    from orisys_autocal.ui import window as window_mod
+
+    _shrink_raster(monkeypatch, window_mod)
+    window = window_mod.AutoCalMainWindow(
+        prefs={
+            "language": "en_US",
+            "theme": "light",
+            "finger_bundle_path": str(tmp_dir / "not-exported"),
+        }
+    )
+    try:
+        assert window.finger_panel.view.flat is None
+        # And the panel still says what to do rather than looking dead.
+        assert window.finger_panel.status_label.text()
+    finally:
+        window.poller.shutdown()
+        window.close()
+
+
+def test_the_preview_legend_is_a_label_not_gl_text(qt_app):
+    """Painting a long legend over the GL surface is what made it unreadable."""
+    widget = FingerPanel(stack=PlatformStack())
+    try:
+        assert widget.legend_label.text()
+        assert widget.legend_label.wordWrap()
+    finally:
+        widget.deleteLater()
+
+
+# ------------------------------------------------- the initial view and axes
+
+
+def test_the_initial_view_is_the_commissioned_orientation(qt_app, bundle):
+    """Sensor +Y up the screen and sensor +Z toward the viewer, by construction.
+
+    The user asked for this state to be the configuration the screen opens in, so
+    it is asserted in screen terms rather than against the raw angles: what matters
+    is where the axes land in the picture. Widget y grows *downward*, so "up the
+    screen" is a **negative** pixel offset — the sign that this test exists to get
+    right, because getting it wrong draws the tool axis pointing down.
+    """
+    from orisys_autocal.sensor import to_sensor
+
+    from orisys_autocal.ui.pose_preview import PosePreview
+
+    preview = PosePreview()
+    try:
+        preview.resize(600, 400)
+        preview.set_bundle(bundle)
+        preview.initial_view()
+
+        stack = PlatformStack()
+        origin = preview.project(bundle.centre_mm)
+        assert origin is not None
+
+        # Platform +Z is sensor +Y on this mount: it must read as up the screen.
+        platform_z = to_sensor(np.array([0.0, 0.0, 1.0]), stack.axis_map)
+        tip = preview.project(bundle.centre_mm + platform_z * 11.8)
+        assert tip is not None
+        assert tip[1] < origin[1] - 5.0, (
+            f"sensor +Y must point up the screen, but moved {tip[1] - origin[1]:+.1f} px"
+        )
+
+        # Sensor +Z is the sensing face's own axis and must face the viewer. It is
+        # the platform's -Y on this mount. ``forward`` points from the eye into the
+        # scene, so a surface normal aimed *at* the camera has a positive dot with
+        # it — the opposite of what the depth term in a projection does, and the
+        # sign this assertion got wrong first time.
+        eye, _distance = preview._camera()  # noqa: SLF001
+        _centre, forward, _right, _up = preview._view_basis(eye)  # noqa: SLF001
+        lens_axis = to_sensor(np.array([0.0, -1.0, 0.0]), stack.axis_map)
+        lens_axis = lens_axis / np.linalg.norm(lens_axis)
+        assert float(lens_axis @ forward) > 0.2, (
+            f"sensor +Z must face the viewer, but points away: "
+            f"{float(lens_axis @ forward):+.3f}"
+        )
+    finally:
+        preview.deleteLater()
+
+
+def test_orbiting_and_then_resetting_returns_to_the_initial_view(qt_app, bundle):
+    from orisys_autocal.ui.pose_preview import (
+        INITIAL_PITCH_RAD,
+        INITIAL_YAW_RAD,
+        PosePreview,
+    )
+
+    preview = PosePreview()
+    try:
+        preview.set_bundle(bundle)
+        preview._yaw = 1.9  # noqa: SLF001 - pretend the operator dragged
+        preview._pitch = -0.4  # noqa: SLF001
+        preview._distance_scale = 3.0  # noqa: SLF001
+
+        preview.reset_view()
+
+        assert preview._yaw == pytest.approx(INITIAL_YAW_RAD)  # noqa: SLF001
+        assert preview._pitch == pytest.approx(INITIAL_PITCH_RAD)  # noqa: SLF001
+        assert preview._distance_scale == pytest.approx(1.0)  # noqa: SLF001
+    finally:
+        preview.deleteLater()
+
+
+def test_the_motion_axes_can_be_turned_off(qt_app, bundle):
+    from orisys_autocal.ui.pose_preview import PosePreview
+
+    preview = PosePreview()
+    try:
+        assert preview.show_axes
+        preview.set_show_axes(False)
+        assert not preview.show_axes
+        preview.set_show_axes(True)
+        assert preview.show_axes
+    finally:
+        preview.deleteLater()
+
+
+def test_the_axis_labels_project_onto_their_arrows(qt_app, bundle):
+    """The 2D label and the 3D arrow must agree, or the label names nothing."""
+    from orisys_autocal.sensor import to_sensor
+
+    from orisys_autocal.ui.pose_preview import PosePreview
+
+    preview = PosePreview()
+    try:
+        preview.resize(600, 400)
+        preview.set_bundle(bundle)
+        stack = PlatformStack()
+        origin = bundle.centre_mm
+        extent = float(np.linalg.norm(bundle.extent_mm)) or 1.0
+
+        positions = []
+        for axis in ("x", "y", "z"):
+            direction = np.zeros(3)
+            direction[("x", "y", "z").index(axis)] = 1.0
+            tip = origin + to_sensor(direction, stack.axis_map) * extent * 0.45
+            screen = preview.project(tip)
+            assert screen is not None, f"the {axis} axis tip is behind the camera"
+            assert np.isfinite(screen).all()
+            positions.append(screen)
+
+        # Three axes pointing different ways must not project onto one another.
+        rounded = {(round(x, 3), round(y, 3)) for x, y in positions}
+        assert len(rounded) == 3
+
+        # The arrow tips can fall outside a small widget at the default zoom, so
+        # the overlay clamps the label back inside; that is what keeps it visible.
+        clamped = [
+            (
+                min(max(x, 12.0), preview.width() - 12.0),
+                min(max(y, 14.0), preview.height() - 6.0),
+            )
+            for x, y in positions
+        ]
+        for x, y in clamped:
+            assert 0 <= x <= preview.width()
+            assert 0 <= y <= preview.height()
+    finally:
+        preview.deleteLater()
+
+
+def test_a_point_behind_the_camera_has_no_screen_position(qt_app, bundle):
+    """Drawing a label for it would put a stray letter on the widget."""
+    from orisys_autocal.ui.pose_preview import PosePreview
+
+    preview = PosePreview()
+    try:
+        preview.set_bundle(bundle)
+        assert preview.project(bundle.centre_mm) is not None
+    finally:
+        preview.deleteLater()
+
+
+def test_the_flat_image_is_shown_whole_not_cropped(panel):
+    """At native size the 1024 image was cropped to a corner of the UV space."""
+    view = panel.view
+    view.resize(240, 300)
+    view.set_flat(view.flat)
+
+    shown = view.pixmap()
+    assert shown is not None and not shown.isNull()
+    assert shown.width() <= view.width()
+    assert shown.height() <= view.height()
+    # Square image, square display: the aspect ratio is preserved, not squashed.
+    assert shown.width() == pytest.approx(shown.height(), abs=2)
+
+
+def test_a_click_on_the_scaled_image_maps_to_the_pixel_under_it(panel):
+    """Scaling must not move the mapping; a click in the corner is the corner."""
+    view = panel.view
+    view.resize(240, 300)
+    view.set_flat(view.flat)
+    shown = view.pixmap()
+    offset_x = (view.width() - shown.width()) / 2.0
+    offset_y = (view.height() - shown.height()) / 2.0
+
+    centre = view.pixel_at(QPointF(offset_x + shown.width() / 2.0, offset_y + shown.height() / 2.0))
+    assert centre is not None
+    assert centre[0] == pytest.approx(view.flat.width // 2, abs=3)
+    assert centre[1] == pytest.approx(view.flat.height // 2, abs=3)
+
+    corner = view.pixel_at(
+        QPointF(offset_x + shown.width() - 1, offset_y + shown.height() - 1)
+    )
+    assert corner is not None
+    # One display pixel covers several image pixels at this scale (240 vs 1024),
+    # so the last clickable pixel is within a scale-step of the true corner.
+    step = view.flat.width / shown.width() + 1
+    assert corner[0] > view.flat.width - step
+    assert corner[1] > view.flat.height - step
+
+    assert view.pixel_at(QPointF(offset_x - 5.0, offset_y + shown.height() / 2)) is None
+
+
+# ------------------------------------------------------- the motion simulation
+
+
+def test_the_play_button_needs_a_picked_point(panel):
+    """A button that silently does nothing is worse than a disabled one."""
+    assert not panel.preview.can_simulate
+    assert not panel.sim_button.isEnabled()
+
+    flat = panel.view.flat
+    row = panel.handle_pick(*_first_surface_pixel(flat))
+
+    assert row is not None
+    assert panel.preview.can_simulate
+    assert panel.sim_button.isEnabled()
+
+
+def test_clearing_the_points_disables_the_simulation_again(panel):
+    panel.handle_pick(*_first_surface_pixel(panel.view.flat))
+    panel.clear_points()
+    assert not panel.preview.can_simulate
+    assert not panel.sim_button.isEnabled()
+
+
+def _run_simulation(preview) -> None:
+    """Drive the timer by hand, so the test does not sleep."""
+    from orisys_autocal.ui.pose_preview import SIM_DURATION_MS, SIM_FRAME_MS
+
+    preview.stop_simulation()
+    preview.start_simulation()
+    for _ in range(int(SIM_DURATION_MS / SIM_FRAME_MS) + 2):
+        preview._sim_tick()  # noqa: SLF001 - stepping the animation deterministically
+
+
+def test_playing_the_motion_puts_the_normal_on_the_tool_axis(qt_app, bundle):
+    """The two invariants the whole animation exists to show.
+
+    After playback the picked normal must lie along the tool axis, and the contact
+    must sit at the platform coordinates the readouts report. Asserting the second
+    against ``target_xy_mm`` rather than the origin is the point: the tables move to
+    the solved X/Y, and every earlier attempt at this transform got a frame wrong,
+    so it is checked against the solver's own output instead of a bare (0, 0, 0).
+    """
+    from orisys_autocal.ui.pose_preview import PosePreview
+
+    stack = PlatformStack()
+    tool_local = to_sensor(np.asarray(stack.tool_direction, dtype=float), stack.axis_map)
+    tool_local = tool_local / np.linalg.norm(tool_local)
+
+    preview = PosePreview()
+    try:
+        preview.set_bundle(bundle)
+        normals = bundle.vertex_normals()
+        worst_angle = 0.0
+        for index in (0, 1, 7, 40, 200):
+            normal = normals[index]
+            point = bundle.vertices_mm[index]
+            pose = stack.solve(point, normal)
+
+            preview.set_point(point, normal)
+            preview.set_pose(pose)
+            _run_simulation(preview)
+            rotation, translation = preview._sim_transform()  # noqa: SLF001
+
+            aligned = rotation @ (normal / np.linalg.norm(normal))
+            aligned = aligned / np.linalg.norm(aligned)
+            worst_angle = max(
+                worst_angle,
+                float(np.degrees(np.arccos(np.clip(float(aligned @ tool_local), -1.0, 1.0)))),
+            )
+
+            # The contact ends at the tool, which is drawn at the patch's centre.
+            # Asserting the *absolute* platform X/Y here would be asserting the old
+            # behaviour: moving the model to its machine coordinates instead of to
+            # the tool put it off screen, because the camera looks at the tool.
+            landed = rotation @ point + translation
+            assert np.allclose(landed, bundle.centre_mm, atol=1e-6)
+            # And the marker does not take that translation: it is the measurement,
+            # so the body slides out from under it.
+            assert np.allclose(
+                preview._placed_contact(point), rotation @ point  # noqa: SLF001
+            )
+
+        assert worst_angle < 0.5, (
+            f"after playing, a normal is still {worst_angle:.3f} deg off the tool axis"
+        )
+    finally:
+        preview.stop_simulation()
+        preview.deleteLater()
+
+
+def test_the_simulation_rotates_first_and_then_translates(qt_app, bundle):
+    """The bench contract: stages settle before the table moves."""
+    from orisys_autocal.ui.pose_preview import PosePreview
+
+    stack = PlatformStack()
+    surface_normal = bundle.vertex_normals()[0]
+    point = bundle.vertices_mm[0]
+    pose = stack.solve(point, surface_normal)
+
+    preview = PosePreview()
+    try:
+        preview.set_bundle(bundle)
+        preview.set_point(point, surface_normal)
+        preview.set_pose(pose)
+
+        # Mid-rotation: the body has turned and the table has not moved yet.
+        preview.start_simulation()
+        for _ in range(4):
+            preview._sim_tick()  # noqa: SLF001
+        rotation_share, translation_share = preview.simulation_phases()  # noqa: SLF001
+        assert rotation_share > 0.0
+        assert translation_share == 0.0
+        rotation, translation = preview._sim_transform()  # noqa: SLF001
+        assert float(np.linalg.norm(translation)) == pytest.approx(0.0)
+        assert not np.allclose(rotation, np.eye(3))
+
+        # Finish: both phases complete.
+        _run_simulation(preview)
+        rotation_share, translation_share = preview.simulation_phases()  # noqa: SLF001
+        assert rotation_share == pytest.approx(1.0)
+        assert translation_share == pytest.approx(1.0)
+    finally:
+        preview.stop_simulation()
+        preview.deleteLater()
+
+
+def test_the_simulation_stops_and_can_be_replayed(qt_app, bundle):
+    from orisys_autocal.ui.pose_preview import PosePreview
+
+    stack = PlatformStack()
+    point = bundle.vertices_mm[0]
+    pose = stack.solve(point, bundle.vertex_normals()[0])
+    preview = PosePreview()
+    try:
+        preview.set_bundle(bundle)
+        preview.set_point(point, bundle.vertex_normals()[0])
+        preview.set_pose(pose)
+
+        preview.start_simulation()
+        assert preview.simulating
+        preview.stop_simulation()
+        assert not preview.simulating
+        # Stopped means back at the start, not frozen mid-move.
+        rotation_share, translation_share = preview.simulation_phases()  # noqa: SLF001
+        assert rotation_share == 0.0 and translation_share == 0.0
+
+        _run_simulation(preview)
+        assert not preview.simulating
+        preview.start_simulation()
+        assert preview.simulating
+    finally:
+        preview.stop_simulation()
+        preview.deleteLater()
+
+
+def test_the_simulation_cannot_start_without_a_pose(qt_app, bundle):
+    from orisys_autocal.ui.pose_preview import PosePreview
+
+    preview = PosePreview()
+    try:
+        preview.set_bundle(bundle)
+        assert not preview.can_simulate
+        preview.start_simulation()
+        assert not preview.simulating
+        assert preview._sim_transform() is None  # noqa: SLF001
+    finally:
+        preview.deleteLater()
+
+
+def test_a_pick_returns_the_row_it_also_shows(panel):
+    flat = panel.view.flat
+    x, y = _first_surface_pixel(flat)
+
+    row = panel.handle_pick(x, y)
+
+    assert row is not None
+    assert panel.point_count() == 1
+    # What the table shows is what the caller was handed; a panel that rendered
+    # one thing and returned another would make an export silently wrong.
+    stored = panel.points()[0]
+    for key, value in row.items():
+        assert stored[key] == pytest.approx(value)
+
+
+def test_a_pick_carries_a_pose_that_aligns(panel):
+    flat = panel.view.flat
+    x, y = _first_surface_pixel(flat)
+
+    row = panel.handle_pick(x, y)
+
+    assert row["residual"] < 1.0
+    # The platform X/Y must be the contact point the solver reported, so a
+    # plausible-looking origin here would mean the frames were dropped.
+    point, _normal, _face = flat.sample(x, y)
+    assert not np.allclose([row["x"], row["y"], row["z"]], 0.0)
+    assert abs(row["x"] - point[0]) < 1e-9
+
+
+def test_a_gap_pixel_is_refused_with_a_reason(panel):
+    """A UV-island gap must not become a calibration point at the origin."""
+    flat = panel.view.flat
+    gap = None
+    for y in range(flat.normals.shape[0]):
+        for x in range(flat.normals.shape[1]):
+            if flat.sample(x, y) is None:
+                gap = (x, y)
+                break
+        if gap is not None:
+            break
+    assert gap is not None, "the raster has no gap pixels to test the refusal with"
+
+    assert panel.handle_pick(*gap) is None
+    assert panel.point_count() == 0
+    assert panel.status_label.text()
+
+
+def test_picking_with_no_bundle_says_so(qt_app):
+    widget = FingerPanel(stack=PlatformStack())
+    try:
+        assert widget.handle_pick(10, 10) is None
+        assert widget.point_count() == 0
+        assert widget.status_label.text()
+    finally:
+        widget.deleteLater()
+
+
+def test_clearing_points_empties_the_table(panel):
+    flat = panel.view.flat
+    x, y = _first_surface_pixel(flat)
+    panel.handle_pick(x, y)
+    assert panel.point_count() == 1
+
+    panel.clear_points()
+
+    assert panel.point_count() == 0
+    assert panel.points() == []
+
+
+def test_the_plate_thickness_reaches_the_solve(panel):
+    """The mounting-plate entry has to reach the solve, or it is decoration.
+
+    The plate translates the sensor along the platform's tool axis, so it moves
+    the contact point's Z and leaves its X/Y alone. Asserting the X/Y moved would
+    be asserting the wrong model; asserting Z moved catches a plate field that is
+    read but dropped — which is what it did before ``contact_z_mm`` existed.
+    """
+    flat = panel.view.flat
+    x, y = _first_surface_pixel(flat)
+    point, normal, _face = flat.sample(x, y)
+
+    thin = panel.stack.solve(point, normal, plate_thickness_mm=0.0)
+    thick = panel.stack.solve(point, normal, plate_thickness_mm=4.0)
+
+    assert thick.contact_z_mm - thin.contact_z_mm == pytest.approx(4.0)
+    assert thick.target_xy_mm == pytest.approx(thin.target_xy_mm)
+    # Picking through the panel uses the spin box, so the two must agree.
+    panel.clear_points()
+    panel.set_plate_thickness(0.0)
+    thin_row = panel.handle_pick(x, y)
+    panel.clear_points()
+    panel.set_plate_thickness(4.0)
+    thick_row = panel.handle_pick(x, y)
+    assert thin_row is not None and thick_row is not None
+    assert thick_row["residual"] == pytest.approx(thin_row["residual"], abs=1e-6)
+    assert thick_row["Z"] - thin_row["Z"] == pytest.approx(4.0)
+    assert thick_row["X"] == pytest.approx(thin_row["X"])
+    assert thick_row["Y"] == pytest.approx(thin_row["Y"])
+
+
+def test_the_readouts_name_the_frame_mapping(panel):
+    """The one line the bench will want to check when x and y are interchanged."""
+    text = panel._readout_labels["frames"].text()  # noqa: SLF001
+    assert "x" in text and "z" in text
+    assert "-y" in text or "−y" in text
+
+
+def test_the_preview_refuses_to_draw_without_a_context(qt_app):
+    from orisys_autocal.ui.pose_preview import PosePreview
+
+    preview = PosePreview()
+    try:
+        assert not preview.has_context()
+        # Rendering must be a no-op, not a crash: Qt calls this during teardown
+        # on some platforms.
+        preview.repaint()
+        # And the widget must be able to say why it has no picture, rather than
+        # leaving a black rectangle unexplained.
+        preview.set_bundle(None)
+        assert preview.problem
+    finally:
+        preview.deleteLater()
+
+
+def test_the_panel_shows_why_the_preview_is_blank(qt_app):
+    """A blank preview with no reason reads as a broken screen."""
+    widget = FingerPanel(stack=PlatformStack())
+    try:
+        widget.preview.set_bundle(None)
+        widget._refresh_reach()  # noqa: SLF001
+        if not widget.preview.has_context():
+            assert widget.reach_label.text()
+    finally:
+        widget.deleteLater()