roboapi 0.4.2__py3-none-any.whl

roboapi/hardware/franka/desk.py

@@ -0,0 +1,555 @@
+"""Franka Desk sessions, supported controller protocols and end-effector configuration."""
+
+from __future__ import annotations
+
+import json
+import logging
+import threading
+import time
+from contextlib import suppress
+from pathlib import Path
+from urllib.parse import quote
+
+import numpy as np
+
+from .settings import STANDARD_TOOL
+from ...errors import ControlNotReady
+
+logger = logging.getLogger(__name__)
+
+
+class DeskLeaseError(RuntimeError):
+    pass
+
+
+def ensure_programming(desk):
+    """Prepare native guiding or tool configuration using an owned Desk session."""
+    if desk.get_fci_status() == "Active":
+        desk.disable_fci()
+    if getattr(desk.operating_mode, "value", None) != "Programming":
+        if desk.api == "v1":
+            desk.send_control_api_request(
+                "/api/system/operating-mode:change",
+                content={"desiredOperatingMode": "Programming"},
+            )
+        else:
+            desk.set_mode_programming()
+    deadline = time.monotonic() + 10
+    while (getattr(desk.operating_mode, "value", None) != "Programming"
+           or desk.get_fci_status() == "Active"):
+        if time.monotonic() >= deadline:
+            raise ControlNotReady("Desk did not enter Programming. Check the robot state and retry.")
+        time.sleep(0.05)
+
+
+class _DeskLease:
+    def __init__(
+        self, desk, *, timeout: float = 15.0, poll_period: float = 0.5,
+        release_attempts: int = 3, api: str | None = None,
+    ) -> None:
+        self.desk = desk
+        self.timeout = timeout
+        self.poll_period = poll_period
+        self.release_attempts = release_attempts
+        api = api or desk.api
+        if api not in ("v1", "panda4", "fr3-legacy"):
+            raise ValueError("Unsupported Desk API")
+        self.api = api
+        self.opened = False
+        self.acquired = False
+        self.fault: str | None = None
+        self._stop = threading.Event()
+        self._thread: threading.Thread | None = None
+
+    def _owner(self) -> str | None:
+        if self.api == "fr3-legacy":
+            active = self.desk._control_status()["activeToken"]
+            return None if active is None else str(active.get("ownedBy") or "unknown owner")
+        path = "/admin/api/control-token" if self.api == "panda4" else "/api/system/control-token"
+        response = self.desk.send_api_request(path, method="GET", response_encoding="json")
+        if not isinstance(response, dict):
+            raise DeskLeaseError("invalid Desk control-token response")
+        if self.api == "panda4":
+            if "activeToken" not in response:
+                raise DeskLeaseError("missing Panda control-token status")
+            active = response["activeToken"]
+            if active is None:
+                return None
+            if not isinstance(active, dict) or "id" not in active:
+                raise DeskLeaseError("invalid Panda control-token owner")
+            return str(active.get("ownedBy") or "unknown owner")
+        # The v1 API returns {} when control is available.
+        if not response:
+            return None
+        if "owner" not in response:
+            raise DeskLeaseError("missing Desk control-token owner")
+        owner = response.get("owner")
+        return str(owner) if owner else None
+
+    def acquire(self, *, prepare=None, activate=True) -> None:
+        try:
+            self.desk.open(timeout=self.timeout)
+            self.opened = True
+            owner = self._owner()
+            if owner is not None:
+                raise ControlNotReady(f"Release Desk control held by {owner!r}, then retry.")
+            self.desk.take_control(wait_timeout=self.timeout, force=False)
+            self.acquired = True
+            if prepare is not None:
+                prepare(self.desk)
+            if not activate:
+                return
+            self.desk.set_mode_execution()
+            if "UNLOCKED" not in str(self.desk.brake_state).upper():
+                self.desk.unlock_brakes()
+            self.desk.enable_fci()
+            deadline = time.monotonic() + self.timeout
+            while self.desk.get_fci_status() != "Active" and time.monotonic() < deadline:
+                time.sleep(0.05)
+            self.verify()
+            self._stop.clear()
+            self._thread = threading.Thread(target=self._monitor, name="franka-desk", daemon=True)
+            self._thread.start()
+        except Exception:
+            with suppress(Exception):
+                self.release()
+            raise
+
+    def verify(self) -> None:
+        if not self.acquired or not bool(self.desk.has_control):
+            raise DeskLeaseError("Desk lease lost control ownership")
+        status = str(self.desk.get_fci_status())
+        if status != "Active":
+            raise DeskLeaseError(f"FCI left Active state while leased: {status}")
+
+    def check(self) -> None:
+        if self.fault is not None:
+            raise DeskLeaseError(self.fault)
+
+    def _monitor(self) -> None:
+        while not self._stop.wait(self.poll_period):
+            try:
+                self.verify()
+            except Exception as exc:
+                self.fault = str(exc)
+                return
+
+    def release(self) -> None:
+        self._stop.set()
+        thread, self._thread = self._thread, None
+        if thread is not None:
+            thread.join(timeout=self.timeout + 1.0)
+            if thread.is_alive():
+                raise DeskLeaseError("Desk monitor did not stop; token was not released")
+        errors = []
+        if self.acquired:
+            for attempt in range(1, self.release_attempts + 1):
+                try:
+                    if not self.opened or not bool(getattr(self.desk, "is_open", False)):
+                        self.desk.open(timeout=self.timeout)
+                        self.opened = True
+                    if self._owner() is None:
+                        self.acquired = False
+                        break
+                except Exception as exc:
+                    errors.append(f"attempt {attempt} owner check: {type(exc).__name__}: {exc}")
+                try:
+                    self.desk.release_control()
+                except Exception as exc:
+                    errors.append(f"attempt {attempt} release: {type(exc).__name__}: {exc}")
+                    try:
+                        if self._owner() is None:
+                            self.acquired = False
+                            break
+                    except Exception as owner_exc:
+                        errors.append(
+                            f"attempt {attempt} recheck: {type(owner_exc).__name__}: {owner_exc}"
+                        )
+                else:
+                    self.acquired = False
+                    try:
+                        owner = self._owner()
+                    except Exception as exc:
+                        logger.warning("Desk owner verification after release failed: %s", exc)
+                    else:
+                        if owner is not None:
+                            errors.append(f"Desk control remained owned by {owner!r} after release")
+                    break
+                if attempt < self.release_attempts:
+                    time.sleep(0.2)
+        if self.opened and not self.acquired:
+            self.desk.close()
+            self.opened = False
+        if self.acquired:
+            raise DeskLeaseError(
+                "Desk token release failed: " + ("; ".join(errors) or "owner remained set")
+            )
+
+
+class _LegacyOwnership:
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        self._pending_control: tuple[str, str] | None = None
+
+    def _clear_pending_control(self):
+        if self._pending_control is None:
+            return
+        token_id, token = self._pending_control
+        for attempt in range(3):
+            try:
+                status = self._control_status()
+                active, pending = status["activeToken"], status.get("tokenRequest")
+                if active is not None and str(active["id"]) == token_id:
+                    self.send_api_request("/admin/api/control-token", method="DELETE", content={"token": token})
+                elif pending is not None and str(pending["id"]) == token_id:
+                    self.send_api_request(
+                        "/admin/api/control-token/request/" + token_id,
+                        method="DELETE", content={"token": token},
+                    )
+                self._pending_control = None
+                return
+            except Exception:
+                if attempt == 2:
+                    raise
+                time.sleep(0.05)
+
+    def close(self):
+        # Keep the request credential and session if cleanup fails, so close can be retried.
+        self._clear_pending_control()
+        super().close()
+
+    def _get_has_control(self):
+        active = self._control_status()["activeToken"]
+        return active is not None and str(active["id"]) == self.control_token_id
+
+    def _take_control(self, wait_timeout=30.0, force=False):
+        if force:
+            raise self._sdk.DeskError("Forced Desk control takeover is not supported")
+        status = self._control_status()
+        if status["activeToken"] is not None or status.get("tokenRequest") is not None:
+            raise self._sdk.DeskError("Desk control is already owned or requested by another session")
+        response = self.send_api_request(
+            "/admin/api/control-token/request", content={"requestedBy": self.username}, response_encoding="json",
+        )
+        if not isinstance(response, dict) or "id" not in response or not isinstance(response.get("token"), str) or not response["token"]:
+            raise self._sdk.DeskError("Invalid Desk control request response; inspect Desk ownership")
+        token_id, token = str(response["id"]), response["token"]
+        if not token_id.isdecimal():
+            raise self._sdk.DeskError("Invalid Desk control request identity; inspect Desk ownership")
+        # franky adopts the token only after _take_control returns successfully.
+        self._pending_control = token_id, token
+        deadline = time.monotonic() + wait_timeout
+        try:
+            while True:
+                active = self._control_status()["activeToken"]
+                if active is not None and str(active["id"]) == token_id:
+                    return token_id, token
+                if time.monotonic() >= deadline:
+                    raise self._sdk.TakeControlTimeoutError("Desk did not grant this control request")
+                time.sleep(min(0.05, max(0.0, deadline - time.monotonic())))
+        except BaseException as error:
+            try:
+                self._clear_pending_control()
+            except Exception:
+                raise self._sdk.DeskError("Desk control cleanup is still pending; retry closing the session") from error
+            raise
+
+    def get_fci_status(self):
+        return "Active" if self._control_status()["fciActive"] else "Inactive"
+
+    def enable_fci(self):
+        self.send_control_api_request("/admin/api/control-token/fci", content={"token": self.control_token})
+
+    def disable_fci(self):
+        self.send_control_api_request(
+            "/admin/api/control-token/fci", method="DELETE", content={"token": self.control_token},
+        )
+
+
+class _System4Endpoints(_LegacyOwnership):
+    api = "panda4"
+
+    def _control_status(self):
+        status = self.send_api_request("/admin/api/control-token", method="GET", response_encoding="json")
+        if not isinstance(status, dict) or "activeToken" not in status or "fciActive" not in status:
+            raise self._sdk.DeskError("Invalid Panda control-token status")
+        for field in ("activeToken", "tokenRequest"):
+            token = status.get(field)
+            if token is not None and (not isinstance(token, dict) or "id" not in token):
+                raise self._sdk.DeskError("Invalid Panda control-token identity")
+        if not isinstance(status["fciActive"], bool):
+            raise self._sdk.DeskError("Invalid Panda FCI status")
+        return status
+
+    def _get_system_status(self):
+        # System 4.x can label JSON responses as text; do not infer the parser from MIME.
+        return self.send_api_request("/desk/api/system/status", method="GET", response_encoding="json")
+
+    def _get_brake_state(self):
+        brakes = self._get_system_status().get("brakesOpen")
+        if not isinstance(brakes, list) or len(brakes) != 7 or any(type(b) is not bool for b in brakes):
+            raise self._sdk.DeskError("Invalid Panda brake state")
+        if all(brakes):
+            return self._sdk.BrakeState.UNLOCKED
+        if not any(brakes):
+            return self._sdk.BrakeState.LOCKED
+        raise self._sdk.DeskError("Panda joints have inconsistent brake states")
+
+    def _get_operating_mode(self):
+        mode = self._get_system_status().get("operationalMode")
+        if mode == "EXEC":
+            return self._sdk.OperatingMode.EXECUTION
+        if mode in ("SS1", "SS2"):
+            return self._sdk.OperatingMode.PROGRAMMING
+        raise self._sdk.DeskError("Unrecognized Panda operational mode")
+
+    def set_mode_execution(self):
+        # On 4.x activating FCI selects EXEC, after brakes are opened. There is no
+        # separate operating-mode route. Check the physical stop without changing it.
+        status = self._get_system_status()
+        if status.get("x3Enabled") is not True or status.get("operationalMode") not in ("SS1", "SS2", "EXEC"):
+            raise self._sdk.DeskError("Panda physical stop/mode prevents execution; inspect on site")
+
+    def unlock_brakes(self):
+        self.send_control_api_request("/desk/api/robot/open-brakes", content={"force": False})
+
+    def lock_brakes(self):
+        self.send_control_api_request("/desk/api/robot/close-brakes")
+
+    def set_mode_programming(self):
+        # Releasing a token may disable FCI while leaving the controller in EXEC.
+        if self.get_fci_status() == "Active" or self.operating_mode == self._sdk.OperatingMode.EXECUTION:
+            self.disable_fci()
+
+    def execute_self_test(self):
+        raise self._sdk.DeskError("Panda system 4.x self-tests must be handled through Desk on site")
+
+
+class _LegacyFR3Endpoints(_LegacyOwnership):
+    """Pre-v1 FR3 web protocol; retain the shared token request/cleanup lifecycle."""
+
+    api = "fr3-legacy"
+
+    def _get_system_status(self):
+        return self.send_api_request("/admin/api/system-status", method="GET", response_encoding="json")
+
+    def _control_status(self):
+        status = self._get_system_status().get("controlToken")
+        if (not isinstance(status, dict) or "activeToken" not in status
+                or not isinstance(status.get("fciActive"), bool)):
+            raise self._sdk.DeskError("Invalid legacy FR3 control-token status")
+        for field in ("activeToken", "tokenRequest"):
+            token = status.get(field)
+            if token is not None and (not isinstance(token, dict) or "id" not in token):
+                raise self._sdk.DeskError("Invalid legacy FR3 control-token identity")
+        return status
+
+    def _get_operating_mode(self):
+        return self._sdk.DeskWebSession._get_operating_mode(self)
+
+    def _get_brake_state(self):
+        return self._sdk.DeskWebSession._get_brake_state(self)
+
+    def set_mode_execution(self):
+        self._sdk.DeskWebSession.set_mode_execution(self)
+
+    def set_mode_programming(self):
+        if self.get_fci_status() == "Active":
+            self.disable_fci()
+        self._sdk.DeskWebSession.set_mode_programming(self)
+
+    def enable_fci(self):
+        try:
+            super().enable_fci()
+        except self._sdk.FrankaAPIError as exc:
+            if exc.http_code != 404:
+                raise
+            # Older clients expose these two activation routes. Only a missing
+            # endpoint selects the SDK's form-encoded route; other failures propagate.
+            self._sdk.DeskWebSession.enable_fci(self)
+
+    def unlock_brakes(self):
+        self._sdk.DeskWebSession.unlock_brakes(self)
+
+    def lock_brakes(self):
+        self._sdk.DeskWebSession.lock_brakes(self)
+
+
+def _desk_type(api):
+    import franky
+
+    if api == "v1":
+        return franky.Desk
+    if api not in ("panda4", "fr3-legacy"):
+        raise ValueError("Unsupported Desk API")
+    import franky.desk as sdk
+
+    endpoints = _System4Endpoints if api == "panda4" else _LegacyFR3Endpoints
+
+    class LegacySession(endpoints, sdk.DeskWebSession):
+        _sdk = sdk
+
+    return LegacySession
+
+
+def create_desk(cfg, password):
+    import httpx
+
+    if cfg.robot_model == "panda":
+        api = "panda4"
+    else:
+        # Probe a read-only endpoint. Authentication and network failures must not
+        # silently select another protocol, and probing never takes robot control.
+        response = httpx.get(
+            "https://" + cfg.host + "/api/system",
+            auth=(cfg.desk_user, password), verify=False, trust_env=False, timeout=10,
+        )
+        if response.status_code == 404:
+            api = "fr3-legacy"
+        else:
+            response.raise_for_status()
+            api = "v1"
+    desk = _desk_type(api)(cfg.host, cfg.desk_user, password)
+    desk.api = api
+    return desk
+
+
+_PROFILES = "/admin/api/end-effector/profiles"
+_INERTIA_ENTRIES = (("x11", 0, 0), ("x12", 0, 1), ("x13", 0, 2),
+                    ("x22", 1, 1), ("x23", 1, 2), ("x33", 2, 2))
+
+
+def tool_inertial():
+    """Encode the standard assembly as fresh Desk inertial fields."""
+    inertia = STANDARD_TOOL.inertia_kg_m2
+    return {
+        "mass": STANDARD_TOOL.mass_kg,
+        "centerOfMass": dict(zip("xyz", STANDARD_TOOL.com_flange_m)),
+        "inertia": {key: inertia[i][j] for key, i, j in _INERTIA_ENTRIES},
+    }
+
+
+def _same_load(actual, expected):
+    try:
+        a = [actual["mass"], *[actual["centerOfMass"][k] for k in "xyz"],
+             *[actual["inertia"][k] for k in expected["inertia"]]]
+        b = [expected["mass"], *expected["centerOfMass"].values(), *expected["inertia"].values()]
+        return np.all(np.isfinite(a)) and np.allclose(a, b, rtol=1e-6, atol=1e-8)
+    except (KeyError, TypeError, ValueError):
+        return False
+
+
+def tool_tcp():
+    """Desk TCP coincides with the flange in both position and orientation."""
+    return {"translation": dict.fromkeys("xyz", 0.0),
+            "rotation": dict.fromkeys(("roll", "pitch", "yaw"), 0.0)}
+
+
+def _same_tcp(actual):
+    try:
+        values = [actual[group][axis] for group, axes in
+                  (("translation", "xyz"), ("rotation", ("roll", "pitch", "yaw")))
+                  for axis in axes]
+        return np.all(np.isfinite(values)) and np.allclose(values, 0.0, rtol=0, atol=1e-8)
+    except (KeyError, TypeError, ValueError):
+        return False
+
+
+def apply_tool_configuration(cfg, control_desk):
+    """Apply the standard inertial data and flange-centre TCP before FCI starts."""
+    import franky
+    from .feedback import read_robot_status
+
+    expected = tool_inertial()
+    password = Path(cfg.desk_password_file).expanduser().read_text().strip()
+    # Profiles use the web admin login, including when control uses the v1 API.
+    admin = franky.DeskWebSession(cfg.host, cfg.desk_user, password)
+    headers = {"Content-Type": "application/json", "X-Control-Token": control_desk.control_token}
+    try:
+        admin.open(timeout=15.0)
+        if control_desk.api in ("fr3-legacy", "panda4"):
+            try:
+                actual = read_robot_status(cfg, admin)["endEffectorConfiguration"]
+                expected_flat = tool_configuration_data()
+                matches = all(
+                    np.shape(actual[key]) == np.shape(expected_flat[key])
+                    and np.all(np.isfinite(actual[key]))
+                    and np.allclose(actual[key], expected_flat[key], rtol=1e-6, atol=1e-8)
+                    for key in expected_flat
+                )
+            except (KeyError, TypeError, ValueError) as exc:
+                raise ValueError("Unsupported Desk end-effector feedback") from exc
+            if not matches:
+                raise ControlNotReady(
+                    "Desk end-effector load or TCP does not match the standard configuration. Run "
+                    "'roboapi export-franka-tool', import endeffector-config.json in "
+                    "Desk's end-effector settings, then retry."
+                )
+            return
+
+        def request(path, method="GET", content=None):
+            return admin.send_api_request(path, method=method, content=content,
+                                          headers=headers, response_encoding="json")
+
+        active = request(_PROFILES + "/active")
+        if not isinstance(active, dict) or not all(k in active for k in ("id", "name", "deviceId", "tcp", "inertial")):
+            raise ValueError("Desk returned an invalid active end-effector profile.")
+        body = {"name": active["name"], "deviceId": active["deviceId"],
+                "tcp": tool_tcp(), "inertial": expected}
+        profile_id = active["id"]
+        needs_update = (profile_id == "ee-no-profile" or not _same_load(active["inertial"], expected)
+                        or not _same_tcp(active["tcp"]))
+        if needs_update:
+            ensure_programming(control_desk)
+        if profile_id == "ee-no-profile":
+            body.update(name="roboapi", deviceId="ee-no-gripper")
+            profiles = request(_PROFILES)
+            if not isinstance(profiles, list) or not all(isinstance(p, dict) for p in profiles):
+                raise ValueError("Desk returned an invalid end-effector profile list.")
+            matches = [p for p in profiles if p.get("name") == "roboapi"]
+            if len(matches) > 1:
+                raise ValueError("Desk has multiple roboapi end-effector profiles.")
+            if matches:
+                profile_id = matches[0]["id"]
+            else:
+                try:
+                    created = request(_PROFILES, "POST", body)
+                except Exception as exc:
+                    # franky's transport treats HTTP 201 as an error, although creation succeeded.
+                    if getattr(exc, "http_code", None) != 201:
+                        raise
+                    created = json.loads(exc.message)
+                profile_id = created["id"]
+        if not isinstance(profile_id, str) or not profile_id:
+            raise ValueError("Desk returned an invalid end-effector profile ID.")
+        if needs_update:
+            request(_PROFILES + "/" + quote(profile_id, safe=""), "PUT", body)
+            request(_PROFILES + "/active", "PUT", profile_id)
+        applied = request(_PROFILES + "/active")
+        if (not isinstance(applied, dict) or applied.get("id") != profile_id
+                or not _same_tcp(applied.get("tcp"))
+                or applied.get("deviceId") != body["deviceId"]
+                or not _same_load(applied.get("inertial"), expected)):
+            raise ValueError("Desk did not apply the standard end-effector load and flange-centre TCP.")
+    finally:
+        admin.close()
+
+
+def tool_configuration_data():
+    """Legacy Desk import data for the standard load and identity flange-to-TCP transform."""
+    return {
+        "mass": STANDARD_TOOL.mass_kg,
+        "centerOfMass": list(STANDARD_TOOL.com_flange_m),
+        "inertia": np.asarray(STANDARD_TOOL.inertia_kg_m2).ravel(order="F").tolist(),
+        "transformation": np.eye(4).ravel(order="F").tolist(),
+    }
+
+
+def export_tool_configuration(output):
+    """Export the complete standard end-effector configuration without connecting to Desk."""
+    data = tool_configuration_data()
+    target = Path(output).expanduser()
+    with target.open("x", encoding="utf-8") as stream:
+        json.dump(data, stream, indent=2, allow_nan=False)
+        stream.write("\n")
+    return target