roboapi 0.4.2__py3-none-any.whl

roboapi/hardware/franka/arm.py

@@ -0,0 +1,476 @@
+"""Franka: native base/flange poses -> feedback-leashed IK -> joint impedance."""
+
+from __future__ import annotations
+
+import collections
+import logging
+import os
+import threading
+import time
+from contextlib import contextmanager
+from dataclasses import dataclass, replace
+from pathlib import Path
+
+import numpy as np
+
+from ... import geometry as geo
+from ...errors import ControlNotReady
+from .desk import DeskLeaseError, _DeskLease, apply_tool_configuration, create_desk
+from .ik import solve_feedback_leashed_ik
+from .limits import load_limits
+from .settings import COLLISION_FORCE, COLLISION_TORQUE, IKConfig
+
+logger = logging.getLogger(__name__)
+
+NUM_JOINTS = 7
+
+
+@contextmanager
+def _realtime_control_start(realtime_config: str, *, inherit: bool = True):
+    """Let each new native control thread inherit FIFO, then restore its caller.
+
+    Tracking and stop motions create native threads through move().
+    Elevate only during asynchronous startup; joining and Python work run under
+    the caller's original scheduler. Robot construction can itself elevate its
+    caller, so it uses this scope only to restore that original scheduler.
+    """
+    if realtime_config != "enforce":
+        yield
+        return
+    policy = os.sched_getscheduler(0)
+    param = os.sched_getparam(0)
+    priority = os.sched_get_priority_max(os.SCHED_FIFO)
+    try:
+        if inherit:
+            os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(priority))
+            if (os.sched_getscheduler(0) != os.SCHED_FIFO
+                    or os.sched_getparam(0).sched_priority != priority):
+                raise RuntimeError("Franka control startup requires SCHED_FIFO at maximum priority")
+        yield
+    finally:
+        os.sched_setscheduler(0, policy, param)
+        if (os.sched_getscheduler(0) != policy
+                or os.sched_getparam(0).sched_priority != param.sched_priority):
+            raise RuntimeError("Failed to restore Franka control caller's scheduling")
+
+
+class _JointSession:
+    """The small part of franky's JointImpedanceTracker needed here."""
+
+    def __init__(self, robot, franky, cfg) -> None:
+        self.robot = robot
+        self.franky = franky
+        self._realtime_config = cfg.realtime_config
+        gains = cfg.joint_impedance
+        self.motion = franky.JointImpedanceTrackingMotion(
+            stiffness=np.asarray(gains.stiffness),
+            damping=np.asarray(gains.damping),
+            cartesian_stiffness=np.asarray(gains.cartesian_stiffness),
+            cartesian_damping=np.asarray(gains.cartesian_damping),
+            max_delta_tau=gains.max_delta_tau,
+            gains_time_constant=gains.gains_time_constant,
+        )
+
+    def start(self, q: np.ndarray) -> None:
+        self.set_target(q, np.zeros(NUM_JOINTS))
+        with _realtime_control_start(self._realtime_config):
+            self.robot.move(
+                self.motion,
+                asynchronous=True,
+                limit_rate=True,
+                cutoff_frequency=100.0,
+            )
+
+    def set_target(self, q: np.ndarray, dq: np.ndarray) -> None:
+        self.motion.set_reference(self.franky.JointReference(q=q, dq=dq))
+
+    def stop(self) -> None:
+        if bool(self.robot.is_in_control):
+            motion = self.franky.TorqueStopMotion()
+            with _realtime_control_start(self._realtime_config):
+                self.robot.move(motion, asynchronous=True)
+        try:
+            self.robot.join_motion()
+        except Exception as exc:
+            # Defensive parity with franky's tracker wrapper: self-preemption means the
+            # torque loop did stop; every other ControlException remains actionable.
+            if not isinstance(exc, getattr(self.franky, "ControlException", ())) or (
+                "Move command preempted!" not in str(exc)
+            ):
+                raise
+
+
+@dataclass(frozen=True)
+class FrankaSnapshot:
+    sample_time: float
+    received_at: float
+    q: np.ndarray
+    dq: np.ndarray
+    flange_pose_wxyz: np.ndarray
+    jacobian: np.ndarray
+
+
+class FrankaArm:
+    """Owns Desk, one Franka connection, and one 50 Hz Hybrid worker."""
+
+    def __init__(self, cfg) -> None:
+        self.cfg = cfg
+        self._franky = None
+        self._robot = None
+        self._model = None
+        self._lease: _DeskLease | None = None
+        self._session: _JointSession | None = None
+        self._thread: threading.Thread | None = None
+        self._stop = threading.Event()
+        self._started = threading.Event()
+        self._fault: str | None = None
+        self.tool_load_ready = False
+        self._target: tuple[np.ndarray, np.ndarray] | None = None
+        self._snapshot: FrankaSnapshot | None = None
+        self._snapshot_lock = threading.Lock()
+        self._trace = collections.deque(maxlen=cfg.trace_capacity)
+        self._trace_lock = threading.Lock()
+        control_period = 1.0 / float(cfg.hybrid_hz)
+        self._limits = load_limits(cfg.robot_model, None)
+        self._ik = IKConfig(
+            nominal_dt=control_period,
+            max_rotation_step=float(cfg.cartesian_angular_speed) * control_period,
+            posture_gain=0.20,
+            posture_max_velocity=0.10,
+        )
+        self._configure_limits()
+
+    def _configure_limits(self) -> None:
+        self._lower = np.asarray(self._limits.joint_lower)
+        self._upper = np.asarray(self._limits.joint_upper)
+        self._ik = replace(
+            self._ik,
+            max_joint_velocity=self._limits.joint_velocity,
+            max_joint_acceleration=self._limits.joint_acceleration,
+            max_translation_velocity=self._limits.cartesian_velocity[0],
+            max_rotation_velocity=self._limits.cartesian_velocity[1],
+        )
+
+    @property
+    def live(self) -> bool:
+        return (
+            self._robot is not None
+            and self._fault is None
+            and self._thread is not None
+            and self._thread.is_alive()
+        )
+
+    @property
+    def last_error(self) -> str | None:
+        return self._fault
+
+    def connect(self) -> None:
+        self._connect(recover_from_errors=False)
+
+    def _acquire_desk(self) -> None:
+        if self._robot is not None:
+            raise RuntimeError("Franka is already connected")
+        self.tool_load_ready = False
+        if self._lease is not None:
+            # A previous failed connect may have left an authenticated lease awaiting
+            # release. Never overwrite that handle with a new one.
+            self._lease.release()
+            self._lease = None
+        import franky
+
+        password_path = Path(self.cfg.desk_password_file).expanduser()
+        password = password_path.read_text().strip()
+        if not password:
+            raise RuntimeError(f"Franka Desk password file is empty: {password_path}")
+        self._franky = franky
+        self._fault = None
+        self._lease = _DeskLease(create_desk(self.cfg, password))
+
+        def prepare(desk):
+            try:
+                apply_tool_configuration(self.cfg, desk)
+            except ControlNotReady:
+                raise
+            except Exception as exc:
+                raise RuntimeError(f"Franka end-effector initialization failed: {exc}") from exc
+            self.tool_load_ready = True
+
+        try:
+            self._lease.acquire(prepare=prepare)
+        except ControlNotReady:
+            raise
+        except Exception as exc:
+            raise RuntimeError(f"Franka Desk preparation failed: {exc}") from exc
+
+    def _connect(self, *, recover_from_errors: bool) -> None:
+        if self._robot is not None:
+            raise RuntimeError("Franka is already connected")
+        try:
+            self._acquire_desk()
+            franky = self._franky
+            realtime = (
+                franky.RealtimeConfig.Enforce
+                if self.cfg.realtime_config == "enforce"
+                else franky.RealtimeConfig.Ignore
+            )
+            with _realtime_control_start(self.cfg.realtime_config, inherit=False):
+                self._robot = franky.Robot(self.cfg.host, realtime_config=realtime)
+            self._limits = load_limits(
+                self.cfg.robot_model, getattr(self._robot, "model_urdf", None),
+            )
+            self._configure_limits()
+            if recover_from_errors:
+                self._robot.recover_from_errors()
+            # This synchronous SDK call must succeed before any control worker starts.
+            self._robot.set_collision_behavior(COLLISION_TORQUE, COLLISION_FORCE)
+            logger.info(
+                "Franka collision thresholds applied: joints=%s Nm; Cartesian=%s (N,N,N,Nm,Nm,Nm)",
+                COLLISION_TORQUE, COLLISION_FORCE,
+            )
+            self._model = self._robot.model
+            snapshot = self._read_snapshot()
+            self._ik = replace(self._ik, posture_reference=tuple(snapshot.q))
+            self._cache_snapshot(snapshot)
+            self._start_worker()
+        except Exception:
+            try:
+                self._teardown()
+            except Exception as cleanup_exc:
+                logger.error("Franka connect cleanup failed; preserving original error: %s", cleanup_exc)
+            raise
+
+    def disconnect(self) -> None:
+        self._teardown()
+
+    def _teardown(self) -> None:
+        self._stop_worker()
+        if self._robot is not None and bool(self._robot.is_in_control):
+            self._latch_fault("Franka control session is still active; Desk token was not released")
+            raise RuntimeError(self._fault)
+        self._model = None
+        self._robot = None
+        lease = self._lease
+        if lease is not None:
+            lease.release()
+            self._lease = None
+
+    def _start_worker(self) -> None:
+        self._target = None
+        self._stop.clear()
+        self._started.clear()
+        self._thread = threading.Thread(target=self._worker, name="franka-hybrid", daemon=True)
+        self._thread.start()
+        if not self._started.wait(2.0):
+            raise RuntimeError("Franka Hybrid worker did not start")
+        if self._fault is not None:
+            raise RuntimeError(self._fault)
+
+    def _stop_worker(self) -> None:
+        self._stop.set()
+        thread = self._thread
+        if thread is not None:
+            thread.join(timeout=4.0)
+            if thread.is_alive():
+                self._latch_fault("Franka Hybrid worker did not stop")
+                raise RuntimeError(self._fault)
+        self._thread = None
+        self._target = None
+
+    def _latch_fault(self, message: str) -> None:
+        if self._fault is None:
+            self._fault = message
+
+    def wait_for_fault(self, timeout: float = 4.0) -> str | None:
+        """Collect a late native exception without delaying fault detection or polling."""
+        thread = self._thread
+        if self._fault is not None and thread is not None and thread is not threading.current_thread():
+            thread.join(timeout=timeout)
+            if thread.is_alive():
+                logger.warning("Franka fault cleanup still pending after %.1f s: %s", timeout, self._fault)
+        return self._fault
+
+    def _read_snapshot(self) -> FrankaSnapshot:
+        state = self._robot.state
+        now = time.monotonic()
+        flange = self._model.pose(self._franky.Frame.Flange, state)
+        flange_matrix = np.asarray(flange.matrix, dtype=np.float64)
+        return FrankaSnapshot(
+            sample_time=float(state.time.to_sec()),
+            received_at=now,
+            q=np.asarray(state.q, dtype=np.float64).copy(),
+            dq=np.asarray(state.dq, dtype=np.float64).copy(),
+            flange_pose_wxyz=geo.matrix_to_pose7(flange_matrix),
+            jacobian=np.asarray(
+                self._model.zero_jacobian(self._franky.Frame.Flange, state), dtype=np.float64
+            ).copy(),
+        )
+
+    def _check_hardware(self) -> None:
+        if bool(self._robot.has_errors):
+            raise RuntimeError("Franka reflex or robot error is active")
+        if not bool(self._robot.is_in_control):
+            raise RuntimeError("joint impedance tracker terminated unexpectedly")
+
+    def _check_freshness(
+        self, snapshot: FrankaSnapshot, last_sample: float | None, last_received: float
+    ) -> tuple[float, float, float]:
+        """Track repeated samples for diagnostics; native SDK faults end control."""
+        if last_sample is not None and snapshot.sample_time < last_sample - 1e-9:
+            raise RuntimeError("Franka state clock moved backwards")
+        if last_sample is None or snapshot.sample_time > last_sample + 1e-9:
+            last_sample = snapshot.sample_time
+            last_received = snapshot.received_at
+        sample_age = snapshot.received_at - last_received
+        return last_sample, last_received, sample_age
+
+    def _cache_snapshot(self, snapshot: FrankaSnapshot) -> None:
+        with self._snapshot_lock:
+            self._snapshot = snapshot
+
+    def _worker(self) -> None:
+        period = 1.0 / float(self.cfg.hybrid_hz)
+        previous_dq = np.zeros(NUM_JOINTS)
+        last_sample = None
+        sample_received_at = time.monotonic()
+        deadline = time.monotonic()
+        try:
+            self._lease.check()
+            snapshot = self._read_snapshot()
+            # A worker always starts from a measured, discontinuity-free reference. Keep
+            # that reference fixed until a Cartesian target replaces it. Updating the joint
+            # reference from every subsequent measurement would turn the nominal idle hold
+            # into unintended free guiding: an external push would move both the arm and its
+            # reference, leaving no restoring error.
+            idle_hold_q = snapshot.q.copy()
+            idle_hold_pose = snapshot.flange_pose_wxyz.copy()
+            self._session = _JointSession(self._robot, self._franky, self.cfg)
+            self._session.start(idle_hold_q)
+            self._started.set()
+            while not self._stop.is_set():
+                deadline += period
+                remaining = deadline - time.monotonic()
+                if remaining > 0:
+                    self._stop.wait(remaining)
+                if self._stop.is_set():
+                    break
+                self._lease.check()
+                self._check_hardware()
+                snapshot = self._read_snapshot()
+                last_sample, sample_received_at, sample_age = self._check_freshness(
+                    snapshot, last_sample, sample_received_at
+                )
+                self._cache_snapshot(snapshot)
+
+                target = self._target
+                hold_reason = None
+                if target is None:
+                    target_pose = idle_hold_pose
+                    qd, dqd = idle_hold_q, np.zeros(NUM_JOINTS)
+                    pose_error = np.zeros(6)
+                    margin = float(np.min(np.minimum(snapshot.q - self._lower, self._upper - snapshot.q)))
+                    task_scale, velocity_limited, posture_limited = 0.0, False, False
+                    hold_reason = "no_target"
+                else:
+                    target_pose, target_twist = target
+                    solved = solve_feedback_leashed_ik(
+                        q=snapshot.q,
+                        current_pose7=geo.obs_pose(snapshot.flange_pose_wxyz),
+                        target_pose7=geo.obs_pose(target_pose),
+                        target_twist=target_twist,
+                        jacobian=snapshot.jacobian,
+                        lower_limits=self._lower,
+                        upper_limits=self._upper,
+                        dt=period,
+                        config=self._ik,
+                        previous_dq=previous_dq,
+                        joint_velocity_bounds=self._limits.velocity_bounds(snapshot.q),
+                    )
+                    qd, dqd = solved.qd, solved.dq
+                    pose_error = solved.pose_error
+                    margin = solved.minimum_joint_margin
+                    task_scale = solved.task_scale
+                    velocity_limited = solved.joint_velocity_limited
+                    posture_limited = solved.posture_limited
+                self._session.set_target(qd, dqd)
+                previous_dq = np.asarray(dqd).copy()
+                self._append_trace(
+                    snapshot, target_pose, qd, dqd, pose_error, margin, task_scale,
+                    velocity_limited, posture_limited, hold_reason, sample_age,
+                )
+        except Exception as exc:
+            logger.exception("Franka Hybrid worker failed")
+            self._latch_fault(str(exc))
+            self._started.set()
+        finally:
+            session, self._session = self._session, None
+            if session is not None:
+                try:
+                    session.stop()
+                except Exception as exc:
+                    logger.exception("Franka joint impedance tracker failed during shutdown")
+                    # The SDK may only expose its original control exception at join,
+                    # after its cached flags have only reported a generic worker fault.
+                    message = f"{type(exc).__name__}: {exc}"
+                    if self._fault is not None:
+                        message += f"\nEarlier worker fault: {self._fault}"
+                    self._fault = message
+
+    def _append_trace(
+        self, snapshot, target, qd, dqd, pose_error, margin, scale,
+        velocity_limited, posture_limited, hold_reason, sample_age,
+    ) -> None:
+        item = {
+            "t_monotonic": snapshot.received_at,
+            "sample_ms": round(sample_age * 1e3, 3),
+            "target_pose": np.asarray(target).tolist(),
+            "measured_pose": snapshot.flange_pose_wxyz.tolist(),
+            "native_frame": "franka_base",
+            "q": snapshot.q.tolist(),
+            "dq": snapshot.dq.tolist(),
+            "qd": np.asarray(qd).tolist(),
+            "dqd": np.asarray(dqd).tolist(),
+            "pose_error": np.asarray(pose_error).tolist(),
+            "minimum_joint_margin": float(margin),
+            "task_scale": float(scale),
+            "joint_velocity_limited": bool(velocity_limited),
+            "posture_limited": bool(posture_limited),
+            "ik_hold_reason": hold_reason,
+        }
+        with self._trace_lock:
+            self._trace.append(item)
+
+    def drain_trace(self) -> list[dict]:
+        with self._trace_lock:
+            trace = list(self._trace)
+            self._trace.clear()
+        return trace
+
+    def get_joints(self) -> np.ndarray:
+        with self._snapshot_lock:
+            if self._snapshot is None:
+                raise RuntimeError("Franka is not connected")
+            return self._snapshot.q.copy()
+
+    def get_eef_pose(self) -> np.ndarray:
+        with self._snapshot_lock:
+            if self._snapshot is None:
+                raise RuntimeError("Franka is not connected")
+            return self._snapshot.flange_pose_wxyz.copy()
+
+    def servo_pose(self, pose7: np.ndarray, twist: np.ndarray | None = None, *, position_only=False) -> None:
+        del position_only  # The shared Cartesian targets use joint impedance without added admittance.
+        pose = np.asarray(pose7, dtype=np.float64)
+        velocity = np.zeros(6) if twist is None else np.asarray(twist, dtype=np.float64)
+        if pose.shape != (7,) or not np.all(np.isfinite(pose)):
+            raise ValueError("servo_pose expects 7 finite pose values")
+        if velocity.shape != (6,) or not np.all(np.isfinite(velocity)):
+            raise ValueError("servo_pose expects 6 finite twist values")
+        if not self.live:
+            raise RuntimeError(f"not_alive: {self.last_error}. Call recover().")
+        pose = np.concatenate([pose[:3], geo.normalize_quat(pose[3:])])
+        self._target = (pose, velocity.copy())
+
+    def recover(self) -> None:
+        """A hardware fault is cleared only by this explicit reconnect."""
+        self._teardown()
+        self._connect(recover_from_errors=True)