[MSD-499][feat] FixedPositionsActuator: smart detection of the referencing position

src/odemis/driver/actuator.py

@@ -27,6 +27,7 @@
 import math
 import numbers
 import threading
+import time
 from concurrent.futures._base import CancelledError, FINISHED
 from concurrent import futures
 from typing import Dict, Union, Set
@@ -1377,9 +1378,14 @@ def __init__(self, name, role, dependencies, axis_name, positions, cycle=None,
         if cycle is not None:
             if ref_start is None:
                 # The reference switch is typically at 0, so a little after that is often a good
-                # starting point (assuming the referencing goes towards negative direction).
-                self._ref_start = self._cycle / len(self._positions)
+                # starting point. As we don't know which direction goes the referencing, we
+                # start with two candidates, and the faster one will be used.
+                self._ref_start_auto = True
+                self._ref_start_candidates = [cycle * 0.1, cycle * 0.9]
+                self._ref_candidate_times = [None, None]
+                self._ref_start = self._ref_start_candidates[0]
             else:
+                self._ref_start_auto = False
                 self._ref_start = ref_start
             if not all(0 <= p < cycle for p in positions.keys()):
                 raise ValueError("Positions must be between 0 and %s (non inclusive)" % (cycle,))
@@ -1492,20 +1498,14 @@ def _doMoveAbs(self, pos):
                 move = {self._caxis: req_pos}
                 self._dependency.moveAbs(move).result()
             else:
-                cur_pos = self._dependency.position.value[self._caxis]
-
-                # After moving more than the equivalent of a full cycle, reference again to get rid
+                # After moving more than the equivalent of 2 full cycles, reference again to get rid
                 # of accumulated error.
-                if self._move_sum >= self._cycle:
+                if self._move_sum >= 2 * self._cycle:
                     logging.debug("Re-referencing axis %s (-> %s) after moving %s",
                                   self._axis, self._caxis, self._move_sum)
-                    # Move near the reference switch, using the shortest way, to save a bit of time
-                    shift = util.rot_shortest_move(cur_pos, self._ref_start, self._cycle)
-                    self._dependency.moveRel({self._caxis: shift}).result()
-                    self._dependency.reference({self._caxis}).result()
-                    self._move_sum = 0
-                    cur_pos = self._dependency.position.value[self._caxis]
+                    self._quick_reference()
 
+                cur_pos = self._dependency.position.value[self._caxis]
                 # Optimize by moving through the closest way
                 shift = util.rot_shortest_move(cur_pos, req_pos, self._cycle)
                 self._move_sum += abs(shift)
@@ -1517,6 +1517,60 @@ def _doMoveAbs(self, pos):
         finally:
             self._dependency.position.subscribe(self._update_dep_position, init=True)
 
+    def _quick_reference(self):
+        """
+        Do a quick reference by moving to a position near the reference switch and then referencing.
+        Assuming we know which direction uses the reference procedure, *on a cyclic axis*,
+        it's faster than potentially doing a full turn.
+        When ref_start was not provided at init, the method learns which of two candidate start
+        positions (10% or 90% of the cycle) yields a faster reference operation, and
+        permanently uses the faster one once both have been tried.
+        """
+        cur_pos = self._dependency.position.value[self._caxis]
+        shift = util.rot_shortest_move(cur_pos, self._ref_start, self._cycle)
+
+        t_premove_start = time.monotonic()
+        self._dependency.moveRel({self._caxis: shift}).result()
+        t_premove = time.monotonic() - t_premove_start
+
+        t_ref_start = time.monotonic()
+        self._dependency.reference({self._caxis}).result()
+        t_ref = time.monotonic() - t_ref_start
+
+        if self._ref_start_auto:
+            idx = self._ref_start_candidates.index(self._ref_start)
+            if self._ref_candidate_times[idx] is None:
+                # First time measuring this candidate: record its duration and decide what to do next.
+                self._ref_candidate_times[idx] = t_ref
+                logging.debug("Candidate ref_start %g took %.3f s for reference (pre-move %.3f s)",
+                              self._ref_start_candidates[idx], t_ref, t_premove)
+
+                other_idx = 1 - idx
+                if self._ref_candidate_times[other_idx] is None:
+                    # Other candidate not tried yet; switch to it if this one was slow
+                    is_long = t_ref > (2 * t_premove + 0.01)
+                    if is_long:
+                        logging.debug("Reference from %g was slow (%.3f s > 2 * %.3f s), "
+                                      "trying the other candidate (%g) next time",
+                                      self._ref_start_candidates[idx], t_ref, t_premove,
+                                      self._ref_start_candidates[other_idx])
+                        self._ref_start = self._ref_start_candidates[other_idx]
+                    else:  # Fast enough => stick to it
+                        logging.debug(
+                            "Reference from %g was short enough (%.3f s < 2 * %.3f s), will keep using it.",
+                            self._ref_start_candidates[idx], t_ref, t_premove)
+                        self._ref_start_auto = False
+                else:
+                    # Both candidates have been measured; pick the faster one permanently
+                    best_idx = 0 if self._ref_candidate_times[0] <= self._ref_candidate_times[1] else 1
+                    self._ref_start = self._ref_start_candidates[best_idx]
+                    self._ref_start_auto = False
+                    logging.info("Settled on ref_start %g (%.3f s) over %g (%.3f s)",
+                                 self._ref_start_candidates[best_idx], self._ref_candidate_times[best_idx],
+                                 self._ref_start_candidates[1 - best_idx], self._ref_candidate_times[1 - best_idx])
+
+        self._move_sum = 0
+
     def _doReference(self, axes):
         logging.debug("Referencing axis %s (-> %s)", self._axis, self._caxis)
         f = self._dependency.reference({self._caxis})

src/odemis/driver/test/actuator_test.py

@@ -26,6 +26,7 @@
 import random
 import time
 import unittest
+import unittest.mock
 
 import odemis
 import simulated_test
@@ -192,6 +193,110 @@ def tearDown(self):
         super(FixedPositionsTest, self).tearDown()
 
 
+class TestFixedPositionsActuatorAdaptiveRef(unittest.TestCase):
+    """Tests the adaptive ref_start learning in _quick_reference()."""
+
+    def setUp(self):
+        self.dependency = simulated.Stage("sstage_adaptive", "test", {"a"})
+        self.dev = FixedPositionsActuator(
+            "stage_adaptive", "stage",
+            {"x": self.dependency}, "a",
+            {0: "pos0", 0.01: "pos1", 0.02: "pos2",
+             0.03: "pos3", 0.04: "pos4", 0.05: "pos5"},
+            cycle=0.06,
+        )
+        # Provide a mock reference() on the dependency since simulated.Stage doesn't have one
+        self.dependency.reference = unittest.mock.MagicMock(
+            return_value=model.InstantaneousFuture()
+        )
+
+    def tearDown(self):
+        self.dev.terminate()
+        self.dependency.terminate()
+
+    def _run_quick_reference_with_timing(self, t_premove: float, t_ref: float):
+        """
+        Call _quick_reference() with mocked time.monotonic() so that the
+        reported pre-move duration is t_premove and the reference duration is t_ref.
+
+        :param t_premove: simulated pre-move duration in seconds
+        :param t_ref: simulated reference duration in seconds
+        """
+        # time.monotonic() is called 4 times: start_premove, end_premove, start_ref, end_ref
+        side_effects = [0.0, t_premove, t_premove, t_premove + t_ref]
+        with unittest.mock.patch("odemis.driver.actuator.time.monotonic",
+                                 side_effect=side_effects):
+            self.dev._quick_reference()
+
+    def test_initial_state(self):
+        """Without explicit ref_start, auto mode is enabled and the first candidate is cycle * 0.1."""
+        self.assertTrue(self.dev._ref_start_auto)
+        self.assertAlmostEqual(self.dev._ref_start, 0.06 * 0.1)
+        self.assertEqual(self.dev._ref_candidate_times, [None, None])
+
+    def test_explicit_ref_start_disables_adaptation(self):
+        """With an explicit ref_start, adaptive learning should be disabled."""
+        dep2 = simulated.Stage("sstage_explicit", "test", {"a"})
+        dev2 = FixedPositionsActuator(
+            "stage_explicit", "stage",
+            {"x": dep2}, "a",
+            {0: "pos0", 0.01: "pos1", 0.02: "pos2",
+             0.03: "pos3", 0.04: "pos4", 0.05: "pos5"},
+            cycle=0.06,
+            ref_start=0.005,
+        )
+        try:
+            self.assertFalse(dev2._ref_start_auto)
+            self.assertAlmostEqual(dev2._ref_start, 0.005)
+        finally:
+            dev2.terminate()
+            dep2.terminate()
+
+    def test_slow_candidate_a_switches_to_candidate_b(self):
+        """If candidate A is slow (t_ref > 2 * t_premove), switch to candidate B next time."""
+        # t_premove=0.1 s, t_ref=0.5 s  →  0.5 > 2*0.1, so "long"
+        self._run_quick_reference_with_timing(t_premove=0.1, t_ref=0.5)
+
+        self.assertAlmostEqual(self.dev._ref_candidate_times[0], 0.5)
+        self.assertIsNone(self.dev._ref_candidate_times[1])
+        self.assertAlmostEqual(self.dev._ref_start, 0.06 * 0.9)
+        self.assertTrue(self.dev._ref_start_auto)
+
+    def test_fast_candidate_a_stays_on_candidate_a(self):
+        """If candidate A is fast (t_ref <= 2 * t_premove), keep using candidate A."""
+        # t_premove=0.1 s, t_ref=0.15 s  →  0.15 <= 2*0.1, so "fast"
+        self._run_quick_reference_with_timing(t_premove=0.1, t_ref=0.15)
+
+        self.assertAlmostEqual(self.dev._ref_candidate_times[0], 0.15)
+        self.assertIsNone(self.dev._ref_candidate_times[1])
+        self.assertAlmostEqual(self.dev._ref_start, 0.06 * 0.1)
+        self.assertFalse(self.dev._ref_start_auto)
+
+    def test_both_candidates_tried_picks_faster_b(self):
+        """After both candidates are timed, the one with the smaller t_ref is used permanently."""
+        # First call: slow A → switches to B
+        self._run_quick_reference_with_timing(t_premove=0.1, t_ref=0.5)
+        # Second call: B is faster
+        self._run_quick_reference_with_timing(t_premove=0.1, t_ref=0.2)
+
+        self.assertAlmostEqual(self.dev._ref_candidate_times[0], 0.5)
+        self.assertAlmostEqual(self.dev._ref_candidate_times[1], 0.2)
+        self.assertAlmostEqual(self.dev._ref_start, 0.06 * 0.9)  # B is faster
+        self.assertFalse(self.dev._ref_start_auto)  # learning complete
+
+    def test_both_candidates_tried_picks_faster_a(self):
+        """If A is faster than B after both are tried, A is selected permanently."""
+        # First call: slow A → switches to B
+        self._run_quick_reference_with_timing(t_premove=0.1, t_ref=0.5)
+        # Second call: B is even slower than A
+        self._run_quick_reference_with_timing(t_premove=0.1, t_ref=0.8)
+
+        self.assertAlmostEqual(self.dev._ref_candidate_times[0], 0.5)
+        self.assertAlmostEqual(self.dev._ref_candidate_times[1], 0.8)
+        self.assertAlmostEqual(self.dev._ref_start, 0.06 * 0.1)  # A is faster
+        self.assertFalse(self.dev._ref_start_auto)  # learning complete
+
+
 class FixedPositionsTestAntibacklash(unittest.TestCase):
 
     def setUp(self):