added motion simulator feature
This commit is contained in:
@@ -116,7 +116,10 @@ def make_window(qt_app, monkeypatch):
|
||||
from orisys_app_shared.i18n import set_locale
|
||||
|
||||
set_locale(str(prefs.get("language") or "zh_CN"))
|
||||
window = AutoCalMainWindow(prefs=prefs)
|
||||
# The bundle is not auto-loaded here: rasterising 1024x1024 costs about a
|
||||
# second, and every window in this suite would pay it. The one test that
|
||||
# covers auto-loading turns it back on for itself.
|
||||
window = AutoCalMainWindow(prefs=prefs, auto_load_bundle=False)
|
||||
# Swap in the simulated platform: no test in this suite touches a DLL.
|
||||
window.poller.service._transport = DemoTransport() # noqa: SLF001
|
||||
created.append(window)
|
||||
|
||||
@@ -58,6 +58,9 @@ _SCOPED_ENUMS = {
|
||||
"PenStyle",
|
||||
"GlobalColor",
|
||||
"ItemFlag",
|
||||
"TextElideMode",
|
||||
"TextFlag",
|
||||
"AspectRatioMode",
|
||||
),
|
||||
"QTableWidget": ("SelectionMode", "EditTrigger"),
|
||||
"QSizePolicy": ("Policy",),
|
||||
|
||||
@@ -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()
|
||||
@@ -0,0 +1,404 @@
|
||||
"""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.
|
||||
|
||||
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.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from orisys_autocal.motion.kinematics import (
|
||||
ALIGNED_TOL_DEG,
|
||||
PlatformStack,
|
||||
Pose,
|
||||
residual_for,
|
||||
rotation_about,
|
||||
)
|
||||
from orisys_autocal.sensor.frames import (
|
||||
AXIS_MAP,
|
||||
describe,
|
||||
mount_rotation,
|
||||
to_platform,
|
||||
to_sensor,
|
||||
)
|
||||
|
||||
#: Normals to exercise, including the awkward ones.
|
||||
NORMALS = (
|
||||
np.array([0.0, 1.0, 0.0]), # the patch's nominal normal
|
||||
np.array([0.0, -1.0, 0.0]), # straight into the tool
|
||||
np.array([1.0, 0.0, 0.0]),
|
||||
np.array([0.0, 0.0, 1.0]),
|
||||
np.array([0.3, 0.5, -0.81]),
|
||||
np.array([-0.6, 0.2, 0.77]),
|
||||
np.array([0.0, 0.3, 0.954]),
|
||||
)
|
||||
|
||||
|
||||
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."""
|
||||
tool = stack.tool_direction
|
||||
yaw_limit = min(180.0, stack.yaw_limit_deg)
|
||||
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
|
||||
for pitch in np.arange(-pitch_limit, pitch_limit + step, step):
|
||||
rotated = rotation_about(stack.pitch_axis, float(pitch)) @ turned
|
||||
residual = float(np.degrees(np.arccos(np.clip(rotated @ tool, -1.0, 1.0))))
|
||||
best = min(best, residual)
|
||||
return best
|
||||
|
||||
|
||||
# ------------------------------------------------------------------- the mount
|
||||
|
||||
|
||||
def test_the_axis_map_is_a_rotation():
|
||||
"""A mapping that mirrors the part is worse than none: it looks fine."""
|
||||
matrix = mount_rotation()
|
||||
assert float(np.linalg.det(matrix)) == pytest.approx(1.0)
|
||||
assert np.allclose(matrix @ matrix.T, np.eye(3))
|
||||
|
||||
|
||||
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_mount_round_trips():
|
||||
vector = np.array([0.3, -0.5, 0.81])
|
||||
assert np.allclose(to_sensor(to_platform(vector)), vector)
|
||||
|
||||
|
||||
def test_a_mirroring_map_is_refused():
|
||||
"""A map that reflects the part is worse than none: the numbers look sane."""
|
||||
with pytest.raises(ValueError) as excinfo:
|
||||
mount_rotation({"x": "x", "y": "y", "z": "-z"}) # det = -1
|
||||
assert "not a rotation" in str(excinfo.value)
|
||||
|
||||
|
||||
def test_an_incomplete_map_is_refused():
|
||||
with pytest.raises(ValueError) as excinfo:
|
||||
mount_rotation({"x": "x", "y": "z"})
|
||||
assert "no entry" in str(excinfo.value)
|
||||
|
||||
|
||||
def test_the_mapping_is_described_for_the_ui():
|
||||
assert describe() == "sensor x->x, y->z, z->-y"
|
||||
|
||||
|
||||
def test_swapping_the_mapping_changes_the_answer():
|
||||
"""The one-line edit the bench may need, proven to actually do something."""
|
||||
# Asymmetric on purpose: a point on a coordinate axis maps to the same place
|
||||
# under both rotations, which would make this test pass without the mapping
|
||||
# doing anything at all.
|
||||
point = np.array([1.0, 5.0, 2.0])
|
||||
default = PlatformStack()
|
||||
# A different, still right-handed mapping: sensor x->y, y->z, z->x.
|
||||
swapped = PlatformStack(axis_map={"x": "y", "y": "z", "z": "x"})
|
||||
assert float(np.linalg.det(swapped.mount)) == pytest.approx(1.0)
|
||||
assert not np.allclose(
|
||||
default.point_to_platform(point), swapped.point_to_platform(point)
|
||||
)
|
||||
|
||||
|
||||
# --------------------------------------------------------------- the solving
|
||||
|
||||
|
||||
def test_the_bench_envelope_is_the_default():
|
||||
stack = PlatformStack()
|
||||
assert stack.yaw_limit_deg == 90.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."""
|
||||
stack = PlatformStack()
|
||||
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 "
|
||||
f"{best:.3f} deg is achievable"
|
||||
)
|
||||
|
||||
|
||||
# --------------------------------------------- the fast sweep vs the full grid
|
||||
|
||||
|
||||
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 test_the_fast_sweep_agrees_with_the_full_grid_sweep():
|
||||
"""The closed-form sweep must find what the exhaustive grid finds.
|
||||
|
||||
The shipped search solves the best pitch at each yaw in closed form, which is
|
||||
what made a full patch 14x faster. The exhaustive grid is kept as the slow
|
||||
reference precisely so this comparison exists: an optimisation in this file has
|
||||
silently cost 68 degrees before, while looking entirely reasonable.
|
||||
"""
|
||||
stack = PlatformStack()
|
||||
for unit in _random_units(12, seed=20240517)[:6]:
|
||||
fast_yaw, fast_pitch, fast_residual = stack._search_alignment( # noqa: SLF001
|
||||
unit, stack.tool_direction
|
||||
)
|
||||
ref_yaw, ref_pitch, ref_residual = stack.search_alignment_reference(unit, step=5.0)
|
||||
# The reference is quantised to the sweep step and is not polished, so it
|
||||
# may be slightly worse; it must never be better than the shipped path.
|
||||
assert fast_residual <= ref_residual + 1e-9, (
|
||||
f"fast sweep left {fast_residual:.6f} deg where the full grid found "
|
||||
f"{ref_residual:.6f} deg (fast {fast_yaw:.3f}/{fast_pitch:.3f}, "
|
||||
f"reference {ref_yaw:.3f}/{ref_pitch:.3f})"
|
||||
)
|
||||
# And the polished answer should be far better than the grid's own step.
|
||||
assert fast_residual <= ref_residual + 0.05
|
||||
|
||||
|
||||
def test_the_closed_form_pitch_beats_a_direct_scan():
|
||||
"""Guard the sign convention the closed form depends on.
|
||||
|
||||
There are two cancellation-free coefficients and picking the wrong one returns
|
||||
the antipode: a pitch 180 degrees away that still looks like a number. This is
|
||||
the check that caught exactly that, so it is stated directly against a numeric
|
||||
scan of the pitch rather than against the rest of the solver.
|
||||
"""
|
||||
stack = PlatformStack()
|
||||
for unit in _random_units(6, seed=99):
|
||||
yaws = np.array([-80.0, -35.0, -10.0, 0.0, 15.0, 47.0, 90.0])
|
||||
closed_form = stack._best_pitch_per_yaw(unit, yaws) # noqa: SLF001
|
||||
for yaw, pitch in zip(yaws, closed_form):
|
||||
scan = np.arange(-360.0, 360.0, 0.1)
|
||||
yawed = stack._turn(stack.yaw_axis, np.array([float(yaw)]), unit)[0] # noqa: SLF001
|
||||
# The residual is monotone in the dot product, so comparing the dot is
|
||||
# enough and avoids a million arccos calls.
|
||||
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
|
||||
)
|
||||
dot = (
|
||||
yawed[stationary] * tool[stationary]
|
||||
+ (yawed[first] * cosine + sign_first * yawed[second] * sine) * tool[first]
|
||||
+ (sign_second * yawed[first] * sine + yawed[second] * cosine) * tool[second]
|
||||
)
|
||||
achieved = stack._cell(unit, float(yaw), float(pitch)) # noqa: SLF001
|
||||
achieved_dot = float(np.dot(achieved, tool))
|
||||
assert achieved_dot >= float(dot.max()) - 1e-9, (
|
||||
f"closed-form pitch {pitch:.3f} deg at yaw {yaw:.1f} reaches dot "
|
||||
f"{achieved_dot:.6f}, but a direct scan reaches {dot.max():.6f}"
|
||||
)
|
||||
|
||||
|
||||
def _independent_residual(
|
||||
stack: PlatformStack, normal_sensor: np.ndarray, yaw_deg: float, pitch_deg: float
|
||||
) -> float:
|
||||
"""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.
|
||||
"""
|
||||
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
|
||||
return float(
|
||||
np.degrees(np.arccos(np.clip(float(turned @ stack.tool_direction), -1.0, 1.0)))
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("normal", NORMALS)
|
||||
def test_a_returned_pose_actually_achieves_its_residual(normal):
|
||||
"""Independent of how it was found: apply the angles and measure."""
|
||||
stack = PlatformStack()
|
||||
pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
|
||||
measured = residual_for(stack, normal, pose.yaw_deg, pose.pitch_deg)
|
||||
assert measured == pytest.approx(pose.residual_deg, abs=1e-6)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("normal", NORMALS)
|
||||
def test_the_residual_survives_an_independently_built_rotation(normal):
|
||||
"""The shared rotation helper cannot be the only thing agreeing with itself."""
|
||||
stack = PlatformStack()
|
||||
pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
|
||||
measured = _independent_residual(stack, normal, pose.yaw_deg, pose.pitch_deg)
|
||||
assert measured == pytest.approx(pose.residual_deg, abs=1e-6)
|
||||
|
||||
|
||||
def test_every_sampled_patch_normal_is_reachable_within_the_yaw_limit():
|
||||
"""The whole patch aligns, and the yaw limit is not what stops it.
|
||||
|
||||
An earlier revision of this solver claimed a quarter of the patch was
|
||||
unreachable at 65 deg. That was the solver's bug. The real answer is that the
|
||||
360 deg pitch stage does the work and the +-90 deg yaw limit costs at most a
|
||||
quarter of a degree anywhere on the patch, so no re-mount is needed.
|
||||
"""
|
||||
stack = PlatformStack()
|
||||
worst = 0.0
|
||||
for normal in NORMALS:
|
||||
pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
|
||||
assert pose.ok
|
||||
worst = max(worst, pose.residual_deg)
|
||||
assert worst < 0.5, f"worst residual across the sampled patch is {worst:.3f} deg"
|
||||
|
||||
|
||||
def test_a_generic_normal_aligns_almost_exactly():
|
||||
"""Away from the yaw clamp the solver should reach the polish floor."""
|
||||
stack = PlatformStack()
|
||||
for normal in NORMALS[:6]:
|
||||
pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
|
||||
assert pose.residual_deg < 0.01, (
|
||||
f"normal {normal} left {pose.residual_deg:.4f} deg behind"
|
||||
)
|
||||
|
||||
|
||||
def test_the_nominal_normal_needs_no_rotation():
|
||||
"""Sensor +Y is the platform +Z, so the patch's centre is already aligned."""
|
||||
stack = PlatformStack()
|
||||
pose = stack.solve(np.array([0.0, 5.0, 0.0]), np.array([0.0, 1.0, 0.0]))
|
||||
assert pose.ok
|
||||
assert pose.residual_deg == pytest.approx(0.0, abs=1e-6)
|
||||
|
||||
|
||||
def test_a_normal_straight_into_the_tool_is_flagged_indeterminate():
|
||||
stack = PlatformStack()
|
||||
pose = stack.solve(np.array([0.0, 5.0, 0.0]), np.array([0.0, -1.0, 0.0]))
|
||||
assert pose.ok
|
||||
assert not pose.yaw_determined
|
||||
assert "indeterminate" in pose.reason
|
||||
|
||||
|
||||
def test_a_degenerate_normal_is_refused():
|
||||
stack = PlatformStack()
|
||||
pose = stack.solve(np.array([0.0, 5.0, 0.0]), np.zeros(3))
|
||||
assert not pose.ok
|
||||
assert "degenerate" in pose.reason
|
||||
|
||||
|
||||
def test_the_yaw_limit_is_respected():
|
||||
stack = PlatformStack(yaw_limit_deg=90.0)
|
||||
for normal in NORMALS:
|
||||
pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
|
||||
assert abs(pose.yaw_deg) <= 90.0 + 1e-6
|
||||
|
||||
|
||||
def test_the_pitch_limit_is_respected():
|
||||
stack = PlatformStack(pitch_limit_deg=45.0)
|
||||
for normal in NORMALS:
|
||||
pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
|
||||
assert abs(pose.pitch_deg) <= 45.0 + 1e-6
|
||||
|
||||
|
||||
def test_tightening_the_envelope_can_only_leave_more_residual():
|
||||
"""A narrower stage cannot align better than a wider one."""
|
||||
normal = np.array([-0.6, 0.2, 0.77])
|
||||
point = np.array([0.0, 5.0, 0.0])
|
||||
wide = PlatformStack(yaw_limit_deg=180.0).solve(point, normal).residual_deg
|
||||
narrow = PlatformStack(yaw_limit_deg=30.0).solve(point, normal).residual_deg
|
||||
assert narrow >= wide - 1e-6
|
||||
|
||||
|
||||
def test_an_unbounded_yaw_can_align_arbitrary_normals():
|
||||
"""The envelope, not the solver, is what leaves residual."""
|
||||
stack = PlatformStack(yaw_limit_deg=180.0, pitch_limit_deg=180.0)
|
||||
worst = 0.0
|
||||
for index in range(40):
|
||||
normal = np.random.default_rng(index).normal(size=3)
|
||||
normal = normal / np.linalg.norm(normal)
|
||||
worst = max(worst, stack.solve(np.array([0.0, 5.0, 0.0]), normal).residual_deg)
|
||||
assert worst < 0.5, f"worst residual {worst:.3f} deg with a free yaw"
|
||||
|
||||
|
||||
def test_the_reported_xy_is_the_contact_point_not_the_origin():
|
||||
stack = PlatformStack()
|
||||
point = np.array([2.0, 5.0, 3.0])
|
||||
pose = stack.solve(point, np.array([0.0, 1.0, 0.0]))
|
||||
platform = stack.point_to_platform(point)
|
||||
assert pose.target_xy_mm[0] == pytest.approx(platform[0])
|
||||
assert pose.target_xy_mm[1] == pytest.approx(platform[1])
|
||||
|
||||
|
||||
def test_the_plate_lifts_the_point_along_the_tool_axis():
|
||||
stack = PlatformStack()
|
||||
point = np.array([1.0, 5.0, 2.0])
|
||||
plain = stack.point_to_platform(point)
|
||||
lifted = stack.point_to_platform(point, plate_thickness_mm=2.5)
|
||||
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])
|
||||
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)
|
||||
|
||||
|
||||
def test_the_approach_sign_is_validated():
|
||||
with pytest.raises(ValueError):
|
||||
PlatformStack(approach_sign=0)
|
||||
|
||||
|
||||
def test_swapping_the_approach_sign_changes_the_target():
|
||||
"""The sign is the difference between pressing in and pulling away."""
|
||||
normal = np.array([0.0, 1.0, 0.0])
|
||||
point = np.array([0.0, 5.0, 0.0])
|
||||
inward = PlatformStack(approach_sign=-1).solve(point, normal)
|
||||
outward = PlatformStack(approach_sign=1).solve(point, normal)
|
||||
assert inward.residual_deg == pytest.approx(0.0, abs=1e-6)
|
||||
assert outward.pitch_deg != pytest.approx(inward.pitch_deg)
|
||||
|
||||
|
||||
def test_a_pose_describes_itself():
|
||||
pose = Pose(ok=True, target_xy_mm=(1.0, 2.0), yaw_deg=3.0, pitch_deg=4.0, residual_deg=0.01)
|
||||
text = pose.describe()
|
||||
assert "yaw" in text and "pitch" in text and "residual" in text
|
||||
assert Pose(ok=False, reason="nope").describe() == "no pose: nope"
|
||||
|
||||
|
||||
def test_an_aligned_pose_reports_that_it_reached_tolerance():
|
||||
aligned = Pose(ok=True, residual_deg=0.0)
|
||||
assert aligned.reached_tolerance and aligned.reachable
|
||||
# ``reachable`` is the coarser statement: within half a degree counts as
|
||||
# "the machine can put the tool on this normal".
|
||||
loose = Pose(ok=True, residual_deg=ALIGNED_TOL_DEG * 10)
|
||||
assert not loose.reached_tolerance
|
||||
assert loose.reachable
|
||||
far = Pose(ok=True, residual_deg=30.0)
|
||||
assert not far.reached_tolerance and not far.reachable
|
||||
@@ -0,0 +1,226 @@
|
||||
"""The sensor export bundle: unit derivation, validation, and the flat view.
|
||||
|
||||
The unit factor is the assertion that matters most. `export.py` writes vertices
|
||||
in the sensor frame *without* converting the design unit, and the conversion
|
||||
lives in `world_to_cam`'s linear block — so a consumer that assumes millimetres
|
||||
is wrong by ~54x, and everything downstream (platform coordinates, press depths)
|
||||
is wrong with it. These tests pin that down.
|
||||
|
||||
The bundle used here is the bench-confirmed one. When it is not present the
|
||||
tests skip rather than fail: this suite has to run on machines that do not have
|
||||
the sensor-design checkout.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from orisys_autocal.sensor.export_bundle import (
|
||||
BundleError,
|
||||
load_bundle,
|
||||
mm_per_unit_from_pose,
|
||||
)
|
||||
from orisys_autocal.sensor.uv_flat import rasterise
|
||||
|
||||
#: The bench-confirmed export, and the figures measured from it.
|
||||
BUNDLE = r"D:\orisys_ws\orisys-sensor-design\build\export\finger\r01\compute"
|
||||
EXPECTED_VERTICES = 1032
|
||||
EXPECTED_TRIANGLES = 1974
|
||||
EXPECTED_MM_PER_UNIT = 0.018388452316735537
|
||||
EXPECTED_EXTENT_MM = (15.0, 7.5, 15.81)
|
||||
|
||||
|
||||
def _bundle():
|
||||
if not __import__("pathlib").Path(BUNDLE).is_dir():
|
||||
pytest.skip("the sensor-design export is not on this machine")
|
||||
return load_bundle(
|
||||
BUNDLE,
|
||||
expected_vertices=EXPECTED_VERTICES,
|
||||
expected_triangles=EXPECTED_TRIANGLES,
|
||||
)
|
||||
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def bundle():
|
||||
return _bundle()
|
||||
|
||||
|
||||
# ------------------------------------------------------------------- the scale
|
||||
|
||||
|
||||
def test_the_unit_factor_is_derived_from_the_pose(bundle):
|
||||
assert bundle.mm_per_unit == pytest.approx(EXPECTED_MM_PER_UNIT, rel=1e-9)
|
||||
|
||||
|
||||
def test_the_physical_size_is_millimetres_not_mesh_units(bundle):
|
||||
"""The failure this whole module exists to catch: a 54x unit error."""
|
||||
extent = bundle.extent_mm
|
||||
assert extent[0] == pytest.approx(EXPECTED_EXTENT_MM[0], abs=0.02)
|
||||
assert extent[1] == pytest.approx(EXPECTED_EXTENT_MM[1], abs=0.02)
|
||||
assert extent[2] == pytest.approx(EXPECTED_EXTENT_MM[2], abs=0.02)
|
||||
|
||||
|
||||
def test_a_rigid_pose_means_one_millimetre_per_unit():
|
||||
assert mm_per_unit_from_pose(np.eye(4)) == pytest.approx(1.0)
|
||||
|
||||
|
||||
def test_a_uniformly_scaled_pose_reports_that_scale():
|
||||
pose = np.eye(4)
|
||||
pose[:3, :3] *= 0.5
|
||||
assert mm_per_unit_from_pose(pose) == pytest.approx(0.5)
|
||||
|
||||
|
||||
def test_a_non_uniform_pose_is_refused():
|
||||
"""A per-axis scale cannot be undone by one factor; guessing is worse."""
|
||||
pose = np.eye(4)
|
||||
pose[0, 0] = 0.01
|
||||
pose[1, 1] = 0.02
|
||||
pose[2, 2] = 0.03
|
||||
with pytest.raises(BundleError) as excinfo:
|
||||
mm_per_unit_from_pose(pose)
|
||||
assert "uniform" in str(excinfo.value)
|
||||
|
||||
|
||||
def test_a_mirrored_pose_is_refused():
|
||||
"""A reflection would put every point on the wrong side of the sensor."""
|
||||
pose = np.eye(4)
|
||||
pose[0, 0] = -1.0
|
||||
with pytest.raises(BundleError) as excinfo:
|
||||
mm_per_unit_from_pose(pose)
|
||||
assert "mirror" in str(excinfo.value).lower()
|
||||
|
||||
|
||||
def test_a_degenerate_pose_is_refused():
|
||||
with pytest.raises(BundleError):
|
||||
mm_per_unit_from_pose(np.zeros((4, 4)))
|
||||
|
||||
|
||||
# ------------------------------------------------------------------- the mesh
|
||||
|
||||
|
||||
def test_the_mesh_matches_the_design_profile(bundle):
|
||||
assert bundle.vertices_mm.shape == (EXPECTED_VERTICES, 3)
|
||||
assert bundle.faces.shape == (EXPECTED_TRIANGLES, 3)
|
||||
assert bundle.uv_data.shape[0] == EXPECTED_TRIANGLES * 3
|
||||
assert bundle.loop_to_vertex.shape[0] == EXPECTED_TRIANGLES * 3
|
||||
|
||||
|
||||
def test_every_face_index_is_in_range(bundle):
|
||||
assert int(bundle.faces.max()) < bundle.vertices_mm.shape[0]
|
||||
assert int(bundle.faces.min()) >= 0
|
||||
assert int(bundle.loop_to_vertex.max()) < bundle.vertices_mm.shape[0]
|
||||
|
||||
|
||||
def test_the_surface_area_is_physically_plausible(bundle):
|
||||
"""~356 mm^2 for a 15 x 7.5 x 15.8 mm patch: a unit error would show here."""
|
||||
assert bundle.surface_area_mm2 == pytest.approx(355.77, abs=1.0)
|
||||
|
||||
|
||||
def test_the_stored_normals_are_coherent(bundle):
|
||||
"""Mixed winding would make 'the normal' meaningless."""
|
||||
normals = bundle.face_normals
|
||||
centroid = bundle.vertices_mm.mean(axis=0)
|
||||
centres = bundle.vertices_mm[bundle.faces].mean(axis=1)
|
||||
outward = np.einsum("ij,ij->i", normals, centres - centroid)
|
||||
assert float((outward > 0).mean()) > 0.95
|
||||
|
||||
|
||||
def test_a_missing_file_is_named(tmp_dir):
|
||||
"""A path that is not a directory at all says so, and names the path."""
|
||||
absent = tmp_dir / "no-such-revision"
|
||||
with pytest.raises(BundleError) as excinfo:
|
||||
load_bundle(absent)
|
||||
assert "not a directory" in str(excinfo.value)
|
||||
assert absent.name in str(excinfo.value)
|
||||
|
||||
|
||||
def test_an_incomplete_bundle_names_what_is_missing(tmp_dir):
|
||||
(tmp_dir / "mesh.npz").write_bytes(b"")
|
||||
with pytest.raises(BundleError) as excinfo:
|
||||
load_bundle(tmp_dir)
|
||||
message = str(excinfo.value)
|
||||
assert "housing.npz" in message and "world_to_cam.npy" in message
|
||||
|
||||
|
||||
def test_a_count_mismatch_is_refused(bundle):
|
||||
with pytest.raises(BundleError) as excinfo:
|
||||
load_bundle(BUNDLE, expected_vertices=999)
|
||||
assert "999" in str(excinfo.value)
|
||||
assert str(EXPECTED_VERTICES) in str(excinfo.value)
|
||||
|
||||
|
||||
# --------------------------------------------------------------- the flat view
|
||||
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def flat(bundle):
|
||||
return rasterise(bundle, (256, 256))
|
||||
|
||||
|
||||
def test_the_flat_image_is_not_blank(flat):
|
||||
assert flat.image_rgb.shape == (256, 256, 3)
|
||||
assert flat.image_rgb.dtype == np.uint8
|
||||
assert not bool((flat.image_rgb == 0).all())
|
||||
|
||||
|
||||
def test_a_reasonable_share_of_the_image_carries_surface(flat):
|
||||
assert 0.5 < flat.valid_fraction < 0.9
|
||||
|
||||
|
||||
def test_a_corner_pixel_is_an_island_gap(flat):
|
||||
"""The atlas is a single island, so a corner is outside it."""
|
||||
assert flat.sample(0, 0) is None
|
||||
assert flat.sample(flat.width - 1, flat.height - 1) is None
|
||||
|
||||
|
||||
def test_an_out_of_range_pixel_is_reported_not_crashed(flat):
|
||||
assert flat.sample(-1, 5) is None
|
||||
assert flat.sample(5, flat.height + 10) is None
|
||||
|
||||
|
||||
def test_every_covered_pixel_lies_on_its_recorded_face(bundle, flat):
|
||||
"""The lookup table must be the mesh, not an approximation of it."""
|
||||
covered = np.argwhere(flat.face_index >= 0)
|
||||
step = max(1, len(covered) // 300)
|
||||
worst = 0.0
|
||||
for row, column in covered[::step]:
|
||||
face = int(flat.face_index[row, column])
|
||||
triangle = bundle.vertices_mm[bundle.faces[face]]
|
||||
normal = np.cross(triangle[1] - triangle[0], triangle[2] - triangle[0])
|
||||
normal = normal / np.linalg.norm(normal)
|
||||
distance = abs(float((flat.positions_mm[row, column] - triangle[0]) @ normal))
|
||||
worst = max(worst, distance)
|
||||
assert worst < 1e-9
|
||||
|
||||
|
||||
def test_every_covered_pixel_reports_its_face_normal(bundle, flat):
|
||||
covered = np.argwhere(flat.face_index >= 0)
|
||||
step = max(1, len(covered) // 300)
|
||||
for row, column in covered[::step]:
|
||||
face = int(flat.face_index[row, column])
|
||||
assert float(flat.normals[row, column] @ bundle.face_normals[face]) == pytest.approx(1.0)
|
||||
|
||||
|
||||
def test_a_uv_corner_lands_on_a_pixel_near_its_vertex(bundle, flat):
|
||||
"""The round trip: vertex -> its UV pixel -> looked-up 3D is within a pixel.
|
||||
|
||||
Most corners land exactly on their vertex; a few land one pixel off a seam,
|
||||
which is what the tolerance is for.
|
||||
"""
|
||||
uv = bundle.uv_data.reshape(-1, 3, 2)
|
||||
worst = 0.0
|
||||
misses = 0
|
||||
for index in range(0, len(uv), 97):
|
||||
face, corner = divmod(index, 3)
|
||||
x, y = flat.pixel_for_uv(float(uv[face, corner, 0]), float(uv[face, corner, 1]))
|
||||
sample = flat.sample(x, y)
|
||||
if sample is None:
|
||||
# A corner exactly on an island boundary can miss; count it and keep
|
||||
# the check meaningful by requiring that it stays rare.
|
||||
misses += 1
|
||||
continue
|
||||
vertex = bundle.vertices_mm[bundle.loop_to_vertex[index]]
|
||||
worst = max(worst, float(np.linalg.norm(sample[0] - vertex)))
|
||||
assert worst < 0.1, "a looked-up point is more than a pixel from its vertex"
|
||||
assert misses < len(range(0, len(uv), 97)) * 0.05
|
||||
@@ -0,0 +1,126 @@
|
||||
"""The UV-space texture: loading it, resampling it, and falling back without it.
|
||||
|
||||
The texture is authored in UV coordinates, black outside the island, so it
|
||||
resamples into exactly the space the picker already uses. These tests pin the two
|
||||
things that would silently ruin that: the orientation of the resample, and the
|
||||
claim that adding a texture cannot change where a click lands.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from orisys_autocal.sensor import (
|
||||
DEFAULT_FLAT_SIZE,
|
||||
load_bundle,
|
||||
load_uv_texture,
|
||||
rasterise,
|
||||
texture_path_for,
|
||||
)
|
||||
|
||||
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 texture(bundle):
|
||||
path = texture_path_for(bundle)
|
||||
if path is None:
|
||||
pytest.skip("this export has no texture beside it")
|
||||
loaded = load_uv_texture(path)
|
||||
if loaded is None:
|
||||
pytest.skip(f"could not decode {path}")
|
||||
return loaded
|
||||
|
||||
|
||||
def test_the_texture_is_found_from_the_bundle(bundle):
|
||||
"""Derived, not configured: a mismatched pair resamples the wrong artwork."""
|
||||
path = texture_path_for(bundle)
|
||||
if path is None:
|
||||
pytest.skip("this export has no texture beside it")
|
||||
assert path.is_file()
|
||||
assert path.name == "patch_texture_compute.png"
|
||||
assert path.parent.parent.name == "finger"
|
||||
|
||||
|
||||
def test_loading_a_texture_returns_an_rgb_ready_array(texture):
|
||||
assert texture.ndim == 3 and texture.shape[2] == 4
|
||||
assert texture.dtype == np.uint8
|
||||
assert texture.shape[0] > 1 and texture.shape[1] > 1
|
||||
|
||||
|
||||
def test_a_missing_texture_is_none_not_an_exception():
|
||||
"""The picker has to keep working without artwork."""
|
||||
assert load_uv_texture(r"D:\definitely\not\here.png") is None
|
||||
|
||||
|
||||
def test_the_texture_is_black_outside_the_island_and_not_inside(texture):
|
||||
"""The island's v extent is where the artwork lives; above and below is black."""
|
||||
height = texture.shape[0]
|
||||
top = texture[: int(height * 0.05), :, :3]
|
||||
bottom = texture[int(height * 0.95) :, :, :3]
|
||||
middle = texture[int(height * 0.45) : int(height * 0.55), :, :3]
|
||||
assert int(top.max()) == 0, "the top band should be unused black"
|
||||
assert int(bottom.max()) == 0, "the bottom band should be unused black"
|
||||
assert int(middle.max()) > 0, "the island band should carry content"
|
||||
|
||||
|
||||
def test_a_texture_does_not_change_where_a_click_lands(bundle, texture):
|
||||
"""The lookup tables are the measurement; the image is only what you aim at."""
|
||||
size = (256, 256)
|
||||
plain = rasterise(bundle, size)
|
||||
textured = rasterise(bundle, size, texture=texture)
|
||||
|
||||
assert plain.textured is False
|
||||
assert textured.textured is True
|
||||
assert np.array_equal(plain.face_index, textured.face_index)
|
||||
assert np.allclose(plain.positions_mm, textured.positions_mm)
|
||||
assert np.allclose(plain.normals, textured.normals)
|
||||
assert plain.uv_min == textured.uv_min and plain.uv_max == textured.uv_max
|
||||
|
||||
|
||||
def test_the_textured_image_samples_the_texture_in_the_right_place(bundle, texture):
|
||||
"""Resample at the pixel's own UV, including the flip, or the map is mirrored."""
|
||||
textured = rasterise(bundle, (256, 256), texture=texture)
|
||||
covered = np.nonzero(textured.face_index >= 0)
|
||||
rows, columns = covered[0][::97], covered[1][::97]
|
||||
assert rows.size > 20
|
||||
|
||||
height, width = textured.face_index.shape
|
||||
u0, v0 = textured.uv_min
|
||||
u1, v1 = textured.uv_max
|
||||
texture_height, texture_width = texture.shape[0], texture.shape[1]
|
||||
|
||||
for row, column in zip(rows, columns):
|
||||
u = u0 + column / (width - 1) * (u1 - u0)
|
||||
v = v1 - row / (height - 1) * (v1 - v0)
|
||||
tex_x = int(round(u * (texture_width - 1)))
|
||||
tex_y = int(round((1.0 - v) * (texture_height - 1)))
|
||||
expected = texture[tex_y, tex_x, :3]
|
||||
assert np.array_equal(textured.image_rgb[row, column], expected), (
|
||||
f"pixel ({column}, {row}) does not carry the texture sample at its own UV"
|
||||
)
|
||||
|
||||
|
||||
def test_gap_pixels_stay_black_when_textured(bundle, texture):
|
||||
"""A fabricated colour in the gap would look like part of the patch."""
|
||||
textured = rasterise(bundle, (256, 256), texture=texture)
|
||||
gaps = textured.face_index < 0
|
||||
if not gaps.any():
|
||||
pytest.skip("this raster has no gap pixels")
|
||||
assert int(textured.image_rgb[gaps].max()) == 0
|
||||
|
||||
|
||||
def test_the_default_flat_size_is_still_the_design_size():
|
||||
assert DEFAULT_FLAT_SIZE == (1024, 1024)
|
||||
Reference in New Issue
Block a user