wip: testing package extension

Project.toml

@@ -22,6 +22,12 @@ SparseConnectivityTracer = "9f842d2f-2579-4b1d-911e-f412cf18a3f5"
 SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35"
 StableRNGs = "860ef19b-820b-49d6-a774-d7a799459cd3"
 
+[weakdeps]
+LinearMPC = "82e1c212-e1a2-49d2-b26a-a31d6968e3bd"
+
+[extensions]
+LinearMPCext = "LinearMPC"
+
 [compat]
 ControlSystemsBase = "1.18.2"
 DAQP = "0.6, 0.7.1"
@@ -32,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"
@@ -52,10 +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"
 
 [targets]
-test = ["Test", "TestItems", "TestItemRunner", "Documenter", "Plots", "DAQP", "FiniteDiff"]
+test = ["Test", "TestItems", "TestItemRunner", "Documenter", "Plots", "DAQP", "FiniteDiff", "LinearMPC"]

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)

examples/TestMPC.jl

@@ -0,0 +1,5 @@
+module TestMPC
+
+greet() = print("Hello World!")
+
+end # module TestMPC

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])

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)
+

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)
+=#

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])

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)

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])