roboapi 0.4.2__py3-none-any.whl

roboapi/hardware/franka/feedback.py

@@ -0,0 +1,196 @@
+"""Read measured flange poses from Desk without taking control or enabling FCI."""
+
+from __future__ import annotations
+
+import json
+import ssl
+import threading
+import time
+from copy import deepcopy
+from pathlib import Path
+
+import numpy as np
+
+from ... import geometry as geo
+from ...errors import ControlNotReady
+
+
+def connect_socket(host, token, path):
+    from websockets.sync.client import connect
+
+    try:
+        return connect("wss://" + host + path,
+                       ssl=ssl._create_unverified_context(), proxy=None,
+                       additional_headers={"authorization": token},
+                       open_timeout=5, close_timeout=2, max_size=65536, max_queue=4)
+    except Exception as exc:
+        raise RuntimeError(f"Franka Desk WebSocket {path} failed: {exc}") from exc
+
+
+def read_robot_status(cfg, admin):
+    """Read one robot status without using FR3-only routes on Panda."""
+    if cfg.robot_model == "panda":
+        with connect_socket(cfg.host, admin.token, "/desk/api/robot/status") as socket:
+            return json.loads(socket.recv(timeout=5))
+    return admin.send_api_request(
+        "/admin/api/system-status", method="GET", response_encoding="json",
+    )["robot"]
+
+
+def has_robot_errors(errors):
+    if isinstance(errors, dict) and all(type(value) is bool for value in errors.values()):
+        return any(errors.values())
+    if isinstance(errors, list) and all(isinstance(value, str) for value in errors):
+        return bool(errors)
+    raise ValueError("Unsupported Desk robot-error feedback")
+
+
+def transform(values):
+    values = np.asarray(values, dtype=float)
+    if values.shape != (16,):
+        raise ValueError("Invalid Franka transform")
+    matrix = values.reshape(4, 4, order="F")
+    geo.matrix_to_pose7(matrix)
+    rotation = matrix[:3, :3]
+    if not np.allclose(rotation.T @ rotation, np.eye(3), atol=1e-6) or not np.isclose(np.linalg.det(rotation), 1.0):
+        raise ValueError("Invalid Franka rotation")
+    return matrix
+
+
+def parse_state(configuration, status, serial, *, robot_model="fr3"):
+    try:
+        robot = status if robot_model == "panda" else status["robot"]
+        ee = robot["endEffectorConfiguration"]["transformation"]
+        pose = configuration["cartesianPose"]
+        errors = robot["robotErrors"]
+    except (KeyError, TypeError) as exc:
+        raise ValueError("Unsupported Desk feedback format. Check the robot system version.") from exc
+    flange = transform(pose) @ np.linalg.inv(transform(ee))
+    return {"timestamp": time.monotonic(), "pose": geo.matrix_to_pose7(flange).tolist(),
+            "has_errors": has_robot_errors(errors),
+            "device": {"serial": serial}}
+
+
+class FrankaFeedbackReader:
+    def __init__(self, cfg):
+        self.cfg = cfg
+        self._admin = self._socket = self._status_socket = self._thread = None
+        self._lock = threading.Lock()
+        self._state = None
+        self._error = None
+        self._stop = threading.Event()
+
+    def _connect_socket(self, path):
+        # Robot traffic stays on the robot LAN, independent of desktop proxy settings.
+        return connect_socket(self.cfg.host, self._admin.token, path)
+
+    def connect(self):
+        import franky
+
+        self.disconnect()
+        self._error = None
+        self._stop.clear()
+        password = Path(self.cfg.desk_password_file).expanduser().read_text().strip()
+        self._admin = franky.DeskWebSession(self.cfg.host, self.cfg.desk_user, password)
+        try:
+            self._admin.open(timeout=10)
+            self._serial = self._read_serial()
+            status_path = ("/desk/api/robot/status" if self.cfg.robot_model == "panda"
+                           else "/admin/api/system-status")
+            self._status_socket = self._connect_socket(status_path)
+            self._status = json.loads(self._status_socket.recv(timeout=5))
+            self._socket = self._connect_socket("/desk/api/robot/configuration")
+            self._thread = threading.Thread(target=self._read, name="franka-feedback", daemon=True)
+            self._thread.start()
+            deadline = time.monotonic() + 5
+            while not self.live:
+                if self._error is not None:
+                    raise self._error
+                if time.monotonic() >= deadline:
+                    raise ValueError("No live Franka feedback. Check Desk access.")
+                time.sleep(0.02)
+        except Exception as exc:
+            self.disconnect()
+            if isinstance(exc, (ControlNotReady, RuntimeError, ValueError, OSError)):
+                raise
+            raise RuntimeError(f"Franka Desk feedback failed: {exc}") from exc
+
+    def _read_serial(self):
+        try:
+            with self._connect_socket("/admin/api/robot/nameplate") as socket:
+                nameplate = json.loads(socket.recv(timeout=5))
+            if nameplate.get("status") != "Connected":
+                raise ControlNotReady("Franka arm is not connected to Desk")
+            serial = nameplate.get("robotSerial")
+        except RuntimeError as exc:
+            response = getattr(exc.__cause__, "response", None)
+            if getattr(response, "status_code", None) != 404:
+                raise
+            # System 5.6 exposes the serial separately, without the newer nameplate feed.
+            serial = self._admin.send_api_request(
+                "/admin/api/robot/serial", method="GET", response_encoding="text",
+            )
+            try:
+                serial = json.loads(serial)
+            except ValueError:
+                pass
+        if not isinstance(serial, str) or not serial.strip():
+            raise ValueError("Desk returned an invalid arm serial number")
+        return serial.strip()
+
+    def _read(self):
+        try:
+            while not self._stop.is_set():
+                configuration = json.loads(self._socket.recv(timeout=1))
+                # Desk sends status changes separately from the periodic pose stream.
+                while True:
+                    try:
+                        self._status = json.loads(self._status_socket.recv(timeout=0))
+                    except TimeoutError:
+                        break
+                state = parse_state(configuration, self._status, self._serial,
+                                    robot_model=self.cfg.robot_model)
+                self.validate_device(state["device"])
+                with self._lock:
+                    self._state = state
+        except Exception as exc:
+            if not self._stop.is_set():
+                self._error = exc
+
+    @property
+    def live(self):
+        with self._lock:
+            return (not self._stop.is_set() and self._state is not None and self._error is None
+                    and time.monotonic() - self._state["timestamp"] <= 0.5)
+
+    def snapshot(self):
+        with self._lock:
+            if self._state is None or self._error or time.monotonic() - self._state["timestamp"] > 0.5:
+                raise ValueError(str(self._error) if self._error else "Franka feedback is stale.")
+            return deepcopy(self._state)
+
+    @property
+    def last_error(self):
+        if self._stop.is_set():
+            return None
+        return str(self._error) if self._error else (
+            "Franka read-only feedback is stale" if self._state is not None and not self.live else None)
+
+    def validate_device(self, device):
+        if set(device) != {"serial"} or not isinstance(device["serial"], str) or not device["serial"]:
+            raise ValueError("Invalid Franka calibration device information")
+
+    def disconnect(self):
+        self._stop.set()
+        for connection in (self._socket, self._status_socket):
+            if connection is not None:
+                connection.close()
+        if self._thread is not None:
+            self._thread.join(timeout=3)
+            if self._thread.is_alive():
+                raise RuntimeError("Franka feedback reader did not stop")
+        if self._admin is not None:
+            self._admin.close()
+        self._admin = self._socket = self._status_socket = self._thread = None
+        with self._lock:
+            self._state = None

roboapi/hardware/franka/guiding.py

@@ -0,0 +1,104 @@
+"""Native Programming-mode guiding; FCI is reserved for robot motion."""
+
+from pathlib import Path
+import json
+import time
+
+from ...errors import ControlNotReady
+from .desk import _DeskLease, apply_tool_configuration, create_desk, ensure_programming
+from .feedback import has_robot_errors, read_robot_status
+
+
+class FrankaGuiding:
+    def __init__(self, cfg):
+        self.cfg = cfg
+        self.lease = self.admin = None
+        self.axis = None
+        self.tool_load_ready = False
+
+    @property
+    def active(self):
+        return self.lease is not None and self.lease.acquired
+
+    def start(self, axis=None):
+        import franky
+
+        if self.lease is not None:
+            raise RuntimeError("Franka guiding is already open")
+        self.tool_load_ready = False
+        password = Path(self.cfg.desk_password_file).expanduser().read_text().strip()
+        desk = create_desk(self.cfg, password)
+        self.lease = _DeskLease(desk)
+        try:
+            self.lease.acquire(activate=False)
+            self.admin = franky.DeskWebSession(self.cfg.host, self.cfg.desk_user, password)
+            self.admin.open(timeout=10)
+            self.check_stopped()
+            apply_tool_configuration(self.cfg, desk)
+            self.tool_load_ready = True
+            self.set_axis(axis)
+        except Exception as exc:
+            self.close()
+            if isinstance(exc, (ControlNotReady, RuntimeError, ValueError, OSError)):
+                raise
+            raise RuntimeError(f"Franka guiding preparation failed: {exc}") from exc
+
+    def _api(self, path, **kwargs):
+        try:
+            return self.admin.send_api_request(path, **kwargs)
+        except Exception as exc:
+            raise RuntimeError(f"Franka Desk request failed: {exc}") from exc
+
+    def check_stopped(self):
+        if not self.active or not self.lease.desk.has_control:
+            raise ControlNotReady("Desk control changed. Start calibration again.")
+        if self.cfg.robot_model == "panda":
+            robot = read_robot_status(self.cfg, self.admin)
+        else:
+            state = self._api("/admin/api/system-status", method="GET", response_encoding="json")
+            if state["safety"].get("safeInputState", {}).get("guidingEnableButton") == "Active":
+                raise ControlNotReady("Release the arm buttons, then retry.")
+            robot = state["robot"]
+        if has_robot_errors(robot["robotErrors"]):
+            raise ControlNotReady("Clear the robot error in Desk, then retry.")
+        ensure_programming(self.lease.desk)
+
+    def _request(self, path, field, value):
+        return self._api(
+            "/desk/api/robot/guiding/" + path, method="PUT",
+            headers={"X-Control-Token": self.lease.desk.control_token,
+                     "Content-Type": "application/x-www-form-urlencoded"},
+            content={field: json.dumps(value)}, content_encoding="x-www-form-urlencoded",
+            response_encoding="text",
+        )
+
+    def set_axis(self, axis):
+        if axis not in (None, "x", "y", "z"):
+            raise ValueError("Guiding axis must be x, y or z")
+        self.check_stopped()
+        if axis is not None:
+            config = {"translation_" + key: key == axis for key in "xyz"}
+            config.update({"rotation_" + key: False for key in "xyz"})
+            config["elbow"] = False
+            self._request("configuration", "configuration", config)
+        self._request("mode", "mode", "free" if axis is None else "user")
+        expected = "free" if axis is None else "user"
+        deadline = time.monotonic() + 3
+        while True:
+            mode = self._api("/desk/api/robot/guiding/mode", method="GET", response_encoding="json")
+            actual = (self._api("/desk/api/robot/guiding/configuration", method="GET", response_encoding="json")
+                      if axis is not None else None)
+            if mode["availableModes"][mode["currentMode"]] == expected and (axis is None or actual == config):
+                break
+            if time.monotonic() >= deadline:
+                raise RuntimeError("Desk did not apply the guiding constraint")
+            time.sleep(0.05)
+        self.axis = axis
+
+    def close(self):
+        if self.lease is not None:
+            self.lease.release()
+            self.lease = None
+        if self.admin is not None:
+            self.admin.close()
+            self.admin = None