added motion trajectory simulation

apps/autocal/tests/test_kinematics.py

@@ -1,19 +1,20 @@
 """The simulated 5-axis solve: residuals, the stage envelope, and the frames.
 
-The solver is the part of this feature that is easiest to get wrong and hardest
-to notice: a wrong derivation still returns two plausible angles. So the central
-test here does not check a formula — it brute-forces the stage envelope and
-requires the solver to be no worse than the best pose that exists.
+The shipped solver is **closed form** (see ``test_ik_analytical.py``, which checks
+it against both the sweep and an exhaustive brute force). What this module covers
+is everything around it: the mount mapping, the envelope, the frames, the reported
+contact point, and the selectable sweep path.
 
-That check is what caught the four wrong versions before it: rotating the
-sensor-frame normal instead of the platform-frame one, composing the stages in
-the wrong order, a seeded search that skipped the right basin, and a sweep that
-ranked yaws by the wrong quantity and so reported a quarter of the patch as
-unreachable when all of it aligns.
+The solver is the part of this feature that is easiest to get wrong and hardest
+to notice — a wrong derivation still returns two plausible angles — so its results
+are still checked against a brute-force search, just not with a fine grid over
+seven normals when the closed form now guarantees exactness on its own.
 """
 
 from __future__ import annotations
 
+import dataclasses
+
 import numpy as np
 import pytest
 
@@ -47,7 +48,13 @@ NORMALS = (
 def _brute_force_best(
     stack: PlatformStack, unit_platform: np.ndarray, *, step: float = 0.25
 ) -> float:
-    """The lowest residual any pose in the envelope achieves — by search."""
+    """The lowest residual any pose in the envelope achieves — by search.
+
+    Kept, but no longer run over the whole grid for every normal: the shipped
+    solver is closed form and ``test_ik_analytical.py`` checks it against an
+    exhaustive sweep. This survives as the reference for the *sweep* path, which
+    is still selectable and still needs a floor to be measured against.
+    """
     tool = stack.tool_direction
     yaw_limit = min(180.0, stack.yaw_limit_deg)
     pitch_limit = min(360.0, stack.pitch_limit_deg)
@@ -122,20 +129,61 @@ def test_swapping_the_mapping_changes_the_answer():
 
 def test_the_bench_envelope_is_the_default():
     stack = PlatformStack()
-    assert stack.yaw_limit_deg == 90.0
+    # Confirmed at the bench and against the trunnion drawing: the vertical stage
+    # turns a full half-turn either way, the horizontal one turns continuously.
+    assert stack.yaw_limit_deg == 180.0
     assert stack.pitch_limit_deg == 360.0
 
 
-@pytest.mark.parametrize("normal", NORMALS)
-def test_the_solver_is_never_worse_than_a_brute_force_search(normal):
-    """The test that pins the derivation down."""
+def test_the_nominal_normal_needs_no_rotation_and_lands_on_plus_z():
+    """The polarity check, and the anchor for the whole solve.
+
+    The sensor sits at the bottom of its shell with the sensing face up, so the
+    face points along ``+tool_axis`` and the nominal pose is *no rotation at all*.
+
+    This is the test that was missing. ``approach_sign`` was ``-1``, aiming every
+    normal at ``-Z``; the solver dutifully used pitch 180 and turned the sensor
+    over, and nothing noticed because the residual is sign-blind — a face turned
+    fully over still scored 0.0000 degrees. Assert the direction landed in, not
+    just the angle between the lines.
+    """
     stack = PlatformStack()
+    assert stack.approach_sign == 1
+    assert np.allclose(stack.tool_direction, [0.0, 0.0, 1.0])
+
+    nominal = np.array([0.0, 1.0, 0.0])  # sensor +Y, the direction the face looks
+    unit = stack.normal_to_platform(nominal)
+    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 = 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.
+    assert abs(pose.pitch_deg) < 1e-6
+    assert abs(pose.yaw_deg) < 1e-6
+    assert pose.upright
+
+
+def test_the_sweep_is_never_worse_than_a_brute_force_search():
+    """The sweep path still needs a floor, but one normal is enough to pin it.
+
+    The shipped solver is closed form and ``test_ik_analytical.py`` checks it
+    against an exhaustive sweep over many normals. This keeps the *search* path
+    honest with one coarse brute force, rather than seven fine ones — the fine
+    version cost about three seconds per normal to confirm something the closed
+    form now guarantees outright.
+    """
+    stack = PlatformStack()
+    normal = NORMALS[4]
     unit = stack.normal_to_platform(normal)
     unit = unit / np.linalg.norm(unit)
-    pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
-    best = _brute_force_best(stack, unit, step=0.5)
-    assert pose.residual_deg <= best + 0.05, (
-        f"the solver returned {pose.residual_deg:.3f} deg where "
+    pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal, method="search")
+    best = _brute_force_best(stack, unit, step=2.0)
+    assert pose.residual_deg <= best + 0.1, (
+        f"the sweep returned {pose.residual_deg:.3f} deg where "
         f"{best:.3f} deg is achievable"
     )
 
@@ -361,13 +409,134 @@ def test_the_plate_lifts_the_point_along_the_tool_axis():
     assert np.allclose(lifted - plain, [0.0, 0.0, 2.5])
 
 
-def test_the_tool_offset_shifts_the_reported_point():
-    stack = PlatformStack(tool_offset_mm=(1.0, 2.0, 3.0))
-    point = np.array([0.0, 5.0, 0.0])
+def test_the_tool_position_is_the_same_number_the_move_uses():
+    """One field for "where the tool is", not two that can drift apart.
+
+    There used to be a separate ``tool_offset_mm`` added to the *reported* contact
+    while the drawing left the geometry alone: the panel then reported one landing
+    point and drew another, 10 mm away. The reported move, the drawn move and the
+    landing check all read ``tool_xy_mm`` now, and this pins that.
+    """
+    stack = PlatformStack(tool_xy_mm=(10.0, -4.0))
+    point = np.array([2.0, 9.0, 1.0])
+    normal = np.array([0.3, 0.9, 0.2])
+    normal = normal / np.linalg.norm(normal)
+    pose = stack.solve(point, normal)
+    landed = stack.platform_rotation(pose.yaw_deg, pose.pitch_deg) @ stack.setup_point(point)
+    final = landed + np.array([pose.table_xy_mm[0], pose.table_xy_mm[1], 0.0])
+    assert final[0] == pytest.approx(10.0, abs=1e-9)
+    assert final[1] == pytest.approx(-4.0, abs=1e-9)
+    # And there is no second place a tool position could hide.
+    assert not hasattr(stack, "tool_offset_mm")
+
+
+def test_the_reported_xy_is_where_the_point_is_not_where_the_table_goes():
+    """Two different numbers, kept apart on purpose."""
+    stack = PlatformStack(tool_xy_mm=(3.0, 3.0))
+    point = np.array([2.0, 5.0, 3.0])
+    stored = stack.setup_point(point)
     pose = stack.solve(point, np.array([0.0, 1.0, 0.0]))
-    plain = PlatformStack().solve(point, np.array([0.0, 1.0, 0.0]))
-    assert pose.target_xy_mm[0] == pytest.approx(plain.target_xy_mm[0] + 1.0)
-    assert pose.target_xy_mm[1] == pytest.approx(plain.target_xy_mm[1] + 2.0)
+    assert pose.target_xy_mm[0] == pytest.approx(stored[0])
+    assert pose.target_xy_mm[1] == pytest.approx(stored[1])
+    # The move is what carries that point to the tool, so it is the rest of the way.
+    assert pose.table_xy_mm[0] == pytest.approx(3.0 - stored[0])
+    assert pose.table_xy_mm[1] == pytest.approx(3.0 - stored[1])
+
+
+# ------------------------------------------------- the setup and the tool position
+
+
+def test_the_default_tool_is_above_the_table_origin():
+    """The default has to be the origin, or every reported move is off by a patch."""
+    stack = PlatformStack()
+    assert stack.tool_xy_mm == (0.0, 0.0)
+    point = np.array([3.0, 8.0, 1.0])
+    pose = stack.solve(point, np.array([0.0, 1.0, 0.0]))
+    # With the tool at the origin, the move is the contact's position negated: the
+    # table has to bring a point that is over there back to the middle.
+    assert pose.table_xy_mm[0] == pytest.approx(-pose.target_xy_mm[0])
+    assert pose.table_xy_mm[1] == pytest.approx(-pose.target_xy_mm[1])
+
+
+def test_the_table_move_lands_the_contact_on_the_tool():
+    """The one invariant the whole machine exists to satisfy."""
+    stack = PlatformStack(
+        tool_xy_mm=(4.0, -7.0),
+        sensor_offset_mm=(1.0, 2.0, 3.0),
+        sensor_yaw_deg=25.0,
+    )
+    for point, normal in (
+        (np.array([2.0, 9.0, 1.0]), np.array([0.3, 0.9, 0.2])),
+        (np.array([-4.0, 6.0, 3.0]), np.array([-0.6, 0.5, 0.4])),
+        (np.array([0.0, 11.0, 0.0]), np.array([0.0, 1.0, 0.0])),
+    ):
+        pose = stack.solve(point, normal / np.linalg.norm(normal))
+        assert pose.ok
+        landed = stack.platform_rotation(pose.yaw_deg, pose.pitch_deg) @ stack.setup_point(
+            point
+        )
+        final = landed + np.array([pose.table_xy_mm[0], pose.table_xy_mm[1], 0.0])
+        assert final[0] == pytest.approx(stack.tool_xy_mm[0], abs=1e-9)
+        assert final[1] == pytest.approx(stack.tool_xy_mm[1], abs=1e-9)
+
+
+def test_the_mounting_offset_moves_the_point_and_the_mounting_yaw_the_whole_frame():
+    """x/y translate the contact; the yaw turns it about the table origin."""
+    stack = PlatformStack()
+    point = np.array([2.0, 6.0, 1.0])
+    normal = np.array([0.2, 0.95, 0.1])
+    normal = normal / np.linalg.norm(normal)
+    base = stack.solve(point, normal)
+
+    shifted = dataclasses.replace(stack, sensor_offset_mm=(1.5, -2.5, 0.0))
+    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)
+    assert moved.contact_z_mm == pytest.approx(base.contact_z_mm)
+
+    turned = dataclasses.replace(stack, sensor_yaw_deg=35.0)
+    yawed = turned.solve(point, normal)
+    # A yaw about the table's z keeps the radius from the origin and the height.
+    assert float(np.hypot(*yawed.target_xy_mm)) == pytest.approx(
+        float(np.hypot(*base.target_xy_mm)), rel=1e-9
+    )
+    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)
+    assert yawed.residual_deg == pytest.approx(base.residual_deg, abs=1e-9)
+
+
+def test_the_setup_z_is_the_mounting_plate():
+    """The plate is not a separate input: it is the z of the mounting pose."""
+    stack = PlatformStack()
+    point = np.array([1.0, 7.0, 2.0])
+    normal = np.array([0.1, 0.99, 0.0])
+    normal = normal / np.linalg.norm(normal)
+    plain = stack.solve(point, normal)
+
+    lifted = dataclasses.replace(stack, sensor_offset_mm=(0.0, 0.0, 4.25))
+    raised = lifted.solve(point, normal)
+    assert raised.contact_z_mm - plain.contact_z_mm == pytest.approx(4.25)
+    assert raised.target_xy_mm == pytest.approx(plain.target_xy_mm)
+    # And it is the same number as the plate parameter it replaced.
+    assert lifted.setup_point(point)[2] == pytest.approx(
+        stack.point_to_platform(point, plate_thickness_mm=4.25)[2]
+    )
+
+
+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])
+
+
+def test_a_non_finite_sensor_yaw_is_refused():
+    with pytest.raises(ValueError):
+        PlatformStack(sensor_yaw_deg=float("nan")).setup_rotation()
 
 
 def test_the_approach_sign_is_validated():