优化传感器对准压头

apps/autocal/tests/test_finger_panel.py

@@ -169,19 +169,14 @@ def test_the_initial_view_is_the_commissioned_orientation(qt_app, bundle):
             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.
+        # The commissioned camera sits in front of the shell and a little below
+        # it, looking back toward platform −Y. That is the ad049d12 preview.
         eye, _distance = preview._camera()  # noqa: SLF001
+        centre = preview._view_target()  # 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}"
-        )
+        assert eye[1] > centre[1]
+        assert eye[2] < centre[2]
+        assert forward[1] < -0.5, f"the view must look toward −Y, but forward is {forward}"
     finally:
         preview.deleteLater()
 
@@ -1253,8 +1248,8 @@ def test_the_tool_position_sets_the_move_and_not_the_reading(panel):
 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
+    assert "x->-x" in text
+    assert "z->y" in text
 
 
 def test_the_readout_names_the_chosen_pose_and_the_key_that_chose_it(panel):
@@ -1271,7 +1266,7 @@ def test_the_readout_names_the_chosen_pose_and_the_key_that_chose_it(panel):
 
     pose = panel._last_pose  # noqa: SLF001
     text = panel._readout_labels["branch"].text()  # noqa: SLF001
-    assert pose.branch_reason in {"only", "alignment", "in_range", "upright", "smallest"}
+    assert pose.branch_reason in {"only", "alignment", "in_range", "upright", "smallest", "crown"}
     assert t(f"finger.branch_{pose.branch_reason}") in text
     # And the alternative's cost is on screen too, not just the chosen one's.
     assert f"{pose.other_travel_deg:.1f}" in text
@@ -1294,7 +1289,9 @@ def test_the_press_readout_says_why_it_is_held_and_then_names_the_target(panel):
     targets, held = panel.solved_pose()
     assert held == ("z",)
     assert "z" not in targets
-    assert targets["x"] == pytest.approx(pose.table_xy_mm[0])
+    from orisys_autocal.sensor.frames import TABLE_X_GAIN
+
+    assert targets["x"] == pytest.approx(-TABLE_X_GAIN * pose.table_xy_mm[0])
 
     # With the bench's zero entered, the same contact height becomes a coordinate.
     panel.set_press(PressAxis(zero_mm=2.0, rising=True, calibrated=True))

apps/autocal/tests/test_ik_analytical.py

@@ -39,12 +39,8 @@ def _random_units(count: int, seed: int) -> list[np.ndarray]:
 
 
 def _apply(stack: PlatformStack, unit: np.ndarray, yaw_deg: float, pitch_deg: float) -> np.ndarray:
-    """Re-apply a returned pose with the library's own rotations."""
-    return (
-        rotation_about(stack.pitch_axis, pitch_deg)
-        @ rotation_about(stack.yaw_axis, yaw_deg)
-        @ unit
-    )
+    """Re-apply a returned pose with the same rotation the drives execute."""
+    return stack.platform_rotation(yaw_deg, pitch_deg) @ unit
 
 
 # ------------------------------------------------------------------ exactness
@@ -108,11 +104,10 @@ def test_the_two_methods_agree_inside_the_envelope():
 
 
 def test_the_two_pitch_bindings_are_the_same_family():
-    """Confirmed at the bench: a 90 degree yaw takes one binding to the other.
+    """``R_y(p)·R_z(t)`` and ``R_x(p)·R_z(t − 90)`` reach the same alignments.
 
-    ``R_y(p)·R_z(t)`` and ``R_x(p)·R_z(t − 90)`` agree to machine precision. So
-    which axis is called pitch is a naming and zero-offset choice, not a
-    correctness question — the two reach exactly the same set of alignments.
+    This is a property of the right-hand matrices. The drive does not get to
+    pick either binding: pitch is about ``x`` with the measured command sign.
     """
     unit = np.array([0.3, 0.5, -0.81])
     unit = unit / np.linalg.norm(unit)

apps/autocal/tests/test_kinematics.py

@@ -59,9 +59,9 @@ def _brute_force_best(
     pitch_limit = min(360.0, stack.pitch_limit_deg)
     best = 1e9
     for yaw in np.arange(-yaw_limit, yaw_limit + step, step):
-        turned = rotation_about(stack.yaw_axis, float(yaw)) @ unit_platform
+        turned = stack.yaw_rotation(float(yaw)) @ unit_platform
         for pitch in np.arange(-pitch_limit, pitch_limit + step, step):
-            rotated = rotation_about(stack.pitch_axis, float(pitch)) @ turned
+            rotated = stack.pitch_rotation(float(pitch)) @ turned
             residual = float(np.degrees(np.arccos(np.clip(rotated @ tool, -1.0, 1.0))))
             best = min(best, residual)
     return best
@@ -77,13 +77,78 @@ def test_the_axis_map_is_a_rotation():
     assert np.allclose(matrix @ matrix.T, np.eye(3))
 
 
+def test_a_crown_point_moves_along_y_and_does_not_yaw():
+    """The top face used to be a Y move. A 1.6° sideways lean must not yaw 90°."""
+    stack = PlatformStack(sensor_offset_mm=(0.0, 0.0, 8.0), pitch_limit_deg=90.0)
+    point = np.array([0.150, 8.355, 5.483])
+    normal = np.array([0.028, 1.0, 0.0])
+    normal = normal / np.linalg.norm(normal)
+    pose = stack.solve(point, normal)
+    assert pose.yaw_deg == pytest.approx(0.0, abs=1e-6)
+    assert pose.pitch_deg == pytest.approx(0.0, abs=1e-6)
+    assert pose.table_xy_mm[1] == pytest.approx(-5.483, abs=0.02)
+    assert abs(pose.table_xy_mm[0]) < 0.5
+    assert pose.branch_reason == "crown"
+    assert pose.reachable
+    assert pose.reason == ""
+
+
+def test_a_sideways_normal_still_yaws():
+    """A lean the pitch stage cannot nod away still takes the exact yaw."""
+    stack = PlatformStack()
+    normal = np.array([0.7, 0.7, 0.1])
+    normal = normal / np.linalg.norm(normal)
+    pose = stack.solve(np.zeros(3), normal)
+    assert pose.residual_deg < 1e-4
+    assert abs(pose.yaw_deg) > 20.0
+    assert pose.branch_reason != "crown"
+
+
+def test_positive_stage_commands_match_the_drives():
+    """Bench 2026-10-15, one axis at a time from the commissioned zero.
+
+    Yaw +10° turned the tip clockwise viewed from above. Positive pitch nods the
+    dome toward the operator (platform −Y). The preview uses this same rotation,
+    so a commanded angle draws the motion the drive will make.
+    """
+    stack = PlatformStack()
+    assert stack.yaw_axis == "z" and stack.pitch_axis == "x"
+    assert np.allclose(stack.platform_rotation(0.0, 0.0), np.eye(3))
+
+    tip = stack.platform_rotation(10.0, 0.0) @ np.array([0.0, 1.0, 0.0])
+    # Clockwise from above: the tip, pointing at +Y, swings toward +X.
+    assert tip[0] > 0.0
+    assert tip[1] > 0.0
+    assert abs(tip[2]) < 1e-12
+
+    dome = stack.platform_rotation(0.0, 10.0) @ np.array([0.0, 0.0, 1.0])
+    # A nod, not a roll: the face leans toward −Y and does not pick up +X.
+    assert dome[1] < 0.0
+    assert abs(dome[0]) < 1e-12
+    assert dome[2] > 0.0
+
+
 def test_the_sensor_y_maps_to_the_platform_z():
     """The bench's frame relation, stated once so it can be corrected once."""
     assert np.allclose(to_platform(np.array([0.0, 1.0, 0.0])), [0.0, 0.0, 1.0])
 
 
-def test_the_sensor_x_maps_to_the_platform_x():
-    assert np.allclose(to_platform(np.array([1.0, 0.0, 0.0])), [1.0, 0.0, 0.0])
+def test_the_sensor_x_maps_to_the_platform_minus_x():
+    """Both table axes were reversed on the bench. −X keeps the map a rotation."""
+    assert np.allclose(to_platform(np.array([1.0, 0.0, 0.0])), [-1.0, 0.0, 0.0])
+
+
+def test_the_sensor_z_maps_to_the_platform_y():
+    """Shell length and machine Y point the same way, so the tip is +Y."""
+    assert np.allclose(to_platform(np.array([0.0, 0.0, 1.0])), [0.0, 1.0, 0.0])
+
+
+def test_a_tip_side_point_commands_negative_table_y():
+    """Bringing a point toward the tip under the tool moves the table to −Y."""
+    pose = PlatformStack().solve(np.array([0.0, 8.0, 6.0]), np.array([0.0, 1.0, 0.0]))
+    assert pose.table_xy_mm[1] == pytest.approx(-6.0)
+    side = PlatformStack().solve(np.array([2.0, 8.0, 0.0]), np.array([0.0, 1.0, 0.0]))
+    assert side.table_xy_mm[0] == pytest.approx(2.0)
 
 
 def test_the_mount_round_trips():
@@ -105,7 +170,7 @@ def test_an_incomplete_map_is_refused():
 
 
 def test_the_mapping_is_described_for_the_ui():
-    assert describe() == "sensor x->x, y->z, z->-y"
+    assert describe() == "sensor x->-x, y->z, z->y"
 
 
 def test_swapping_the_mapping_changes_the_answer():
@@ -155,9 +220,7 @@ def test_the_nominal_normal_needs_no_rotation_and_lands_on_plus_z():
     unit = unit / np.linalg.norm(unit)
     pose = stack.solve(np.array([0.0, 5.0, 0.0]), nominal)
 
-    landed = rotation_about(stack.pitch_axis, pose.pitch_deg) @ rotation_about(
-        stack.yaw_axis, pose.yaw_deg
-    ) @ unit
+    landed = stack.platform_rotation(pose.yaw_deg, pose.pitch_deg) @ unit
     landed = landed / np.linalg.norm(landed)
     assert landed[2] > 0.0, f"the face was aimed at {np.round(landed, 3)}, not +Z"
     # And it gets there without turning: the commissioned pose is the identity.
@@ -241,7 +304,7 @@ def test_the_closed_form_pitch_beats_a_direct_scan():
             tool = stack.tool_direction
             cosine, sine = np.cos(np.radians(scan)), np.sin(np.radians(scan))
             stationary, first, second, sign_first, sign_second = (
-                stack._rotation_coefficients(stack.pitch_axis)  # noqa: SLF001
+                stack._signed_coefficients(stack.pitch_axis)  # noqa: SLF001
             )
             dot = (
                 yawed[stationary] * tool[stationary]
@@ -262,28 +325,11 @@ def _independent_residual(
     """The residual worked out from raw matrices built here, not by the solver.
 
     ``residual_for`` shares ``rotation_about`` with the solver, so it cannot catch
-    an error in that shared helper. This builds the two 3x3 matrices from the
-    rotation formulas directly, so the two paths agree only if both are right.
+    an error in that shared helper. The matrices live in ``_independent_rotation``.
     """
     normal = stack.normal_to_platform(np.asarray(normal_sensor, dtype=np.float64))
     unit = normal / np.linalg.norm(normal)
-
-    yaw, pitch = np.radians(yaw_deg), np.radians(pitch_deg)
-    about_yaw = np.array(
-        [
-            [np.cos(yaw), -np.sin(yaw), 0.0],
-            [np.sin(yaw), np.cos(yaw), 0.0],
-            [0.0, 0.0, 1.0],
-        ]
-    )
-    about_pitch = np.array(
-        [
-            [np.cos(pitch), 0.0, np.sin(pitch)],
-            [0.0, 1.0, 0.0],
-            [-np.sin(pitch), 0.0, np.cos(pitch)],
-        ]
-    )
-    turned = about_pitch @ about_yaw @ unit
+    turned = _independent_rotation(yaw_deg, pitch_deg) @ unit
     return float(
         np.degrees(np.arccos(np.clip(float(turned @ stack.tool_direction), -1.0, 1.0)))
     )
@@ -368,10 +414,10 @@ def test_the_full_yaw_travel_keeps_the_second_solution_of_a_measured_point():
     """±180 is what keeps "cannot reach" from being reported for an aligned pose.
 
     ``docs/motion-correction-plan.zh.md`` measures a point at (-2.698, 5.186, 18.228)
-    with normal (-0.343, 0.699, 0.627). It has two exact solutions — -61.319°/+45.636°
-    and +118.681°/-45.636° — and the second sits outside the ±90 note this bench was
-    once configured with. At ±180 both branches are reachable, so a change in which
-    branch is preferred cannot strand the point on "cannot reach".
+    with normal (-0.343, 0.699, 0.627). Under the drive's rotation it has two exact
+    solutions, -28.681° and +151.319° of yaw, and the second sits outside the ±90
+    note this bench was once configured with. At ±180 both branches are reachable,
+    so a change in which branch is preferred cannot strand the point on "cannot reach".
     """
     point = np.array([-2.698, 5.186, 18.228])
     normal = np.array([-0.343, 0.699, 0.627])
@@ -383,7 +429,7 @@ def test_the_full_yaw_travel_keeps_the_second_solution_of_a_measured_point():
     wide_branches = wide.alignment_branches(unit, wide.tool_direction)
     narrow_branches = narrow.alignment_branches(unit, narrow.tool_direction)
 
-    assert {round(branch.yaw_deg, 3) for branch in wide_branches} == {-61.319, 118.681}
+    assert {round(branch.yaw_deg, 3) for branch in wide_branches} == {-28.681, 151.319}
     assert all(branch.in_range for branch in wide_branches)
     assert sum(branch.in_range for branch in narrow_branches) == 1, (
         "±90 was expected to drop one of the two exact solutions"
@@ -493,6 +539,20 @@ def test_the_default_tool_is_above_the_table_origin():
     assert pose.table_xy_mm[1] == pytest.approx(-pose.target_xy_mm[1])
 
 
+def test_the_drive_x_is_opposite_the_model_slide_and_a_little_shorter():
+    """The X motor is opposite the model, and the slide is 10/11 of it.
+
+    The preview keeps the full model slide, so the drawn contact stays on the tool.
+    """
+    from orisys_autocal.sensor.frames import TABLE_X_GAIN, machine_table_xy
+
+    pose = PlatformStack().solve(np.array([3.0, 8.0, 1.0]), np.array([0.0, 1.0, 0.0]))
+    drive_x, drive_y = machine_table_xy(pose.table_xy_mm)
+    assert drive_x == pytest.approx(-TABLE_X_GAIN * pose.table_xy_mm[0])
+    assert drive_y == pytest.approx(pose.table_xy_mm[1])
+    assert f"{drive_x:+.3f}" in pose.describe()
+
+
 def test_the_table_move_lands_the_contact_on_the_tool():
     """The one invariant the whole machine exists to satisfy."""
     stack = PlatformStack(
@@ -531,12 +591,11 @@ def test_the_mounting_offset_moves_the_point_and_the_mounting_yaw_the_whole_fram
     moved = shifted.solve(point, normal)
     assert moved.target_xy_mm[0] - base.target_xy_mm[0] == pytest.approx(1.5)
     assert moved.target_xy_mm[1] - base.target_xy_mm[1] == pytest.approx(-2.5)
-    # An in-plane offset leaves the *mounting* height alone, but not the height the
-    # stages present: the same angles now lift a contact that sits further from the
-    # trunnion axis. This line used to compare contact_z_mm on both sides, which
-    # only held while that field was reported before the rotation.
+    # An in-plane offset leaves the mounting height alone. The stages then present
+    # a different height, because the same angles tilt a contact that sits somewhere
+    # else relative to the pitch axis.
     assert moved.mounted_point_mm[2] == pytest.approx(base.mounted_point_mm[2])
-    assert moved.contact_z_mm - base.contact_z_mm > 0.1
+    assert abs(moved.contact_z_mm - base.contact_z_mm) > 0.1
 
     turned = dataclasses.replace(stack, sensor_yaw_deg=35.0)
     yawed = turned.solve(point, normal)
@@ -546,9 +605,19 @@ def test_the_mounting_offset_moves_the_point_and_the_mounting_yaw_the_whole_fram
     )
     assert yawed.contact_z_mm == pytest.approx(base.contact_z_mm)
     # The mounting yaw is absorbed by the stage: it has to turn that much less.
-    difference = (base.yaw_deg - yawed.yaw_deg) % 360.0
-    assert min(difference, 360.0 - difference) == pytest.approx(35.0, abs=1e-9)
-    assert yawed.pitch_deg == pytest.approx(base.pitch_deg)
+    # Mounting yaw is right-handed about Z. Positive stage yaw is the opposite
+    # sense, so the same-pitch branch's command rises by the mounting angle.
+    # The shorter branch may be the other one, a half-turn away.
+    unit = turned.setup_normal(normal)
+    unit = unit / float(np.linalg.norm(unit))
+    same_pitch = [
+        branch
+        for branch in turned.alignment_branches(unit, turned.tool_direction)
+        if branch.pitch_deg == pytest.approx(base.pitch_deg)
+    ]
+    assert len(same_pitch) == 1
+    shift = (same_pitch[0].yaw_deg - base.yaw_deg + 180.0) % 360.0 - 180.0
+    assert shift == pytest.approx(35.0, abs=1e-9)
     assert yawed.residual_deg == pytest.approx(base.residual_deg, abs=1e-9)
 
 
@@ -574,12 +643,22 @@ def test_the_setup_z_is_the_mounting_plate():
     )
 
 
+def test_the_bench_mount_points_the_tip_forward():
+    """Yaw 180 put the tip on the near side. Zero yaw points it away."""
+    stack = PlatformStack(sensor_yaw_deg=0.0)
+    tip = stack.setup_point(np.array([0.0, 0.0, 1.0]))
+    cable = stack.setup_point(np.array([0.0, 0.0, -1.0]))
+    assert tip[:2] == pytest.approx((0.0, 1.0))
+    assert cable[:2] == pytest.approx((0.0, -1.0))
+    assert stack.setup_point(np.array([0.0, 1.0, 0.0]))[2] == pytest.approx(1.0)
+
+
 def test_the_setup_normal_is_the_mounting_yaw_applied_to_the_mount():
     stack = PlatformStack(sensor_yaw_deg=90.0)
     normal = np.array([1.0, 0.0, 0.0])
-    # Sensor +x is platform +x; a 90 degree mounting yaw sends it to platform +y.
-    assert np.allclose(stack.setup_normal(normal), [0.0, 1.0, 0.0], atol=1e-12)
-    assert stack.normal_to_platform(normal) == pytest.approx([1.0, 0.0, 0.0])
+    # Sensor +x is platform −x; a +90° mounting yaw about Z sends it to platform −y.
+    assert np.allclose(stack.setup_normal(normal), [0.0, -1.0, 0.0], atol=1e-12)
+    assert stack.normal_to_platform(normal) == pytest.approx([-1.0, 0.0, 0.0])
 
 
 def test_a_non_finite_sensor_yaw_is_refused():
@@ -625,20 +704,23 @@ def test_an_aligned_pose_reports_that_it_reached_tolerance():
 
 
 def _independent_rotation(yaw_deg: float, pitch_deg: float) -> np.ndarray:
-    """``R_pitch @ R_yaw`` from the formulas, not through ``rotation_about``."""
+    """``R_pitch @ R_yaw`` for the drive directions, not through ``rotation_about``.
+
+    Positive yaw is clockwise from above. Positive pitch nods ``+Z`` toward ``−Y``.
+    """
     yaw, pitch = np.radians(yaw_deg), np.radians(pitch_deg)
     about_yaw = np.array(
         [
-            [np.cos(yaw), -np.sin(yaw), 0.0],
-            [np.sin(yaw), np.cos(yaw), 0.0],
+            [np.cos(yaw), np.sin(yaw), 0.0],
+            [-np.sin(yaw), np.cos(yaw), 0.0],
             [0.0, 0.0, 1.0],
         ]
     )
     about_pitch = np.array(
         [
-            [np.cos(pitch), 0.0, np.sin(pitch)],
-            [0.0, 1.0, 0.0],
-            [-np.sin(pitch), 0.0, np.cos(pitch)],
+            [1.0, 0.0, 0.0],
+            [0.0, np.cos(pitch), -np.sin(pitch)],
+            [0.0, np.sin(pitch), np.cos(pitch)],
         ]
     )
     return about_pitch @ about_yaw

apps/autocal/tests/test_sensor_bundle.py

@@ -17,6 +17,7 @@ import numpy as np
 import pytest
 from orisys_autocal.sensor.export_bundle import (
     BundleError,
+    housing_tail_origin_mm,
     load_bundle,
     mm_per_unit_from_pose,
 )
@@ -116,6 +117,29 @@ def test_the_surface_area_is_physically_plausible(bundle):
     assert bundle.surface_area_mm2 == pytest.approx(355.77, abs=1.0)
 
 
+def test_the_origin_is_the_unsensed_housing_tail():
+    """The tail is the housing end the sensing patch does not cover."""
+    housing = np.array(
+        [
+            [-2.0, 0.0, 0.0],
+            [2.0, 0.0, 0.0],
+            [0.0, 4.0, 0.2],
+            [-1.0, 0.1, 20.0],
+            [1.0, 0.1, 20.2],
+        ]
+    )
+    sensing = np.array([[0.0, 2.0, 5.0], [0.0, 2.0, 19.8]])
+    origin = housing_tail_origin_mm(housing, sensing)
+    assert origin[0] == pytest.approx(0.0)
+    assert origin[1] == pytest.approx(0.0)
+    assert origin[2] == pytest.approx(0.0)
+
+
+def test_a_loaded_bundle_puts_the_tail_on_the_origin(bundle):
+    origin = housing_tail_origin_mm(bundle.housing_vertices_mm, bundle.vertices_mm)
+    assert origin == pytest.approx(np.zeros(3), abs=1e-6)
+
+
 def test_the_stored_normals_are_coherent(bundle):
     """Mixed winding would make 'the normal' meaningless."""
     normals = bundle.face_normals