diff --git a/positronic/drivers/roboarm/franka.py b/positronic/drivers/roboarm/franka.py index 7f7370868..ee2a48bcf 100644 --- a/positronic/drivers/roboarm/franka.py +++ b/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()) diff --git a/positronic/drivers/roboarm/yam.py b/positronic/drivers/roboarm/yam.py index 6611068ee..fb2003bd8 100644 --- a/positronic/drivers/roboarm/yam.py +++ b/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: diff --git a/positronic/drivers/tests/test_utils.py b/positronic/drivers/tests/test_utils.py index 26873f2bd..44e9d748e 100644 --- a/positronic/drivers/tests/test_utils.py +++ b/positronic/drivers/tests/test_utils.py @@ -174,8 +174,10 @@ 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) @@ -183,8 +185,34 @@ def test_a_streamed_grip_waits_for_the_call_queue_to_be_empty(asking): 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) diff --git a/positronic/drivers/utils.py b/positronic/drivers/utils.py index aead06e36..65fa5ee0c 100644 --- a/positronic/drivers/utils.py +++ b/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):