Drive the Trossen station from the leader arm beside it

.basedpyright/baseline.json

@@ -1155,30 +1155,6 @@
             }
         ],
         "./positronic/data_collection.py": [
-            {
-                "code": "reportOptionalOperand",
-                "range": {
-                    "startColumn": 25,
-                    "endColumn": 48,
-                    "lineCount": 1
-                }
-            },
-            {
-                "code": "reportOptionalMemberAccess",
-                "range": {
-                    "startColumn": 89,
-                    "endColumn": 92,
-                    "lineCount": 1
-                }
-            },
-            {
-                "code": "reportPossiblyUnboundVariable",
-                "range": {
-                    "startColumn": 64,
-                    "endColumn": 80,
-                    "lineCount": 1
-                }
-            },
             {
                 "code": "reportReturnType",
                 "range": {
@@ -1211,14 +1187,6 @@
                     "lineCount": 1
                 }
             },
-            {
-                "code": "reportArgumentType",
-                "range": {
-                    "startColumn": 73,
-                    "endColumn": 99,
-                    "lineCount": 1
-                }
-            },
             {
                 "code": "reportAttributeAccessIssue",
                 "range": {
@@ -1227,14 +1195,6 @@
                     "lineCount": 1
                 }
             },
-            {
-                "code": "reportAttributeAccessIssue",
-                "range": {
-                    "startColumn": 66,
-                    "endColumn": 71,
-                    "lineCount": 1
-                }
-            },
             {
                 "code": "reportAssignmentType",
                 "range": {
@@ -3331,30 +3291,6 @@
                     "lineCount": 1
                 }
             },
-            {
-                "code": "reportArgumentType",
-                "range": {
-                    "startColumn": 46,
-                    "endColumn": 50,
-                    "lineCount": 1
-                }
-            },
-            {
-                "code": "reportAttributeAccessIssue",
-                "range": {
-                    "startColumn": 43,
-                    "endColumn": 49,
-                    "lineCount": 1
-                }
-            },
-            {
-                "code": "reportOptionalSubscript",
-                "range": {
-                    "startColumn": 32,
-                    "endColumn": 38,
-                    "lineCount": 1
-                }
-            },
             {
                 "code": "reportReturnType",
                 "range": {
@@ -3997,22 +3933,6 @@
             }
         ],
         "./positronic/drivers/webxr.py": [
-            {
-                "code": "reportArgumentType",
-                "range": {
-                    "startColumn": 12,
-                    "endColumn": 38,
-                    "lineCount": 1
-                }
-            },
-            {
-                "code": "reportArgumentType",
-                "range": {
-                    "startColumn": 12,
-                    "endColumn": 30,
-                    "lineCount": 1
-                }
-            },
             {
                 "code": "reportArgumentType",
                 "range": {
@@ -4021,22 +3941,6 @@
                     "lineCount": 1
                 }
             },
-            {
-                "code": "reportAttributeAccessIssue",
-                "range": {
-                    "startColumn": 59,
-                    "endColumn": 70,
-                    "lineCount": 1
-                }
-            },
-            {
-                "code": "reportAttributeAccessIssue",
-                "range": {
-                    "startColumn": 59,
-                    "endColumn": 70,
-                    "lineCount": 1
-                }
-            },
             {
                 "code": "reportArgumentType",
                 "range": {
@@ -6977,4 +6881,4 @@
             }
         ]
     }
-}
+}

pimm/logging.py

@@ -39,6 +39,7 @@
     'boto3',
     's3transfer',  # per part of a multipart upload
     'asyncio',  # per selector event, under its debug mode
+    'linuxpy',  # per ioctl, which is several times a frame for every camera
 )
 
 

pimm/tests/test_calls.py

@@ -175,6 +175,17 @@ def run(self, should_stop, clock):
         yield Sleep(0.001)
 
 
+class Interrupted(ControlSystem):
+    """Ends on the interrupt an operator sends, which reaches every process of a run at once."""
+
+    def __init__(self):
+        self.add = ControlSystemHandler[tuple[int, int], int](self)
+
+    def run(self, should_stop, clock):
+        yield Sleep(0.001)
+        raise KeyboardInterrupt
+
+
 class TestAllOf:
     def test_one_answer_stands_for_many(self, bound):
         caller, handler = bound
@@ -267,6 +278,18 @@ def test_a_handler_whose_system_ends_answers_the_calls_it_never_reached(self):
             world.run([client, deaf])
         assert isinstance(client.results[0], HandlerStopped)
 
+    def test_a_handler_the_operator_interrupts_answers_nothing(self):
+        """An interrupt can land inside a manager call, and the connection then carries half a message.
+
+        Reading it again returns the tail of another, so an interrupted system says nothing at all.
+        """
+        client, interrupted = Client([(1, 2)]), Interrupted()
+        with World(virtual_time=True) as world:
+            world.connect(client.add, interrupted.add)
+            with pytest.raises(KeyboardInterrupt):
+                world.run([client, interrupted])
+        assert client.results == []
+
     def test_a_stopped_handler_survives_the_trip_to_another_process(self):
         """A reply crosses a pipe as pickle, and an exception is rebuilt by calling its class with its args."""
         assert isinstance(pickle.loads(pickle.dumps(HandlerStopped())), HandlerStopped)

pimm/tests/test_world.py

@@ -9,6 +9,7 @@
 
 import pytest
 
+import pimm.world
 from pimm.core import (
     ControlSystem,
     ControlSystemEmitter,
@@ -99,6 +100,24 @@ def read_from_buffer(self, buffer: memoryview | bytes) -> None:
         self.value = struct.unpack('d', buffer[:8])[0]
 
 
+def test_a_process_that_took_an_interrupt_stops_talking_to_the_manager(monkeypatch):
+    """An interrupt can land inside a call to the manager, and that connection then holds half a message.
+
+    Reading it again returns what another call asked for, so a reader takes a value off a channel it never
+    subscribed to. A process that has taken one neither reads nor sends after it.
+    """
+    with World() as world:
+        emitter, receiver = world.mp_pipes()
+        assert not isinstance(receiver, list)  # one receiver, which is what `mp_pipes` makes by default
+        emitter.emit('before', ts=1)
+        assert receiver.read() is not None
+
+        monkeypatch.setattr(pimm.world, '_interrupted', True)
+
+        emitter.emit('after', ts=2)  # dropped, not sent
+        assert receiver.read() is None
+
+
 class TestQueueEmitter:
     """Test the QueueEmitter class."""
 

pimm/world.py

@@ -11,6 +11,7 @@
 import traceback
 from collections import Counter, defaultdict, deque
 from collections.abc import Callable, Iterator, Mapping
+from contextlib import contextmanager
 from enum import IntEnum
 from multiprocessing import resource_tracker
 from multiprocessing.managers import ValueProxy
@@ -42,6 +43,7 @@
 logger = logging.getLogger(__name__)
 
 T = TypeVar('T')
+U = TypeVar('U')
 Req = TypeVar('Req')
 Res = TypeVar('Res')
 
@@ -75,6 +77,28 @@ def emit(self, data: T, ts: int = -1):
                 pass
 
 
+# Set in a process that has taken an interrupt. An interrupt can land inside a call to the manager, and
+# that connection then holds half a message: the next call over it returns what another one asked for, so
+# a reader takes a value from a channel it never subscribed to. Nothing may be sent or read after it.
+_interrupted = False
+
+
+@contextmanager
+def _noting_interrupt() -> Iterator[None]:
+    """Record an interrupt taken inside the block, and let it go on.
+
+    A connection is torn by an interrupt that lands in the middle of a call over it, so every process that
+    reaches a transport records its own -- there is nowhere else the tearing can happen, and no process has
+    to have had a handler installed for it.
+    """
+    global _interrupted
+    try:
+        yield
+    except KeyboardInterrupt:
+        _interrupted = True
+        raise
+
+
 class MultiprocessEmitter(SignalEmitter[T]):
     """Signal emitter that transparently bridges processes.
 
@@ -188,9 +212,12 @@ def _emit_shared_memory(self, data: SMCompliant, ts: int) -> bool:
 
         return True
 
+    @_noting_interrupt()
     def emit(self, data: T, ts: int = -1):
+        if _interrupted:
+            return
         ts = ts if ts >= 0 else self._clock.now_ns()
-        mode = self._ensure_mode(data)
+        mode = self._ensure_mode(data)  # itself a call to the manager, so it sits inside the guard
 
         if mode is TransportMode.SHARED_MEMORY:
             if not isinstance(data, SMCompliant):
@@ -275,6 +302,10 @@ def _read_queue(self) -> Message[T] | None:
         except Empty:
             message = None
         else:
+            if not isinstance(message, Message):
+                # An interrupt that lands inside a manager call leaves that connection holding half a
+                # message, and every read after it comes back as whatever another call asked for.
+                raise ConnectionError(f'the queue was read after an interrupt tore its connection: {message!r}')
             self._last_queue_message = Message(message.data, message.ts, True)
             if self._mode is TransportMode.UNDECIDED:
                 self._mode = TransportMode.QUEUE
@@ -332,8 +363,11 @@ def _read_shared_memory(self) -> Message[T] | None:
             self._up_value.value = False
             return Message(data=self._out_value, ts=self._ts_value.value, updated=updated)  # instead of True
 
+    @_noting_interrupt()
     def read(self) -> Message[T] | None:
-        mode = self.transport_mode
+        if _interrupted:
+            return None
+        mode = self.transport_mode  # itself a call to the manager, so it sits inside the guard
 
         if mode is TransportMode.SHARED_MEMORY:
             return self._read_shared_memory()
@@ -456,11 +490,19 @@ def __init__(self, cs: ControlSystem):
         self.__name__ = f'{type(cs).__name__}.run'
 
     def __call__(self, should_stop: SignalReceiver, clock: Clock) -> Iterator[Command]:
+        interrupted = False
         try:
             yield from self.cs.run(should_stop, clock)
+        except KeyboardInterrupt:
+            # An interrupt lands in every process at once, and it can land inside a manager call, which
+            # leaves that connection in the middle of a message. Reading it again returns the tail of
+            # somebody else's, so a call is left unanswered here rather than answered from a torn stream.
+            interrupted = True
+            raise
         finally:
-            for handler in handlers_of(self.cs):
-                handler.fail_queued()
+            if not interrupted:
+                for handler in handlers_of(self.cs):
+                    handler.fail_queued()
 
 
 def _bg_wrapper(
@@ -645,10 +687,10 @@ def interleave(self, *loops: ControlLoop) -> Iterator[Command]:
     def connect(
         self,
         source: ControlSystemEmitter[T] | ControlSystemCaller[Req, Res],
-        target: ControlSystemReceiver[T] | ControlSystemHandler[Req, Res],
+        target: ControlSystemReceiver[U] | ControlSystemHandler[Req, Res],
         *,
         emitter_wrapper: Callable[[SignalEmitter[T]], SignalEmitter[T]] = identity,
-        receiver_wrapper: Callable[[SignalReceiver[T]], SignalReceiver[T]] = identity,
+        receiver_wrapper: Callable[[SignalReceiver[T]], SignalReceiver[U]] = identity,
     ) -> None:
         """Declare a logical connection: an Emitter feeding a Receiver, or a Caller invoking a Handler.
 

positronic/assets/mujoco/trossen_wxai/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2025, Trossen Robotics
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

positronic/assets/mujoco/trossen_wxai/README.md

@@ -0,0 +1,24 @@
+# Trossen WidowX AI Description
+
+Robot description (MJCF) of the Trossen WidowX AI arm, from
+[trossen_arm_mujoco](https://github.com/TrossenRobotics/trossen_arm_mujoco)
+(`trossen_arm_mujoco/assets/wxai/wxai_follower.xml` at revision
+[`8d9389b`](https://github.com/TrossenRobotics/trossen_arm_mujoco/commit/8d9389b46ba02bdbf6d5e086e09d20717ccfed68)),
+BSD 3-Clause, see [LICENSE](./LICENSE).
+
+`meshdir` names `assets`, the directory beside this file that holds the meshes.
+
+`ee_site` is the frame the arm controller reports its Cartesian position in: the site sits 0.156 m along
+the flange's x axis, which is the `t_flange_tool` offset the `trossen_arm` SDK carries for the standard
+`wxai_v0` end effector. Forward kinematics on this model and the pose the controller reports agree to
+0.13 mm and 0.01 degrees, measured on firmware 1.11.1.
+
+`wxai_follower.urdf` is the same arm as URDF, from
+[trossen_arm_description](https://github.com/TrossenRobotics/trossen_arm_description)
+(`urdf/generated/wxai/wxai_follower.urdf`), BSD 3-Clause, with `meshes/trossen_black.png` beside the STLs
+it already shares with the MJCF. The driver solves against the MJCF and the codecs against the URDF, so
+`ee_gripper_link` of the URDF and `ee_site` of the MJCF must name one place:
+`test_the_trossen_urdf_and_its_mjcf_put_the_control_frame_in_the_same_place` measures that they do.
+
+The URDF names its meshes the way its own ROS package does; the driver shortens each to the file beside
+this README before it publishes the model.