|
| 1 | +// End-to-end co-safe-LTL synthesis over a CONTINUOUS system (Package-Delivery |
| 2 | +// pattern), tying together: sparse abstraction (full dynamics IMDP) + LTLf->DFA + |
| 3 | +// IMDP x DFA product + robust solver. Verified by Monte-Carlo: simulate the |
| 4 | +// continuous closed loop, evaluate the realised label trace against the LTLf |
| 5 | +// formula, and check the empirical satisfaction probability >= the synthesized |
| 6 | +// robust lower bound. |
| 7 | +// |
| 8 | +// Spec: F(pickup & F deliver) -- reach the pickup region, then later the deliver |
| 9 | +// region. 2-D robot x0'=0.9 x0 + 1.4 u0 ; x1'=0.8 x1 + 1.4 u1 + N(0,sigma^2). |
| 10 | +// |
| 11 | +// Build: c++ -std=c++17 -O2 benchmarks/validate_cosafe.cpp \ |
| 12 | +// src/abstraction.cpp src/solve.cpp src/omaximization.cpp src/graph_utils.cpp \ |
| 13 | +// src/ltl.cpp src/product.cpp -o /tmp/cosafe |
| 14 | +#include "../src/abstraction.h" |
| 15 | +#include "../src/ltl.h" |
| 16 | +#include "../src/product.h" |
| 17 | +#include "../src/solve.h" |
| 18 | +#include "../src/omaximization.h" |
| 19 | +#include <cstdio> |
| 20 | +#include <vector> |
| 21 | +#include <set> |
| 22 | +#include <random> |
| 23 | +#include <cmath> |
| 24 | + |
| 25 | +using namespace impact; |
| 26 | + |
| 27 | +int main() { |
| 28 | + // --- continuous system + grid (full dynamics IMDP: empty target box) --- |
| 29 | + abstraction::SystemND s; |
| 30 | + s.dim_x = 2; s.dim_u = 2; |
| 31 | + s.xlb = {-5, -5}; s.xub = {5, 5}; s.eta = {0.5, 0.5}; |
| 32 | + s.ulb = {-1, -1}; s.uub = {1, 1}; s.ueta = {0.5, 0.5}; // 5 pts/dim |
| 33 | + s.A = {{0.9, 0.0}, {0.0, 0.8}}; |
| 34 | + s.B = {{1.4, 0.0}, {0.0, 1.4}}; |
| 35 | + s.c = {0.0, 0.0}; |
| 36 | + const double sd = 0.2; |
| 37 | + s.sigma = {sd, sd}; |
| 38 | + s.tlo = {1e18, 1e18}; s.thi = {-1e18, -1e18}; // empty -> full IMDP |
| 39 | + |
| 40 | + auto ab = abstraction::buildSparseReachND(s, 1e-9); |
| 41 | + const int N = ab.nCells; |
| 42 | + |
| 43 | + std::vector<int> Nd(2); std::vector<long long> stride(2); long long NN = 1; |
| 44 | + for (int i = 0; i < 2; ++i) { Nd[i] = (int)std::llround((s.xub[i]-s.xlb[i])/s.eta[i]); stride[i]=NN; NN*=Nd[i]; } |
| 45 | + auto centre = [&](int lin, double& x0, double& x1) { |
| 46 | + int j0 = lin % Nd[0], j1 = (lin / Nd[0]) % Nd[1]; |
| 47 | + x0 = s.xlb[0] + (j0 + 0.5)*s.eta[0]; x1 = s.xlb[1] + (j1 + 0.5)*s.eta[1]; |
| 48 | + }; |
| 49 | + auto locate = [&](double x0, double x1, int& lin)->bool { |
| 50 | + int j0 = (int)std::floor((x0-s.xlb[0])/s.eta[0]), j1 = (int)std::floor((x1-s.xlb[1])/s.eta[1]); |
| 51 | + if (j0<0||j0>=Nd[0]||j1<0||j1>=Nd[1]) return false; lin = (int)(j0*stride[0]+j1*stride[1]); return true; |
| 52 | + }; |
| 53 | + |
| 54 | + // --- labels: pickup region [3,5]^2, deliver region [-5,-3]^2 (by cell centre) --- |
| 55 | + auto labelOf = [&](double x0, double x1) { |
| 56 | + ltl::Letter L; |
| 57 | + if (x0>=3 && x0<=5 && x1>=3 && x1<=5) L.insert("pickup"); |
| 58 | + if (x0>=-5 && x0<=-3 && x1>=-5 && x1<=-3) L.insert("deliver"); |
| 59 | + return L; |
| 60 | + }; |
| 61 | + std::vector<ltl::Letter> labels(ab.model.size()); // cells + TARGET + SINK |
| 62 | + for (int c = 0; c < N; ++c) { double x0,x1; centre(c,x0,x1); labels[c] = labelOf(x0,x1); } |
| 63 | + // TARGET (N) and SINK (N+1) carry no labels (already empty). |
| 64 | + |
| 65 | + // --- LTLf DFA + product + robust synthesis --- |
| 66 | + auto* aut = ltl::compileFinite("F(pickup & F deliver)", {"pickup", "deliver"}); |
| 67 | + ltl::DFA dfa = ltl::toDFA(aut); |
| 68 | + double cx0, cx1; (void)cx0; |
| 69 | + // pick a start far from both regions |
| 70 | + int startCell; locate(0.0, 0.0, startCell); |
| 71 | + auto P = product::build(ab.model, labels, dfa, startCell); |
| 72 | + auto val = solve::maxReachPessimistic(P.model, P.targets, 1e-6); |
| 73 | + |
| 74 | + // --- extract robust policy on the product --- |
| 75 | + std::vector<int> policy(P.model.size(), 0); |
| 76 | + for (size_t p = 0; p < P.model.size(); ++p) { |
| 77 | + double best = -1; int bi = 0; |
| 78 | + for (size_t a = 0; a < P.model[p].size(); ++a) { |
| 79 | + std::vector<double> lo, hi, V; |
| 80 | + for (const auto& iv : P.model[p][a]) { lo.push_back(iv.lo); hi.push_back(iv.hi); V.push_back(val.lower[iv.to]); } |
| 81 | + double q = omax::optimize(lo, hi, V, omax::Sense::Min).value; |
| 82 | + if (q > best) { best = q; bi = (int)a; } |
| 83 | + } |
| 84 | + policy[p] = bi; |
| 85 | + } |
| 86 | + const int nQ = dfa.nStates; |
| 87 | + |
| 88 | + // --- Monte-Carlo: simulate continuous loop, evaluate the realised trace --- |
| 89 | + std::mt19937 rng(11); |
| 90 | + std::normal_distribution<double> g(0.0, sd); |
| 91 | + const int TRIALS = 3000, HORIZON = 300; |
| 92 | + printf("Co-safe F(pickup & F deliver) over a continuous robot\n"); |
| 93 | + printf("%-12s %8s %10s %6s\n", "start", "lower", "empirical", "ok?"); |
| 94 | + double tests[][2] = { {0.0,0.0}, {-2.0,2.0}, {2.0,-2.0}, {0.0,3.0} }; |
| 95 | + int ok = 0, ntot = 0; |
| 96 | + for (auto& st : tests) { |
| 97 | + int c0; if (!locate(st[0], st[1], c0)) continue; |
| 98 | + int q0 = dfa.trans[dfa.start][ltl::letterIndex(dfa, labels[c0])]; |
| 99 | + int pstart = c0 * nQ + q0; |
| 100 | + double lower = val.lower[pstart]; |
| 101 | + int succ = 0; |
| 102 | + for (int t = 0; t < TRIALS; ++t) { |
| 103 | + double x0 = st[0], x1 = st[1]; |
| 104 | + int q = dfa.trans[dfa.start][ltl::letterIndex(dfa, labels[c0])]; |
| 105 | + bool done = false; |
| 106 | + for (int k = 0; k < HORIZON && !done; ++k) { |
| 107 | + if (dfa.accepting[q]) { ++succ; done = true; break; } |
| 108 | + int c; if (!locate(x0, x1, c)) { done = true; break; } // left grid |
| 109 | + int p = c * nQ + q; |
| 110 | + const auto& u = ab.actions[policy[p] < (int)ab.actions.size() ? policy[p] : 0]; |
| 111 | + x0 = 0.9*x0 + 1.4*u[0] + g(rng); |
| 112 | + x1 = 0.8*x1 + 1.4*u[1] + g(rng); |
| 113 | + int c2; if (!locate(x0, x1, c2)) { done = true; break; } |
| 114 | + q = dfa.trans[q][ltl::letterIndex(dfa, labels[c2])]; // DFA reads entered cell |
| 115 | + } |
| 116 | + } |
| 117 | + double emp = (double)succ / TRIALS; |
| 118 | + double ci = 1.96 * std::sqrt(std::max(emp*(1-emp),1e-9)/TRIALS); |
| 119 | + bool good = emp >= lower - ci - 1e-2; |
| 120 | + ok += good; ++ntot; |
| 121 | + printf("(%4.1f,%4.1f) %8.3f %8.3f %4s\n", st[0], st[1], lower, emp, good ? "yes":"NO"); |
| 122 | + } |
| 123 | + printf("\n%d/%d starts: empirical co-safe satisfaction >= robust lower bound.\n", ok, ntot); |
| 124 | + printf("cells=%d product_states=%zu dfa_states=%d nnz=%lld\n", N, P.model.size(), nQ, ab.nnz); |
| 125 | + ltl::destroy(aut); |
| 126 | + return ok == ntot ? 0 : 1; |
| 127 | +} |
0 commit comments