roboapi 0.4.2__py3-none-any.whl

roboapi/embodiment.py

@@ -0,0 +1,251 @@
+"""Physical device composition and access through the shared coordinate mapping."""
+
+from __future__ import annotations
+
+import logging
+import threading
+from contextlib import contextmanager
+from dataclasses import replace
+
+import numpy as np
+
+from .config import Config
+from .errors import CalibrationRequired, ControlNotReady, NotAlive
+from .semantics import TRAINING_FRAME
+
+logger = logging.getLogger(__name__)
+
+
+class Embodiment:
+    """One arm, gripper and camera; device lifecycle and mapped Cartesian access."""
+
+    def __init__(self, cfg: Config, arm, gripper, camera) -> None:
+        self.cfg = cfg
+        self.arm = arm
+        self.gripper = gripper
+        self.camera = camera
+        self.command_lock = threading.Lock()
+        self.connected = False
+        self.calibrated = False
+        self.control_ready = False
+        self.feedback = None
+        self.preview_errors = {}
+        self.preview_notices = {}
+
+    def new_feedback_reader(self):
+        arm = self.cfg.arm
+        if arm.type == "ur":
+            from .hardware.ur.feedback import URFeedbackReader
+            return URFeedbackReader(arm.ip)
+        from .hardware.franka.feedback import FrankaFeedbackReader
+        return FrankaFeedbackReader(arm)
+
+    def new_guiding_session(self):
+        if self.cfg.arm.type == "franka":
+            from .hardware.franka.guiding import FrankaGuiding
+            return FrankaGuiding(self.cfg.arm)
+        return None
+
+    def close_feedback(self) -> None:
+        reader, self.feedback = self.feedback, None
+        if reader is not None:
+            try:
+                reader.disconnect()
+            except BaseException:
+                self.feedback = reader
+                raise
+
+    @contextmanager
+    def prepare_feedback(self):
+        """Publish feedback only after connection and caller checks succeed."""
+        self.close_feedback()
+        reader = self.new_feedback_reader()
+        try:
+            reader.connect()
+            yield reader
+        except BaseException:
+            self.feedback = reader
+            try:
+                self.close_feedback()
+            except Exception:
+                logger.warning("Feedback cleanup failed", exc_info=True)
+            raise
+        self.preview_errors.pop("arm", None)
+        self.preview_notices.pop("arm", None)
+        self.feedback = reader
+
+    def connect_preview(self) -> None:
+        """Connect independent read-only sources without initializing arm or gripper control."""
+        for name in ("camera", "arm"):
+            try:
+                if name == "camera":
+                    self.camera.connect()
+                elif not self.connected and not getattr(self.feedback, "live", False):
+                    with self.prepare_feedback():
+                        pass
+                self.preview_errors.pop(name, None)
+                self.preview_notices.pop(name, None)
+            except ControlNotReady as exc:
+                self.preview_errors.pop(name, None)
+                self.preview_notices[name] = str(exc)
+                logger.info("%s preview: %s", name, exc)
+            except Exception as exc:
+                self.preview_notices.pop(name, None)
+                self.preview_errors[name] = str(exc)
+                logger.warning("%s preview unavailable: %s", name, exc)
+
+    def connect(self) -> None:
+        if self.connected:
+            return
+        if not self.calibrated:
+            raise CalibrationRequired("finish installation calibration before connecting control")
+        self.arm.check_motion_ready()
+        self.close_feedback()
+        self.preview_errors.clear()
+        self.preview_notices.clear()
+        connected = []
+
+        def rollback_part(part) -> None:
+            try:
+                part.disconnect()
+            except Exception as exc:  # rollback must not hide the original failure
+                logger.warning("connect rollback(%s) failed: %s", type(part).__name__, exc)
+
+        try:
+            # Arm last: it acquires the Desk token and starts torque control.
+            for part in (self.gripper, self.camera, self.arm):
+                try:
+                    part.connect()
+                    connected.append(part)
+                except Exception:
+                    # A component can allocate resources before connect() reports failure (the
+                    # camera starts its reader thread before its first-frame timeout). Clean up
+                    # the failing component as well as all earlier components.
+                    rollback_part(part)
+                    raise
+        except Exception:
+            for part in reversed(connected):
+                rollback_part(part)
+            raise
+        self.connected = True
+
+    def disconnect_control(self) -> None:
+        """Leave control before teaching, preserving camera preview and surfacing failures."""
+        self.control_ready = False
+        self.arm.disconnect()
+        self.gripper.disconnect()
+        self.connected = False
+
+    def disconnect(self) -> None:
+        self.close_feedback()
+        for part in (self.arm, self.camera, self.gripper):
+            try:
+                part.disconnect()
+            except Exception as exc:  # teardown must not mask the original failure
+                logger.warning("disconnect(%s) failed: %s", type(part).__name__, exc)
+        self.connected = False
+        self.preview_errors.clear()
+        self.preview_notices.clear()
+
+    def require_control(self) -> None:
+        if not self.control_ready:
+            raise CalibrationRequired("calibration_required: finish coordinate setup before using robot tools")
+
+    def status(self) -> dict:
+        arm_control_connected = bool(self.connected and self.arm.live)
+        arm_connected = arm_control_connected or bool(getattr(self.feedback, "live", False))
+        gripper_connected = bool(self.connected and self.gripper.live)
+        camera_connected = bool(self.camera.live)
+        age = None
+        if camera_connected:
+            try:
+                _, age = self.camera.read()
+            except Exception:
+                camera_connected = False
+        connected = arm_connected and gripper_connected and camera_connected
+        errors = self.device_errors
+        return {"connected": connected,
+                "alive": bool(self.connected and connected and age is not None and age <= self.cfg.camera.stale_after_s),
+                "arm_connected": arm_connected, "gripper_connected": gripper_connected,
+                "arm_control_connected": arm_control_connected,
+                "camera_connected": camera_connected,
+                "camera_age_ms": age * 1e3 if age is not None else None,
+                "camera_stale_after_ms": self.cfg.camera.stale_after_s * 1e3,
+                "device_notices": dict(self.preview_notices),
+                "device_errors": errors,
+                "last_error": next(filter(None, errors.values()), None)}
+
+    @property
+    def alive(self) -> bool:
+        return self.status()["alive"]
+
+    @property
+    def device_errors(self) -> dict:
+        return {"arm": self.arm.last_error if self.connected else (
+                    getattr(self.feedback, "last_error", None) or self.preview_errors.get("arm")),
+                "gripper": self.gripper.last_error if self.connected else None,
+                "camera": getattr(self.camera, "last_error", None) or self.preview_errors.get("camera")}
+
+    @property
+    def last_error(self) -> str | None:
+        return next(filter(None, self.device_errors.values()), None)
+
+    def require_alive(self) -> None:
+        self.require_control()
+        if not self.alive:
+            raise NotAlive(f"not_alive: {self.last_error or 'camera stale or disconnected'}. Call recover().")
+
+    def get_state(self) -> dict:
+        """Full state. `joint_states` lives here and in the log, but not on the MCP surface:
+        the agent cannot act on six radians, and the policy observation does not contain them."""
+        return {
+            "joint_states": self.arm.get_joints().tolist(),
+            "eef_pose": self.arm.get_eef_pose().tolist(),
+            "eef_frame": TRAINING_FRAME,
+            "gripper_state": float(self.gripper.get_ratio()),
+            "alive": self.alive,
+        }
+
+    def get_camera(self) -> tuple[np.ndarray, float]:
+        return self.camera.read()
+
+    def hold(self) -> None:
+        """Atomically stop UR/Robotiq; other arms retain their measured-state hold."""
+        with self.command_lock:
+            try:
+                if hold := getattr(self.arm, "hold", None):
+                    hold()
+                elif self.arm.live:
+                    self.arm.servo_pose(self.arm.get_eef_pose(), np.zeros(6))
+            finally:
+                if hold := getattr(self.gripper, "hold", None):
+                    hold()
+
+
+def build_embodiment(cfg: Config) -> Embodiment:
+    """Assemble one synchronous robot stack with lazy optional-driver imports."""
+    cfg.validate()
+    if cfg.arm.type == "ur":
+        from .hardware.ur import create_arm
+        arm = create_arm(cfg.arm)
+    else:
+        from .hardware.franka import FrankaArm
+        arm = FrankaArm(cfg.arm)
+
+    from .geometry import FrameAdapter
+    from .hardware.base import FramedArm
+
+    identity = [0, 0, 0, 1, 0, 0, 0]
+    arm = FramedArm(arm, FrameAdapter(identity, identity))
+
+    if cfg.gripper.type == "urcap":
+        from .hardware.robotiq import RobotiqURCap
+        gripper = RobotiqURCap(replace(cfg.gripper, ip=cfg.gripper.ip or cfg.arm.ip))
+    else:
+        from .hardware.robotiq import RobotiqModbus
+        gripper = RobotiqModbus(cfg.gripper)
+
+    from .hardware.camera import V4L2Camera
+    camera = V4L2Camera(cfg.camera.device)
+
+    return Embodiment(cfg, arm, gripper, camera)

roboapi/errors.py

@@ -0,0 +1,37 @@
+"""Expected runtime conditions exposed by all operation interfaces."""
+
+
+class PolicyUnavailable(RuntimeError):
+    """The model API cannot complete the current operation."""
+
+
+class RobotError(RuntimeError):
+    """Base for expected conditions returned through the MCP surface."""
+
+    error = "robot_error"
+    hint = ""
+
+
+class NotAlive(RobotError):
+    error = "not_alive"
+    hint = "call recover()"
+
+
+class ControlNotReady(RobotError):
+    error = "control_not_ready"
+    hint = "follow the controller instruction and retry the same operation"
+
+
+class LoopRunning(RobotError):
+    error = "loop_running"
+    hint = "call stop() first"
+
+
+class CalibrationRequired(RobotError):
+    error = "calibration_required"
+    hint = "open Devices > Calibrate in the web interface, confirm safety and finish coordinate calibration"
+
+
+class CalibrationError(RobotError):
+    error = "calibration_action_failed"
+    hint = "follow the installation phase and inspect detail"

roboapi/geometry.py

@@ -0,0 +1,337 @@
+"""Pose representations and the conversions between them.
+
+Three representations meet in this project and they are easy to confuse:
+
+  * **pose7d (ours, public)** -- ``[x, y, z, qw, qx, qy, qz]``. Everything above the driver
+    boundary speaks this: `get_state`, `servo_pose`, the MCP surface.
+  * **pose7d (policy wire)** -- ``[x, y, z, qx, qy, qz, qw]``. scipy order. The
+    observation we send the policy server uses THIS, so `obs_pose` reorders at that seam.
+
+Also **rotvec6** ``[x, y, z, rx, ry, rz]`` (axis-angle), which is what UR's ``servoL`` and
+``getActualTCPPose`` speak.
+"""
+
+from __future__ import annotations
+
+from typing import NamedTuple
+
+import numpy as np
+from scipy.spatial.transform import Rotation, Slerp
+
+
+def quat_wxyz_to_xyzw(q: np.ndarray) -> np.ndarray:
+    q = np.asarray(q, dtype=np.float64)
+    return np.concatenate([q[..., 1:4], q[..., 0:1]], axis=-1)
+
+
+def quat_xyzw_to_wxyz(q: np.ndarray) -> np.ndarray:
+    q = np.asarray(q, dtype=np.float64)
+    return np.concatenate([q[..., 3:4], q[..., 0:3]], axis=-1)
+
+
+def normalize_quat(q: np.ndarray) -> np.ndarray:
+    q = np.asarray(q, dtype=np.float64)
+    n = np.linalg.norm(q, axis=-1, keepdims=True)
+    if np.any(n == 0.0):
+        raise ValueError("zero-norm quaternion")
+    return q / n
+
+
+def pose7_to_matrix(pose7: np.ndarray) -> np.ndarray:
+    """Our pose7d (wxyz) -> a homogeneous transform."""
+    p = np.asarray(pose7, dtype=np.float64)
+    if p.shape != (7,) or not np.all(np.isfinite(p)):
+        raise ValueError("pose7 must contain 7 finite values")
+    out = np.eye(4)
+    out[:3, :3] = Rotation.from_quat(quat_wxyz_to_xyzw(normalize_quat(p[3:7]))).as_matrix()
+    out[:3, 3] = p[:3]
+    return out
+
+
+def matrix_to_pose7(matrix: np.ndarray) -> np.ndarray:
+    """A homogeneous transform -> our pose7d (wxyz)."""
+    transform = np.asarray(matrix, dtype=np.float64)
+    if transform.shape != (4, 4) or not np.all(np.isfinite(transform)):
+        raise ValueError("transform must be a finite 4x4 matrix")
+    if not np.allclose(transform[3], [0.0, 0.0, 0.0, 1.0], atol=1e-9):
+        raise ValueError("transform has an invalid homogeneous row")
+    quat = Rotation.from_matrix(transform[:3, :3]).as_quat()
+    return np.concatenate([transform[:3, 3], quat_xyzw_to_wxyz(quat)])
+
+
+class FrameAdapter:
+    """Map poses and point velocities between native and training coordinates.
+
+    Tool rotation changes the axes at the flange centre, never the control point.
+    Geometric twists describe velocity at that point, not at the world origin.
+    """
+
+    def __init__(self, task_from_base_pose7, native_from_tool_pose7):
+        self.task_from_base = pose7_to_matrix(task_from_base_pose7)
+        self.native_from_tool = pose7_to_matrix(native_from_tool_pose7)
+        if np.any(np.abs(self.native_from_tool[:3, 3]) > 1e-12):
+            raise ValueError("tool translation is not supported; control point is the flange centre")
+        self.base_from_task = np.linalg.inv(self.task_from_base)
+        self.tool_from_native = np.linalg.inv(self.native_from_tool)
+
+    def to_task(self, pose7) -> np.ndarray:
+        return matrix_to_pose7(self.task_from_base @ pose7_to_matrix(pose7) @ self.native_from_tool)
+
+    def to_native(self, pose7) -> np.ndarray:
+        return matrix_to_pose7(self.base_from_task @ pose7_to_matrix(pose7) @ self.tool_from_native)
+
+    def twist_to_native(self, twist) -> np.ndarray:
+        values = np.asarray(twist, dtype=float)
+        if values.shape != (6,) or not np.all(np.isfinite(values)):
+            raise ValueError("twist must contain 6 finite values")
+        rotation = self.base_from_task[:3, :3]
+        return np.concatenate([rotation @ values[:3], rotation @ values[3:]])
+
+
+def pose_twists(poses: np.ndarray, dt: float) -> np.ndarray:
+    """World-frame central-difference twists with zero chunk endpoints."""
+    values = np.asarray(poses, dtype=np.float64)
+    if values.ndim != 2 or values.shape[1] != 7 or not np.all(np.isfinite(values)):
+        raise ValueError("poses must be an Nx7 finite array")
+    if not np.isfinite(dt) or dt <= 0.0:
+        raise ValueError("dt must be positive")
+    twists = np.zeros((len(values), 6), dtype=np.float64)
+    if len(values) < 3:
+        return twists
+    twists[1:-1, :3] = (values[2:, :3] - values[:-2, :3]) / (2.0 * dt)
+    rotations = Rotation.from_quat(quat_wxyz_to_xyzw(normalize_quat(values[:, 3:7])))
+    for i in range(1, len(values) - 1):
+        twists[i, 3:] = (rotations[i + 1] * rotations[i - 1].inv()).as_rotvec() / (2.0 * dt)
+    return twists
+
+
+def obs_pose(pose7: np.ndarray) -> np.ndarray:
+    """Our pose7d (wxyz) -> the policy server's pose7d (xyzw)."""
+    p = np.asarray(pose7, dtype=np.float64)
+    return np.concatenate([p[..., 0:3], quat_wxyz_to_xyzw(p[..., 3:7])], axis=-1)
+
+
+def pose7_to_rotvec6(pose7: np.ndarray) -> np.ndarray:
+    """Our pose7d (wxyz) -> UR's ``[x, y, z, rx, ry, rz]``."""
+    p = np.asarray(pose7, dtype=np.float64)
+    if p.shape[-1] != 7:
+        raise ValueError(f"pose7 expects last dim 7, got {p.shape[-1]}")
+    rotvec = Rotation.from_quat(quat_wxyz_to_xyzw(normalize_quat(p[..., 3:7]))).as_rotvec()
+    return np.concatenate([p[..., 0:3], rotvec], axis=-1)
+
+
+def rotvec6_to_pose7(vec6: np.ndarray) -> np.ndarray:
+    """UR's ``[x, y, z, rx, ry, rz]`` -> our pose7d (wxyz)."""
+    v = np.asarray(vec6, dtype=np.float64)
+    if v.shape[-1] != 6:
+        raise ValueError(f"rotvec6 expects last dim 6, got {v.shape[-1]}")
+    quat = Rotation.from_rotvec(v[..., 3:6]).as_quat()  # xyzw
+    return np.concatenate([v[..., 0:3], quat_xyzw_to_wxyz(quat)], axis=-1)
+
+
+def pose_delta(a: np.ndarray, b: np.ndarray) -> tuple[float, float]:
+    """(translation distance in m, rotation angle in rad) between two pose7d."""
+    a = np.asarray(a, dtype=np.float64)
+    b = np.asarray(b, dtype=np.float64)
+    ra = Rotation.from_quat(quat_wxyz_to_xyzw(normalize_quat(a[3:7])))
+    rb = Rotation.from_quat(quat_wxyz_to_xyzw(normalize_quat(b[3:7])))
+    return float(np.linalg.norm(b[0:3] - a[0:3])), float((rb * ra.inv()).magnitude())
+
+
+# -- the agent-facing rotation representation ----------------------------------
+#
+# Internal poses use quaternions. MCP exposes roll/pitch/yaw in degrees.
+#
+# Roll is about fixed base X, pitch about fixed Y, yaw about fixed Z,
+# composed as R = Rz(yaw) @ Ry(pitch) @ Rx(roll). That is scipy's extrinsic "xyz".
+#
+# CAVEAT: near pitch = +/-90 deg roll and yaw become degenerate (gimbal lock) and the values
+# read back are unstable. Relative moves avoid it entirely because they compose rotations
+# rather than adding angles; absolute moves near vertical-pitch do not.
+
+def pose7_to_xyzrpy(pose7: np.ndarray) -> np.ndarray:
+    """pose7d (wxyz) -> ``[x, y, z, roll, pitch, yaw]`` with the angles in degrees."""
+    p = np.asarray(pose7, dtype=np.float64)
+    rpy = Rotation.from_quat(quat_wxyz_to_xyzw(normalize_quat(p[3:7]))).as_euler("xyz", degrees=True)
+    return np.concatenate([p[0:3], rpy])
+
+
+def xyzrpy_to_pose7(xyzrpy: np.ndarray) -> np.ndarray:
+    """``[x, y, z, roll, pitch, yaw]`` (degrees) -> pose7d (wxyz)."""
+    v = np.asarray(xyzrpy, dtype=np.float64)
+    if v.shape[-1] != 6:
+        raise ValueError(f"xyzrpy expects last dim 6, got {v.shape[-1]}")
+    quat = Rotation.from_euler("xyz", v[3:6], degrees=True).as_quat()
+    return np.concatenate([v[0:3], quat_xyzw_to_wxyz(quat)])
+
+
+def rotate_pose(pose7: np.ndarray, rpy_deg) -> np.ndarray:
+    """Apply a base-frame rpy delta to a pose: ``R_new = R_delta @ R_current``.
+
+    Composed, not added. Adding Euler angles is wrong for anything but tiny deltas, and it is
+    what makes naive relative rotation drift.
+    """
+    p = np.asarray(pose7, dtype=np.float64)
+    delta = Rotation.from_euler("xyz", np.asarray(rpy_deg, dtype=np.float64), degrees=True)
+    cur = Rotation.from_quat(quat_wxyz_to_xyzw(normalize_quat(p[3:7])))
+    return np.concatenate([p[0:3], quat_xyzw_to_wxyz((delta * cur).as_quat())])
+
+
+# Peak-to-average speed ratio for smootherstep. Scale duration by this factor
+# to respect the configured peak speed while easing in and out at the endpoints.
+SMOOTHERSTEP_PEAK = 1.875
+
+
+def smootherstep(n: int) -> np.ndarray:
+    """`n` samples of the smootherstep easing over (0, 1].
+
+    Shared by every interpolated track, so the gripper stays in step with the pose instead of
+    following a second, subtly different curve.
+    """
+    tau = np.arange(1, n + 1, dtype=np.float64) / n
+    return tau * tau * tau * (tau * (tau * 6.0 - 15.0) + 10.0)
+
+
+def interpolate_pose(start7: np.ndarray, end7: np.ndarray, n: int) -> np.ndarray:
+    """`n` pose7d samples from start to end: smootherstep in time, lerp + slerp in space."""
+    a = np.asarray(start7, dtype=np.float64)
+    b = np.asarray(end7, dtype=np.float64)
+    s = smootherstep(n)
+    xyz = a[0:3] + np.outer(s, b[0:3] - a[0:3])
+    rots = Rotation.from_quat(
+        np.stack([quat_wxyz_to_xyzw(normalize_quat(a[3:7])), quat_wxyz_to_xyzw(normalize_quat(b[3:7]))])
+    )
+    quats = quat_xyzw_to_wxyz(Slerp([0.0, 1.0], rots)(s).as_quat())
+    return np.concatenate([xyz, quats], axis=1)
+
+
+def target_pose(current7: np.ndarray, x=None, y=None, z=None,
+                roll=None, pitch=None, yaw=None, relative: bool = False) -> np.ndarray:
+    """Build a pose7d target from partially specified agent input.
+
+    Omitted axes hold their current value, which is what makes "lift 5 cm" and "move over the
+    bin" one-liners instead of a full pose the caller has to reconstruct correctly.
+    Relative rotation COMPOSES in the base frame (``R_delta @ R_current``); it does not add
+    Euler angles.
+    """
+    cur = np.asarray(current7, dtype=np.float64)
+    cur_xyzrpy = pose7_to_xyzrpy(cur)
+    if relative:
+        xyz = cur_xyzrpy[0:3] + np.array([x or 0.0, y or 0.0, z or 0.0], dtype=np.float64)
+        rotated = rotate_pose(cur, [roll or 0.0, pitch or 0.0, yaw or 0.0])
+        return np.concatenate([xyz, rotated[3:7]])
+    picked = [
+        cur_xyzrpy[i] if v is None else float(v)
+        for i, v in enumerate((x, y, z, roll, pitch, yaw))
+    ]
+    return xyzrpy_to_pose7(picked)
+
+
+# -- waypoint paths --------------------------------------------------------------
+
+# A leg shorter than this is a repeated point. Below it the arc-length parameterisation would
+# divide by nearly nothing and a gripper change on it would plan an absurd duration.
+MIN_LEG_M = 1e-4
+
+
+class Path(NamedTuple):
+    poses: np.ndarray    # (T, 7) pose7d, one frame per `dt`
+    grips: np.ndarray    # (T,)
+    passed: np.ndarray   # (T,) int: waypoints passed once frame i has been sent
+    blends: np.ndarray   # (n,) corner radius actually used at each waypoint (last is 0)
+    path_m: float
+    planned_s: float
+
+
+def plan_path(start7, targets7, g_start: float, grips, blends, *, speed: float,
+              angular_speed: float, gripper_travel_s: float, dt: float, max_s: float = 120.0) -> Path:
+    """Sparse waypoints -> one dense track at `dt`, the same shape a policy chunk has.
+
+    Geometry: the polyline through the points, with every interior corner replaced by a
+    quadratic Bezier of radius `blend`, clamped to half the shorter adjacent leg (as UR's
+    moveL does). That makes the path C1 and keeps it INSIDE each corner: a curve that passes
+    through a corner point at speed must leave the polyline's hull somewhere, and an
+    interpolating spline leaves it on the outside by an amount set by the neighbours -- on an
+    LLM-authored "descend to the object" that is a dip below the object. A corner with blend 0,
+    and the last point, are stops: the track comes to rest there and starts again. A leg of
+    zero length (a repeated point that changes the gripper) is bracketed by stops by the clamp
+    alone, so a dwell needs no special case.
+
+    Timing: between stops one smootherstep profile runs over the polyline arc length, and the
+    Bezier is parameterised on that same arc length. The velocity is then continuous
+    everywhere and the arm only slows INSIDE a corner (to cos(turn/2) of the profile at the
+    apex), never above the profile. Rotation and gripper are slerp / lerp per leg on the same
+    parameter, so each waypoint's orientation and gripper value are met as it is passed.
+
+    Ponytail: rotation and gripper are slaved to arc length, so one leg that is rotation- or
+    gripper-limited slows the whole stretch between stops, and a short leg carrying a big
+    rotation slows it a lot. `max_s` bounds each planned stretch, independently of policy run
+    duration. It refuses the absurd cases with a hint; the upgrade path is
+    per-leg time allocation with a rate ramp across each blend. Not needed at cm-scale legs.
+    """
+    P = np.vstack([np.asarray(start7, dtype=np.float64)[None, :], np.asarray(targets7, dtype=np.float64)])
+    G = np.r_[float(g_start), np.asarray(grips, dtype=np.float64)]
+    n = len(P) - 1
+    if n < 1:
+        raise ValueError("a path needs at least one waypoint")
+    L = np.linalg.norm(np.diff(P[:, :3], axis=0), axis=1)          # leg j: P[j] -> P[j+1]
+    L[L < MIN_LEG_M] = 0.0
+    dth = np.array([pose_delta(P[j], P[j + 1])[1] for j in range(n)])
+    dg = np.abs(np.diff(G))
+    r = np.zeros(n + 1)                                              # corner radius at point i
+    for i in range(1, n):
+        r[i] = min(float(blends[i - 1]), L[i - 1] / 2.0, L[i] / 2.0)
+    stops = [0] + [i for i in range(1, n + 1) if r[i] == 0.0]
+
+    poses, gr, passed = [], [], []
+    planned = 0.0
+    for a, b in zip(stops[:-1], stops[1:]):                          # one run: points a..b, legs a..b-1
+        legs = slice(a, b)
+        cum = np.r_[0.0, np.cumsum(L[legs])]
+        total = float(cum[-1])
+        if total == 0.0:                                             # a repeated point: dwell
+            T = SMOOTHERSTEP_PEAK * max(dth[a] / angular_speed, dg[a] * gripper_travel_s)
+            m = _frames(T, dt, max_s, a)
+            s = smootherstep(m)
+            pos = interpolate_pose(P[a], P[a + 1], m)
+            g = G[a] + s * (G[a + 1] - G[a])
+            done = np.where(s >= 1.0, b, a)
+        else:
+            nominal = np.maximum.reduce([L[legs] / speed, dth[legs] / angular_speed, dg[legs] * gripper_travel_s])
+            slowest = int(np.argmax(nominal * total / L[legs]))
+            T = SMOOTHERSTEP_PEAK * float(nominal[slowest] * total / L[legs][slowest])
+            m = _frames(T, dt, max_s, a + slowest)
+            sigma = smootherstep(m) * total
+            j = np.clip(np.searchsorted(cum, sigma, side="right") - 1, 0, b - a - 1)
+            frac = (sigma - cum[j]) / L[a + j]
+            xyz = P[a + j, :3] + frac[:, None] * (P[a + j + 1, :3] - P[a + j, :3])
+            for i in range(a + 1, b):                                # interior corners, all blended
+                lo = cum[i - a] - r[i]
+                mask = (sigma >= lo) & (sigma <= cum[i - a] + r[i])
+                if not mask.any():
+                    continue
+                t = ((sigma[mask] - lo) / (2.0 * r[i]))[:, None]
+                A = P[i, :3] - r[i] * (P[i, :3] - P[i - 1, :3]) / L[i - 1]
+                B = P[i, :3] + r[i] * (P[i + 1, :3] - P[i, :3]) / L[i]
+                xyz[mask] = (1 - t) ** 2 * A + 2 * (1 - t) * t * P[i, :3] + t**2 * B
+            rots = Rotation.from_quat(quat_wxyz_to_xyzw(normalize_quat(P[a:b + 1, 3:7])))
+            quat = quat_xyzw_to_wxyz(Slerp(cum, rots)(sigma).as_quat())
+            pos = np.concatenate([xyz, quat], axis=1)
+            g = np.interp(sigma, cum, G[a:b + 1])
+            done = a + np.searchsorted(cum[1:], sigma, side="right")
+        poses.append(pos)
+        gr.append(g)
+        passed.append(done)
+        planned += T
+    return Path(np.concatenate(poses), np.concatenate(gr), np.concatenate(passed).astype(int),
+                r[1:], float(L.sum()), planned)
+
+
+def _frames(T: float, dt: float, max_s: float, leg: int) -> int:
+    if T > max_s:
+        raise ValueError(
+            f"the stretch around waypoint {leg + 1} plans {T:.0f} s, over the {max_s:.0f} s ceiling: a "
+            f"short leg carrying a large rotation or gripper change slows everything between "
+            f"stops. Give that waypoint blend=0, or its own repeated point."
+        )
+    return max(1, int(np.ceil(T / dt)))

roboapi/hardware/__init__.py

@@ -0,0 +1,5 @@
+"""Hardware interfaces and composable device adapters."""
+
+from .base import Arm, FramedArm, Gripper
+
+__all__ = ["Arm", "FramedArm", "Gripper"]