roboapi 0.4.2__py3-none-any.whl

roboapi/hardware/franka/settings.py

@@ -0,0 +1,116 @@
+"""Franka model definitions, control settings and the standard end-effector assembly."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+from ...config.models import ArmConfig
+from .limits import load_limits
+
+
+ROBOT_MODELS = ("fr3", "panda")
+
+
+@dataclass(frozen=True)
+class ToolLoad:
+    mass_kg: float
+    com_flange_m: tuple[float, float, float]
+    inertia_kg_m2: tuple[tuple[float, float, float], ...]
+
+
+STANDARD_TOOL = ToolLoad(
+    mass_kg=1.3828473321773211,
+    com_flange_m=(0.020587322750448254, 0.0021358917079591813, 0.04633173957609609),
+    inertia_kg_m2=((0.00031, 0, 0), (0, 0.00031, 0), (0, 0, 0.00031)),
+)
+
+COLLISION_TORQUE = (100.0,) * 7  # External joint torques, N·m.
+COLLISION_FORCE = (100.0,) * 6  # Cartesian forces (N) and torques (N·m).
+
+
+@dataclass
+class JointImpedanceConfig:
+    """Franka's one supported torque-control path."""
+
+    stiffness: list[float] = field(default_factory=lambda: [40, 30, 50, 25, 35, 25, 10])
+    damping: list[float] = field(default_factory=lambda: [4, 6, 5, 5, 3, 2, 1])
+    cartesian_stiffness: list[float] = field(default_factory=lambda: [400, 400, 400, 15, 15, 15])
+    cartesian_damping: list[float] = field(default_factory=lambda: [37, 37, 37, 2, 2, 2])
+    max_delta_tau: float = 2.0
+    gains_time_constant: float = 0.30
+
+
+@dataclass(frozen=True)
+class IKConfig:
+    nominal_dt: float = 0.02
+    target_lead_time: float = 0.005
+    max_translation_step: float = 0.0225
+    max_rotation_step: float = 0.045
+    max_joint_step: tuple[float, ...] = (0.06,) * 7
+    max_translation_velocity: float = 3.0
+    max_rotation_velocity: float = 2.5
+    max_joint_velocity: tuple[float, ...] = load_limits("fr3", None).joint_velocity
+    regularization: float = 0.01
+    singularity_soft_threshold: float = 0.08
+    singularity_hard_threshold: float = 0.03
+    singularity_max_regularization: float = 0.08
+    joint_limit_margin: float = 0.30
+    nullspace_gain: float = 0.025
+    # Continuous secondary posture objective.  The correction is projected onto
+    # the structural 7-DoF nullspace after the Cartesian QP, so it cannot trade
+    # away task velocity. A zero gain disables this objective.
+    posture_reference: tuple[float, ...] = (0.0,) * 7
+    posture_weights: tuple[float, ...] = (1.0,) * 7
+    posture_gain: float = 0.0
+    posture_max_velocity: float = 0.25
+    posture_task_velocity_threshold: float = 0.01
+    posture_rotation_velocity_threshold: float = 0.03
+    translation_feedback_gain: float = 12.0
+    rotation_feedback_gain: float = 12.0
+    max_joint_acceleration: tuple[float, ...] = load_limits("fr3", None).joint_acceleration
+
+
+@dataclass
+class FrankaArmConfig(ArmConfig):
+    type: str = "franka"
+    host: str = ""
+    realtime_config: str = "enforce"  # enforce | ignore  (ignore = dev box without PREEMPT_RT)
+    desk_user: str = "franka"
+    desk_password_file: str = "~/.config/roboapi/franka-desk-password"
+    robot_model: str = "fr3"
+    cartesian_angular_speed: float = 0.5
+    hybrid_hz: float = 50.0
+    trace_capacity: int = 6000
+    joint_impedance: JointImpedanceConfig = field(default_factory=JointImpedanceConfig)
+
+    @classmethod
+    def for_model(cls, model: str, *, host: str) -> FrankaArmConfig:
+        if model not in ROBOT_MODELS:
+            raise ValueError("arm.robot_model must be panda|fr3")
+        return cls(host=host, robot_model=model)
+
+    def validate(self) -> None:
+        if not self.host:
+            raise ValueError("arm.type=franka requires arm.host")
+        if self.realtime_config not in ("enforce", "ignore"):
+            raise ValueError("arm.realtime_config must be enforce|ignore")
+        if self.robot_model not in ROBOT_MODELS:
+            raise ValueError("arm.robot_model must be panda|fr3")
+        for name, value, size in (
+            ("joint_impedance.stiffness", self.joint_impedance.stiffness, 7),
+            ("joint_impedance.damping", self.joint_impedance.damping, 7),
+            ("joint_impedance.cartesian_stiffness", self.joint_impedance.cartesian_stiffness, 6),
+            ("joint_impedance.cartesian_damping", self.joint_impedance.cartesian_damping, 6),
+        ):
+            if len(value) != size or not all(isinstance(x, (int, float)) and float("-inf") < x < float("inf") for x in value):
+                raise ValueError(f"arm.{name} must contain {size} finite values")
+        if not self.desk_user or not self.desk_password_file:
+            raise ValueError("franka Desk user and password file are required")
+        if self.hybrid_hz <= 0 or self.trace_capacity < 1:
+            raise ValueError("franka hybrid_hz/trace_capacity must be positive")
+        for name, value in (
+            ("joint_impedance.max_delta_tau", self.joint_impedance.max_delta_tau),
+            ("joint_impedance.gains_time_constant", self.joint_impedance.gains_time_constant),
+        ):
+            if not isinstance(value, (int, float)) or not 0.0 < value < float("inf"):
+                raise ValueError(f"arm.{name} must be a positive finite value")

roboapi/hardware/robotiq.py

@@ -0,0 +1,350 @@
+"""Robotiq 2F control over Modbus RTU or the URCap TCP socket.
+
+Both transports expose opening in [0, 1], where 0 is closed and 1 is open.
+The shared conversion maps this to raw device positions, 0=open and 255=closed.
+"""
+
+from __future__ import annotations
+
+import logging
+import re
+import socket
+import threading
+import time
+
+logger = logging.getLogger(__name__)
+
+_POS_MAX = 255
+_STA_ACTIVE = 3
+
+
+def raw_from_ratio(ratio: float) -> int:
+    """Opening ratio [0,1] (0=closed, 1=open) -> Robotiq raw [0,255] (0=open, 255=closed)."""
+    return int(round(_POS_MAX * (1.0 - min(1.0, max(0.0, float(ratio))))))
+
+
+def ratio_from_raw(raw: int) -> float:
+    return 1.0 - (float(raw) / _POS_MAX)
+
+
+class _RobotiqBase:
+    """Device logic. Subclasses supply only the transport: open/close, the three commands,
+    and one status read.
+
+    A background thread sends the newest target only, so serial or socket I/O never blocks
+    the 30 Hz dispatch loop, and refreshes the position cache on a deadline.
+    """
+
+    def __init__(self, cfg) -> None:
+        self.cfg = cfg
+        self._cond = threading.Condition()
+        self._target: int | None = None
+        self._last_sent: int | None = None
+        self._last_raw = 0
+        self._stop = False
+        self._thread: threading.Thread | None = None
+        self._fails = 0
+        self._error: str | None = None
+        self._connected = False
+        self._motion_abort = threading.Event()
+        self._io_lock = threading.Lock()
+
+    # -- transport hooks ---------------------------------------------------
+    def _open(self) -> None: raise NotImplementedError
+    def _close(self) -> None: raise NotImplementedError
+    def _cmd_reset(self) -> None: raise NotImplementedError
+    def _cmd_activate(self) -> None: raise NotImplementedError
+    def _cmd_goto(self, pos: int, speed: int, force: int) -> None: raise NotImplementedError
+
+    def _read_status(self) -> dict:
+        """`sta` (3 = active), `obj` (0 = moving), `pos` (0 open .. 255 closed), `fault`."""
+        raise NotImplementedError
+
+    def _fail(self, detail: str) -> None:
+        self._fails += 1
+        self._error = detail
+
+    def _ok(self) -> None:
+        self._fails = 0
+        self._error = None
+
+    # -- protocol ----------------------------------------------------------
+    def activate(self) -> None:
+        self._cmd_reset()
+        time.sleep(0.5)
+        self._cmd_activate()
+        deadline = time.time() + 10.0
+        last = None
+        while time.time() < deadline:
+            last = self._read_status()
+            if last["sta"] == _STA_ACTIVE and last["fault"] == 0:
+                return
+            time.sleep(0.2)
+        raise TimeoutError(f"gripper activation did not complete; last status={last}")
+
+    def _goto(self, pos: int, wait: bool = False, should_stop=None) -> None:
+        with self._cond:
+            if wait:
+                self._motion_abort.clear()
+                self._target = None
+            if should_stop is not None and should_stop():
+                raise RuntimeError("gripper_move_cancelled")
+            self._cmd_goto(pos, self.cfg.speed, self.cfg.force)
+        if not wait:
+            return
+        deadline = time.monotonic() + 5.0
+        while time.monotonic() < deadline:
+            if self._motion_abort.is_set() or (should_stop is not None and should_stop()):
+                self.hold()
+                raise RuntimeError("gripper_move_cancelled")
+            if self._read_status()["obj"] != 0:  # reached the target or gripped something
+                return
+            time.sleep(0.05)
+        self.hold()
+        raise TimeoutError(f"gripper did not reach {pos} within 5s")
+
+    # -- Gripper -----------------------------------------------------------
+    @property
+    def live(self) -> bool:
+        return self._connected and self._fails < 3
+
+    @property
+    def last_error(self) -> str | None:
+        return self._error
+
+    def connect(self) -> None:
+        self._open()
+        self._connected = True
+        status = self._read_status()
+        if status["sta"] == _STA_ACTIVE and status["fault"] == 0x09:
+            # An idle-link watchdog can clear on resumed reads; avoid an unnecessary
+            # activation sweep before giving the already-active gripper another poll.
+            time.sleep(0.2)
+            status = self._read_status()
+        if status["sta"] != _STA_ACTIVE or status["fault"] != 0:
+            logger.info("gripper inactive/faulted (%s); activating", status)
+            self.activate()
+        self._last_raw = self._read_status()["pos"]
+        self._stop = False
+        self._thread = threading.Thread(target=self._run, name="robotiq", daemon=True)
+        self._thread.start()
+
+    def disconnect(self) -> None:
+        self._motion_abort.set()
+        with self._cond:
+            self._stop = True
+            self._target = self._last_sent = None
+            self._cond.notify()
+        if self._thread is not None:
+            self._thread.join(timeout=2.0)
+            self._thread = None
+        if self._connected:
+            try:
+                self._cmd_activate()  # drop GTO: never leave the gripper chasing a target
+            except Exception:
+                pass
+            try:
+                self._close()
+            finally:
+                self._connected = False
+
+    def get_ratio(self) -> float:
+        return ratio_from_raw(self._last_raw)
+
+    def set_ratio(self, ratio: float) -> None:
+        with self._cond:
+            self._target = raw_from_ratio(ratio)
+            self._cond.notify()
+
+    def hold(self) -> None:
+        """Cancel queued and active GTO without releasing a held object."""
+        self._motion_abort.set()
+        with self._cond:
+            self._target = self._last_sent = None
+            if self._connected:
+                self._cmd_activate()
+
+    def move_blocking(self, ratio: float, should_stop=None) -> None:
+        target = raw_from_ratio(ratio)
+        self._goto(target, wait=True, should_stop=should_stop)
+        raw = self._read_status()["pos"]
+        with self._cond:
+            if self._motion_abort.is_set() or (should_stop is not None and should_stop()):
+                raise RuntimeError("gripper_move_cancelled")
+            self._last_sent = target
+            self._last_raw = raw
+
+    def recover(self) -> None:
+        """Reopen the connection and activate the gripper if needed."""
+        try:
+            self.disconnect()
+        except Exception as exc:
+            logger.warning("gripper disconnect during recover failed: %s", exc)
+        self._ok()
+        self.connect()
+
+    def _run(self) -> None:
+        """Event-driven send of the newest target only, plus a deadline-paced status refresh.
+        No unconditional sleep, so a notify can never be lost behind one."""
+        next_status = time.monotonic()
+        while True:
+            with self._cond:
+                while not self._stop and self._target is None and time.monotonic() < next_status:
+                    self._cond.wait(timeout=max(0.0, next_status - time.monotonic()))
+                if self._stop:
+                    return
+                target, self._target = self._target, None
+                if target is not None and target != self._last_sent:
+                    try:
+                        self._goto(target)
+                        self._last_sent = target
+                    except Exception as exc:
+                        logger.warning("gripper goto(%s) failed: %s", target, exc)
+            now = time.monotonic()
+            if now >= next_status:
+                try:
+                    self._last_raw = self._read_status()["pos"]
+                except Exception as exc:
+                    logger.debug("gripper status read failed: %s", exc)
+                next_status = now + 1.0 / self.cfg.rate_hz
+
+
+class RobotiqModbus(_RobotiqBase):
+    """Modbus RTU over pyserial. Framing, CRC, the inter-frame gap and exception decoding are
+    minimalmodbus's job; only the retry policy stays ours, because RS-485 noise is normal and
+    the library gives up on the first bad frame."""
+
+    _WRITE_REG = 0x03E8  # 1000
+    _READ_REG = 0x07D0   # 2000
+
+    def __init__(self, cfg) -> None:
+        super().__init__(cfg)
+
+    def _open(self) -> None:
+        import minimalmodbus
+        import serial
+
+        inst = minimalmodbus.Instrument(self.cfg.port, self.cfg.slave_id, mode=minimalmodbus.MODE_RTU)
+        inst.serial.baudrate = self.cfg.baudrate
+        inst.serial.bytesize = serial.EIGHTBITS
+        inst.serial.parity = serial.PARITY_NONE
+        inst.serial.stopbits = serial.STOPBITS_ONE
+        inst.serial.timeout = self.cfg.timeout_s
+        inst.clear_buffers_before_each_transaction = True
+        inst.close_port_after_each_call = False
+        self._inst = inst
+
+    def _close(self) -> None:
+        self._inst.serial.close()
+
+    def _retry(self, fn, *args):
+        # minimalmodbus owns framing, but not cross-thread serialization.  Keep the whole
+        # transaction (including buffer recovery and retry) atomic so the status poller and
+        # a blocking reset cannot consume bytes from each other's RTU response.
+        with self._io_lock:
+            last = None
+            for _ in range(2):
+                try:
+                    out = fn(*args)
+                    self._ok()
+                    return out
+                except Exception as exc:
+                    last = exc
+                    try:
+                        self._inst.serial.reset_input_buffer()
+                        self._inst.serial.reset_output_buffer()
+                    except Exception:
+                        pass
+            self._fail(f"modbus: {last}")
+            raise last
+
+    def _write(self, action: int, pos: int = 0, speed: int = 0, force: int = 0) -> None:
+        # Robotiq's 6-byte output block as three big-endian registers.
+        self._retry(self._inst.write_registers, self._WRITE_REG,
+                    [(action << 8), pos, (speed << 8) | force])
+
+    def _cmd_reset(self) -> None: self._write(0x00)
+    def _cmd_activate(self) -> None: self._write(0x01)                       # rACT, GTO clear
+    def _cmd_goto(self, pos, speed, force) -> None: self._write(0x09, pos, speed, force)
+
+    def _read_status(self) -> dict:
+        # One read returns the whole status block -- unlike the URCap socket, which needs a
+        # round trip per variable.
+        regs = self._retry(self._inst.read_registers, self._READ_REG, 3, 4)
+        status, fault, pos = regs[0] >> 8, regs[1] >> 8, regs[2] >> 8
+        return {"sta": (status >> 4) & 0x03, "obj": (status >> 6) & 0x03, "pos": pos, "fault": fault}
+
+
+class RobotiqURCap(_RobotiqBase):
+    """The URCap socket on TCP 63352, served by the UR controller the gripper hangs off.
+
+    Text protocol: ``SET k v ...`` -> ack, ``GET k`` -> "k value". Status costs one round trip
+    per variable, so the whole block is read in one place and cached like the Modbus one.
+    """
+
+    def _open(self) -> None:
+        self._sock = socket.create_connection((self.cfg.ip, self.cfg.tcp_port), self.cfg.timeout_s)
+        self._sock.settimeout(self.cfg.timeout_s)
+
+    def _close(self) -> None:
+        self._sock.close()
+
+    def _txn(self, line: str) -> str:
+        try:
+            with self._io_lock:
+                deadline = time.monotonic() + self.cfg.timeout_s
+                self._sock.settimeout(self.cfg.timeout_s)
+                self._sock.sendall((line + "\n").encode())
+                self._sock.settimeout(max(0.001, deadline - time.monotonic()))
+                reply = bytearray(self._sock.recv(1024))
+                if not reply:
+                    raise ConnectionError("URCap closed the connection")
+                # URCap replies need not end in newline. A read-only STA query gives
+                # numeric replies an unambiguous boundary, including split POS digits.
+                remaining = deadline - time.monotonic()
+                if remaining <= 0:
+                    raise TimeoutError("URCap reply deadline exceeded")
+                self._sock.settimeout(remaining)
+                self._sock.sendall(b"GET STA\n")
+                primary = "ack" if line.startswith("SET ") else re.escape(line.split()[1]) + r" [0-9]{1,3}"
+                pattern = re.compile(r"\s*(" + primary + r")\s*STA [0-3]\s*")
+                while True:
+                    match = pattern.fullmatch(reply.decode("ascii"))
+                    if match:
+                        resp = match.group(1)
+                        break
+                    remaining = deadline - time.monotonic()
+                    if remaining <= 0 or len(reply) > 1024:
+                        raise TimeoutError("incomplete or invalid URCap reply")
+                    self._sock.settimeout(remaining)
+                    chunk = self._sock.recv(1024)
+                    if not chunk:
+                        raise ConnectionError("URCap closed the connection")
+                    reply.extend(chunk)
+            self._ok()
+            return resp
+        except Exception as exc:
+            self._sock.close()  # partial replies must never belong to a later transaction
+            self._connected = False
+            self._fail(f"urcap: {exc}")
+            raise
+
+    def _set(self, **kv: int) -> None:
+        self._txn("SET" + "".join(f" {k} {v}" for k, v in kv.items()))
+
+    def _get(self, var: str) -> int:
+        value = int(self._txn(f"GET {var}").split()[-1])
+        if not 0 <= value <= (3 if var in {"STA", "OBJ"} else 255):
+            self._fail("invalid URCap " + var + " value")
+            self._sock.close()
+            self._connected = False
+            raise ValueError(self.last_error)
+        return value
+
+    def _cmd_reset(self) -> None: self._set(ACT=0, ATR=0)
+    def _cmd_activate(self) -> None: self._set(ACT=1, GTO=0)
+    def _cmd_goto(self, pos, speed, force) -> None: self._set(POS=pos, SPE=speed, FOR=force, GTO=1)
+
+    def _read_status(self) -> dict:
+        # This protocol exposes no fault variable, so activation is driven off STA alone.
+        return {"sta": self._get("STA"), "obj": self._get("OBJ"), "pos": self._get("POS"), "fault": 0}

roboapi/hardware/ur/__init__.py

@@ -0,0 +1,16 @@
+"""UR arm assembly by controller generation."""
+
+from .settings import CB3ArmConfig, ESeriesArmConfig
+
+
+def create_arm(cfg):
+    if isinstance(cfg, CB3ArmConfig):
+        from .position import PositionArm
+        return PositionArm(cfg)
+    if isinstance(cfg, ESeriesArmConfig):
+        from .compliant import CompliantArm
+        return CompliantArm(cfg)
+    raise ValueError("UR requires a CB3 or e-Series configuration")
+
+
+__all__ = ["create_arm"]

roboapi/hardware/ur/build.py

@@ -0,0 +1,92 @@
+"""Build the locked UR e-Series control runtime, entirely in the supplied directories."""
+from __future__ import annotations
+import hashlib
+import json
+import os
+from pathlib import Path, PurePosixPath
+import subprocess
+import tarfile
+import urllib.request
+
+import tyro
+
+
+def run(*args):
+    subprocess.run(args, check=True)
+
+
+def source(name, record, root, archives):
+    """Use the immutable GitHub commit tarball; record/check every source byte."""
+    archives.mkdir(parents=True, exist_ok=True)
+    archive = archives / (name + ".tar.gz")
+    repository = record["repository"].removeprefix("https://github.com/").removesuffix(".git")
+    url = "https://api.github.com/repos/" + repository + "/tarball/" + record["ref"]
+    if not archive.exists():
+        request = urllib.request.Request(url, headers={"User-Agent": "roboapi-native-build"})
+        with urllib.request.urlopen(request) as response:
+            data = response.read()
+        archive.write_bytes(data)
+    path = root / (name + "-" + record["ref"] + "-archive")
+    hashes = {}
+    with tarfile.open(archive, "r:gz") as bundle:
+        for entry in bundle:
+            parts = PurePosixPath(entry.name).parts
+            if parts and not parts[0].endswith(record["ref"][:7]):
+                raise ValueError(name + " tarball is not for the locked GitHub commit")
+            if len(parts) < 2 or not entry.isfile():
+                continue
+            relative = PurePosixPath(*parts[1:])
+            if ".." in relative.parts or relative.is_absolute():
+                raise ValueError("invalid dependency archive path")
+            target = path.joinpath(*relative.parts)
+            data = bundle.extractfile(entry).read()
+            if target.is_symlink() or (target.exists() and target.read_bytes() != data):
+                raise ValueError(name + " source is modified: " + str(relative))
+            if not target.exists():
+                target.parent.mkdir(parents=True, exist_ok=True)
+                target.write_bytes(data)
+                target.chmod(entry.mode & 0o777)
+            hashes[str(relative)] = hashlib.sha256(data).hexdigest()
+    (root / (name + "-source.json")).write_text(json.dumps({
+        "ref": record["ref"], "url": url, "archive_sha256": hashlib.sha256(archive.read_bytes()).hexdigest(),
+        "files": hashes,
+    }, indent=2) + "\n")
+    return path
+
+
+def build_runtime(build_dir: Path, install_dir: Path | None = None,
+                  archive_dir: Path | None = None, jobs: int = 4) -> None:
+    """Build and install the locked UR e-Series control runtime.
+
+    Args:
+        build_dir: Build directory for sources, temporary files and CMake output.
+        install_dir: Installation prefix; defaults to BUILD_DIR/install.
+        archive_dir: Already downloaded locked URCL/Ruckig tarballs; defaults to BUILD_DIR/archives.
+        jobs: Number of parallel build jobs.
+    """
+    native = Path(__file__).with_name("native")
+    build = build_dir.resolve()
+    scratch = build / "tmp"
+    scratch.mkdir(parents=True, exist_ok=True)
+    os.environ["TMPDIR"] = str(scratch)
+    install = install_dir.resolve() if install_dir else build / "install"
+    sources = build / "sources"
+    sources.mkdir(parents=True, exist_ok=True)
+    lock = json.loads((native / "dependencies.lock.json").read_text())
+    archives = archive_dir.resolve() if archive_dir else build / "archives"
+    deps = {name: source(name, lock[name], sources, archives) for name in ("urcl", "ruckig")}
+    run("cmake", "-S", str(native), "-B", str(build / "cmake"),
+        "-DCMAKE_BUILD_TYPE=Release", "-DBUILD_TESTING=OFF",
+        "-DCMAKE_EXPORT_NO_PACKAGE_REGISTRY=ON",
+        "-DURCL_SOURCE_DIR=" + str(deps["urcl"]), "-DRUCKIG_SOURCE_DIR=" + str(deps["ruckig"]),
+        "-DCMAKE_INSTALL_PREFIX=" + str(install))
+    run("cmake", "--build", str(build / "cmake"), "--parallel", str(jobs))
+    subprocess.run(["ctest", "--output-on-failure"], cwd=build / "cmake", check=True)
+    run("cmake", "--install", str(build / "cmake"))
+    print("Native UR executable:", install / "bin/roboapi-ur-loop")
+    print("Matching URScript:", install / "share/roboapi-ur-loop/external_control.urscript")
+
+
+if __name__ == "__main__":
+    tyro.cli(build_runtime, prog="python -m roboapi.hardware.ur.build",
+             config=(tyro.conf.DisallowNone,))