diff --git a/src/dodal/beamlines/i05.py b/src/dodal/beamlines/i05.py index 511047ca1b..2ca7d00d06 100644 --- a/src/dodal/beamlines/i05.py +++ b/src/dodal/beamlines/i05.py @@ -1,19 +1,31 @@ from dodal.beamlines.i05_shared import devices as i05_shared_devices from dodal.common.beamlines.beamline_utils import set_beamline as set_utils_beamline from dodal.device_manager import DeviceManager +from dodal.devices.beamlines.i05 import I05Goniometer from dodal.devices.temperture_controller import Lakeshore336 from dodal.log import set_beamline as set_log_beamline from dodal.utils import BeamlinePrefix, get_beamline_name -devices = DeviceManager() -devices.include(i05_shared_devices) - BL = get_beamline_name("i05") PREFIX = BeamlinePrefix(BL) set_log_beamline(BL) set_utils_beamline(BL) +devices = DeviceManager() +devices.include(i05_shared_devices) + @devices.factory() def sample_temperature_controller() -> Lakeshore336: return Lakeshore336(prefix=f"{PREFIX.beamline_prefix}-EA-TCTRL-02:") + + +@devices.factory() +def sa() -> I05Goniometer: + """Sample Manipulator.""" + return I05Goniometer( + f"{PREFIX.beamline_prefix}-EA-SM-01:", + x_infix="SAX", + y_infix="SAY", + z_infix="SAZ", + ) diff --git a/src/dodal/beamlines/i05_1.py b/src/dodal/beamlines/i05_1.py index 6381d4b034..93842aaa5f 100644 --- a/src/dodal/beamlines/i05_1.py +++ b/src/dodal/beamlines/i05_1.py @@ -1,6 +1,7 @@ from dodal.beamlines.i05_shared import devices as i05_shared_devices from dodal.common.beamlines.beamline_utils import set_beamline as set_utils_beamline from dodal.device_manager import DeviceManager +from dodal.devices.beamlines.i05_1 import XYZPolarAzimuthDefocusStage from dodal.log import set_beamline as set_log_beamline from dodal.utils import BeamlinePrefix, get_beamline_name @@ -11,3 +12,9 @@ devices = DeviceManager() devices.include(i05_shared_devices) + + +@devices.factory +def sm() -> XYZPolarAzimuthDefocusStage: + """Sample Manipulator.""" + return XYZPolarAzimuthDefocusStage(prefix=f"{PREFIX.beamline_prefix}-EA-SM-01:") diff --git a/src/dodal/common/maths.py b/src/dodal/common/maths.py index ca9492136c..9b8ffa4a43 100644 --- a/src/dodal/common/maths.py +++ b/src/dodal/common/maths.py @@ -116,3 +116,45 @@ def contains(self, x: float, y: float) -> bool: self.get_min_x() <= x <= self.get_max_x() and self.get_min_y() <= y <= self.get_max_y() ) + + +""" +| Rotation | Formula for X_rot | Formula for Y_rot | +| -------- | ----------------- | ----------------- | +| CW | x cosθ + y sinθ | -x sinθ + y cosθ | +| CCW | x cosθ - y sinθ | x sinθ + y cosθ | +""" + + +def do_rotation(x: float, y: float, rotation_matrix: np.ndarray) -> tuple[float, float]: + positions = np.array([x, y]) + rotation = rotation_matrix @ positions + return float(rotation[0]), float(rotation[1]) + + +def rotate_clockwise( + theta: float, + x: float, + y: float, +) -> tuple[float, float]: + rotation_matrix = np.array( + [ + [np.cos(theta), np.sin(theta)], + [-np.sin(theta), np.cos(theta)], + ] + ) + return do_rotation(x, y, rotation_matrix) + + +def rotate_counter_clockwise( + theta: float, + x: float, + y: float, +) -> tuple[float, float]: + rotation_matrix = np.array( + [ + [np.cos(theta), -np.sin(theta)], + [np.sin(theta), np.cos(theta)], + ] + ) + return do_rotation(x, y, rotation_matrix) diff --git a/src/dodal/devices/beamlines/i05/__init__.py b/src/dodal/devices/beamlines/i05/__init__.py index 5bedbdc16e..b11147cc6d 100644 --- a/src/dodal/devices/beamlines/i05/__init__.py +++ b/src/dodal/devices/beamlines/i05/__init__.py @@ -1,3 +1,4 @@ -from dodal.devices.beamlines.i05.enums import Grating +from .enums import Grating +from .i05_motors import I05Goniometer -__all__ = ["Grating"] +__all__ = ["Grating", "I05Goniometer"] diff --git a/src/dodal/devices/beamlines/i05/i05_motors.py b/src/dodal/devices/beamlines/i05/i05_motors.py new file mode 100644 index 0000000000..9c68b6511e --- /dev/null +++ b/src/dodal/devices/beamlines/i05/i05_motors.py @@ -0,0 +1,69 @@ +from dodal.devices.beamlines.i05_shared.rotation_signals import ( + create_rotational_ij_component_signals, +) +from dodal.devices.motors import ( + _AZIMUTH, + _POLAR, + _TILT, + _X, + _Y, + _Z, + XYZPolarAzimuthTiltStage, +) + + +class I05Goniometer(XYZPolarAzimuthTiltStage): + """Six-axis stage with a standard xyz stage and three axis of rotation: polar, + azimuth, and tilt. + + In addition, it defines two virtual translational axes, `perp` and `long`, which + form a rotated Cartesian frame within the x-y plane. + + - `long`: Translation along the longitudinal direction of the rotated in-plane + coordinate frame defined by ``rotation_angle_deg``. + + - `perp`: Translation perpendicular to `long` within the x-y plane. + + The `perp` and `long` axes are derived from the underlying x and y motors using a + fixed rotation angle (default 50 degrees). From the user's point of view, these + behave as ordinary orthogonal Cartesian translation axes aligned with physically + meaningful directions on the sample, while internally coordinating motion of the x + and y motors. + + Unlike sample-frame axes that rotate with a live rotation motor, these axes are + defined at a constant orientation set by `rotation_angle_deg`. + """ + + def __init__( + self, + prefix: str, + x_infix: str = _X, + y_infix: str = _Y, + z_infix: str = _Z, + polar_infix: str = _POLAR, + azimuth_infix: str = _AZIMUTH, + tilt_infix: str = _TILT, + rotation_angle_deg: float = 50.0, + name: str = "", + ): + self.rotation_angle_deg = rotation_angle_deg + + super().__init__( + prefix, + name, + x_infix=x_infix, + y_infix=y_infix, + z_infix=z_infix, + polar_infix=polar_infix, + azimuth_infix=azimuth_infix, + tilt_infix=tilt_infix, + ) + + with self.add_children_as_readables(): + self.perp, self.long = create_rotational_ij_component_signals( + i_read=self.x.user_readback, + i_write=self.x, + j_read=self.y.user_readback, + j_write=self.y, + angle_deg=self.rotation_angle_deg, + ) diff --git a/src/dodal/devices/beamlines/i05_1/__init__.py b/src/dodal/devices/beamlines/i05_1/__init__.py new file mode 100644 index 0000000000..5cb81dab9d --- /dev/null +++ b/src/dodal/devices/beamlines/i05_1/__init__.py @@ -0,0 +1,3 @@ +from .i05_1_motors import XYZPolarAzimuthDefocusStage + +__all__ = ["XYZPolarAzimuthDefocusStage"] diff --git a/src/dodal/devices/beamlines/i05_1/i05_1_motors.py b/src/dodal/devices/beamlines/i05_1/i05_1_motors.py new file mode 100644 index 0000000000..38dc7d22f9 --- /dev/null +++ b/src/dodal/devices/beamlines/i05_1/i05_1_motors.py @@ -0,0 +1,75 @@ +from ophyd_async.epics.motor import Motor + +from dodal.devices.beamlines.i05_shared.rotation_signals import ( + create_rotational_ij_component_signals, +) +from dodal.devices.motors import XYZPolarAzimuthStage + + +class XYZPolarAzimuthDefocusStage(XYZPolarAzimuthStage): + """Six-axis stage with a standard xyz stage and three axis of rotation: polar, + azimuth, and defocus. + + This device exposes four virtual translational axes that are defined in frames + of reference attached to the sample: + + - `hor` and `vert`: + Horizontal and vertical translation axes in the sample frame. + These axes are derived from the lab-frame x and y motors and rotate + with the azimuth angle, so that motion along `hor` and `vert` + remains aligned with the sample regardless of its azimuthal rotation. + + - `long` and `perp`: + Longitudinal and perpendicular translation axes in the tilted sample + frame. These axes are derived from the lab-frame z motor and the + sample-frame `hor` axis, and rotate with the polar angle. + Motion along `long` follows the sample's longitudinal direction, + while `perp` moves perpendicular to it within the polar rotation plane. + + All four virtual axes behave as ordinary orthogonal Cartesian translations + from the user's point of view, while internally coordinating motion of the + underlying motors to account for the current rotation angles. + + This allows users to position the sample in physically meaningful, sample-aligned + coordinates without needing to manually compensate for azimuth or polar rotations. + """ + + def __init__( + self, + prefix: str, + x_infix="SMX", + y_infix="SMY", + z_infix="SMZ", + polar_infix="POL", + azimuth_infix="AZM", + defocus_infix="SMDF", + name="", + ): + super().__init__( + prefix, + name, + x_infix=x_infix, + y_infix=y_infix, + z_infix=z_infix, + polar_infix=polar_infix, + azimuth_infix=azimuth_infix, + ) + + with self.add_children_as_readables(): + self.defocus = Motor(prefix + defocus_infix) + self.hor, self.vert = create_rotational_ij_component_signals( + i_read=self.x.user_readback, + j_read=self.y.user_readback, + i_write=self.x, + j_write=self.y, + angle_deg=self.azimuth.user_readback, + clockwise_frame=True, + ) + self.long, self.perp = create_rotational_ij_component_signals( + i_read=self.z.user_readback, + i_write=self.z, + j_read=self.hor, + j_write=self.hor, + angle_deg=self.polar.user_readback, + clockwise_frame=False, + ) diff --git a/src/dodal/devices/beamlines/i05_shared/__init__.py b/src/dodal/devices/beamlines/i05_shared/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/dodal/devices/beamlines/i05_shared/rotation_signals.py b/src/dodal/devices/beamlines/i05_shared/rotation_signals.py new file mode 100644 index 0000000000..a3fc44cdd2 --- /dev/null +++ b/src/dodal/devices/beamlines/i05_shared/rotation_signals.py @@ -0,0 +1,89 @@ +import asyncio +from math import radians + +from bluesky.protocols import Movable +from bluesky.utils import maybe_await +from ophyd_async.core import ( + SignalR, + SignalRW, + derived_signal_rw, +) + +from dodal.common.maths import rotate_clockwise, rotate_counter_clockwise + + +async def _get_angle_deg(angle_deg: SignalR[float] | float) -> float: + if isinstance(angle_deg, SignalR): + return await angle_deg.get_value() + return angle_deg + + +def create_rotational_ij_component_signals( + i_read: SignalR[float], + j_read: SignalR[float], + i_write: Movable[float], + j_write: Movable[float], + angle_deg: float | SignalR[float], + clockwise_frame: bool = True, +) -> tuple[SignalRW[float], SignalRW[float]]: + """Create virtual i/j signals representing a Cartesian coordinate frame + that is rotated by a given angle relative to the underlying equipment axes. + + The returned signals expose the position of the system in a *rotated frame + of reference* (e.g. the sample or stage frame), while transparently mapping + reads and writes onto the real i/j signals in the fixed equipment (lab) frame. + + From the user's point of view, i and j behave like ordinary orthogonal + Cartesian axes attached to the rotating object. Internally, all reads apply + a rotation to the real motor positions, and all writes apply the inverse + rotation so that the requested motion is achieved in the rotated frame. + + Args: + i_read (SignalR): SignalR representing the i motor readback. + j_read (SignalR): representing the j motor readback. + i_write (Movable): object for setting the i position. + j_write (Movable): object for setting the j position. + angle_deg (float | SignalR): Rotation angle in degrees. + clockwise_frame (boolean): If True, the rotated frame is defined using a + clockwise rotation; otherwise, a counter-clockwise rotation is used. + + Returns: + tuple[SignalRW[float], SignalRW[float]] Two virtual read/write signals + corresponding to the rotated i and j components. + """ + rotate = rotate_clockwise if clockwise_frame else rotate_counter_clockwise + inverse_rotate = rotate_counter_clockwise if clockwise_frame else rotate_clockwise + + async def _read_rotated() -> tuple[float, float, float]: + i, j, ang = await asyncio.gather( + i_read.get_value(), + j_read.get_value(), + _get_angle_deg(angle_deg), + ) + return (*rotate(radians(ang), i, j), ang) + + async def _write_rotated(i_rot: float, j_rot: float, ang: float) -> None: + i_new, j_new = inverse_rotate(radians(ang), i_rot, j_rot) + await asyncio.gather( + maybe_await(i_write.set(i_new)), + maybe_await(j_write.set(j_new)), + ) + + def _read_i(i: float, j: float, ang: float) -> float: + return rotate(radians(ang), i, j)[0] + + async def _set_i(value: float) -> None: + i_rot, j_rot, ang = await _read_rotated() + await _write_rotated(value, j_rot, ang) + + def _read_j(i: float, j: float, ang: float) -> float: + return rotate(radians(ang), i, j)[1] + + async def _set_j(value: float) -> None: + i_rot, j_rot, ang = await _read_rotated() + await _write_rotated(i_rot, value, ang) + + return ( + derived_signal_rw(_read_i, _set_i, i=i_read, j=j_read, ang=angle_deg), + derived_signal_rw(_read_j, _set_j, i=i_read, j=j_read, ang=angle_deg), + ) diff --git a/src/dodal/devices/motors.py b/src/dodal/devices/motors.py index 086b62f6b4..fc84bcc5a2 100644 --- a/src/dodal/devices/motors.py +++ b/src/dodal/devices/motors.py @@ -2,9 +2,11 @@ import math from abc import ABC -from ophyd_async.core import StandardReadable, derived_signal_rw +from ophyd_async.core import SignalRW, StandardReadable, derived_signal_rw from ophyd_async.epics.motor import Motor +from dodal.common.maths import rotate_clockwise, rotate_counter_clockwise + _X, _Y, _Z = "X", "Y", "Z" _OMEGA = "OMEGA" @@ -275,7 +277,7 @@ def __init__( with self.add_children_as_readables(): self.kappa = Motor(prefix + kappa_infix) self.phi = Motor(prefix + phi_infix) - super().__init__(prefix, name, x_infix, y_infix, z_infix) + super().__init__(prefix, name, x_infix, y_infix, z_infix, omega_infix) self.vertical_in_lab_space = create_axis_perp_to_rotation( self.omega, self.y, self.z @@ -315,7 +317,9 @@ def __init__( super().__init__(name) -def create_axis_perp_to_rotation(motor_theta: Motor, motor_i: Motor, motor_j: Motor): +def create_axis_perp_to_rotation( + motor_theta: Motor, motor_i: Motor, motor_j: Motor +) -> SignalRW[float]: """Given a signal that controls a motor in a rotation axis and two other signals controlling motors on a pair of orthogonal axes, these axes being in the rotating frame of reference created by the first axis, create a derived signal @@ -337,19 +341,18 @@ def create_axis_perp_to_rotation(motor_theta: Motor, motor_i: Motor, motor_j: Mo a move here is entirely parallel with the derived axis. """ - def _get(j_val: float, i_val: float, rot_value: float) -> float: - i_component = i_val * math.cos(math.radians(rot_value)) - j_component = j_val * math.sin(math.radians(rot_value)) - return i_component + j_component + def _get(j_val: float, i_val: float, rot_deg_value: float) -> float: + x, y = rotate_clockwise(math.radians(rot_deg_value), i_val, j_val) + return x async def _set(vertical_value: float) -> None: - rot_value = await motor_theta.user_readback.get_value() - i_component = vertical_value * math.cos(math.radians(rot_value)) - j_component = vertical_value * math.sin(math.radians(rot_value)) + rot_deg_value = await motor_theta.user_readback.get_value() + theta = math.radians(rot_deg_value) + i_component, j_component = rotate_counter_clockwise(theta, vertical_value, 0.0) await asyncio.gather( motor_i.set(i_component), motor_j.set(j_component), - motor_theta.set(rot_value), + motor_theta.set(rot_deg_value), ) return derived_signal_rw( @@ -357,5 +360,5 @@ async def _set(vertical_value: float) -> None: _set, i_val=motor_i, j_val=motor_j, - rot_value=motor_theta, + rot_deg_value=motor_theta, ) diff --git a/tests/common/test_maths.py b/tests/common/test_maths.py index 562658a14a..c4de2c34e5 100644 --- a/tests/common/test_maths.py +++ b/tests/common/test_maths.py @@ -1,7 +1,15 @@ +from math import pi, sqrt + +import numpy as np import pytest from dodal.common import in_micros, step_to_num -from dodal.common.maths import Rectangle2D +from dodal.common.maths import ( + Rectangle2D, + do_rotation, + rotate_clockwise, + rotate_counter_clockwise, +) @pytest.mark.parametrize( @@ -108,3 +116,74 @@ def test_rectangle_min_max( assert rect.get_max_y() == max_y assert rect.get_min_x() == min_x assert rect.get_min_y() == min_y + + +@pytest.mark.parametrize( + "theta,x,y,expected", + [ + (0.0, 1.0, 2.0, (1.0, 2.0)), + (pi / 2, 1.0, 0.0, (0.0, -1.0)), # 90 degress clockwise + (pi, 1.0, 2.0, (-1.0, -2.0)), + (2 * pi, 3.0, -4.0, (3.0, -4.0)), + ], +) +def test_rotate_clockwise_known_angles( + theta: float, x: float, y: float, expected: tuple[float, float] +) -> None: + x_new, y_new = rotate_clockwise(theta, x, y) + assert x_new == pytest.approx(expected[0]) + assert y_new == pytest.approx(expected[1]) + + +@pytest.mark.parametrize( + "theta,x,y,expected", + [ + (0.0, 1.0, 2.0, (1.0, 2.0)), + (pi / 2, 1.0, 0.0, (0.0, 1.0)), # 90 degrees counter clockwise + (pi, 1.0, 2.0, (-1.0, -2.0)), + ], +) +def test_rotate_counter_clockwise_known_angles( + theta: float, x: float, y: float, expected: tuple[float, float] +) -> None: + x_new, y_new = rotate_counter_clockwise(theta, x, y) + assert x_new == pytest.approx(expected[0]) + assert y_new == pytest.approx(expected[1]) + + +def test_clockwise_and_counter_clockwise_are_inverses() -> None: + x, y = 2.5, -1.3 + theta = 0.73 + + x_rot, y_rot = rotate_clockwise(theta, x, y) + x_back, y_back = rotate_counter_clockwise(theta, x_rot, y_rot) + + assert x_back == pytest.approx(x) + assert y_back == pytest.approx(y) + + +def test_rotation_preserves_length(): + x, y = 3.0, 4.0 + theta = 1.234 + + x_rot, y_rot = rotate_clockwise(theta, x, y) + original_length = sqrt(x**2 + y**2) + rotated_length = sqrt(x_rot**2 + y_rot**2) + + assert rotated_length == pytest.approx(original_length) + + +def test_do_rotation_matches_manual_matrix_multiply() -> None: + x, y = 1.2, -0.4 + theta = 0.5 + rotation_matrix = np.array( + [ + [np.cos(theta), -np.sin(theta)], + [np.sin(theta), np.cos(theta)], + ] + ) + x_new, y_new = do_rotation(x, y, rotation_matrix) + expected = rotation_matrix @ np.array([x, y]) + + assert x_new == pytest.approx(expected[0]) + assert y_new == pytest.approx(expected[1]) diff --git a/tests/devices/beamlines/i05/__init__.py b/tests/devices/beamlines/i05/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/tests/devices/beamlines/i05/test_i05_motors.py b/tests/devices/beamlines/i05/test_i05_motors.py new file mode 100644 index 0000000000..3dead059d4 --- /dev/null +++ b/tests/devices/beamlines/i05/test_i05_motors.py @@ -0,0 +1,72 @@ +import asyncio +from math import cos, radians, sin + +import pytest +from ophyd_async.core import init_devices +from ophyd_async.testing import assert_reading, partial_reading + +from dodal.devices.beamlines.i05 import I05Goniometer +from tests.devices.beamlines.i05_shared.rotation_signal_test_util import ( + RotatedCartesianFrameTestConfig, + assert_rotated_axes_are_orthogonal, +) + + +def expected_perp_read(x: float, y: float, theta: float) -> float: + return x * cos(theta) + y * sin(theta) + + +def expected_long_read(x: float, y: float, theta: float) -> float: + return x * -sin(theta) + y * cos(theta) + + +@pytest.fixture +async def goniometer() -> I05Goniometer: + with init_devices(mock=True): + goniometer = I05Goniometer("TEST:") + return goniometer + + +async def test_goniometer_read(goniometer: I05Goniometer) -> None: + x, y = 10, 5 + theta = radians(goniometer.rotation_angle_deg) + expected_perp = expected_perp_read(x, y, theta) + expected_long = expected_long_read(x, y, theta) + + await asyncio.gather(goniometer.x.set(x), goniometer.y.set(y)) + + await assert_reading( + goniometer, + { + goniometer.x.name: partial_reading(x), + goniometer.y.name: partial_reading(y), + goniometer.z.name: partial_reading(0), + goniometer.polar.name: partial_reading(0), + goniometer.azimuth.name: partial_reading(0), + goniometer.tilt.name: partial_reading(0), + goniometer.perp.name: partial_reading(expected_perp), + goniometer.long.name: partial_reading(expected_long), + }, + ) + + +async def test_goniometer_perp_and_long_set(goniometer: I05Goniometer) -> None: + frame_config = RotatedCartesianFrameTestConfig( + i_read=goniometer.x.user_readback, + j_read=goniometer.y.user_readback, + i_write=goniometer.x, + j_write=goniometer.y, + angle_deg=goniometer.rotation_angle_deg, + expected_i_read_func=expected_perp_read, + expected_j_read_func=expected_long_read, + i_rotation_axis=goniometer.perp, + j_rotation_axis=goniometer.long, + ) + await assert_rotated_axes_are_orthogonal( + i_val=10, + j_val=5, + angle_deg_val=goniometer.rotation_angle_deg, + new_i_axis_value=20, + new_j_axis_value=20, + frame_config=frame_config, + ) diff --git a/tests/devices/beamlines/i05_1/__init__.py b/tests/devices/beamlines/i05_1/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/tests/devices/beamlines/i05_1/test_i05_1_motors.py b/tests/devices/beamlines/i05_1/test_i05_1_motors.py new file mode 100644 index 0000000000..b82363abb4 --- /dev/null +++ b/tests/devices/beamlines/i05_1/test_i05_1_motors.py @@ -0,0 +1,120 @@ +import asyncio +from math import cos, radians, sin + +import pytest +from ophyd_async.core import init_devices +from ophyd_async.testing import assert_reading, partial_reading + +from dodal.devices.beamlines.i05_1 import XYZPolarAzimuthDefocusStage +from tests.devices.beamlines.i05_shared.rotation_signal_test_util import ( + RotatedCartesianFrameTestConfig, + assert_rotated_axes_are_orthogonal, +) + + +@pytest.fixture +def xyzpad_stage() -> XYZPolarAzimuthDefocusStage: + with init_devices(mock=True): + xyzpad_stage = XYZPolarAzimuthDefocusStage("TEST:") + return xyzpad_stage + + +def expected_hor_read(x: float, y: float, azimuth_theta: float) -> float: + return x * cos(azimuth_theta) + y * sin(azimuth_theta) + + +def expected_vert_read(x: float, y: float, azimuth_theta: float) -> float: + return x * -sin(azimuth_theta) + y * cos(azimuth_theta) + + +def expected_long_read(z: float, hor: float, polar_theta: float) -> float: + return z * cos(polar_theta) + hor * -sin(polar_theta) + + +def expected_perp_read(z: float, hor: float, polar_theta: float) -> float: + return z * sin(polar_theta) + hor * cos(polar_theta) + + +async def test_xyzpad_stage_read(xyzpad_stage: XYZPolarAzimuthDefocusStage) -> None: + x, y = 10, 5 + azimuth_angle_deg = 45 + azimuth_theta = radians(azimuth_angle_deg) + expected_hor = expected_hor_read(x, y, azimuth_theta) + expected_vert = expected_vert_read(x, y, azimuth_theta) + + z = 15 + polar_angle_deg = 35 + polar_theta = radians(polar_angle_deg) + expected_long = expected_long_read(z, expected_hor, polar_theta) + expected_perp = expected_perp_read(z, expected_hor, polar_theta) + + await asyncio.gather( + xyzpad_stage.x.set(x), + xyzpad_stage.y.set(y), + xyzpad_stage.z.set(z), + xyzpad_stage.azimuth.set(azimuth_angle_deg), + xyzpad_stage.polar.set(polar_angle_deg), + ) + await assert_reading( + xyzpad_stage, + { + xyzpad_stage.x.name: partial_reading(x), + xyzpad_stage.y.name: partial_reading(y), + xyzpad_stage.z.name: partial_reading(z), + xyzpad_stage.polar.name: partial_reading(polar_angle_deg), + xyzpad_stage.azimuth.name: partial_reading(azimuth_angle_deg), + xyzpad_stage.defocus.name: partial_reading(0), + xyzpad_stage.hor.name: partial_reading(expected_hor), + xyzpad_stage.vert.name: partial_reading(expected_vert), + xyzpad_stage.long.name: partial_reading(expected_long), + xyzpad_stage.perp.name: partial_reading(expected_perp), + }, + ) + + +async def test_xyzpad_hor_and_vert_set( + xyzpad_stage: XYZPolarAzimuthDefocusStage, +) -> None: + frame_config = RotatedCartesianFrameTestConfig( + i_read=xyzpad_stage.x.user_readback, + j_read=xyzpad_stage.y.user_readback, + i_write=xyzpad_stage.x, + j_write=xyzpad_stage.y, + angle_deg=xyzpad_stage.azimuth, + expected_i_read_func=expected_hor_read, + expected_j_read_func=expected_vert_read, + i_rotation_axis=xyzpad_stage.hor, + j_rotation_axis=xyzpad_stage.vert, + ) + await assert_rotated_axes_are_orthogonal( + i_val=10, + j_val=5, + angle_deg_val=45, + new_i_axis_value=20, + new_j_axis_value=20, + frame_config=frame_config, + ) + + +async def test_xyzpad_long_and_perp_set( + xyzpad_stage: XYZPolarAzimuthDefocusStage, +) -> None: + frame_config = RotatedCartesianFrameTestConfig( + i_read=xyzpad_stage.z.user_readback, + j_read=xyzpad_stage.hor, + i_write=xyzpad_stage.z, + j_write=xyzpad_stage.hor, + angle_deg=xyzpad_stage.polar, + expected_i_read_func=expected_long_read, + expected_j_read_func=expected_perp_read, + i_rotation_axis=xyzpad_stage.long, + j_rotation_axis=xyzpad_stage.perp, + ) + await assert_rotated_axes_are_orthogonal( + i_val=10, + j_val=5, + angle_deg_val=45, + new_i_axis_value=20, + new_j_axis_value=20, + frame_config=frame_config, + ) diff --git a/tests/devices/beamlines/i05_shared/__init__.py b/tests/devices/beamlines/i05_shared/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/tests/devices/beamlines/i05_shared/rotation_signal_test_util.py b/tests/devices/beamlines/i05_shared/rotation_signal_test_util.py new file mode 100644 index 0000000000..175ee48ed3 --- /dev/null +++ b/tests/devices/beamlines/i05_shared/rotation_signal_test_util.py @@ -0,0 +1,139 @@ +import asyncio +from collections.abc import Callable +from dataclasses import dataclass +from math import radians + +import pytest +from bluesky.protocols import Movable +from bluesky.utils import maybe_await +from ophyd_async.core import SignalR, SignalRW + + +@dataclass +class RotatedCartesianFrameTestConfig: + i_read: SignalR[float] + j_read: SignalR[float] + i_write: Movable[float] + j_write: Movable[float] + angle_deg: float | Movable[float] + expected_i_read_func: Callable[[float, float, float], float] + expected_j_read_func: Callable[[float, float, float], float] + i_rotation_axis: SignalRW[float] + j_rotation_axis: SignalRW[float] + + +async def _set_initial_state( + i_val: float, + j_val: float, + angle_deg_val: float, + frame_config: RotatedCartesianFrameTestConfig, +) -> None: + await asyncio.gather( + maybe_await(frame_config.i_write.set(i_val)), + maybe_await(frame_config.j_write.set(j_val)), + ) + if isinstance(frame_config.angle_deg, Movable): + await maybe_await(frame_config.angle_deg.set(angle_deg_val)) + + +async def _read_rotated_components( + frame_config: RotatedCartesianFrameTestConfig, theta: float +) -> tuple[float, float]: + i_new, j_new = await asyncio.gather( + frame_config.i_read.get_value(), + frame_config.j_read.get_value(), + ) + return ( + frame_config.expected_i_read_func(i_new, j_new, theta), + frame_config.expected_j_read_func(i_new, j_new, theta), + ) + + +async def _assert_setting_axis_preserves_other( + set_axis: SignalRW[float], + set_value: float, + expected_i: float, + expected_j: float, + frame_config: RotatedCartesianFrameTestConfig, + theta: float, +) -> None: + await set_axis.set(set_value) + i_after, j_after = await _read_rotated_components( + frame_config=frame_config, theta=theta + ) + assert i_after == pytest.approx(expected_i) + assert j_after == pytest.approx(expected_j) + + +async def assert_rotated_axes_are_orthogonal( + i_val: float, + j_val: float, + angle_deg_val: float, + new_i_axis_value: float, + new_j_axis_value: float, + frame_config: RotatedCartesianFrameTestConfig, +) -> None: + """Assert that the virtual rotated axes behave as independent Cartesian + coordinates. + + This helper verifies that setting one virtual axis in a rotated frame + updates only that axis while preserving the orthogonal axis in the same + rotated frame. + + Specifically, the test: + - Initializes the underlying i/j motors and rotation angle. + - Sets the virtual i' axis and asserts that: + * i' moves to the requested value + * j' remains unchanged + - Resets the system. + - Sets the virtual j' axis and asserts that: + * j' moves to the requested value + * i' remains unchanged + + This confirms that the virtual axes form a proper orthogonal Cartesian + coordinate frame, and that inverse rotations are applied correctly when + writing to the underlying motors. + + Args: + i_val: Initial value for the underlying i-axis. + j_val: Initial value for the underlying j-axis. + angle_deg_val: Rotation angle (in degrees) defining the rotated frame. + new_i_axis_value: Target value for the virtual i' axis. + new_j_axis_value: Target value for the virtual j' axis. + frame_config: Configuration describing how virtual rotated axes are derived from + the underlying motors, including expected readout functions. + """ + # Test setting i′ preserves j′ + await _set_initial_state( + i_val=i_val, + j_val=j_val, + angle_deg_val=angle_deg_val, + frame_config=frame_config, + ) + theta = radians(angle_deg_val) + expected_j = frame_config.expected_j_read_func(i_val, j_val, theta) + + await _assert_setting_axis_preserves_other( + set_axis=frame_config.i_rotation_axis, + set_value=new_i_axis_value, + expected_i=new_i_axis_value, + expected_j=expected_j, + frame_config=frame_config, + theta=theta, + ) + # Test setting j′ preserves i′ + await _set_initial_state( + i_val=i_val, + j_val=j_val, + angle_deg_val=angle_deg_val, + frame_config=frame_config, + ) + expected_i = frame_config.expected_i_read_func(i_val, j_val, theta) + await _assert_setting_axis_preserves_other( + set_axis=frame_config.j_rotation_axis, + set_value=new_j_axis_value, + expected_i=expected_i, + expected_j=new_j_axis_value, + frame_config=frame_config, + theta=theta, + )