roboapi 0.4.2__py3-none-any.whl

roboapi/__init__.py

@@ -0,0 +1,2 @@
+"""RoboAPI — single arm + Robotiq gripper + wrist camera + remote VLA policy,
+packaged as a tool an LLM agent can call. See DESIGN.md."""

roboapi/calibration/__init__.py

@@ -0,0 +1,6 @@
+"""Cartesian installation workflow and its current local profile."""
+
+from .profile import CalibrationProfile, load_profile, save_profile
+from .session import Calibration
+
+__all__ = ["Calibration", "CalibrationProfile", "load_profile", "save_profile"]

roboapi/calibration/profile.py

@@ -0,0 +1,139 @@
+"""One Cartesian installation calibration and atomic local storage."""
+
+from __future__ import annotations
+
+import json
+import math
+import os
+import tempfile
+from dataclasses import asdict, dataclass
+from numbers import Real
+from pathlib import Path
+
+import numpy as np
+
+from .. import geometry as geo
+from ..semantics import HOME_POSE7
+from ..motion import pose_reached
+
+DEFAULT_PROFILE_PATH = "~/.config/roboapi/calibration.json"
+
+
+def _pose(value, name):
+    if (not isinstance(value, (list, tuple, np.ndarray)) or np.shape(value) != (7,)
+            or any(isinstance(x, (bool, np.bool_)) or not isinstance(x, Real)
+                   or not math.isfinite(x) for x in value)):
+        raise ValueError(f"{name} must contain 7 finite values")
+    geo.pose7_to_matrix(value)
+    return [float(x) for x in value]
+
+
+def _plane(value):
+    if not isinstance(value, (list, tuple, np.ndarray)) or np.shape(value) != (3, 7):
+        raise ValueError("reference plane requires three Cartesian poses: A, B and C")
+    return [_pose(pose, name) for pose, name in zip(value, ("A", "B", "C"))]
+
+
+def fit_home_plane(home, plane):
+    """Align Home, the level plane's upward normal and A-to-B heading.
+
+    Home is above the reference plane. All samples use the same tool orientation and
+    fixed contact point, so contact-point offsets cancel in the flange-position differences.
+    """
+    home = np.asarray(_pose(home, "Home"))
+    a, b, c = np.asarray(_plane(plane))[:, :3]
+    ab, ac = b - a, c - a
+    ab_length, ac_length = np.linalg.norm(ab), np.linalg.norm(ac)
+    if ab_length <= 1e-6:
+        raise ValueError("Place B away from A toward task +X.")
+    normal = np.cross(ab, ac)
+    if ac_length <= 1e-6 or np.linalg.norm(normal) <= 1e-6 * ab_length * ac_length:
+        raise ValueError("Place C on the reference surface, away from line AB.")
+    z_axis = normal / np.linalg.norm(normal)
+    height = np.dot(home[:3] - a, z_axis)
+    if abs(height) <= 1e-6:
+        raise ValueError("Home must be above the reference surface.")
+    if height < 0:
+        z_axis = -z_axis
+    x_axis = ab - np.dot(ab, z_axis) * z_axis
+    x_axis /= np.linalg.norm(x_axis)
+    task_from_base = np.eye(4)
+    task_from_base[:3, :3] = np.column_stack((x_axis, np.cross(z_axis, x_axis), z_axis)).T
+    task_from_base[:3, 3] = np.asarray(HOME_POSE7[:3]) - task_from_base[:3, :3] @ home[:3]
+    native_from_tool = np.eye(4)
+    native_from_tool[:3, :3] = (
+        (task_from_base[:3, :3] @ geo.pose7_to_matrix(home)[:3, :3]).T
+        @ geo.pose7_to_matrix(HOME_POSE7)[:3, :3]
+    )
+    return geo.FrameAdapter(geo.matrix_to_pose7(task_from_base), geo.matrix_to_pose7(native_from_tool))
+
+
+@dataclass
+class CalibrationProfile:
+    identity: str
+    device: dict
+    home_native_pose7: list[float]
+    plane_native_poses7: list[list[float]]
+    check_native_pose7: list[float] | None = None
+
+    def __post_init__(self):
+        if not isinstance(self.identity, str) or not self.identity:
+            raise ValueError("calibration installation identity is required")
+        if not isinstance(self.device, dict) or not self.device:
+            raise ValueError("calibration device information is required")
+        # Device-specific validity is checked by the read-only reader.
+        json.dumps(self.device, allow_nan=False)
+        self.plane_native_poses7 = _plane(self.plane_native_poses7)
+        for name in ("home_native_pose7", "check_native_pose7"):
+            value = getattr(self, name)
+            if value is not None:
+                setattr(self, name, _pose(value, name))
+            elif name != "check_native_pose7":
+                raise ValueError(f"{name} is required")
+
+    def adapter(self):
+        return fit_home_plane(self.home_native_pose7, self.plane_native_poses7)
+
+    def validate(self, identity):
+        self.__post_init__()
+        if self.identity != identity:
+            raise ValueError("calibration does not match this installation")
+        self.adapter()
+        if self.check_native_pose7 is None:
+            raise ValueError("an independent return to Home must verify calibration")
+        if not pose_reached(self.home_native_pose7, self.check_native_pose7):
+            raise ValueError("Home return did not complete")
+
+    def to_dict(self):
+        return asdict(self)
+
+
+def save_profile(profile, path=DEFAULT_PROFILE_PATH):
+    """Replace the current local calibration atomically, with owner-only permissions."""
+    profile = CalibrationProfile(**profile.to_dict())
+    profile.validate(profile.identity)
+    target = Path(path).expanduser()
+    target.parent.mkdir(parents=True, exist_ok=True)
+    fd, temporary = tempfile.mkstemp(prefix=f".{target.name}.", dir=target.parent)
+    try:
+        os.fchmod(fd, 0o600)
+        with os.fdopen(fd, "w", encoding="utf-8") as stream:
+            json.dump(profile.to_dict(), stream, indent=2, sort_keys=True, allow_nan=False)
+            stream.write("\n")
+            stream.flush()
+            os.fsync(stream.fileno())
+        os.replace(temporary, target)
+    finally:
+        if os.path.exists(temporary):
+            os.unlink(temporary)
+    return target
+
+
+def load_profile(path=DEFAULT_PROFILE_PATH):
+    raw = json.loads(Path(path).expanduser().read_text(encoding="utf-8"))
+    if not isinstance(raw, dict):
+        raise ValueError("calibration must be a JSON object")
+    try:
+        return CalibrationProfile(**raw)
+    except TypeError as exc:
+        raise ValueError(f"invalid calibration: {exc}") from exc

roboapi/calibration/session.py

@@ -0,0 +1,350 @@
+"""Home and level-plane teaching with one automatic return to Home."""
+
+from __future__ import annotations
+
+import hashlib
+import json
+import logging
+import time
+from dataclasses import replace
+from pathlib import Path
+
+import numpy as np
+
+from .. import geometry as geo
+from ..semantics import ACTION_DT
+from ..operations import exclusive
+from ..errors import ControlNotReady
+from ..motion import dispatch, pose_reached, wait_for_pose
+from .profile import CalibrationProfile, load_profile, save_profile
+
+logger = logging.getLogger(__name__)
+
+
+class Calibration:
+    def __init__(self, runtime):
+        self.runtime = runtime
+        self.emb = emb = runtime.emb
+        self.operations = runtime.operations
+        self.mode = lambda: runtime.mode
+        self.cfg = emb.cfg.calibration
+        arm = emb.cfg.arm
+        identity = [arm.type, getattr(arm, "host", "") or getattr(arm, "ip", ""),
+                    self.cfg.tool_id, emb.cfg.gripper.type, emb.cfg.gripper.port,
+                    emb.cfg.camera.type, emb.cfg.camera.device]
+        self.identity = hashlib.sha256(json.dumps(identity).encode()).hexdigest()
+        self.profile = self.draft = None
+        self._motion_connection = False
+        self.guiding = emb.new_guiding_session()
+        self.guiding_axis = None
+        self._last_sample_timestamp = None
+        self.home_pose = None
+        self.plane_poses = []
+        self.device = None
+        self.phase = "acquire"
+        self.error = self.notice = None
+        try:
+            profile = load_profile(self.cfg.profile_file)
+            self._validate_profile(profile)
+            self._install(profile)
+        except FileNotFoundError:
+            pass
+        except (OSError, ValueError, TypeError) as exc:
+            self.error = f"Calibration is invalid. Calibrate again. {exc}"
+
+    def _validate_profile(self, profile):
+        profile.validate(self.identity)
+        self.emb.new_feedback_reader().validate_device(profile.device)
+
+    def _install(self, profile):
+        self.emb.arm.set_adapter(profile.adapter())
+        self.emb.calibrated = True
+        self.profile = profile
+        self.phase = "complete"
+
+    def status(self):
+        profile = self.draft if self.phase == "preview" else self.profile if self.phase == "complete" else None
+        preview = {"axes_in_native_base": profile.adapter().base_from_task[:3, :3].T.tolist()} if profile else None
+        return {"phase": self.phase, "busy": self.operations.busy,
+                "teaching_method": "guiding" if self.emb.cfg.arm.type == "franka" else "pendant",
+                "guiding_axis": self.guiding_axis,
+                "connected": bool(getattr(self.emb.feedback, "live", False)),
+                "control_connected": self.emb.connected or self._motion_connection,
+                "control_enabled": self.emb.control_ready and self.emb.connected,
+                "complete": self.emb.calibrated, "error": self.error, "notice": self.notice,
+                "tool_load_ready": ((self.guiding.tool_load_ready or self.emb.arm.native.tool_load_ready)
+                                    if self.guiding else None),
+                "preview": preview,
+                "profile_file": str(Path(self.cfg.profile_file).expanduser().resolve()),
+                "hardware_released": not self.emb.connected and not self._motion_connection
+                                     and not (self.guiding and self.guiding.active)}
+
+    def interrupt(self):
+        self.operations.abort.set()
+
+    def close(self):
+        self.emb.close_feedback()
+        if self.guiding is not None:
+            self.guiding.close()
+        if self._motion_connection:
+            self.emb.arm.native.disconnect()
+            self._motion_connection = False
+
+    def _open_reader(self):
+        try:
+            if self.guiding is not None:
+                self.guiding.start(self.guiding_axis)
+            with self.emb.prepare_feedback() as reader:
+                state = self._snapshot(reader)
+                self._last_sample_timestamp = state["timestamp"]
+        except Exception:
+            if self.guiding is not None:
+                self.guiding.close()
+            raise
+        return state
+
+    def _snapshot(self, reader=None):
+        reader = self.emb.feedback if reader is None else reader
+        if reader is None or not reader.live:
+            raise ValueError("Read-only feedback is unavailable. Start again.")
+        state = reader.snapshot()
+        reader.validate_device(state["device"])
+        if self.device is not None and state["device"] != self.device:
+            raise ValueError("Robot settings changed. Exit and calibrate again.")
+        return state
+
+    def _sample(self):
+        if self.guiding is not None:
+            self.guiding.check_stopped()
+        deadline = time.monotonic() + 0.6
+        while True:
+            self._check_stop()
+            state = self._snapshot()
+            self._check_stop()
+            if state.get("has_errors"):
+                raise ControlNotReady("Clear the robot error, then capture again.")
+            if self._last_sample_timestamp is None or state["timestamp"] > self._last_sample_timestamp:
+                pose = np.asarray(state["pose"], dtype=float)
+                geo.pose7_to_matrix(pose)
+                self._last_sample_timestamp = state["timestamp"]
+                return pose.tolist()
+            if time.monotonic() >= deadline:
+                raise ValueError("No fresh feedback. Check the connection and capture again.")
+            time.sleep(0.02)
+
+    def _require_phase(self, phase):
+        if self.phase != phase:
+            raise ValueError("Follow the current teaching step.")
+
+    def _check_stop(self):
+        if self.operations.abort.is_set():
+            raise ValueError("Calibration stopped.")
+
+    def _return_to(self, target, name):
+        """Borrow position control for a captured pose, then restore read-only sampling."""
+        self.emb.arm.check_motion_ready()
+        if self.guiding is not None:
+            self.guiding.check_stopped()
+        self.close()
+        native, cfg = self.emb.arm.native, self.emb.cfg
+        original_cfg = native.cfg if cfg.arm.type == "ur" else None
+        if getattr(original_cfg, "zero_ft_on_start", False):
+            # C is a contact sample. Preserve the sensor zero until ordinary control at Home.
+            native.cfg = replace(original_cfg, zero_ft_on_start=False)
+        self._motion_connection = True
+        completed = False
+        try:
+            self._check_stop()
+            native.connect()
+            self._check_stop()
+            if cfg.arm.type == "ur":
+                device = {**self.device, "tcp_offset": list(native.get_tcp_offset())}
+                self.emb.new_feedback_reader().validate_device(device)
+                if not np.allclose(device["tcp_offset"], self.device["tcp_offset"], atol=1e-6, rtol=0):
+                    raise ValueError("The control connection changed the TCP. Calibrate again.")
+            plan = geo.plan_path(
+                native.get_eef_pose(), [target], 0.0, [0.0], [0.0],
+                speed=cfg.arm.cartesian_speed, angular_speed=cfg.arm.cartesian_angular_speed,
+                gripper_travel_s=0.0, dt=ACTION_DT,
+            )
+            def interrupted():
+                self._check_stop()
+                if not native.live:
+                    raise ValueError(native.last_error or "Arm control is unavailable.")
+                return False
+
+            dispatch(native, plan.poses, should_stop=interrupted, position_only=True)
+            wait_for_pose(native, target, interrupted, name=name)
+            completed = True
+        finally:
+            try:
+                native.disconnect()
+            finally:
+                if original_cfg is not None:
+                    native.cfg = original_cfg
+            self._motion_connection = False
+            try:
+                self._open_reader()
+            except Exception:
+                if completed:
+                    raise
+                logger.warning("Read-only feedback could not resume after a failed return", exc_info=True)
+        pose = self._sample()
+        if not pose_reached(target, pose):
+            raise ValueError(f"{name} was not reached. Check the robot and retry Return {name}.")
+        return pose
+
+    def _return_home(self):
+        self._require_phase("return_home")
+        pose = self._return_to(self.home_pose, "Home")
+        self.draft = replace(self.draft, check_native_pose7=pose)
+        self.phase = "preview"
+
+    @exclusive
+    def action(self, action, data=None):
+        try:
+            return self._action(action, {} if data is None else data)
+        except ControlNotReady as exc:
+            self.notice = str(exc)
+            raise
+        except (ValueError, OSError, RuntimeError) as exc:
+            self.error = str(exc)
+            raise
+
+    def _action(self, action, data):
+        if not isinstance(data, dict):
+            raise ValueError("request must be an object")
+        if self.mode() != "idle":
+            raise ValueError("Stop the current action first.")
+        if action not in {"acquire", "confirm_home", "confirm_plane_a", "confirm_plane_b", "confirm_plane_c", "return_home", "return_a",
+                          "save", "undo", "enable_control", "set_guiding_axis"}:
+            raise ValueError("unknown calibration action")
+        self.operations.reset_abort()
+        self.error = self.notice = None
+        if action == "acquire":
+            if self.phase not in ("acquire", "complete"):
+                raise ValueError("Exit the current calibration first.")
+            if data.get("confirmed") is not True:
+                raise ValueError("Confirm level-plane teaching and the automatic return to Home after C.")
+            self.close()
+            self.emb.disconnect_control()
+            self._check_stop()
+            self.device = None
+            self.home_pose = self.draft = None
+            self.plane_poses = []
+            self.guiding_axis = None
+            try:
+                self.device = self._open_reader()["device"]
+                self._check_stop()
+            except Exception:
+                self.close()
+                raise
+            self.emb.control_ready = False
+            self.phase = "teach_home"
+        elif action == "undo":
+            previous = {
+                "plane_a": ("teach_home", 0),
+                "plane_b": ("plane_a", 0),
+                "plane_c": ("plane_b", 1),
+                "return_home": ("plane_c", 2),
+                "preview": ("plane_c", 2),
+            }.get(self.phase)
+            if previous is None:
+                raise ValueError("There is no recorded step to undo.")
+            phase, count = previous
+            if phase == "teach_home":
+                if self.guiding is not None:
+                    self.guiding.set_axis(None)
+                self.guiding_axis = None
+                self.home_pose = None
+            self.plane_poses = self.plane_poses[:count]
+            self.draft = None
+            self.phase = phase
+        elif action == "enable_control":
+            self._require_phase("complete")
+            self._validate_profile(self.profile)
+            if data.get("confirmed") is not True or data.get("initialization_confirmed") is not True:
+                raise ValueError("Confirm that the area is safe and hardware/gripper initialization is allowed.")
+            self.close()
+            try:
+                with self.emb.prepare_feedback() as reader:
+                    if reader.snapshot()["device"] != self.profile.device:
+                        raise ValueError("Robot settings do not match calibration. Calibrate again.")
+                self.emb.close_feedback()
+                if self.operations.abort.is_set():
+                    raise ValueError("Connection was stopped.")
+                self.runtime.connect()
+                if self.emb.cfg.arm.type == "ur":
+                    offset = list(self.emb.arm.native.get_tcp_offset())
+                    self.emb.new_feedback_reader().validate_device({
+                        "serial": self.profile.device["serial"],
+                        "tcp_offset": offset,
+                    })
+                    if not np.allclose(offset, self.profile.device["tcp_offset"], atol=1e-6, rtol=0):
+                        raise ValueError("The control connection changed the TCP. Calibrate again.")
+                if not self.emb.alive or self.operations.abort.is_set():
+                    raise ValueError("The devices are not ready or the connection was stopped.")
+                self.emb.control_ready = True
+            except Exception:
+                self.emb.control_ready = False
+                self.close()
+                self.runtime.disconnect()
+                self.emb.connect_preview()
+                raise
+        elif action == "confirm_home":
+            self._require_phase("teach_home")
+            pose = self._sample()
+            if self.guiding is not None:
+                self.guiding.set_axis("z")
+                self.guiding_axis = "z"
+            self.home_pose = pose
+            self.phase = "plane_a"
+        elif action == "set_guiding_axis":
+            if self.guiding is None or self.phase not in {"plane_a", "plane_b", "plane_c"}:
+                raise ValueError("Single-axis guiding is available during Franka plane teaching.")
+            axis = data.get("axis")
+            if axis not in ("x", "y", "z"):
+                raise ValueError("Guiding axis must be x, y or z")
+            self.guiding.set_axis(axis)
+            self.guiding_axis = axis
+        elif action == "confirm_plane_a":
+            self._require_phase("plane_a")
+            self.plane_poses = [self._sample()]
+            self.phase = "plane_b"
+        elif action == "confirm_plane_b":
+            self._require_phase("plane_b")
+            pose = self._sample()
+            if np.linalg.norm(np.asarray(pose[:3]) - self.plane_poses[0][:3]) <= 1e-6:
+                raise ValueError("Place B away from A toward task +X.")
+            self.plane_poses.append(pose)
+            self.phase = "plane_c"
+        elif action == "confirm_plane_c":
+            self._require_phase("plane_c")
+            plane = [*self.plane_poses, self._sample()]
+            candidate = CalibrationProfile(self.identity, self.device, self.home_pose, plane)
+            candidate.adapter()
+            self.plane_poses = plane
+            self.draft = candidate
+            self.phase = "return_home"
+            self._return_home()
+        elif action == "return_home":
+            self._return_home()
+        elif action == "return_a":
+            if self.phase not in {"plane_b", "plane_c"}:
+                raise ValueError("Capture A before using Return A during plane teaching.")
+            self._return_to(self.plane_poses[0], "A")
+        elif action == "save":
+            self._require_phase("preview")
+            if data.get("confirmed") is not True:
+                raise ValueError("Confirm saving the calibration.")
+            self._snapshot()
+            self._validate_profile(self.draft)
+            if self.operations.abort.is_set():
+                raise ValueError("Saving was stopped.")
+            save_profile(self.draft, self.cfg.profile_file)
+            self._install(self.draft)
+            if self.guiding is not None:
+                self.guiding.close()
+        return self.status()
+
+