1import json
  2from pathlib import Path
  3import numpy as np
  4
  5SEED = 1339
  6DT = 0.002
  7T = 2.0
  8TS = np.arange(0.0, T + DT / 2, DT)
  9ALPHA = 0.10
 10EPS = 0.02
 11
 12
 13def smooth_positive(y, width=31):
 14    width = int(width) | 1
 15    pad = width // 2
 16    yp = np.pad(y, (pad, pad), mode="edge")
 17    return np.convolve(yp, np.ones(width) / width, mode="valid") + 1e-8
 18
 19
 20def demonstrations(rng, n=160):
 21    x0 = rng.uniform(-1.0, 1.0, n)
 22    k = rng.normal(1.0, 0.07, n)
 23    x = x0[:, None] * np.exp(-k[:, None] * TS[None, :])
 24    x += rng.normal(0.0, 0.006, x.shape)
 25    return x
 26
 27
 28def infer_funnel(xdemo):
 29    raw = EPS + np.quantile(np.abs(xdemo), 1.0 - ALPHA, axis=0)
 30    return smooth_positive(raw)
 31
 32
 33def rollout(x0, ubar, k_request, rho, aware):
 34    x = float(x0)
 35    xs, us, gains = [], [], []
 36    for j in range(len(TS)):
 37        r = float(rho[j])
 38        k = min(k_request, ubar / r) if aware else k_request
 39        u = float(np.clip(-k * x, -ubar, ubar))
 40        xs.append(x)
 41        us.append(u)
 42        gains.append(k)
 43        x += DT * u
 44    return np.asarray(xs), np.asarray(us), np.asarray(gains)
 45
 46
 47def fit_decay(x):
 48    sel = (TS >= 0.15) & (TS <= 0.8) & (np.abs(x) > 1e-5)
 49    return -float(np.polyfit(TS[sel], np.log(np.abs(x[sel])), 1)[0])
 50
 51
 52def main():
 53    rng = np.random.default_rng(SEED)
 54    demos = demonstrations(rng)
 55    rho = infer_funnel(demos)
 56    # Prediction 1: actuator-aware requested gain is clipped at k_max=ubar/rho.
 57    # Use the initial funnel radius, where the scalar bound is sharpest.
 58    rho0 = float(rho[0])
 59    ubar = 0.35
 60    predicted_kmax = ubar / rho0
 61    gain_rows = []
 62    for requested in [0.5 * predicted_kmax, 0.9 * predicted_kmax,
 63                      1.1 * predicted_kmax, 2.0 * predicted_kmax]:
 64        _, _, gains = rollout(1.0, ubar, requested, rho, True)
 65        gain_rows.append({"requested": requested,
 66                          "observed_initial_gain": float(gains[0]),
 67                          "ratio_to_bound": float(gains[0] / predicted_kmax)})
 68
 69    # Prediction 2: below the bound, normalized error decays as exp(-k t),
 70    # while above it actuator clipping causes a slower initial decay.
 71    decay_rows = []
 72    for requested in [0.5 * predicted_kmax, 0.9 * predicted_kmax,
 73                      1.1 * predicted_kmax, 2.0 * predicted_kmax]:
 74        for aware in [False, True]:
 75            x, u, gains = rollout(1.0, ubar, requested, rho, aware)
 76            decay_rows.append({"requested": requested, "aware": aware,
 77                               "predicted_decay": requested,
 78                               "observed_decay": fit_decay(x),
 79                               "max_abs_u": float(np.max(np.abs(u))),
 80                               "max_normalized_error": float(np.max(np.abs(x) / rho)),
 81                               "violation_fraction": float(np.mean(np.abs(x) > rho))})
 82
 83    # Prediction 3: increasing actuator authority increases the feasible gain
 84    # linearly, k_max=ubar/rho0, and the aware policy realizes that scaling.
 85    scaling_rows = []
 86    requested = 10.0
 87    for limit in [0.15, 0.25, 0.35, 0.50, 0.70]:
 88        _, _, gains = rollout(1.0, limit, requested, rho, True)
 89        predicted = limit / rho0
 90        scaling_rows.append({"ubar": limit, "predicted_kmax": predicted,
 91                             "observed_initial_gain": float(gains[0]),
 92                             "relative_error": float(abs(gains[0] - predicted) / predicted)})
 93
 94    # Held-out initial states: compare fixed-gain saturated baseline to funnel-aware law.
 95    heldout = np.linspace(-1.0, 1.0, 41)
 96    k_baseline = 2.0 * predicted_kmax
 97    comparisons = []
 98    for aware in [False, True]:
 99        vals = []
100        for x0 in heldout:
101            x, u, _ = rollout(x0, ubar, k_baseline, rho, aware)
102            vals.append([abs(x[-1]), np.max(np.abs(x) / rho), np.mean(np.abs(x) > rho), np.mean(np.abs(u))])
103        a = np.asarray(vals)
104        comparisons.append({"controller": "funnel_aware" if aware else "fixed_gain_saturated",
105                            "final_abs_error_mean": float(a[:, 0].mean()),
106                            "max_normalized_error_mean": float(a[:, 1].mean()),
107                            "violation_fraction_mean": float(a[:, 2].mean()),
108                            "mean_abs_control": float(a[:, 3].mean())})
109
110    # Cheap numerical derivative check of the normalized-error identity.
111    x, _, _ = rollout(0.8, ubar, 0.5 * predicted_kmax, rho, True)
112    z = x / rho
113    zd_num = np.gradient(z, DT)
114    edot = np.gradient(x, DT)
115    rhodot = np.gradient(rho, DT)
116    zd_formula = edot / rho - x * rhodot / (rho ** 2)
117    identity_rel_error = float(np.max(np.abs(zd_num - zd_formula)) / (np.max(np.abs(zd_num)) + 1e-9))
118
119    results = {
120        "seed": SEED,
121        "n_demonstrations": len(demos),
122        "rho_initial": rho0,
123        "rho_final": float(rho[-1]),
124        "predictions": {
125            "actuator_boundary": {"predicted_kmax_ubar_over_rho": predicted_kmax, "sweep": gain_rows},
126            "decay_and_saturation": decay_rows,
127            "linear_authority_scaling": scaling_rows,
128            "normalized_error_identity_relative_error": identity_rel_error,
129        },
130        "comparison": comparisons,
131    }
132    Path("results.json").write_text(json.dumps(results, indent=2))
133    print(json.dumps(results, indent=2))
134
135
136if __name__ == "__main__":
137    main()