roboapi 0.4.2__py3-none-any.whl

roboapi/hardware/base.py

@@ -0,0 +1,113 @@
+"""Native device interfaces and the shared Cartesian coordinate boundary.
+
+Arms stream Cartesian targets through non-blocking ``servo_pose`` calls. Each driver
+owns its control loop; the runtime owns paths, timing and returning to Home.
+"""
+
+from __future__ import annotations
+
+from typing import Protocol, runtime_checkable
+
+import numpy as np
+
+from ..geometry import FrameAdapter
+from ..semantics import TRAINING_FRAME
+
+
+@runtime_checkable
+class Arm(Protocol):
+    @property
+    def live(self) -> bool:
+        """False means: do not command this arm. ``last_error`` says why."""
+
+    @property
+    def last_error(self) -> str | None: ...
+
+    def connect(self) -> None: ...
+
+    def disconnect(self) -> None: ...
+
+    def get_joints(self) -> np.ndarray:
+        """Joint positions in rad. 6 for UR, 7 for Franka -- callers must not assume a length."""
+
+    def get_eef_pose(self) -> np.ndarray:
+        """pose7d ``[x, y, z, qw, qx, qy, qz]``."""
+
+    def servo_pose(self, pose7: np.ndarray, twist: np.ndarray | None = None, *, position_only=False) -> None:
+        """Update a Cartesian target, optionally without added force-driven offsets."""
+
+    def recover(self) -> None:
+        """Restore device control after a fault. The runtime then returns to Home."""
+
+
+@runtime_checkable
+class Gripper(Protocol):
+    @property
+    def live(self) -> bool: ...
+
+    @property
+    def last_error(self) -> str | None: ...
+
+    def connect(self) -> None: ...
+
+    def disconnect(self) -> None: ...
+
+    def get_ratio(self) -> float: ...
+
+    def set_ratio(self, ratio: float) -> None: ...
+
+    def move_blocking(self, ratio: float, should_stop=None) -> None: ...
+
+    def recover(self) -> None: ...
+
+
+class FramedArm:
+    """One bidirectional frame boundary for every native arm implementation.
+
+    Changing calibration is an idle-only operation owned by the runtime. This
+    adapter never starts a thread or sends motion when a profile is installed.
+    """
+
+    def __init__(self, native: Arm, adapter: FrameAdapter) -> None:
+        self.native = native
+        self.adapter = adapter
+
+    def __getattr__(self, name):
+        return getattr(self.native, name)
+
+    def set_adapter(self, adapter: FrameAdapter) -> None:
+        self.adapter = adapter
+
+    def get_eef_pose(self) -> np.ndarray:
+        return self.adapter.to_task(self.native.get_eef_pose())
+
+    def check_motion_ready(self) -> None:
+        """Check driver-specific prerequisites before an explicit robot operation."""
+        check = getattr(self.native, "check_motion_ready", None)
+        if check is not None:
+            check()
+
+    def servo_pose(self, pose7: np.ndarray, twist: np.ndarray | None = None, *, position_only=False) -> None:
+        adapter = self.adapter
+        self.native.servo_pose(
+            adapter.to_native(pose7),
+            None if twist is None else adapter.twist_to_native(twist),
+            position_only=position_only,
+        )
+
+    def drain_trace(self) -> list[dict]:
+        drain = getattr(self.native, "drain_trace", None)
+        if drain is None:
+            return []
+        adapter = self.adapter
+        trace = drain()
+        for item in trace:
+            for key in ("target_pose", "measured_pose"):
+                item[key] = adapter.to_task(item[key]).tolist()
+            # Pose errors are geometric vectors at the same control point, not
+            # spatial screw coordinates: only rotate their linear/angular axes.
+            error = np.asarray(item["pose_error"]).reshape(2, 3)
+            item["pose_error"] = (error @ adapter.task_from_base[:3, :3].T).ravel().tolist()
+            item["eef_frame"] = TRAINING_FRAME
+        return trace
+

roboapi/hardware/camera.py

@@ -0,0 +1,167 @@
+"""Fixed-mode wrist capture with one owner, fresh frames and hotplug recovery."""
+
+from __future__ import annotations
+
+import threading
+import time
+
+import numpy as np
+
+
+class V4L2Camera:
+    """V4L2 MJPG at 1280×720 and 30 FPS; one reader thread, latest frame only."""
+
+    def __init__(self, device: str) -> None:
+        self._device = device
+        self._cap = None
+        self._lock = threading.Lock()
+        self._lifecycle = threading.Lock()
+        self._frame: np.ndarray | None = None
+        self._stamp = 0.0
+        self._read_started_at = 0.0
+        self._stop = threading.Event()
+        self._thread: threading.Thread | None = None
+        self._error: str | None = None
+        self._first_attempt = threading.Event()
+        self.failure_count = 0
+        self.on_event = None
+        self.retry_s = 1.0
+
+    @property
+    def live(self) -> bool:
+        with self._lock:
+            return not self._stop.is_set() and self._frame is not None and self._error is None
+
+    @property
+    def last_error(self) -> str | None:
+        return self._error
+
+    def connect(self) -> None:
+        with self._lifecycle:
+            if self._thread is not None and self._thread.is_alive():
+                if self._stop.is_set():
+                    raise RuntimeError("camera is still stopping")
+            else:
+                self._stop.clear()
+                self._first_attempt.clear()
+                self._thread = threading.Thread(target=self._run, name="camera", daemon=True)
+                self._thread.start()
+        # An absent camera leaves preview available while the owner retries in the background.
+        self._first_attempt.wait(5.0)
+
+    def _open(self):
+        import cv2
+
+        cap = cv2.VideoCapture(self._device, cv2.CAP_V4L2)
+        try:
+            if not cap.isOpened():
+                raise RuntimeError(f"cannot open camera {self._device}")
+            mode = (
+                (cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG")),
+                (cv2.CAP_PROP_FRAME_WIDTH, 1280),
+                (cv2.CAP_PROP_FRAME_HEIGHT, 720),
+                (cv2.CAP_PROP_FPS, 30),
+            )
+            for prop, value in mode:
+                cap.set(prop, value)
+            actual = [cap.get(prop) for prop, _ in mode]
+            if not np.allclose(actual, [value for _, value in mode], rtol=0, atol=1e-3):
+                fourcc, width, height, fps = actual
+                code = int(fourcc) if np.isfinite(fourcc) else 0
+                codec = "".join(chr((code >> (8 * i)) & 0xff) for i in range(4)).strip("\x00") or "unknown"
+                raise RuntimeError(
+                    f"camera {self._device} requires MJPG 1280x720 at 30 FPS; "
+                    f"negotiated {codec} {width:g}x{height:g} at {fps:g} FPS"
+                )
+            cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+            return cap
+        except Exception:
+            cap.release()
+            raise
+
+    def _run(self) -> None:
+        import cv2
+
+        failed = False
+        while not self._stop.is_set():
+            cap = None
+            try:
+                cap = self._open()
+                self._cap = cap
+                while not self._stop.is_set():
+                    started_at = time.monotonic()
+                    ok, bgr = cap.read()
+                    if not ok:
+                        raise RuntimeError("camera frame read failed")
+                    rgb = cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB)
+                    with self._lock:
+                        if self._stop.is_set():
+                            break
+                        self._frame = rgb
+                        self._stamp = time.monotonic()
+                        self._read_started_at = started_at
+                        self._error = None
+                    if failed and self.on_event:
+                        self.on_event("Camera reconnected")
+                    failed = False
+                    self._first_attempt.set()
+            except Exception as exc:
+                if self._stop.is_set():
+                    break
+                with self._lock:
+                    self._frame = None
+                    self._stamp = self._read_started_at = 0.0
+                    self._error = str(exc)
+                    self.failure_count += 1
+                if not failed and not self._stop.is_set() and self.on_event:
+                    self.on_event(str(exc), "error")
+                failed = True
+                self._first_attempt.set()
+            finally:
+                if cap is not None:
+                    cap.release()
+                self._cap = None
+            self._stop.wait(self.retry_s)
+
+    def read(self) -> tuple[np.ndarray, float]:
+        with self._lock:
+            if self._frame is None:
+                raise RuntimeError(f"no frame yet: {self._error or 'camera not started'}")
+            return self._frame, time.monotonic() - self._stamp
+
+    def read_after(self, after: float | None, timeout_s: float, should_stop=None):
+        """Wait for a read begun after the motion boundary, skipping any in-flight read.
+
+        These are host receipt/read timestamps, not sensor exposure timestamps. V4L2's
+        device/USB latency still needs capture timestamps for exact sensor/state alignment.
+        """
+        deadline = time.monotonic() + timeout_s
+        while True:
+            if should_stop is not None and should_stop():
+                return None
+            if after is None:
+                return self.read()
+            with self._lock:
+                if self._error:
+                    raise RuntimeError(self._error)
+                if self._frame is not None and self._read_started_at > after:
+                    return self._frame, time.monotonic() - self._stamp
+            remaining = deadline - time.monotonic()
+            if remaining <= 0:
+                raise TimeoutError("camera produced no new frame after the motion boundary")
+            if self._stop.is_set():
+                raise RuntimeError("camera disconnected while waiting for a new frame")
+            time.sleep(min(0.005, remaining))
+
+    def disconnect(self) -> None:
+        with self._lifecycle:
+            self._stop.set()
+            with self._lock:
+                self._frame = None
+                self._stamp = self._read_started_at = 0.0
+                self._error = None
+            if self._thread is not None:
+                self._thread.join(timeout=2.0)
+                if not self._thread.is_alive():
+                    self._thread = None
+            # Only the reader releases capture. A blocked read must not gain a second owner.

roboapi/hardware/franka/__init__.py

@@ -0,0 +1,5 @@
+"""Franka hardware adapter."""
+
+from .arm import FrankaArm
+
+__all__ = ["FrankaArm"]