"""A software safety filter between a learned policy and the servos. Every control period the monitor receives the measured joint positions and the policy's commanded positions (or None if no command arrived). It 1. stops if the watchdog has expired (no command for longer than the timeout); 2. stops if the command is malformed (wrong shape, NaN or infinity); 3. clamps each joint to its position limits; 4. limits each joint's step to v_max * dt (a speed limit); 5. stops if the forward-kinematics tool position leaves the workspace box. A stop is latched: the arm holds its measured position until reset() is called by an operator. This filter is a software layer. It does not replace an independent hardware safety function such as an e-stop circuit. """ from __future__ import annotations from dataclasses import dataclass, field from enum import Enum from typing import Callable import numpy as np class Status(Enum): OK = "ok" CLAMPED = "clamped" # command changed to stay within position or speed limits STOP_WATCHDOG = "stop: watchdog" STOP_INVALID = "stop: invalid command" STOP_WORKSPACE = "stop: workspace" @dataclass class Watchdog: timeout: float # seconds last_feed: float | None = None def feed(self, t: float) -> None: self.last_feed = t def expired(self, t: float) -> bool: # A watchdog that was never fed counts as expired: no motion before the first command. return self.last_feed is None or (t - self.last_feed) > self.timeout @dataclass class SafetyConfig: q_min: np.ndarray # rad, per joint q_max: np.ndarray # rad, per joint v_max: np.ndarray # rad/s, per joint box_min: np.ndarray # m, workspace lower corner (same length as fk output) box_max: np.ndarray # m, workspace upper corner dt: float # s, control period watchdog_timeout: float # s @dataclass class SafetyMonitor: config: SafetyConfig fk: Callable[[np.ndarray], np.ndarray] watchdog: Watchdog = field(init=False) last_cmd: np.ndarray | None = field(default=None, init=False) stopped: Status | None = field(default=None, init=False) def __post_init__(self) -> None: self.watchdog = Watchdog(self.config.watchdog_timeout) def reset(self) -> None: """Operator acknowledgement: clear a latched stop.""" self.stopped = None self.last_cmd = None def _stop(self, status: Status, q_meas: np.ndarray) -> tuple[np.ndarray, Status]: self.stopped = status self.last_cmd = q_meas.copy() return q_meas.copy(), status def step(self, t: float, q_meas: np.ndarray, cmd: np.ndarray | None) -> tuple[np.ndarray, Status]: c = self.config q_meas = np.asarray(q_meas, dtype=float) if cmd is not None: self.watchdog.feed(t) if self.stopped is not None: return q_meas.copy(), self.stopped if self.watchdog.expired(t): return self._stop(Status.STOP_WATCHDOG, q_meas) if cmd is None: # Within the timeout: repeat the last safe command. ref = self.last_cmd if self.last_cmd is not None else q_meas return ref.copy(), Status.OK cmd = np.asarray(cmd, dtype=float) if cmd.shape != q_meas.shape or not np.all(np.isfinite(cmd)): return self._stop(Status.STOP_INVALID, q_meas) out = np.clip(cmd, c.q_min, c.q_max) ref = self.last_cmd if self.last_cmd is not None else q_meas max_step = c.v_max * c.dt out = np.clip(out, ref - max_step, ref + max_step) p = np.asarray(self.fk(out), dtype=float) if np.any(p < c.box_min) or np.any(p > c.box_max): return self._stop(Status.STOP_WORKSPACE, q_meas) self.last_cmd = out status = Status.OK if np.allclose(out, cmd, rtol=0.0, atol=1e-12) else Status.CLAMPED return out.copy(), status def planar_2link_fk(q: np.ndarray, l1: float = 0.12, l2: float = 0.12) -> np.ndarray: """Tool (x, y) of a planar two-link arm; example link lengths in metres.""" return np.array([l1 * np.cos(q[0]) + l2 * np.cos(q[0] + q[1]), l1 * np.sin(q[0]) + l2 * np.sin(q[0] + q[1])]) def _run(mon: SafetyMonitor, q: np.ndarray, commands: list[np.ndarray | None], dt: float) -> None: for k, cmd in enumerate(commands): t = k * dt out, st = mon.step(t, q, cmd) cmd_s = "none" if cmd is None else np.array2string(cmd, precision=2) print(f"{t:4.2f} {cmd_s:12} {np.array2string(out, precision=2):12} {st.value}") q = out # assume the servos track the command exactly if __name__ == "__main__": # Example limits, not taken from a real arm. cfg = SafetyConfig(q_min=np.array([-1.5, -2.0]), q_max=np.array([1.5, 2.0]), v_max=np.array([2.0, 2.0]), box_min=np.array([0.02, -0.20]), box_max=np.array([0.24, 0.10]), dt=0.02, watchdog_timeout=0.1) print("Scenario 1: limits and workspace") print("t command output status") _run(SafetyMonitor(cfg, planar_2link_fk), np.array([0.0, 0.5]), [np.array([0.0, 0.5]), np.array([0.3, 0.5]), np.array([0.1, 3.0])] + [np.array([1.0, 0.5])] * 4, cfg.dt) print("Scenario 2: commands stop arriving") print("t command output status") _run(SafetyMonitor(cfg, planar_2link_fk), np.array([0.0, 0.5]), [np.array([0.0, 0.5])] + [None] * 7, cfg.dt)