import numpy as np import pytest from safety_monitor import SafetyConfig, SafetyMonitor, Status, Watchdog, planar_2link_fk DT = 0.02 def make() -> SafetyMonitor: 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=DT, watchdog_timeout=0.1) return SafetyMonitor(cfg, planar_2link_fk) Q0 = np.array([0.0, 0.5]) def test_passes_a_safe_command_unchanged() -> None: mon = make() out, st = mon.step(0.0, Q0, np.array([0.01, 0.51])) assert st is Status.OK and np.allclose(out, [0.01, 0.51]) def test_speed_limit_clamps_each_step() -> None: mon = make() out, st = mon.step(0.0, Q0, np.array([0.5, 0.5])) assert st is Status.CLAMPED assert out[0] == pytest.approx(2.0 * DT) def test_position_limit_clamps() -> None: mon = make() q = np.array([-0.5, 1.98]) out, st = mon.step(0.0, q, np.array([-0.5, 2.1])) assert st is Status.CLAMPED and out[1] == pytest.approx(2.0) def test_workspace_violation_stops_and_latches() -> None: mon = make() q = Q0.copy() statuses = [] for k in range(10): q, st = mon.step(k * DT, q, np.array([1.0, 0.5])) statuses.append(st) assert Status.STOP_WORKSPACE in statuses first = statuses.index(Status.STOP_WORKSPACE) assert all(s is Status.STOP_WORKSPACE for s in statuses[first:]) # latched assert planar_2link_fk(q)[1] <= 0.10 # never left the box mon.reset() out, st = mon.step(1.0, q, q - np.array([0.02, 0.0])) # move back inside assert st is Status.OK def test_invalid_commands_stop() -> None: for bad in (np.array([np.nan, 0.5]), np.array([0.0, np.inf]), np.array([0.0, 0.5, 0.1])): mon = make() out, st = mon.step(0.0, Q0, bad) assert st is Status.STOP_INVALID and np.allclose(out, Q0) def test_watchdog_stops_when_commands_stop() -> None: mon = make() mon.step(0.0, Q0, Q0) out, st = mon.step(0.05, Q0, None) # within the timeout: hold the last command assert st is Status.OK and np.allclose(out, Q0) out, st = mon.step(0.15, Q0, None) # 0.15 s since the last command > 0.1 s assert st is Status.STOP_WATCHDOG out, st = mon.step(0.17, Q0, Q0) # a late command does not clear the stop assert st is Status.STOP_WATCHDOG def test_no_motion_before_first_command() -> None: mon = make() out, st = mon.step(0.0, Q0, None) assert st is Status.STOP_WATCHDOG def test_watchdog_unit() -> None: wd = Watchdog(0.1) assert wd.expired(0.0) wd.feed(1.0) assert not wd.expired(1.05) and wd.expired(1.2)