ykcheungab
334 lines
14 KiB
Python
334 lines
14 KiB
Python
"""The closed-form solve: exactness, agreement with the sweep, and what it reports.
|
||
|
||
The sweep in ``test_kinematics.py`` is the reference this path is checked against.
|
||
It is slow but line-by-line verifiable, and solvers in this file have been
|
||
confidently wrong five times; a fast path with no reference is how that happens.
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import math
|
||
from pathlib import Path
|
||
|
||
import numpy as np
|
||
import pytest
|
||
from orisys_autocal.motion.kinematics import (
|
||
Branch,
|
||
PlatformStack,
|
||
rotation_about,
|
||
)
|
||
from orisys_autocal.sensor import load_bundle
|
||
|
||
#: The revision the bench confirmed. Skipped rather than failed when absent: the
|
||
#: export lives in a sibling design repo, and a machine without it has not broken
|
||
#: AutoCal.
|
||
BUNDLE = Path(r"D:\orisys_ws\orisys-sensor-design\build\export\finger\r01\compute")
|
||
|
||
|
||
@pytest.fixture(scope="module")
|
||
def bundle():
|
||
if not BUNDLE.is_dir():
|
||
pytest.skip(f"no sensor export at {BUNDLE}")
|
||
return load_bundle(BUNDLE)
|
||
|
||
|
||
def _random_units(count: int, seed: int) -> list[np.ndarray]:
|
||
generator = np.random.default_rng(seed)
|
||
vectors = generator.normal(size=(count, 3))
|
||
return [vector / np.linalg.norm(vector) for vector in vectors]
|
||
|
||
|
||
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
|
||
)
|
||
|
||
|
||
# ------------------------------------------------------------------ exactness
|
||
|
||
|
||
def test_the_closed_form_is_exact_on_random_normals():
|
||
"""Two atan2 calls should land on the answer, not near it."""
|
||
stack = PlatformStack()
|
||
worst = 0.0
|
||
for unit in _random_units(2000, seed=20240517):
|
||
branches = stack.alignment_branches(unit, stack.tool_direction)
|
||
assert branches, f"no branch found for {unit}"
|
||
worst = max(worst, min(branch.residual_deg for branch in branches))
|
||
assert worst < 1e-4, f"closed form left {worst:.3e} deg on the table"
|
||
|
||
|
||
def test_every_branch_reproduces_its_own_claimed_residual():
|
||
"""The number reported must be the number the rotations actually give."""
|
||
stack = PlatformStack()
|
||
for unit in _random_units(200, seed=7):
|
||
for branch in stack.alignment_branches(unit, stack.tool_direction):
|
||
landed = _apply(stack, unit, branch.yaw_deg, branch.pitch_deg)
|
||
measured = math.degrees(
|
||
math.acos(
|
||
max(-1.0, min(1.0, float(landed @ stack.tool_direction)))
|
||
)
|
||
)
|
||
# ``acos`` near 1 loses about sqrt(eps) in radians because the
|
||
# derivative blows up there; 1e-5 deg is the floating-point floor,
|
||
# not slack in the answer.
|
||
assert measured == pytest.approx(branch.residual_deg, abs=1e-5)
|
||
|
||
|
||
# ------------------------------------------------------------------ agreement
|
||
|
||
|
||
def test_the_closed_form_is_no_worse_than_the_sweep():
|
||
"""The sweep is the reference; the fast path may beat it but never lose."""
|
||
stack = PlatformStack()
|
||
for unit in _random_units(400, seed=99):
|
||
fast = stack._search_alignment(unit, stack.tool_direction) # noqa: SLF001
|
||
branches = stack.alignment_branches(unit, stack.tool_direction)
|
||
best = min(branch.residual_deg for branch in branches)
|
||
assert best <= fast[2] + 1e-9, (
|
||
f"closed form left {best:.6f} deg where the sweep found {fast[2]:.6f}"
|
||
)
|
||
|
||
|
||
def test_the_two_methods_agree_inside_the_envelope():
|
||
stack = PlatformStack()
|
||
for unit in _random_units(200, seed=1234):
|
||
analytical = stack.solve(np.zeros(3), unit, method="analytical")
|
||
search = stack.solve(np.zeros(3), unit, method="search")
|
||
assert analytical.residual_deg <= search.residual_deg + 1e-6
|
||
for pose in (analytical, search):
|
||
assert abs(pose.yaw_deg) <= stack.yaw_limit_deg + 1e-6
|
||
assert abs(pose.pitch_deg) <= stack.pitch_limit_deg + 1e-6
|
||
|
||
|
||
# ----------------------------------------------------------- axis equivalence
|
||
|
||
|
||
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)`` 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.
|
||
"""
|
||
unit = np.array([0.3, 0.5, -0.81])
|
||
unit = unit / np.linalg.norm(unit)
|
||
tool = np.array([0.0, 0.0, -1.0])
|
||
worst = 0.0
|
||
for yaw in (-120.0, -45.0, 0.0, 37.0, 90.0, 150.0):
|
||
for pitch in (-90.0, -20.0, 0.0, 15.0, 75.0):
|
||
about_y = rotation_about("y", pitch) @ rotation_about("z", yaw) @ unit
|
||
about_x = rotation_about("x", pitch) @ rotation_about("z", yaw - 90.0) @ unit
|
||
worst = max(worst, abs(float(about_y @ tool) - float(about_x @ tool)))
|
||
assert worst == pytest.approx(0.0, abs=1e-12), (
|
||
f"the two pitch bindings diverge in alignment by {worst:.3e}"
|
||
)
|
||
|
||
|
||
def test_the_offset_between_the_bindings_is_a_constant_ninety_degrees():
|
||
"""A constant shift, so it is a commissioning zero rather than a correction."""
|
||
unit = np.array([0.3, 0.5, -0.81])
|
||
unit = unit / np.linalg.norm(unit)
|
||
tool = np.array([0.0, 0.0, -1.0])
|
||
for yaw in (-120.0, -30.0, 0.0, 45.0, 120.0):
|
||
for pitch in (-90.0, 0.0, 25.0, 75.0):
|
||
about_y = rotation_about("y", pitch) @ rotation_about("z", yaw) @ unit
|
||
same = rotation_about("x", pitch) @ rotation_about("z", yaw - 90.0) @ unit
|
||
assert float(about_y @ tool) == pytest.approx(float(same @ tool), abs=1e-12)
|
||
|
||
|
||
# ---------------------------------------------------------------- reporting
|
||
|
||
|
||
def test_the_normal_is_aligned_to_plus_z_not_minus_z(bundle):
|
||
"""The regression that would have caught the sensor being turned over.
|
||
|
||
``approach_sign`` was ``-1``, which asked the solver to bring every normal onto
|
||
**-Z** — the sensing face is the *top* surface of a sensor mounted at the bottom
|
||
of its shell, so it points up, and the tool feeds down onto it. Nothing caught
|
||
it because ``arccos`` is sign-blind: a face turned fully over still scored
|
||
``0.0000`` degrees, and every test here asserted only that the residual was
|
||
small. The failure showed up exclusively as the model flipping upside down.
|
||
|
||
So this asserts the *direction* the normal lands in, which is the thing the
|
||
residual cannot express.
|
||
"""
|
||
stack = PlatformStack()
|
||
assert stack.approach_sign == 1
|
||
assert np.allclose(stack.tool_direction, [0.0, 0.0, 1.0])
|
||
|
||
for normal in bundle.vertex_normals():
|
||
unit = stack.normal_to_platform(normal)
|
||
unit = unit / np.linalg.norm(unit)
|
||
pose = stack.solve(np.zeros(3), normal)
|
||
# Rotate in the **platform** frame, which is where the residual is measured.
|
||
# ``stage_rotation`` is the same rotation conjugated into the mesh's frame,
|
||
# so using it here compares a sensor-frame vector against a platform-frame
|
||
# target and reports a mismatch that does not exist.
|
||
landed = _apply(stack, unit, pose.yaw_deg, pose.pitch_deg)
|
||
landed = landed / np.linalg.norm(landed)
|
||
# Aligned with the face's aim, not merely parallel to its line.
|
||
assert landed[2] > 0.0, (
|
||
f"the normal landed on {np.round(landed, 3)} — pointing away from +Z"
|
||
)
|
||
|
||
|
||
def test_the_nominal_pose_is_upright(bundle):
|
||
"""No rotation is the commissioned pose, and it must read as upright."""
|
||
stack = PlatformStack()
|
||
assert stack.uprightness(0.0, 0.0) == pytest.approx(1.0)
|
||
|
||
|
||
def test_the_surface_normals_point_up_so_the_face_faces_the_tool(bundle):
|
||
"""A sanity check on the geometry the polarity has to agree with.
|
||
|
||
The mean normal has a positive platform-Z component: the patch faces upward.
|
||
If this ever inverted, ``approach_sign`` would need revisiting rather than the
|
||
solver.
|
||
"""
|
||
stack = PlatformStack()
|
||
platform = np.array(
|
||
[stack.normal_to_platform(n) for n in bundle.vertex_normals()]
|
||
)
|
||
platform /= np.linalg.norm(platform, axis=1)[:, None]
|
||
assert float(platform[:, 2].mean()) > 0.0, (
|
||
f"mean platform-Z component of the normals is {platform[:, 2].mean():+.3f}"
|
||
)
|
||
|
||
|
||
def test_few_vertices_end_turned_over_and_those_are_edge_on(bundle):
|
||
"""Turning the face over is the failure mode; measure it and bound it.
|
||
|
||
The remainder are vertices whose normals are 60-90 degrees off the tool axis —
|
||
the rim of the dome. Aligning an edge-on face has to roll the body about that
|
||
normal, and the sign is decided by rounding, so they cannot be pushed to zero
|
||
and should not be asserted away.
|
||
"""
|
||
stack = PlatformStack()
|
||
normals = bundle.vertex_normals()
|
||
flipped = 0
|
||
for normal in normals:
|
||
pose = stack.solve(np.zeros(3), normal)
|
||
if stack.uprightness(pose.yaw_deg, pose.pitch_deg) < -0.5:
|
||
flipped += 1
|
||
assert flipped <= len(normals) * 0.03, (
|
||
f"{flipped} of {len(normals)} vertices ended turned over"
|
||
)
|
||
|
||
|
||
def test_a_pose_reports_whether_it_is_upright():
|
||
"""A pose can align the normal and still turn the sensor over."""
|
||
stack = PlatformStack()
|
||
seen = {True: 0, False: 0}
|
||
for unit in _random_units(300, seed=3):
|
||
pose = stack.solve(np.zeros(3), unit)
|
||
seen[pose.upright] += 1
|
||
# Whatever it says must match the measured orientation.
|
||
assert pose.upright == (stack.uprightness(pose.yaw_deg, pose.pitch_deg) >= 0.0)
|
||
assert seen[True] > 0
|
||
|
||
|
||
def test_an_out_of_range_pose_is_reported_and_never_silently_clamped():
|
||
"""Tight travel must give a measured residual, not a pose the stage cannot reach."""
|
||
stack = PlatformStack(pitch_limit_deg=45.0)
|
||
for unit in _random_units(50, seed=5):
|
||
pose = stack.solve(np.zeros(3), unit)
|
||
assert abs(pose.pitch_deg) <= 45.0 + 1e-6
|
||
assert abs(pose.yaw_deg) <= stack.yaw_limit_deg + 1e-6
|
||
if not pose.in_range:
|
||
assert "travel" in pose.reason
|
||
|
||
|
||
def test_nothing_is_reported_out_of_range_when_the_travel_is_ample():
|
||
stack = PlatformStack()
|
||
for unit in _random_units(300, seed=11):
|
||
pose = stack.solve(np.zeros(3), unit)
|
||
assert pose.in_range, f"{unit} reported out of range at full travel"
|
||
assert pose.reason == "" or "indeterminate" in pose.reason
|
||
|
||
|
||
def test_reachability_agrees_with_the_envelope():
|
||
"""Every normal is reachable when the yaw can turn a full half-turn."""
|
||
stack = PlatformStack()
|
||
for unit in _random_units(500, seed=13):
|
||
reachable, why = stack.reachable(unit, stack.tool_direction)
|
||
assert reachable, f"{unit} unreachable: {why}"
|
||
# And with the pitch stage able to turn, no normal is rejected either.
|
||
assert stack.reachable(
|
||
np.array([0.0, 0.0, 1.0]), stack.tool_direction
|
||
)[0]
|
||
|
||
|
||
# --------------------------------------------------------------------- poles
|
||
|
||
|
||
@pytest.mark.parametrize(
|
||
"normal",
|
||
[
|
||
np.array([0.0, 1.0, 0.0]), # on the tool axis
|
||
np.array([0.0, -1.0, 0.0]), # on the tool axis, other way
|
||
np.array([1.0, 0.0, 0.0]), # on the yaw plane
|
||
np.array([0.0, 0.0, 1.0]), # on the yaw axis
|
||
],
|
||
)
|
||
def test_poles_still_return_a_usable_pose(normal):
|
||
stack = PlatformStack()
|
||
pose = stack.solve(np.zeros(3), normal)
|
||
assert pose.ok
|
||
assert math.isfinite(pose.yaw_deg) and math.isfinite(pose.pitch_deg)
|
||
landed = _apply(stack, stack.normal_to_platform(normal) / np.linalg.norm(
|
||
stack.normal_to_platform(normal)
|
||
), pose.yaw_deg, pose.pitch_deg)
|
||
measured = math.degrees(
|
||
math.acos(max(-1.0, min(1.0, float(landed @ stack.tool_direction))))
|
||
)
|
||
assert measured <= stack.tolerance_deg + 1e-6
|
||
|
||
|
||
def test_the_degenerate_normal_is_still_refused():
|
||
stack = PlatformStack()
|
||
pose = stack.solve(np.zeros(3), np.zeros(3))
|
||
assert not pose.ok
|
||
assert "degenerate" in pose.reason
|
||
|
||
|
||
# ----------------------------------------------------------------- selection
|
||
|
||
|
||
def test_select_branch_prefers_in_range_then_upright_then_small():
|
||
reachable_upright = Branch(10.0, 5.0, 1e-6, True, True)
|
||
reachable_inverted = Branch(10.0, 5.0, 1e-6, False, True)
|
||
unreachable_upright = Branch(179.0, 5.0, 1e-6, True, False)
|
||
|
||
# In range beats everything: an inverted pose the machine can reach is worth
|
||
# more than an upright one it cannot.
|
||
assert PlatformStack.select_branch(
|
||
[unreachable_upright, reachable_inverted, reachable_upright]
|
||
) is reachable_upright
|
||
# Among reachable poses, upright wins.
|
||
assert PlatformStack.select_branch(
|
||
[reachable_inverted, reachable_upright]
|
||
) is reachable_upright
|
||
# Among poses that are out of range and one of them upright, upright wins.
|
||
unreachable_inverted = Branch(179.0, 5.0, 1e-6, False, False)
|
||
assert PlatformStack.select_branch(
|
||
[unreachable_inverted, unreachable_upright]
|
||
) is unreachable_upright
|
||
# A materially better alignment beats an upright pose.
|
||
much_better = Branch(10.0, 5.0, 0.0, False, True)
|
||
worse_upright = Branch(10.0, 5.0, 5.0, True, True)
|
||
assert PlatformStack.select_branch([much_better, worse_upright]) is much_better
|
||
# And with everything else equal, the smaller move wins.
|
||
smaller = Branch(5.0, 5.0, 1e-6, True, True)
|
||
larger = Branch(60.0, 5.0, 1e-6, True, True)
|
||
assert PlatformStack.select_branch([larger, smaller]) is smaller
|
||
|
||
|
||
def test_an_unknown_method_is_refused():
|
||
stack = PlatformStack()
|
||
with pytest.raises(ValueError):
|
||
stack.solve(np.zeros(3), np.array([0.0, 1.0, 0.0]), method="nonsense")
|