Offer the Franka arm's error recovery on a channel a console asks on

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
 

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