1import json, math, random
 2from pathlib import Path
 3import numpy as np
 4
 5# Robust HOCBF for p_dot=v, v_dot=u+w, h>=0, with linear alpha_i(s)=k_i s.
 6# For lower wall p>=pmin: u >= -what + eps -(k1+k2)v-k1*k2*(p-pmin)
 7# For upper wall p<=pmax: u <= -what - eps -(k1+k2)v+k1*k2*(pmax-p)
 8
 9def interval(p, v, pmin, pmax, umax, what=0.0, eps=0.0, k1=2.0, k2=2.0):
10    lo = -what + eps - (k1+k2)*v - k1*k2*(p-pmin)
11    hi = -what - eps - (k1+k2)*v + k1*k2*(pmax-p)
12    return max(-umax, lo), min(umax, hi)
13
14def project(u, lo, hi):
15    # Emergency action is the closest actuator endpoint if uncertainty makes I empty.
16    return float(np.clip(u, lo, hi)) if lo <= hi else float(np.clip(u, [-1, 1][int(u < 0)], [1, -1][int(u < 0)]))
17
18def nominal_policy(p, v, target=0.0):
19    # Small neural-policy surrogate: bounded PD policy with deliberately aggressive targets.
20    return float(np.clip(-2.5*(p-target)-1.3*v, -1.0, 1.0))
21
22def exact_margin(p, v, **kw):
23    lo, hi = interval(p, v, **kw)
24    return hi-lo
25
26def math_sweeps():
27    # Use a wide actuator only for algebraic verification, avoiding saturation masking.
28    pars=dict(pmin=-1.0,pmax=1.0,umax=10.0,k1=2.0,k2=2.0)
29    # Prediction 1: M(eps)=M(0)-2 eps, until actuator clipping changes the slope.
30    p,v=0.25,0.35
31    m0=exact_margin(p,v,what=0,eps=0,**pars)
32    eps_grid=np.linspace(0,2.4,13)
33    margins=np.array([exact_margin(p,v,what=0,eps=e,**pars) for e in eps_grid])
34    # Use only nonempty/non-clipped points to estimate slope; report all values.
35    slope=float(np.polyfit(eps_grid[margins>0],margins[margins>0],1)[0])
36    # Prediction 2: symmetric disturbance uncertainty becomes infeasible at eps=M0/2.
37    epscrit=m0/2
38    # Directly estimate the first epsilon at which the interval is empty.
39    dense=np.linspace(0, 5.0, 5001)
40    dense_m=np.array([exact_margin(p,v,what=0,eps=e,**pars) for e in dense])
41    first_empty=dense[np.flatnonzero(dense_m < 0)[0]] if np.any(dense_m < 0) else None
42    # Prediction 3: with no actuator clipping, uncertainty shifts each bound by eps.
43    p2,v2=.0,0.
44    l0,h0=interval(p2,v2,**pars,what=0,eps=0)
45    shifts=[]
46    for e in [.1,.2,.4]:
47        l,h=interval(p2,v2,**pars,what=0,eps=e)
48        shifts.append((e,l-l0,h-h0))
49    return {'state':[p,v], 'M0':m0, 'eps_grid':eps_grid.tolist(), 'margins':margins.tolist(),
50            'fitted_margin_slope':slope, 'predicted_margin_slope':-2.0,
51            'predicted_eps_critical':epscrit, 'observed_eps_critical':first_empty,
52            'bound_shift_samples':shifts, 'predicted_shifts': 'lower +eps, upper -eps'}
53
54def simulate(mode, disturbance, eps_bound, seed=7, T=8.0, dt=.002):
55    rng=np.random.default_rng(seed)
56    p,v=.72,-.05
57    pmin,pmax,umax=-1.,1.,1.
58    max_violation=0.; interventions=[]; margins=[]; bound_misses=0
59    n=int(T/dt)
60    for i in range(n):
61        t=i*dt
62        # bounded time-varying disturbance, plus fixed observer estimate zero
63        w=disturbance*(0.65*math.sin(1.7*t)+0.35*math.sin(4.1*t+0.3))
64        un=nominal_policy(p,v,target=-.65 if t<3 else .65)
65        if mode=='clip':
66            lo,hi=-umax,umax
67            us=float(np.clip(un,lo,hi))
68            margin=2*umax
69        elif mode=='cbf':
70            lo,hi=interval(p,v,pmin,pmax,umax,what=0,eps=0)
71            us=project(un,lo,hi); margin=hi-lo
72        else:
73            lo,hi=interval(p,v,pmin,pmax,umax,what=0,eps=eps_bound)
74            us=project(un,lo,hi); margin=hi-lo
75        # Euler is sufficiently fine for this sanity test; include continuous sampled states.
76        interventions.append(abs(us-un)); margins.append(margin)
77        p += dt*v
78        v += dt*(us+w)
79        max_violation=max(max_violation,max(0,pmin-p,p-pmax))
80        if abs(w)>eps_bound+1e-9: bound_misses += 1
81    return {'max_violation':float(max_violation), 'mean_intervention':float(np.mean(interventions)),
82            'min_margin':float(np.min(margins)), 'bound_misses':bound_misses}
83
84def run():
85    random.seed(7); np.random.seed(7)
86    math_result=math_sweeps()
87    rows=[]
88    for d in [0,.1,.2,.3,.4,.5,.6,.8,1.0]:
89        rows.append({'disturbance_amplitude':d,
90          'clip':simulate('clip',d,.35), 'ordinary_cbf':simulate('cbf',d,.35),
91          'robust_cbf':simulate('robust',d,.35)})
92    result={'math_verification':math_result,'closed_loop':rows,
93      'notes':'Disturbance bound is eps=.35; actual sinusoid peak equals amplitude. Empty robust intervals use closest actuator endpoint emergency fallback.'}
94    Path('results.json').write_text(json.dumps(result,indent=2))
95    print(json.dumps(result,indent=2))
96
97if __name__=='__main__': run()