diff --git a/positronic/drivers/roboarm/franka.py b/positronic/drivers/roboarm/franka.py index b045a95b7..8ce2d665e 100644 --- a/positronic/drivers/roboarm/franka.py +++ b/positronic/drivers/roboarm/franka.py @@ -6,6 +6,7 @@ import time import xml.etree.ElementTree as ET from collections.abc import Callable, Generator, Iterator, Mapping +from enum import Enum, auto from pathlib import Path from typing import Any, NamedTuple @@ -460,6 +461,13 @@ def close_if_idle(self, goal: pf.Goal) -> None: self._closed = True +class RecoveryOutcome(Enum): + """Whether the arm came out of error.""" + + CLEARED = auto() + NOT_CLEARED = auto() + + class Robot(pimm.ControlSystem): def __init__( self, @@ -499,6 +507,8 @@ def __init__( self.sync_move = pimm.calls.ControlSystemHandler[command.CommandType, None](self) self.state = pimm.ControlSystemEmitter[FrankaState](self) self.robot_meta = pimm.ControlSystemEmitter(self) + # FOOTGUN: recovers whatever ``state().error`` reads, since a latched Reflex reads 0. + self.recover = pimm.calls.ControlSystemHandler[None, RecoveryOutcome](self) self._load = load self._collision_coeff = collision_coeff self._desk_credentials = _read_desk_credentials() if manage_desk else None @@ -610,6 +620,28 @@ def _arm(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock, safe_inputs: safe_inputs, ) + @staticmethod + def _recover(robot: pf.Robot, asked: list[pimm.calls.Call[None, RecoveryOutcome]]) -> None: + """Run the arm's error recovery once, and answer every caller that asked for it on this tick. + + A throw reaches the callers that asked; one nobody asked for reaches no caller, so it ends the run. + """ + try: + cleared = robot.recover_from_errors() + # rules-allow: swallowed-error — the throw is handed to every caller that asked + except Exception as exc: + if not asked: + raise + logger.exception('The recovery a console asked for failed') + for call in asked: + call.set_exception(exc) + return + if asked: + logger.info(f'A console asked to clear a fault; recover_from_errors returned {cleared}') + outcome = RecoveryOutcome.CLEARED if cleared else RecoveryOutcome.NOT_CLEARED + for call in asked: + call.set_result(outcome) + def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[pimm.Command]: 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: @@ -633,8 +665,10 @@ def run(self, should_stop: pimm.SignalReceiver, clock: pimm.Clock) -> Iterator[p if entered_error: logger.warning(f'Robot error: {st.error_message}') - if in_error: - robot.recover_from_errors() + asked_to_recover = list(self.recover.incoming()) + if asked_to_recover or in_error: + self._recover(robot, asked_to_recover) + # This tick commands nothing; the next one reads the arm the recovery left behind. yield arm.limiter.wait() continue diff --git a/positronic/drivers/roboarm/tests/test_franka.py b/positronic/drivers/roboarm/tests/test_franka.py index e58f81ca9..a23961399 100644 --- a/positronic/drivers/roboarm/tests/test_franka.py +++ b/positronic/drivers/roboarm/tests/test_franka.py @@ -65,8 +65,9 @@ class FakeArm: """In-memory ``pf.Robot``: a commanded joint target is reached after ``polls_to_reach`` reads of ``goal``. ``goal_status`` pins the reported status, so a move that never lands can be scripted; ``raises``, once - set, is what every call but ``stop`` raises, and ``ik_raises`` what only the solver raises; ``error`` is - the vendor fault flag every state carries. + set, is what every call but ``stop`` raises, ``ik_raises`` what only the solver raises, and + ``recover_raises`` what only ``recover_from_errors`` raises; ``error`` is the vendor fault flag every + state carries, and ``recover_clears`` whether a recovery puts it back to 0. """ def __init__(self, q, *, polls_to_reach: int = 2, goal_status: 'franka.pf.GoalStatus | None' = None): @@ -78,6 +79,8 @@ def __init__(self, q, *, polls_to_reach: int = 2, goal_status: 'franka.pf.GoalSt self.raises: Exception | None = None self.raises_once: Exception | None = None self.ik_raises: Exception | None = None + self.recover_raises: Exception | None = None + self.recover_clears = False self.polls_to_reach = polls_to_reach self._polls = 0 self.goal_status = goal_status @@ -110,8 +113,13 @@ def set_target_joints(self, target) -> None: self.targets.append(np.asarray(target, dtype=np.float64)) self._polls = 0 - def recover_from_errors(self) -> None: + def recover_from_errors(self) -> bool: self._record(Call.RECOVER_FROM_ERRORS) + if self.recover_raises is not None: + raise self.recover_raises + if self.recover_clears: + self.error = 0 + return self.error == 0 def stop(self) -> None: self.calls.append(Call.STOP) @@ -216,6 +224,13 @@ def _mover(world: pimm.World, driver: franka.Robot) -> pimm.calls.Caller[command return caller +def _recoverer(world: pimm.World, driver: franka.Robot) -> pimm.calls.Caller[None, franka.RecoveryOutcome]: + """A caller on ``driver.recover``, for a test that pumps its generator rather than running a World.""" + caller = pimm.calls.ControlSystemCaller[None, franka.RecoveryOutcome](driver) + wire_call(world, caller, driver.recover) + return caller + + def test_park_drives_the_arm_to_the_park_pose(): arm = FakeArm(JOGGED) @@ -1077,3 +1092,146 @@ def test_a_command_pinning_no_mode_returns_the_arm_to_its_native_law(desk): _drive(loop, clock) assert isinstance(arm.modes[mark], franka.pf.InternalImpedance) + + +def test_a_console_recover_call_is_answered_that_the_fault_cleared(desk, world): + """A console calls the arm to clear a latched fault: the driver runs the recovery and the answer to + that call carries what it returned.""" + arm = FakeArm(PARK) + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + clock = MockClock() + loop = driver.run(StopFlag(), clock) + + for _ in range(3): # init + the opening move + next(loop) + before = arm.calls.count(Call.RECOVER_FROM_ERRORS) + answer = _recoverer(world, driver)(None) + next(loop) + + assert arm.calls.count(Call.RECOVER_FROM_ERRORS) == before + 1 + assert answer.result() is franka.RecoveryOutcome.CLEARED + + +def test_a_console_recover_call_is_answered_that_the_fault_did_not_clear(desk, world): + """The recovery a console calls for reaches a fault libfranka will not clear, and the answer says so.""" + arm = FakeArm(PARK) + arm.error = 1 # a fault recover_from_errors does not clear + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + clock = MockClock() + loop = driver.run(StopFlag(), clock) + + for _ in range(3): # init + the opening move + next(loop) + answer = _recoverer(world, driver)(None) + next(loop) + + assert answer.result() is franka.RecoveryOutcome.NOT_CLEARED + + +def test_a_recovery_the_vendor_fails_answers_the_console_rather_than_ending_the_run(desk, world): + """libfranka throws mid-recovery on an arm the tick also reads in error. One recovery serves the + console and the fault, so the throw reaches the caller and the run goes on.""" + arm = FakeArm(PARK) + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + recover = _recoverer(world, driver) + clock = MockClock() + loop = driver.run(StopFlag(), clock) + + for _ in range(3): # init + the opening move + next(loop) + arm.error = 1 + arm.recover_raises = RuntimeError('libfranka: control command rejected') + answer = recover(None) + before = arm.calls.count(Call.RECOVER_FROM_ERRORS) + next(loop) + + assert arm.calls.count(Call.RECOVER_FROM_ERRORS) == before + 1, 'the tick ran the recovery twice' + assert answer.done() + with pytest.raises(RuntimeError, match='control command rejected'): + answer.result() + + arm.recover_raises, arm.error = None, 0 + answer = recover(None) + next(loop) + assert answer.result() is franka.RecoveryOutcome.CLEARED, 'the run ended on the failed recovery' + + +def test_a_recovery_that_clears_the_fault_leaves_the_tick_no_second_one(desk, world): + """The console's recovery clears the fault this tick read, so nothing is left for the automatic retry: + it would run on a reading one call out of date.""" + arm = FakeArm(PARK) + arm.recover_clears = True + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + recover = _recoverer(world, driver) + clock = MockClock() + loop = driver.run(StopFlag(), clock) + + for _ in range(3): # init + the opening move + next(loop) + arm.error = 1 + answer = recover(None) + before = arm.calls.count(Call.RECOVER_FROM_ERRORS) + next(loop) + + assert arm.calls.count(Call.RECOVER_FROM_ERRORS) == before + 1, 'the tick ran the recovery twice' + assert answer.result() is franka.RecoveryOutcome.CLEARED + assert arm.error == 0 + + +def test_an_arm_in_error_recovers_with_no_console_asking(desk, world): + """The driver clears a fault it reads itself, whether or not a console asked.""" + arm = FakeArm(PARK) + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + _recoverer(world, driver) + clock = MockClock() + loop = driver.run(StopFlag(), clock) + + for _ in range(3): # init + the opening move + next(loop) + arm.error = 1 + before = arm.calls.count(Call.RECOVER_FROM_ERRORS) + next(loop) + + assert arm.calls.count(Call.RECOVER_FROM_ERRORS) == before + 1 + + +def test_a_recovery_no_console_asked_for_lets_the_vendor_throw_end_the_run(desk, world): + """A throw from the recovery the driver runs for itself has no caller to hand it to, so it ends the + run rather than going unreported.""" + arm = FakeArm(PARK) + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + _recoverer(world, driver) + clock = MockClock() + loop = driver.run(StopFlag(), clock) + + for _ in range(3): # init + the opening move + next(loop) + arm.error = 1 + arm.recover_raises = RuntimeError('libfranka: control command rejected') + + with pytest.raises(RuntimeError, match='control command rejected'): + next(loop) + + +def test_an_arm_with_no_fault_nobody_called_runs_no_recovery(desk, world): + """An arm carrying no fault gives the driver nothing to recover from, so only a call runs a recovery.""" + arm = FakeArm(PARK) + driver = _driver(arm) + driver.state._bind(RecordingEmitter()) + _recoverer(world, driver) + clock = MockClock() + loop = driver.run(StopFlag(), clock) + + for _ in range(3): # init + the opening move + next(loop) + before = arm.calls.count(Call.RECOVER_FROM_ERRORS) + for _ in range(5): + next(loop) + + assert arm.calls.count(Call.RECOVER_FROM_ERRORS) == before