451 lines
17 KiB
Python
451 lines
17 KiB
Python
"""Moving the platform to a simulated pose, and stopping it.
|
|
|
|
Three things are being protected here, in the order they matter:
|
|
|
|
1. **The guards refuse before anything moves.** The service is the only place that
|
|
decides whether a pose move may run, so every refusal is tested there — and a
|
|
refusal must arrive *before* the first axis is commanded, because a sequence that
|
|
stops halfway leaves the machine in a pose nobody simulated.
|
|
2. **The order is the model's order.** The kinematics composes ``R_pitch · R_yaw``
|
|
(yaw acts on the body first), so the machine yaws before it pitches; table X/Y
|
|
last. A different order would sweep the sensor through poses the operator never
|
|
saw in the preview.
|
|
3. **Stop is a real stop.** The mover is driven by snapshots, not by a loop, so a
|
|
stop runs at the next tick and no further axis is commanded. A mover that sat in
|
|
a loop could not be interrupted at all.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import time
|
|
|
|
import pytest
|
|
|
|
from orisys_autocal.motion import (
|
|
MotionResult,
|
|
MotionService,
|
|
Snapshot,
|
|
plan_pose_move,
|
|
plan_return_to_initial,
|
|
)
|
|
from orisys_autocal.motion.models import AxisState, AxisStatusBits, BusStatus
|
|
from orisys_autocal.motion.pose_move import (
|
|
MOVE_ORDER,
|
|
PoseMover,
|
|
SETTLE_DEG,
|
|
SETTLE_MM,
|
|
)
|
|
from orisys_autocal.motion.transport import DemoTransport
|
|
from orisys_autocal.preferences import default_preferences
|
|
|
|
ROLES = {"x": 0, "y": 1, "pitch": 2, "yaw": 3, "z": 4}
|
|
LIMITS = default_preferences()["safety_limits"]
|
|
|
|
|
|
def make_service(**kwargs) -> MotionService:
|
|
"""A connected, initialised platform — the state a move is allowed in."""
|
|
service = MotionService(
|
|
DemoTransport(),
|
|
axis_roles=kwargs.pop("roles", ROLES),
|
|
limits=kwargs.pop("limits", LIMITS),
|
|
**kwargs,
|
|
)
|
|
service._spec = object() # noqa: SLF001 - connected, without opening anything
|
|
service._bus = BusStatus(bus_type="ecat", init_status=1, total_axes=5) # noqa: SLF001
|
|
return service
|
|
|
|
|
|
class FakePoller:
|
|
"""Records submissions and runs them, so a plan's commands can be inspected."""
|
|
|
|
def __init__(self, service: MotionService) -> None:
|
|
self.service = service
|
|
self.commands: list = []
|
|
|
|
def submit(self, action):
|
|
self.commands.append(action)
|
|
# Not executed here: the mover is driven by snapshots, and a real poller runs
|
|
# one command per tick. Running them synchronously would hide the ordering
|
|
# this test exists to check.
|
|
return len(self.commands)
|
|
|
|
|
|
def reading(axis: int, dpos: float, *, idle: bool = True, alarm: bool = False) -> AxisState:
|
|
return AxisState(
|
|
axis=axis,
|
|
dpos=dpos,
|
|
idle=idle,
|
|
status=AxisStatusBits(raw=512 if alarm else 0),
|
|
)
|
|
|
|
|
|
def snapshot_of(service: MotionService, positions: dict[int, float], **kwargs) -> Snapshot:
|
|
return Snapshot(
|
|
connected=True,
|
|
bus=service.bus,
|
|
axes=tuple(
|
|
reading(axis, value, **kwargs) for axis, value in positions.items()
|
|
),
|
|
)
|
|
|
|
|
|
# --------------------------------------------------------------------- planning
|
|
|
|
|
|
def test_the_plan_uses_the_roles_and_each_role_own_unit():
|
|
service = make_service()
|
|
plan = plan_pose_move(
|
|
service, {"x": 1.5, "y": -2.0, "yaw": 10.0, "pitch": -20.0}, hold=("z",)
|
|
)
|
|
by_role = {step.role: step for step in plan.steps}
|
|
assert by_role["x"].axis == 0
|
|
assert by_role["yaw"].axis == 3
|
|
assert by_role["pitch"].axis == 2
|
|
# 500 units per mm on the bench, and the measured rotary constants.
|
|
assert by_role["x"].units == pytest.approx(750.0)
|
|
assert by_role["yaw"].units == pytest.approx(10.0 * 29500.0 / 90.0)
|
|
assert by_role["pitch"].units == pytest.approx(-20.0 * 12500.0 / 90.0)
|
|
assert by_role["x"].unit == "mm"
|
|
assert by_role["yaw"].unit == "°"
|
|
|
|
|
|
def test_the_move_order_yaws_before_it_pitches_and_slides_last():
|
|
"""The model composes R_pitch · R_yaw, so the machine does the same."""
|
|
service = make_service()
|
|
plan = plan_pose_move(service, {"x": 1.0, "y": 1.0, "yaw": 1.0, "pitch": 1.0})
|
|
assert [step.role for step in plan.steps] == ["yaw", "pitch", "x", "y"]
|
|
assert MOVE_ORDER.index("yaw") < MOVE_ORDER.index("pitch")
|
|
assert MOVE_ORDER.index("pitch") < MOVE_ORDER.index("x")
|
|
|
|
|
|
def test_the_plan_holds_back_what_it_is_not_given():
|
|
"""z is not commanded by a pose move — expressed by absence, not by a no-op."""
|
|
service = make_service()
|
|
plan = plan_pose_move(service, {"x": 1.0, "y": 1.0}, hold=("z",))
|
|
assert "z" not in {step.role for step in plan.steps}
|
|
assert plan.held == ("z",)
|
|
assert "z" in plan.describe()
|
|
|
|
|
|
def test_returning_to_zero_moves_the_five_axes_together():
|
|
service = make_service()
|
|
plan = plan_return_to_initial(service)
|
|
assert plan.together
|
|
assert [step.role for step in plan.steps] == ["yaw", "pitch", "x", "y", "z"]
|
|
assert plan.held == ()
|
|
assert all(step.target == 0.0 for step in plan.steps)
|
|
|
|
|
|
def test_return_to_initial_can_park_y_forward():
|
|
service = make_service()
|
|
plan = plan_return_to_initial(service, x_mm=-1.0, y_mm=15.0)
|
|
targets = {step.role: step.target for step in plan.steps}
|
|
assert targets == {"yaw": 0.0, "pitch": 0.0, "x": -1.0, "y": 15.0, "z": 0.0}
|
|
|
|
|
|
def test_pitch_positive_direction_matches_the_drive():
|
|
service = make_service()
|
|
pitch = ROLES["pitch"]
|
|
yaw = ROLES["yaw"]
|
|
seen: list[bool] = []
|
|
service.transport.jog = lambda axis, forward: seen.append((axis, forward)) # type: ignore[method-assign]
|
|
|
|
assert service.jog(pitch, True).ok
|
|
assert seen == [(pitch, True)]
|
|
assert service.move_rel(pitch, 90.0).ok
|
|
assert service.move_rel(yaw, 90.0).ok
|
|
assert service.transport.get_dpos(pitch) == pytest.approx(90.0)
|
|
assert service.transport.get_dpos(yaw) == pytest.approx(90.0)
|
|
snapshot = service.poll()
|
|
assert snapshot.axis(pitch).dpos == pytest.approx(90.0)
|
|
assert snapshot.axis(yaw).dpos == pytest.approx(90.0)
|
|
|
|
|
|
# ----------------------------------------------------------------------- guards
|
|
|
|
|
|
def test_a_move_is_refused_with_no_controller():
|
|
# English, so the assertion reads as the sentence the operator would see. The
|
|
# service localises its refusals; the wording is not the point, the refusal is.
|
|
service = MotionService(
|
|
DemoTransport(), axis_roles=ROLES, limits=LIMITS, language="en_US"
|
|
)
|
|
refusal = service.check_axes_move({"x": 1.0})
|
|
assert refusal is not None and "Not connected" in refusal.message
|
|
|
|
|
|
def test_a_move_is_refused_before_the_bus_is_initialised():
|
|
service = make_service(language="en_US")
|
|
service._bus = BusStatus(bus_type="unknown", init_status=0) # noqa: SLF001
|
|
refusal = service.check_axes_move({"x": 1.0})
|
|
assert refusal is not None and "initialis" in refusal.message
|
|
|
|
|
|
def test_a_target_past_the_travel_limit_is_refused_with_the_numbers():
|
|
service = make_service()
|
|
refusal = service.check_axes_move({"x": LIMITS["max_travel_mm"] + 0.5})
|
|
assert refusal is not None
|
|
assert f"{LIMITS['max_travel_mm'] + 0.5:.3f}" in refusal.message
|
|
|
|
|
|
def test_an_angle_is_checked_against_its_own_limit_not_the_millimetre_one():
|
|
"""A 400° yaw is past the stage envelope however roomy the table is."""
|
|
service = make_service()
|
|
assert service.check_axes_move({"yaw": 400.0}) is not None
|
|
assert service.check_axes_move({"yaw": 170.0}) is None
|
|
# And a millimetre target is not checked against the degree limit.
|
|
assert service.check_axes_move({"x": 200.0}) is not None
|
|
|
|
|
|
def test_a_limit_that_was_never_set_refuses_rather_than_meaning_unlimited():
|
|
service = make_service(limits={})
|
|
refusal = service.check_axes_move({"x": 1.0})
|
|
assert refusal is not None and "P0" in refusal.message
|
|
|
|
|
|
def test_a_duplicated_drive_is_refused():
|
|
"""Two roles on one axis means one of them silently does not move."""
|
|
service = make_service(roles={"x": 0, "y": 0, "pitch": 2, "yaw": 3, "z": 4})
|
|
problem = service.roles_problem()
|
|
assert problem
|
|
refusal = service.check_axes_move({"x": 1.0, "y": 1.0})
|
|
assert refusal is not None and refusal.message == problem
|
|
|
|
|
|
def test_an_incomplete_mapping_is_refused():
|
|
service = make_service(roles={"x": 0})
|
|
refusal = service.check_axes_move({"x": 1.0})
|
|
assert refusal is not None and "yaw" in refusal.message
|
|
|
|
|
|
def test_a_rotary_role_needs_a_calibrated_axis():
|
|
"""Axis 0 has no user-units-per-degree constant, so it cannot be a yaw."""
|
|
service = make_service(roles={"x": 4, "y": 1, "pitch": 2, "yaw": 0, "z": 3})
|
|
refusal = service.check_axes_move({"yaw": 1.0})
|
|
assert refusal is not None and "yaw" in refusal.message
|
|
|
|
|
|
def test_a_non_finite_target_is_refused():
|
|
service = make_service()
|
|
for value in (float("nan"), float("inf")):
|
|
refusal = service.check_axes_move({"x": value})
|
|
assert refusal is not None
|
|
|
|
|
|
def test_the_guard_names_an_unknown_direction():
|
|
service = make_service()
|
|
refusal = service.check_axes_move({"roll": 1.0})
|
|
assert refusal is not None and "roll" in refusal.message
|
|
|
|
|
|
# ------------------------------------------------------------------------ mover
|
|
|
|
|
|
def test_an_execute_plan_moves_the_four_axes_together_and_leaves_z():
|
|
"""Calibration execute starts yaw, pitch and X/Y in one command. Z stays put."""
|
|
service = make_service()
|
|
called: list[list[tuple[int, float]]] = []
|
|
|
|
def record(moves):
|
|
called.append(list(moves))
|
|
return MotionResult.success()
|
|
|
|
service.move_abs_many = record # type: ignore[method-assign]
|
|
poller = FakePoller(service)
|
|
mover = PoseMover(service, poller.submit)
|
|
plan = plan_pose_move(
|
|
service,
|
|
{"x": 2.0, "y": 1.0, "yaw": 10.0, "pitch": -5.0},
|
|
hold=("z",),
|
|
together=True,
|
|
)
|
|
assert plan.together
|
|
assert [step.role for step in plan.steps] == ["yaw", "pitch", "x", "y"]
|
|
assert mover.start(plan).ok
|
|
assert len(poller.commands) == 1
|
|
poller.commands[0](service)
|
|
assert [axis for axis, _value in called[0]] == [3, 2, 0, 1]
|
|
|
|
mover.on_snapshot(
|
|
snapshot_of(service, {0: 1000.0, 1: 0.0, 2: 0.0, 3: 0.0}, idle=False)
|
|
)
|
|
assert mover.running
|
|
assert mover.progress_text().startswith("同时")
|
|
assert len(poller.commands) == 1, "a later axis was queued instead of moving with the first"
|
|
|
|
mover.on_snapshot(
|
|
snapshot_of(
|
|
service,
|
|
{
|
|
0: 1000.0,
|
|
1: 500.0,
|
|
2: -5.0 * 12500.0 / 90.0,
|
|
3: 10.0 * 29500.0 / 90.0,
|
|
},
|
|
)
|
|
)
|
|
assert mover.state.finished
|
|
|
|
|
|
def test_return_to_zero_is_one_command_for_the_five_axes():
|
|
service = make_service()
|
|
called: list[list[tuple[int, float]]] = []
|
|
|
|
def record(moves):
|
|
called.append(list(moves))
|
|
return MotionResult.success()
|
|
|
|
service.move_abs_many = record # type: ignore[method-assign]
|
|
poller = FakePoller(service)
|
|
mover = PoseMover(service, poller.submit)
|
|
plan = plan_return_to_initial(service)
|
|
assert mover.start(plan).ok
|
|
assert len(poller.commands) == 1
|
|
poller.commands[0](service)
|
|
assert [axis for axis, _value in called[0]] == [3, 2, 0, 1, 4]
|
|
assert mover.progress_text().startswith("同时")
|
|
|
|
|
|
def test_the_mover_commands_one_axis_and_waits_for_it_to_arrive():
|
|
service = make_service()
|
|
poller = FakePoller(service)
|
|
mover = PoseMover(service, poller.submit)
|
|
plan = plan_pose_move(service, {"x": 2.0, "y": 1.0})
|
|
assert mover.start(plan).ok
|
|
assert len(poller.commands) == 1, "more than the first axis was commanded at once"
|
|
|
|
# x is at its target and idle -> the second axis goes out.
|
|
mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 0.0}))
|
|
assert len(poller.commands) == 2
|
|
assert mover.state.step.role == "y"
|
|
|
|
# y arrives -> the plan is done.
|
|
mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 500.0}))
|
|
assert not mover.running
|
|
assert mover.state.finished
|
|
|
|
|
|
def test_the_mover_waits_while_an_axis_is_still_moving():
|
|
service = make_service()
|
|
poller = FakePoller(service)
|
|
mover = PoseMover(service, poller.submit)
|
|
mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
|
|
mover.on_snapshot(snapshot_of(service, {0: 990.0, 1: 0.0}, idle=False))
|
|
assert len(poller.commands) == 1
|
|
assert mover.state.index == 0
|
|
|
|
|
|
def test_a_long_yaw_is_not_cut_off_while_it_is_still_moving(monkeypatch):
|
|
"""A yaw past 90° can take longer than a minute. Keep it while the axis moves.
|
|
|
|
The old step cap abandoned the plan after 20s and the window then stopped
|
|
every axis, so a 180° yaw never arrived.
|
|
"""
|
|
clock = {"t": 1_000.0}
|
|
monkeypatch.setattr(
|
|
"orisys_autocal.motion.pose_move.time.monotonic", lambda: clock["t"]
|
|
)
|
|
service = make_service(
|
|
limits={**LIMITS, "max_yaw_deg": 180.0, "max_pitch_deg": 360.0}
|
|
)
|
|
poller = FakePoller(service)
|
|
mover = PoseMover(service, poller.submit)
|
|
assert mover.start(plan_pose_move(service, {"yaw": 180.0})).ok
|
|
clock["t"] += 120.0
|
|
mover.on_snapshot(snapshot_of(service, {3: 1_000.0}, idle=False))
|
|
assert mover.running
|
|
assert mover.state.failure == ""
|
|
assert len(poller.commands) == 1
|
|
|
|
|
|
def test_the_mover_fails_when_an_axis_stops_short():
|
|
"""A drive that has stopped is not going to arrive: say so, do not wait."""
|
|
service = make_service()
|
|
poller = FakePoller(service)
|
|
mover = PoseMover(service, poller.submit)
|
|
mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
|
|
mover.on_snapshot(snapshot_of(service, {0: 10.0, 1: 0.0}))
|
|
assert mover.running, "gave up before the stall grace elapsed"
|
|
mover.state._idle_since = time.monotonic() - 10.0 # noqa: SLF001
|
|
mover.on_snapshot(snapshot_of(service, {0: 10.0, 1: 0.0}))
|
|
assert not mover.running
|
|
assert "stopped short" in mover.state.failure
|
|
assert len(poller.commands) == 1, "a further axis was commanded after the failure"
|
|
|
|
|
|
def test_the_mover_stops_on_an_alarm():
|
|
service = make_service()
|
|
poller = FakePoller(service)
|
|
mover = PoseMover(service, poller.submit)
|
|
mover.start(plan_pose_move(service, {"x": 2.0}))
|
|
mover.on_snapshot(snapshot_of(service, {0: 0.0}, alarm=True))
|
|
assert not mover.running
|
|
assert "alarm" in mover.state.failure
|
|
|
|
|
|
def test_a_tolerance_accepts_an_axis_that_reports_its_final_position_late():
|
|
service = make_service()
|
|
poller = FakePoller(service)
|
|
mover = PoseMover(service, poller.submit)
|
|
plan = plan_pose_move(service, {"x": 2.0, "y": 1.0})
|
|
mover.start(plan)
|
|
# 1 unit inside SETTLE_MM of 1000 units for 2 mm.
|
|
mover.on_snapshot(snapshot_of(service, {0: 1000.0 - 1.0, 1: 0.0}))
|
|
assert len(poller.commands) == 2, "a step within tolerance was treated as a miss"
|
|
assert SETTLE_MM > 0 and SETTLE_DEG > 0
|
|
|
|
|
|
def test_stopping_cancels_the_rest_of_the_plan():
|
|
"""The stop is a separate command, so the mover must stop queueing first."""
|
|
service = make_service()
|
|
poller = FakePoller(service)
|
|
mover = PoseMover(service, poller.submit)
|
|
mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
|
|
assert mover.running
|
|
mover.cancel("stopped")
|
|
assert not mover.running
|
|
assert mover.state.cancelled
|
|
# Nothing further, even if the axis happens to arrive afterwards.
|
|
mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 500.0}))
|
|
assert len(poller.commands) == 1
|
|
|
|
|
|
def test_a_second_plan_cannot_start_on_top_of_a_running_one():
|
|
service = make_service()
|
|
poller = FakePoller(service)
|
|
mover = PoseMover(service, poller.submit)
|
|
mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
|
|
second = mover.start(plan_pose_move(service, {"x": 3.0}))
|
|
assert not second.ok
|
|
assert len(poller.commands) == 1
|
|
|
|
|
|
def test_the_mover_refuses_a_plan_it_would_not_be_allowed_to_run():
|
|
service = MotionService(DemoTransport(), axis_roles=ROLES, limits=LIMITS)
|
|
mover = PoseMover(service, FakePoller(service).submit)
|
|
result = mover.start(plan_pose_move(service, {"x": 1.0}))
|
|
assert not result.ok
|
|
assert not mover.running
|
|
|
|
|
|
def test_the_progress_line_names_the_step_it_is_on():
|
|
service = make_service()
|
|
mover = PoseMover(service, FakePoller(service).submit)
|
|
mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
|
|
text = mover.progress_text()
|
|
assert "1/2" in text and "x" in text
|
|
|
|
|
|
def test_the_units_round_trip_through_the_service():
|
|
"""A target planned in mm must arrive at the drive as the same mm."""
|
|
service = make_service()
|
|
for role, value in (("x", 3.5), ("y", -2.25), ("yaw", 45.0), ("pitch", -30.0)):
|
|
units = service.to_units(role, value)
|
|
axis = service.role_axis(role)
|
|
if role in ("x", "y"):
|
|
assert service.to_mm(axis, units) == pytest.approx(value)
|
|
else:
|
|
from orisys_autocal.motion import user_units_to_deg
|
|
|
|
assert user_units_to_deg(axis, units) == pytest.approx(value)
|