From 489456712ba6700d7bc15b082a66c6340622a149 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 7 Nov 2025 09:42:31 -0500 Subject: [PATCH 1/6] - doc: estimation Horizon `He` --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index b35894ded..213f9db1a 100644 --- a/README.md +++ b/README.md @@ -51,7 +51,7 @@ Our goal is controlling the first output $y_1$, but the second one $y_2$ should 35: ```julia -mhe = MovingHorizonEstimator(model) +mhe = MovingHorizonEstimator(model, He=10) mpc = LinMPC(mhe, Mwt=[1, 0], Nwt=[0.1]) mpc = setconstraint!(mpc, ymax=[Inf, 35]) ``` From f21409330069c46a7de3fd661d042e21d02e4bb8 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 7 Nov 2025 10:24:39 -0500 Subject: [PATCH 2/6] added: `LinearMPC` as a package extension --- Project.toml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Project.toml b/Project.toml index 9634f41de..cfc45d87d 100644 --- a/Project.toml +++ b/Project.toml @@ -21,6 +21,12 @@ SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" SparseConnectivityTracer = "9f842d2f-2579-4b1d-911e-f412cf18a3f5" SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35" +[weakdeps] +LinearMPC = "82e1c212-e1a2-49d2-b26a-a31d6968e3bd" + +[extensions] +LinearMPCext = "LinearMPC" + [compat] ControlSystemsBase = "1.18.2" DAQP = "0.6, 0.7.1" From 8a3867a507e923ff309175855ddf86ead00ffef5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 7 Nov 2025 10:39:54 -0500 Subject: [PATCH 3/6] add: extension module --- ext/LinearMPCext.jl | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 ext/LinearMPCext.jl diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl new file mode 100644 index 000000000..1510c079e --- /dev/null +++ b/ext/LinearMPCext.jl @@ -0,0 +1,9 @@ +module LinearMPCext + +using LinearMPCext + +export hi + +hi() = println("hello world!") + +end # LinearMPCext \ No newline at end of file From 58c96044631dbb0fd24e5f6d361c1a0e2b63bc8b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 7 Nov 2025 13:14:18 -0500 Subject: [PATCH 4/6] wip: extension --- Project.toml | 3 ++- ext/LinearMPCext.jl | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Project.toml b/Project.toml index cfc45d87d..e65e2f5b6 100644 --- a/Project.toml +++ b/Project.toml @@ -60,6 +60,7 @@ Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" TestItemRunner = "f8b46487-2199-4994-9208-9a1283c18c0a" TestItems = "1c621080-faea-4a02-84b6-bbd5e436b8fe" +LinearMPC = "82e1c212-e1a2-49d2-b26a-a31d6968e3bd" [targets] -test = ["Test", "TestItems", "TestItemRunner", "Documenter", "Plots", "DAQP", "FiniteDiff"] +test = ["Test", "TestItems", "TestItemRunner", "Documenter", "Plots", "DAQP", "FiniteDiff", "LinearMPC"] diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl index 1510c079e..b182dc814 100644 --- a/ext/LinearMPCext.jl +++ b/ext/LinearMPCext.jl @@ -1,6 +1,6 @@ module LinearMPCext -using LinearMPCext +using ModelPredictiveControl, LinearMPC export hi From 4ce2a1d69638a07b6b3f82671c6d7f0465e1dcb7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 21 Nov 2025 00:54:11 -0500 Subject: [PATCH 5/6] adding: scripts to `examples` folder --- examples/Knitrodebug.jl | 23 ++ examples/TestMPC.jl | 5 + examples/Unodebug.jl | 36 ++ examples/bench_empc_hessian.jl | 86 +++++ examples/bench_linearize.jl | 80 +++++ examples/better_default_backends.jl | 62 ++++ examples/debug_SMC.jl | 13 + examples/di.jl | 18 + examples/ekf.jl | 50 +++ examples/extension.jl | 88 +++++ examples/lin_mpc_article.jl | 111 +++++++ examples/manual_estim.jl | 26 ++ examples/multithreads.jl | 52 +++ examples/mytests-mpc.jl | 499 ++++++++++++++++++++++++++++ examples/newtest.jl | 76 +++++ examples/nonlin_mpc_article.jl | 342 +++++++++++++++++++ examples/test_di.jl | 22 ++ examples/test_jump_hessian.jl | 63 ++++ examples/yoo.jl | 42 +++ 19 files changed, 1694 insertions(+) create mode 100644 examples/Knitrodebug.jl create mode 100644 examples/TestMPC.jl create mode 100644 examples/Unodebug.jl create mode 100644 examples/bench_empc_hessian.jl create mode 100644 examples/bench_linearize.jl create mode 100644 examples/better_default_backends.jl create mode 100644 examples/debug_SMC.jl create mode 100644 examples/di.jl create mode 100644 examples/ekf.jl create mode 100644 examples/extension.jl create mode 100644 examples/lin_mpc_article.jl create mode 100644 examples/manual_estim.jl create mode 100644 examples/multithreads.jl create mode 100644 examples/mytests-mpc.jl create mode 100644 examples/newtest.jl create mode 100644 examples/nonlin_mpc_article.jl create mode 100644 examples/test_di.jl create mode 100644 examples/test_jump_hessian.jl create mode 100644 examples/yoo.jl diff --git a/examples/Knitrodebug.jl b/examples/Knitrodebug.jl new file mode 100644 index 000000000..1c314da15 --- /dev/null +++ b/examples/Knitrodebug.jl @@ -0,0 +1,23 @@ +using ModelPredictiveControl +using JuMP, KNITRO, Ipopt + +A = [ 0.800737 0.0 + 0.0 0.606531 ] +Bu = [ 0.378599 0.378599 + -0.291167 0.291167 ] +C = [ 1.0 0.0 + 0.0 1.0 ] +f(x,u,_,_) = A*x + Bu*u +h(x,_,_) = C*x +model = NonLinModel(f, h, 4.0, 2, 2, 2, solver=nothing) + +optim = Model(KNITRO.Optimizer) # Model(Ipopt.Optimizer) # Ipopt does work here +oracle = true # true leads to `LOCALLY_INFEASIBLE` on KNITRO dev version, false works + +nmpc = NonLinMPC(model; Hp=10, direct=false, Cwt=Inf, optim, oracle) +nmpc = setconstraint!(nmpc, ymin=[-2,-2],ymax=[+2,+2]) +unset_time_limit_sec(nmpc.optim) +unset_silent(nmpc.optim) +set_optimizer_attribute(nmpc.optim, "outlev", 6) +u = moveinput!(nmpc, [1, 1]) +#solution_summary(nmpc.optim) \ No newline at end of file diff --git a/examples/TestMPC.jl b/examples/TestMPC.jl new file mode 100644 index 000000000..05bdd0b0d --- /dev/null +++ b/examples/TestMPC.jl @@ -0,0 +1,5 @@ +module TestMPC + +greet() = print("Hello World!") + +end # module TestMPC diff --git a/examples/Unodebug.jl b/examples/Unodebug.jl new file mode 100644 index 000000000..b621c1e87 --- /dev/null +++ b/examples/Unodebug.jl @@ -0,0 +1,36 @@ +using ModelPredictiveControl, JuMP +using UnoSolver + +N = 35 # number of JuMP.optimize! calls + +function f!(ẋ, x, u, _ , p) + g, L, K, m = p # [m/s²], [m], [kg/s], [kg] + θ, ω = x[1], x[2] # [rad], [rad/s] + τ = u[1] # [Nm] + ẋ[1] = ω + ẋ[2] = -g/L*sin(θ) - K/m*ω + τ/m/L^2 + return nothing +end +h!(y, x, _ , _ ) = (y[1] = 180/π*x[1]; nothing) # [°] +p = [9.8, 0.4, 1.2, 0.3] +nu, nx, ny, Ts = 1, 2, 1, 0.1 +model = NonLinModel(f!, h!, Ts, nu, nx, ny; p) +p_plant = copy(p); p_plant[3] = p[3]*1.25 +plant = NonLinModel(f!, h!, Ts, nu, nx, ny; p=p_plant) +Hp, Hc, Mwt, Nwt = 20, 2, [0.5], [2.5] +α=0.01; σQ=[0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1] +σQint_ym = zeros(0) +umin, umax = [-1.5], [+1.5] +transcription = MultipleShooting() +optim = Model(()->UnoSolver.Optimizer(preset="filtersqp")) +oracle = true +hessian = true +nmpc = NonLinMPC(model; + Hp, Hc, Mwt, Nwt, Cwt=Inf, transcription, oracle, hessian, optim, + α, σQ, σR, nint_u, σQint_u, σQint_ym +) +nmpc = setconstraint!(nmpc; umin, umax) +unset_time_limit_sec(nmpc.optim) +sim!(nmpc, N, [180.0]; plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0]) +@time sim!(nmpc, N, [180.0]; plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0]) +@profview sim!(nmpc, N, [180.0]; plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0]) \ No newline at end of file diff --git a/examples/bench_empc_hessian.jl b/examples/bench_empc_hessian.jl new file mode 100644 index 000000000..6cf90a813 --- /dev/null +++ b/examples/bench_empc_hessian.jl @@ -0,0 +1,86 @@ +using ModelPredictiveControl, Plots +function f!(ẋ, x, u, _ , p) + g, L, K, m = p # [m/s²], [m], [kg/s], [kg] + θ, ω = x[1], x[2] # [rad], [rad/s] + τ = u[1] # [Nm] + ẋ[1] = ω + ẋ[2] = -g/L*sin(θ) - K/m*ω + τ/m/L^2 +end +h!(y, x, _ , _ ) = (y[1] = 180/π*x[1]) # [°] +p = [9.8, 0.4, 1.2, 0.3] +nu = 1; nx = 2; ny = 1; Ts = 0.1 +model = NonLinModel(f!, h!, Ts, nu, nx, ny; p) +vu = ["\$τ\$ (Nm)"] +vx = ["\$θ\$ (rad)", "\$ω\$ (rad/s)"] +vy = ["\$θ\$ (°)"] +model = setname!(model; u=vu, x=vx, y=vy) + +## ========================================= +σQ = [0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1] + +## ========================================= +p_plant = copy(p); p_plant[3] = 1.25*p[3] +N = 35; u = [0.5]; + +## ========================================= +Hp, Hc, Mwt, Nwt, Cwt = 20, 2, [0.5], [2.5], Inf +umin, umax = [-1.5], [+1.5] + +h2!(y, x, _ , _ ) = (y[1] = 180/π*x[1]; y[2]=x[2]) +nu, nx, ny = 1, 2, 2 +model2 = NonLinModel(f!, h2!, Ts, nu, nx, ny; p) +plant2 = NonLinModel(f!, h2!, Ts, nu, nx, ny; p=p_plant) +model2 = setname!(model2, u=vu, x=vx, y=[vy; vx[2]]) +plant2 = setname!(plant2, u=vu, x=vx, y=[vy; vx[2]]) +estim2 = UnscentedKalmanFilter(model2; σQ, σR, + nint_u, σQint_u, i_ym=[1]) + +function JE(UE, ŶE, _ , p) + Ts = p + τ, ω = UE[1:end-1], ŶE[2:2:end-1] + return Ts*sum(τ.*ω) +end +p = Ts; Mwt2 = [Mwt; 0.0]; Ewt = 3.5e3 +empc = NonLinMPC(estim2; Hp, Hc, + Nwt, Mwt=Mwt2, Cwt, JE, Ewt, p, oracle=true, transcription=MultipleShooting(), hessian=true) +empc = setconstraint!(empc; umin, umax) + +## ========================================= +using JuMP; unset_time_limit_sec(empc.optim) + +## ========================================= +x_0 = [0, 0]; x̂_0 = [0, 0, 0]; ry = [180; 0] +res2_ry = sim!(empc, N, ry; plant=plant2, x_0, x̂_0) +plot(res2_ry, ploty=[1]) + + +## ========================================= +## ========= Benchmark ===================== +## ========================================= +using BenchmarkTools +using JuMP, Ipopt, KNITRO + +optim = JuMP.Model(Ipopt.Optimizer, add_bridges=false) +empc_ipopt = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=Mwt2, Cwt, JE, Ewt, optim, p, oracle=true, transcription=MultipleShooting(), hessian=true) +empc_ipopt = setconstraint!(empc_ipopt; umin, umax) +JuMP.unset_time_limit_sec(empc_ipopt.optim) +sim!(empc_ipopt, N, ry; plant=plant2, x_0=x_0, x̂_0=x̂_0) +@profview sim!(empc_ipopt, N, ry; plant=plant2, x_0=x_0, x̂_0=x̂_0) + +y_step = [10.0, 0.0] + +bm = @benchmark( + sim!($empc_ipopt, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + samples=50, + seconds=10*60 + ) +@show btime_EMPC_track_solver_IP = median(bm) + + +bm = @benchmark( + sim!($empc_ipopt, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0, y_step=$y_step), + samples=50, + seconds=10*60 + ) +@show btime_EMPC_regul_solver_IP = median(bm) + diff --git a/examples/bench_linearize.jl b/examples/bench_linearize.jl new file mode 100644 index 000000000..ed1572162 --- /dev/null +++ b/examples/bench_linearize.jl @@ -0,0 +1,80 @@ +# spell-checker: disable + +using Revise + +using ModelPredictiveControl + +using LinearAlgebra +using ControlSystemsBase +using BenchmarkTools +using DifferentiationInterface + +#= +using DifferentiationInterface +using SparseConnectivityTracer +using SparseMatrixColorings + +f1(x::AbstractVector) = diff(x .^ 2) + diff(reverse(x .^ 2)) +f2(x::AbstractVector) = sum(f1(x) .^ 2) + +dense_forward_backend = AutoForwardDiff() +sparse_forward_backend = AutoSparse( + dense_forward_backend; # any object from ADTypes + sparsity_detector=TracerSparsityDetector(), + coloring_algorithm=GreedyColoringAlgorithm(), +) + +x = rand(1000) +jac_prep_sparse = prepare_jacobian(f1, sparse_forward_backend, zero(x)) + +J = similar(sparsity_pattern(jac_prep_sparse), eltype(x)) +jacobian!(f1, J, jac_prep_sparse, sparse_forward_backend, x) +=# + + +function f3!(dx, x, u, d, p) + mul!(dx, p.A, x) + mul!(dx, p.Bu, u, 1, 1) + mul!(dx, p.Bd, d, 1, 1) + return nothing +end +function h3!(y, x, d, p) + mul!(y, p.C, x) + mul!(y, p.Dd, d, 1, 1) + return nothing +end + +A=[0 0.5; -0.2 -0.1] +Bu=[0; 0.5] +Bd=[0; 0.5] +C=[0.4 0] +Dd=[0] +nonLinModel3 = NonLinModel(f3!, h3!, 1.0, 1, 2, 1, 1, solver=RungeKutta(4, supersample=2), p=(;A, Bu, Bd, C, Dd)) + + +linearizeModel = linearize(nonLinModel3, x=[1,1], u=[1], d=[1]) + +@benchmark linearize!($linearizeModel, $nonLinModel3, x=$[2,2], u=$[1], d=$[1]) + +linearize2!(linemodel, model) = (linearize!(linemodel, model); nothing) + +#= +linfunc! = nonLinModel3.linfunc! +xnext = linearizeModel.buffer.x +y = linearizeModel.buffer.y +A = linearizeModel.A +Bu = linearizeModel.Bu +Bd = linearizeModel.Bd +C = linearizeModel.C +Dd = linearizeModel.Dd +x = [2.0, 2.0] +u = [1.0] +d = [1.0] +cx = Constant(x) +cu = Constant(u) +cd = Constant(d) +backend = nonLinModel3.jacobian + + +@code_warntype linfunc!(xnext, y, A, Bu, C, Bd, Dd, backend, x, u, d, cx, cu, cd) +=# \ No newline at end of file diff --git a/examples/better_default_backends.jl b/examples/better_default_backends.jl new file mode 100644 index 000000000..a2359437b --- /dev/null +++ b/examples/better_default_backends.jl @@ -0,0 +1,62 @@ +using ModelPredictiveControl, JuMP +using Ipopt, UnoSolver + +N = 35*10 # number of solves/optimize! calls + +function f!(ẋ, x, u, _ , p) + g, L, K, m = p # [m/s²], [m], [kg/s], [kg] + θ, ω = x[1], x[2] # [rad], [rad/s] + τ = u[1] # [Nm] + ẋ[1] = ω + ẋ[2] = -g/L*sin(θ) - K/m*ω + τ/m/L^2 + return nothing +end +p = [9.8, 0.4, 1.2, 0.3] +nu, nx, ny, Ts = 1, 2, 1, 0.1 + +p_plant = copy(p) +p_plant[3] = 1.25*p[3] + +h(x, _ , _ ) = [180/π*x[1], x[2]] +nu, nx, ny = 1, 2, 2 +model = NonLinModel(f!, h, Ts, nu, nx, ny; p=p) +plant = NonLinModel(f!, h, Ts, nu, nx, ny; p=p_plant) + +Hp, Hc, Mwt, Nwt = 20, 2, [0.5, 0], [2.5] +α=0.01; σQ=[0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1] +σQint_ym = zeros(0) + +estim = UnscentedKalmanFilter(model; σQ, σR, nint_u, σQint_u, i_ym=[1]) + +umin, umax = [-1.5], [+1.5] +transcription = MultipleShooting() +oracle = true +hessian = true +optim = JuMP.Model(()->UnoSolver.Optimizer(preset="filtersqp")) + +function gc!(LHS, Ue, Ŷe, _, p, ϵ) + Pmax = p + i_τ, i_ω = 1, 2 + for i in eachindex(LHS) + τ, ω = Ue[i_τ], Ŷe[i_ω] + P = τ*ω + LHS[i] = P - Pmax - ϵ + i_τ += 1 + i_ω += 2 + end + return nothing +end +Cwt, Pmax, nc = 1e5, 3, Hp+1 + +nmpc = NonLinMPC(estim; + Hp, Hc, Mwt, Nwt, + #Cwt, gc!, nc, p=Pmax, + transcription, oracle, hessian, + optim +) +nmpc = setconstraint!(nmpc; umin, umax) +unset_time_limit_sec(nmpc.optim) +#unset_silent(nmpc.optim) +sim!(nmpc, N, [180.0, 0]; plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0]) +@time sim!(nmpc, N, [180.0, 0]; plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0]) +@profview sim!(nmpc, N, [180.0, 0]; plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0]) \ No newline at end of file diff --git a/examples/debug_SMC.jl b/examples/debug_SMC.jl new file mode 100644 index 000000000..7acb1611b --- /dev/null +++ b/examples/debug_SMC.jl @@ -0,0 +1,13 @@ +using ModelPredictiveControl +using DifferentiationInterface, SparseConnectivityTracer, SparseMatrixColorings +import ForwardDiff +hessian = AutoSparse( + AutoForwardDiff(); + sparsity_detector = TracerSparsityDetector(), + coloring_algorithm = GreedyColoringAlgorithm(; postprocessing=true) +) +f(x,u,_,_) = 0.5*x + 0.5*u +h(x,_,_) = x +model = NonLinModel(f, h, 10.0, 1, 1, 1, solver=nothing) +nmpc = NonLinMPC(model; Hp=5, direct=false, hessian) +moveinput!(nmpc) \ No newline at end of file diff --git a/examples/di.jl b/examples/di.jl new file mode 100644 index 000000000..4724edcac --- /dev/null +++ b/examples/di.jl @@ -0,0 +1,18 @@ +using ModelPredictiveControl, ControlSystemsBase + +Ts = 4.0 +A = [ 0.800737 0.0 0.0 0.0 + 0.0 0.606531 0.0 0.0 + 0.0 0.0 0.8 0.0 + 0.0 0.0 0.0 0.6 ] +Bu = [ 0.378599 0.378599 + -0.291167 0.291167 + 0.0 0.0 + 0.0 0.0 ] +Bd = [ 0; 0; 0.5; 0.5;; ] +C = [ 1.0 0.0 0.684 0.0 + 0.0 1.0 0.0 -0.4736 ] +Dd = [ 0.19; -0.148;; ] +Du = zeros(2,2) +model = LinModel(ss(A,[Bu Bd],C,[Du Dd],Ts),Ts,i_d=[3]) +model = setop!(model, uop=[10,10], yop=[50,30], dop=[5]) diff --git a/examples/ekf.jl b/examples/ekf.jl new file mode 100644 index 000000000..d98ca4cbf --- /dev/null +++ b/examples/ekf.jl @@ -0,0 +1,50 @@ +using ModelPredictiveControl, ControlSystemsBase +using JuMP, Ipopt, MadNLP +using BenchmarkTools + +Ts = 4.0 +A = [ 0.800737 0.0 0.0 0.0 + 0.0 0.606531 0.0 0.0 + 0.0 0.0 0.8 0.0 + 0.0 0.0 0.0 0.6 ] +Bu = [ 0.378599 0.378599 + -0.291167 0.291167 + 0.0 0.0 + 0.0 0.0 ] +Bd = [ 0; 0; 0.5; 0.5;; ] +C = [ 1.0 0.0 0.684 0.0 + 0.0 1.0 0.0 -0.4736 ] +Dd = [ 0.19; -0.148;; ] +Du = zeros(2,2) +#model = LinModel(ss(A,[Bu Bd],C,[Du Dd],Ts),Ts,i_d=[3]) +#model = setop!(model, uop=[10,10], yop=[50,30], dop=[5]) +#mpc = LinMPC(model, transcription=MultipleShooting())#, Cwt=Inf) +#mpc = setconstraint!(mpc, ymax=[Inf,30.5]) + +using LinearAlgebra + +function f!(xnext, x, u, d, p) + mul!(xnext, p.A, x) + mul!(xnext, p.Bu, u, 1, 1) + mul!(xnext, p.Bd, d, 1, 1) + return nothing +end + +function h!(y, x, d, p) + mul!(y, p.C, x) + mul!(y, p.Dd, d, 1, 1) + return nothing +end + +model = NonLinModel(f!, h!, Ts, 2, 4, 2, 1, solver=nothing, p=(;A,Bu,Bd,C,Dd)) +model = setop!(model, uop=[10,10], yop=[50,30], dop=[5]) + +ekf = ExtendedKalmanFilter(model, nint_u=[1, 1]) + +y = [50.0, 30.0] +u = [10.0, 10.0] +d = [5.0] +preparestate!(ekf, y, d) +updatestate!(ekf, u, y, d) + +@benchmark (preparestate!($ekf, $y, $d); updatestate!($ekf, $u, $y, $d)) diff --git a/examples/extension.jl b/examples/extension.jl new file mode 100644 index 000000000..c097be0da --- /dev/null +++ b/examples/extension.jl @@ -0,0 +1,88 @@ +using LinearMPC +using ModelPredictiveControl + +using LinearMPC +# Continuous time system dx = A x + B u, y = C x +A = [0 1 0 0; 0 -10 9.81 0; 0 0 0 1; 0 -20 39.24 0]; +B = 100*[0;1.0;0;2.0;;]; +C = [1.0 0 0 0; 0 0 1.0 0]; + + +# create an MPC control with sample time 0.01, prediction/control horizon 50/5 +Ts = 0.01 +mpc = LinearMPC.MPC(A,B,Ts;C,Np=50,Nc=5); + +# ...existing code... +using LinearMPC +using ModelPredictiveControl + +# Continuous time system dx = A x + B u, y = C x +A = [0 1 0 0; 0 -10 9.81 0; 0 0 0 1; 0 -20 39.24 0]; +B = 100*[0;1.0;0;2.0]; +C = [1.0 0 0 0; 0 0 1.0 0]; + +# create an MPC control with sample time 0.01, prediction/control horizon 50/5 +Ts = 0.01 +mpc = LinearMPC.MPC(A,B,Ts; C=C, Np=50, Nc=5); + +# set the objective functions weights +set_objective!(mpc; Q=[1.2^2,1], R=[0.0], Rr=[1.0]) + +# set actuator limits +set_bounds!(mpc; umin=[-2.0],umax=[2.0]) + +u = compute_control(mpc,[0,0,0,0], r = [1, 0]) + + +Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) = begin + model, weights = mpc.estim.model, mpc.weights + + A = mpc.estim. + B = mpc.estim.B̂u + Ts = model.Ts + + C = mpc.estim.Ĉ + Np = mpc.Hp + Nc = mpc.Hc + + + newmpc = LinearMPC.MPC(A, B, Ts; C, Np, Nc) + + + Q = weights.M_Hp[1:model.ny, 1:model.ny] + Rr = weights.Ñ_Hc[1:model.nu, 1:model.nu] + R = weights.L_Hp[1:model.nu, 1:model.nu] + # Qf = weights.M_Hp[end-model.ny+1:end, end-model.ny+1:end] + + LinearMPC.set_objective!(newmpc; Q, Rr, R) + + + Umin, Umax = mpc.con.U0min + mpc.Uop, mpc.con.U0max + mpc.Uop + Ymin, Ymax = mpc.con.Y0min + mpc.Yop, mpc.con.Y0max + mpc.Yop + + umin, umax = Umin[1:model.nu], Umax[1:model.nu] + ymin, ymax = Ymin[1:model.ny], Ymax[1:model.ny] + + LinearMPC.set_bounds!(newmpc; umin, umax, ymin, ymax) + + return newmpc +end + +using ModelPredictiveControl, ControlSystemsBase +G = [ tf( 2 , [10, 1])*delay(20) + tf( 10, [4, 1]) ] +Ts = 1.0 +model = LinModel(G, Ts) + +mpc = LinMPC(model, Mwt=[1, 0], Nwt=[0.1]) +mpc = setconstraint!(mpc, ymax=[Inf, 35]) + +mpc2 = convert(LinearMPC.MPC, mpc) + +sim = LinearMPC.Simulation(mpc2;x0=zeros(24),r=[5,0],N=10) + +using PlotThemes, Plots +#theme(:default) +theme(:dark) +default(fontfamily="Computer Modern"); scalefontsizes(1.1) +plot(sim) diff --git a/examples/lin_mpc_article.jl b/examples/lin_mpc_article.jl new file mode 100644 index 000000000..bf411998b --- /dev/null +++ b/examples/lin_mpc_article.jl @@ -0,0 +1,111 @@ +# ========================================== +# ========== GLOBAL SETTINGS =============== +# ========================================== +run_benchmarks = true + +# ========================================== +# ========== FEEDBACK ====================== +# ========================================== + +using ModelPredictiveControl, ControlSystemsBase +G = [ tf(1.90, [18, 1]) tf(1.90, [18, 1]); + tf(-0.74,[8, 1]) tf(0.74, [8, 1]) ] +uop, yop = [20, 20], [50, 30] +vu , vy = ["\$u_c\$", "\$u_h\$"], ["\$y_L\$", "\$y_T\$"] +model = setop!(LinModel(G, 2.0); uop, yop) +model = setname!(model; u=vu, y=vy) + +## ========================================= +mpc = setconstraint!(LinMPC(model), ymin=[45, -Inf]) + +## ========================================= +function test_mpc(mpc, plant) + plant.x0 .= 0; y = plant() # or evaloutput(plant) + initstate!(mpc, plant.uop, y) + N = 75; ry = [50, 30]; ul = 0 + U, Y, Ry = zeros(2, N), zeros(2, N), zeros(2, N) + for i = 1:N + i == 26 && (ry = [48, 35]) + i == 51 && (ul = -10) + y = plant() # simulated measurements + preparestate!(mpc, y) # prepare mpc estimate + u = mpc(ry) # or moveinput!(mpc, ry) + U[:,i], Y[:,i], Ry[:,i] = u, y, ry + updatestate!(mpc, u, y) # update mpc estimate + updatestate!(plant, u+[0,ul]) # update simulator + end + return U, Y, Ry +end +U_data, Y_data, Ry_data = test_mpc(mpc, model) + +## ========================================= +res = SimResult(mpc, U_data, Y_data; Ry_data) +using Plots; plot(res) + +## ========================================= +## ========= Benchmark ===================== +## ========================================= +using BenchmarkTools +using JuMP, OSQP, DAQP + +if run_benchmarks + optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) + mpc_osqp = setconstraint!(LinMPC(model; optim), ymin=[45, -Inf]) + JuMP.unset_time_limit_sec(mpc_osqp.optim) + bm = @benchmark test_mpc($mpc_osqp, $model) samples=500 + @show btime_solver_OS = median(bm) + + optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) + mpc_daqp = setconstraint!(LinMPC(model; optim), ymin=[45, -Inf]) + bm = @benchmark test_mpc($mpc_daqp, $model) samples=500 + @show btime_solver_AS = median(bm) +end + +## ========================================= +## ========= Feedforward =================== +## ========================================= +model_d = LinModel([G G[1:2, 2]], 2.0; i_d=[3]) +model_d = setop!(model_d; uop, yop, dop=[20]) +model_d = setname!(model_d; u=vu, y=vy, d=["\$u_l\$"]) + +## ========================================= +mpc_d = setconstraint!(LinMPC(model_d), ymin=[45, -Inf]) +function test_mpc_d(mpc_d, plant) + plant.x0 .= 0; y = plant(); d = [20] + initstate!(mpc_d, plant.uop, y, d) + N = 75; ry = [50, 30]; ul = 0 + U, Y, Ry = zeros(2, N), zeros(2, N), zeros(2, N) + for i = 1:N + i == 26 && (ry = [48, 35]) + i == 51 && (ul = -10) + y, d = plant(), [20+ul] # simulated measurements + preparestate!(mpc_d, y, d) # d in arguments + u = mpc_d(ry, d) # d in arguments + U[:,i], Y[:,i], Ry[:,i] = u, y, ry + updatestate!(mpc_d, u, y, d) # d in arguments + updatestate!(plant, u+[0,ul]) + end + return U, Y, Ry +end +U_data, Y_data, Ry_data = test_mpc_d(mpc_d, model) +res = SimResult(mpc, U_data, Y_data; Ry_data) +plot(res) + +## ========================================= +## ========= Benchmark ===================== +## ========================================= +using BenchmarkTools +using JuMP, OSQP, DAQP + +if run_benchmarks + optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) + mpc_d_osqp = setconstraint!(LinMPC(model_d; optim), ymin=[45, -Inf]) + JuMP.unset_time_limit_sec(mpc_d_osqp.optim) + bm = @benchmark test_mpc_d($mpc_d_osqp, $model) samples=500 + @show btime_solver_OS = median(bm) + + optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) + mpc_d_daqp = setconstraint!(LinMPC(model_d; optim), ymin=[45, -Inf]) + bm = @benchmark test_mpc_d($mpc_d_daqp, $model) samples=500 + @show btime_solver_AS = median(bm) +end \ No newline at end of file diff --git a/examples/manual_estim.jl b/examples/manual_estim.jl new file mode 100644 index 000000000..b0c13a4d2 --- /dev/null +++ b/examples/manual_estim.jl @@ -0,0 +1,26 @@ +using ModelPredictiveControl, ControlSystemsBase + +function man_sim() + f(x,u,_,_) = 0.5*sin.(x + u) + h(x,_,_) = x + model = NonLinModel(f, h, 10.0, 1, 1, 1, solver=nothing) + linModel = linearize(model, x=[0], u=[0]) + man = ManualEstimator(linModel, nint_u=[1]) + mpc = LinMPC(man) + estim = MovingHorizonEstimator(model, nint_u=[1], He=5) + estim = setconstraint!(estim, v̂min=[-0.001], v̂max=[0.001]) + initstate!(estim, [0], [0]) + y_data, ŷ_data = zeros(5), zeros(5) + for i=1:5 + y = model() # simulated measurement + x̂ = preparestate!(estim, y) # correct nonlinear MHE state estimate + ŷ = estim() # nonlinear MHE estimated output + setstate!(mpc, x̂) # update MPC with the MHE corrected state + u = moveinput!(mpc, [0]) + y_data[i], ŷ_data[i] = y[1], ŷ[1] + updatestate!(estim, u, y) # update nonlinear MHE estimation + updatestate!(model, u .+ 0.5) # update plant simulator with load disturbance + end + return collect([y_data ŷ_data]') +end +YandŶ = round.(man_sim(), digits=6) \ No newline at end of file diff --git a/examples/multithreads.jl b/examples/multithreads.jl new file mode 100644 index 000000000..63510c948 --- /dev/null +++ b/examples/multithreads.jl @@ -0,0 +1,52 @@ +using Base.Threads: nthreads, @threads, @spawn +using Base.Iterators: partition + +a = zeros(10) + +Threads.@threads for i = 1:10 + a[i] = Threads.threadid() +end + +using ModelPredictiveControl, ControlSystemsBase + +f! = (ẋ, x, u, _, _) -> (ẋ .= 0.1*(2*u[]-x[])) +h! = (y, x, _, _) -> (y .= x) +model = NonLinModel(f!, h!, 2.0, 1, 1, 1) +mpc = NonLinMPC(model, nint_ym=0, Hp=5, transcription=TrapezoidalCollocation(f_threads=false)) + +nu = model.nu +nk = model.nk +nx̂ = mpc.estim.nx̂ +Hp, Hc = mpc.Hp, mpc.Hc +geq = -1*ones(nx̂*Hp) +X̂0 = -1*ones(nx̂*Hp) +Û0 = -1*ones(nu*Hp) +K0 = -1*ones(nk*Hp) + +ΔU = zeros(nu*Hc) +U0 = ones(nu*Hp) +X̂0_Z̃ = sim!(model, 6).X_data[2:end] +ϵ = 0.0 +Z̃ = [ΔU; X̂0_Z̃; ϵ] + +ModelPredictiveControl.con_nonlinprogeq!( + geq, X̂0, Û0, K0, + mpc, model, mpc.transcription, U0, Z̃ +) +println(geq) +all(geq .≤ 5e-3) || error("NOT ALL 0.0 VALUES IN GEQ VECTOR!") +#= +@profview_allocs for i in 1:10000 + ModelPredictiveControl.con_nonlinprogeq!( + geq, X̂0, Û0, K0, + mpc, model, mpc.transcription, U0, Z̃ +) end + =# + +using BenchmarkTools +@btime ModelPredictiveControl.con_nonlinprogeq!( + $geq, $X̂0, $Û0, $K0, + $mpc, $model, $mpc.transcription, $U0, $Z̃ +) + + diff --git a/examples/mytests-mpc.jl b/examples/mytests-mpc.jl new file mode 100644 index 000000000..dbbbf7100 --- /dev/null +++ b/examples/mytests-mpc.jl @@ -0,0 +1,499 @@ +# spell-checker: disable + +using Revise + +using ModelPredictiveControl + +using JuMP, DAQP, Ipopt, OSQP +using LinearAlgebra +using ControlSystemsBase +using BenchmarkTools + +using PlotThemes, Plots +#theme(:default) +theme(:dark) +default(fontfamily="Computer Modern"); scalefontsizes(1.1) + + +Ts = 4.0 +A = [ 0.800737 0.0 0.0 0.0 + 0.0 0.606531 0.0 0.0 + 0.0 0.0 0.8 0.0 + 0.0 0.0 0.0 0.6 ] +Bu = [ 0.378599 0.378599 + -0.291167 0.291167 + 0.0 0.0 + 0.0 0.0 ] +Bd = [ 0; 0; 0.5; 0.5;; ] +C = [ 1.0 0.0 0.684 0.0 + 0.0 1.0 0.0 -0.4736 ] +Dd = [ 0.19; -0.148;; ] +Du = zeros(2, 2) + +linModel1 = LinModel(ss(A,Bu,C,0,Ts),Ts) +linModel2 = LinModel(ss(A,[Bu Bd],C,[Du Dd],Ts),Ts,i_d=[3]) +sys = [ tf(1.90,[18.0,1]) tf(1.90,[18.0,1]) tf(1.90,[18.0,1]); + tf(-0.74,[8.0,1]) tf(0.74,[8.0,1]) tf(-0.74,[8.0,1]) ] +linModel3 = LinModel(sys,Ts,i_d=[3]) +linModel4 = setop!(LinModel(A, Bu, C, Bd, Dd, Ts), uop=[10,10],yop=[50,30],dop=[5]) +linModel5 = LinModel([delay(4) delay(8)]*sys,Ts,i_d=[3]) + +f(x,u,_,_) = A*x + Bu*u +h(x,_,_) = C*x + +function f2!(xnext, x, u, d, (A, Bu, Bd, C, Dd)) + mul!(xnext, A, x) + mul!(xnext, Bu, u, 1, 1) + mul!(xnext, Bd, d, 1, 1) + return xnext +end +function h2!(y, x, d, (A, Bu, Bd, C, Dd)) + mul!(y, C, x) + mul!(y, Dd, d, 1, 1) + return y +end + +nonLinModel1 = NonLinModel{Float32}(f,h,Ts,2,4,2,solver=nothing) +nonLinModel2 = setop!( + NonLinModel(f2!,h2!,Ts,2,4,2,1,p=(A, Bu, Bd, C, Dd), solver=nothing), + uop=[10,10],yop=[50,30],dop=[5] + ) +#= +nonLinModel2 = setop!(NonLinModel( + (x,u,d)->A*x+Bu*u+Bd*d, + (x,d) ->C*x+Dd*d, + Ts , 2, 4, 2, 1), uop=[10,10], yop=[50,30], dop=[5] +) +=# +f3!, h3! = let A=[0 0.5; -0.2 -0.1], Bu=[0; 0.5], Bd=[0; 0.5], C=[0.4 0], Dd=[0] + function f!(dx, x, u, d, _ ) + mul!(dx, A, x) + mul!(dx, Bu, u, 1, 1) + mul!(dx, Bd, d, 1, 1) + return nothing + end + function h!(y, x, d, _ ) + mul!(y, C, x) + mul!(y, Dd, d, 1, 1) + return nothing + end + f!, h! +end +nonLinModel3 = NonLinModel(f3!, h3!, 1.0, 1, 2, 1, 1, solver=RungeKutta(4, supersample=2)) + +plot(sim!(nonLinModel3, 100)) + +linearizeModel = linearize(nonLinModel3, x=[1,1], u=[1], d=[1]) + + +# @btime linearize!($linearizeModel, $nonLinModel3, x=$[2,2], u=$[1], d=$[1]) +@profview for i=1:10000; linearize!(linearizeModel, nonLinModel3, x=[2,2], u=[1], d=[1]); end; + + +internalModel1 = InternalModel(linModel1) +internalModel2 = InternalModel(linModel1,stoch_ym=[tf([1,0],[1,-1],Ts) 0; 0 tf([1,0],[1,-1],Ts)]) +internalModel3 = InternalModel(linModel1,i_ym=[2]) +internalModel3 = InternalModel(nonLinModel2) + + +initstate!(internalModel1,[0,0],[1,1]) + +preparestate!(internalModel1, [0, 0]) +updatestate!(internalModel1, [1, 1], [1, 1]) + +luenberger = Luenberger(linModel1) + +preparestate!(luenberger, [0,0]) +updatestate!(luenberger, [1,1], [1,1]) + + +mpcluen = LinMPC( + luenberger, + M_Hp=Diagonal(collect(1.01:0.01:1.2)), + N_Hc=Diagonal([0.1,0.11,0.12,0.13]), + L_Hp=Diagonal(collect(0.001:0.001:0.02)) +) + +initstate!(mpcluen, [0,0], [0,0]) + +preparestate!(mpcluen, [0,0]) +moveinput!(mpcluen, [10, 10]) + +mpc_ms = LinMPC(linModel4, transcription=MultipleShooting(), Hp=3, Hc=3) + + + + +mhe = MovingHorizonEstimator(nonLinModel2, He=5, Cwt=1e5, direct=true) + +unset_time_limit_sec(mhe.optim) +#set_attribute(mhe.optim, "nlp_scaling_max_gradient", 1/mhe.C*10.0)# 5*1e-5) +#set_attribute(mhe.optim, "nlp_scaling_constr_target_gradient", 1e-4) +#set_attribute(mhe.optim, "nlp_scaling_obj_target_gradient" , 100*1e-4) +mhe = setconstraint!(mhe, x̂min=[-10,-10,-10,-10,-10,-10], X̂max=[1;1;1;1;10;10;fill(1.5,6*5)]) +mhe = setconstraint!(mhe, V̂min=-1.1:-0.1:-2.0,v̂max=[0.1,0.1]) +#mhe = setconstraint!(mhe, ŵmin=fill(-0.1,6)) + +initstate!(mhe, [11, 11], [50, 30], [5]) + +res_mhe1 = sim!(mhe, 30, x_0=[0,0,0,0], x̂_0=[0,0,0,0,0,0]) +@time sim!(mhe, 30, x_0=[0,0,0,0], x̂_0=[0,0,0,0,0,0]) +info = getinfo(mhe) +p = plot(res_mhe1, plotd=false, plotu=false, plotxwithx̂=true, plotx̂min=false) +display(p) + + +mhe2 = MovingHorizonEstimator(linModel4, He=5, direct=true) + +setmodel!(mhe2, linModel4) + +unset_time_limit_sec(mhe2.optim) +mhe2 = setconstraint!(mhe2, x̂min=[-10,-10,-10,-10,-10,-10], X̂max=[1;1;1;1;10;10;fill(1.5,6*5)]) +mhe2 = setconstraint!(mhe2, V̂min=-1.1:-0.1:-2.0,v̂max=[0.1,0.1]) +#mhe2 = setconstraint!(mhe2, ŵmin=fill(-0.1,6)) + +preparestate!(mhe2, [50, 30], [5]) +updatestate!(mhe2, [10, 11], [50, 30], [5]) + + + +res_mhe2 = sim!(mhe2, 30, x_0=[0,0,0,0], x̂_0=[0,0,0,0,0,0]) +@time sim!(mhe2, 30, x_0=[0,0,0,0], x̂_0=[0,0,0,0,0,0]) +display(getinfo(mhe2)) +p = plot(res_mhe2, plotd=false, plotu=false, plotxwithx̂=true, plotx̂min=false) +display(p) + + + +mpcexplicit = ExplicitMPC(LinModel(append(tf(3,[2, 1]), tf(2, [6, 1])), 0.1), Hp=10000, Hc=1) +preparestate!(mpcexplicit, [0, 0]) +moveinput!(mpcexplicit, [10, 10]) + +kalmanFilter1 = KalmanFilter(linModel1) +setmodel!(kalmanFilter1, linModel1) + + +kalmanFilter2 = KalmanFilter(linModel1,nint_ym=0,direct=false) + +updatestate!(kalmanFilter2,[1, 1],[1, 1]) + +initstate!(kalmanFilter1,[0,0],[2,1]) + +ssKalmanFilter1 = SteadyKalmanFilter(linModel1) + +preparestate!(ssKalmanFilter1, [0, 0]) +updatestate!(ssKalmanFilter1, [1, 1], [1, 1]) + + +ssKalmanFilter2 = SteadyKalmanFilter(linModel1,nint_ym=0,direct=false) +snxX̂sKalmanFilter3 = SteadyKalmanFilter(linModel1,i_ym=[2],nint_u=[0, 1], nint_ym=[1]) + +updatestate!(ssKalmanFilter2,[1, 1],[1,1]) + +initstate!(ssKalmanFilter1,[0,0],[2,1]) + +extKalmanFilter = ExtendedKalmanFilter(nonLinModel2, direct=false) + +initstate!(extKalmanFilter, [10,10], [50,30], [5]) +updatestate!(extKalmanFilter, [10,11], [50,30], [5]) + +mpc_t = setconstraint!(LinMPC(LinModel(ss(0.5, 0.5, 1, 0, 1.0)), Hp=5), x̂min=[-1e-3,-Inf], x̂max=[1e-3,+Inf]) +preparestate!(mpc_t, [0]) +moveinput!(mpc_t, [100]) +println(getinfo(mpc_t)[:Ŷ]) + +f_t(x,u,_,_) = 0.5*x + 0.5*u +h_t(x,_,_) = x +nmpc_t = setconstraint!( + NonLinMPC(NonLinModel(f_t,h_t,0.001, 1, 1, 1, solver=nothing), Cwt=Inf, Hp=5), + x̂min=[-1e-3,-Inf], + x̂max=[1e-3,+Inf] +) + +preparestate!(nmpc_t, [0]) +moveinput!(nmpc_t, [100]) +println(getinfo(nmpc_t)[:Ŷ]) + + +uscKalmanFilter1 = UnscentedKalmanFilter(linModel1, nint_u=[1, 1]) + +preparestate!(uscKalmanFilter1, [0, 0]) +updatestate!(uscKalmanFilter1,[0,0],[2,1]) + +initstate!(uscKalmanFilter1,[0,0],[2,1]) + +nmpc1 = NonLinMPC(uscKalmanFilter1) + + +nmpc2 = NonLinMPC(nonLinModel2, Hp=15, Hc=1, Mwt=[1, 1] , Nwt=[0.1, 0.1], Cwt=1e5) + + +setconstraint!(nmpc2, c_umin=[0,0], c_umax=[0,0]) +setconstraint!(nmpc2, c_ymin=[1,1], c_ymax=[1,1]) +setconstraint!(nmpc2, umin=[5, 9.9], umax=[Inf,Inf]) +setconstraint!(nmpc2, ymin=[-Inf,-Inf], ymax=[55, 35]) +setconstraint!(nmpc2, Δumin=[-Inf,-Inf],Δumax=[+Inf,+Inf]) + + +nx = linModel4.nx +kf = KalmanFilter(linModel2, σP_0=10*ones(nx), σQ=0.01*ones(nx), σR=[0.1, 0.1], σQint_ym=0.05*ones(2), σPint_ym_0=10*ones(2)) + +setmodel!(kf, linModel4) + +mpc = LinMPC(kf, Hp=15, Hc=2, Mwt=[1, 1], Cwt=1e5)#, optim=Model(DAQP.Optimizer, add_bridges=false)) + + +setconstraint!(mpc, c_umin=[0,0], c_umax=[0,0]) +setconstraint!(mpc, c_ymin=[1,1], c_ymax=[1,1]) +setconstraint!(mpc, umin=[5, 9.9], umax=[Inf,Inf]) +setconstraint!(mpc, Δumin=[-Inf,-Inf],Δumax=[+Inf,+Inf]) +setconstraint!(mpc, ymin=[-Inf,-Inf], ymax=[1000, 1000]) + +setmodel!(mpc, linModel4) + +preparestate!(mpc, mpc.estim.model.yop, mpc.estim.model.dop) +moveinput!(mpc, [60, 40], mpc.estim.model.dop) +info = getinfo(mpc) + +setconstraint!(mpc, ymax=[55, 35]) +moveinput!(mpc, [60, 40], mpc.estim.model.dop) +info = getinfo(mpc) + + +empc = ExplicitMPC(linModel4, Hp=15, Hc=1, Mwt=[1, 1] , Nwt=[0.1, 0.1]) +sim!(empc, empc.Hp+10, x_0=[0,0,0,0]) +@time sim!(empc, empc.Hp+10, x_0=[0,0,0,0]) + +nmpc = NonLinMPC(kf, Hp=15, Hc=1, Mwt=[1, 1] , Nwt=[0.1, 0.1], Cwt=1e5, Ewt=0.0) + +setconstraint!(nmpc, c_umin=[0,0], c_umax=[0,0]) +setconstraint!(nmpc, c_ymin=[1,1], c_ymax=[1,1]) +setconstraint!(nmpc, umin=[5, 9.9], umax=[Inf,Inf]) +setconstraint!(nmpc, ymin=[-Inf,-Inf], ymax=[55, 35]) +setconstraint!(nmpc, Δumin=[-Inf,-Inf],Δumax=[+Inf,+Inf]) + +preparestate!(nmpc, nmpc.estim.model.yop, nmpc.estim.model.dop) +moveinput!(nmpc, nmpc.estim.model.yop + [0, 2], nmpc.estim.model.dop) +display(getinfo(nmpc)) + +if Sys.free_memory()/2^30 < 6.0 + GC.gc() +end + +function test_mpc(model, mpc, Nrepeat=1) + N = 200 + u_data = zeros(model.nu,N) + y_data = zeros(model.ny,N) + r_data = zeros(model.ny,N) + d_data = zeros(model.nd,N) + for j=1:Nrepeat + u = model.uop + d = model.dop + r = [50,32] + setstate!(model, zeros(model.nx)) + setstate!(mpc, zeros(mpc.estim.nx̂)) + initstate!(mpc,u,model(d),d) + for k = 0:N-1 + if k == 40 + r[2] = 29 + end + if k == 100 + r[2] = 36 + end + if k == 150 + d = [3] + end + y = model(d) + if k ≥ 180 + y[1] += 15 + end + preparestate!(mpc, y, d) + u = moveinput!(mpc, r, d) + u_data[:,k+1] = u + y_data[:,k+1] = y + r_data[:,k+1] = r + d_data[:,k+1] = d + updatestate!(mpc, u, y, d) + updatestate!(model, u, d) + end + end + return u_data, y_data, r_data, d_data +end + +test_mpc(linModel4 , mpc) +@btime test_mpc($linModel4 , $mpc, 100) + +@profview test_mpc(linModel4, mpc, 2500) +@profview test_mpc(nonLinModel2, nmpc2, 3) + +resM = sim!(nonLinModel2, mpc.Hp+10, [1,-1]) +psM = plot(resM) +display(psM) + +res = sim!(mpc, mpc.Hp+10, x_0=[0,0,0,0]) +ps = plot(res, plotx=true) +display(ps) + +res_manual = SimResult(res.obj, res.U_data, res.Y_data; Ry_data=res.Ry_data) +ps_manual = plot(res_manual) +display(ps_manual) + +res2 = sim!(uscKalmanFilter1, mpc.Hp+10) +# @time sim!(uscKalmanFilter1, mpc.Hp+10) +ps2 = plot(res2, plotxwithx̂=true) +display(ps2) + +test_mpc(linModel4, nmpc) +@time u_data, y_data, r_data, d_data = test_mpc(linModel4, nmpc) + +test_mpc(nonLinModel2, nmpc2) +@time u_data, y_data, r_data, d_data = test_mpc(nonLinModel2, nmpc2) + +N = size(r_data, 2) +p1 = plot(0:N-1,y_data[1,:],label=raw"$y_1$") +plot!(0:N-1,r_data[1,:],label=raw"$r_1$",linestyle=:dash, linetype=:steppost) +p2 = plot(0:N-1,y_data[2,:],label=raw"$y_2$") +plot!(0:N-1,r_data[2,:],label=raw"$r_2$",linestyle=:dash, linetype=:steppost) +py = plot(p1,p2, layout=[1,1]) + +p1 = plot(0:N-1,u_data[1,:],label=raw"$u_1$",linetype=:steppost) +p2 = plot(0:N-1,u_data[2,:],label=raw"$u_2$",linetype=:steppost) +pu = plot(p1,p2, layout=[1,1]) + +pd = plot(0:N-1,d_data[1,:],label=raw"$d_1$") + +display(pd) +display(pu) +display(py) + + + + +using ModelPredictiveControl, JuMP, Plots +function f!(ẋ, x, u, _ , p) + g, L, K, m = p # [m/s²], [m], [kg/s], [kg] + θ, ω = x[1], x[2] # [rad], [rad/s] + τ = u[1] # [Nm] + ẋ[1] = ω + ẋ[2] = -g/L*sin(θ) - K/m*ω + τ/m/L^2 + return nothing +end +h!(y, x, _ , _ ) = (y[1] = 180/π*x[1]; nothing) # [°] +p = [9.8, 0.4, 1.2, 0.3] +nu, nx, ny, Ts = 1, 2, 1, 0.1 +vu, vx, vy = ["\$τ\$ (Nm)"], ["\$θ\$ (rad)", "\$ω\$ (rad/s)"], ["\$θ\$ (°)"] + +model = setname!(NonLinModel(f!, h!, Ts, nu, nx, ny; p); u=vu, x=vx, y=vy) + +p_plant = copy(p); p_plant[3] = p[3]*1.25 +plant = setname!(NonLinModel(f!, h!, Ts, nu, nx, ny; p=p_plant); u=vu, x=vx, y=vy) + +N = 35 + +sim!(model, N) |> plot |> display + + +Hp, Hc, Mwt, Nwt = 20, 2, [0.5], [2.5] +α=0.01; σQ=[0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1] +σQint_ym = zeros(0) + +function gc!(LHS, Ue, Ŷe, D̂e, p, ϵ) + Hp, umax = p + for i = 1:Hp+1 + LHS[i] = Ue[i] - umax[1] + end + return nothing +end + + +function gc(Ue, Ŷe, D̂e, p, ϵ) + LHS = similar(Ue) + Hp, umax = p + for i = 1:Hp+1 + LHS[i] = Ue[i] - umax[1] + end + return LHS +end + + +umin, umax = [-1.5], [+1.5] +nc = Hp+1 +transcription = MultipleShooting()#SingleShooting()#TrapezoidalCollocation(f_threads=true) +using UnoSolver +optim = Model(()->UnoSolver.Optimizer(preset="filtersqp")); +oracle = true +using DifferentiationInterface, SparseConnectivityTracer, SparseMatrixColorings +import ForwardDiff +hessian = AutoSparse( + AutoForwardDiff(); + sparsity_detector = TracerSparsityDetector(), + coloring_algorithm = GreedyColoringAlgorithm( decompression=:substitution) +) +#nint_u = 0 +#σQint_u = zeros(0) +#σQint_ym = [0.1] + +nmpc = NonLinMPC(model; + Hp, Hc, Mwt, Nwt, Cwt=Inf, p=(Hp,umax), transcription, oracle, hessian, optim, + α, σQ, σR, nint_u, σQint_u, σQint_ym +) +nmpc = setconstraint!(nmpc; umin, umax) +unset_time_limit_sec(nmpc.optim) + +res_ry = sim!(nmpc, N, [180.0]; plant, x_0=[0, 0], x̂_0=[0, 0, 0]) +display(plot(res_ry)) +#@benchmark sim!($nmpc, $N, $[180.0]; plant=$plant, x_0=$[0, 0], x̂_0=$[0, 0, 0]) samples=50 seconds=1*60 +@profview sim!(nmpc, 1, [180.0]; plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0]) + + +#TODO: investigate the numerical error with debug log and find if its related to UKF + + +#= +mhe = MovingHorizonEstimator(model, He=2, nint_ym=0) +unset_time_limit_sec(mhe.optim) +mhe = setconstraint!(mhe, v̂min=[-1], v̂max=[1]) +using Logging; debuglogger = ConsoleLogger(stderr, Logging.Debug) +res_mhe = #with_logger(debuglogger) do +sim!(mhe, N, [1.0]; plant=plant, x_0=[0, 0], x̂_0=[0, 0]) +#end +@time sim!(mhe, N, [1.0]; plant=plant, x_0=[0, 0], x̂_0=[0, 0]) +display(plot(res_mhe)) + + + +preparestate!(nmpc, [0]) +u = moveinput!(nmpc, [180]) +updatestate!(nmpc, u, [0]) +preparestate!(nmpc, [0]) + + + +linModelA = LinModel(ss(0.5, 0.5, 1, 0, 1.0)) + + +function gcLin!(LHS, Ue, Ŷe, D̂e, p, ϵ) + Hp = p + ymax = 1.0 + for i = 1:Hp+1 + LHS[i] = Ŷe[i] - ymax[1] + end + return nothing +end + +Hp=5 +nmpcA = NonLinMPC(linModelA, Hp=Hp, p=Hp, gc=gcLin!, nc=Hp+1) +unset_time_limit_sec(nmpcA.optim) + +preparestate!(nmpcA, [0]) +u = moveinput!(nmpcA, [1.0]) +updatestate!(nmpcA, u, [0]) +preparestate!(nmpcA, [0]) + +res_ryA = sim!(nmpcA, N, [2.0]) +display(plot(res_ryA)) + +#@btime sim!($nmpcA, $N, $[2.0]) samples=50 seconds=10*60 + +nothing +=# \ No newline at end of file diff --git a/examples/newtest.jl b/examples/newtest.jl new file mode 100644 index 000000000..0bb953c70 --- /dev/null +++ b/examples/newtest.jl @@ -0,0 +1,76 @@ +using ModelPredictiveControl, Preferences +set_preferences!(ModelPredictiveControl, "precompile_workload" => false; force=true) + +using ModelPredictiveControl, ControlSystemsBase +using JuMP, Ipopt, MadNLP +using BenchmarkTools + +Ts = 4.0 +A = [ 0.800737 0.0 0.0 0.0 + 0.0 0.606531 0.0 0.0 + 0.0 0.0 0.8 0.0 + 0.0 0.0 0.0 0.6 ] +Bu = [ 0.378599 0.378599 + -0.291167 0.291167 + 0.0 0.0 + 0.0 0.0 ] +Bd = [ 0; 0; 0.5; 0.5;; ] +C = [ 1.0 0.0 0.684 0.0 + 0.0 1.0 0.0 -0.4736 ] +Dd = [ 0.19; -0.148;; ] +Du = zeros(2,2) +model = LinModel(ss(A,[Bu Bd],C,[Du Dd],Ts),Ts,i_d=[3]) +model = setop!(model, uop=[10,10], yop=[50,30], dop=[5]) + +using BenchmarkTools + +updatestate!(model, [0.0, 0.0], [0.0]) +#@btime updatestate!($model, $[0.0, 0.0], $[0.0]) +#@ballocations updatestate!($model, $[0.0, 0.0], $[0.0]) +#@btime evaloutput($model, $[0.0]) + +#mpc = LinMPC(model, Hp=10, Hc=[1, 3, 3, 1, 2], transcription=MultipleShooting())#, Cwt=Inf) +#mpc = setconstraint!(mpc, ymin=[48,29],ymax=[52,30.5]) + +f(x,u,d,p) = p.A*x + p.Bu*u + p.Bd*d +h(x,d,p) = p.C*x + p.Dd*d +model = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=(;A,Bu,Bd,C,Dd)) +model = setop!(model, uop=[10,10], yop=[50,30], dop=[5]) + +#using Logging; debuglogger = ConsoleLogger(stderr, Logging.Debug) +# = with_logger(debuglogger) do +#end + +using DifferentiationInterface, SparseConnectivityTracer, SparseMatrixColorings +import ForwardDiff, Symbolics + +# using JuMP, MadNLP +# optim = Model(MadNLP.Optimizer) + +hessian = false + +mpc = NonLinMPC(model; Hp=10, transcription=MultipleShooting(), Cwt=Inf, hessian) +mpc = setconstraint!(mpc, ymin=[48,29],ymax=[52,30.5]) +#unset_silent(mpc.optim) +using JuMP; unset_time_limit_sec(mpc.optim) +res = sim!(mpc, 15, x̂_0=zeros(6), x_0=zeros(4)) +obj = mpc + +# mhe = MovingHorizonEstimator(model; He=10, hessian)#, optim) +# using JuMP; unset_time_limit_sec(mhe.optim) +# mhe = setconstraint!(mhe, x̂max=[0.0,Inf,Inf,Inf,Inf,Inf]) +# #unset_silent(mhe.optim) +# res = sim!(mhe, 15, x̂_0=zeros(6)) +# obj = mhe + +using PlotThemes, Plots +#theme(:default) +theme(:dark) +default(fontfamily="Computer Modern"); scalefontsizes(1.1) +plot(res) |> display + +#using Logging; debuglogger = ConsoleLogger(stderr, Logging.Debug) +#with_logger(debuglogger) do +@benchmark sim!($obj, 15, x̂_0=$zeros(6), x_0=$zeros(4)) seconds = 30 +#end + diff --git a/examples/nonlin_mpc_article.jl b/examples/nonlin_mpc_article.jl new file mode 100644 index 000000000..42c9b00c1 --- /dev/null +++ b/examples/nonlin_mpc_article.jl @@ -0,0 +1,342 @@ +# ========================================== +# ========== GLOBAL SETTINGS =============== +# ========================================== +run_benchmarks = true +benchmark_knitro = false # put false if no KNITRO license: +benchmark_madnlp = false + +# ========================================== +# ========== STATE ESTIMATOR =============== +# ========================================== +using ModelPredictiveControl +function f!(ẋ, x, u, _ , p) + g, L, K, m = p # [m/s²], [m], [kg/s], [kg] + θ, ω = x[1], x[2] # [rad], [rad/s] + τ = u[1] # [Nm] + ẋ[1] = ω + ẋ[2] = -g/L*sin(θ) - K/m*ω + τ/m/L^2 +end +h!(y, x, _ , _ ) = (y[1] = 180/π*x[1]) # [°] +p = [9.8, 0.4, 1.2, 0.3] +nu = 1; nx = 2; ny = 1; Ts = 0.1 +model = NonLinModel(f!, h!, Ts, nu, nx, ny; p) +vu = ["\$τ\$ (Nm)"] +vx = ["\$θ\$ (rad)", "\$ω\$ (rad/s)"] +vy = ["\$θ\$ (°)"] +model = setname!(model; u=vu, x=vx, y=vy) + +## ========================================= +σQ = [0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1] +estim = UnscentedKalmanFilter(model; σQ, σR, nint_u, σQint_u) + +## ========================================= +p_plant = copy(p); p_plant[3] = 1.25*p[3] +plant = NonLinModel(f!, h!, Ts, nu, nx, ny; p=p_plant) +N = 35; u = [0.5]; +res = sim!(estim, N, u; plant, y_noise=[0.5]) +using Plots; plot(res, plotu=false, plotxwithx̂=true) |> display + +## ========================================== +## ========== NONLINEAR MPC ================= +## ========================================== + +## ========================================= +Hp, Hc, Mwt, Nwt, Cwt = 20, 2, [0.5], [2.5], Inf +#using DifferentiationInterface, SparseConnectivityTracer, SparseMatrixColorings +#import Symbolics, FastDifferentiation +#backend = #AutoSparse( + #AutoFastDifferentiation()#AutoSymbolics(); + #sparsity_detector = TracerSparsityDetector(), + #coloring_algorithm = GreedyColoringAlgorithm() +#) +nmpc = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt) +umin, umax = [-1.5], [+1.5] +nmpc = setconstraint!(nmpc; umin, umax) + +## ========================================= +using JuMP; unset_time_limit_sec(nmpc.optim) + +## ========================================= +x_0 = [0, 0]; x̂_0 = [0, 0, 0]; ry = [180] +res_ry = sim!(nmpc, N, ry; plant, x_0, x̂_0) +@profview sim!(nmpc, N, ry; plant, x_0, x̂_0) +plot(res_ry) |> display + +## ========================================= +## ========= Benchmark ===================== +## ========================================= +using BenchmarkTools +using JuMP, Ipopt, KNITRO +using MadNLP + +if run_benchmarks + optim = JuMP.Model(Ipopt.Optimizer, add_bridges=false) + nmpc_ipopt = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim) + nmpc_ipopt = setconstraint!(nmpc_ipopt; umin, umax) + JuMP.unset_time_limit_sec(nmpc_ipopt.optim) + bm = @benchmark( + sim!($nmpc_ipopt, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + samples=50, + seconds=10*60 + ) + @show btime_NMPC_track_solver_IP = median(bm) + + if benchmark_madnlp + optim = JuMP.Model(MadNLP.Optimizer, add_bridges=false) + nmpc_madnlp = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim) + nmpc_madnlp = setconstraint!(nmpc_madnlp; umin, umax) + JuMP.unset_time_limit_sec(nmpc_madnlp.optim) + bm = @benchmark( + sim!($nmpc_madnlp, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + samples=50, + seconds=10*60 + ) + @show btime_NMPC_track_solver_IP2 = median(bm) + end + + if benchmark_knitro + optim = JuMP.Model(KNITRO.Optimizer, add_bridges=false) + set_attribute(optim, "algorithm", 4) # 4th algorithm is SQP + nmpc_knitro = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim) + nmpc_knitro = setconstraint!(nmpc_knitro; umin, umax) + JuMP.unset_time_limit_sec(nmpc_knitro.optim) + bm = @benchmark( + sim!($nmpc_knitro, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + samples=50, + seconds=10*60 + ) + @show btime_NMPC_track_solver_SQ = median(bm) + end +end + + +## ========================================= +x_0 = [π, 0]; x̂_0 = [π, 0, 0]; y_step = [10] +res_yd = sim!(nmpc, N, [180.0]; plant, x_0, x̂_0, y_step) +plot(res_yd) |> display + +## ========================================= +## ========= Benchmark ===================== +## ========================================= +if run_benchmarks + bm = @benchmark( + sim!($nmpc_ipopt, $N, $[180.0]; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, y_step=$y_step), + samples=50, + seconds=10*60 + ) + @show btime_NMPC_regul_solver_IP = median(bm) + + if benchmark_madnlp + bm = @benchmark( + sim!($nmpc_madnlp, $N, $[180.0]; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, y_step=$y_step), + samples=50, + seconds=10*60 + ) + @show btime_NMPC_regul_solver_IP2 = median(bm) + end + + if benchmark_knitro + bm = @benchmark( + sim!($nmpc_knitro, $N, $[180.0]; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, y_step=$y_step), + samples=50, + seconds=10*60 + ) + @show btime_NMPC_regul_solver_SQ = median(bm) + end +end + +# ========================================== +# ========== ECONOMIC MPC ================== +# ========================================== +h2!(y, x, _ , _ ) = (y[1] = 180/π*x[1]; y[2]=x[2]) +nu, nx, ny = 1, 2, 2 +model2 = NonLinModel(f!, h2!, Ts, nu, nx, ny; p) +plant2 = NonLinModel(f!, h2!, Ts, nu, nx, ny; p=p_plant) +model2 = setname!(model2, u=vu, x=vx, y=[vy; vx[2]]) +plant2 = setname!(plant2, u=vu, x=vx, y=[vy; vx[2]]) +estim2 = UnscentedKalmanFilter(model2; σQ, σR, + nint_u, σQint_u, i_ym=[1]) + + +## ========================================= +function JE(UE, ŶE, _ , p) + Ts = p + τ, ω = UE[1:end-1], ŶE[2:2:end-1] + return Ts*sum(τ.*ω) +end +p = Ts; Mwt2 = [Mwt; 0.0]; Ewt = 3.5e3 +empc = NonLinMPC(estim2; Hp, Hc, + Nwt, Mwt=Mwt2, Cwt, JE, Ewt, p) +empc = setconstraint!(empc; umin, umax) + +## ========================================= +using JuMP; unset_time_limit_sec(empc.optim) + +## ========================================= +x_0 = [0, 0]; x̂_0 = [0, 0, 0]; ry = [180; 0] +res2_ry = sim!(empc, N, ry; plant=plant2, x_0, x̂_0) +plot(res2_ry, ploty=[1]) |> display + +## ========================================= +function calcW(res) + τ, ω = res.U_data[1, 1:end-1], res.X_data[2, 1:end-1] + return Ts*sum(τ.*ω) +end +display(Dict(:W_nmpc => calcW(res_ry), :W_empc => calcW(res2_ry))) + +## ========================================= +## ========= Benchmark ===================== +## ========================================= +using BenchmarkTools +using JuMP, Ipopt, KNITRO + +if run_benchmarks + optim = JuMP.Model(Ipopt.Optimizer, add_bridges=false) + empc_ipopt = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=Mwt2, Cwt, JE, Ewt, optim, p) + empc_ipopt = setconstraint!(empc_ipopt; umin, umax) + JuMP.unset_time_limit_sec(empc_ipopt.optim) + bm = @benchmark( + sim!($empc_ipopt, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + samples=50, + seconds=10*60 + ) + @show btime_EMPC_track_solver_IP = median(bm) + + if benchmark_madnlp + optim = JuMP.Model(MadNLP.Optimizer, add_bridges=false) + empc_madnlp = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=Mwt2, Cwt, JE, Ewt, optim, p) + empc_madnlp = setconstraint!(empc_madnlp; umin, umax) + JuMP.unset_time_limit_sec(empc_madnlp.optim) + bm = @benchmark( + sim!($empc_madnlp, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + samples=50, + seconds=10*60 + ) + @show btime_EMPC_track_solver_IP2 = median(bm) + end + + if benchmark_knitro + optim = JuMP.Model(KNITRO.Optimizer, add_bridges=false) + set_attribute(optim, "algorithm", 4) # 4th algorithm is SQP + empc_knitro = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=Mwt2, Cwt, JE, Ewt, optim, p) + empc_knitro = setconstraint!(empc_knitro; umin, umax) + JuMP.unset_time_limit_sec(empc_knitro.optim) + bm = @benchmark( + sim!($empc_knitro, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + samples=50, + seconds=10*60 + ) + @show btime_EMPC_track_solver_SQ = median(bm) + end +end + +## ========================================= +x_0 = [π, 0]; x̂_0 = [π, 0, 0]; y_step = [10; 0] +res2_yd = sim!(empc, N, ry; plant=plant2, + x_0, x̂_0, y_step) +plot(res2_yd, ploty=[1]) |> display + +## ========================================= +display(Dict(:W_nmpc => calcW(res_yd), :W_empc => calcW(res2_yd))) + +## ========================================= +## ========= Benchmark ===================== +## ========================================= +if run_benchmarks + bm = @benchmark( + sim!($empc_ipopt, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0, y_step=$y_step), + samples=50, + seconds=10*60 + ) + @show btime_EMPC_regul_solver_IP = median(bm) + + if benchmark_madnlp + bm = @benchmark( + sim!($empc_madnlp, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0, y_step=$y_step), + samples=50, + seconds=10*60 + ) + @show btime_EMPC_regul_solver_IP2 = median(bm) + end + + if benchmark_knitro + bm = @benchmark( + sim!($empc_knitro, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0, y_step=$y_step), + samples=50, + seconds=10*60 + ) + @show btime_EMPC_regul_solver_SQ = median(bm) + end +end + +## ========================================== +## ====== SUCCESSIVE LINEARIZATION MPC ====== +## ========================================== +# using Pkg; Pkg.add(["JuMP","DAQP"]) +using JuMP, DAQP +optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) + +## ========================================== +linmodel = linearize(model, x=[0, 0], u=[0]) +kf = KalmanFilter(linmodel; σQ, σR, nint_u, σQint_u) +mpc3 = LinMPC(kf; Hp, Hc, Mwt, Nwt, Cwt, optim) +mpc3 = setconstraint!(mpc3; umin, umax) + +## ========================================== +function sim2!(mpc, nlmodel, N, ry, plant, x, 𝕩̂, y_step) + U, Y, Ry = zeros(1, N), zeros(1, N), zeros(1, N) + setstate!(plant, x); setstate!(mpc, 𝕩̂) + initstate!(mpc, [0], plant()) + linmodel = linearize(nlmodel; u=[0], x=𝕩̂[1:2]) + setmodel!(mpc, linmodel) + for i = 1:N + y = plant() + y_step + 𝕩̂ = preparestate!(mpc, y) + u = mpc(ry) + linearize!(linmodel, nlmodel; u, x=𝕩̂[1:2]) + setmodel!(mpc, linmodel) + U[:,i], Y[:,i], Ry[:,i] = u, y, ry + updatestate!(mpc, u, y) + updatestate!(plant, u) + end + U_data, Y_data, Ry_data = U, Y, Ry + return SimResult(mpc, U_data, Y_data; Ry_data) +end + +## ========================================== +x_0 = [0, 0]; x̂_0 = [0, 0, 0]; ry = [180] +res3_ry = sim2!(mpc3, model, N, ry, plant, x_0, 𝕩̂_0, [0]) +plot(res3_ry) |> display + +## ========================================= +## ========= Benchmark ===================== +## ========================================= +using BenchmarkTools + +if run_benchmarks + x_0 = [0, 0]; x̂_0 = [0, 0, 0]; ry = [180]; y_step=[0] + bm = @benchmark( + sim2!($mpc3, $model, $N, $ry, $plant, $x_0, $x̂_0, $y_step), + samples=500, + seconds=10*60 + ) + @show btime_SLMPC_track_solver_AS = median(bm) +end + +## ========================================= +x_0 = [π, 0]; 𝕩̂_0 = [π, 0, 0]; ry = [180] +res3_yd = sim2!(mpc3, model, N, ry, plant, x_0, 𝕩̂_0, [10]) +plot(res3_yd) |> display + +## ========================================= +## ========= Benchmark ===================== +## ========================================= +if run_benchmarks + x_0 = [π, 0]; x̂_0 = [π, 0, 0]; ry = [180]; y_step=[10] + bm = @benchmark( + sim2!($mpc3, $model, $N, $ry, $plant, $x_0, $x̂_0, $y_step), + samples=500, + seconds=10*60 + ) + @show btime_SLMPC_regul_solver_AS = median(bm) +end diff --git a/examples/test_di.jl b/examples/test_di.jl new file mode 100644 index 000000000..5cfa44870 --- /dev/null +++ b/examples/test_di.jl @@ -0,0 +1,22 @@ +using DifferentiationInterface, SparseConnectivityTracer, SparseMatrixColorings +import ForwardDiff +using LinearAlgebra, SparseArrays + +cons(x) = map(abs2, x) +lag(x, μ) = dot(μ, cons(x)) + +backend = AutoSparse( + AutoForwardDiff(); + sparsity_detector = TracerSparsityDetector(), + coloring_algorithm = GreedyColoringAlgorithm(), +) + +typical_x = zeros(10) +typical_μ1 = zeros(10) +typical_μ2 = spzeros(10) + +prep1 = prepare_hessian(lag, backend, typical_x, Constant(typical_μ1)); +prep2 = prepare_hessian(lag, backend, typical_x, Constant(typical_μ2)); + +display(sparsity_pattern(prep1)) +display(sparsity_pattern(prep2)) \ No newline at end of file diff --git a/examples/test_jump_hessian.jl b/examples/test_jump_hessian.jl new file mode 100644 index 000000000..40bd473e3 --- /dev/null +++ b/examples/test_jump_hessian.jl @@ -0,0 +1,63 @@ +using JuMP +import DifferentiationInterface +import ForwardDiff +import Ipopt +import Test + +f(x::T...) where {T} = (1 - x[1])^2 + 100 * (x[2] - x[1]^2)^2 + +function di_∇f( + g::AbstractVector{T}, + x::Vararg{T,N}; + backend = DifferentiationInterface.AutoForwardDiff(), +) where {T,N} + DifferentiationInterface.gradient!(splat(f), g, backend, collect(x)) + return +end + +function di_∇²f( + H::AbstractMatrix{T}, + x::Vararg{T,N}; + backend = DifferentiationInterface.AutoForwardDiff(), +) where {T,N} + H_dense = DifferentiationInterface.hessian(splat(f), backend, collect(x)) + for i in 1:N, j in 1:i + H[i, j] = H_dense[i, j] + end + return +end + +""" + di_derivatives(f::Function; backend) -> Tuple{Function,Function} + +Return a tuple of functions that evaluate the gradient and Hessian of `f` using +DifferentiationInterface.jl with any given `backend`. +""" +function di_derivatives(f::Function; backend) + function ∇f(g::AbstractVector{T}, x::Vararg{T,N}) where {T,N} + DifferentiationInterface.gradient!(splat(f), g, backend, collect(x)) + return + end + function ∇²f(H::AbstractMatrix{T}, x::Vararg{T,N}) where {T,N} + H_dense = + DifferentiationInterface.hessian(splat(f), backend, collect(x)) + for i in 1:N, j in 1:i + H[i, j] = H_dense[i, j] + end + return + end + return ∇f, ∇²f +end + +function di_rosenbrock(; backend) + model = Model(Ipopt.Optimizer) + set_silent(model) + @variable(model, x[1:2]) + @operator(model, op_rosenbrock, 2, f, di_derivatives(f; backend)...) + @objective(model, Min, op_rosenbrock(x[1], x[2])) + optimize!(model) + assert_is_solved_and_feasible(model) + return value.(x) +end + +di_rosenbrock(; backend = DifferentiationInterface.AutoForwardDiff()) \ No newline at end of file diff --git a/examples/yoo.jl b/examples/yoo.jl new file mode 100644 index 000000000..119cb9287 --- /dev/null +++ b/examples/yoo.jl @@ -0,0 +1,42 @@ +using ControlSystemsBase, LinearAlgebra, ModelPredictiveControl + +sys = [ + tf(1.90, [1800, 1]) tf(1.90, [1800, 1]); + tf(-0.74,[800, 1]) tf(0.74, [800, 1]) +] +Ts = 400.0 +model = setop!(LinModel(sys, Ts), uop=[10, 10], yop=[50, 30]) +y = model() + +sys2 = minreal(ss(sys)) + +function f!(xnext, x, u, _ , p) + A, B, _ = p + mul!(xnext, A , x) + mul!(xnext, B, u, 1, 1) + return nothing +end +function h!(y, x, _ , p) + _, _, C = p + mul!(y, C, x) + return nothing +end + +nlmodel = setop!( + NonLinModel(f!, h!, Ts, 2, 2, 2, solver=RungeKutta(4), p=(sys2.A, sys2.B, sys2.C)), + uop=[10, 10], yop=[50, 30] +) +y = nlmodel() + +function JE( _ , Ŷe, _ , R̂y) + Ŷ = @views Ŷe[3:end] + Ȳ = R̂y - Ŷ + return dot(Ȳ, Ȳ) +end +R̂y = repeat([55; 30], 10) +empc = setconstraint!( + NonLinMPC(nlmodel, Mwt=[0, 0], Hp=10, Cwt=Inf, Ewt=1, JE=JE, p=R̂y), ymin=[45, -Inf] +) +preparestate!(empc, [55, 30]) +u = empc() +sim!(empc, 2) \ No newline at end of file From 65017903b6c132d58c291f5af2114731e42336d1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 10 Dec 2025 14:01:58 -0500 Subject: [PATCH 6/6] wip: extension (still not found) --- Project.toml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index a5b5b76f4..6705afd74 100644 --- a/Project.toml +++ b/Project.toml @@ -38,6 +38,7 @@ ForwardDiff = "0.10, 1" Ipopt = "1" JuMP = "1.21" LinearAlgebra = "1.10" +LinearMPC = "0.7.0" Logging = "1.10" MathOptInterface = "1.46" OSQP = "0.8" @@ -58,11 +59,11 @@ julia = "1.10" DAQP = "c47d62df-3981-49c8-9651-128b1cd08617" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" +LinearMPC = "82e1c212-e1a2-49d2-b26a-a31d6968e3bd" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" TestItemRunner = "f8b46487-2199-4994-9208-9a1283c18c0a" TestItems = "1c621080-faea-4a02-84b6-bbd5e436b8fe" -LinearMPC = "82e1c212-e1a2-49d2-b26a-a31d6968e3bd" [targets] test = ["Test", "TestItems", "TestItemRunner", "Documenter", "Plots", "DAQP", "FiniteDiff", "LinearMPC"]