From 373bc2fcb4f0bf2d5fa831fd8ffa816b39beadf2 Mon Sep 17 00:00:00 2001 From: Vladimir Yakunin Date: Mon, 7 Sep 2026 14:21:33 +0000 Subject: [PATCH 1/2] Name the triggered safe input when the arm refuses a move The rig's control box trips safe input x31 on its own. While an input is triggered, libfranka refuses every move and says only "Move command rejected: command not possible in the current mode ("Other")". It prints that from the control thread, unstamped and outside Python. The control box raises no error flag for a safe input, so the driver reads none and the run log names no cause. On 2026-08-25 one run printed hundreds of those lines and stopped at 25 of 45 episodes. A watch thread now reads the safe inputs from Desk every 0.5 s, over a Desk client it owns: the read needs no control token, and the session that drives the arm stays on one thread. It logs one stamped line for an input that changes state. The run loop logs the first move the arm refuses, with libfranka's reason and the inputs the control box reports as triggered, then the count once the arm accepts moves again. A refused move fails and ends the run. Nothing retries it: the control box logs a trip to one second and the driver polls at 0.5 s, so a fast press by a person and a bouncing contact read the same. The refusal log and the brakes both need the goal, so the loop reads it once and hands it to each. Ticket: Positronic-Robotics/internal#729 #refs --- positronic/drivers/roboarm/franka.py | 158 ++++++++++- positronic/drivers/roboarm/tests/conftest.py | 1 + .../drivers/roboarm/tests/test_franka.py | 252 +++++++++++++++++- 3 files changed, 395 insertions(+), 16 deletions(-) diff --git a/positronic/drivers/roboarm/franka.py b/positronic/drivers/roboarm/franka.py index 7f7370868..67797f375 100644 --- a/positronic/drivers/roboarm/franka.py +++ b/positronic/drivers/roboarm/franka.py @@ -2,11 +2,12 @@ import functools import logging import os +import threading import time import xml.etree.ElementTree as ET -from collections.abc import Callable, Generator, Iterator +from collections.abc import Callable, Generator, Iterator, Mapping from pathlib import Path -from typing import Any +from typing import Any, NamedTuple import numpy as np @@ -102,6 +103,105 @@ def _revolute_joint_names(urdf_xml): # Where the driver leaves the arm: taking control it travels here, and handing it back it returns here. _PARK_JOINTS = np.array([0.0, -0.31, 0.0, -1.65, 0.0, 1.522, 0.0]) +# The field Desk answers the safe inputs in. +SAFE_INPUT_STATE = 'safeInputState' + + +class _Reading(NamedTuple): + """One reading of the safe inputs, published in one assignment so a reader never sees half of it.""" + + sampled: bool + triggered: frozenset[str] + + +class _SafeInputs: + """The control box's safe inputs, which libfranka does not report and Desk does. + + A thread reads them every ``_POLL_S``, over a Desk client this owns: the read needs no control + token, and the session that drives the arm must stay on one thread. + """ + + # Desk's own words for a safe input that permits motion. The control box answers a phrase, and its + # safety log records the same two: 'Not triggered (Motion permitted)' and 'Triggered (Motion prohibited)'. + _MOTION_PERMITTED = 'not triggered' + # How often the watch thread reads the safe inputs. + _POLL_S = 0.5 + + def __init__(self, ip: str, credentials: tuple[str, str] | None): + self._ip = ip + self._credentials = credentials + self._desk: Desk | None = None + self._reading = _Reading(False, frozenset()) + self._unreadable = False + self._stop = threading.Event() + self._thread: threading.Thread | None = None + + @property + def triggered(self) -> list[str]: + """The safe inputs the last reading found triggered; empty where no reading is in hand.""" + reading = self._reading + return sorted(reading.triggered) if reading.sampled else [] + + @staticmethod + def _triggered(reading: object) -> bool: + """Whether Desk reports a safe input as triggered. + + A reading this does not recognise counts as triggered: the driver cannot read it as clear. + """ + return _SafeInputs._MOTION_PERMITTED not in str(reading).casefold() + + def sample(self) -> None: + """Take one reading, and log a safe input that changed.""" + credentials = self._credentials + if credentials is None: + return + try: + desk = self._desk + if desk is None: + desk = Desk(self._ip, *credentials) + desk._authenticate() # Desk publishes the read; the login behind it stays underscored + self._desk = desk + self._note(desk.safety_status()[SAFE_INPUT_STATE]) + # rules-allow: swallowed-error — a control box that stops answering must not end the run; the + # reading goes stale instead, and a stale reading names no input. + except Exception as exc: + if not self._unreadable: + logger.error(f'Cannot read the safe inputs: {exc}') + self._desk, self._unreadable = None, True + self._reading = self._reading._replace(sampled=False) + + def _note(self, state: Mapping[str, object]) -> None: + """Record a reading, and log a safe input whose state changed.""" + sampled, was_triggered = self._reading + if not sampled: + logger.info(f'The control box reports its safe inputs as {dict(state)}') + self._unreadable = False + triggered = frozenset(name for name, reading in state.items() if self._triggered(reading)) + if triggered != was_triggered: + if triggered: + logger.warning(f'The control box prohibits motion: safe inputs {sorted(triggered)} are triggered') + else: + logger.info('The control box permits motion: every safe input is clear') + self._reading = _Reading(True, triggered) + + def __enter__(self) -> '_SafeInputs': + """Take the first reading, then keep it fresh on a thread until the block ends.""" + if self._credentials is not None: + self.sample() + self._thread = threading.Thread(target=self._sample_until_stopped, name='franka-safe-inputs', daemon=True) + self._thread.start() + return self + + def __exit__(self, exc_type, exc, tb) -> None: + self._stop.set() + if self._thread is not None: + self._thread.join(timeout=self._POLL_S * 4) + self._thread = None + + def _sample_until_stopped(self) -> None: + while not self._stop.wait(self._POLL_S): + self.sample() + class _Arm(DriverRun[command.CommandType]): """The arm the driver drives: the robot handle, and the state and moves that go with it.""" @@ -112,6 +212,8 @@ class _Arm(DriverRun[command.CommandType]): _MAX_JOINT_VELOCITY = np.array([2.62, 2.62, 2.62, 2.62, 5.26, 4.18, 5.26]) # On top of the travel itself: the robot's controller ramps in and out of its speed cap, and settles late _MOVE_GRACE_S = 5.0 + # How long the arm must accept moves again before the count of the moves it refused is logged. + _REFUSAL_QUIET_S = 2.0 def __init__( self, @@ -122,12 +224,17 @@ def __init__( dynamics_factor: float, should_stop: pimm.SignalReceiver, clock: pimm.Clock, + safe_inputs: _SafeInputs, ): super().__init__(sync_move, async_move, should_stop, clock, hz=2000) self.robot = robot self.out = out self.state = FrankaState() self._dynamics_factor = dynamics_factor + self.safe_inputs = safe_inputs + self._refusals = 0 + self._refused = False + self._quiet_at = 0.0 def __enter__(self) -> '_Arm': return self @@ -159,6 +266,7 @@ def command_target(self, target: np.ndarray, mode: command.ControlModeType | Non """Put the arm under ``mode`` and publish ``target`` to it, in that order with nothing in between.""" self.robot.set_control_mode(self._to_pf_mode(mode)) # the robot no-ops a mode already running self.robot.set_target_joints(target) + self._refused = False # the goal just dispatched is not the one the last reading found refused def await_goal( self, should_stop: Callable[[], bool], pace: Callable[[], pimm.Command] @@ -172,6 +280,7 @@ def await_goal( if goal.status == pf.GoalStatus.REACHED: return MoveStatus.ARRIVED if goal.status != pf.GoalStatus.IN_FLIGHT: + self.note_refusals(goal) raise RuntimeError(f'the arm stopped short of its target: {goal.reason or goal.status}') yield pace() return MoveStatus.GAVE_UP @@ -181,6 +290,27 @@ def _travel_s(self, q: np.ndarray, target: np.ndarray) -> float: cap = self._MAX_JOINT_VELOCITY * self._dynamics_factor return self._MOVE_GRACE_S + float(np.max(np.abs(target - q) / cap)) + def note_refusals(self, goal: pf.Goal) -> None: + """Log the moves the arm refuses: the first one as it happens, the rest as a count once they stop. + + libfranka prints the same rejection from the control thread, unstamped and outside Python. + """ + refused = goal.status is pf.GoalStatus.ABORTED + if refused and not self._refused: + self._refusals += 1 + self._quiet_at = self.clock.now() + self._REFUSAL_QUIET_S + if self._refusals == 1: + triggered = self.safe_inputs.triggered + cause = f'; safe inputs {triggered} are triggered' if triggered else '' + logger.warning(f'The arm refused a move: {goal.reason or goal.status}{cause}') + self._refused = refused + # A goal the arm reached breaks the streak outright; any other unrefused one has to hold for the quiet time. + reached = goal.status is pf.GoalStatus.REACHED + if self._refusals and (reached or (not refused and self.clock.now() >= self._quiet_at)): + if self._refusals > 1: # the line above already reported a single one, with its reason + logger.warning(f'The arm refused {self._refusals} moves in a row; it accepts them again') + self._refusals = 0 + def move_to( self, target: np.ndarray, mode: command.ControlModeType | None, *, at_teardown: bool = False ) -> Generator[pimm.Command, None, MoveStatus]: @@ -310,15 +440,15 @@ def opened(self) -> Iterator[None]: finally: self._idle_since = self._clock.now() - def close_if_idle(self) -> None: + def close_if_idle(self, goal: pf.Goal) -> None: """Close the brakes once the idle time passes with the arm at rest. - A streamed setpoint is published and done with, so the goal is the only thing that says the arm is + A streamed setpoint is published and done with, so ``goal`` is the only thing that says the arm is still travelling towards it. """ if self._closed or self._desk is None or self._after_idle_s is None: return - if self._robot.goal().status == pf.GoalStatus.IN_FLIGHT: + if goal.status == pf.GoalStatus.IN_FLIGHT: self._idle_since = self._clock.now() return if self._clock.now() - self._idle_since < self._after_idle_s: @@ -466,14 +596,22 @@ def _robot(self) -> pf.Robot: self._ip, realtime_config=pf.RealtimeConfig.Ignore, relative_dynamics_factor=self._relative_dynamics_factor ) - def _arm(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> _Arm: + def _arm(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock, safe_inputs: _SafeInputs) -> _Arm: """The arm this run drives, built from the driver's configuration.""" return _Arm( - self._robot, self.sync_move, self.commands, self.state, self._relative_dynamics_factor, should_stop, clock + self._robot, + self.sync_move, + self.commands, + self.state, + self._relative_dynamics_factor, + should_stop, + clock, + safe_inputs, ) def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[pimm.Command]: - with self._desk_session() as desk, self._arm(should_stop, clock) as arm: + safe_inputs = _SafeInputs(self._ip, self._desk_credentials) + with self._desk_session() as desk, safe_inputs, self._arm(should_stop, clock, safe_inputs) as arm: robot = arm.robot self._init_robot(robot) self.robot_meta.emit(Robot._build_robot_meta(robot)) @@ -487,6 +625,8 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p while not should_stop.value: st = robot.state() arm.publish(st) + goal = robot.goal() + arm.note_refusals(goal) in_error, entered_error = _check_error(st.error != 0, in_error) if entered_error: @@ -505,7 +645,7 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p with brakes.opened(), log_failure(asked): arm.command_target(arm.to_joints(asked), asked.mode) else: - brakes.close_if_idle() + brakes.close_if_idle(goal) yield arm.limiter.wait() diff --git a/positronic/drivers/roboarm/tests/conftest.py b/positronic/drivers/roboarm/tests/conftest.py index 0612414dc..08f4f9701 100644 --- a/positronic/drivers/roboarm/tests/conftest.py +++ b/positronic/drivers/roboarm/tests/conftest.py @@ -47,6 +47,7 @@ def __init__( vendor = types.ModuleType(VENDOR) vendor.__dict__.update( GoalStatus=GoalStatus, + Goal=object, State=object, Robot=object, RealtimeConfig=types.SimpleNamespace(Ignore=object()), diff --git a/positronic/drivers/roboarm/tests/test_franka.py b/positronic/drivers/roboarm/tests/test_franka.py index fcb9f4cbe..23abd2df8 100644 --- a/positronic/drivers/roboarm/tests/test_franka.py +++ b/positronic/drivers/roboarm/tests/test_franka.py @@ -1,3 +1,4 @@ +import logging from dataclasses import dataclass from enum import StrEnum from pathlib import Path @@ -17,6 +18,9 @@ PARK = np.array([0.0, -0.31, 0.0, -1.65, 0.0, 1.522, 0.0]) JOGGED = PARK + np.array([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) IMPEDANCE = command.Impedance(kq=(40.0,) * 7, kqd=(4.0,) * 7, kx=(750.0,) * 6, kxd=(37.0,) * 6) +# What the control box answers for a safe input, in its own words. +CLEAR = 'Not triggered (Motion permitted)' +STOPPED = 'Triggered (Motion prohibited)' class Call(StrEnum): @@ -41,6 +45,12 @@ class _Goal: reason: str | None +# A goal the arm would not take, and one it did. +REFUSED = _Goal(franka.pf.GoalStatus.ABORTED, 'scripted') +ACCEPTED = _Goal(franka.pf.GoalStatus.IN_FLIGHT, None) +ARRIVED = _Goal(franka.pf.GoalStatus.REACHED, None) + + @dataclass class _ArmState: q: np.ndarray @@ -127,13 +137,14 @@ def set_load(self, *load) -> None: class FakeDesk: - """In-memory ``Desk``: records that the session prepared the robot and released control, and every brake - operation the driver asked for.""" + """In-memory ``Desk``: records that the session prepared the robot and released control, records every + brake operation the driver asked for, and reports whatever ``safe_inputs`` holds.""" def __init__(self): self.prepared = False self.released = False self.calls: list[Call] = [] + self.safe_inputs = dict.fromkeys(('x31', 'x32', 'x33', 'x4'), CLEAR) def __enter__(self) -> 'FakeDesk': return self @@ -151,6 +162,12 @@ def open_brakes(self) -> None: def close_brakes(self) -> None: self.calls.append(Call.CLOSE_BRAKES) + def _authenticate(self) -> None: + pass + + def safety_status(self) -> dict[str, Any]: + return {franka.SAFE_INPUT_STATE: dict(self.safe_inputs)} + @pytest.fixture def desk(monkeypatch) -> FakeDesk: @@ -175,10 +192,20 @@ def _drive(loop, clock: MockClock | None = None) -> None: clock.advance(wait.seconds) +def _safe_inputs(driver: franka.Robot) -> franka._SafeInputs: + """The watch the driver builds for itself, from the Desk credentials its configuration reaches.""" + return franka._SafeInputs(driver._ip, driver._desk_credentials) + + +def _arm(driver: franka.Robot, clock: MockClock) -> franka._Arm: + """The driver's arm, watching the safe inputs its own configuration reaches.""" + return driver._arm(StopFlag(), clock, _safe_inputs(driver)) + + def _drive_park(driver: franka.Robot, arm: FakeArm) -> MockClock: """Park ``arm`` under a clock that moves only by the waits the park itself asks for.""" clock = MockClock() - _drive(driver._arm(StopFlag(), clock).park(), clock) + _drive(_arm(driver, clock).park(), clock) return clock @@ -202,7 +229,7 @@ def test_the_park_waits_by_yielding_rather_than_blocking(): """A driver's waits are the world's to honour, teardown included: the park asks for them, never sleeps.""" arm = FakeArm(JOGGED, polls_to_reach=3) - commands = list(_driver(arm, manage_desk=False)._arm(StopFlag(), MockClock()).park()) + commands = list(_arm(_driver(arm, manage_desk=False), MockClock()).park()) assert commands and all(isinstance(command, pimm.Sleep | pimm.Yield) for command in commands) @@ -219,7 +246,7 @@ def test_park_gives_up_when_the_goal_stops_advancing(): def test_park_gives_up_when_the_arm_does_not_arrive_in_time(): arm = FakeArm(JOGGED, polls_to_reach=10**9) clock = MockClock() - parking = _driver(arm, manage_desk=False)._arm(StopFlag(), clock) + parking = _arm(_driver(arm, manage_desk=False), clock) budget = parking._travel_s(JOGGED, PARK) _drive(parking.park(), clock) @@ -692,7 +719,7 @@ def test_a_move_that_lands_as_its_deadline_expires_is_an_arrival(): driver = _driver(arm, manage_desk=False) driver.state._bind(RecordingEmitter()) clock = MockClock() - travel = driver._arm(StopFlag(), clock).move_to(JOGGED, None) + travel = _arm(driver, clock).move_to(JOGGED, None) next(travel) # the first poll: the goal is in flight clock.advance(60.0) # the deadline expires @@ -711,7 +738,7 @@ def test_a_fault_that_lands_with_the_arrival_reads_error_rather_than_available() driver = _driver(arm, manage_desk=False) states = RecordingEmitter() driver.state._bind(states) - travel = driver._arm(StopFlag(), MockClock()).move_to(JOGGED, None) + travel = _arm(driver, MockClock()).move_to(JOGGED, None) arm.error = 1 with pytest.raises(StopIteration) as done: @@ -751,6 +778,217 @@ def test_a_sync_move_that_never_arrives_times_out_and_holds_where_the_arm_stoppe assert states.emitted[-1][1].status == RobotStatus.ERROR +def _refusals(caplog) -> list[str]: + """The lines the refusal log wrote.""" + return [record.message for record in caplog.records if record.message.startswith('The arm refused a move')] + + +def test_a_reading_the_driver_does_not_recognise_counts_as_a_triggered_safe_input(): + """A phrase the driver does not recognise reads as triggered, never as clear.""" + assert not franka._SafeInputs._triggered(CLEAR) + assert franka._SafeInputs._triggered(STOPPED) + assert franka._SafeInputs._triggered('a phrase this control box has never sent') + + +def test_the_driver_logs_a_safe_input_that_changes(desk, caplog): + """A safe input that goes triggered logs a prohibition, and one that clears logs a permission.""" + caplog.set_level(logging.INFO) + watch = _safe_inputs(_driver(FakeArm(PARK))) + + watch.sample() + desk.safe_inputs['x31'] = STOPPED + watch.sample() + desk.safe_inputs['x31'] = CLEAR + watch.sample() + + assert "safe inputs ['x31'] are triggered" in caplog.text + assert 'permits motion' in caplog.text + + +def test_entering_the_watch_leaves_a_reading_in_hand(desk): + """The watch samples before it returns, so it never reports a clear box it has not read.""" + watch = _safe_inputs(_driver(FakeArm(PARK))) + desk.safe_inputs['x31'] = STOPPED + + with watch: + assert watch.triggered == ['x31'] + + +def test_the_refusal_the_arm_logs_names_the_safe_input_that_prohibits_motion(desk, caplog): + """libfranka's own words name no cause, so the refused move carries the input the control box reports.""" + driver = _driver(FakeArm(PARK)) + watch = _safe_inputs(driver) + desk.safe_inputs['x31'] = STOPPED + watch.sample() + + driver._arm(StopFlag(), MockClock(), watch).note_refusals(REFUSED) + + assert _refusals(caplog) == ["The arm refused a move: scripted; safe inputs ['x31'] are triggered"] + + +def test_a_refusal_names_no_safe_input_where_nothing_reads_them(desk, caplog): + """Without a reading there is nothing to attribute the refusal to, and the line says only what the arm said.""" + driver = _driver(FakeArm(PARK), manage_desk=False) + + _arm(driver, MockClock()).note_refusals(REFUSED) + + assert _refusals(caplog) == ['The arm refused a move: scripted'] + + +def test_a_refusal_names_no_safe_input_the_control_box_has_stopped_confirming(desk, caplog): + """A trip nobody can confirm still standing is not evidence about the move the arm refuses now.""" + driver = _driver(FakeArm(PARK)) + watch = _safe_inputs(driver) + desk.safe_inputs['x31'] = STOPPED + watch.sample() # a trip is on the record, and then the control box goes quiet + + def unreachable() -> dict[str, Any]: + raise ConnectionError('the control box stopped answering') + + desk.safety_status = unreachable + watch.sample() + driver._arm(StopFlag(), MockClock(), watch).note_refusals(REFUSED) + + assert _refusals(caplog) == ['The arm refused a move: scripted'] + + +def test_the_driver_logs_one_line_for_a_wall_of_refusals(desk, caplog): + """The wall is hundreds of lines libfranka prints itself, so a line per refusal buries the one that names why.""" + driver = _driver(FakeArm(PARK)) + watching = _arm(driver, MockClock()) + + watching.note_refusals(REFUSED) + watching.note_refusals(REFUSED) + + assert _refusals(caplog) == ['The arm refused a move: scripted'], 'the wall of refusals was logged in full' + + +def test_a_second_move_the_arm_refuses_is_counted_with_the_first(desk, caplog): + """The count is the diagnosis, so refusals spanning two moves must not read as one.""" + arm = FakeArm(PARK) + driver = _driver(arm) + clock = MockClock() + watching = _arm(driver, clock) + + watching.note_refusals(REFUSED) # the arm refuses one move + watching.command_target(PARK, None) # the next is dispatched + watching.note_refusals(REFUSED) # and refused in its turn + clock.advance(franka._Arm._REFUSAL_QUIET_S) + watching.note_refusals(ACCEPTED) # then the arm takes a goal again + + assert 'The arm refused 2 moves in a row' in caplog.text + + +def test_a_refusal_that_never_lets_up_is_not_reported_as_recovered(desk, caplog): + """The summary says the arm accepts moves again, so a goal it has accepted is what earns it.""" + driver = _driver(FakeArm(PARK)) + clock = MockClock() + watching = _arm(driver, clock) + + watching.note_refusals(REFUSED) + watching.command_target(PARK, None) + watching.note_refusals(REFUSED) # and every goal after it stays refused + clock.advance(franka._Arm._REFUSAL_QUIET_S * 3) + watching.note_refusals(REFUSED) + + assert 'accepts them again' not in caplog.text + + +def test_a_move_a_safe_input_stopped_fails_rather_than_going_again(desk): + """The driver cannot tell a bouncing contact from a person's hand, so a trip ends the move every time.""" + arm = FakeArm(PARK, goal_status=franka.pf.GoalStatus.ABORTED) + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + clock = MockClock() + desk.safe_inputs['x31'] = STOPPED + travel = _arm(driver, clock).move_to(JOGGED, None) + + with pytest.raises(RuntimeError, match='stopped short'): + _drive(travel, clock) + + assert clock.now() == 0.0, 'the move waited on the safe input rather than failing' + assert arm.calls.count(Call.SET_TARGET_JOINTS) == 1, 'the arm was sent to the target a second time' + assert arm.calls.count(Call.RECOVER_FROM_ERRORS) == 0, 'a triggered safe input was answered with a recovery' + + +def test_a_refused_sync_move_logs_the_refusal_itself(desk, world, caplog): + """The move that fails logs the refusal itself.""" + arm = FakeArm(PARK) + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + move = _mover(world, driver) + clock = MockClock() + watch = _safe_inputs(driver) + desk.safe_inputs['x31'] = STOPPED + watch.sample() + driving = driver._arm(StopFlag(), clock, watch) + answer = move(command.JointPosition(JOGGED)) + asked = driving.moves.next_request() + assert isinstance(asked, pimm.calls.Call) + arm.goal_status = franka.pf.GoalStatus.ABORTED # the arm refuses only once the move is under way + + _drive(driving.sync_move(asked), clock) + + with pytest.raises(RuntimeError, match='stopped short'): + answer.result() + assert _refusals(caplog) == ["The arm refused a move: scripted; safe inputs ['x31'] are triggered"] + + +def test_the_teardown_park_logs_the_move_the_arm_refused(desk, caplog): + """The park swallows its own failure, so the refusal has to be recorded before it does.""" + arm = FakeArm(PARK, goal_status=franka.pf.GoalStatus.ABORTED) + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + clock = MockClock() + watch = _safe_inputs(driver) + desk.safe_inputs['x31'] = STOPPED + watch.sample() + + _drive(driver._arm(StopFlag(), clock, watch).park(at_teardown=True), clock) + + assert _refusals(caplog) == ["The arm refused a move: scripted; safe inputs ['x31'] are triggered"] + + +def test_a_move_the_arm_reached_ends_the_refusal_streak(desk, caplog): + """The count says the refusals ran in a row, so a goal the arm reached has to end it.""" + driver = _driver(FakeArm(PARK)) + clock = MockClock() + watching = _arm(driver, clock) + + watching.note_refusals(REFUSED) + watching.note_refusals(ARRIVED) # the arm reaches a goal, inside the quiet time + watching.command_target(PARK, None) + watching.note_refusals(REFUSED) # and refuses a later one, which starts its own streak + clock.advance(franka._Arm._REFUSAL_QUIET_S) + watching.note_refusals(ARRIVED) + + assert 'moves in a row' not in caplog.text + assert len(_refusals(caplog)) == 2, 'the two refusals were counted as one streak' + + +def test_a_run_whose_move_the_arm_refuses_fails_the_asker_and_logs_the_refusal(desk, world, caplog): + """End to end: the move fails the caller, and the refusal is logged as it fails.""" + arm = FakeArm(PARK) + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + move = _mover(world, driver) + clock = MockClock() + loop = driver.run(StopFlag(), clock) + + for _ in range(3): # init + the opening move + next(loop) + arm.goal_status = franka.pf.GoalStatus.ABORTED + answer = move(command.JointPosition(JOGGED)) + next(loop) # into the move, which the arm refuses + + with pytest.raises(RuntimeError, match='stopped short'): + answer.result() + assert _refusals(caplog) == ['The arm refused a move: scripted'] + + next(loop) # and the loop carries on rather than raising + assert arm.calls.count(Call.SET_TARGET_JOINTS) == 2, 'the refused move was made again' + + def test_a_commands_mode_reaches_the_arm_with_the_gains_it_named(desk): """Skipping a mode already running is the vendor's, so the driver hands over every command's.""" arm = FakeArm(PARK) From c1cc820e8ac2f94a6fcf3832d43e9e97b2b918e3 Mon Sep 17 00:00:00 2001 From: Vladimir Yakunin Date: Sat, 12 Sep 2026 12:09:49 +0000 Subject: [PATCH 2/2] Record a refusal the deadline would otherwise drop `move_to` reads the goal once more after the poll loop exits, then replaces it with a hold target. A goal that turned ABORTED in the last gap before the deadline was never passed to `note_refusals`, so that refusal reached no log. The run loop's own per-tick call catches such a goal on its next iteration; the teardown park has no next iteration. Ticket: Positronic-Robotics/internal#729 #refs --- positronic/drivers/roboarm/franka.py | 3 +- .../drivers/roboarm/tests/test_franka.py | 41 +++++++++++++++++++ 2 files changed, 43 insertions(+), 1 deletion(-) diff --git a/positronic/drivers/roboarm/franka.py b/positronic/drivers/roboarm/franka.py index 67797f375..b045a95b7 100644 --- a/positronic/drivers/roboarm/franka.py +++ b/positronic/drivers/roboarm/franka.py @@ -344,7 +344,8 @@ def should_stop() -> bool: 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: + if expired() and (missed := self.robot.goal()).status != pf.GoalStatus.REACHED: + self.note_refusals(missed) # the hold target below replaces it, and any refusal it carries # 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}') diff --git a/positronic/drivers/roboarm/tests/test_franka.py b/positronic/drivers/roboarm/tests/test_franka.py index 23abd2df8..e58f81ca9 100644 --- a/positronic/drivers/roboarm/tests/test_franka.py +++ b/positronic/drivers/roboarm/tests/test_franka.py @@ -949,6 +949,47 @@ def test_the_teardown_park_logs_the_move_the_arm_refused(desk, caplog): assert _refusals(caplog) == ["The arm refused a move: scripted; safe inputs ['x31'] are triggered"] +def test_a_move_the_arm_refused_as_its_deadline_expired_is_still_logged(desk, caplog): + """The hold target the deadline sets replaces the goal, so nothing after this reading can name the refusal.""" + arm = FakeArm(PARK, polls_to_reach=10**9) # it never lands on a poll of its own + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + clock = MockClock() + watch = _safe_inputs(driver) + desk.safe_inputs['x31'] = STOPPED + watch.sample() + travel = driver._arm(StopFlag(), clock, watch).move_to(JOGGED, None) + + next(travel) # the first poll: the goal is in flight + clock.advance(60.0) # the deadline expires + arm.goal_status = franka.pf.GoalStatus.ABORTED # and the arm refuses in the same moment + + with pytest.raises(TimeoutError, match='stopped short'): + next(travel) + + assert _refusals(caplog) == ["The arm refused a move: scripted; safe inputs ['x31'] are triggered"] + + +def test_a_move_that_merely_ran_out_of_time_is_no_refusal(desk, caplog): + """The count is of refusals, and a goal still in flight at the deadline has refused nothing.""" + arm = FakeArm(PARK, polls_to_reach=10**9) # it never lands on a poll of its own + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + clock = MockClock() + watch = _safe_inputs(driver) + desk.safe_inputs['x31'] = STOPPED + watch.sample() + travel = driver._arm(StopFlag(), clock, watch).move_to(JOGGED, None) + + next(travel) # the first poll: the goal is in flight + clock.advance(60.0) # the deadline expires, and the goal is still in flight + + with pytest.raises(TimeoutError, match='stopped short'): + next(travel) + + assert _refusals(caplog) == [] + + def test_a_move_the_arm_reached_ends_the_refusal_streak(desk, caplog): """The count says the refusals ran in a row, so a goal the arm reached has to end it.""" driver = _driver(FakeArm(PARK))