EX-001 Example

#!/usr/bin/env python3
"""EX-001：直接配点法算例 —— 最小功双积分器（min-effort double integrator）。

问题（与 Kelly 2017 教程同型的最小功问题，PPR-004）：

    min  ∫_0^T |u(t)| dt
    s.t. ẋ = v,  v̇ = u
         x(0)=v(0)=0,  x(T)=1,  v(T)=0,  |u(t)| ≤ u_max

转录（CPT-020 配点法）：N 个等距区间，梯形配点约束

    x_{k+1} = x_k + h(v_k + v_{k+1})/2
    v_{k+1} = v_k + h(u_k + u_{k+1})/2

目标函数对 |u| 不可微，引入松弛变量 σ_k ≥ |u_k|（CPT-027），
用梯形权重离散目标：min Σ w_k σ_k（w_0=w_N=h/2，其余 h）。
动力学与目标对 (x, v, u, σ) 线性 → 该转录恰为线性规划，
用 scipy.optimize.linprog(method="highs") 精确求解（全局最优）。

解析解（bang-off-bang，CPT-024）：固定 T 大于最小时间时，最小功解为
“加速–巡航–减速”三相位：u=+u_max on [0,τ]，u=0 on [τ, T−τ]，
u=−u_max on [T−τ, T]，其中 τ 满足 −τ² + Tτ = x_target/u_max。
本算例取 x_target=1、u_max=1、T=3 ⇒ τ=(T−√(T²−4))/2≈0.381966，
切换时刻 t₁=τ、t₂=T−τ，最小功 = u_max·2τ ≈ 0.763932。
（T 取最小值 2 时三相位退化为两相位 bang-bang；实测梯形配点在
T=2 处离散可达集略小于 1，LP 不可行——离散化误差的直观体现，
故本算例取 T=3。）

确定性：给定参数下 HiGHS 可复现；输出数值统一四舍五入到 9 位。
输出：examples/EX-001.json（机器结果）+ examples/EX-001.svg（图）。

运行：examples/.venv/bin/python examples/EX-001.py
"""
from __future__ import annotations

import hashlib
import json
import math
from pathlib import Path

import numpy as np
from scipy.optimize import linprog

HERE = Path(__file__).resolve().parent
X_TARGET = 1.0
U_MAX = 1.0
T_FINAL = 3.0                                        # > 最小时间 2√(x/u)
# 三相位解析解：τ² - T·τ + x_target/u_max = 0
TAU = (T_FINAL - math.sqrt(T_FINAL**2
                           - 4.0 * X_TARGET / U_MAX)) / 2.0
T_SWITCH1 = TAU                                      # 加速 → 巡航
T_SWITCH2 = T_FINAL - TAU                            # 巡航 → 减速
EFFORT_ANALYTIC = U_MAX * 2.0 * TAU                  # ≈ 0.763932023
MESHES = [10, 20, 40, 80, 160]


def analytic(t):
    """解析 bang-off-bang 解（标量或 ndarray）。"""
    t = np.asarray(t, dtype=float)
    phase1 = t <= T_SWITCH1
    phase3 = t >= T_SWITCH2
    phase2 = ~phase1 & ~phase3
    x = np.where(phase1, t**2 / 2.0,
                 np.where(phase2, TAU**2 / 2.0 + TAU * (t - TAU),
                          X_TARGET - (T_FINAL - t) ** 2 / 2.0))
    v = np.where(phase1, t, np.where(phase2, TAU, T_FINAL - t))
    u = np.where(phase1, U_MAX, np.where(phase2, 0.0, -U_MAX))
    return x, v, u


def solve_collocation(n):
    """梯形配点 + 松弛变量 → LP；返回解与诊断量。"""
    h = T_FINAL / n
    nv = n + 1
    # 变量分块：x(0..n), v(0..n), u(0..n), s(0..n)
    def ix(k):   # x
        return k

    def iv(k):
        return nv + k

    def iu(k):
        return 2 * nv + k

    def is_(k):
        return 3 * nv + k

    nvar = 4 * nv
    c = np.zeros(nvar)
    w = np.full(nv, h)
    w[0] = w[-1] = h / 2.0
    for k in range(nv):
        c[is_(k)] = w[k]

    # 等式约束
    rows, rhs = [], []

    def eq(coefs, b):
        r = np.zeros(nvar)
        for i, val in coefs:
            r[i] += val
        rows.append(r)
        rhs.append(b)

    eq([(ix(0), 1.0)], 0.0)
    eq([(iv(0), 1.0)], 0.0)
    eq([(ix(n), 1.0)], X_TARGET)
    eq([(iv(n), 1.0)], 0.0)
    for k in range(n):
        eq([(ix(k + 1), 1.0), (ix(k), -1.0),
            (iv(k), -h / 2.0), (iv(k + 1), -h / 2.0)], 0.0)
        eq([(iv(k + 1), 1.0), (iv(k), -1.0),
            (iu(k), -h / 2.0), (iu(k + 1), -h / 2.0)], 0.0)
    a_eq = np.array(rows)
    b_eq = np.array(rhs)

    # 不等式：u_k - s_k <= 0, -u_k - s_k <= 0
    ub_rows = []
    for k in range(nv):
        r = np.zeros(nvar)
        r[iu(k)] = 1.0
        r[is_(k)] = -1.0
        ub_rows.append(r)
        r = np.zeros(nvar)
        r[iu(k)] = -1.0
        r[is_(k)] = -1.0
        ub_rows.append(r)
    a_ub = np.array(ub_rows)
    b_ub = np.zeros(len(ub_rows))

    bounds = ([(None, None)] * nv + [(None, None)] * nv
              + [(-U_MAX, U_MAX)] * nv + [(0.0, None)] * nv)

    res = linprog(c, A_ub=a_ub, b_ub=b_ub, A_eq=a_eq, b_eq=b_eq,
                  bounds=bounds, method="highs")
    if not res.success:
        raise RuntimeError(f"N={n}: LP failed: {res.message}")

    z = res.x
    t = np.linspace(0.0, T_FINAL, nv)
    x = z[[ix(k) for k in range(nv)]]
    v = z[[iv(k) for k in range(nv)]]
    u = z[[iu(k) for k in range(nv)]]
    xa, va, ua = analytic(t)
    # 切换点处 u 的两侧取值不同，误差统计排除切换邻域节点
    away = ((np.abs(t - T_SWITCH1) > h)
            & (np.abs(t - T_SWITCH2) > h))

    def cross(level):
        """u 首次穿越 level 的插值时刻（相邻节点线性插值）。"""
        for k in range(nv - 1):
            a, b = u[k] - level, u[k + 1] - level
            if a * b < 0:
                return float(t[k] + (t[k + 1] - t[k]) * (-a) / (b - a))
        return None

    t1 = cross(U_MAX / 2.0)
    t2 = cross(-U_MAX / 2.0)
    return {
        "N": n,
        "h": round(h, 9),
        "objective": round(float(res.fun), 9),
        "objective_err_vs_analytic": round(float(res.fun)
                                           - EFFORT_ANALYTIC, 9),
        "switch1_time": round(t1, 9) if t1 is not None else None,
        "switch1_err": (round(t1 - T_SWITCH1, 9)
                        if t1 is not None else None),
        "switch2_time": round(t2, 9) if t2 is not None else None,
        "switch2_err": (round(t2 - T_SWITCH2, 9)
                        if t2 is not None else None),
        "max_abs_u": round(float(np.max(np.abs(u))), 9),
        "x_err_inf": round(float(np.max(np.abs(x - xa))), 9),
        "v_err_inf": round(float(np.max(np.abs(v - va))), 9),
        "u_err_inf_away_from_switch": round(
            float(np.max(np.abs(u[away] - ua[away]))) if away.any()
            else 0.0, 9),
        "terminal_x": round(float(x[-1]), 9),
        "terminal_v": round(float(v[-1]), 9),
    }, (t, x, v, u)


def svg_plot(t, x, v, u, path):
    """手写 SVG：三段面板（x/v/u），数值解实线 + 解析解虚线。"""
    W, H = 760, 560
    pad_l, pad_r, pad_t, pad_b = 56, 16, 26, 30
    pw = W - pad_l - pad_r
    panel_h = (H - pad_t - pad_b - 2 * 14) / 3.0
    xa, va, ua = analytic(t)
    panels = [
        ("x(t)", x, xa, (0.0, max(1.0, float(np.max(x))))),
        ("v(t)", v, va, (min(0.0, float(np.min(v))),
                         max(1.0, float(np.max(v))))),
        ("u(t)", u, ua, (-1.2, 1.2)),
    ]

    def pts(arr, lo, hi, y0):
        n = len(arr)
        out = []
        for i, val in enumerate(arr):
            px = pad_l + pw * i / max(1, n - 1)
            py = y0 + panel_h - panel_h * (val - lo) / (hi - lo)
            out.append(f"{px:.2f},{py:.2f}")
        return " ".join(out)

    parts = [f'<svg xmlns="http://www.w3.org/2000/svg" '
             f'width="{W}" height="{H}" viewBox="0 0 {W} {H}" '
             f'font-family="sans-serif" font-size="12">',
             f'<rect width="{W}" height="{H}" fill="#ffffff"/>']
    for j, (label, num, ana, (lo, hi)) in enumerate(panels):
        y0 = pad_t + j * (panel_h + 14)
        parts.append(f'<rect x="{pad_l}" y="{y0:.2f}" width="{pw}" '
                     f'height="{panel_h:.2f}" fill="#fafafa" '
                     f'stroke="#dddddd"/>')
        if lo < 0 < hi:      # 零线
            zy = y0 + panel_h - panel_h * (0 - lo) / (hi - lo)
            parts.append(f'<line x1="{pad_l}" y1="{zy:.2f}" '
                         f'x2="{pad_l + pw}" y2="{zy:.2f}" '
                         f'stroke="#e0e0e0"/>')
        parts.append(f'<polyline points="{pts(ana, lo, hi, y0)}" '
                     f'fill="none" stroke="#1f77b4" stroke-width="1.4" '
                     f'stroke-dasharray="5,4"/>')
        parts.append(f'<polyline points="{pts(num, lo, hi, y0)}" '
                     f'fill="none" stroke="#d62728" stroke-width="2"/>')
        parts.append(f'<text x="6" y="{y0 + panel_h / 2:.2f}" '
                     f'fill="#333">{label}</text>')
        parts.append(f'<text x="{pad_l}" y="{y0 - 6:.2f}" '
                     f'fill="#666">{lo:g} … {hi:g}</text>')
    parts.append(f'<text x="{pad_l}" y="{H - 8}" fill="#666">'
                 f't（红：配点数值解；蓝虚线：解析 bang-bang 解）'
                 f'</text>')
    parts.append("</svg>")
    path.write_text("\n".join(parts) + "\n", encoding="utf-8")


def main():
    cases = []
    finest = None
    for n in MESHES:
        case, series = solve_collocation(n)
        cases.append(case)
        finest = series
    out = {
        "problem": {
            "name": "minimum-effort double integrator",
            "x_target": X_TARGET,
            "u_max": U_MAX,
            "T": T_FINAL,
            "analytic": {
                "u": ("bang-off-bang: +u_max on [0,tau], 0 on "
                      "[tau,T-tau], -u_max on [T-tau,T]"),
                "tau": round(TAU, 9),
                "switch_times": [round(T_SWITCH1, 9),
                                 round(T_SWITCH2, 9)],
                "effort": round(EFFORT_ANALYTIC, 9),
            },
        },
        "transcription": {
            "method": "trapezoidal collocation",
            "slack_for_abs_control": True,
            "lp_solver": "scipy.optimize.linprog(method=highs)",
            "variables_per_node": ["x", "v", "u", "sigma"],
        },
        "cases": cases,
    }
    jpath = HERE / "EX-001.json"
    jpath.write_text(json.dumps(out, ensure_ascii=False, indent=2,
                                sort_keys=True) + "\n", encoding="utf-8")
    svg_plot(*finest, HERE / "EX-001.svg")
    digest = hashlib.sha256(jpath.read_bytes().rstrip()).hexdigest()
    print(f"wrote {jpath.name} (result-hash sha256:{digest})")
    print(f"wrote EX-001.svg")
    for c in cases:
        print(f"  N={c['N']:>3}  obj={c['objective']:.9f}  "
              f"t1={c['switch1_time']:.6f}  t2={c['switch2_time']:.6f}  "
              f"x_err={c['x_err_inf']:.2e}  "
              f"u_err={c['u_err_inf_away_from_switch']:.2e}")


if __name__ == "__main__":
    main()