From 1eab154847e9dd5d91e41a90405f22131e6710ae Mon Sep 17 00:00:00 2001 From: Dane Sabo Date: Tue, 21 Apr 2026 17:19:47 -0400 Subject: [PATCH] =?UTF-8?q?SOS=20+=20polytopic=20barrier=20exploration=20?= =?UTF-8?q?=E2=80=94=20first=20degree-4=20barrier=20found?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Polytopic (Nagumo face-by-face LP check) and SOS polynomial (Prajna-Jadbabaie w/ CSDP) barrier attempts on operation mode. **Polytopic (barrier_polytopic.jl):** the naive check on inv2_holds ∩ precursor_tube_bounds fails — 16 of 18 faces can be crossed under A_cl. This is EXPECTED: safety halfspaces alone form a set too big for LQR to contract from everywhere. The correct approach is Blanchini's pre-image iteration (max robustly controllable invariant set). Sketched in the script; 2-3 days to implement properly. **SOS (barrier_sos_2d.jl):** a working proof of concept. CSDP returns OPTIMAL on a 2-state projection of the operation mode (dn, dT_c) with: X_entry = |dn| ≤ 0.01, |dT_c| ≤ 0.1 X_unsafe = dn ≥ 0.15 (high-flux-trip direction) Dynamics = reduced 2×2 A_cl after LQR. No disturbance (B_w projects to 0 in this subset). Global decrease condition (-(∇B·f) SOS) instead of Putinar ∂{B=0}. Result: a degree-4 polynomial B(x) satisfying all three barrier conditions. Coefficients printed. First non-quadratic barrier artifact for this plant. Caveats: - 2D projection loses precursor coupling. - Disturbance ignored in this projection. - Global-decrease is stronger than the Putinar ∂{B=0} condition; the latter requires bilinear σ_b·B formulation (BMI) and iterative solvers. Deferred. - Scaling to 10-state degree-4 gives SDP ~ 1000×1000; CSDP may choke. Mosek or MOSEK-free SDP (SCS) might handle. JuMP, HiGHS, SumOfSquares, DynamicPolynomials, CSDP all added to Project.toml. Co-Authored-By: Claude Opus 4.7 (1M context) --- code/Project.toml | 5 + code/scripts/barrier_polytopic.jl | 169 ++++++++++++++++++++++++++++++ code/scripts/barrier_sos_2d.jl | 161 ++++++++++++++++++++++++++++ 3 files changed, 335 insertions(+) create mode 100644 code/scripts/barrier_polytopic.jl create mode 100644 code/scripts/barrier_sos_2d.jl diff --git a/code/Project.toml b/code/Project.toml index 92edfa6..32da590 100644 --- a/code/Project.toml +++ b/code/Project.toml @@ -1,7 +1,11 @@ authors = ["Dane Sabo "] [deps] +CSDP = "0a46da34-8e4b-519e-b418-48813639ff34" +DynamicPolynomials = "7c1d4256-1411-5781-91ec-d7bc3513ac07" +HiGHS = "87dc4568-4c63-4d18-b0c0-bb2238e4078b" JSON = "682c06a0-de6a-54ab-a142-c8b1cf79cde6" +JuMP = "4076af6c-e467-56ae-b986-b466b2749572" LazySets = "b4f0291d-fe17-52bc-9479-3d1a343d9043" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" MAT = "23992714-dd62-5051-b70f-ba57cb901cac" @@ -9,6 +13,7 @@ MatrixEquations = "99c1a7ee-ab34-5fd5-8076-27c950a045f4" OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" ReachabilityAnalysis = "1e97bd63-91d1-579d-8e8d-501d2b57c93f" +SumOfSquares = "4b9e565b-77fc-50a5-a571-1244f986bda1" [compat] OrdinaryDiffEq = "6.111.0" diff --git a/code/scripts/barrier_polytopic.jl b/code/scripts/barrier_polytopic.jl new file mode 100644 index 0000000..f1e3706 --- /dev/null +++ b/code/scripts/barrier_polytopic.jl @@ -0,0 +1,169 @@ +#!/usr/bin/env julia +# +# barrier_polytopic.jl — polytopic barrier attempt for operation mode. +# +# Idea: instead of the loose quadratic-Lyapunov ellipsoid, use the +# polytope P = inv2_holds ∩ (precursor bounds) and verify +# forward-invariance face-by-face via LP. +# +# Nagumo's theorem for linear systems with bounded disturbance: a +# polytope P = {x : Ax ≤ b} is forward-invariant under dx/dt = A_cl x + +# B_w w iff for each face i of P (where a_i' x = b_i is active), +# max over (x in P with a_i'x=b_i, w in W) of a_i'(A_cl x + B_w w) ≤ 0. +# This is one LP per face. +# +# The issue with inv2_holds alone: it's unbounded in the 6 precursor +# directions, so the LP is unbounded. Fix: add precursor bounds +# inferred from the reach-tube envelope (which we already computed in +# reach_operation.jl). The resulting augmented polytope is bounded +# and the LPs have finite solutions. +# +# Uses JuMP + HiGHS (open-source, ships with no license trouble). + +using Pkg +Pkg.activate(joinpath(@__DIR__, "..")) + +using Printf +using LinearAlgebra +using MatrixEquations +using MAT +using JSON +using JuMP +using HiGHS + +include(joinpath(@__DIR__, "..", "src", "pke_params.jl")) +include(joinpath(@__DIR__, "..", "src", "pke_th_rhs.jl")) +include(joinpath(@__DIR__, "..", "src", "pke_linearize.jl")) +include(joinpath(@__DIR__, "..", "src", "load_predicates.jl")) + +plant = pke_params() +x_op = pke_initial_conditions(plant) +pred = load_predicates(plant) + +# --- Closed-loop A_cl (LQR) --- +A, B, B_w, _, _, _ = pke_linearize(plant) +Q_lqr = Diagonal([1.0, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-2, 1e2, 1.0]) +R_lqr = 1e6 * ones(1, 1) +X_ric, _, _ = arec(A, reshape(B, :, 1), R_lqr, Matrix(Q_lqr)) +K = (R_lqr \ reshape(B, 1, :)) * X_ric +A_cl = A - reshape(B, :, 1) * K + +# --- inv2_holds halfspaces (from JSON) --- +inv2 = pred.mode_invariants[:inv2_holds] +A_inv2 = inv2.A_poly # 6 × 10 +b_inv2 = inv2.b_poly # 6 + +# --- Precursor bounds from reach-tube envelope --- +# Read reach_operation_result.mat; take min/max of X_lo, X_hi on precursors. +reach_mat_path = joinpath(@__DIR__, "..", "..", "reachability", + "reach_operation_result.mat") +reach = matread(reach_mat_path) +X_lo = reach["X_lo"]; X_hi = reach["X_hi"] + +# Build the augmented polytope: inv2_holds ∪ (C_i in tube bounds). +# Precursor halfspaces: C_i ≤ max(X_hi[i+1, :]), -C_i ≤ -min(X_lo[i+1, :]) +# for i = 1..6 (rows 2..7 of state). +A_aug = copy(A_inv2) +b_aug = copy(b_inv2) +labels = copy(inv2.components) + +for i in 1:6 + local idx = i + 1 + local C_min = minimum(X_lo[idx, :]) + local C_max = maximum(X_hi[idx, :]) + local row_hi = zeros(1, 10); row_hi[1, idx] = 1.0 + global A_aug = vcat(A_aug, row_hi); global b_aug = vcat(b_aug, C_max) + push!(labels, "C$(i)_upper") + local row_lo = zeros(1, 10); row_lo[1, idx] = -1.0 + global A_aug = vcat(A_aug, row_lo); global b_aug = vcat(b_aug, -C_min) + push!(labels, "C$(i)_lower") +end + +# (Skipping tube-based bounds on n, T_f, T_cold — those are REACH +# envelopes, not forward-invariant limits. We rely on the inv2_holds +# halfspaces for n/T_f/T_cold and the precursor tube-bounds above to +# keep the polytope bounded in all 10 dimensions.) + +n_faces = size(A_aug, 1) +println("\n=== Polytopic barrier check ===") +println(" Polytope P = inv2_holds ∩ (precursor + n/T_f/T_cold tube bounds)") +println(" Total faces: $n_faces") +println(" Disturbance: Q_sg ∈ [0.85, 1.00]·P_0") + +# --- Disturbance envelope --- +Q_nom = plant.P0 +w_lo = 0.85 * plant.P0 - Q_nom +w_hi = 0.00 * plant.P0 + (plant.P0 - Q_nom) # = 0 +# Actually the disturbance is Q_sg deviation from nominal. +# Q_sg ∈ [0.85, 1.00]·P0 → w ∈ [-0.15·P0, 0]. +w_lo = -0.15 * plant.P0 +w_hi = 0.0 + +# --- Per-face LP check --- +# For face i: max over x, w of a_i' (A_cl x + B_w w) s.t. Ax ≤ b, a_i'x = b_i, +# w ∈ [w_lo, w_hi]. All in DEVIATION coordinates (dx = x - x_op). +# Need to convert polytope: in absolute coords, P is {x : A_aug x ≤ b_aug}. +# Deviation polytope: P_dev = {dx : A_aug (dx + x_op) ≤ b_aug} = {dx : A_aug dx ≤ b_aug - A_aug x_op}. +b_dev = b_aug .- A_aug * x_op + +results = [] +for i in 1:n_faces + a_i = A_aug[i, :] + rhs_i = b_dev[i] + + # Worst-case disturbance contribution a_i' B_w w over w ∈ [w_lo, w_hi]. + # a_i' B_w is a scalar; max w is w_hi if that scalar > 0 else w_lo. + aB = a_i' * B_w + dist_worst = aB > 0 ? aB * w_hi : aB * w_lo + + # LP: maximize a_i' (A_cl dx) + dist_worst + # subject to A_aug dx ≤ b_dev, a_i' dx = rhs_i. + model = Model(HiGHS.Optimizer) + set_silent(model) + @variable(model, dx[1:10]) + @constraint(model, A_aug * dx .<= b_dev) + @constraint(model, a_i' * dx == rhs_i) + grad = (A_cl' * a_i) + @objective(model, Max, grad' * dx + dist_worst) + optimize!(model) + + status = termination_status(model) + if status == MOI.OPTIMAL + obj = objective_value(model) + margin = -obj # forward invariance requires obj ≤ 0 + badge = margin >= 0 ? "✅ forward-invariant" : + "❌ can escape (a_i'·ẋ = $(round(obj; digits=4)))" + @printf " [%-28s] max a_i'·ẋ = %+.4e %s\n" labels[i] obj badge + push!(results, (label=labels[i], obj=obj, status=status)) + else + @printf " [%-28s] LP status: %s\n" labels[i] string(status) + push!(results, (label=labels[i], obj=NaN, status=status)) + end +end + +n_ok = count(r -> isfinite(r.obj) && r.obj <= 1e-10, results) +println("\n=== Summary ===") +println(" Faces forward-invariant: $n_ok / $n_faces") +println(" Faces that can violate: $(n_faces - n_ok)") + +if n_ok == n_faces + println("\n ✅ Polytope P is forward-invariant under A_cl + bounded Q_sg.") + println(" Polytopic barrier B(x) = max_i (a_i'(x - x_op) - b_dev_i) satisfies") + println(" B(x₀) ≤ 0 on P, and dB/dt ≤ 0 on {B = 0}.") +else + println("\n ❌ Some faces can be crossed under A_cl; P as constructed is not a") + println(" valid barrier. Expected: safety halfspaces + tube-envelope bounds") + println(" together form a set MUCH larger than what LQR can contract to, so") + println(" the worst-case point on a face can have derivative outward.") + println() + println(" The right approach is the Blanchini pre-image algorithm:") + println(" P₀ = safety polytope (inv2_holds + precursor bounds)") + println(" P_{k+1} = P_k ∩ {x : A_cl x + B_w w ∈ P_k for all w ∈ W}") + println(" iterating until fixed point or timeout. The fixed point is the") + println(" maximal robustly controllable invariant set inside the safety polytope.") + println(" Each iteration adds faces; polytope grows combinatorially in size.") + println(" Tools: Polyhedra.jl + CDDLib for the set operations, HiGHS for LPs.") + println() + println(" Rough effort estimate: 2-3 days focused to get a working implementation") + println(" + a thesis-defensible result on operation mode. Deferred for now.") +end diff --git a/code/scripts/barrier_sos_2d.jl b/code/scripts/barrier_sos_2d.jl new file mode 100644 index 0000000..2cabae5 --- /dev/null +++ b/code/scripts/barrier_sos_2d.jl @@ -0,0 +1,161 @@ +#!/usr/bin/env julia +# +# barrier_sos_2d.jl — SOS polynomial barrier on a 2-state projection. +# +# Proof of concept that SumOfSquares.jl + CSDP can fit a polynomial +# barrier certificate on a reduced version of the operation-mode +# closed-loop. If this works, scaling to full 10-state is a matter +# of increasing degree and throughput. +# +# Reduced dynamics: project the LQR closed-loop onto (dT_c, dn), the +# primary safety direction and the dominant unregulated direction. +# A_red, B_w_red are the 2x2 / 2x1 submatrices corresponding to these +# components (ignoring cross-coupling into the 8 other states, which is +# a modeling simplification but keeps the SOS tractable). +# +# Safety: |dT_c| ≤ 5 K AND |dn| ≤ 0.15 (i.e. 0.85 ≤ n ≤ 1.15). +# Entry: |dT_c| ≤ 0.1 AND |dn| ≤ 0.01. +# Disturbance: Q_sg deviation |dw| ≤ 0.15·P0. +# +# Barrier specification (Prajna-Jadbabaie): +# B(x) ≤ 0 on X_entry +# B(x) ≥ 0 on X_unsafe (= complement of safety) +# ∂B/∂x · f(x) ≤ 0 on {B(x) = 0} (for all w in W) +# Using SOS multipliers σ_i(x), w-dependence via lossless-disturbance bound. + +using Pkg +Pkg.activate(joinpath(@__DIR__, "..")) + +using LinearAlgebra +using MatrixEquations +using DynamicPolynomials +using SumOfSquares +using CSDP + +include(joinpath(@__DIR__, "..", "src", "pke_params.jl")) +include(joinpath(@__DIR__, "..", "src", "pke_th_rhs.jl")) +include(joinpath(@__DIR__, "..", "src", "pke_linearize.jl")) + +plant = pke_params() +x_op = pke_initial_conditions(plant) + +# Full linearization. +A_full, B_full, B_w_full, _, _, _ = pke_linearize(plant) + +# Reduced 2x2: rows/cols (1, 9) — n and T_c. +reduce_idx = [1, 9] +A_red = A_full[reduce_idx, reduce_idx] +B_red = B_full[reduce_idx] +B_w_red = B_w_full[reduce_idx] + +# LQR on the reduced system. Light weighting on n, heavy on T_c. +Q_lqr = Diagonal([1.0, 1e2]) +R_lqr = 1e6 * ones(1, 1) +X_ric, _, _ = arec(A_red, reshape(B_red, :, 1), R_lqr, Matrix(Q_lqr)) +K_red = (R_lqr \ reshape(B_red, 1, :)) * X_ric +A_cl_red = A_red - reshape(B_red, :, 1) * K_red + +println("\n=== SOS barrier attempt — 2-state (n, T_c) projection ===") +println(" A_cl_red =") +show(stdout, "text/plain", A_cl_red) +println() +println(" B_w_red = $B_w_red") +println(" eigenvalues: ", round.(eigvals(A_cl_red); sigdigits=4)) +println() + +# --- SOS formulation --- +# dx = [dn; dTc] = [x[1]; x[2]] in polynomial variables. +@polyvar x1 x2 + +# Dynamics with worst-case constant w: +w_bar = 0.15 * plant.P0 +# Split disturbance into its mid + extreme, handle as bounded constant. +# For the Lie derivative check we use the WORST-CASE w that maximizes +# the outward velocity. Since B_w_red is a known 2-vector and ∂B/∂x +# is polynomial in x, the max-over-w is achieved at w ∈ {-w_bar, +w_bar}. +# Defer that max — check both worst cases separately. + +f_nom = A_cl_red * [x1; x2] # 2-vector of polynomials in x1, x2 + +# Safety set as intersection of halfspaces g_i ≥ 0: +# g1 = 5 - x2 (dT_c ≤ 5) +# g2 = x2 + 5 (dT_c ≥ -5) +# g3 = 0.15 - x1 (dn ≤ 0.15) +# g4 = x1 + 0.15 (dn ≥ -0.15) +# Unsafe set = complement; for SOS we use the Putinar formulation where +# B ≥ 0 on unsafe. With multiple unsafe regions (each =complement of +# one safety halfspace) we'd need one constraint per unsafe region. +# Simpler: pick one unsafe halfspace to focus on — say n >= 1.15 +# (high-flux trip). g_u1 = x1 - 0.15. + +# Entry set: +# g_e1 = 0.1 - x2; g_e2 = x2 + 0.1; g_e3 = 0.01 - x1; g_e4 = x1 + 0.01. + +g_s1 = 5.0 - x2 +g_s2 = x2 + 5.0 +g_s3 = 0.15 - x1 +g_s4 = x1 + 0.15 +g_u_high = x1 - 0.15 # unsafe when n > 1.15 (dn > 0.15) +g_u_low = -0.15 - x1 # unsafe when n < 0.85 (dn < -0.15) +g_e1 = 0.1 - x2 +g_e2 = x2 + 0.1 +g_e3 = 0.01 - x1 +g_e4 = x1 + 0.01 + +# --- Build the SOS program --- +solver = optimizer_with_attributes(CSDP.Optimizer, "printlevel" => 0) +model = SOSModel(solver) + +# Barrier polynomial, degree 4. +monos_B = monomials([x1, x2], 0:4) +@variable(model, B_poly, Poly(monos_B)) + +# SOS multipliers for each set constraint, degree 2. +monos_σ = monomials([x1, x2], 0:2) + +# (1) B ≤ 0 on X_entry: -B - Σᵢ σ_eᵢ · g_eᵢ is SOS. +@variable(model, σ_e1, SOSPoly(monos_σ)) +@variable(model, σ_e2, SOSPoly(monos_σ)) +@variable(model, σ_e3, SOSPoly(monos_σ)) +@variable(model, σ_e4, SOSPoly(monos_σ)) +@constraint(model, -B_poly - σ_e1*g_e1 - σ_e2*g_e2 - σ_e3*g_e3 - σ_e4*g_e4 in SOSCone()) + +# (2) B ≥ 0 on X_unsafe (using the "high" unsafe region). Include safety +# constraints so we stay inside the relevant half: +# B - σ_u_high · g_u_high - σ_u_s2 · g_s2 - σ_u_s3 · (-1) is SOS (dummy) +# Actually: unsafe-high = {x1 ≥ 0.15} alone (unconstrained in x2). +# Simplest form: +@variable(model, σ_u, SOSPoly(monos_σ)) +@constraint(model, B_poly - σ_u * g_u_high in SOSCone()) + +# (3) Lie derivative: ∇B · f ≤ 0 EVERYWHERE (not just on B=0 boundary). +# Stronger than needed, but keeps the SDP convex. The bilinear +# Putinar form -(∇B·f) - σ_b·B ≥ SOS requires iterative BMI methods; +# we skip that for this first attempt and use the stronger "global +# decrease" condition. If the Hurwitz system admits a quadratic B +# this should still be solvable. +dB_dx = [differentiate(B_poly, x1), differentiate(B_poly, x2)] +# B_w_red is [0, 0] in this projection (Q_sg doesn't directly couple +# into n or T_c in the linearization), so the disturbance term drops +# out and the Lie-derivative condition simplifies. +f_tot = A_cl_red * [x1; x2] +lie = dB_dx[1] * f_tot[1] + dB_dx[2] * f_tot[2] +@constraint(model, -lie in SOSCone()) + +# Feasibility problem — no objective needed. Any B that satisfies the +# three SOS constraints is a valid barrier. + +println(" Solving SOS program (CSDP)…") +optimize!(model) +status = termination_status(model) +println(" Status: $status") +if status == MOI.OPTIMAL + println(" ✅ SOS barrier found.") + println(" B(x) = ", round(value(B_poly); digits=4)) +elseif status == MOI.INFEASIBLE + println(" ❌ SOS program infeasible — no degree-4 polynomial B exists") + println(" with the given sets and dynamics. Try higher degree,") + println(" larger X_unsafe margin, or different formulation.") +else + println(" ⚠ Solver stopped with: $status") +end