ykcheungab
bcc55a3b97
朝上判断误用平台轴、接触高度报旋转前数值、多点对准复用缓存姿态，且多点会话被复核入口拒绝；
现按修正计划改正并接通 autocal-multipoint-1.0 只读复核，缺失帧源如实报告而不补零。
离线验证：新回归在 dc5a6f8 上失败、此处通过，panel_check 通过，两截图点接触高度复现
19.0510 / 21.5781 mm；实机项未做，详见 .scratch/autocal-motion-fix/verification.md。
800 lines
34 KiB
Python
800 lines
34 KiB
Python
"""The simulated 5-axis solve: residuals, the stage envelope, and the frames.
|
|
|
|
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.
|
|
|
|
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
|
|
from orisys_autocal.motion.kinematics import (
|
|
ALIGNED_TOL_DEG,
|
|
Branch,
|
|
PlatformStack,
|
|
Pose,
|
|
residual_for,
|
|
rotation_about,
|
|
)
|
|
from orisys_autocal.sensor.frames import (
|
|
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.
|
|
|
|
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)
|
|
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()
|
|
# 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
|
|
|
|
|
|
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, 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"
|
|
)
|
|
|
|
|
|
# --------------------------------------------- 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_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".
|
|
"""
|
|
point = np.array([-2.698, 5.186, 18.228])
|
|
normal = np.array([-0.343, 0.699, 0.627])
|
|
wide = PlatformStack(yaw_limit_deg=180.0, pitch_limit_deg=90.0)
|
|
narrow = PlatformStack(yaw_limit_deg=90.0, pitch_limit_deg=90.0)
|
|
|
|
unit = wide.setup_normal(normal)
|
|
unit = unit / float(np.linalg.norm(unit))
|
|
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 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"
|
|
)
|
|
assert wide.solve(point, normal).in_range
|
|
|
|
|
|
def test_a_yaw_limit_past_the_trunnion_travel_is_capped():
|
|
"""The stage turns ±180. A stored 360 must not widen the envelope."""
|
|
stack = PlatformStack(yaw_limit_deg=360.0, pitch_limit_deg=90.0)
|
|
assert stack._yaw_limit_effective() == 180.0 # the cap lives here, not in the caller
|
|
for normal in NORMALS:
|
|
pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
|
|
assert abs(pose.yaw_deg) <= 180.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_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]))
|
|
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)
|
|
# The reported contact is that same landing point, height included — the
|
|
# whole point of the move is to arrive there, not merely at its xy.
|
|
assert pose.contact_point_mm == pytest.approx(tuple(final), abs=1e-9)
|
|
assert pose.contact_z_mm == pytest.approx(float(final[2]), 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)
|
|
# 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.
|
|
assert moved.mounted_point_mm[2] == pytest.approx(base.mounted_point_mm[2])
|
|
assert moved.contact_z_mm - base.contact_z_mm > 0.1
|
|
|
|
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)
|
|
# The plate lifts the mounting pose by exactly its thickness. The height the
|
|
# stages present rises by less, because the lift is applied before the stage
|
|
# rotation and the rotation tilts it — 4.2285 mm of the 4.25 mm here.
|
|
assert raised.mounted_point_mm[2] - plain.mounted_point_mm[2] == pytest.approx(4.25)
|
|
assert 0.0 < raised.contact_z_mm - plain.contact_z_mm < 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():
|
|
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
|
|
|
|
|
|
# --------------------------------------- the branch, the height and the up axis
|
|
|
|
|
|
def _independent_rotation(yaw_deg: float, pitch_deg: float) -> np.ndarray:
|
|
"""``R_pitch @ R_yaw`` from the formulas, not through ``rotation_about``."""
|
|
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)],
|
|
]
|
|
)
|
|
return about_pitch @ about_yaw
|
|
|
|
|
|
#: The two measured points from the bench screenshots, in the sensor frame:
|
|
#: point, normal, mounting height, and the height the stages present. The mounting
|
|
#: height is the plate (8 mm) over the point's own height; the presented height is
|
|
#: what the plan records as 19.051 and 21.578 mm.
|
|
MEASURED = (
|
|
((4.872, 5.444, 16.176), (0.595, 0.673, 0.439), 13.444, 19.051),
|
|
((-2.698, 5.186, 18.228), (-0.343, 0.699, 0.627), 13.186, 21.578),
|
|
)
|
|
|
|
|
|
def _measured_stack() -> PlatformStack:
|
|
"""The bench's mounting: the plate is the mounting pose's z, 8 mm."""
|
|
return PlatformStack(sensor_offset_mm=(0.0, 0.0, 8.0))
|
|
|
|
|
|
def test_the_contact_height_is_the_height_after_the_stages_turn():
|
|
"""Defect: the pose reported the contact's height *before* the stages turned.
|
|
|
|
On the bench that is the difference between 13.444 mm and 19.051 mm — the
|
|
stages lift the contact several millimetres, and any press-axis target is
|
|
derived from the height the contact is *presented* at, not from where it sits
|
|
with the stages at zero.
|
|
"""
|
|
stack = _measured_stack()
|
|
for point, normal, mounting, presented in MEASURED:
|
|
pose = stack.solve(np.array(point), np.array(normal) / np.linalg.norm(normal))
|
|
assert pose.mounted_point_mm[2] == pytest.approx(mounting, abs=5e-4)
|
|
assert pose.contact_z_mm == pytest.approx(presented, abs=5e-4)
|
|
|
|
# The same height again, from a rotation built here rather than by the
|
|
# solver: the landing point of the reported angles.
|
|
landed = _independent_rotation(pose.yaw_deg, pose.pitch_deg) @ stack.setup_point(
|
|
np.array(point)
|
|
)
|
|
assert pose.contact_z_mm == pytest.approx(float(landed[2]), abs=1e-9)
|
|
assert pose.contact_point_mm == pytest.approx(
|
|
(stack.tool_xy_mm[0], stack.tool_xy_mm[1], float(landed[2])), abs=1e-9
|
|
)
|
|
assert pose.contact_point_mm[2] > pose.mounted_point_mm[2]
|
|
|
|
|
|
def test_the_pose_aligns_the_normal_and_keeps_the_body_up():
|
|
"""Task-1 acceptance: independent rotation, tool axis and the platform's up.
|
|
|
|
"Up" is the direction the sensing face nominally aims — platform +Z, which is
|
|
where the mount sends the sensor's own up axis — not a horizontal axis of the
|
|
platform frame.
|
|
"""
|
|
stack = _measured_stack()
|
|
up = np.array([0.0, 0.0, 1.0])
|
|
for point, normal, _mounting, _presented in MEASURED:
|
|
pose = stack.solve(np.array(point), np.array(normal) / np.linalg.norm(normal))
|
|
rotate = _independent_rotation(pose.yaw_deg, pose.pitch_deg)
|
|
|
|
aligned = rotate @ stack.setup_normal(np.array(normal))
|
|
angle = np.degrees(np.arccos(np.clip(float(aligned @ up), -1.0, 1.0)))
|
|
assert angle <= 1e-5
|
|
|
|
# ``uprightness`` is that same rotation carried onto the up axis: the
|
|
# component left pointing up, 1.0 when nothing has tipped at all.
|
|
assert stack.uprightness(pose.yaw_deg, pose.pitch_deg) == pytest.approx(
|
|
float((rotate @ up)[2]), abs=1e-12
|
|
)
|
|
assert 0.0 < stack.uprightness(pose.yaw_deg, pose.pitch_deg) <= 1.0
|
|
assert pose.upright
|
|
|
|
|
|
def test_both_exact_poses_of_a_measured_point_leave_the_sensor_face_up():
|
|
"""Defect: the up test measured the body against a *horizontal* axis.
|
|
|
|
The two exact poses of a measured point are one pose rolled 180 degrees about
|
|
the vertical: same contact, same height, mirrored azimuth. Neither turns the
|
|
sensing face over, so flagging one of them inverted was the bug — and the
|
|
panel, the preview and the guards all read that flag.
|
|
"""
|
|
stack = PlatformStack()
|
|
first = _independent_rotation(-143.580, 47.692)
|
|
second = _independent_rotation(36.420, -47.692)
|
|
assert second == pytest.approx(rotation_about("z", 180.0) @ first, abs=1e-9)
|
|
|
|
for yaw_deg, pitch_deg in (
|
|
(36.420, -47.692),
|
|
(-143.580, 47.692),
|
|
(-61.319, 45.636),
|
|
(118.681, -45.636),
|
|
):
|
|
assert stack.uprightness(yaw_deg, pitch_deg) > 0.0
|
|
assert stack.ends_upright(yaw_deg, pitch_deg)
|
|
|
|
|
|
def test_equivalent_poses_are_chosen_by_the_machine_not_by_the_residual():
|
|
"""Two exact poses differ by float noise; that must not decide the answer.
|
|
|
|
The right-hand screenshot point, both ways round: 84 degrees of stage travel
|
|
against 191, and the shorter one carries the marginally *larger* residual.
|
|
"""
|
|
short = Branch(
|
|
yaw_deg=36.420, pitch_deg=-47.692, residual_deg=1e-9, upright=True, in_range=True
|
|
)
|
|
long_way = Branch(
|
|
yaw_deg=-143.580, pitch_deg=47.692, residual_deg=0.0, upright=True, in_range=True
|
|
)
|
|
assert PlatformStack.select_branch([long_way, short]) is short
|
|
assert PlatformStack.branch_reason([long_way, short], short) == "smallest"
|
|
|
|
|
|
def test_a_materially_better_alignment_still_wins():
|
|
"""Equivalent is a tolerance, not an excuse to ignore the residual."""
|
|
coarse = Branch(yaw_deg=0.0, pitch_deg=0.0, residual_deg=3.0, upright=True, in_range=True)
|
|
exact = Branch(yaw_deg=80.0, pitch_deg=40.0, residual_deg=0.05, upright=True, in_range=True)
|
|
assert PlatformStack.select_branch([coarse, exact]) is exact
|
|
assert PlatformStack.branch_reason([coarse, exact], exact) == "alignment"
|
|
|
|
|
|
def test_real_roundoff_does_not_choose_a_half_turn_over_the_shorter_pose():
|
|
stack = PlatformStack(pitch_limit_deg=90)
|
|
normal = np.array([-0.499590704094966, 0.6734102426572587, -0.5449107940444822])
|
|
unit = stack.setup_normal(normal)
|
|
branches = stack.alignment_branches(unit, stack.tool_direction)
|
|
short = min(branches, key=lambda branch: branch.travel_deg)
|
|
for branch in branches:
|
|
turned = stack.platform_rotation(branch.yaw_deg, branch.pitch_deg) @ unit
|
|
np.testing.assert_allclose(turned, [0, 0, 1], atol=1e-12)
|
|
assert stack.select_branch(branches) is short
|
|
|
|
|
|
def test_the_branch_keys_are_applied_in_the_order_the_machine_cares_about():
|
|
"""The envelope, then which way the body ends up, then the shorter turn."""
|
|
inside = Branch(
|
|
yaw_deg=-61.319, pitch_deg=45.636, residual_deg=0.0, upright=True, in_range=True
|
|
)
|
|
outside = Branch(
|
|
yaw_deg=0.0, pitch_deg=0.0, residual_deg=0.0, upright=True, in_range=False
|
|
)
|
|
assert PlatformStack.select_branch([inside, outside]) is inside
|
|
assert PlatformStack.branch_reason([inside, outside], inside) == "in_range"
|
|
|
|
upright = Branch(
|
|
yaw_deg=118.681, pitch_deg=-45.636, residual_deg=0.0, upright=True, in_range=True
|
|
)
|
|
rolled = Branch(
|
|
yaw_deg=10.0, pitch_deg=10.0, residual_deg=0.0, upright=False, in_range=True
|
|
)
|
|
assert PlatformStack.select_branch([upright, rolled]) is upright
|
|
assert PlatformStack.branch_reason([upright, rolled], upright) == "upright"
|
|
|
|
|
|
def test_a_lone_branch_is_its_own_reason_and_an_empty_list_is_refused():
|
|
only = Branch(yaw_deg=10.0, pitch_deg=10.0, residual_deg=0.0, upright=True, in_range=True)
|
|
assert PlatformStack.select_branch([only]) is only
|
|
assert PlatformStack.branch_reason([only], only) == "only"
|
|
with pytest.raises(ValueError):
|
|
PlatformStack.select_branch([])
|