From 8a9504e11423097bdd2d8e737ad2ea229b6ad084 Mon Sep 17 00:00:00 2001 From: DarksaCY Date: Wed, 2 Sep 2026 15:41:53 +0300 Subject: [PATCH 1/8] Ask a device for the setpoint it is streamed now A follower parked with `h` left the pose it was put at and went back, in one step, to where its leader had been seconds earlier. A streamed setpoint crosses to the driver on a queue, which hands the oldest over first, and the driver takes one a tick. A device that reads slower than it is written to therefore falls behind by everything in between, and a move owns the device for its whole travel, in which nothing is read at all. What the driver did after the move was work through the setpoints the move had superseded, at a tick each. `Moves.streamed` reads the newest and lets go of the rest, and the setpoints streamed at a device a move owns go the same way. Every arm driver reads its commands through it, and the arm now stands where the last setpoint asks rather than where an older one did. --- positronic/drivers/tests/test_utils.py | 36 +++++++++++++++++++++++--- positronic/drivers/utils.py | 22 +++++++++++++--- 2 files changed, 51 insertions(+), 7 deletions(-) diff --git a/positronic/drivers/tests/test_utils.py b/positronic/drivers/tests/test_utils.py index 26873f2bd..522783f8e 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(): diff --git a/positronic/drivers/utils.py b/positronic/drivers/utils.py index aead06e36..41971df96 100644 --- a/positronic/drivers/utils.py +++ b/positronic/drivers/utils.py @@ -78,18 +78,32 @@ def target(self) -> np.ndarray | float: assert self._call is not None, 'no move is in flight' return self._target + def streamed(self) -> T | None: + """The newest setpoint streamed at the device; reading it lets go of every setpoint older than it. + + A setpoint says where the device is wanted now. A transport that queues them hands the oldest over + first, so a device asked for one a tick is driven through what its asker has already done — seconds + of it where a move owned the device and nothing was read at all. + """ + 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 the setpoints + streamed at it while it travels are let go for the same reason. """ + streamed = self.streamed() 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 streamed def accept( self, call: pimm.calls.Call[T, None], target: np.ndarray | float, tol: float, now: float, timeout_s: float @@ -191,6 +205,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.streamed() return grip if moves.settle(grip, now) is MoveStatus.GAVE_UP else None asked = moves.next_request() if isinstance(asked, pimm.calls.Call): From 021f79f515a185803db13bc0b179ea6e1adc8dae Mon Sep 17 00:00:00 2001 From: DarksaCY Date: Thu, 3 Sep 2026 19:15:25 +0300 Subject: [PATCH 2/8] Name the read of a streamed setpoint for what it takes `Moves.streamed` reads like a property, and one of its two callers wants only what it drops and throws the value away. `take_newest_setpoint` says that the call takes the setpoints as well as reads them. Its docstring said which run the queue backlog was found on; it states the constraint the method is written against and stops there. --- positronic/drivers/utils.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/positronic/drivers/utils.py b/positronic/drivers/utils.py index 41971df96..415f65273 100644 --- a/positronic/drivers/utils.py +++ b/positronic/drivers/utils.py @@ -78,12 +78,11 @@ def target(self) -> np.ndarray | float: assert self._call is not None, 'no move is in flight' return self._target - def streamed(self) -> T | None: - """The newest setpoint streamed at the device; reading it lets go of every setpoint older than it. + def take_newest_setpoint(self) -> T | None: + """The newest setpoint streamed at the device, letting go of every setpoint older than it. - A setpoint says where the device is wanted now. A transport that queues them hands the oldest over - first, so a device asked for one a tick is driven through what its asker has already done — seconds - of it where a move owned the device and nothing was read at all. + 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: @@ -98,12 +97,12 @@ def next_request(self) -> pimm.calls.Call[T, None] | T | None: puts the device somewhere else. A device a move already owns is asked for nothing, and the setpoints streamed at it while it travels are let go for the same reason. """ - streamed = self.streamed() + newest = self.take_newest_setpoint() if self.busy: return None if (call := next(self._sync_move.incoming(), None)) is not None: return call - return streamed + return newest def accept( self, call: pimm.calls.Call[T, None], target: np.ndarray | float, tol: float, now: float, timeout_s: float @@ -206,7 +205,7 @@ def grip_setpoint(moves: Moves[float], grip: float, now: float) -> float | None: """ if moves.active: # A width streamed at fingers a move owns is older than where the move puts them. - moves.streamed() + 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): From fc7432929fc452631a394958dd92e8003e1715a8 Mon Sep 17 00:00:00 2001 From: DarksaCY Date: Mon, 7 Sep 2026 19:48:25 +0300 Subject: [PATCH 3/8] Let go of the setpoints written while a device was travelling `next_request` drained the stream only while the driver was polling it, and the Franka and YAM loops stop polling for the whole travel of a `sync_move` -- they are held inside `yield from`. So the first poll after the arm arrived handed back a setpoint written on the way there, and drove the arm straight off the target its asker had just been told it reached. `Moves` now notes the tick a call it handed out stopped owning the device, and lets go of every setpoint stamped before it. `next_request` takes the time to note, as `accept` and `settle` already do. The same shape was fixed on the controller side by `_travel`, which drops what arrives while the arms move. --- positronic/drivers/roboarm/franka.py | 2 +- positronic/drivers/roboarm/kinova/driver.py | 2 +- positronic/drivers/roboarm/so101/driver.py | 2 +- positronic/drivers/roboarm/yam.py | 2 +- positronic/drivers/tests/test_utils.py | 27 +++++++++++++++++---- positronic/drivers/utils.py | 19 +++++++++++---- positronic/simulator/mujoco/sim.py | 2 +- 7 files changed, 41 insertions(+), 15 deletions(-) diff --git a/positronic/drivers/roboarm/franka.py b/positronic/drivers/roboarm/franka.py index 7f7370868..495d1dd42 100644 --- a/positronic/drivers/roboarm/franka.py +++ b/positronic/drivers/roboarm/franka.py @@ -497,7 +497,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p yield arm.limiter.wait() continue - asked = arm.moves.next_request() + asked = arm.moves.next_request(clock.now()) if isinstance(asked, pimm.calls.Call): with brakes.opened(): yield from arm.sync_move(asked) diff --git a/positronic/drivers/roboarm/kinova/driver.py b/positronic/drivers/roboarm/kinova/driver.py index 54328fa65..cf190128f 100644 --- a/positronic/drivers/roboarm/kinova/driver.py +++ b/positronic/drivers/roboarm/kinova/driver.py @@ -188,7 +188,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p with self._arm(api, should_stop, clock) as arm: while not should_stop.value: arm.settle() - asked = arm.moves.next_request() + asked = arm.moves.next_request(clock.now()) if isinstance(asked, pimm.calls.Call): arm.sync_move(asked) elif asked is not None: diff --git a/positronic/drivers/roboarm/so101/driver.py b/positronic/drivers/roboarm/so101/driver.py index ed58b0694..a3046e6ad 100644 --- a/positronic/drivers/roboarm/so101/driver.py +++ b/positronic/drivers/roboarm/so101/driver.py @@ -234,7 +234,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p if (grip := pimm.value_updated(self.target_grip)) is not None: arm.hold_grip(grip) arm.settle() - asked = arm.moves.next_request() + asked = arm.moves.next_request(clock.now()) if isinstance(asked, pimm.calls.Call): arm.sync_move(asked) elif asked is not None: diff --git a/positronic/drivers/roboarm/yam.py b/positronic/drivers/roboarm/yam.py index 6611068ee..b43000de7 100644 --- a/positronic/drivers/roboarm/yam.py +++ b/positronic/drivers/roboarm/yam.py @@ -375,7 +375,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p grip_target = float(grip) q = chain.observations()[_JOINT_POS] - asked = chain.moves.next_request() + asked = chain.moves.next_request(clock.now()) if isinstance(asked, pimm.calls.Call): q_target, grip_target = yield from chain.sync_move(asked, q, grip_target) elif asked is not None: diff --git a/positronic/drivers/tests/test_utils.py b/positronic/drivers/tests/test_utils.py index 522783f8e..b0754b027 100644 --- a/positronic/drivers/tests/test_utils.py +++ b/positronic/drivers/tests/test_utils.py @@ -249,33 +249,50 @@ def test_a_settled_move_holds_the_device_against_the_next_one(asking): """Taking another move first would put BUSY over the state the settled move's asker is owed.""" ask, moves, _ = asking ask(0.0) - accepted = moves.next_request() + accepted = moves.next_request(0.0) assert isinstance(accepted, pimm.calls.Call) moves.accept(accepted, 0.0, TOL, now=0.0, timeout_s=3.0) ask(1.0) assert moves.settle(TOL / 2, now=0.1) is MoveStatus.ARRIVED - assert moves.next_request() is None, 'settled, and its asker not yet told' + assert moves.next_request(0.0) is None, 'settled, and its asker not yet told' moves.answer() - assert isinstance(moves.next_request(), pimm.calls.Call) + assert isinstance(moves.next_request(0.0), pimm.calls.Call) def test_a_device_still_travelling_is_asked_for_nothing(asking): """A setpoint applied mid-travel fights the move, and its asker is owed the arrival it was promised.""" ask, moves, stream = asking ask(1.0) - travelling = moves.next_request() + travelling = moves.next_request(0.0) assert isinstance(travelling, pimm.calls.Call) moves.accept(travelling, 1.0, TOL, now=0.0, timeout_s=3.0) stream.push(0.25) ask(0.5) - assert moves.next_request() is None + assert moves.next_request(0.0) is None 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(now=0.0) + assert isinstance(call, pimm.calls.Call) + + stream.push(0.25, ts=int(0.5e9)) # written while the device travelled, and never polled for + call.set_result(None) # the driver blocked for the whole travel and answers on its way out + + assert moves.next_request(now=1.0) is None + + stream.push(0.75, ts=int(1.5e9)) + assert moves.next_request(now=2.0) == 0.75 + + 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 415f65273..686deec94 100644 --- a/positronic/drivers/utils.py +++ b/positronic/drivers/utils.py @@ -43,6 +43,9 @@ class Moves(Generic[T]): def __init__(self, sync_move: pimm.calls.ControlSystemHandler[T, None], async_move: pimm.SignalReceiver[T]): self._sync_move = sync_move self._async_move = async_move + # A setpoint written before this says where the device was wanted on the way to where it now is + self._stale_before = 0.0 + self._handed_out = False self._call: pimm.calls.Call[T, None] | None = None self._target: np.ndarray | float = 0.0 self._tol = 0.0 @@ -79,28 +82,34 @@ def target(self) -> np.ndarray | float: 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. + """The newest setpoint streamed at the device, letting go of every setpoint older than it, and of + every one written before the move the device last made was over. 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 + if message.ts * 1e-9 >= self._stale_before: # the transport stamps in nanoseconds + latest = message.data return latest - def next_request(self) -> pimm.calls.Call[T, None] | T | None: + def next_request(self, now: float) -> 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: 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 the setpoints - streamed at it while it travels are let go for the same reason. + streamed at it while it travels are let go for the same reason -- including where the driver was + held inside the call for the whole travel and polled nothing in between. """ + if self._handed_out and not self.busy: + self._stale_before, self._handed_out = now, False newest = self.take_newest_setpoint() if self.busy: return None if (call := next(self._sync_move.incoming(), None)) is not None: + self._handed_out = True return call return newest @@ -207,7 +216,7 @@ def grip_setpoint(moves: Moves[float], grip: float, now: float) -> float | None: # 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() + asked = moves.next_request(now) if isinstance(asked, pimm.calls.Call): with pimm.calls.raise_to(asked): # a width the fingers cannot be put at is the asker's to hear about target = _clamped(asked.request) diff --git a/positronic/simulator/mujoco/sim.py b/positronic/simulator/mujoco/sim.py index 6b49c25f5..194a871fa 100644 --- a/positronic/simulator/mujoco/sim.py +++ b/positronic/simulator/mujoco/sim.py @@ -183,7 +183,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p self.reset(dict(redraw.request or {}).get(eval_keys.SEED)) redraw.set_result(None) - command = self._moves.next_request() + command = self._moves.next_request(now) if isinstance(command, pimm.calls.Call): self._accept_move(command, now) elif self._error: From 7cae5dad7585492956d88e1cc7d0e98a0a600518 Mon Sep 17 00:00:00 2001 From: DarksaCY Date: Mon, 7 Sep 2026 19:59:57 +0300 Subject: [PATCH 4/8] Note the tick a move ends on where it ends `next_request` inferred the end of a move from the first poll that found the device free. A driver that keeps the move in flight settles it at the end of a tick and polls again only after its limiter sleeps, so a setpoint written in between -- newer than the move -- was read as older and let go. A one-shot command was then lost outright. `settle` notes the tick it ends the move on, and `accept` gives up the inference for the moves it takes. What is left of the inference is the driver held inside the call for the whole travel, which polls again as soon as it returns. --- positronic/drivers/tests/test_utils.py | 16 ++++++++++++++++ positronic/drivers/utils.py | 5 +++++ 2 files changed, 21 insertions(+) diff --git a/positronic/drivers/tests/test_utils.py b/positronic/drivers/tests/test_utils.py index b0754b027..8aa133358 100644 --- a/positronic/drivers/tests/test_utils.py +++ b/positronic/drivers/tests/test_utils.py @@ -293,6 +293,22 @@ def test_a_setpoint_written_while_a_blocking_move_travelled_is_let_go(asking): assert moves.next_request(now=2.0) == 0.75 +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(now=0.0) + 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, ts=int(0.15e9)) # written while the driver slept, and after the move was over + + assert moves.next_request(now=0.2) == 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 686deec94..d49ba4659 100644 --- a/positronic/drivers/utils.py +++ b/positronic/drivers/utils.py @@ -104,6 +104,8 @@ def next_request(self, now: float) -> pimm.calls.Call[T, None] | T | None: held inside the call for the whole travel and polled nothing in between. """ if self._handed_out and not self.busy: + # A driver held inside the call polls again as soon as the travel is over, so this is the tick + # it ended on. One that keeps the move in flight says when it ended itself, in ``settle``. self._stale_before, self._handed_out = now, False newest = self.take_newest_setpoint() if self.busy: @@ -119,6 +121,7 @@ def accept( """Take `call` as the move in flight, aiming at `target` within `tol`, with `timeout_s` to get there.""" self._call, self._target, self._tol = call, target, tol self._deadline = now + timeout_s + self._handed_out = False # the move is in flight, and ``settle`` says which tick it ends on def fail(self, exc: BaseException) -> None: """Hand a settled move its own outcome, and `exc` to one still in flight. Both, if there are both.""" @@ -140,10 +143,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._stale_before = now 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._stale_before = now return MoveStatus.GAVE_UP return MoveStatus.MOVING From 659eb74428a80254e407e3a453e8d07bf007331d Mon Sep 17 00:00:00 2001 From: DarksaCY Date: Tue, 8 Sep 2026 09:09:26 +0300 Subject: [PATCH 5/8] Let a blocking driver say when its travel ended `next_request` inferred the end of a blocking move from the first poll that found the device free, and that inference was wrong three ways. Franka and YAM yield to their limiter before polling again, so a setpoint written after the travel but before the poll was stamped before the cutoff and thrown away. A call refused before the device moved -- a target out of reach -- still armed the cutoff, though nothing had travelled. And the drain ran before the `busy` check, so a setpoint that arrived while a settled move waited to be answered was read and dropped. `Moves.finished` takes the time instead, and the two drivers held inside the call report it when the travel returns. A device a settled move still owns is asked for nothing and its stream is left alone until the answer is out. --- positronic/drivers/roboarm/franka.py | 6 ++- positronic/drivers/roboarm/yam.py | 6 ++- positronic/drivers/tests/test_utils.py | 55 ++++++++++++++++++++++++-- positronic/drivers/utils.py | 24 +++++------ 4 files changed, 74 insertions(+), 17 deletions(-) diff --git a/positronic/drivers/roboarm/franka.py b/positronic/drivers/roboarm/franka.py index 495d1dd42..3f8b8f0af 100644 --- a/positronic/drivers/roboarm/franka.py +++ b/positronic/drivers/roboarm/franka.py @@ -260,7 +260,11 @@ 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: + arrived = yield from self.move_to(self.to_joints(cmd), cmd.mode) + # The loop read nothing for the whole travel, so what was streamed at the arm in the meantime + # says where it was wanted on the way here. + self.moves.finished(self.clock.now()) + if arrived 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 b43000de7..dd94154d1 100644 --- a/positronic/drivers/roboarm/yam.py +++ b/positronic/drivers/roboarm/yam.py @@ -292,7 +292,11 @@ def sync_move( """ try: target = self.to_joints(call.request, q) - if (yield from self.move_to(target, grip)) is MoveStatus.ARRIVED: + arrived = yield from self.move_to(target, grip) + # The loop read nothing for the whole travel, so what was streamed at the chain in the meantime + # says where it was wanted on the way here. + self.moves.finished(self.clock.now()) + if arrived 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 8aa133358..0613c4c63 100644 --- a/positronic/drivers/tests/test_utils.py +++ b/positronic/drivers/tests/test_utils.py @@ -285,12 +285,59 @@ def test_a_setpoint_written_while_a_blocking_move_travelled_is_let_go(asking): assert isinstance(call, pimm.calls.Call) stream.push(0.25, ts=int(0.5e9)) # written while the device travelled, and never polled for - call.set_result(None) # the driver blocked for the whole travel and answers on its way out + call.set_result(None) + moves.finished(1.0) # the driver blocked for the whole travel and says when it ended - assert moves.next_request(now=1.0) is None + assert moves.next_request(now=1.5) is None - stream.push(0.75, ts=int(1.5e9)) - assert moves.next_request(now=2.0) == 0.75 + stream.push(0.75, ts=int(2.0e9)) + assert moves.next_request(now=2.5) == 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 the tick it polls on is later than + the tick the travel ended on. A setpoint written in between is newer than the move.""" + ask, moves, stream = asking + ask(1.0) + call = moves.next_request(now=0.0) + assert isinstance(call, pimm.calls.Call) + call.set_result(None) + moves.finished(1.0) + + stream.push(0.25, ts=int(1.5e9)) # written after the travel, while the driver slept + + assert moves.next_request(now=2.0) == 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(now=0.0) + assert isinstance(call, pimm.calls.Call) + call.set_exception(ValueError('out of reach')) # refused before the device took a step + + stream.push(0.25, ts=int(0.5e9)) + + assert moves.next_request(now=1.0) == 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(now=0.0) + 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, ts=int(0.2e9)) # settled, and its asker not yet told + assert moves.next_request(now=0.3) is None + + moves.answer() + assert moves.next_request(now=0.4) == 0.25 def test_a_setpoint_written_after_a_move_arrived_is_kept(asking): diff --git a/positronic/drivers/utils.py b/positronic/drivers/utils.py index d49ba4659..e8ac7811f 100644 --- a/positronic/drivers/utils.py +++ b/positronic/drivers/utils.py @@ -45,7 +45,6 @@ def __init__(self, sync_move: pimm.calls.ControlSystemHandler[T, None], async_mo self._async_move = async_move # A setpoint written before this says where the device was wanted on the way to where it now is self._stale_before = 0.0 - self._handed_out = False self._call: pimm.calls.Call[T, None] | None = None self._target: np.ndarray | float = 0.0 self._tol = 0.0 @@ -99,29 +98,32 @@ def next_request(self, now: float) -> pimm.calls.Call[T, None] | T | None: nobody waits on, or 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 the setpoints - streamed at it while it travels are let go for the same reason -- including where the driver was - held inside the call for the whole travel and polled nothing in between. + 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: a setpoint written in between + arrived after the move and is what the device does next. """ - if self._handed_out and not self.busy: - # A driver held inside the call polls again as soon as the travel is over, so this is the tick - # it ended on. One that keeps the move in flight says when it ended itself, in ``settle``. - self._stale_before, self._handed_out = now, False - newest = self.take_newest_setpoint() if self.busy: return None + newest = self.take_newest_setpoint() if (call := next(self._sync_move.incoming(), None)) is not None: - self._handed_out = True return call return newest + def finished(self, now: float) -> None: + """The travel a driver was held inside is over, as of ``now``. + + A driver that keeps its move in flight has ``settle`` for this. One that blocks for the whole + travel reads nothing while it moves, so every setpoint written before it returns says where the + device was wanted on the way to the pose it now holds. + """ + self._stale_before = now + def accept( self, call: pimm.calls.Call[T, None], target: np.ndarray | float, tol: float, now: float, timeout_s: float ) -> None: """Take `call` as the move in flight, aiming at `target` within `tol`, with `timeout_s` to get there.""" self._call, self._target, self._tol = call, target, tol self._deadline = now + timeout_s - self._handed_out = False # the move is in flight, and ``settle`` says which tick it ends on def fail(self, exc: BaseException) -> None: """Hand a settled move its own outcome, and `exc` to one still in flight. Both, if there are both.""" From 932015e10cefc3a25425c95c5a9e1fb460f8ae7a Mon Sep 17 00:00:00 2001 From: DarksaCY Date: Tue, 8 Sep 2026 09:34:37 +0300 Subject: [PATCH 6/8] Let go of the stream when a move ends, rather than dating it The cutoff read timestamps, and three things it could not read. A simulated evaluation gives every loop of one scheduler pass the same stamp, so a command written while a move was still active carried the exact cutoff and was kept. `Moves.finished` drops what is queued at the moment the move ends instead, which is the same fact without the arithmetic -- and `next_request` takes no time at all now. `move_to` that raises after the arm travelled skipped the report, so a setpoint from during the failed travel retargeted the arm on the next tick. Franka and YAM report in a `finally`, and refuse a target the arm cannot hold before they enter it. `next_request` drained the stream before it knew whether the call would be taken, so a setpoint queued behind a call the driver then refused was thrown away. The stream is read only where a setpoint is what comes back. --- positronic/drivers/roboarm/franka.py | 13 +++-- positronic/drivers/roboarm/kinova/driver.py | 2 +- positronic/drivers/roboarm/so101/driver.py | 2 +- positronic/drivers/roboarm/yam.py | 14 +++--- positronic/drivers/tests/test_utils.py | 54 ++++++++++----------- positronic/drivers/utils.py | 35 ++++++------- positronic/simulator/mujoco/sim.py | 2 +- 7 files changed, 61 insertions(+), 61 deletions(-) diff --git a/positronic/drivers/roboarm/franka.py b/positronic/drivers/roboarm/franka.py index 3f8b8f0af..b846e96e3 100644 --- a/positronic/drivers/roboarm/franka.py +++ b/positronic/drivers/roboarm/franka.py @@ -260,10 +260,13 @@ 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: - arrived = yield from self.move_to(self.to_joints(cmd), cmd.mode) - # The loop read nothing for the whole travel, so what was streamed at the arm in the meantime - # says where it was wanted on the way here. - self.moves.finished(self.clock.now()) + target = self.to_joints(cmd) # a target the arm cannot hold is refused before it moves + try: + arrived = yield from self.move_to(target, cmd.mode) + finally: + # The arm travelled, however that ended, and the loop read nothing while it did: what was + # streamed at it in the meantime says where it was wanted on the way here. + self.moves.finished() if arrived is MoveStatus.ARRIVED: call.set_result(None) else: @@ -501,7 +504,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p yield arm.limiter.wait() continue - asked = arm.moves.next_request(clock.now()) + asked = arm.moves.next_request() if isinstance(asked, pimm.calls.Call): with brakes.opened(): yield from arm.sync_move(asked) diff --git a/positronic/drivers/roboarm/kinova/driver.py b/positronic/drivers/roboarm/kinova/driver.py index cf190128f..54328fa65 100644 --- a/positronic/drivers/roboarm/kinova/driver.py +++ b/positronic/drivers/roboarm/kinova/driver.py @@ -188,7 +188,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p with self._arm(api, should_stop, clock) as arm: while not should_stop.value: arm.settle() - asked = arm.moves.next_request(clock.now()) + asked = arm.moves.next_request() if isinstance(asked, pimm.calls.Call): arm.sync_move(asked) elif asked is not None: diff --git a/positronic/drivers/roboarm/so101/driver.py b/positronic/drivers/roboarm/so101/driver.py index a3046e6ad..ed58b0694 100644 --- a/positronic/drivers/roboarm/so101/driver.py +++ b/positronic/drivers/roboarm/so101/driver.py @@ -234,7 +234,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p if (grip := pimm.value_updated(self.target_grip)) is not None: arm.hold_grip(grip) arm.settle() - asked = arm.moves.next_request(clock.now()) + asked = arm.moves.next_request() if isinstance(asked, pimm.calls.Call): arm.sync_move(asked) elif asked is not None: diff --git a/positronic/drivers/roboarm/yam.py b/positronic/drivers/roboarm/yam.py index dd94154d1..3bd612464 100644 --- a/positronic/drivers/roboarm/yam.py +++ b/positronic/drivers/roboarm/yam.py @@ -291,11 +291,13 @@ def sync_move( Only an arrival earns the target: commanding it part-way is the jump the ramp exists to avoid. """ try: - target = self.to_joints(call.request, q) - arrived = yield from self.move_to(target, grip) - # The loop read nothing for the whole travel, so what was streamed at the chain in the meantime - # says where it was wanted on the way here. - self.moves.finished(self.clock.now()) + target = self.to_joints(call.request, q) # a target the chain cannot hold is refused first + try: + arrived = yield from self.move_to(target, grip) + finally: + # The chain travelled, however that ended, and the loop read nothing while it did: what was + # streamed at it in the meantime says where it was wanted on the way here. + self.moves.finished() if arrived is MoveStatus.ARRIVED: call.set_result(None) return target, grip @@ -379,7 +381,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p grip_target = float(grip) q = chain.observations()[_JOINT_POS] - asked = chain.moves.next_request(clock.now()) + asked = chain.moves.next_request() if isinstance(asked, pimm.calls.Call): q_target, grip_target = yield from chain.sync_move(asked, q, grip_target) elif asked is not None: diff --git a/positronic/drivers/tests/test_utils.py b/positronic/drivers/tests/test_utils.py index 0613c4c63..d242b16bd 100644 --- a/positronic/drivers/tests/test_utils.py +++ b/positronic/drivers/tests/test_utils.py @@ -249,30 +249,30 @@ def test_a_settled_move_holds_the_device_against_the_next_one(asking): """Taking another move first would put BUSY over the state the settled move's asker is owed.""" ask, moves, _ = asking ask(0.0) - accepted = moves.next_request(0.0) + accepted = moves.next_request() assert isinstance(accepted, pimm.calls.Call) moves.accept(accepted, 0.0, TOL, now=0.0, timeout_s=3.0) ask(1.0) assert moves.settle(TOL / 2, now=0.1) is MoveStatus.ARRIVED - assert moves.next_request(0.0) is None, 'settled, and its asker not yet told' + assert moves.next_request() is None, 'settled, and its asker not yet told' moves.answer() - assert isinstance(moves.next_request(0.0), pimm.calls.Call) + assert isinstance(moves.next_request(), pimm.calls.Call) def test_a_device_still_travelling_is_asked_for_nothing(asking): """A setpoint applied mid-travel fights the move, and its asker is owed the arrival it was promised.""" ask, moves, stream = asking ask(1.0) - travelling = moves.next_request(0.0) + travelling = moves.next_request() assert isinstance(travelling, pimm.calls.Call) moves.accept(travelling, 1.0, TOL, now=0.0, timeout_s=3.0) stream.push(0.25) ask(0.5) - assert moves.next_request(0.0) is None + assert moves.next_request() is None assert moves.settle(0.0, now=0.1) is MoveStatus.MOVING @@ -281,32 +281,32 @@ def test_a_setpoint_written_while_a_blocking_move_travelled_is_let_go(asking): 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(now=0.0) + call = moves.next_request() assert isinstance(call, pimm.calls.Call) - stream.push(0.25, ts=int(0.5e9)) # written while the device travelled, and never polled for + stream.push(0.25) # written while the device travelled, and never polled for call.set_result(None) - moves.finished(1.0) # the driver blocked for the whole travel and says when it ended + moves.finished() # the driver blocked for the whole travel and says so on its way out - assert moves.next_request(now=1.5) is None + assert moves.next_request() is None - stream.push(0.75, ts=int(2.0e9)) - assert moves.next_request(now=2.5) == 0.75 + 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 the tick it polls on is later than - the tick the travel ended on. A setpoint written in between is newer than the move.""" + """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(now=0.0) + call = moves.next_request() assert isinstance(call, pimm.calls.Call) call.set_result(None) - moves.finished(1.0) + moves.finished() - stream.push(0.25, ts=int(1.5e9)) # written after the travel, while the driver slept + stream.push(0.25) # written after the travel, while the driver slept - assert moves.next_request(now=2.0) == 0.25 + assert moves.next_request() == 0.25 def test_a_call_refused_before_the_device_moved_leaves_the_stream_alone(asking): @@ -314,13 +314,13 @@ def test_a_call_refused_before_the_device_moved_leaves_the_stream_alone(asking): says where it was wanted on the way anywhere.""" ask, moves, stream = asking ask(1.0) - call = moves.next_request(now=0.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, ts=int(0.5e9)) + stream.push(0.25) - assert moves.next_request(now=1.0) == 0.25 + assert moves.next_request() == 0.25 def test_a_setpoint_written_while_a_settled_move_waits_to_be_answered_is_kept(asking): @@ -328,16 +328,16 @@ def test_a_setpoint_written_while_a_settled_move_waits_to_be_answered_is_kept(as setpoint that arrives in between was written after the move ended.""" ask, moves, stream = asking ask(1.0) - call = moves.next_request(now=0.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, ts=int(0.2e9)) # settled, and its asker not yet told - assert moves.next_request(now=0.3) is None + stream.push(0.25) # settled, and its asker not yet told + assert moves.next_request() is None moves.answer() - assert moves.next_request(now=0.4) == 0.25 + assert moves.next_request() == 0.25 def test_a_setpoint_written_after_a_move_arrived_is_kept(asking): @@ -345,15 +345,15 @@ def test_a_setpoint_written_after_a_move_arrived_is_kept(asking): 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(now=0.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, ts=int(0.15e9)) # written while the driver slept, and after the move was over + stream.push(0.25) # written while the driver slept, and after the move was over - assert moves.next_request(now=0.2) == 0.25 + assert moves.next_request() == 0.25 def test_a_run_that_dies_with_one_move_settled_and_another_in_flight_answers_both(): diff --git a/positronic/drivers/utils.py b/positronic/drivers/utils.py index e8ac7811f..f94329719 100644 --- a/positronic/drivers/utils.py +++ b/positronic/drivers/utils.py @@ -43,8 +43,6 @@ class Moves(Generic[T]): def __init__(self, sync_move: pimm.calls.ControlSystemHandler[T, None], async_move: pimm.SignalReceiver[T]): self._sync_move = sync_move self._async_move = async_move - # A setpoint written before this says where the device was wanted on the way to where it now is - self._stale_before = 0.0 self._call: pimm.calls.Call[T, None] | None = None self._target: np.ndarray | float = 0.0 self._tol = 0.0 @@ -81,42 +79,39 @@ def target(self) -> np.ndarray | float: 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, and of - every one written before the move the device last made was over. + """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: - if message.ts * 1e-9 >= self._stale_before: # the transport stamps in nanoseconds - latest = message.data + latest = message.data return latest - def next_request(self, now: float) -> pimm.calls.Call[T, None] | T | None: + 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: 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: a setpoint written in between - arrived after the move and is what the device does next. + 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 - newest = self.take_newest_setpoint() if (call := next(self._sync_move.incoming(), None)) is not None: return call - return newest + return self.take_newest_setpoint() - def finished(self, now: float) -> None: - """The travel a driver was held inside is over, as of ``now``. + def finished(self) -> None: + """The move that owned the device is over, so what was streamed at it while it travelled goes. - A driver that keeps its move in flight has ``settle`` for this. One that blocks for the whole - travel reads nothing while it moves, so every setpoint written before it returns says where the - device was wanted on the way to the pose it now holds. + 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. A driver that keeps its move in flight has + ``settle`` for this; one held inside the call for the whole travel says so itself. """ - self._stale_before = now + self.take_newest_setpoint() def accept( self, call: pimm.calls.Call[T, None], target: np.ndarray | float, tol: float, now: float, timeout_s: float @@ -145,12 +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._stale_before = now + self.finished() 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._stale_before = now + self.finished() return MoveStatus.GAVE_UP return MoveStatus.MOVING @@ -223,7 +218,7 @@ def grip_setpoint(moves: Moves[float], grip: float, now: float) -> float | None: # 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(now) + asked = moves.next_request() if isinstance(asked, pimm.calls.Call): with pimm.calls.raise_to(asked): # a width the fingers cannot be put at is the asker's to hear about target = _clamped(asked.request) diff --git a/positronic/simulator/mujoco/sim.py b/positronic/simulator/mujoco/sim.py index 194a871fa..6b49c25f5 100644 --- a/positronic/simulator/mujoco/sim.py +++ b/positronic/simulator/mujoco/sim.py @@ -183,7 +183,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p self.reset(dict(redraw.request or {}).get(eval_keys.SEED)) redraw.set_result(None) - command = self._moves.next_request(now) + command = self._moves.next_request() if isinstance(command, pimm.calls.Call): self._accept_move(command, now) elif self._error: From a5d9a2e044d58459bb0ea82bf0afa3cc60ecaa3c Mon Sep 17 00:00:00 2001 From: DarksaCY Date: Tue, 8 Sep 2026 11:14:44 +0300 Subject: [PATCH 7/8] Let go of the stream only once the device has the target The report sat in a `finally` around the whole travel, and both `move_to` bodies do work before the device is commanded -- Franka reads the arm and applies the mode, YAM reads the chain and takes its starting posture. A failure there discarded the streamed setpoints although nothing had moved. Each reports from inside its own travel, after the target is applied. `Moves.finished` said an event at the caller and did a destructive read. It is `discard_streamed_setpoints`, which is what it does. --- positronic/drivers/roboarm/franka.py | 37 +++++++++++---------- positronic/drivers/roboarm/yam.py | 45 ++++++++++++++------------ positronic/drivers/tests/test_utils.py | 4 +-- positronic/drivers/utils.py | 12 +++---- 4 files changed, 50 insertions(+), 48 deletions(-) diff --git a/positronic/drivers/roboarm/franka.py b/positronic/drivers/roboarm/franka.py index b846e96e3..1c813b748 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,13 +265,7 @@ 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: - target = self.to_joints(cmd) # a target the arm cannot hold is refused before it moves - try: - arrived = yield from self.move_to(target, cmd.mode) - finally: - # The arm travelled, however that ended, and the loop read nothing while it did: what was - # streamed at it in the meantime says where it was wanted on the way here. - self.moves.finished() + arrived = yield from self.move_to(self.to_joints(cmd), cmd.mode) if arrived is MoveStatus.ARRIVED: call.set_result(None) else: diff --git a/positronic/drivers/roboarm/yam.py b/positronic/drivers/roboarm/yam.py index 3bd612464..60d247dab 100644 --- a/positronic/drivers/roboarm/yam.py +++ b/positronic/drivers/roboarm/yam.py @@ -251,20 +251,28 @@ 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() + commanded = False + 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)) + commanded = True + 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: + if commanded: + # The chain took the target, so it travelled however this ends, 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 @@ -291,13 +299,8 @@ def sync_move( Only an arrival earns the target: commanding it part-way is the jump the ramp exists to avoid. """ try: - target = self.to_joints(call.request, q) # a target the chain cannot hold is refused first - try: - arrived = yield from self.move_to(target, grip) - finally: - # The chain travelled, however that ended, and the loop read nothing while it did: what was - # streamed at it in the meantime says where it was wanted on the way here. - self.moves.finished() + target = self.to_joints(call.request, q) + arrived = yield from self.move_to(target, grip) if arrived is MoveStatus.ARRIVED: call.set_result(None) return target, grip diff --git a/positronic/drivers/tests/test_utils.py b/positronic/drivers/tests/test_utils.py index d242b16bd..44e9d748e 100644 --- a/positronic/drivers/tests/test_utils.py +++ b/positronic/drivers/tests/test_utils.py @@ -286,7 +286,7 @@ def test_a_setpoint_written_while_a_blocking_move_travelled_is_let_go(asking): stream.push(0.25) # written while the device travelled, and never polled for call.set_result(None) - moves.finished() # the driver blocked for the whole travel and says so on its way out + moves.discard_streamed_setpoints() # the driver blocked for the whole travel and lets go of what queued up assert moves.next_request() is None @@ -302,7 +302,7 @@ def test_a_setpoint_written_after_a_blocking_move_ended_survives_the_wait_for_th call = moves.next_request() assert isinstance(call, pimm.calls.Call) call.set_result(None) - moves.finished() + moves.discard_streamed_setpoints() stream.push(0.25) # written after the travel, while the driver slept diff --git a/positronic/drivers/utils.py b/positronic/drivers/utils.py index f94329719..65fa5ee0c 100644 --- a/positronic/drivers/utils.py +++ b/positronic/drivers/utils.py @@ -104,12 +104,12 @@ def next_request(self) -> pimm.calls.Call[T, None] | T | None: return call return self.take_newest_setpoint() - def finished(self) -> None: - """The move that owned the device is over, so what was streamed at it while it travelled goes. + 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. A driver that keeps its move in flight has - ``settle`` for this; one held inside the call for the whole travel says so itself. + 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() @@ -140,12 +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.finished() + 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.finished() + self.discard_streamed_setpoints() return MoveStatus.GAVE_UP return MoveStatus.MOVING From b0ba2257449b693dab72e89194dbf6129a61fafd Mon Sep 17 00:00:00 2001 From: DarksaCY Date: Tue, 8 Sep 2026 11:27:47 +0300 Subject: [PATCH 8/8] Let go of the stream after a move that had nowhere to travel The YAM discard hung on the ramp having run, and a call that names where the chain already stands never enters it: the call answered and the next poll drove the chain away with a setpoint written before it. The chain is read before the ramp either way, so the discard follows that read. `arrived` held any `MoveStatus`, `GAVE_UP` included; it is `status`. --- positronic/drivers/roboarm/franka.py | 4 ++-- positronic/drivers/roboarm/yam.py | 13 +++++-------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/positronic/drivers/roboarm/franka.py b/positronic/drivers/roboarm/franka.py index 1c813b748..ee2a48bcf 100644 --- a/positronic/drivers/roboarm/franka.py +++ b/positronic/drivers/roboarm/franka.py @@ -265,8 +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: - arrived = yield from self.move_to(self.to_joints(cmd), cmd.mode) - if arrived 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 60d247dab..fb2003bd8 100644 --- a/positronic/drivers/roboarm/yam.py +++ b/positronic/drivers/roboarm/yam.py @@ -251,7 +251,6 @@ 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() - commanded = False try: while not self._arrived(obs := self.observations(), target, grip): if self.should_stop.value: @@ -263,16 +262,14 @@ def move_to(self, target: np.ndarray, grip: float) -> Generator[pimm.Command, No # 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)) - commanded = True 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: - if commanded: - # The chain took the target, so it travelled however this ends, 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() + # 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 @@ -300,8 +297,8 @@ def sync_move( """ try: target = self.to_joints(call.request, q) - arrived = yield from self.move_to(target, grip) - if arrived 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: