This notebook shows how to use CDD to compute controlled invariant sets for an hybrid system.
We consider the cruise_control.jl
example of HybridSystems.jl which comes from this paper.
include(Pkg.dir("HybridSystems", "examples", "cruise_control.jl"));
const va = 15.6
const vb = 24.5
const vc = 29.5
const v = (va, vb, vc)
const U = 4.
const m0 = 500
const T = 2
const N = 10
const M = 1
const H = 0.8;
macro _time(x)
quote
y = @timed $(esc(x))
# y[1] is returned value
# y[2] is time in seconds
y[2]
end
end
using Gurobi
lpsolver = GurobiSolver(OutputFlag=0);
function liftu(S, sys::HybridSystems.DiscreteLinearControlSystem)
[sys.A sys.B] \ S
end
function new_constraint(hs, S, q, t)
@assert source(hs, t) == q
σ = symbol(hs, t)
r = target(hs, t)
ABset = liftu(S[1], hs.resetmaps[σ])
project(ABset, 1:statedim(hs, q))
end
function new_constraints(hs, S, q)
map(t -> new_constraint(hs, S, q, t), out_transitions(hs, q))
end
function add_hrep!(S, h::HalfSpace)
# I was using LP cycling errors when using CDD's LP solver
if issubset(S, h) # If S ⊆ h, then adding h will not change affect S
false
else
push!(S, SimpleHRepresentation(reshape(h.a, 1, length(h.a)), [h.β]))
true
end
end
function add_constraint!(S, P)
added = count(map(h -> add_hrep!(S, h), ineqs(P))) + count(map(h -> add_hrep!(S, h), eqs(P)))
removehredundancy!(S) # CDD throws LP cycling error
added
end
function add_constraints!(S::Polyhedron, Ps::Vector{<:Polyhedron})
sum(P -> add_constraint!(S, P), Ps)
end
function set_iteration!(hs, S)
Ps = map(q -> new_constraints(hs, S, q), states(hs))
added = add_constraints!.(S, Ps)
@show added
end
function iterate!(hs, S, nit)
map(i -> (gc(); @_time set_iteration!(hs, S)), 1:nit)
end
Mmax = 1
nit = 2
t = zeros(Mmax, nit)
Hs = Vector{HybridSystem}(Mmax)
CIS = Vector{Vector{Polyhedron}}(Mmax)
for m in 1:Mmax
Hs[m] = cruise_control_example(N, m, vmin = 5., v=(va, vb, vc), U=U, H=H, sym=false, m0=500);
I0 = Hs[m].invariants;
@show nineqs(I0[1])
CIS[m] = deepcopy(I0);
@show m
t[m, :] = iterate!(Hs[m], CIS[m], nit)
@show t[m, :]
end
t
Mmax = size(t, 1)
nit = 1
previt = size(t, 2)
t = [t zeros(Mmax, nit)]
totit = size(t, 2)
for m in 1:Mmax
t[m, previt+(1:nit)] = iterate!(Hs[m], CIS[m], nit)
@show t[m, :]
end
t
import Plots
Plots.pyplot()
D = [1, 2]
Plots.plot(project(Hs[1].invariants[1], D))
Plots.plot!(project(CIS[1][1], D))
Plots.savefig("dist_trailerspeed.png")
t