Ask a device for the setpoint it is streamed now

positronic/drivers/roboarm/franka.py

@@ -206,18 +206,23 @@ def should_stop() -> bool:
 
         try:
             self.command_target(target, mode)
-            for wait in self.await_goal(should_stop, self.limiter.wait):
-                st = self.robot.state()
-                self.state.encode(st, RobotStatus.BUSY)
-                self.out.emit(self.state)
-                if st.error != 0 and not at_teardown:
-                    self.robot.recover_from_errors()
-                yield wait
-            # The loop exits before it polls again, so a goal that landed as the deadline passed is unseen.
-            if expired() and self.robot.goal().status != pf.GoalStatus.REACHED:
-                # The robot still tracks the goal it missed, and would resume the move once the arm comes free.
-                self.robot.set_target_joints(self.robot.state().q)
-                raise TimeoutError(f'the arm stopped short of {target}')
+            try:
+                for wait in self.await_goal(should_stop, self.limiter.wait):
+                    st = self.robot.state()
+                    self.state.encode(st, RobotStatus.BUSY)
+                    self.out.emit(self.state)
+                    if st.error != 0 and not at_teardown:
+                        self.robot.recover_from_errors()
+                    yield wait
+                # The loop exits before it polls again, so a goal that landed as the deadline passed is unseen.
+                if expired() and self.robot.goal().status != pf.GoalStatus.REACHED:
+                    # The robot still tracks the goal it missed, and would resume it once the arm comes free.
+                    self.robot.set_target_joints(self.robot.state().q)
+                    raise TimeoutError(f'the arm stopped short of {target}')
+            finally:
+                # The arm has the target, so it travelled however this ends, and the loop read nothing
+                # while it did: what was streamed at it in the meantime is where it was wanted on the way.
+                self.moves.discard_streamed_setpoints()
         except Exception:
             self.moves.errored = True
             raise
@@ -260,7 +265,8 @@ def sync_move(self, call: pimm.calls.Call[command.CommandType, None]) -> Iterato
         """Put the arm where ``call`` asks and answer it once the state saying so is out."""
         cmd = call.request
         try:
-            if (yield from self.move_to(self.to_joints(cmd), cmd.mode)) is MoveStatus.ARRIVED:
+            status = yield from self.move_to(self.to_joints(cmd), cmd.mode)
+            if status is MoveStatus.ARRIVED:
                 call.set_result(None)
             else:
                 call.set_exception(MoveAbandoned())

positronic/drivers/roboarm/yam.py

@@ -251,20 +251,25 @@ def move_to(self, target: np.ndarray, grip: float) -> Generator[pimm.Command, No
         try:
             start = np.asarray(self.observations()[_JOINT_POS], dtype=np.float64)
             started = self.clock.now()
-            while not self._arrived(obs := self.observations(), target, grip):
-                if self.should_stop.value:
-                    return MoveStatus.GAVE_UP
-                elapsed = self.clock.now() - started
-                if elapsed > self._MOVE_TIME_S + self._SETTLE_S:
-                    raise TimeoutError(f'the chain stopped short of {target} at grip {grip}')
-                # Ramped rather than commanded outright, so the chain travels at a pace the joints can hold,
-                # and held at the target afterwards while it settles the last of the way in.
-                alpha = min(elapsed / self._MOVE_TIME_S, 1.0)
-                self.vendor.command_joint_pos(np.append((1 - alpha) * start + alpha * target, 1.0 - grip))
-                self.encode(obs, RobotStatus.BUSY)  # the driver owns the chain until it arrives
-                self.out.emit(self.state)
-                self.grip_out.emit(self._grip(obs))
-                yield self.limiter.wait()
+            try:
+                while not self._arrived(obs := self.observations(), target, grip):
+                    if self.should_stop.value:
+                        return MoveStatus.GAVE_UP
+                    elapsed = self.clock.now() - started
+                    if elapsed > self._MOVE_TIME_S + self._SETTLE_S:
+                        raise TimeoutError(f'the chain stopped short of {target} at grip {grip}')
+                    # Ramped rather than commanded outright, so the chain travels at a pace the joints can
+                    # hold, and held at the target afterwards while it settles the last of the way in.
+                    alpha = min(elapsed / self._MOVE_TIME_S, 1.0)
+                    self.vendor.command_joint_pos(np.append((1 - alpha) * start + alpha * target, 1.0 - grip))
+                    self.encode(obs, RobotStatus.BUSY)  # the driver owns the chain until it arrives
+                    self.out.emit(self.state)
+                    self.grip_out.emit(self._grip(obs))
+                    yield self.limiter.wait()
+            finally:
+                # The chain was read, so it either travelled or already stood at the target, and the loop
+                # read nothing while it did: what was streamed at it says where it was wanted on the way.
+                self.moves.discard_streamed_setpoints()
         except Exception:
             self.moves.errored = True
             raise
@@ -292,7 +297,8 @@ def sync_move(
         """
         try:
             target = self.to_joints(call.request, q)
-            if (yield from self.move_to(target, grip)) is MoveStatus.ARRIVED:
+            status = yield from self.move_to(target, grip)
+            if status is MoveStatus.ARRIVED:
                 call.set_result(None)
                 return target, grip
         except Exception as exc:

positronic/drivers/tests/test_utils.py

@@ -174,17 +174,45 @@ def test_a_grip_asked_for_past_the_range_is_tracked_against_a_width_the_fingers_
     assert answer.result() is None
 
 
-def test_a_streamed_grip_waits_for_the_call_queue_to_be_empty(asking):
-    """A signal holds only its latest value, so a stream read in the same tick as a call would be lost."""
+def test_a_setpoint_the_device_was_moved_away_from_is_let_go(asking):
+    """A setpoint says where the device is wanted now, and the move taken after it puts the device
+    somewhere else. Applying the setpoint once the move lands takes the device back off the pose it was
+    asked for, in one step and with nobody asking."""
     ask, moves, stream = asking
     stream.push(0.25)
     ask(0.9)
 
     assert grip_setpoint(moves, grip=0.0, now=0.0) == 0.9
     assert grip_setpoint(moves, grip=0.9, now=0.1) is None  # the call arrives
     moves.answer()
-    assert grip_setpoint(moves, grip=0.9, now=0.2) == 0.25  # the stream, still waiting
-    assert grip_setpoint(moves, grip=0.25, now=0.3) is None
+    assert grip_setpoint(moves, grip=0.9, now=0.2) is None, 'the setpoint the move superseded reached the device'
+
+
+def test_the_newest_setpoint_is_what_the_device_is_asked_for(asking):
+    """A transport that queues setpoints hands the oldest over first. A device asked for one a tick is
+    driven through what its asker has already done, and falls further behind the longer it runs."""
+    _ask, moves, stream = asking
+    for width in (0.1, 0.2, 0.3):
+        stream.push(width)
+
+    assert grip_setpoint(moves, grip=0.0, now=0.0) == 0.3
+    assert grip_setpoint(moves, grip=0.3, now=0.1) is None, 'the device was asked for a width it had passed'
+
+
+def test_setpoints_streamed_at_a_travelling_device_do_not_reach_it_when_the_move_lands(asking):
+    """Nothing reads the stream for as long as a move owns the device, so what arrives in that time is
+    where the asker wanted the device before the move — every setpoint of it, oldest first."""
+    ask, moves, stream = asking
+    ask(0.9)
+    assert grip_setpoint(moves, grip=0.0, now=0.0) == 0.9
+
+    stream.push(0.25)
+    stream.push(0.30)
+    assert grip_setpoint(moves, grip=0.0, now=0.1) is None  # the move still travels
+    assert grip_setpoint(moves, grip=0.9, now=0.2) is None  # ... and lands
+    moves.answer()
+
+    assert grip_setpoint(moves, grip=0.9, now=0.3) is None, 'the device was driven back through the stream'
 
 
 def test_how_long_a_move_gets_is_the_driver_s_to_say():
@@ -248,6 +276,86 @@ def test_a_device_still_travelling_is_asked_for_nothing(asking):
     assert moves.settle(0.0, now=0.1) is MoveStatus.MOVING
 
 
+def test_a_setpoint_written_while_a_blocking_move_travelled_is_let_go(asking):
+    """A driver held inside the call reads nothing while it moves, so what queued up is older than the pose
+    it now holds -- and applying it would drive the device straight back off the target it was asked for."""
+    ask, moves, stream = asking
+    ask(1.0)
+    call = moves.next_request()
+    assert isinstance(call, pimm.calls.Call)
+
+    stream.push(0.25)  # written while the device travelled, and never polled for
+    call.set_result(None)
+    moves.discard_streamed_setpoints()  # the driver blocked for the whole travel and lets go of what queued up
+
+    assert moves.next_request() is None
+
+    stream.push(0.75)
+    assert moves.next_request() == 0.75
+
+
+def test_a_setpoint_written_after_a_blocking_move_ended_survives_the_wait_for_the_next_poll(asking):
+    """A blocking driver yields to its limiter before it polls again, so a setpoint can arrive between the
+    end of the travel and the poll. It was written after the move, and is what the device does next."""
+    ask, moves, stream = asking
+    ask(1.0)
+    call = moves.next_request()
+    assert isinstance(call, pimm.calls.Call)
+    call.set_result(None)
+    moves.discard_streamed_setpoints()
+
+    stream.push(0.25)  # written after the travel, while the driver slept
+
+    assert moves.next_request() == 0.25
+
+
+def test_a_call_refused_before_the_device_moved_leaves_the_stream_alone(asking):
+    """A target the device cannot be put at is answered without it moving, so nothing streamed at it since
+    says where it was wanted on the way anywhere."""
+    ask, moves, stream = asking
+    ask(1.0)
+    call = moves.next_request()
+    assert isinstance(call, pimm.calls.Call)
+    call.set_exception(ValueError('out of reach'))  # refused before the device took a step
+
+    stream.push(0.25)
+
+    assert moves.next_request() == 0.25
+
+
+def test_a_setpoint_written_while_a_settled_move_waits_to_be_answered_is_kept(asking):
+    """A move settles before the state that goes with it is published, and its asker is answered after. A
+    setpoint that arrives in between was written after the move ended."""
+    ask, moves, stream = asking
+    ask(1.0)
+    call = moves.next_request()
+    assert isinstance(call, pimm.calls.Call)
+    moves.accept(call, 1.0, TOL, now=0.0, timeout_s=3.0)
+    assert moves.settle(1.0, now=0.1) is MoveStatus.ARRIVED
+
+    stream.push(0.25)  # settled, and its asker not yet told
+    assert moves.next_request() is None
+
+    moves.answer()
+    assert moves.next_request() == 0.25
+
+
+def test_a_setpoint_written_after_a_move_arrived_is_kept(asking):
+    """A move settles at the end of a tick, and a driver that keeps it in flight polls again only after its
+    limiter sleeps. A setpoint written in between is newer than the move, and is what the device does next."""
+    ask, moves, stream = asking
+    ask(1.0)
+    call = moves.next_request()
+    assert isinstance(call, pimm.calls.Call)
+    moves.accept(call, 1.0, TOL, now=0.0, timeout_s=3.0)
+    assert moves.settle(1.0, now=0.1) is MoveStatus.ARRIVED
+    moves.answer()
+
+    stream.push(0.25)  # written while the driver slept, and after the move was over
+
+    assert moves.next_request() == 0.25
+
+
 def test_a_run_that_dies_with_one_move_settled_and_another_in_flight_answers_both():
     """One outcome each: the settled move earned its answer, the travelling one is owed what killed it."""
     moves, landed = _accepted(0.0)

positronic/drivers/utils.py

@@ -78,18 +78,40 @@ def target(self) -> np.ndarray | float:
         assert self._call is not None, 'no move is in flight'
         return self._target
 
+    def take_newest_setpoint(self) -> T | None:
+        """The newest setpoint streamed at the device, letting go of every setpoint older than it.
+
+        A transport that queues setpoints hands the oldest over first, and a setpoint says where the device
+        is wanted now, not where it was wanted when the setpoint was written.
+        """
+        latest = None
+        while (message := self._async_move.read()) is not None and message.updated:
+            latest = message.data
+        return latest
+
     def next_request(self) -> pimm.calls.Call[T, None] | T | None:
         """What the device is asked for now: a call whose asker waits to hear it arrive, a streamed setpoint
         nobody waits on, or nothing.
 
-        A call comes first, because a signal holds only its latest value and a setpoint read in the same tick
-        would be lost. A device a move already owns is asked for nothing.
+        A call comes first: a setpoint says where the device is wanted now, and the move that follows it
+        puts the device somewhere else. A device a move already owns is asked for nothing, and neither is
+        one whose move has settled and whose asker is still owed the news. The stream is read only when a
+        setpoint is what comes back, so a call refused before the device moved leaves it where it stands.
         """
         if self.busy:
             return None
         if (call := next(self._sync_move.incoming(), None)) is not None:
             return call
-        return pimm.value_updated(self._async_move)
+        return self.take_newest_setpoint()
+
+    def discard_streamed_setpoints(self) -> None:
+        """Let go of every setpoint streamed at the device while a move owned it.
+
+        Those setpoints say where the device was wanted on the way to the pose it now holds, and applying
+        one would drive it straight back off that pose. ``settle`` does this for a move it ends; a driver
+        held inside the call for the whole travel does it when the travel is over.
+        """
+        self.take_newest_setpoint()
 
     def accept(
         self, call: pimm.calls.Call[T, None], target: np.ndarray | float, tol: float, now: float, timeout_s: float
@@ -118,10 +140,12 @@ def settle(self, position: np.ndarray | float, now: float) -> MoveStatus:
         assert self._call is not None, 'no move is in flight'
         if bool(np.all(np.abs(np.asarray(position) - np.asarray(self._target)) < self._tol)):
             self._settled, self._call, self.errored = (self._call, None), None, False
+            self.discard_streamed_setpoints()
             return MoveStatus.ARRIVED
         if now >= self._deadline:
             short = TimeoutError(f'stopped at {np.round(position, 3)}, short of {np.round(self._target, 3)}')
             self._settled, self._call, self.errored = (self._call, short), None, True
+            self.discard_streamed_setpoints()
             return MoveStatus.GAVE_UP
         return MoveStatus.MOVING
 
@@ -191,6 +215,8 @@ def grip_setpoint(moves: Moves[float], grip: float, now: float) -> float | None:
     driver writes before calling ``Moves.answer``.
     """
     if moves.active:
+        # A width streamed at fingers a move owns is older than where the move puts them.
+        moves.take_newest_setpoint()
         return grip if moves.settle(grip, now) is MoveStatus.GAVE_UP else None
     asked = moves.next_request()
     if isinstance(asked, pimm.calls.Call):