1import json, math
  2from pathlib import Path
  3import numpy as np
  4
  5EPS = 0.03
  6R_OBS = 0.60
  7D_SAFE = 0.25
  8K1 = 2.0
  9K2 = 2.0
 10AMAX = 3.0
 11
 12
 13def barrier(p, v, c=np.zeros(2), k1=K1, k2=K2):
 14    q = np.asarray(p, float) - c
 15    rho = math.sqrt(float(q @ q) + EPS**2)
 16    h = rho - R_OBS - D_SAFE
 17    g = q / rho
 18    H = np.eye(2) / rho - np.outer(q, q) / rho**3
 19    hd = float(g @ v)
 20    psi1 = hd + k1 * h
 21    # psi2 = g @ a + b, because p_dot=v and v_dot=a
 22    b = float(v @ H @ v + k1 * hd + k2 * psi1)
 23    return h, psi1, g, H, b
 24
 25
 26def project_box_halfspace(a_nom, g, b, amax=AMAX):
 27    """Exact small 2-D Euclidean projection onto box and g.a+b >= 0."""
 28    a_nom = np.asarray(a_nom, float)
 29    box = np.clip(a_nom, -amax, amax)
 30    if float(g @ box + b) >= -1e-12:
 31        return box, True, False
 32    req = -b
 33    cand = []
 34    gg = float(g @ g)
 35    z = a_nom + (req - float(g @ a_nom)) / gg * g
 36    if np.all(z <= amax + 1e-10) and np.all(z >= -amax - 1e-10):
 37        cand.append(z)
 38    for i in range(2):
 39        j = 1 - i
 40        for xi in (-amax, amax):
 41            if abs(g[j]) > 1e-12:
 42                zj = (req - g[i] * xi) / g[j]
 43                if -amax - 1e-10 <= zj <= amax + 1e-10:
 44                    z = np.zeros(2); z[i] = xi; z[j] = zj; cand.append(z)
 45    for x in (-amax, amax):
 46        for y in (-amax, amax):
 47            z = np.array([x, y], float)
 48            if float(g @ z) >= req - 1e-10: cand.append(z)
 49    if not cand:
 50        return box, False, True
 51    best = min(cand, key=lambda z: float((z-a_nom) @ (z-a_nom)))
 52    return best, True, True
 53
 54
 55def shield(p, v, a_nom, amax=AMAX, k1=K1, k2=K2):
 56    h, psi1, g, H, b = barrier(p, v, k1=k1, k2=k2)
 57    a, feasible, active = project_box_halfspace(a_nom, g, b, amax)
 58    return a, feasible, active, h, psi1, float(g @ a + b)
 59
 60
 61def derivative_check(seed=4):
 62    rng = np.random.default_rng(seed)
 63    errs_h, errs_psi = [], []
 64    dt = 1e-6
 65    for _ in range(100):
 66        p = rng.normal(size=2); p *= 1.3 / np.linalg.norm(p)
 67        v = rng.normal(size=2)
 68        a = rng.normal(size=2)
 69        h, psi, g, H, b = barrier(p, v)
 70        hp = barrier(p + dt*v, v + dt*a)[0]
 71        psip = barrier(p + dt*v, v + dt*a)[1]
 72        errs_h.append(abs((hp-h)/dt - float(g@v)))
 73        errs_psi.append(abs((psip-psi)/dt - float(g@a + v@H@v + K1*float(g@v))))
 74    return {"max_abs_hdot_error": float(max(errs_h)), "max_abs_psi1dot_error": float(max(errs_psi))}
 75
 76
 77def rollout(dt, shielded, scale, T=4.0):
 78    # Fixed nominal policy drives through the obstacle; shield should brake/deflect.
 79    p = np.array([-2.0, 0.0]); v = np.zeros(2); c = np.zeros(2)
 80    min_h = 1e9; activ = 0; infeas = 0; proj = 0.0; n = int(T/dt)
 81    for _ in range(n):
 82        a_nom = np.array([scale*2.2, 0.0]) - 0.8*v
 83        h, psi, g, H, b = barrier(p, v, c)
 84        min_h = min(min_h, h)
 85        if shielded:
 86            a, feasible, active, _, _, residual = shield(p,v,a_nom)
 87            activ += int(active); infeas += int(not feasible)
 88            proj += float(np.linalg.norm(a-a_nom))
 89        else:
 90            a = np.clip(a_nom, -AMAX, AMAX)
 91        # Semi-implicit Euler, matching the continuous double-integrator model.
 92        v = v + dt*a; p = p + dt*v
 93    min_h = min(min_h, barrier(p,v,c)[0])
 94    return {"min_h": float(min_h), "violation": float(max(0,-min_h)),
 95            "activation_rate": activ/n, "infeasible_rate": infeas/n,
 96            "mean_projection": proj/n}
 97
 98
 99def activation_sweep():
100    out=[]
101    p=np.array([-0.9,0.0]); v=np.array([0.0,0.0])
102    for scale in np.linspace(0, 8, 17):
103        a_nom=np.array([scale,0.0])
104        a, feasible, active, h, psi, res=shield(p,v,a_nom)
105        out.append({"scale":float(scale), "active":bool(active), "feasible":bool(feasible),
106                    "projection":float(np.linalg.norm(a-a_nom)), "residual":float(res)})
107    return out
108
109
110def gain_sweep():
111    # At a fixed near-boundary state, required normal acceleration grows with k2*psi1.
112    p=np.array([-0.86,0.0]); v=np.array([-0.15,0.0])
113    rows=[]
114    for k2 in [0.0, 0.5, 1.0, 2.0, 4.0, 8.0, 16.0, 24.0]:
115        h,psi,g,H,b=barrier(p,v,k1=K1,k2=k2)
116        req=-b
117        rows.append({"k2":k2,"h":h,"psi1":psi,"normal_required":float(req),
118                     "predicted_linear_term":float(-k2*psi), "feasible_under_amax": bool(req <= AMAX + 1e-12)})
119    return rows
120
121
122def main():
123    results={"derivative_check":derivative_check(), "activation_sweep":activation_sweep(),
124             "gain_sweep":gain_sweep(), "activation_threshold_exact": float(barrier(np.array([-0.9,0.0]), np.zeros(2))[4]), "rollouts":{}}
125    for scale in [0.8,1.2,1.6,2.0]:
126        results["rollouts"][str(scale)]={}
127        for dt in [0.02,0.01,0.005]:
128            results["rollouts"][str(scale)][str(dt)]={
129                "unshielded":rollout(dt,False,scale), "shielded":rollout(dt,True,scale)}
130    Path("results.json").write_text(json.dumps(results, indent=2))
131    print(json.dumps(results, indent=2))
132
133if __name__ == "__main__":
134    main()