diff --git a/positronic/drivers/roboarm/franka.py b/positronic/drivers/roboarm/franka.py index 7f7370868..b045a95b7 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]: @@ -214,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}') @@ -310,15 +441,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 +597,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 +626,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 +646,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..e58f81ca9 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,258 @@ 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_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)) + 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)