1import json, random
 2from pathlib import Path
 3import numpy as np
 4
 5# Scalar robust control-affine plant: xdot = u + w, |w| <= wbar.
 6# V=x^2/2 and h=1-x^2. The QP uses nonnegative CLF/CBF slacks.
 7
 8def quantities(x, wbar, cV=0.4, ch=1.0):
 9    V = .5*x*x
10    # CLF: gradV*f + margin + cV*V <= sV
11    aV, bV = x, wbar*abs(x) + cV*V
12    # CBF: gradH*f - margin + alpha(h) >= -sH.
13    # Multiplying by -1 gives aH*u+bH <= sH.
14    aH, bH = 2*x, 2*wbar*abs(x) - ch*(1-x*x)
15    return aV, bV, aH, bH, V
16
17def shield(x, unet, wbar, umax=1.0, rho=100.0, cV=.4, ch=1.0):
18    a,b,c,d,V = quantities(x,wbar,cV,ch)
19    cuts=[-umax, umax]
20    for q,r in ((a,b),(c,d)):
21        if abs(q)>1e-12:
22            z=-r/q
23            if -umax <= z <= umax: cuts.append(z)
24    cuts=sorted(set(cuts)); candidates=list(cuts)
25    for lo,hi in zip(cuts[:-1],cuts[1:]):
26        mid=(lo+hi)/2
27        active1=(a*mid+b)>0; active2=(c*mid+d)>0
28        den=1.; num=unet
29        if active1: den += 2*rho*a*a; num -= 2*rho*a*b
30        if active2: den += 2*rho*c*c; num -= 2*rho*c*d
31        candidates.append(min(hi,max(lo,num/den)))
32    def obj(u): return .5*(u-unet)**2 + rho*max(0,a*u+b)**2 + rho*max(0,c*u+d)**2
33    u=min(candidates,key=obj)
34    return float(u), float(max(0,a*u+b)), float(max(0,c*u+d)), float(obj(u))
35
36def check_scaling():
37    x=.6; cV=.4; rows=[]
38    for wb in np.linspace(0,.8,9):
39        pred=wb*abs(x)+cV*.5*x*x
40        # Direct numerical robust CLF residual at u=0.
41        a,b,_,_,_=quantities(x,float(wb),cV)
42        observed=a*0+b
43        rows.append({'wbar':float(wb),'predicted_margin':float(pred),'observed_margin':float(observed)})
44    slope=float(np.polyfit([r['wbar'] for r in rows],[r['observed_margin'] for r in rows],1)[0])
45    return rows,slope
46
47def check_boundary():
48    # At x=-1, CLF requires u >= wbar+cV/2; CBF requires u >= wbar.
49    # Thus zero-slack feasibility ends at wbar=umax-cV/2=.8.
50    umax=1.; cV=.4; predicted=umax-cV/2; rows=[]
51    for wb in np.linspace(.4,1.2,17):
52        u,sv,sh,_=shield(-1.,0.,float(wb),umax=umax,rho=1e6,cV=cV)
53        rows.append({'wbar':float(wb),'u':u,'total_slack':float(sv+sh),
54                     'zero_slack':bool(sv+sh<1e-5)})
55    onset=next((r['wbar'] for r in rows if not r['zero_slack']),None)
56    return predicted,rows,onset
57
58def envelope():
59    # For feasible robust CLF, adversarial V derivative satisfies Vdot <= -cV V.
60    x=.8; wb=.2; dt=.002; T=2.; cV=.4; vs=[]; sl=[]; residual=[]
61    for _ in range(int(T/dt)):
62        u,s,h,_=shield(x,1.,wb,umax=1.,rho=1e6,cV=cV)
63        w=wb*(1 if x>=0 else -1)
64        residual.append(x*(u+w)+cV*.5*x*x)
65        x += dt*(u+w); vs.append(.5*x*x); sl.append(s)
66    t=np.arange(1,len(vs)+1)*dt; v0=.5*.8*.8
67    bound=v0*np.exp(-cV*t)
68    return {'max_V_over_zero_disturbance_bound':float(max(np.array(vs)/(bound+1e-12))),
69            'max_robust_clf_residual':float(max(residual)), 'max_clf_slack':float(max(sl)),
70            'final_V':float(vs[-1]), 'initial_V':v0, 'target_rate':cV}
71
72def baseline_episode(wb=.2,n=1000,dt=.002):
73    x=.8; violations=0
74    for _ in range(n):
75        x += dt*(1.+wb*(1 if x>=0 else -1)); violations += abs(x)>1
76    return {'violation_fraction':violations/n,'final_abs_x':abs(x)}
77
78def shield_episode(wb=.2,n=1000,dt=.002):
79    x=.8; violations=0; dev=[]; sl=[]
80    for _ in range(n):
81        u,s,h,_=shield(x,1.,wb,rho=1e6)
82        x += dt*(u+wb*(1 if x>=0 else -1)); violations += abs(x)>1
83        dev.append(abs(u-1.)); sl.append(s+h)
84    return {'violation_fraction':violations/n,'final_abs_x':abs(x),
85            'mean_action_deviation':float(np.mean(dev)),'mean_slack':float(np.mean(sl))}
86
87def main():
88    np.random.seed(7); random.seed(7)
89    scale,slope=check_scaling(); pred,rows,onset=check_boundary()
90    out={'scaling':{'rows':scale,'observed_slope':slope,'predicted_slope':.6},
91         'boundary':{'predicted_wbar_onset':pred,'observed_grid_onset':onset,'rows':rows},
92         'envelope':envelope(),'baseline_episode':baseline_episode(),'shield_episode':shield_episode()}
93    Path('results.json').write_text(json.dumps(out,indent=2)); print(json.dumps(out,indent=2))
94if __name__=='__main__': main()