From 9ddb1fcc0871773c4d1e5fa6359de318654debda Mon Sep 17 00:00:00 2001 From: johnaoga Date: Tue, 11 Jun 2024 10:44:15 +0200 Subject: [PATCH 1/7] power converter --- .gitignore | 4 +- docs/Project.toml | 1 + docs/src/examples/solvers/Power Converter.jl | 44 +++ problems/pc-osqp.jl | 354 +++++++++++++++++++ 4 files changed, 402 insertions(+), 1 deletion(-) create mode 100644 docs/src/examples/solvers/Power Converter.jl create mode 100644 problems/pc-osqp.jl diff --git a/.gitignore b/.gitignore index 9ea3e21b..f2535454 100644 --- a/.gitignore +++ b/.gitignore @@ -19,4 +19,6 @@ paper/paper.pdf *.out *.bbl *.synctex.gz -*.log \ No newline at end of file +*.log +Control Systems/v-infinite-horizon-pwer-converter/sdp.py +Control Systems/v-infinite-horizon-pwer-converter/vtailcost.py diff --git a/docs/Project.toml b/docs/Project.toml index 2bc30576..2db676b5 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -1,4 +1,5 @@ [deps] +BlockDiagonals = "0a1fb500-61f7-11e9-3c65-f5ef3456f9f0" CDDLib = "3391f64e-dcde-5f30-b752-e11513730f60" Clarabel = "61c947e1-3e6d-4ee4-985a-eec8c727bd6e" Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" diff --git a/docs/src/examples/solvers/Power Converter.jl b/docs/src/examples/solvers/Power Converter.jl new file mode 100644 index 00000000..8681e9c5 --- /dev/null +++ b/docs/src/examples/solvers/Power Converter.jl @@ -0,0 +1,44 @@ +using Test #src +# # Example: DC-DC converter solved by [Naive abstraction](https://github.com/dionysos-dev/Dionysos.jl/blob/master/docs/src/manual/manual.md#solvers). +# +#md # [![Binder](https://mybinder.org/badge_logo.svg)](@__BINDER_ROOT_URL__/generated/DC-DC converter.ipynb) +#md # [![nbviewer](https://img.shields.io/badge/show-nbviewer-579ACA.svg)](@__NBVIEWER_ROOT_URL__/generated/DC-DC converter.ipynb) +# +# We consider a boost DC-DC converter which has been widely studied from the point of view of hybrid control, see for example in [1, V.A],[2],[3]. +# This is a **safety problem** for a **switching system**. +# +# ![Boost DC-DC converter.](https://github.com/dionysos-dev/Dionysos.jl/blob/master/docs/assets/dcdcboost.jpg?raw=true) +# +# The state of the system is given by $x(t) = \begin{bmatrix} i_l(t) & v_c(t) \end{bmatrix}^\top$. +# The switching system has two modes consisting in two-dimensional affine dynamics: +# ```math +# \dot{x} = f_p(x) = A_p x + b_p,\quad p=1,2 +# ``` +# with +# ```math +# A_1 = \begin{bmatrix} -\frac{r_l}{x_l} &0 \\ 0 & -\frac{1}{x_c}\frac{1}{r_0+r_c} \end{bmatrix}, A_2= \begin{bmatrix} -\frac{1}{x_l}\left(r_l+\frac{r_0r_c}{r_0+r_c}\right) & -\frac{1}{x_l}\frac{r_0}{r_0+r_c} \\ \frac{1}{x_c}\frac{r_0}{r_0+r_c} & -\frac{1}{x_c}\frac{1}{r_0+r_c} \end{bmatrix}, b_1 = b_2 = \begin{bmatrix} \frac{v_s}{x_l}\\0\end{bmatrix}. +# ``` +# The goal is to design a controller to keep the state of the system in a safety region around the reference desired value, using as input only the switching +# signal. In order to study the concrete system and its symbolic abstraction in a unified framework, we will solve the problem +# for the sampled system with a sampling time $\tau$. For the construction of the relations in the abstraction, it is necessary to over-approximate attainable sets of +# a particular cell. In this example, we consider the use of a growth bound function [4, VIII.2, VIII.5] which is one of the possible methods to over-approximate +# attainable sets of a particular cell based on the state reach by its center. +# + +# First, let us import [StaticArrays](https://github.com/JuliaArrays/StaticArrays.jl) and [Plots](https://github.com/JuliaPlots/Plots.jl). + +using StaticArrays, Plots + +# At this point, we import the useful Dionysos sub-modules. +using Dionysos +const DI = Dionysos +const UT = DI.Utils +const DO = DI.Domain +const ST = DI.System +const SY = DI.Symbolic +const OP = DI.Optim +const AB = OP.Abstraction + +# ### Definition of the system +# we can import the module containing the DCDC problem like this +include(joinpath(dirname(dirname(pathof(Dionysos))), "problems", "pc-osqp.jl")) diff --git a/problems/pc-osqp.jl b/problems/pc-osqp.jl new file mode 100644 index 00000000..4409b05b --- /dev/null +++ b/problems/pc-osqp.jl @@ -0,0 +1,354 @@ +module PCOSQP +using Test +# First, let us import [StaticArrays](https://github.com/JuliaArrays/StaticArrays.jl). + +using StaticArrays +using MathematicalSystems +using LinearAlgebra +using BlockDiagonals + +# At this point, we import the useful Dionysos sub-module for this problem: +using Dionysos +const DI = Dionysos +const UT = DI.Utils +const DO = DI.Domain +const PB = DI.Problem +const ST = DI.System +const SY = DI.Symbolic + +# Define the structure for the ADP parameters: +struct ADPParameters + gamma::Float64 + N_adp::UnitRange{Int} + delta::Float64 + N_tail::Int + + function ADPParameters() + new(0.95, 1:5, 5.5, 50) + end +end + +# Define the structure for the simulation parameters: +struct SimulationParameters + Ts::Float64 + freq::Float64 + torque::Float64 + t0::Float64 + init_periods::Int + sim_periods::Int + flag_steady_trans::Int + + function SimulationParameters() + Ts = 25.0e-06 # Sampling time + freq = 50.0 # Switching frequency + torque = 1.0 # Desired torque + t0 = 0.0 # Initial time + init_periods = 1 # Number of periods to settle before simulation + sim_periods = 2 # Numer of simulated periods + flag_steady_trans = 0 # Flag Steady State (0) or Transients (1) + + new(Ts, freq, torque, t0, init_periods, sim_periods, flag_steady_trans) + end +end + +# Define the structure for the parameters: +struct Parameters + wr::Float64 + is::Float64 + Rs::Float64 + Rr::Float64 + Xls::Float64 + Xlr::Float64 + Xm::Float64 + Vdc::Float64 + xc::Float64 + Vrat::Float64 + Irat::Float64 + RealPower::Float64 + ApparentPower::Float64 + RotationalSpeed::Float64 + iVdc::Float64 + k1::Float64 + k2::Float64 + fsw_des::Float64 + kT::Float64 + P::SMatrix + invP::SMatrix + adp_params::ADPParameters + sim_params::SimulationParameters + + function Parameters() + ## P is a matrix of 2 rows and 3 columns with the following values: + Park = (2.0 / 3.0) * @SMatrix [1.0 -0.5 -0.5; 0.0 sqrt(3.0)/2.0 -sqrt(3.0)/2.0] + + invPark = @SMatrix [1.0 0.0; -0.5 sqrt(3.0)/2.0; -0.5 -sqrt(3.0)/2.0] + + new( + 0.9911, # wr - Nominal speed + 1.0, #is - pu + 0.0108, #Rs - pu + 0.0091, #Rr - pu + 0.1493, #Xls - pu + 0.1104, #Xlr - pu + 2.3489, #Xm - pu + 1.930, #Vdc - pu + 11.769, #xc - pu + 3300, #Vrat - Voltage - V + 356, #Irat - Current - A + 1.587, #RealPower - MW + 2.035, #ApparentPower - MVA + 596, #RotationalSpeed - rpm + 5.2 * 1000, #iVdc - kV + #switching filter parameters + 0.8e03, #k1 + 0.8e03, #k2 + 300, #fsw_des: desired switching frequency + # Torque constant to fix [pu] units + 1.2361, #kT + # Define Park transformation and its inverse + Park, # P + invPark, # invP + ADPParameters(), # ADP parameters + SimulationParameters() # Simulation parameters + ) + end +end + +# Define the structure for the derived parameters: +struct DerivedParameters + Vb::Float64 + Ib::Float64 + Fb::Float64 + Xs::Float64 + Xr::Float64 + taus::Float64 # tau of the stator + taur::Float64 # tau of the rotor + Dval::Float64 + Tspu::Float64 + omegar::Float64 + F::SMatrix + G_first::SMatrix + A_phys::SMatrix + B_phys::SMatrix + A_osc::SMatrix + B_osc::SMatrix + A_prev::SMatrix + B_prev::SMatrix + a1::Float64 + a2::Float64 + A_sw::SMatrix + B_sw::SMatrix + A_fsw::SMatrix + A::SMatrix + B::SMatrix + C::SMatrix + W::SMatrix + G::SMatrix + T::SMatrix + M::SMatrix + + function DerivedParameters(this::Parameters) + # Definition of the parameters of the system: + + #- From [9].sect I + Vb = sqrt(1.0 / 3.0) * this.Vrat + Ib = sqrt(2) * this.Irat + Fb = this.sim_params.freq + #- From [9].sect II.B + ## Inductances + Xs = this.Xls + this.Xm + Xr = this.Xlr + this.Xm + Dval = 0.6266 # Xs * Xr - this.Xm^2 + + ## Time constants + Tau_s = Xr * Dval / (this.Rs * Xr^2 + this.Rr * this.Xm^2) # not sure about this + Tau_r = Xr / this.Rr + + Tspu = this.sim_params.Ts * this.wr + omega_r = this.wr + + # Physical system matrices + F = @SMatrix [ -1.0/Tau_s 0.0 this.Xm/(Tau_r*Dval) omega_r * this.Xm/Dval; + 0.0 -1.0/Tau_s -omega_r * this.Xm/Dval this.Xm/(Tau_r*Dval); + this.Xm/Tau_r 0.0 -1.0/Tau_r -omega_r; + 0.0 this.Xm/Tau_r omega_r -1.0/Tau_r] + + # Define the matrix G + Gtmp = @SMatrix [1.0 0.0; 0.0 1; 0.0 0.0; 0.0 0.0] + G_first = (Xr / Dval * this.Vdc / 2.0) * Gtmp * this.P + + ##using LinearAlgebra, StaticArrays + + # Discretize physical system + A_phys = exp(F .* Tspu) + B_phys = -(inv(F) * (I - A_phys) * G_first) + + # Concatenate oscillating states + A_osc = @SMatrix [cos(Tspu) -sin(Tspu); + sin(Tspu) cos(Tspu)] + B_osc = zeros(Float64, 2, 3) + + # Concatenate previous input as a state + A_prev = zeros(Float64, 3, 3) + B_prev = I(3) + + # Concatenate filter states + a1 = 1.0 - 1.0 / this.k1 + a2 = 1.0 - 1.0 / this.k2 + + A_sw = @SMatrix [a1 0.0; (1.0-a1) a2] + B_sw = (1.0 / this.fsw_des * 1.0 / 12 * (1 - a1) / this.sim_params.Ts) * @SMatrix [1.0 1.0 1.0; 0.0 0.0 0.0] + + # Concatenate switching frequency state + A_fsw = @SMatrix [1.0] + + # Generate complete system + A = BlockDiagonal([A_phys, A_osc, A_prev, A_sw, A_fsw]) + B = [B_phys zeros(size(B_phys, 1), 3); + B_osc zeros(2, 3); + B_prev zeros(3, 3); + zeros(2, 3) B_sw; + zeros(1, 6)] + + C = @SMatrix [1.0 0.0 0.0 0.0 -1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0; + 0.0 1.0 0.0 0.0 0.0 -1.0 0.0 0.0 0.0 0.0 0.0 0.0; + 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 -this.adp_params.delta this.adp_params.delta] + + # Extract previous input from data + W = [zeros(Float64, 3, 6) I(3) zeros(Float64, 3, 3)] + + # Matrix to extract original input and auxiliary one from state + G = [I(3) zeros(Float64, 3, 3)] + T = [zeros(Float64, 3, 3) I(3)] + + # Generate matrix of possible input combinations + M = zeros(Float64, 3, 27) + M[1, :] = [fill(-1.0, 9); fill(0.0, 9); fill(1.0, 9)] + M[2, :] = repeat([fill(-1.0, 3); fill(0.0, 3); fill(1.0, 3)], outer = [1, 3]) + M[3, :] = repeat([-1.0, 0.0, 1.0], inner = [1], outer = [9]) + + new(Vb, Ib, Fb, Xs, Xr, Tau_s, Tau_r, Dval, Tspu, omega_r, F, G_first, A_phys, B_phys, A_osc, B_osc, A_prev, B_prev, a1, a2, A_sw, B_sw, A_fsw, A, B, C, W, G, T, M) + + #new(Vb, Ib, Fb, Xs, Xr, Tau_s, Tau_r, Dval, Tspu, omega_r, F, G_first, A_phys, B_phys, A_osc, B_osc, A_prev, B_prev, a1, a2, A_sw, B_sw, A_fsw, A, B, C, W, G, T, M) + end +end + +# Define the structure for the simulation results: +struct SimulationResults + X::Any + U::Any + Y_phase::Any + Y_star_phase::Any + T_e::Any + T_e_des::Any + solve_times::Any + + function SimulationResults(X, U, Y_phase, Y_star_phase, T_e, T_e_des, solve_times) + new(X, U, Y_phase, Y_star_phase, T_e, T_e_des, solve_times) + end +end + +function Base.show(io::IO, emp::SimulationResults) + println(io, "SimulationResults") + println(io, "X: ", emp.X) + println(io, "U: ", emp.U) + println(io, "Y_phase: ", emp.Y_phase) + println(io, "Y_star_phase: ", emp.Y_star_phase) + println(io, "T_e: ", emp.T_e) + println(io, "T_e_des: ", emp.T_e_des) + println(io, "solve_times: ", emp.solve_times) +end + +test_parameters = Parameters() +test_derived_parameters = DerivedParameters(test_parameters) + + +function dynamicofsystem(params::Parameters = test_parameters, dparams::DerivedParameters = test_derived_parameters) + + # Get parameters + taus = dparams.taus + taur = params.taur + D = params.Dval + omegar = params.wr + Vdc = params.Vdc + Xm = params.Xm + Xr = dparams.Xr + P = params.P + Tspu = dparams.Tspu + k1 = params.k1 + k2 = params.k2 + Ts = params.sim_params.Ts + + # Define the system dynamics + + # Definition of the growth bound function of $f$: + A2_abs = SMatrix{2, 2}( + -(rL + r0 * rC / (r0 + rC)) / xL, + 5.0 * r0 / (r0 + rC) / xC, + r0 / (r0 + rC) / xL / 5.0, + -1.0 / xC / (r0 + rC), + ) + + L_growthbound = let A1 = A1, A2_abs = A2_abs + u -> u[1] == 1 ? A1 : A2_abs + end + + DF_sys = let A1 = A1, A2 = A2 + (x, u) -> u[1] == 1 ? A1 : A2 + end + bound_DF(u) = 0.0 + bound_DDF(u) = 0.0 + return F_sys, L_growthbound, ngrowthbound, DF_sys, bound_DF, bound_DDF +end + +function system(; + sysnoise = SVector(0.0, 0.0), + measnoise = SVector(0.0, 0.0), + tstep = 0.5, + nsys = 5, + _X_ = UT.HyperRectangle(SVector(1.15, 5.45), SVector(1.55, 5.85)), + _U_ = UT.HyperRectangle(SVector(1), SVector(2)), + xdim = 2, + udim = 1, + approx_mode = "growth", +) + + # Definition of the dynamics functions $f_p$ of the system: + F_sys, L_growthbound, ngrowthbound, DF_sys, bound_DF, bound_DDF = dynamicofsystem() + contsys = nothing + if approx_mode == "growth" + contsys = ST.NewControlSystemGrowthRK4( + tstep, + F_sys, + L_growthbound, + sysnoise, + measnoise, + nsys, + ngrowthbound, + ) + elseif approx_mode == "linearized" + contsys = ST.NewControlSystemLinearizedRK4( + tstep, + F_sys, + DF_sys, + bound_DF, + bound_DDF, + measnoise, + nsys, + ) + end + return MathematicalSystems.ConstrainedBlackBoxControlContinuousSystem( + contsys, + xdim, + udim, + _X_, + _U_, + ) +end + +function problem(; approx_mode = "growth") + sys = system(; approx_mode = approx_mode) + return PB.SafetyProblem(sys, sys.X, sys.X, PB.Infinity()) +end + +end From 17c9883f2e32cba2aaf893dedfe3d4b7144cc217 Mon Sep 17 00:00:00 2001 From: johnaoga Date: Wed, 12 Jun 2024 10:43:28 +0200 Subject: [PATCH 2/7] update the proj pom --- Project.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/Project.toml b/Project.toml index 6a85d72c..bc7e8687 100644 --- a/Project.toml +++ b/Project.toml @@ -5,6 +5,7 @@ version = "0.1.2" [deps] ArnoldiMethod = "ec485272-7323-5ecc-a04f-4719b315124d" +BlockDiagonals = "0a1fb500-61f7-11e9-3c65-f5ef3456f9f0" Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" DiscreteMarkovChains = "8abcb7ef-b365-4f7b-ac38-56893fb62f9f" From 5d1f172a18cafcee070f0a4b3997b34db5b30eeb Mon Sep 17 00:00:00 2001 From: johnaoga Date: Fri, 14 Jun 2024 12:17:13 +0200 Subject: [PATCH 3/7] update the proj pom --- Project.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/Project.toml b/Project.toml index bc7e8687..9f80e670 100644 --- a/Project.toml +++ b/Project.toml @@ -19,6 +19,7 @@ IntervalArithmetic = "d1acc4aa-44c8-5952-acd4-ba5d80a2a253" IntervalLinearAlgebra = "92cbe1ac-9c24-436b-b0c9-5f7317aedcd5" JuMP = "4076af6c-e467-56ae-b986-b466b2749572" LaTeXStrings = "b964fa9f-0449-5b57-a5c2-d3ea65f4040f" +LatexPrint = "d2208f48-c256-5759-9089-c25ed2a93924" LazySets = "b4f0291d-fe17-52bc-9479-3d1a343d9043" Libtiff_jll = "89763e89-9b03-5906-acba-b20f662cd828" LightXML = "9c8b4983-aa76-5018-a973-4c85ecc9e179" From 96b9e52adbf0d358ed06bd59ca81db2a9208c802 Mon Sep 17 00:00:00 2001 From: johnaoga Date: Fri, 14 Jun 2024 12:20:30 +0200 Subject: [PATCH 4/7] jformat --- problems/pc-osqp.jl | 619 +++++++++++++++++++++++--------------------- 1 file changed, 331 insertions(+), 288 deletions(-) diff --git a/problems/pc-osqp.jl b/problems/pc-osqp.jl index 4409b05b..3bb9aaa6 100644 --- a/problems/pc-osqp.jl +++ b/problems/pc-osqp.jl @@ -18,19 +18,19 @@ const SY = DI.Symbolic # Define the structure for the ADP parameters: struct ADPParameters - gamma::Float64 - N_adp::UnitRange{Int} - delta::Float64 - N_tail::Int - - function ADPParameters() - new(0.95, 1:5, 5.5, 50) - end + gamma::Float64 + N_adp::UnitRange{Int} + delta::Float64 + N_tail::Int + + function ADPParameters() + return new(0.95, 1:5, 5.5, 50) + end end # Define the structure for the simulation parameters: struct SimulationParameters - Ts::Float64 + Ts::Float64 freq::Float64 torque::Float64 t0::Float64 @@ -38,317 +38,360 @@ struct SimulationParameters sim_periods::Int flag_steady_trans::Int - function SimulationParameters() - Ts = 25.0e-06 # Sampling time - freq = 50.0 # Switching frequency - torque = 1.0 # Desired torque - t0 = 0.0 # Initial time - init_periods = 1 # Number of periods to settle before simulation - sim_periods = 2 # Numer of simulated periods - flag_steady_trans = 0 # Flag Steady State (0) or Transients (1) - - new(Ts, freq, torque, t0, init_periods, sim_periods, flag_steady_trans) - end + function SimulationParameters() + Ts = 25.0e-06 # Sampling time + freq = 50.0 # Switching frequency + torque = 1.0 # Desired torque + t0 = 0.0 # Initial time + init_periods = 1 # Number of periods to settle before simulation + sim_periods = 2 # Numer of simulated periods + flag_steady_trans = 0 # Flag Steady State (0) or Transients (1) + + return new(Ts, freq, torque, t0, init_periods, sim_periods, flag_steady_trans) + end end # Define the structure for the parameters: struct Parameters - wr::Float64 - is::Float64 - Rs::Float64 - Rr::Float64 - Xls::Float64 - Xlr::Float64 - Xm::Float64 - Vdc::Float64 - xc::Float64 - Vrat::Float64 - Irat::Float64 - RealPower::Float64 - ApparentPower::Float64 - RotationalSpeed::Float64 - iVdc::Float64 - k1::Float64 - k2::Float64 - fsw_des::Float64 - kT::Float64 - P::SMatrix - invP::SMatrix - adp_params::ADPParameters + wr::Float64 + is::Float64 + Rs::Float64 + Rr::Float64 + Xls::Float64 + Xlr::Float64 + Xm::Float64 + Vdc::Float64 + xc::Float64 + Vrat::Float64 + Irat::Float64 + RealPower::Float64 + ApparentPower::Float64 + RotationalSpeed::Float64 + iVdc::Float64 + k1::Float64 + k2::Float64 + fsw_des::Float64 + kT::Float64 + P::SMatrix + invP::SMatrix + adp_params::ADPParameters sim_params::SimulationParameters - function Parameters() - ## P is a matrix of 2 rows and 3 columns with the following values: - Park = (2.0 / 3.0) * @SMatrix [1.0 -0.5 -0.5; 0.0 sqrt(3.0)/2.0 -sqrt(3.0)/2.0] - - invPark = @SMatrix [1.0 0.0; -0.5 sqrt(3.0)/2.0; -0.5 -sqrt(3.0)/2.0] - - new( - 0.9911, # wr - Nominal speed - 1.0, #is - pu - 0.0108, #Rs - pu - 0.0091, #Rr - pu - 0.1493, #Xls - pu - 0.1104, #Xlr - pu - 2.3489, #Xm - pu - 1.930, #Vdc - pu - 11.769, #xc - pu - 3300, #Vrat - Voltage - V - 356, #Irat - Current - A - 1.587, #RealPower - MW - 2.035, #ApparentPower - MVA - 596, #RotationalSpeed - rpm - 5.2 * 1000, #iVdc - kV - #switching filter parameters - 0.8e03, #k1 - 0.8e03, #k2 - 300, #fsw_des: desired switching frequency - # Torque constant to fix [pu] units - 1.2361, #kT - # Define Park transformation and its inverse - Park, # P - invPark, # invP - ADPParameters(), # ADP parameters - SimulationParameters() # Simulation parameters - ) - end + function Parameters() + ## P is a matrix of 2 rows and 3 columns with the following values: + Park = (2.0 / 3.0) * @SMatrix [1.0 -0.5 -0.5; 0.0 sqrt(3.0)/2.0 -sqrt(3.0)/2.0] + + invPark = @SMatrix [1.0 0.0; -0.5 sqrt(3.0)/2.0; -0.5 -sqrt(3.0)/2.0] + + return new( + 0.9911, # wr - Nominal speed + 1.0, #is - pu + 0.0108, #Rs - pu + 0.0091, #Rr - pu + 0.1493, #Xls - pu + 0.1104, #Xlr - pu + 2.3489, #Xm - pu + 1.930, #Vdc - pu + 11.769, #xc - pu + 3300, #Vrat - Voltage - V + 356, #Irat - Current - A + 1.587, #RealPower - MW + 2.035, #ApparentPower - MVA + 596, #RotationalSpeed - rpm + 5.2 * 1000, #iVdc - kV + #switching filter parameters + 0.8e03, #k1 + 0.8e03, #k2 + 300, #fsw_des: desired switching frequency + # Torque constant to fix [pu] units + 1.2361, #kT + # Define Park transformation and its inverse + Park, # P + invPark, # invP + ADPParameters(), # ADP parameters + SimulationParameters(), # Simulation parameters + ) + end end # Define the structure for the derived parameters: struct DerivedParameters - Vb::Float64 - Ib::Float64 - Fb::Float64 - Xs::Float64 - Xr::Float64 - taus::Float64 # tau of the stator - taur::Float64 # tau of the rotor - Dval::Float64 + Vb::Float64 + Ib::Float64 + Fb::Float64 + Xs::Float64 + Xr::Float64 + taus::Float64 # tau of the stator + taur::Float64 # tau of the rotor + Dval::Float64 Tspu::Float64 - omegar::Float64 - F::SMatrix - G_first::SMatrix - A_phys::SMatrix - B_phys::SMatrix - A_osc::SMatrix - B_osc::SMatrix - A_prev::SMatrix - B_prev::SMatrix - a1::Float64 - a2::Float64 - A_sw::SMatrix - B_sw::SMatrix - A_fsw::SMatrix - A::SMatrix - B::SMatrix - C::SMatrix - W::SMatrix - G::SMatrix - T::SMatrix - M::SMatrix - - function DerivedParameters(this::Parameters) - # Definition of the parameters of the system: - - #- From [9].sect I - Vb = sqrt(1.0 / 3.0) * this.Vrat - Ib = sqrt(2) * this.Irat - Fb = this.sim_params.freq - #- From [9].sect II.B - ## Inductances - Xs = this.Xls + this.Xm - Xr = this.Xlr + this.Xm - Dval = 0.6266 # Xs * Xr - this.Xm^2 - - ## Time constants - Tau_s = Xr * Dval / (this.Rs * Xr^2 + this.Rr * this.Xm^2) # not sure about this - Tau_r = Xr / this.Rr - - Tspu = this.sim_params.Ts * this.wr - omega_r = this.wr - - # Physical system matrices - F = @SMatrix [ -1.0/Tau_s 0.0 this.Xm/(Tau_r*Dval) omega_r * this.Xm/Dval; - 0.0 -1.0/Tau_s -omega_r * this.Xm/Dval this.Xm/(Tau_r*Dval); - this.Xm/Tau_r 0.0 -1.0/Tau_r -omega_r; - 0.0 this.Xm/Tau_r omega_r -1.0/Tau_r] - - # Define the matrix G - Gtmp = @SMatrix [1.0 0.0; 0.0 1; 0.0 0.0; 0.0 0.0] - G_first = (Xr / Dval * this.Vdc / 2.0) * Gtmp * this.P - - ##using LinearAlgebra, StaticArrays - - # Discretize physical system - A_phys = exp(F .* Tspu) - B_phys = -(inv(F) * (I - A_phys) * G_first) - - # Concatenate oscillating states - A_osc = @SMatrix [cos(Tspu) -sin(Tspu); - sin(Tspu) cos(Tspu)] - B_osc = zeros(Float64, 2, 3) - - # Concatenate previous input as a state - A_prev = zeros(Float64, 3, 3) - B_prev = I(3) - - # Concatenate filter states - a1 = 1.0 - 1.0 / this.k1 - a2 = 1.0 - 1.0 / this.k2 - - A_sw = @SMatrix [a1 0.0; (1.0-a1) a2] - B_sw = (1.0 / this.fsw_des * 1.0 / 12 * (1 - a1) / this.sim_params.Ts) * @SMatrix [1.0 1.0 1.0; 0.0 0.0 0.0] - - # Concatenate switching frequency state - A_fsw = @SMatrix [1.0] - - # Generate complete system - A = BlockDiagonal([A_phys, A_osc, A_prev, A_sw, A_fsw]) - B = [B_phys zeros(size(B_phys, 1), 3); - B_osc zeros(2, 3); - B_prev zeros(3, 3); - zeros(2, 3) B_sw; - zeros(1, 6)] - - C = @SMatrix [1.0 0.0 0.0 0.0 -1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0; - 0.0 1.0 0.0 0.0 0.0 -1.0 0.0 0.0 0.0 0.0 0.0 0.0; - 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 -this.adp_params.delta this.adp_params.delta] - - # Extract previous input from data - W = [zeros(Float64, 3, 6) I(3) zeros(Float64, 3, 3)] - - # Matrix to extract original input and auxiliary one from state - G = [I(3) zeros(Float64, 3, 3)] - T = [zeros(Float64, 3, 3) I(3)] - - # Generate matrix of possible input combinations - M = zeros(Float64, 3, 27) - M[1, :] = [fill(-1.0, 9); fill(0.0, 9); fill(1.0, 9)] - M[2, :] = repeat([fill(-1.0, 3); fill(0.0, 3); fill(1.0, 3)], outer = [1, 3]) - M[3, :] = repeat([-1.0, 0.0, 1.0], inner = [1], outer = [9]) - - new(Vb, Ib, Fb, Xs, Xr, Tau_s, Tau_r, Dval, Tspu, omega_r, F, G_first, A_phys, B_phys, A_osc, B_osc, A_prev, B_prev, a1, a2, A_sw, B_sw, A_fsw, A, B, C, W, G, T, M) - - #new(Vb, Ib, Fb, Xs, Xr, Tau_s, Tau_r, Dval, Tspu, omega_r, F, G_first, A_phys, B_phys, A_osc, B_osc, A_prev, B_prev, a1, a2, A_sw, B_sw, A_fsw, A, B, C, W, G, T, M) - end + omegar::Float64 + F::SMatrix + G_first::SMatrix + A_phys::SMatrix + B_phys::SMatrix + A_osc::SMatrix + B_osc::SMatrix + A_prev::SMatrix + B_prev::SMatrix + a1::Float64 + a2::Float64 + A_sw::SMatrix + B_sw::SMatrix + A_fsw::SMatrix + A::SMatrix + B::SMatrix + C::SMatrix + W::SMatrix + G::SMatrix + T::SMatrix + M::SMatrix + + function DerivedParameters(this::Parameters) + # Definition of the parameters of the system: + + #- From [9].sect I + Vb = sqrt(1.0 / 3.0) * this.Vrat + Ib = sqrt(2) * this.Irat + Fb = this.sim_params.freq + #- From [9].sect II.B + ## Inductances + Xs = this.Xls + this.Xm + Xr = this.Xlr + this.Xm + Dval = 0.6266 # Xs * Xr - this.Xm^2 + + ## Time constants + Tau_s = Xr * Dval / (this.Rs * Xr^2 + this.Rr * this.Xm^2) # not sure about this + Tau_r = Xr / this.Rr + + Tspu = this.sim_params.Ts * this.wr + omega_r = this.wr + + # Physical system matrices + F = @SMatrix [ + -1.0/Tau_s 0.0 this.Xm/(Tau_r * Dval) omega_r * this.Xm/Dval + 0.0 -1.0/Tau_s -omega_r * this.Xm/Dval this.Xm/(Tau_r * Dval) + this.Xm/Tau_r 0.0 -1.0/Tau_r -omega_r + 0.0 this.Xm/Tau_r omega_r -1.0/Tau_r + ] + + # Define the matrix G + Gtmp = @SMatrix [1.0 0.0; 0.0 1; 0.0 0.0; 0.0 0.0] + G_first = (Xr / Dval * this.Vdc / 2.0) * Gtmp * this.P + + ##using LinearAlgebra, StaticArrays + + # Discretize physical system + A_phys = exp(F .* Tspu) + B_phys = -(inv(F) * (I - A_phys) * G_first) + + # Concatenate oscillating states + A_osc = @SMatrix [ + cos(Tspu) -sin(Tspu) + sin(Tspu) cos(Tspu) + ] + B_osc = zeros(Float64, 2, 3) + + # Concatenate previous input as a state + A_prev = zeros(Float64, 3, 3) + B_prev = I(3) + + # Concatenate filter states + a1 = 1.0 - 1.0 / this.k1 + a2 = 1.0 - 1.0 / this.k2 + + A_sw = @SMatrix [a1 0.0; (1.0-a1) a2] + B_sw = + (1.0 / this.fsw_des * 1.0 / 12 * (1 - a1) / this.sim_params.Ts) * + @SMatrix [1.0 1.0 1.0; 0.0 0.0 0.0] + + # Concatenate switching frequency state + A_fsw = @SMatrix [1.0] + + # Generate complete system + A = BlockDiagonal([A_phys, A_osc, A_prev, A_sw, A_fsw]) + B = [ + B_phys zeros(size(B_phys, 1), 3) + B_osc zeros(2, 3) + B_prev zeros(3, 3) + zeros(2, 3) B_sw + zeros(1, 6) + ] + + C = @SMatrix [ + 1.0 0.0 0.0 0.0 -1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 0.0 -1.0 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 -this.adp_params.delta this.adp_params.delta + ] + + # Extract previous input from data + W = [zeros(Float64, 3, 6) I(3) zeros(Float64, 3, 3)] + + # Matrix to extract original input and auxiliary one from state + G = [I(3) zeros(Float64, 3, 3)] + T = [zeros(Float64, 3, 3) I(3)] + + # Generate matrix of possible input combinations + M = zeros(Float64, 3, 27) + M[1, :] = [fill(-1.0, 9); fill(0.0, 9); fill(1.0, 9)] + M[2, :] = repeat([fill(-1.0, 3); fill(0.0, 3); fill(1.0, 3)]; outer = [1, 3]) + M[3, :] = repeat([-1.0, 0.0, 1.0]; inner = [1], outer = [9]) + + return new( + Vb, + Ib, + Fb, + Xs, + Xr, + Tau_s, + Tau_r, + Dval, + Tspu, + omega_r, + F, + G_first, + A_phys, + B_phys, + A_osc, + B_osc, + A_prev, + B_prev, + a1, + a2, + A_sw, + B_sw, + A_fsw, + A, + B, + C, + W, + G, + T, + M, + ) + + #new(Vb, Ib, Fb, Xs, Xr, Tau_s, Tau_r, Dval, Tspu, omega_r, F, G_first, A_phys, B_phys, A_osc, B_osc, A_prev, B_prev, a1, a2, A_sw, B_sw, A_fsw, A, B, C, W, G, T, M) + end end # Define the structure for the simulation results: struct SimulationResults - X::Any - U::Any - Y_phase::Any - Y_star_phase::Any - T_e::Any - T_e_des::Any - solve_times::Any - - function SimulationResults(X, U, Y_phase, Y_star_phase, T_e, T_e_des, solve_times) - new(X, U, Y_phase, Y_star_phase, T_e, T_e_des, solve_times) - end + X::Any + U::Any + Y_phase::Any + Y_star_phase::Any + T_e::Any + T_e_des::Any + solve_times::Any + + function SimulationResults(X, U, Y_phase, Y_star_phase, T_e, T_e_des, solve_times) + return new(X, U, Y_phase, Y_star_phase, T_e, T_e_des, solve_times) + end end function Base.show(io::IO, emp::SimulationResults) - println(io, "SimulationResults") - println(io, "X: ", emp.X) - println(io, "U: ", emp.U) - println(io, "Y_phase: ", emp.Y_phase) - println(io, "Y_star_phase: ", emp.Y_star_phase) - println(io, "T_e: ", emp.T_e) - println(io, "T_e_des: ", emp.T_e_des) - println(io, "solve_times: ", emp.solve_times) + println(io, "SimulationResults") + println(io, "X: ", emp.X) + println(io, "U: ", emp.U) + println(io, "Y_phase: ", emp.Y_phase) + println(io, "Y_star_phase: ", emp.Y_star_phase) + println(io, "T_e: ", emp.T_e) + println(io, "T_e_des: ", emp.T_e_des) + return println(io, "solve_times: ", emp.solve_times) end test_parameters = Parameters() test_derived_parameters = DerivedParameters(test_parameters) +function dynamicofsystem( + params::Parameters = test_parameters, + dparams::DerivedParameters = test_derived_parameters, +) -function dynamicofsystem(params::Parameters = test_parameters, dparams::DerivedParameters = test_derived_parameters) - - # Get parameters - taus = dparams.taus - taur = params.taur - D = params.Dval - omegar = params.wr - Vdc = params.Vdc - Xm = params.Xm - Xr = dparams.Xr - P = params.P - Tspu = dparams.Tspu - k1 = params.k1 - k2 = params.k2 - Ts = params.sim_params.Ts - - # Define the system dynamics - - # Definition of the growth bound function of $f$: - A2_abs = SMatrix{2, 2}( - -(rL + r0 * rC / (r0 + rC)) / xL, - 5.0 * r0 / (r0 + rC) / xC, - r0 / (r0 + rC) / xL / 5.0, - -1.0 / xC / (r0 + rC), - ) - - L_growthbound = let A1 = A1, A2_abs = A2_abs - u -> u[1] == 1 ? A1 : A2_abs - end - - DF_sys = let A1 = A1, A2 = A2 - (x, u) -> u[1] == 1 ? A1 : A2 - end - bound_DF(u) = 0.0 - bound_DDF(u) = 0.0 - return F_sys, L_growthbound, ngrowthbound, DF_sys, bound_DF, bound_DDF + # Get parameters + taus = dparams.taus + taur = params.taur + D = params.Dval + omegar = params.wr + Vdc = params.Vdc + Xm = params.Xm + Xr = dparams.Xr + P = params.P + Tspu = dparams.Tspu + k1 = params.k1 + k2 = params.k2 + Ts = params.sim_params.Ts + + # Define the system dynamics + + # Definition of the growth bound function of $f$: + A2_abs = SMatrix{2, 2}( + -(rL + r0 * rC / (r0 + rC)) / xL, + 5.0 * r0 / (r0 + rC) / xC, + r0 / (r0 + rC) / xL / 5.0, + -1.0 / xC / (r0 + rC), + ) + + L_growthbound = let A1 = A1, A2_abs = A2_abs + u -> u[1] == 1 ? A1 : A2_abs + end + + DF_sys = let A1 = A1, A2 = A2 + (x, u) -> u[1] == 1 ? A1 : A2 + end + bound_DF(u) = 0.0 + bound_DDF(u) = 0.0 + return F_sys, L_growthbound, ngrowthbound, DF_sys, bound_DF, bound_DDF end function system(; - sysnoise = SVector(0.0, 0.0), - measnoise = SVector(0.0, 0.0), - tstep = 0.5, - nsys = 5, - _X_ = UT.HyperRectangle(SVector(1.15, 5.45), SVector(1.55, 5.85)), - _U_ = UT.HyperRectangle(SVector(1), SVector(2)), - xdim = 2, - udim = 1, - approx_mode = "growth", + sysnoise = SVector(0.0, 0.0), + measnoise = SVector(0.0, 0.0), + tstep = 0.5, + nsys = 5, + _X_ = UT.HyperRectangle(SVector(1.15, 5.45), SVector(1.55, 5.85)), + _U_ = UT.HyperRectangle(SVector(1), SVector(2)), + xdim = 2, + udim = 1, + approx_mode = "growth", ) - # Definition of the dynamics functions $f_p$ of the system: - F_sys, L_growthbound, ngrowthbound, DF_sys, bound_DF, bound_DDF = dynamicofsystem() - contsys = nothing - if approx_mode == "growth" - contsys = ST.NewControlSystemGrowthRK4( - tstep, - F_sys, - L_growthbound, - sysnoise, - measnoise, - nsys, - ngrowthbound, - ) - elseif approx_mode == "linearized" - contsys = ST.NewControlSystemLinearizedRK4( - tstep, - F_sys, - DF_sys, - bound_DF, - bound_DDF, - measnoise, - nsys, - ) - end - return MathematicalSystems.ConstrainedBlackBoxControlContinuousSystem( - contsys, - xdim, - udim, - _X_, - _U_, - ) + # Definition of the dynamics functions $f_p$ of the system: + F_sys, L_growthbound, ngrowthbound, DF_sys, bound_DF, bound_DDF = dynamicofsystem() + contsys = nothing + if approx_mode == "growth" + contsys = ST.NewControlSystemGrowthRK4( + tstep, + F_sys, + L_growthbound, + sysnoise, + measnoise, + nsys, + ngrowthbound, + ) + elseif approx_mode == "linearized" + contsys = ST.NewControlSystemLinearizedRK4( + tstep, + F_sys, + DF_sys, + bound_DF, + bound_DDF, + measnoise, + nsys, + ) + end + return MathematicalSystems.ConstrainedBlackBoxControlContinuousSystem( + contsys, + xdim, + udim, + _X_, + _U_, + ) end function problem(; approx_mode = "growth") - sys = system(; approx_mode = approx_mode) - return PB.SafetyProblem(sys, sys.X, sys.X, PB.Infinity()) + sys = system(; approx_mode = approx_mode) + return PB.SafetyProblem(sys, sys.X, sys.X, PB.Infinity()) end end From d56026dc27c30d3f896e8b04b2f0214f5bb30e9e Mon Sep 17 00:00:00 2001 From: johnaoga Date: Thu, 27 Jun 2024 15:31:57 +0200 Subject: [PATCH 5/7] implementing the resetmaps --- problems/pc-osqp-rmaps.jl | 177 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 177 insertions(+) create mode 100644 problems/pc-osqp-rmaps.jl diff --git a/problems/pc-osqp-rmaps.jl b/problems/pc-osqp-rmaps.jl new file mode 100644 index 00000000..d5a48e2a --- /dev/null +++ b/problems/pc-osqp-rmaps.jl @@ -0,0 +1,177 @@ +module PCOSQPRM +using Test + +using Dionysos: Dionysos +const DI = Dionysos +const UT = DI.Utils +const PR = DI.Problem + +using FillArrays +using Polyhedra +using MathematicalSystems, HybridSystems +using SemialgebraicSets +using LinearAlgebra + +using CDDLib: CDDLib + +function system(lib, T::Type) + + # Generate the automaton nodes + possible_values = [[-1, 0, 1], [-1, 0, 1], [-1, 0, 1], [0, 1], [0, 1], [0, 1]] + nvars = length(possible_values) + + ## generate all possible combinations of the values + function generate_combinations(possible_values, depth) + if depth == 1 + return [[v] for v in possible_values[1]] + end + return [ + [v; c...] for v in possible_values[1] for c in generate_combinations(possible_values[2:end], depth - 1) + ] + end + + ## generate all possible combinations of the values + nodes = generate_combinations(possible_values, nvars) + nnodes = length(nodes) + + domains = [] + + #Automaton: + automaton = GraphAutomaton(nnodes) + indexB = Int[] + + #Resetmaps: (guards + reset) + A = T[ + 0.9999981424449595 9.91220860939534e-12 3.4481569821196496e-7 9.205526977551878e-5 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 + -9.91220860939534e-12 0.9999981424449595 -9.205526977551878e-5 3.4481569821196496e-7 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 + 2.1535327064414276e-7 -2.644213073804883e-12 0.9999999080158245 -2.455696808386769e-5 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 + 2.644213073804883e-12 2.1535327064414276e-7 2.455696808386769e-5 0.9999999080158245 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.9999999996930378 -2.477749999746475e-5 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 2.477749999746475e-5 0.9999999996930378 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.99875 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0012499999999999734 0.99875 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 1.0 + ] + + B = T[ + 6.256252307659102e-5 -3.128126153811649e-5 -3.128126153847453e-5 0.0 0.0 0.0 + -2.0671108273679513e-16 5.418073430928138e-5 -5.418073430907468e-5 0.0 0.0 0.0 + 6.7365306173215874e-12 -3.368357184796113e-12 -3.3681734325413444e-12 0.0 0.0 0.0 + 1.0608933096199221e-16 5.833953603352774e-12 -5.8340596926953345e-12 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 + 1.0 0.0 0.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 0.0 0.0 + 0.0 0.0 1.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.013888888888888593 0.013888888888888593 0.013888888888888593 + 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 + ] + + C = T[ + 1.0 0.0 0.0 0.0 -1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 0.0 -1.0 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 -5.5 5.5 + ] + + #ResetMaps = Vector{ConstrainedLinearControlDiscreteSystem}(undef, length(domains)^2) + ##cU = polyhedron(HalfSpace(T[0, 0, -1], 2) ∩ HalfSpace(T[0, 0, 1], 2), lib) + + function is_enabled(from, to) + from_node = nodes[from] + to_node = nodes[to] + + # On the first 3 elements of the nodes, we cannot go from -1 to 1 or 1 to -1 + for i in 1:3 + if from_node[i] == -1 && to_node[i] == 1 + return false + end + if from_node[i] == 1 && to_node[i] == -1 + return false + end + end + + return true + end + + ntransitions = 0 + function maybe_add_transition(from, to) + #global ntransitions + if is_enabled(from, to) + ntransitions += 1 + add_transition!(automaton, from, to, ntransitions) + append!(indexB, from) + end + end + for from in 1:nnodes, to in 1:nnodes + maybe_add_transition(from, to) + end + + modes = [ConstrainedContinuousIdentitySystem(12, i) for i in 1:nnodes] + resetmaps = [AffineContinuousSystem(A, B*nodes[indexB[i]]) for i in 1:ntransitions] + + system = HybridSystem( + automaton, + # Modes + modes, + # Reset maps + resetmaps, + # Switching + Fill(ControlledSwitching(), ntransitions), + ) + + system.ext[:q_T] = nmodes(system) + system.ext[:q_C] = C + + return system +end + +"""" + problem(lib, T::Type, gamma=0.95, x_0 = [1.0, -6.0], N = 11, tail_cost::Bool = false) + +This function create the system with `PCOSQPRM.system`. + +Then, we define initial conditions (continuous and discrete states) to this system +and set `N` as horizon, i.e., the number of allowed time steps. + +We instantiate our Optimal Control Problem by defining the state and tail costs. +Notice that `state_cost` is defined to be \sum(\gamma^t \times norm(Cx_t)) for each time step `t` +and the `tail_cost` is defined to be \gamma^N V(x_N) that we considering constant in this version. + +Notice that we used `Fill` for all `N` time steps as we consider time-invariant costs. +""" +function problem( + lib = CDDLib.Library(), + T::Type = Float64; + gamma = 0.95, + x_0 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + N = 3, + tail_cost::Bool = false, +) + sys = system(lib, T) + if tail_cost + transition_cost = Fill(UT.ZeroFunction(), nmodes(sys)) + end + + state_cost = [ + mode == sys.ext[:q_T] ? UT.ConstantFunction(zero(T)) : + UT.ConstantFunction(one(T)) for mode in modes(sys) + ] + + transition_cost = UT.QuadraticControlFunction(ones(T, 1, 1)) + problem = PR.OptimalControlProblem( + sys, + (x_0), + sys.ext[:q_T], + Fill(state_cost, N), + Fill(Fill(transition_cost, ntransitions(sys)), N), + N, + ) + return problem +end + +end + From 018acb69a681ba39aa4123250bc929de1ef8c7c4 Mon Sep 17 00:00:00 2001 From: johnaoga Date: Fri, 28 Jun 2024 01:55:08 +0200 Subject: [PATCH 6/7] progress --- docs/src/examples/solvers/Power Converter.jl | 119 ++++++++++++++++++- problems/pc-osqp-rmaps.jl | 17 +-- 2 files changed, 127 insertions(+), 9 deletions(-) diff --git a/docs/src/examples/solvers/Power Converter.jl b/docs/src/examples/solvers/Power Converter.jl index 8681e9c5..c727aee3 100644 --- a/docs/src/examples/solvers/Power Converter.jl +++ b/docs/src/examples/solvers/Power Converter.jl @@ -27,7 +27,13 @@ using Test #src # First, let us import [StaticArrays](https://github.com/JuliaArrays/StaticArrays.jl) and [Plots](https://github.com/JuliaPlots/Plots.jl). -using StaticArrays, Plots +import CDDLib +import GLPK +import OSQP +using JuMP +import Pavito +import HiGHS +import Ipopt # At this point, we import the useful Dionysos sub-modules. using Dionysos @@ -41,4 +47,113 @@ const AB = OP.Abstraction # ### Definition of the system # we can import the module containing the DCDC problem like this -include(joinpath(dirname(dirname(pathof(Dionysos))), "problems", "pc-osqp.jl")) +#include(joinpath(dirname(dirname(pathof(Dionysos))), "problems", "pc-osqp.jl")) +include(joinpath(dirname(dirname(pathof(Dionysos))), "problems", "pc-osqp-rmaps.jl")) + +problem = PCOSQPRM.problem(CDDLib.Library(), Float64); + +# Finally, we select the method presented in [2] as our optimizer + +qp_solver = optimizer_with_attributes( + OSQP.Optimizer, + "eps_abs" => 1e-8, + "eps_rel" => 1e-8, + "max_iter" => 100000, + MOI.Silent() => true, +); + +mip_solver = optimizer_with_attributes(HiGHS.Optimizer, MOI.Silent() => true); + +cont_solver = optimizer_with_attributes(Ipopt.Optimizer, MOI.Silent() => true); + +miqp_solver = optimizer_with_attributes( + Pavito.Optimizer, + "mip_solver" => mip_solver, + "cont_solver" => cont_solver, + MOI.Silent() => true, +); + +algo = optimizer_with_attributes( + OP.BemporadMorari.Optimizer{Float64}, + "continuous_solver" => qp_solver, + "mixed_integer_solver" => miqp_solver, + "indicator" => false, + "log_level" => 0, +); + +# and use it to solve the given problem, with the help of the abstraction layer +# MathOptInterface provided by [JuMP](https://github.com/jump-dev/JuMP.jl) +optimizer = MOI.instantiate(algo) +MOI.set(optimizer, MOI.RawOptimizerAttribute("problem"), problem) +MOI.optimize!(optimizer) + +# We check the solver time +MOI.get(optimizer, MOI.SolveTimeSec()) + +# the termination status +termination = MOI.get(optimizer, MOI.TerminationStatus()) + +# the objective value +objective_value = MOI.get(optimizer, MOI.ObjectiveValue()) + +##@test objective_value ≈ 11.38 atol = 1e-2 #src + +# and recover the corresponding continuous trajectory +xu = MOI.get(optimizer, ST.ContinuousTrajectoryAttribute()); + +# ## A little bit of data visualization now: + +using Plots +using Polyhedra +using HybridSystems +using Suppressor + +#Initialize our canvas +fig = plot(; + aspect_ratio = :equal, + xtickfontsize = 10, + ytickfontsize = 10, + guidefontsize = 16, + titlefontsize = 14, +); +xlims!(-10.5, 3.0) +ylims!(-10.5, 3.0) + +#Plot the discrete modes +for mode in states(problem.system) + t = + (problem.system.ext[:q_T] in [mode, mode + 11]) ? "XT" : + ( + mode == problem.system.ext[:q_A] ? "A" : + ( + mode == problem.system.ext[:q_B] ? "B" : + mode <= 11 ? string(mode) : string(mode - 11) + ) + ) + set = stateset(problem.system, mode) + plot!(set; color = :white) + UT.text_in_set_plot!(fig, set, t) +end + +#Plot obstacles +for i in eachindex(problem.system.ext[:obstacles]) + set = problem.system.ext[:obstacles][i] + plot!(set; color = :black, opacity = 0.5) + UT.text_in_set_plot!(fig, set, "O$i") +end + +#Plot trajectory +x0 = problem.initial_set[2] +x_traj = [x0, xu.x...] +plot!(fig, UT.DrawTrajectory(x_traj)); + +#Plot initial point +plot!(fig, UT.DrawPoint(x0); color = :blue) +annotate!(fig, x0[1], x0[2] - 0.5, "x0") + +# ### References +# +# 1. Gol, E. A., Lazar, M., & Belta, C. (2013). Language-guided controller synthesis for linear systems. IEEE Transactions on Automatic Control, 59(5), 1163-1176. +# 1. Bemporad, A., & Morari, M. (1999). Control of systems integrating logic, dynamics, and constraints. Automatica, 35(3), 407-427. +# 1. Legat B., Bouchat J., Jungers R. M. (2021). Abstraction-based branch and bound approach to Q-learning for hybrid optimal control. 3rd Annual Learning for Dynamics & Control Conference, 2021. +# 1. Legat B., Bouchat J., Jungers R. M. (2021). Abstraction-based branch and bound approach to Q-learning for hybrid optimal control. https://www.codeocean.com/. https://doi.org/10.24433/CO.6650697.v1. diff --git a/problems/pc-osqp-rmaps.jl b/problems/pc-osqp-rmaps.jl index d5a48e2a..2c00db6c 100644 --- a/problems/pc-osqp-rmaps.jl +++ b/problems/pc-osqp-rmaps.jl @@ -123,8 +123,9 @@ function system(lib, T::Type) Fill(ControlledSwitching(), ntransitions), ) - system.ext[:q_T] = nmodes(system) + system.ext[:q_M] = nmodes(system) system.ext[:q_C] = C + system.ext[:q_T] = ntransitions return system end @@ -138,8 +139,8 @@ Then, we define initial conditions (continuous and discrete states) to this syst and set `N` as horizon, i.e., the number of allowed time steps. We instantiate our Optimal Control Problem by defining the state and tail costs. -Notice that `state_cost` is defined to be \sum(\gamma^t \times norm(Cx_t)) for each time step `t` -and the `tail_cost` is defined to be \gamma^N V(x_N) that we considering constant in this version. +Notice that `state_cost` is defined to be `sum(gamma^t * norm(Cx_t))` for each time step `t` +and the `tail_cost` is defined to be `gamma^N V(x_N)` that we considering constant in this version. Notice that we used `Fill` for all `N` time steps as we consider time-invariant costs. """ @@ -157,17 +158,19 @@ function problem( end state_cost = [ - mode == sys.ext[:q_T] ? UT.ConstantFunction(zero(T)) : + mode == sys.ext[:q_M] ? UT.ConstantFunction(zero(T)) : UT.ConstantFunction(one(T)) for mode in modes(sys) ] - transition_cost = UT.QuadraticControlFunction(ones(T, 1, 1)) + transition_cost = UT.QuadraticControlFunction(sys.ext[:q_C] * sys.ext[:q_C]') problem = PR.OptimalControlProblem( sys, (x_0), - sys.ext[:q_T], + sys.ext[:q_M], Fill(state_cost, N), - Fill(Fill(transition_cost, ntransitions(sys)), N), + #transition_cost, + Fill(transition_cost, sys.ext[:q_M]), + #Fill(Fill(transition_cost, sys.ext[:q_M]), N), N, ) return problem From eb66425e1918f041e968b4161a0b96db3fd320b2 Mon Sep 17 00:00:00 2001 From: johnaoga Date: Mon, 1 Jul 2024 19:00:44 +0200 Subject: [PATCH 7/7] progress --- docs/src/examples/solvers/Power Converter.jl | 2 +- problems/pc-osqp-rmaps.jl | 63 +++++++++++++------- src/optim/bemporad_morari.jl | 17 ++++++ 3 files changed, 61 insertions(+), 21 deletions(-) diff --git a/docs/src/examples/solvers/Power Converter.jl b/docs/src/examples/solvers/Power Converter.jl index c727aee3..8d2b9387 100644 --- a/docs/src/examples/solvers/Power Converter.jl +++ b/docs/src/examples/solvers/Power Converter.jl @@ -122,7 +122,7 @@ ylims!(-10.5, 3.0) #Plot the discrete modes for mode in states(problem.system) t = - (problem.system.ext[:q_T] in [mode, mode + 11]) ? "XT" : + (problem.system.ext[:modes] in [mode, mode + 11]) ? "XT" : ( mode == problem.system.ext[:q_A] ? "A" : ( diff --git a/problems/pc-osqp-rmaps.jl b/problems/pc-osqp-rmaps.jl index 2c00db6c..36d87cab 100644 --- a/problems/pc-osqp-rmaps.jl +++ b/problems/pc-osqp-rmaps.jl @@ -34,7 +34,20 @@ function system(lib, T::Type) nodes = generate_combinations(possible_values, nvars) nnodes = length(nodes) - domains = [] + ## domains + function rect(x_l, x_u) + r = HalfSpace([-1], -T(x_l)) ∩ HalfSpace([1], T(x_u)) + return polyhedron(r, lib) + end + function rect(x_l, x_u, y_l, y_u) + r = + HalfSpace([-1, 0], -T(x_l)) ∩ HalfSpace([1, 0], T(x_u)) ∩ + HalfSpace([0, -1], -T(y_l)) ∩ HalfSpace([0, 1], T(y_u)) + return polyhedron(r, lib) + end + + pX = rect(-10,10) + pU = rect(-1, 1) #Automaton: automaton = GraphAutomaton(nnodes) @@ -110,9 +123,14 @@ function system(lib, T::Type) maybe_add_transition(from, to) end - modes = [ConstrainedContinuousIdentitySystem(12, i) for i in 1:nnodes] - resetmaps = [AffineContinuousSystem(A, B*nodes[indexB[i]]) for i in 1:ntransitions] - + #modes = [ConstrainedContinuousIdentitySystem(12, i) for i in 1:nnodes] + modes = [ConstrainedContinuousIdentitySystem(12, FullSpace()) for i in 1:nnodes] + + #L290: ERROR: DimensionMismatch: matrix A has dimensions (6,6), vector B has length 0 + #resetmaps = [ConstrainedAffineMap(A, B*nodes[indexB[i]], FullSpace()) for i in 1:ntransitions] + #resetmaps = Fill(ConstrainedAffineMap(A, B*nodes[indexB[i]], FullSpace()), ntransitions) + resetmaps = Fill(ConstrainedLinearControlMap(A, B, FullSpace(), pU), ntransitions) + system = HybridSystem( automaton, # Modes @@ -121,11 +139,12 @@ function system(lib, T::Type) resetmaps, # Switching Fill(ControlledSwitching(), ntransitions), + #Fill(AutonomousSwitching(), ntransitions), ) - system.ext[:q_M] = nmodes(system) + system.ext[:modes] = nmodes(system) system.ext[:q_C] = C - system.ext[:q_T] = ntransitions + system.ext[:transitions] = ntransitions return system end @@ -148,31 +167,35 @@ function problem( lib = CDDLib.Library(), T::Type = Float64; gamma = 0.95, - x_0 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + q0 = 1, + x_0 = [1 for _ in 1:12], #The first state is the initial state #[0 for i in 1:12], N = 3, tail_cost::Bool = false, ) sys = system(lib, T) - if tail_cost - transition_cost = Fill(UT.ZeroFunction(), nmodes(sys)) - end + #if tail_cost + # transition_cost = Fill(UT.ZeroFunction(), nmodes(sys)) + #end - state_cost = [ - mode == sys.ext[:q_M] ? UT.ConstantFunction(zero(T)) : - UT.ConstantFunction(one(T)) for mode in modes(sys) - ] - transition_cost = UT.QuadraticControlFunction(sys.ext[:q_C] * sys.ext[:q_C]') + #state_cost = UT.QuadraticControlFunction(sys.ext[:q_C]' * sys.ext[:q_C]) + state_cost = UT.QuadraticControlFunction(ones(T, 6, 6)) + + #x_0 = [0 for _ in 1:sys.ext[:modes]] + #problem = PR.OptimalControlProblem(sys, (q0, x_0), sys.ext[:modes], nothing, nothing, N) + + #==# problem = PR.OptimalControlProblem( sys, - (x_0), - sys.ext[:q_M], - Fill(state_cost, N), + (q0, x_0), + sys.ext[:modes], + #state_cost, + Fill(Fill(state_cost, sys.ext[:modes]), N),#Fill(Fill(state_cost, sys.ext[:modes]),N), #transition_cost, - Fill(transition_cost, sys.ext[:q_M]), - #Fill(Fill(transition_cost, sys.ext[:q_M]), N), + Fill(Fill(UT.ZeroFunction(), sys.ext[:transitions]), N), N, ) + #==# return problem end diff --git a/src/optim/bemporad_morari.jl b/src/optim/bemporad_morari.jl index 5e327da5..591bf70c 100644 --- a/src/optim/bemporad_morari.jl +++ b/src/optim/bemporad_morari.jl @@ -194,6 +194,21 @@ function hybrid_constraints( return δ end +function hybrid_constraints( + model, + systems::AbstractVector{<:ConstrainedAffineMap}, + x_prev, + x, + u, + algo::Optimizer{T}, + δ, +) where {T} + println("hybrid_constraints") + system = first(systems) + add_constraint.(model, x - system.A * x_prev - system.c, MOI.EqualTo(zero(T))) + return δ +end + function hybrid_constraints( model, systems::AbstractVector{<:ConstrainedContinuousIdentitySystem}, @@ -206,6 +221,8 @@ function hybrid_constraints( return hybrid_constraints(model, inner_vector(system -> system.X, systems), x, algo, δ) end +#TODO: Needed function for the power convertor bench implementation, but need to be fixed +#TODO: and hybrid_cost implemented or adjusted accordingly function hybrid_constraints( model, systems::AbstractVector{<:ConstrainedLinearControlMap},