1import json, math
  2from pathlib import Path
  3import numpy as np
  4
  5# Discrete simulation of xdot = u + d, with safe set h(x)=1-x >= 0.
  6# The learned/reference model omits d: xhat_next=x+dt*u.
  7# For scalar u, the CBF-QP has the closed-form projection used below.
  8
  9def filter_action(x, u_nom, ebar, *, dt=.1, alpha=1., kappa=1., umin=-2., umax=2.):
 10    h = 1.0 - x
 11    # hdot_model + alpha(h-kappa*ebar) - epsilon >= 0,
 12    # epsilon is the projected disturbance margin kappa*ebar/dt.
 13    upper = alpha * (h - kappa * ebar) - kappa * ebar / dt
 14    u = float(np.clip(min(u_nom, upper), umin, umax))
 15    return u, max(0., u_nom-u), upper
 16
 17def smoothing_crossing(rho, target, raw, max_steps=10000):
 18    """First t (1-indexed) at which ebar >= target for constant raw error."""
 19    e = 0.
 20    for t in range(1, max_steps + 1):
 21        e = rho*e + (1-rho)*raw
 22        if e >= target - 1e-12:
 23            return t
 24    return None
 25
 26def run_episode(kappa, rho=.8, d=.35, filtered=True, seed=0, steps=100):
 27    rng = np.random.default_rng(seed)
 28    x, ebar = 0., 0.
 29    violations, interventions, max_x = 0, 0., x
 30    crossed = None
 31    for t in range(steps):
 32        # A fixed neural-policy-like head drives toward the upper boundary.
 33        u_nom = .85 + .03*rng.normal()
 34        if filtered:
 35            u, intervention, _ = filter_action(x, u_nom, ebar, kappa=kappa)
 36        else:
 37            u, intervention = float(np.clip(u_nom, -2, 2)), 0.
 38        xhat = x + .1*u
 39        # Constant unknown velocity disturbance, observed after applying u.
 40        x = x + .1*(u + d)
 41        ebar = rho*ebar + (1-rho)*abs(x-xhat)
 42        if crossed is None and kappa*ebar/.1 >= d:
 43            crossed = t+1
 44        violations += int(x > 1.0 + 1e-10)
 45        interventions += intervention
 46        max_x = max(max_x, x)
 47    return dict(violations=violations, intervention=interventions, max_x=max_x,
 48                final_ebar=ebar, crossing=crossed)
 49
 50def main():
 51    out = {}
 52    # Core math sanity: if epsilon bounds projected disturbance, robust residual is nonnegative.
 53    rng = np.random.default_rng(11)
 54    residuals = []
 55    for _ in range(10000):
 56        u = rng.uniform(-1, 1); d = rng.uniform(-.4, .4); h = rng.uniform(0, 1)
 57        eps = abs(d) + 1e-9
 58        # hdot=-u-d; model residual minus eps is a lower bound on true residual.
 59        model_res = -u + h - eps
 60        true_res = -u - d + h
 61        residuals.append(true_res - model_res)
 62    out['math_check'] = {'min_true_minus_model_lower_bound': float(min(residuals)),
 63                         'statement': 'true residual >= model residual when epsilon >= |d|'}
 64
 65    # Prediction 1: protection turns on when kappa*ebar/dt reaches d.
 66    # Constant transition error is dt*d, so predicted crossing is
 67    # ceil(log(1-d*dt/(kappa*dt))/log(rho)) = ceil(log(1-1/kappa)/log(rho)) for kappa>1.
 68    rows = []
 69    for k in [0.8, 1., 1.2, 1.5, 2., 3.]:
 70        pred = None if k <= 1 else math.ceil(math.log(1-1/k)/math.log(.8))
 71        r = run_episode(k, rho=.8, d=.35, seed=0)
 72        rows.append({'kappa': k, 'predicted_margin_crossing_step': pred,
 73                     'observed_crossing_step': r['crossing'], 'violations': r['violations']})
 74    out['margin_threshold_sweep'] = rows
 75
 76    # Prediction 2: ebar reaches a fixed fraction q of its asymptote with the
 77    # exponential law t=ceil(log(1-q)/log(rho)); verify several rho values.
 78    smooth = []
 79    for rho in [.5, .8, .9, .95]:
 80        q=.8; pred=math.ceil(math.log(1-q)/math.log(rho))
 81        obs=smoothing_crossing(rho, q*.1*.35, .1*.35)
 82        smooth.append({'rho':rho, 'target_fraction':q, 'predicted_step':pred, 'observed_step':obs})
 83    out['smoothing_law_sweep'] = smooth
 84
 85    # Prediction 3: larger kappa monotonically increases intervention and decreases violations.
 86    pareto=[]
 87    for k in [0., .5, 1., 1.5, 2., 3., 4.]:
 88        vals=[run_episode(k, rho=.8, d=.35, seed=s) for s in range(8)]
 89        pareto.append({'kappa':k, 'mean_violations':float(np.mean([v['violations'] for v in vals])),
 90                       'mean_intervention':float(np.mean([v['intervention'] for v in vals])),
 91                       'mean_max_x':float(np.mean([v['max_x'] for v in vals]))})
 92    base=[run_episode(0, filtered=False, d=.35, seed=s) for s in range(8)]
 93    out['intervention_violation_sweep'] = pareto
 94    out['baseline_static_clipping'] = {'mean_violations':float(np.mean([v['violations'] for v in base])),
 95                                        'mean_max_x':float(np.mean([v['max_x'] for v in base]))}
 96    # Explicit mechanism verdict checks (allow one step numerical tolerance).
 97    out['checks'] = {
 98      'smoothing_matches_exact_recursion': all(abs(x['predicted_step']-x['observed_step'])<=1 for x in smooth),
 99      'intervention_monotone': all(pareto[i+1]['mean_intervention']+1e-9 >= pareto[i]['mean_intervention'] for i in range(len(pareto)-1)),
100      'violations_nonincreasing': all(pareto[i+1]['mean_violations'] <= pareto[i]['mean_violations']+1e-9 for i in range(len(pareto)-1)),
101      'adaptive_beats_clipping_at_kappa_2': pareto[4]['mean_violations'] < out['baseline_static_clipping']['mean_violations']
102    }
103    Path('results.json').write_text(json.dumps(out, indent=2))
104    print(json.dumps(out, indent=2))
105
106if __name__ == '__main__': main()