From ba7db7819439c89c3f160acaf399b1ffd538ac7f Mon Sep 17 00:00:00 2001 From: Mathias Guijarro Date: Fri, 28 Jun 2024 16:16:28 +0200 Subject: [PATCH] test: add test for SimLinearTrajectoryPositioner --- ophyd_devices/sim/sim_utils.py | 53 +++++++++++++++++++++ tests/test_simulation.py | 84 +++++++++++++++++++++++++++++++++- 2 files changed, 135 insertions(+), 2 deletions(-) diff --git a/ophyd_devices/sim/sim_utils.py b/ophyd_devices/sim/sim_utils.py index 38aca43..fffe563 100644 --- a/ophyd_devices/sim/sim_utils.py +++ b/ophyd_devices/sim/sim_utils.py @@ -129,6 +129,59 @@ class LinearTrajectory: def velocity_profile(self): return np.array(self._velocity_profile) + def plot_trajectory(self): + # visual check of LinearTrajectory class + try: + import matplotlib.pyplot as plt + except ImportError: + raise ImportError("plot_trajectory requires matplotlib to be installed") + + initial_time = time.time() + trajectory = LinearTrajectory( + self.initial_position, + self.final_position, + self.max_velocity, + self.acceleration, + initial_time, + ) + + # Simulate some time points + positions = [] + times = [] + while not trajectory.ended: + times.append(time.time()) + pos = trajectory.position(times[-1]) + positions.append(pos) + time.sleep(0.01) + + # Plotting + plt.figure(figsize=(12, 6)) + + # Plot velocity profile + plt.subplot(1, 2, 1) + plt.plot( + trajectory.velocity_profile[:, 0] - initial_time, + trajectory.velocity_profile[:, 1], + label="Velocity", + ) + plt.xlabel("Time (s)") + plt.ylabel("Velocity (m/s)") + plt.title("Velocity Profile") + plt.grid(True) + plt.legend() + + # Plot position profile + plt.subplot(1, 2, 2) + plt.plot(np.array(times) - initial_time, positions, label="Position") + plt.xlabel("Time (s)") + plt.ylabel("Position (m)") + plt.title("Position Profile") + plt.grid(True) + plt.legend() + + plt.tight_layout() + plt.show() + def stop_trajectory(trajectory, stop_time=None): """Return a trajectory that starts to decelerate at stop_time, diff --git a/tests/test_simulation.py b/tests/test_simulation.py index e1be84d..764fcd4 100644 --- a/tests/test_simulation.py +++ b/tests/test_simulation.py @@ -2,6 +2,7 @@ # pylint: disable: all import os +import time from types import SimpleNamespace from unittest import mock @@ -24,9 +25,9 @@ from ophyd_devices.sim.sim_camera import SimCamera from ophyd_devices.sim.sim_flyer import SimFlyer from ophyd_devices.sim.sim_frameworks import H5ImageReplayProxy, SlitProxy from ophyd_devices.sim.sim_monitor import SimMonitor, SimMonitorAsync -from ophyd_devices.sim.sim_positioner import SimPositioner +from ophyd_devices.sim.sim_positioner import SimLinearTrajectoryPositioner, SimPositioner from ophyd_devices.sim.sim_signals import ReadOnlySignal -from ophyd_devices.sim.sim_utils import H5Writer +from ophyd_devices.sim.sim_utils import H5Writer, LinearTrajectory from ophyd_devices.utils.bec_device_base import BECDevice, BECDeviceBase @@ -63,6 +64,14 @@ def positioner(name="positioner"): yield pos +@pytest.fixture(scope="function") +def linear_traj_positioner(name="linear_traj_positioner"): + """Fixture for SimLinearTrajectoryPositioner.""" + dm = DMMock() + pos = SimLinearTrajectoryPositioner(name=name, device_manager=dm) + yield pos + + @pytest.fixture(scope="function") def async_monitor(name="async_monitor"): """Fixture for SimMonitorAsync.""" @@ -236,6 +245,77 @@ def test_positioner_move(positioner): ) +@pytest.mark.parametrize( + "initial_position, final_position, max_velocity, acceleration", + [(0, 100, 5, 20), (0, 1, 5, 20)], # Trapezoidal profile # Triangular profile +) +def test_linear_traj(initial_position, final_position, max_velocity, acceleration): + """Test the LinearTrajectory class""" + initial_time = time.time() + trajectory = LinearTrajectory( + initial_position, final_position, max_velocity, acceleration, initial_time + ) + + # Test acceleration phase + t1 = initial_time + trajectory.time_accel / 2 # Halfway through acceleration phase + pos1 = trajectory.position(t1) + expected_pos1 = initial_position + 0.5 * acceleration * (trajectory.time_accel / 2) ** 2 + assert np.isclose(pos1, expected_pos1), f"Expected {expected_pos1}, got {pos1}" + + # Test constant velocity phase + if trajectory.time_const_vel > 0: + t2 = ( + initial_time + trajectory.time_accel + trajectory.time_const_vel / 2 + ) # Halfway through constant velocity phase + pos2 = trajectory.position(t2) + expected_pos2 = ( + initial_position + + trajectory.distance_to_max_velocity + + max_velocity * (t2 - initial_time - trajectory.time_accel) + ) + assert np.isclose(pos2, expected_pos2), f"Expected {expected_pos2}, got {pos2}" + + # Test deceleration phase + t3 = ( + initial_time + trajectory.total_time - trajectory.time_decel / 2 + ) # Halfway through deceleration phase + pos3 = trajectory.position(t3) + t_decel = t3 - (initial_time + trajectory.time_accel + trajectory.time_const_vel) + expected_pos3 = final_position - 0.5 * acceleration * (trajectory.time_decel / 2) ** 2 + assert np.isclose(pos3, expected_pos3), f"Expected {expected_pos3}, got {pos3}" + + # Test end + t4 = initial_time + trajectory.total_time + 0.1 # Slightly after end + pos4 = trajectory.position(t4) + assert pos4 == final_position + assert trajectory.ended + + +def test_sim_linear_trajectory_positioner(linear_traj_positioner): + vel = 5 # velocity 5 m.s^-1 + acc = 20 # acceleration 20 m.s^-2 + linear_traj_positioner.velocity.set(vel) + linear_traj_positioner.acceleration.set(vel / acc) # acctime 250 ms + linear_traj_positioner.update_frequency = 100 + assert linear_traj_positioner.position == 0 + + t0 = time.time() + trajectory = LinearTrajectory(0, 50, vel, acc, t0) + t2 = ( + t0 + trajectory.time_accel + trajectory.time_const_vel / 2 + ) # Halfway through constant velocity phase + decel_distance = trajectory.position(t0 + trajectory.time_accel) + expected_pos = trajectory.position(t2) + decel_distance + + linear_traj_positioner.move(50) + # move is non-blocking, so sleep until it is time to stop: + time.sleep(t2 - t0) + linear_traj_positioner.stop() + + # ensure position is ok + assert pytest.approx(linear_traj_positioner.position - expected_pos, abs=1e-1) == 0 + + @pytest.mark.parametrize("proxy_active", [True, False]) def test_sim_camera_proxies(camera, proxy_active): """Test mocking compute_method with framework class"""