Feasible Action Mapping Safety Layer / safety_layer_experiment.py
Mechanism confirmed, baseline not beaten
1import json, math, random
2from pathlib import Path
3import numpy as np
4
5# Feasible-action mapping safety layer for a 1D point mass.
6# x=[position, velocity], u=p is held constant over H predictions.
7class ConstantAccelerationSafety:
8 def __init__(self, dt=0.2, H=5, u_min=-1.0, u_max=1.0,
9 pos_min=-2.0, pos_max=2.0, vel_min=-2.0, vel_max=2.0,
10 terminal_pos_min=-0.4, terminal_pos_max=0.4,
11 terminal_vel_min=-0.5, terminal_vel_max=0.5):
12 self.dt, self.H = dt, H
13 self.u_min, self.u_max = u_min, u_max
14 self.pos_min, self.pos_max = pos_min, pos_max
15 self.vel_min, self.vel_max = vel_min, vel_max
16 self.tpos_min, self.tpos_max = terminal_pos_min, terminal_pos_max
17 self.tvel_min, self.tvel_max = terminal_vel_min, terminal_vel_max
18
19 def interval(self, state):
20 """Return exact feasible interval [lo,hi] for constant p."""
21 x0, v0 = map(float, state)
22 lo, hi = self.u_min, self.u_max
23 # x_k = x0 + k dt v0 + .5 (k dt)^2 p; v_k=v0+k dt p
24 for k in range(1, self.H + 1):
25 A = 0.5 * (k*self.dt)**2; B = x0 + k*self.dt*v0
26 for bound, is_lower in [(self.pos_min, True), (self.pos_max, False)]:
27 if is_lower:
28 if A > 0: lo = max(lo, (bound-B)/A)
29 else:
30 if A > 0: hi = min(hi, (bound-B)/A)
31 A = k*self.dt; B = v0
32 for bound, is_lower in [(self.vel_min, True), (self.vel_max, False)]:
33 if is_lower:
34 if A > 0: lo = max(lo, (bound-B)/A)
35 else:
36 if A > 0: hi = min(hi, (bound-B)/A)
37 # Explicit terminal set (at H; duplicate state constraints are harmless).
38 k=self.H; A=0.5*(k*self.dt)**2; B=x0+k*self.dt*v0
39 lo=max(lo,(self.tpos_min-B)/A); hi=min(hi,(self.tpos_max-B)/A)
40 A=k*self.dt; B=v0
41 lo=max(lo,(self.tvel_min-B)/A); hi=min(hi,(self.tvel_max-B)/A)
42 return lo, hi
43
44 def project(self, state, z):
45 lo, hi = self.interval(state)
46 if lo > hi: return None, float('inf'), (lo,hi)
47 p = min(max(float(z), lo), hi)
48 return p, abs(p-float(z)), (lo,hi)
49
50 def rollout(self, state, p):
51 x,v=map(float,state); traj=[]
52 for _ in range(self.H):
53 x=x+self.dt*v+0.5*self.dt*self.dt*p; v=v+self.dt*p
54 traj.append((x,v,p))
55 return np.asarray(traj)
56
57 def certified(self,state,p):
58 if p is None: return False
59 q=self.rollout(state,p)
60 return bool(np.all((q[:,0]>=self.pos_min-1e-9)&(q[:,0]<=self.pos_max+1e-9)&
61 (q[:,1]>=self.vel_min-1e-9)&(q[:,1]<=self.vel_max+1e-9)&
62 (q[:,2]>=self.u_min-1e-9)&(q[:,2]<=self.u_max+1e-9)&
63 (q[-1,0]>=self.tpos_min-1e-9)&(q[-1,0]<=self.tpos_max+1e-9)&
64 (q[-1,1]>=self.tvel_min-1e-9)&(q[-1,1]<=self.tvel_max+1e-9)))
65
66def run():
67 np.random.seed(1341); random.seed(1341)
68 sl=ConstantAccelerationSafety()
69 out={"predictions":{},"simulation":{}}
70 # Prediction 1: projection is identity throughout feasible interval.
71 state=np.array([0.0,0.0]); lo,hi=sl.interval(state)
72 zs=np.linspace(lo,hi,101); ds=np.array([sl.project(state,z)[1] for z in zs])
73 out["predictions"]["inside_identity"]={"predicted":"d=0 for z in [lo,hi]","observed_max_d":float(ds.max()),"interval":[lo,hi]}
74 # Prediction 2: outside distance has slope one (weighted scalar projection).
75 zleft=np.linspace(lo-3,lo-.05,40); zright=np.linspace(hi+.05,hi+3,40)
76 dl=np.array([sl.project(state,z)[1] for z in zleft]); dr=np.array([sl.project(state,z)[1] for z in zright])
77 slope_l=np.polyfit(zleft,dl,1)[0]; slope_r=np.polyfit(zright,dr,1)[0]
78 out["predictions"]["outside_linear_distance"]={"predicted":"d=|z-boundary|, slope=1","observed_left_slope":float(slope_l),"observed_right_slope":float(slope_r),"max_abs_slope_error":float(max(abs(slope_l+1),abs(slope_r-1)))}
79 # Prediction 3: the active terminal-velocity boundary shifts linearly with current velocity.
80 states=np.array([[0.,0.],[0.,0.1],[0.,-0.1],[0.,0.3],[0.,-0.3]])
81 bounds=np.array([sl.interval(s) for s in states])
82 # v_H=v0+H*dt*p and v_H <= terminal_vel_max, so hi shifts by -dv/(H dt).
83 predicted_shift=-(states[:,1]-states[0,1])/(sl.H*sl.dt)
84 observed_shift=bounds[:,1]-bounds[0,1]
85 residual=observed_shift-predicted_shift
86 out["predictions"]["state_dependent_boundary"]={"predicted":"upper boundary shift=-delta_v/(H dt)","predicted_shifts":predicted_shift.tolist(),"observed_hi_shifts":observed_shift.tolist(),"max_abs_residual":float(np.max(np.abs(residual)))}
87 # Certified-regime random test: exact projection must satisfy every constraint.
88 cert=0; feasible=0
89 for _ in range(10000):
90 s=np.random.uniform([-0.35,-0.35],[0.35,0.35]); z=np.random.uniform(-4,4)
91 p,d,b=sl.project(s,z)
92 if p is not None:
93 feasible+=1; cert += int(sl.certified(s,p))
94 out["predictions"]["zero_violation"]={"predicted":"0 violations whenever solver feasible","feasible_cases":feasible,"certified_fraction":cert/max(feasible,1)}
95 # One-step stress test: compare coordinate clipping with the predictive map.
96 clip_bad = safe_bad = 0
97 for _ in range(10000):
98 s=np.random.uniform([-1.8,-1.8],[1.8,1.8]); z=np.random.uniform(-4,4)
99 pc=float(np.clip(z,sl.u_min,sl.u_max)); q=sl.rollout(s,pc)[0]
100 clip_bad += int(not (-2<=q[0]<=2 and -2<=q[1]<=2))
101 ps,_,_=sl.project(s,z)
102 if ps is not None:
103 q=sl.rollout(s,ps)[0]
104 safe_bad += int(not (-2<=q[0]<=2 and -2<=q[1]<=2))
105 out["predictions"]["clipping_comparison"]={"predicted":"predictive projection has no certified one-step violations","clipping_violations":clip_bad,"projection_violations":safe_bad}
106 # Same tiny control task: clipping only limits action; safety projection enforces predicted constraints.
107 def target_policy(s): return 2.2*(0.65-s[0])-0.8*s[1]
108 def episode(method, n=80):
109 s=np.array([-0.8,0.0]); violations=0; reward=0.; projections=[]
110 for t in range(n):
111 z=target_policy(s)+np.random.default_rng(100+t).normal(0,.18)
112 if method=='clip': p=float(np.clip(z,sl.u_min,sl.u_max)); d=0.
113 else:
114 p,d,b=sl.project(s,z)
115 if p is None: p=0.; d=abs(z-p)
116 # execute one step, then assess actual constraints
117 x,v=s; ns=np.array([x+sl.dt*v+.5*sl.dt**2*p,v+sl.dt*p])
118 bad=not (-2<=ns[0]<=2 and -2<=ns[1]<=2 and -1<=p<=1)
119 violations += int(bad)
120 reward -= float((ns[0])**2+0.1*ns[1]**2+0.01*p*p)
121 projections.append(d); s=ns
122 return reward,violations,float(np.mean(projections))
123 reps=30
124 base=np.array([episode('clip') for _ in range(reps)]); idea=np.array([episode('safe') for _ in range(reps)])
125 out["simulation"]={"episodes":reps,"baseline_clip":{"mean_reward":float(base[:,0].mean()),"violations_per_episode":float(base[:,1].mean()),"mean_projection":float(base[:,2].mean())},"idea_projection":{"mean_reward":float(idea[:,0].mean()),"violations_per_episode":float(idea[:,1].mean()),"mean_projection":float(idea[:,2].mean())}}
126 Path('results.json').write_text(json.dumps(out,indent=2))
127 print(json.dumps(out,indent=2))
128 assert ds.max()<1e-10 and abs(slope_l+1)<1e-10 and abs(slope_r-1)<1e-10
129 assert cert==feasible
130if __name__=='__main__': run()