fix(autocal): 校验运动单位并等待命令成功后判断到位
阻止单位不一致的姿态命令,避免提前到达的位置快照误报成功或推进下一轴。 相关检查 164 项通过,2064 组目标及 8256 条运动调用与原基线完全一致。
This commit is contained in:
@@ -12,7 +12,7 @@
|
|||||||
|
|
||||||
[`PoseMover`](../src/orisys_autocal/motion/pose_move.py) 启动前检查计划中的轴编号、物理目标和已换算的用户单位目标仍对应当前配置。工作线程执行前再次读各目标轴的 `UNITS`,全部校验完成后才按原顺序发送原绝对目标。配置或角色映射在排队期间改变时,旧计划被拒绝,要求重新规划;不会偷偷套用新设置。
|
[`PoseMover`](../src/orisys_autocal/motion/pose_move.py) 启动前检查计划中的轴编号、物理目标和已换算的用户单位目标仍对应当前配置。工作线程执行前再次读各目标轴的 `UNITS`,全部校验完成后才按原顺序发送原绝对目标。配置或角色映射在排队期间改变时,旧计划被拒绝,要求重新规划;不会偷偷套用新设置。
|
||||||
|
|
||||||
移动器按命令 token 接收工作线程的拒绝或失败,立即标记该次移动失败,不再靠后续位置恰好相同判定到位。其他命令的结果不会取消当前计划;界面继续使用已有的移动失败处理和停止路径。
|
移动器按命令 token 等待工作线程确认成功,然后才用随后收到的位置快照判断到位或推进下一轴;等待期间仍检查报警及读取失败。提前收到的位置快照不缓存重放。对应命令被拒绝或发生异常时立即标记失败,其他命令的结果不解除等待;界面继续使用已有的移动失败处理和停止路径。
|
||||||
|
|
||||||
执行前增加的是只读参数检查,可能增加通信延迟,不保证与旧版完全相同的发送时刻。控制器被其他软件在最终读取和运动指令之间改动的并发窗口,本次也不能消除;操作期间仍需保证设备配置的单一写入者。
|
执行前增加的是只读参数检查,可能增加通信延迟,不保证与旧版完全相同的发送时刻。控制器被其他软件在最终读取和运动指令之间改动的并发窗口,本次也不能消除;操作期间仍需保证设备配置的单一写入者。
|
||||||
|
|
||||||
@@ -29,8 +29,8 @@
|
|||||||
|
|
||||||
## 验证与限制
|
## 验证与限制
|
||||||
|
|
||||||
回归测试见 [`test_motion_unit_guards.py`](../tests/test_motion_unit_guards.py)。它覆盖既有五轴命令的确定数值和顺序、配置一致的线性换算、异常读回、旋转标定条件、排队期间配置改变、读取失败时整组零发送,以及原始单位调试移动的兼容性。现有测试夹具补充了真实流程所需的单位观测,没有降低断言或放宽限位。
|
回归测试见 [`test_motion_unit_guards.py`](../tests/test_motion_unit_guards.py)。它覆盖既有五轴命令的确定数值和顺序、配置一致的线性换算、异常读回、旋转标定条件、排队期间配置改变、读取失败时整组零发送,以及原始单位调试移动的兼容性。另覆盖分步/整组的快照先于命令结果、拒绝及异常、token 匹配、逐轴确认与等待期间故障。现有测试夹具补充真实流程所需的单位观测和成功回调,没有降低断言或放宽限位。
|
||||||
|
|
||||||
修改前从本地 design 导出保存 1032 顶点,使用默认安装与当前偏好配置,共 2064 组姿态和规划目标。修改后逐元素严格相等,最大差值为 0;使用修改前源码与当前源码在记录型 DemoTransport 上分别执行相同计划,产生的 8256 条绝对运动调用也完全一致。这验证软件输出保持,不构成实机轨迹或物理接触精度证明。
|
修改前从本地 design 导出保存 1032 顶点,使用默认安装与当前偏好配置,共 2064 组姿态和规划目标。修改后逐元素严格相等,最大差值为 0;使用修改前源码与当前源码在记录型 DemoTransport 上分别执行相同计划,产生的 8256 条绝对运动调用也完全一致。这验证软件输出保持,不构成实机轨迹或物理接触精度证明。
|
||||||
|
|
||||||
最终相关检查 136 项通过;Autocal 全套为 720 项通过、3 项失败,无跳过。两项模拟采集失败及一项旧 scratch 路径假设与原始审查记录一致,本轮没有修复。回归测试在修复前复现单位风险,移除当量一致性判断的内存变异也被检出。详细结果见[架构审查](../../../../docs/workflow/autocal-architecture-review.zh.md)。证据位于 app `.scratch/autocal-unit-guards/`,不属于长期归档。
|
首次单位防护验证为 136 项通过,Autocal 全套为 720 项通过、3 项失败。后续审查发现位置快照可能先于命令结果,提前误报成功;2026-10-18 增加上述确认门槛后,相关检查 164 项通过,原实现被新增测试检出 7 项失败。全套结果及限制见[架构审查](../../../../docs/workflow/autocal-architecture-review.zh.md)。证据位于 app `.scratch/autocal-unit-guards/` 与 `.scratch/autocal-command-ack/`,不属于长期归档。
|
||||||
|
|||||||
@@ -31,11 +31,10 @@ import time
|
|||||||
from collections.abc import Mapping, Sequence
|
from collections.abc import Mapping, Sequence
|
||||||
from dataclasses import dataclass, field
|
from dataclasses import dataclass, field
|
||||||
|
|
||||||
from PySide6.QtCore import QObject, Signal
|
from PySide6.QtCore import QObject, Signal, Slot
|
||||||
|
|
||||||
from .models import MotionResult, Snapshot
|
from .models import MotionResult, Snapshot
|
||||||
from .service import LINEAR_ROLES, MotionService
|
from .service import LINEAR_ROLES, MotionService
|
||||||
from .units import user_units_to_deg
|
|
||||||
|
|
||||||
#: How close an axis must be to its target before the next one starts, per unit:
|
#: How close an axis must be to its target before the next one starts, per unit:
|
||||||
#: millimetres for the linear roles, degrees for the rotary ones. One user unit is
|
#: millimetres for the linear roles, degrees for the rotary ones. One user unit is
|
||||||
@@ -141,6 +140,33 @@ class MoveStep:
|
|||||||
return f"{self.role} {self.target:+.4f} {self.unit}"
|
return f"{self.role} {self.target:+.4f} {self.unit}"
|
||||||
|
|
||||||
|
|
||||||
|
def _check_steps(
|
||||||
|
service: MotionService, steps: Sequence[MoveStep], *, refresh_units: bool = False
|
||||||
|
) -> MotionResult | None:
|
||||||
|
"""Reject stale axis/scale bindings; never reinterpret an existing plan."""
|
||||||
|
refusal = service.check_axes_move(
|
||||||
|
{step.role: step.target for step in steps}, refresh_units=refresh_units
|
||||||
|
)
|
||||||
|
if refusal is not None:
|
||||||
|
return refusal
|
||||||
|
for step in steps:
|
||||||
|
if step.axis != service.role_axis(step.role) or step.units != service.to_units(
|
||||||
|
step.role, step.target
|
||||||
|
):
|
||||||
|
return MotionResult.refused(
|
||||||
|
f"{step.role} 的轴映射或单位已改变,请重新规划 / axis mapping or units changed; replan"
|
||||||
|
)
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def _execute_steps(service: MotionService, steps: Sequence[MoveStep]) -> MotionResult:
|
||||||
|
"""Worker-only read check, then the original absolute targets in their order."""
|
||||||
|
refusal = _check_steps(service, steps, refresh_units=True)
|
||||||
|
if refusal is not None:
|
||||||
|
return refusal
|
||||||
|
return service.move_abs_many(tuple((step.axis, step.units) for step in steps))
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class MovePlan:
|
class MovePlan:
|
||||||
"""A sequence of steps, and what it is for."""
|
"""A sequence of steps, and what it is for."""
|
||||||
@@ -254,6 +280,7 @@ class PoseMover(QObject):
|
|||||||
self._service = service
|
self._service = service
|
||||||
self._submit = submit
|
self._submit = submit
|
||||||
self.state = MoveState()
|
self.state = MoveState()
|
||||||
|
self._command_token: int | None = None
|
||||||
|
|
||||||
# ------------------------------------------------------------------ control
|
# ------------------------------------------------------------------ control
|
||||||
|
|
||||||
@@ -271,9 +298,7 @@ class PoseMover(QObject):
|
|||||||
"""Validate and begin. Refusal is returned, not raised."""
|
"""Validate and begin. Refusal is returned, not raised."""
|
||||||
if self.running:
|
if self.running:
|
||||||
return MotionResult.refused("已有运动在执行 / a move is already running")
|
return MotionResult.refused("已有运动在执行 / a move is already running")
|
||||||
refusal = self._service.check_axes_move(
|
refusal = _check_steps(self._service, plan.steps)
|
||||||
{step.role: step.target for step in plan.steps}
|
|
||||||
)
|
|
||||||
if refusal is not None:
|
if refusal is not None:
|
||||||
return refusal
|
return refusal
|
||||||
if not plan.steps:
|
if not plan.steps:
|
||||||
@@ -312,19 +337,21 @@ class PoseMover(QObject):
|
|||||||
if not step.units == step.units: # NaN: no calibration for this role
|
if not step.units == step.units: # NaN: no calibration for this role
|
||||||
self._fail(f"{step.role} 无法换算成用户单位 / cannot convert {step.role}")
|
self._fail(f"{step.role} 无法换算成用户单位 / cannot convert {step.role}")
|
||||||
return
|
return
|
||||||
self._submit(lambda service, step=step: service.move_abs(step.axis, step.units))
|
self._command_token = self._submit(
|
||||||
|
lambda service, step=step: _execute_steps(service, (step,))
|
||||||
|
)
|
||||||
self.stepStarted.emit(step)
|
self.stepStarted.emit(step)
|
||||||
|
|
||||||
def _issue_together(self, plan: MovePlan) -> None:
|
def _issue_together(self, plan: MovePlan) -> None:
|
||||||
"""Command every step in one worker call."""
|
"""Command every step in one worker call."""
|
||||||
moves: list[tuple[int, float]] = []
|
|
||||||
for step in plan.steps:
|
for step in plan.steps:
|
||||||
if not step.units == step.units:
|
if not step.units == step.units:
|
||||||
self._fail(f"{step.role} 无法换算成用户单位 / cannot convert {step.role}")
|
self._fail(f"{step.role} 无法换算成用户单位 / cannot convert {step.role}")
|
||||||
return
|
return
|
||||||
moves.append((step.axis, step.units))
|
|
||||||
self.state._group_idle = {}
|
self.state._group_idle = {}
|
||||||
self._submit(lambda service, moves=tuple(moves): service.move_abs_many(moves))
|
self._command_token = self._submit(
|
||||||
|
lambda service, steps=plan.steps: _execute_steps(service, steps)
|
||||||
|
)
|
||||||
if plan.steps:
|
if plan.steps:
|
||||||
self.stepStarted.emit(plan.steps[0])
|
self.stepStarted.emit(plan.steps[0])
|
||||||
|
|
||||||
@@ -339,6 +366,20 @@ class PoseMover(QObject):
|
|||||||
self.state.plan = None
|
self.state.plan = None
|
||||||
self.failed.emit(message)
|
self.failed.emit(message)
|
||||||
|
|
||||||
|
@Slot(int, object)
|
||||||
|
def on_command_finished(self, token: int, result: object) -> None:
|
||||||
|
"""A rejected queued move must not later be reported as an arrival."""
|
||||||
|
if self.running and token == self._command_token:
|
||||||
|
self._command_token = None
|
||||||
|
if isinstance(result, MotionResult) and not result.ok:
|
||||||
|
self._fail(result.message)
|
||||||
|
|
||||||
|
@Slot(int, str)
|
||||||
|
def on_command_failed(self, token: int, message: str) -> None:
|
||||||
|
if self.running and token == self._command_token:
|
||||||
|
self._command_token = None
|
||||||
|
self._fail(message)
|
||||||
|
|
||||||
# ---------------------------------------------------------------- progress
|
# ---------------------------------------------------------------- progress
|
||||||
|
|
||||||
def on_snapshot(self, snapshot: Snapshot) -> None:
|
def on_snapshot(self, snapshot: Snapshot) -> None:
|
||||||
@@ -364,12 +405,11 @@ class PoseMover(QObject):
|
|||||||
self._fail(f"{step.role} 报警 / alarm on {step.role}")
|
self._fail(f"{step.role} 报警 / alarm on {step.role}")
|
||||||
return
|
return
|
||||||
|
|
||||||
# A linear role reads in millimetres, a rotary one in degrees; the service
|
# Arrival uses the same verified units as the planned physical target.
|
||||||
# converts lengths, the units module converts calibrated angles.
|
|
||||||
if step.unit == "mm":
|
if step.unit == "mm":
|
||||||
travelled = self._service.to_mm(step.axis, state.dpos)
|
travelled = self._service.to_mm(step.axis, state.dpos)
|
||||||
else:
|
else:
|
||||||
travelled = user_units_to_deg(step.axis, state.dpos)
|
travelled = self._service.to_deg(step.axis, state.dpos)
|
||||||
tolerance = SETTLE_MM if step.unit == "mm" else SETTLE_DEG
|
tolerance = SETTLE_MM if step.unit == "mm" else SETTLE_DEG
|
||||||
|
|
||||||
if not state.idle:
|
if not state.idle:
|
||||||
@@ -410,7 +450,7 @@ class PoseMover(QObject):
|
|||||||
if step.unit == "mm":
|
if step.unit == "mm":
|
||||||
travelled = self._service.to_mm(step.axis, state.dpos)
|
travelled = self._service.to_mm(step.axis, state.dpos)
|
||||||
else:
|
else:
|
||||||
travelled = user_units_to_deg(step.axis, state.dpos)
|
travelled = self._service.to_deg(step.axis, state.dpos)
|
||||||
tolerance = SETTLE_MM if step.unit == "mm" else SETTLE_DEG
|
tolerance = SETTLE_MM if step.unit == "mm" else SETTLE_DEG
|
||||||
arrived = state.idle and abs(travelled - step.target) <= tolerance
|
arrived = state.idle and abs(travelled - step.target) <= tolerance
|
||||||
if arrived:
|
if arrived:
|
||||||
|
|||||||
@@ -43,6 +43,7 @@ from .units import (
|
|||||||
mm_to_user_units,
|
mm_to_user_units,
|
||||||
safe_float,
|
safe_float,
|
||||||
units_per_degree,
|
units_per_degree,
|
||||||
|
user_units_to_deg,
|
||||||
user_units_to_mm,
|
user_units_to_mm,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -175,6 +176,7 @@ class MotionService:
|
|||||||
self._limits: dict[str, float] = dict(limits or {})
|
self._limits: dict[str, float] = dict(limits or {})
|
||||||
|
|
||||||
self._spec: ConnectionSpec | None = None
|
self._spec: ConnectionSpec | None = None
|
||||||
|
self._last_axes: dict[int, AxisState] = {}
|
||||||
self._bus = BusStatus()
|
self._bus = BusStatus()
|
||||||
self._params: dict[int, AxisParams] = {
|
self._params: dict[int, AxisParams] = {
|
||||||
axis: AxisParams() for axis in range(self.axis_count)
|
axis: AxisParams() for axis in range(self.axis_count)
|
||||||
@@ -273,25 +275,62 @@ class MotionService:
|
|||||||
key = "max_travel_mm" if role in LINEAR_ROLES else f"max_{role}_deg"
|
key = "max_travel_mm" if role in LINEAR_ROLES else f"max_{role}_deg"
|
||||||
return safe_float(self._limits.get(key), 0.0)
|
return safe_float(self._limits.get(key), 0.0)
|
||||||
|
|
||||||
def units_problem(self, role: str, axis: int) -> str:
|
def units_problem(self, role: str, axis: int, *, refresh: bool = False) -> str:
|
||||||
"""Why a role cannot be commanded in its own unit, or ``""``."""
|
"""Validate expected versus observed units, without changing either.
|
||||||
if role in LINEAR_ROLES:
|
|
||||||
return ""
|
``refresh`` is only for the controller worker immediately before execution.
|
||||||
scale = units_per_degree(axis)
|
UI/planning checks use the last poll and never call the transport.
|
||||||
if not math.isfinite(scale) or scale <= 0:
|
"""
|
||||||
|
expected = self.params(axis).units
|
||||||
|
if not math.isfinite(expected) or expected <= 0:
|
||||||
return self._text(
|
return self._text(
|
||||||
f"{role} 用轴 {axis},但该轴没有角度标定(用户单位/度)",
|
f"轴 {axis} 的 UNITS 无效", f"Axis {axis} has invalid expected UNITS"
|
||||||
f"{role} uses axis {axis}, which has no user-units-per-degree "
|
)
|
||||||
f"calibration",
|
if role not in LINEAR_ROLES and not math.isfinite(
|
||||||
|
units_per_degree(axis, units=expected)
|
||||||
|
):
|
||||||
|
return self._text(
|
||||||
|
f"{role} 用轴 {axis},但该轴没有当前 UNITS={expected:g} 对应的角度标定",
|
||||||
|
f"{role} uses axis {axis}, which has no rotary calibration for UNITS={expected:g}",
|
||||||
|
)
|
||||||
|
if refresh:
|
||||||
|
try:
|
||||||
|
observed = self._transport.get_units(axis)
|
||||||
|
except (ZAuxError, OSError) as exc:
|
||||||
|
return self._text(
|
||||||
|
f"无法读取轴 {axis} 的 UNITS:{exc}",
|
||||||
|
f"Cannot read axis {axis} UNITS: {exc}",
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
state = self._last_axes.get(axis)
|
||||||
|
if state is None or not state.read_ok:
|
||||||
|
return self._text(
|
||||||
|
f"轴 {axis} 的 UNITS 尚未成功读回",
|
||||||
|
f"Axis {axis} UNITS has no successful readback",
|
||||||
|
)
|
||||||
|
observed = state.units
|
||||||
|
if (
|
||||||
|
not math.isfinite(observed)
|
||||||
|
or observed <= 0
|
||||||
|
or not math.isclose(expected, observed, rel_tol=1e-7, abs_tol=0.0)
|
||||||
|
):
|
||||||
|
return self._text(
|
||||||
|
f"轴 {axis} 的 UNITS 不一致:配置 {expected:g},读回 {observed:g};未重新换算运动目标",
|
||||||
|
f"Axis {axis} UNITS mismatch: expected {expected:g}, read {observed:g}; motion was not rescaled",
|
||||||
)
|
)
|
||||||
return ""
|
return ""
|
||||||
|
|
||||||
def check_axes_move(self, targets: Mapping[str, float]) -> MotionResult | None:
|
def check_axes_move(
|
||||||
|
self, targets: Mapping[str, float], *, refresh_units: bool = False
|
||||||
|
) -> MotionResult | None:
|
||||||
"""Refuse a coordinated pose move, or return ``None`` to allow it.
|
"""Refuse a coordinated pose move, or return ``None`` to allow it.
|
||||||
|
|
||||||
``targets`` is per role, in the role's own unit: millimetres for x / y / z,
|
``targets`` is per role, in the role's own unit: millimetres for x / y / z,
|
||||||
degrees for yaw / pitch.
|
degrees for yaw / pitch.
|
||||||
|
|
||||||
|
``refresh_units`` reads the current pulse equivalents and is worker-only.
|
||||||
|
It never writes them or rescales any already planned target.
|
||||||
|
|
||||||
Every reason a move can be wrong is checked *before* the first axis is
|
Every reason a move can be wrong is checked *before* the first axis is
|
||||||
commanded. A pose move is several coordinated moves, and discovering at axis
|
commanded. A pose move is several coordinated moves, and discovering at axis
|
||||||
three that axis four is out of range would leave the machine stopped
|
three that axis four is out of range would leave the machine stopped
|
||||||
@@ -321,7 +360,7 @@ class MotionService:
|
|||||||
self._text(f"{role} 目标不是有效数字", f"{role} target is not a number")
|
self._text(f"{role} 目标不是有效数字", f"{role} target is not a number")
|
||||||
)
|
)
|
||||||
axis = self._axis_roles[role]
|
axis = self._axis_roles[role]
|
||||||
units_problem = self.units_problem(role, axis)
|
units_problem = self.units_problem(role, axis, refresh=refresh_units)
|
||||||
if units_problem:
|
if units_problem:
|
||||||
return MotionResult.refused(units_problem)
|
return MotionResult.refused(units_problem)
|
||||||
for role, value in targets.items():
|
for role, value in targets.items():
|
||||||
@@ -350,11 +389,11 @@ class MotionService:
|
|||||||
if axis is None:
|
if axis is None:
|
||||||
return math.nan
|
return math.nan
|
||||||
if role in LINEAR_ROLES:
|
if role in LINEAR_ROLES:
|
||||||
units = self.params(axis).units or self._units_per_mm
|
units = self.params(axis).units
|
||||||
return mm_to_user_units(
|
return mm_to_user_units(
|
||||||
value, units=units, pulses_per_mm=self._pulses_per_mm
|
value, units=units, pulses_per_mm=self._pulses_per_mm
|
||||||
)
|
)
|
||||||
return deg_to_user_units(axis, value)
|
return deg_to_user_units(axis, value, units=self.params(axis).units)
|
||||||
|
|
||||||
def set_language(self, language: str) -> None:
|
def set_language(self, language: str) -> None:
|
||||||
self.language = language
|
self.language = language
|
||||||
@@ -399,6 +438,7 @@ class MotionService:
|
|||||||
return MotionResult.failed(code, message)
|
return MotionResult.failed(code, message)
|
||||||
|
|
||||||
self._spec = spec
|
self._spec = spec
|
||||||
|
self._last_axes.clear()
|
||||||
# A controller may already have a BAS program running from a previous
|
# A controller may already have a BAS program running from a previous
|
||||||
# session; adopt it rather than claiming the bus is uninitialised. The
|
# session; adopt it rather than claiming the bus is uninitialised. The
|
||||||
# adopted state is *not* marked verified: its globals are a latch, and
|
# adopted state is *not* marked verified: its globals are a latch, and
|
||||||
@@ -610,6 +650,7 @@ class MotionService:
|
|||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
|
self._last_axes.update({state.axis: state for state in states})
|
||||||
return Snapshot(
|
return Snapshot(
|
||||||
connected=True,
|
connected=True,
|
||||||
spec=self._spec,
|
spec=self._spec,
|
||||||
@@ -724,7 +765,6 @@ class MotionService:
|
|||||||
home_status = 0
|
home_status = 0
|
||||||
self._home_done[axis] = home_status == 1
|
self._home_done[axis] = home_status == 1
|
||||||
|
|
||||||
params = self.params(axis)
|
|
||||||
return AxisState(
|
return AxisState(
|
||||||
axis=axis,
|
axis=axis,
|
||||||
enabled=transport.get_axis_enable(axis) != 0,
|
enabled=transport.get_axis_enable(axis) != 0,
|
||||||
@@ -748,6 +788,8 @@ class MotionService:
|
|||||||
return MotionResult.refused(self._text("未连接控制器", "Not connected"))
|
return MotionResult.refused(self._text("未连接控制器", "Not connected"))
|
||||||
if not self._known_axis(axis):
|
if not self._known_axis(axis):
|
||||||
return MotionResult.refused(self._axis_range_message(axis))
|
return MotionResult.refused(self._axis_range_message(axis))
|
||||||
|
# A partial write may already change UNITS even if speed/accel then fails.
|
||||||
|
self._last_axes.pop(axis, None)
|
||||||
try:
|
try:
|
||||||
self._transport.set_units(axis, params.units)
|
self._transport.set_units(axis, params.units)
|
||||||
self._transport.set_speed(axis, params.speed)
|
self._transport.set_speed(axis, params.speed)
|
||||||
@@ -979,7 +1021,7 @@ class MotionService:
|
|||||||
return MotionResult.success(self._text("回零已启动", "Homing started"))
|
return MotionResult.success(self._text("回零已启动", "Homing started"))
|
||||||
|
|
||||||
def _log_axis_readback(self, action: str) -> None:
|
def _log_axis_readback(self, action: str) -> None:
|
||||||
"""Read every axis and log it. Does not write parameters or start motion."""
|
"""Log the last poll without issuing extra controller reads."""
|
||||||
for axis in range(self.axis_count):
|
for axis in range(self.axis_count):
|
||||||
self._log_command(action, axis, MotionResult.success("read"))
|
self._log_command(action, axis, MotionResult.success("read"))
|
||||||
|
|
||||||
@@ -1000,23 +1042,17 @@ class MotionService:
|
|||||||
def _position_text(self, axis: int) -> str:
|
def _position_text(self, axis: int) -> str:
|
||||||
"""Commanded and measured position, for the command log.
|
"""Commanded and measured position, for the command log.
|
||||||
|
|
||||||
Reads only. A failed read is part of the line rather than a second error,
|
These are cached poll values, not a synchronous post-command read. Logging
|
||||||
because the command result itself is what the caller is waiting on.
|
must not access the controller from the calling thread.
|
||||||
"""
|
"""
|
||||||
if not self.connected:
|
state = self._last_axes.get(axis)
|
||||||
|
if not self.connected or state is None or not state.read_ok:
|
||||||
return "dpos=unread mpos=unread status=unread enable=unread"
|
return "dpos=unread mpos=unread status=unread enable=unread"
|
||||||
try:
|
|
||||||
return (
|
return (
|
||||||
f"dpos={self._transport.get_dpos(axis)} "
|
f"last_poll dpos={state.dpos} mpos={state.mpos} "
|
||||||
f"mpos={self._transport.get_mpos(axis)} "
|
f"status={state.status.raw} enable={state.enabled} "
|
||||||
f"status={self._transport.get_axis_status(axis)} "
|
f"atype={state.atype} units={state.units} speed={state.speed}"
|
||||||
f"enable={self._transport.get_axis_enable(axis)} "
|
|
||||||
f"atype={self._transport.get_atype(axis)} "
|
|
||||||
f"units={self._transport.get_units(axis)} "
|
|
||||||
f"speed={self._transport.get_speed(axis)}"
|
|
||||||
)
|
)
|
||||||
except (ZAuxError, OSError) as exc:
|
|
||||||
return f"dpos=unread mpos=unread ({exc})"
|
|
||||||
|
|
||||||
def _known_axis(self, axis: int) -> bool:
|
def _known_axis(self, axis: int) -> bool:
|
||||||
return 0 <= int(axis) < self.axis_count
|
return 0 <= int(axis) < self.axis_count
|
||||||
@@ -1099,11 +1135,19 @@ class MotionService:
|
|||||||
|
|
||||||
def to_mm(self, axis: int, user_units: float) -> float:
|
def to_mm(self, axis: int, user_units: float) -> float:
|
||||||
"""Displayed millimetres for a reading. Derived, never a measurement."""
|
"""Displayed millimetres for a reading. Derived, never a measurement."""
|
||||||
units = self.params(axis).units or self._units_per_mm
|
if self.units_problem("x", axis):
|
||||||
|
return math.nan
|
||||||
|
units = self.params(axis).units
|
||||||
return user_units_to_mm(
|
return user_units_to_mm(
|
||||||
user_units, units=units, pulses_per_mm=self._pulses_per_mm
|
user_units, units=units, pulses_per_mm=self._pulses_per_mm
|
||||||
)
|
)
|
||||||
|
|
||||||
|
def to_deg(self, axis: int, user_units: float) -> float:
|
||||||
|
"""Rotary readback in degrees, or NaN without matching calibrated units."""
|
||||||
|
if self.units_problem("yaw", axis):
|
||||||
|
return math.nan
|
||||||
|
return user_units_to_deg(axis, user_units, units=self.params(axis).units)
|
||||||
|
|
||||||
|
|
||||||
def _replace_bus_type(bus: BusStatus, bus_type: BusType) -> BusStatus:
|
def _replace_bus_type(bus: BusStatus, bus_type: BusType) -> BusStatus:
|
||||||
return BusStatus(
|
return BusStatus(
|
||||||
|
|||||||
@@ -34,6 +34,10 @@ ROTARY_UNITS_PER_90_DEG: dict[int, float] = {
|
|||||||
3: 29500.0,
|
3: 29500.0,
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#: Pulse equivalent at which both rotary user-unit scales were measured.
|
||||||
|
#: Other equivalents require a new calibration; do not rescale commissioned moves.
|
||||||
|
ROTARY_CALIBRATION_UNITS = 10.0
|
||||||
|
|
||||||
|
|
||||||
def safe_float(text: object, fallback: float) -> float:
|
def safe_float(text: object, fallback: float) -> float:
|
||||||
"""Parse ``text`` as a finite float, falling back on anything unusable.
|
"""Parse ``text`` as a finite float, falling back on anything unusable.
|
||||||
@@ -80,25 +84,33 @@ def mm_to_user_units(
|
|||||||
return float(value) * float(pulses_per_mm) / float(units)
|
return float(value) * float(pulses_per_mm) / float(units)
|
||||||
|
|
||||||
|
|
||||||
def units_per_degree(axis: int) -> float:
|
def units_per_degree(axis: int, *, units: float = ROTARY_CALIBRATION_UNITS) -> float:
|
||||||
"""User units per degree for a rotary axis, or NaN if not calibrated."""
|
"""User units per degree for a rotary axis, or NaN if not calibrated."""
|
||||||
per_90 = ROTARY_UNITS_PER_90_DEG.get(int(axis))
|
per_90 = ROTARY_UNITS_PER_90_DEG.get(int(axis))
|
||||||
if per_90 is None or per_90 <= 0:
|
if (
|
||||||
|
per_90 is None
|
||||||
|
or per_90 <= 0
|
||||||
|
or not math.isclose(units, ROTARY_CALIBRATION_UNITS, rel_tol=1e-7, abs_tol=0.0)
|
||||||
|
):
|
||||||
return math.nan
|
return math.nan
|
||||||
return float(per_90) / 90.0
|
return float(per_90) / 90.0
|
||||||
|
|
||||||
|
|
||||||
def user_units_to_deg(axis: int, value: float) -> float:
|
def user_units_to_deg(
|
||||||
|
axis: int, value: float, *, units: float = ROTARY_CALIBRATION_UNITS
|
||||||
|
) -> float:
|
||||||
"""Convert controller user units to degrees for a calibrated rotary axis."""
|
"""Convert controller user units to degrees for a calibrated rotary axis."""
|
||||||
scale = units_per_degree(axis)
|
scale = units_per_degree(axis, units=units)
|
||||||
if not math.isfinite(scale) or scale == 0.0:
|
if not math.isfinite(scale) or scale == 0.0:
|
||||||
return math.nan
|
return math.nan
|
||||||
return float(value) / scale
|
return float(value) / scale
|
||||||
|
|
||||||
|
|
||||||
def deg_to_user_units(axis: int, degrees: float) -> float:
|
def deg_to_user_units(
|
||||||
|
axis: int, degrees: float, *, units: float = ROTARY_CALIBRATION_UNITS
|
||||||
|
) -> float:
|
||||||
"""Inverse of :func:`user_units_to_deg`."""
|
"""Inverse of :func:`user_units_to_deg`."""
|
||||||
scale = units_per_degree(axis)
|
scale = units_per_degree(axis, units=units)
|
||||||
if not math.isfinite(scale):
|
if not math.isfinite(scale):
|
||||||
return math.nan
|
return math.nan
|
||||||
return float(degrees) * scale
|
return float(degrees) * scale
|
||||||
|
|||||||
@@ -857,6 +857,8 @@ class AutoCalMainWindow(QMainWindow):
|
|||||||
# hold the worker thread and a stop would have to wait for the move it is
|
# hold the worker thread and a stop would have to wait for the move it is
|
||||||
# meant to interrupt.
|
# meant to interrupt.
|
||||||
self.mover = PoseMover(self.poller.service, self.poller.submit, self)
|
self.mover = PoseMover(self.poller.service, self.poller.submit, self)
|
||||||
|
self.poller.commandFinished.connect(self.mover.on_command_finished)
|
||||||
|
self.poller.commandFailed.connect(self.mover.on_command_failed)
|
||||||
self.mover.failed.connect(self._on_move_failed)
|
self.mover.failed.connect(self._on_move_failed)
|
||||||
self.mover.finished.connect(self._on_move_finished)
|
self.mover.finished.connect(self._on_move_finished)
|
||||||
self.mover.cancelled.connect(self._on_move_cancelled)
|
self.mover.cancelled.connect(self._on_move_cancelled)
|
||||||
@@ -912,7 +914,7 @@ class AutoCalMainWindow(QMainWindow):
|
|||||||
if state is None or not state.read_ok:
|
if state is None or not state.read_ok:
|
||||||
return None
|
return None
|
||||||
if rotary:
|
if rotary:
|
||||||
return user_units_to_deg(int(axis), state.dpos)
|
return user_units_to_deg(int(axis), state.dpos, units=state.units)
|
||||||
units = state.units or float(self.prefs.get("units_per_mm") or DEFAULT_UNITS)
|
units = state.units or float(self.prefs.get("units_per_mm") or DEFAULT_UNITS)
|
||||||
return user_units_to_mm(state.dpos, units=units)
|
return user_units_to_mm(state.dpos, units=units)
|
||||||
|
|
||||||
|
|||||||
@@ -5,7 +5,15 @@ import math
|
|||||||
|
|
||||||
import pytest
|
import pytest
|
||||||
|
|
||||||
from orisys_autocal.motion.models import AxisParams, BusStatus, ConnectionSpec
|
from orisys_autocal.motion.models import (
|
||||||
|
AxisParams,
|
||||||
|
AxisState,
|
||||||
|
AxisStatusBits,
|
||||||
|
BusStatus,
|
||||||
|
ConnectionSpec,
|
||||||
|
MotionResult,
|
||||||
|
Snapshot,
|
||||||
|
)
|
||||||
from orisys_autocal.motion.pose_move import PoseMover, plan_pose_move
|
from orisys_autocal.motion.pose_move import PoseMover, plan_pose_move
|
||||||
from orisys_autocal.motion.service import MotionService
|
from orisys_autocal.motion.service import MotionService
|
||||||
from orisys_autocal.motion.transport import DemoTransport
|
from orisys_autocal.motion.transport import DemoTransport
|
||||||
@@ -185,3 +193,86 @@ def test_raw_debug_moves_keep_the_existing_user_unit_contract():
|
|||||||
transport.set_units(2, 5.0)
|
transport.set_units(2, 5.0)
|
||||||
assert instance.move_abs(2, 90.0).ok
|
assert instance.move_abs(2, 90.0).ok
|
||||||
assert transport.moves == [(2, 90.0)]
|
assert transport.moves == [(2, 90.0)]
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize("together", [False, True])
|
||||||
|
@pytest.mark.parametrize("failure", ["refused", "exception"])
|
||||||
|
def test_arrival_before_command_failure_never_reports_success(qt_app, together, failure):
|
||||||
|
instance, transport = service()
|
||||||
|
actions = []
|
||||||
|
mover = PoseMover(instance, lambda action: actions.append(action) or len(actions))
|
||||||
|
finished = []
|
||||||
|
mover.finished.connect(finished.append)
|
||||||
|
assert mover.start(plan_pose_move(instance, {"x": 0.0}, together=together)).ok
|
||||||
|
mover.on_snapshot(Snapshot(axes=(instance._last_axes[0],)))
|
||||||
|
assert not finished and mover.running
|
||||||
|
|
||||||
|
if failure == "refused":
|
||||||
|
transport.set_units(0, 5.0)
|
||||||
|
result = actions[0](instance)
|
||||||
|
assert not result.ok and "UNITS" in result.message
|
||||||
|
mover.on_command_finished(1, result)
|
||||||
|
else:
|
||||||
|
mover.on_command_failed(1, "read failed")
|
||||||
|
assert mover.state.failure and not mover.state.finished
|
||||||
|
assert not finished and not mover.running
|
||||||
|
assert transport.moves == []
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize("together", [False, True])
|
||||||
|
def test_only_matching_success_and_a_later_snapshot_allow_arrival(qt_app, together):
|
||||||
|
instance, _transport = service()
|
||||||
|
actions = []
|
||||||
|
mover = PoseMover(instance, lambda action: actions.append(action) or len(actions))
|
||||||
|
assert mover.start(plan_pose_move(instance, {"x": 0.0}, together=together)).ok
|
||||||
|
snapshot = Snapshot(axes=(instance._last_axes[0],))
|
||||||
|
mover.on_command_finished(99, MotionResult.success())
|
||||||
|
mover.on_snapshot(snapshot)
|
||||||
|
assert mover.running and not mover.state.finished
|
||||||
|
result = actions[0](instance)
|
||||||
|
assert result.ok
|
||||||
|
mover.on_command_finished(1, result)
|
||||||
|
assert mover.running, "a pre-acknowledgement snapshot was reused"
|
||||||
|
mover.on_snapshot(Snapshot(axes=(instance._last_axes[0],)))
|
||||||
|
assert mover.state.finished and not mover.running
|
||||||
|
|
||||||
|
|
||||||
|
def test_each_sequential_axis_waits_for_its_own_command_success(qt_app):
|
||||||
|
instance, _transport = service()
|
||||||
|
actions = []
|
||||||
|
mover = PoseMover(instance, lambda action: actions.append(action) or len(actions))
|
||||||
|
assert mover.start(plan_pose_move(instance, {"x": 0.0, "y": 0.0})).ok
|
||||||
|
snapshot = Snapshot(axes=tuple(instance._last_axes[axis] for axis in (0, 1)))
|
||||||
|
mover.on_snapshot(snapshot)
|
||||||
|
assert len(actions) == 1 and mover.state.index == 0
|
||||||
|
result = actions[0](instance)
|
||||||
|
assert result.ok
|
||||||
|
mover.on_command_finished(1, result)
|
||||||
|
mover.on_snapshot(snapshot)
|
||||||
|
assert len(actions) == 2 and mover.state.index == 1
|
||||||
|
mover.on_command_finished(1, MotionResult.success())
|
||||||
|
mover.on_snapshot(snapshot)
|
||||||
|
assert mover.running and not mover.state.finished
|
||||||
|
result = actions[1](instance)
|
||||||
|
assert result.ok
|
||||||
|
mover.on_command_finished(2, result)
|
||||||
|
mover.on_snapshot(snapshot)
|
||||||
|
assert mover.state.finished
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize("together", [False, True])
|
||||||
|
@pytest.mark.parametrize("fault", ["alarm", "read"])
|
||||||
|
def test_axis_faults_remain_visible_while_command_is_pending(qt_app, together, fault):
|
||||||
|
instance, _transport = service()
|
||||||
|
mover = PoseMover(instance, lambda _action: 1)
|
||||||
|
targets = {"x": 0.0, "y": 0.0} if together else {"y": 0.0}
|
||||||
|
assert mover.start(plan_pose_move(instance, targets, together=together)).ok
|
||||||
|
bad_axis = AxisState(
|
||||||
|
axis=1,
|
||||||
|
units=10.0,
|
||||||
|
status=AxisStatusBits(raw=512 if fault == "alarm" else 0),
|
||||||
|
read_ok=fault != "read",
|
||||||
|
)
|
||||||
|
mover.on_snapshot(Snapshot(axes=(AxisState(axis=0, units=10.0), bad_axis)))
|
||||||
|
assert not mover.running and not mover.state.finished
|
||||||
|
assert ("alarm" if fault == "alarm" else "read failed") in mover.state.failure
|
||||||
|
|||||||
@@ -34,6 +34,8 @@ def _connected(make_window, *, confirm: bool = False):
|
|||||||
service = window.poller.service
|
service = window.poller.service
|
||||||
service._spec = object() # noqa: SLF001 - connected, without opening anything
|
service._spec = object() # noqa: SLF001 - connected, without opening anything
|
||||||
service._bus = BusStatus(bus_type="ecat", init_status=1, total_axes=5) # noqa: SLF001
|
service._bus = BusStatus(bus_type="ecat", init_status=1, total_axes=5) # noqa: SLF001
|
||||||
|
# A ready physical-unit move also has a successful unit readback.
|
||||||
|
service._last_axes = {axis: service._read_axis(axis) for axis in range(5)}
|
||||||
return window
|
return window
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -54,11 +54,13 @@ def make_service(**kwargs) -> MotionService:
|
|||||||
)
|
)
|
||||||
service._spec = object() # noqa: SLF001 - connected, without opening anything
|
service._spec = object() # noqa: SLF001 - connected, without opening anything
|
||||||
service._bus = BusStatus(bus_type="ecat", init_status=1, total_axes=5) # noqa: SLF001
|
service._bus = BusStatus(bus_type="ecat", init_status=1, total_axes=5) # noqa: SLF001
|
||||||
|
# Physical-unit planning requires observed units, just as the live poll supplies.
|
||||||
|
service._last_axes = {axis: service._read_axis(axis) for axis in range(5)}
|
||||||
return service
|
return service
|
||||||
|
|
||||||
|
|
||||||
class FakePoller:
|
class FakePoller:
|
||||||
"""Records submissions and runs them, so a plan's commands can be inspected."""
|
"""Records submissions and explicitly acknowledges simulated worker results."""
|
||||||
|
|
||||||
def __init__(self, service: MotionService) -> None:
|
def __init__(self, service: MotionService) -> None:
|
||||||
self.service = service
|
self.service = service
|
||||||
@@ -71,6 +73,9 @@ class FakePoller:
|
|||||||
# this test exists to check.
|
# this test exists to check.
|
||||||
return len(self.commands)
|
return len(self.commands)
|
||||||
|
|
||||||
|
def acknowledge(self, mover: PoseMover) -> None:
|
||||||
|
mover.on_command_finished(len(self.commands), MotionResult.success())
|
||||||
|
|
||||||
|
|
||||||
def reading(axis: int, dpos: float, *, idle: bool = True, alarm: bool = False) -> AxisState:
|
def reading(axis: int, dpos: float, *, idle: bool = True, alarm: bool = False) -> AxisState:
|
||||||
return AxisState(
|
return AxisState(
|
||||||
@@ -284,6 +289,7 @@ def test_an_execute_plan_moves_the_four_axes_together_and_leaves_z():
|
|||||||
assert len(poller.commands) == 1
|
assert len(poller.commands) == 1
|
||||||
poller.commands[0](service)
|
poller.commands[0](service)
|
||||||
assert [axis for axis, _value in called[0]] == [3, 2, 0, 1]
|
assert [axis for axis, _value in called[0]] == [3, 2, 0, 1]
|
||||||
|
poller.acknowledge(mover)
|
||||||
|
|
||||||
mover.on_snapshot(
|
mover.on_snapshot(
|
||||||
snapshot_of(service, {0: 1000.0, 1: 0.0, 2: 0.0, 3: 0.0}, idle=False)
|
snapshot_of(service, {0: 1000.0, 1: 0.0, 2: 0.0, 3: 0.0}, idle=False)
|
||||||
@@ -332,11 +338,13 @@ def test_the_mover_commands_one_axis_and_waits_for_it_to_arrive():
|
|||||||
plan = plan_pose_move(service, {"x": 2.0, "y": 1.0})
|
plan = plan_pose_move(service, {"x": 2.0, "y": 1.0})
|
||||||
assert mover.start(plan).ok
|
assert mover.start(plan).ok
|
||||||
assert len(poller.commands) == 1, "more than the first axis was commanded at once"
|
assert len(poller.commands) == 1, "more than the first axis was commanded at once"
|
||||||
|
poller.acknowledge(mover)
|
||||||
|
|
||||||
# x is at its target and idle -> the second axis goes out.
|
# x is at its target and idle -> the second axis goes out.
|
||||||
mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 0.0}))
|
mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 0.0}))
|
||||||
assert len(poller.commands) == 2
|
assert len(poller.commands) == 2
|
||||||
assert mover.state.step.role == "y"
|
assert mover.state.step.role == "y"
|
||||||
|
poller.acknowledge(mover)
|
||||||
|
|
||||||
# y arrives -> the plan is done.
|
# y arrives -> the plan is done.
|
||||||
mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 500.0}))
|
mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 500.0}))
|
||||||
@@ -349,6 +357,7 @@ def test_the_mover_waits_while_an_axis_is_still_moving():
|
|||||||
poller = FakePoller(service)
|
poller = FakePoller(service)
|
||||||
mover = PoseMover(service, poller.submit)
|
mover = PoseMover(service, poller.submit)
|
||||||
mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
|
mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
|
||||||
|
poller.acknowledge(mover)
|
||||||
mover.on_snapshot(snapshot_of(service, {0: 990.0, 1: 0.0}, idle=False))
|
mover.on_snapshot(snapshot_of(service, {0: 990.0, 1: 0.0}, idle=False))
|
||||||
assert len(poller.commands) == 1
|
assert len(poller.commands) == 1
|
||||||
assert mover.state.index == 0
|
assert mover.state.index == 0
|
||||||
@@ -370,6 +379,7 @@ def test_a_long_yaw_is_not_cut_off_while_it_is_still_moving(monkeypatch):
|
|||||||
poller = FakePoller(service)
|
poller = FakePoller(service)
|
||||||
mover = PoseMover(service, poller.submit)
|
mover = PoseMover(service, poller.submit)
|
||||||
assert mover.start(plan_pose_move(service, {"yaw": 180.0})).ok
|
assert mover.start(plan_pose_move(service, {"yaw": 180.0})).ok
|
||||||
|
poller.acknowledge(mover)
|
||||||
clock["t"] += 120.0
|
clock["t"] += 120.0
|
||||||
mover.on_snapshot(snapshot_of(service, {3: 1_000.0}, idle=False))
|
mover.on_snapshot(snapshot_of(service, {3: 1_000.0}, idle=False))
|
||||||
assert mover.running
|
assert mover.running
|
||||||
@@ -383,6 +393,7 @@ def test_the_mover_fails_when_an_axis_stops_short():
|
|||||||
poller = FakePoller(service)
|
poller = FakePoller(service)
|
||||||
mover = PoseMover(service, poller.submit)
|
mover = PoseMover(service, poller.submit)
|
||||||
mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
|
mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
|
||||||
|
poller.acknowledge(mover)
|
||||||
mover.on_snapshot(snapshot_of(service, {0: 10.0, 1: 0.0}))
|
mover.on_snapshot(snapshot_of(service, {0: 10.0, 1: 0.0}))
|
||||||
assert mover.running, "gave up before the stall grace elapsed"
|
assert mover.running, "gave up before the stall grace elapsed"
|
||||||
mover.state._idle_since = time.monotonic() - 10.0 # noqa: SLF001
|
mover.state._idle_since = time.monotonic() - 10.0 # noqa: SLF001
|
||||||
@@ -408,6 +419,7 @@ def test_a_tolerance_accepts_an_axis_that_reports_its_final_position_late():
|
|||||||
mover = PoseMover(service, poller.submit)
|
mover = PoseMover(service, poller.submit)
|
||||||
plan = plan_pose_move(service, {"x": 2.0, "y": 1.0})
|
plan = plan_pose_move(service, {"x": 2.0, "y": 1.0})
|
||||||
mover.start(plan)
|
mover.start(plan)
|
||||||
|
poller.acknowledge(mover)
|
||||||
# 1 unit inside SETTLE_MM of 1000 units for 2 mm.
|
# 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}))
|
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 len(poller.commands) == 2, "a step within tolerance was treated as a miss"
|
||||||
|
|||||||
Reference in New Issue
Block a user