diff --git a/benchmark/0_bench_setup.jl b/benchmark/0_bench_setup.jl index ac563ffb3..e6629f902 100644 --- a/benchmark/0_bench_setup.jl +++ b/benchmark/0_bench_setup.jl @@ -15,4 +15,28 @@ end linmodel = setop!(LinModel(sys, Ts, i_d=[3]), uop=[10, 50], yop=[50, 30], dop=[5]) nonlinmodel = NonLinModel(f_lin!, h_lin!, Ts, 2, 4, 2, 1, p=linmodel, solver=nothing) nonlinmodel = setop!(nonlinmodel, uop=[10, 50], yop=[50, 30], dop=[5]) -u, d, y = [10, 50], [5], [50, 30] \ No newline at end of file +u, d, y = [10, 50], [5], [50, 30] + +G = [ tf(1.90, [18, 1]) tf(1.90, [18, 1]); + tf(-0.74,[8, 1]) tf(0.74, [8, 1]) ] +uop, yop, dop = [20, 20], [50, 30], [20] +CSTR_model = setop!(LinModel(G, 2.0); uop, yop) +CSTR_model_d = setop!(LinModel([G G[1:2, 2]], 2.0; i_d=[3]); uop, yop, dop) + +function f!(ẋ, x, u, _ , p) + g, L, K, m = p + θ, ω = x[1], x[2] + τ = u[1] + ẋ[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 +pendulum_model = NonLinModel(f!, h!, Ts, nu, nx, ny; p) +pendulum_p = p + +h2!(y, x, _ , _ ) = (y[1] = 180/π*x[1]; y[2]=x[2]) +nu, nx, ny = 1, 2, 2 +pendulum_model2 = NonLinModel(f!, h2!, Ts, nu, nx, ny; p) +pendulum_p2 = p \ No newline at end of file diff --git a/benchmark/1_bench_sim_model.jl b/benchmark/1_bench_sim_model.jl index 13f5edd1e..e58fb6d39 100644 --- a/benchmark/1_bench_sim_model.jl +++ b/benchmark/1_bench_sim_model.jl @@ -1,5 +1,7 @@ -## ----------------- Unit tests ---------------------------------------------------------- -const UNIT_MODEL = SUITE["unit tests"]["SimModel"] +## ---------------------------------------------------------------------------------------- +## ----------------- UNIT TESTS ---------------------------------------------------------- +## ---------------------------------------------------------------------------------------- +const UNIT_MODEL = SUITE["UNIT TESTS"]["SimModel"] UNIT_MODEL["LinModel"]["updatestate!"] = @benchmarkable( @@ -22,6 +24,8 @@ UNIT_MODEL["NonLinModel"]["linearize!"] = linearize!($linmodel, $nonlinmodel); ) -## ----------------- Case studies --------------------------------------------------------- -const CASE_MODEL = SUITE["case studies"]["SimModel"] +## ---------------------------------------------------------------------------------------- +## ----------------- CASE STUDIES --------------------------------------------------------- +## ---------------------------------------------------------------------------------------- +const CASE_MODEL = SUITE["CASE STUDIES"]["SimModel"] # TODO: Add case study benchmarks for SimModel \ No newline at end of file diff --git a/benchmark/2_bench_state_estim.jl b/benchmark/2_bench_state_estim.jl index f86ce9d99..2967d4550 100644 --- a/benchmark/2_bench_state_estim.jl +++ b/benchmark/2_bench_state_estim.jl @@ -1,5 +1,7 @@ -## ----------------- Unit tests ----------------------------------------------------------- -const UNIT_ESTIM = SUITE["unit tests"]["StateEstimator"] +## ---------------------------------------------------------------------------------------- +## ----------------- UNIT TESTS ----------------------------------------------------------- +## ---------------------------------------------------------------------------------------- +const UNIT_ESTIM = SUITE["UNIT TESTS"]["StateEstimator"] skf = SteadyKalmanFilter(linmodel) UNIT_ESTIM["SteadyKalmanFilter"]["preparestate!"] = @@ -193,18 +195,17 @@ UNIT_ESTIM["MovingHorizonEstimator"]["updatestate!"]["NonLinModel"]["Prediction samples=samples, evals=evals, seconds=seconds, ) -## ----------------- Case studies --------------------------------------------------- -const CASE_ESTIM = SUITE["case studies"]["StateEstimator"] +## ---------------------------------------------------------------------------------------- +## ----------------- CASE STUDIES --------------------------------------------------------- +## ---------------------------------------------------------------------------------------- +const CASE_ESTIM = SUITE["CASE STUDIES"]["StateEstimator"] ## ----------------- Case study: CSTR ----------------------------------------------------- -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] -model = setop!(LinModel(G, 2.0); uop, yop) -plant = setop!(LinModel(G, 2.0); uop, yop) +model = CSTR_model +plant = deepcopy(model) plant.A[diagind(plant.A)] .-= 0.1 # plant-model mismatch function test_mhe(mhe, plant) - plant.x0 .= 0; y = plant() + plant.x0 .= 0.1; y = plant() initstate!(mhe, plant.uop, y) N = 75; u = [20, 20]; ul = 0 U, Y, Ŷ = zeros(2, N), zeros(2, N), zeros(2, N) @@ -220,45 +221,46 @@ function test_mhe(mhe, plant) end return U, Y, Ŷ end -He = 10; nint_u = [1, 1]; σQint_u = [1, 2] +He = 4; nint_u = [1, 1]; σQint_u = [1, 2] +v̂min, v̂max = [-1, -0.5], [+1, +0.5] optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) direct = true mhe_cstr_osqp_curr = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) -mhe_cstr_osqp_curr = setconstraint!(mhe_cstr_osqp_curr, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +mhe_cstr_osqp_curr = setconstraint!(mhe_cstr_osqp_curr; v̂min, v̂max) JuMP.unset_time_limit_sec(mhe_cstr_osqp_curr.optim) optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) direct = false mhe_cstr_osqp_pred = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) -mhe_cstr_osqp_pred = setconstraint!(mhe_cstr_osqp_pred, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +mhe_cstr_osqp_pred = setconstraint!(mhe_cstr_osqp_pred; v̂min, v̂max) JuMP.unset_time_limit_sec(mhe_cstr_osqp_pred.optim) optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) direct = true mhe_cstr_daqp_curr = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) -mhe_cstr_daqp_curr = setconstraint!(mhe_cstr_daqp_curr, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +mhe_cstr_daqp_curr = setconstraint!(mhe_cstr_daqp_curr; v̂min, v̂max) JuMP.set_attribute(mhe_cstr_daqp_curr.optim, "eps_prox", 1e-6) # needed to support hessians H≥0 optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) direct = false mhe_cstr_daqp_pred = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) -mhe_cstr_daqp_pred = setconstraint!(mhe_cstr_daqp_pred, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +mhe_cstr_daqp_pred = setconstraint!(mhe_cstr_daqp_pred; v̂min, v̂max) JuMP.set_attribute(mhe_cstr_daqp_pred.optim, "eps_prox", 1e-6) # needed to support hessians H≥0 optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) direct = true mhe_cstr_ipopt_curr = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) -mhe_cstr_ipopt_curr = setconstraint!(mhe_cstr_ipopt_curr, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +mhe_cstr_ipopt_curr = setconstraint!(mhe_cstr_ipopt_curr; v̂min, v̂max) JuMP.unset_time_limit_sec(mhe_cstr_ipopt_curr.optim) optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) direct = false mhe_cstr_ipopt_pred = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) -mhe_cstr_ipopt_pred = setconstraint!(mhe_cstr_ipopt_pred, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +mhe_cstr_ipopt_pred = setconstraint!(mhe_cstr_ipopt_pred; v̂min, v̂max) JuMP.unset_time_limit_sec(mhe_cstr_ipopt_pred.optim) -samples, evals = 500, 1 +samples, evals = 5000, 1 CASE_ESTIM["CSTR"]["MovingHorizonEstimator"]["OSQP"]["Current form"] = @benchmarkable(test_mhe($mhe_cstr_osqp_curr, $plant); samples=samples, evals=evals @@ -282,4 +284,68 @@ CASE_ESTIM["CSTR"]["MovingHorizonEstimator"]["Ipopt"]["Current form"] = CASE_ESTIM["CSTR"]["MovingHorizonEstimator"]["Ipopt"]["Prediction form"] = @benchmarkable(test_mhe($mhe_cstr_ipopt_pred, $plant); samples=samples, evals=evals + ) + +## ---------------------- Case study: pendulum ------------------------------------------- +model, p = pendulum_model, pendulum_p +plant = deepcopy(model) +plant.p[3] = 1.25*p[3] # plant-model mismatch +σQ = [0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1] +He = 3; v̂min, v̂max = [-5.0], [+5.0] +N = 35; + +x_0 = [0.1, 0.1]; x̂_0 = [0, 0, 0]; u = [0.5] + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +direct = true +mhe_pendulum_ipopt_curr = MovingHorizonEstimator( + model; He, σQ, σR, nint_u, σQint_u, optim, direct +) +mhe_pendulum_ipopt_curr = setconstraint!(mhe_pendulum_ipopt_curr; v̂min, v̂max) +JuMP.unset_time_limit_sec(mhe_pendulum_ipopt_curr.optim) + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +direct = false +mhe_pendulum_ipopt_pred = MovingHorizonEstimator( + model; He, σQ, σR, nint_u, σQint_u, optim, direct +) +mhe_pendulum_ipopt_pred = setconstraint!(mhe_pendulum_ipopt_pred; v̂min, v̂max) +JuMP.unset_time_limit_sec(mhe_pendulum_ipopt_pred.optim) + +optim = JuMP.Model(MadNLP.Optimizer, add_bridges=false) +direct = true +mhe_pendulum_madnlp_curr = MovingHorizonEstimator( + model; He, σQ, σR, nint_u, σQint_u, optim, direct +) +mhe_pendulum_madnlp_curr = setconstraint!(mhe_pendulum_madnlp_curr; v̂min, v̂max) +JuMP.unset_time_limit_sec(mhe_pendulum_madnlp_curr.optim) + +optim = JuMP.Model(MadNLP.Optimizer, add_bridges=false) +direct = false +mhe_pendulum_madnlp_pred = MovingHorizonEstimator( + model; He, σQ, σR, nint_u, σQint_u, optim, direct +) +mhe_pendulum_madnlp_pred = setconstraint!(mhe_pendulum_madnlp_pred; v̂min, v̂max) +JuMP.unset_time_limit_sec(mhe_pendulum_madnlp_pred.optim) + +samples, evals, seconds = 10, 1, 15*60 +CASE_ESTIM["Pendulum"]["MovingHorizonEstimator"]["Ipopt"]["Current form"] = + @benchmarkable( + sim!($mhe_pendulum_ipopt_curr, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds + ) +CASE_ESTIM["Pendulum"]["MovingHorizonEstimator"]["Ipopt"]["Prediction form"] = + @benchmarkable( + sim!($mhe_pendulum_ipopt_pred, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds + ) +CASE_ESTIM["Pendulum"]["MovingHorizonEstimator"]["MadNLP"]["Current form"] = + @benchmarkable( + sim!($mhe_pendulum_madnlp_curr, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds + ) +CASE_ESTIM["Pendulum"]["MovingHorizonEstimator"]["MadNLP"]["Prediction form"] = + @benchmarkable( + sim!($mhe_pendulum_madnlp_pred, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds ) \ No newline at end of file diff --git a/benchmark/3_bench_predictive_control.jl b/benchmark/3_bench_predictive_control.jl index 3ffd1a86e..cbad64b85 100644 --- a/benchmark/3_bench_predictive_control.jl +++ b/benchmark/3_bench_predictive_control.jl @@ -1,5 +1,7 @@ -# ---------------------- Unit tests ------------------------------------------------------- -const UNIT_MPC = SUITE["unit tests"]["PredictiveController"] +# ----------------------------------------------------------------------------------------- +# ---------------------- UNIT TESTS ------------------------------------------------------- +# ----------------------------------------------------------------------------------------- +const UNIT_MPC = SUITE["UNIT TESTS"]["PredictiveController"] linmpc_ss = LinMPC( linmodel, transcription=SingleShooting(), @@ -10,7 +12,7 @@ linmpc_ms = LinMPC( Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 ) -samples, evals, seconds = 500, 1, 60 +samples, evals, seconds = 5000, 1, 60 UNIT_MPC["LinMPC"]["moveinput!"]["SingleShooting"] = @benchmarkable( moveinput!($linmpc_ss, $y, $d), @@ -49,7 +51,7 @@ nmpc_nonlin_ms = NonLinMPC( Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 ) -samples, evals, seconds = 500, 1, 60 +samples, evals, seconds = 5000, 1, 60 UNIT_MPC["NonLinMPC"]["moveinput!"]["LinModel"]["SingleShooting"] = @benchmarkable( moveinput!($nmpc_lin_ss, $y, $d), @@ -75,16 +77,14 @@ UNIT_MPC["NonLinMPC"]["moveinput!"]["NonLinModel"]["MultipleShooting"] = samples=samples, evals=evals, seconds=seconds ) +## ---------------------------------------------------------------------------------------- +## ---------------------- CASE STUDIES ---------------------------------------------------- +## ---------------------------------------------------------------------------------------- +const CASE_MPC = SUITE["CASE STUDIES"]["PredictiveController"] -## ---------------------- Case studies ---------------------------------------------------- -const CASE_MPC = SUITE["case studies"]["PredictiveController"] - -## ----------------- Case study: CSTR without feedforward ------------------------ -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] -model = setop!(LinModel(G, 2.0); uop, yop) -plant = setop!(LinModel(G, 2.0); uop, yop) +## ----------------- Case study: CSTR without feedforward --------------------------------- +model = CSTR_model +plant = deepcopy(model) plant.A[diagind(plant.A)] .-= 0.1 # plant-model mismatch function test_mpc(mpc, plant) plant.x0 .= 0; y = plant() @@ -135,7 +135,7 @@ transcription = MultipleShooting() mpc_ipopt_ms = setconstraint!(LinMPC(model; optim, transcription), ymin=[45, -Inf]) JuMP.unset_time_limit_sec(mpc_ipopt_ms.optim) -samples, evals = 500, 1 +samples, evals = 5000, 1 CASE_MPC["CSTR"]["LinMPC"]["Without feedforward"]["OSQP"]["SingleShooting"] = @benchmarkable(test_mpc($mpc_osqp_ss, $plant); samples=samples, evals=evals @@ -158,8 +158,7 @@ CASE_MPC["CSTR"]["LinMPC"]["Without feedforward"]["Ipopt"]["MultipleShooting"] = ) ## ----------------- Case study: CSTR with 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 = CSTR_model_d function test_mpc_d(mpc_d, plant) plant.x0 .= 0; y = plant(); d = [20] initstate!(mpc_d, plant.uop, y, d) @@ -209,7 +208,7 @@ transcription = MultipleShooting() mpc_d_ipopt_ms = setconstraint!(LinMPC(model_d; optim, transcription), ymin=[45, -Inf]) JuMP.unset_time_limit_sec(mpc_d_ipopt_ms.optim) -samples, evals = 500, 1 +samples, evals = 5000, 1 CASE_MPC["CSTR"]["LinMPC"]["With feedforward"]["OSQP"]["SingleShooting"] = @benchmarkable(test_mpc_d($mpc_d_osqp_ss, $plant); samples=samples, evals=evals @@ -233,21 +232,11 @@ CASE_MPC["CSTR"]["LinMPC"]["With feedforward"]["Ipopt"]["MultipleShooting"] = # ----------------- Case study: Pendulum noneconomic ----------------------------- -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) +model, p = pendulum_model, pendulum_p σ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) +plant = deepcopy(model) +plant.p[3] = 1.25*p[3] # plant-model mismatch N = 35; u = [0.5]; Hp, Hc, Mwt, Nwt, Cwt = 20, 2, [0.5], [2.5], Inf @@ -285,7 +274,7 @@ JuMP.unset_time_limit_sec(nmpc_madnlp_ss.optim) # MadNLP_QNopt = MadNLP.QuasiNewtonOptions(; max_history=42) # JuMP.set_attribute(nmpc_madnlp_ms.optim, "quasi_newton_options", MadNLP_QNopt) -samples, evals, seconds = 50, 1, 15*60 +samples, evals, seconds = 100, 1, 15*60 CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["SingleShooting"] = @benchmarkable( sim!($nmpc_ipopt_ss, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), @@ -303,10 +292,9 @@ CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["MadNLP"]["SingleShooting"] = ) # ----------------- Case study: Pendulum economic -------------------------------- -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, p = pendulum_model2, pendulum_p2 +plant2 = deepcopy(model2) +plant2.p[3] = 1.25*p[3] # plant-model mismatch estim2 = UnscentedKalmanFilter(model2; σQ, σR, nint_u, σQint_u, i_ym=[1]) function JE(UE, ŶE, _ , p) Ts = p @@ -336,7 +324,7 @@ JuMP.unset_time_limit_sec(empc_madnlp_ss.optim) # TODO: test EMPC with MadNLP and MultipleShooting, see comment above. -samples, evals, seconds = 50, 1, 15*60 +samples, evals, seconds = 100, 1, 15*60 CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["Ipopt"]["SingleShooting"] = @benchmarkable( sim!($empc_ipopt_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), @@ -388,7 +376,7 @@ JuMP.unset_time_limit_sec(nmpc2_ipopt_ms.optim) # TODO: test custom constraints with MadNLP and SingleShooting, see comment above. # TODO: test custom constraints with MadNLP and MultipleShooting, see comment above. -samples, evals, seconds = 50, 1, 15*60 +samples, evals, seconds = 100, 1, 15*60 CASE_MPC["Pendulum"]["NonLinMPC"]["Custom constraints"]["Ipopt"]["SingleShooting"] = @benchmarkable( sim!($nmpc2_ipopt_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), @@ -458,7 +446,7 @@ mpc3_ipopt_ms = LinMPC(kf; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) mpc3_ipopt_ms = setconstraint!(mpc3_ipopt_ms; umin, umax) JuMP.unset_time_limit_sec(mpc3_ipopt_ms.optim) -samples, evals = 500, 1 +samples, evals = 5000, 1 CASE_MPC["Pendulum"]["LinMPC"]["Successive linearization"]["OSQP"]["SingleShooting"] = @benchmarkable( sim2!($mpc3_osqp_ss, $model, $N, $ry, $plant, $x_0, $x̂_0, $y_step), diff --git a/benchmark/benchmarks.jl b/benchmark/benchmarks.jl index b42128711..4a83ede08 100644 --- a/benchmark/benchmarks.jl +++ b/benchmark/benchmarks.jl @@ -4,9 +4,8 @@ using JuMP, OSQP, DAQP, Ipopt, MadNLP const SUITE = BenchmarkGroup(["ModelPredictiveControl"]) -SUITE["unit tests"] = BenchmarkGroup(["allocation-free", "allocations", "single call"]) -SUITE["case studies"] = BenchmarkGroup(["performance", "speed" ,"integration"]) - +SUITE["UNIT TESTS"] = BenchmarkGroup(["allocation-free", "allocations", "single call"]) +SUITE["CASE STUDIES"] = BenchmarkGroup(["performance", "speed" ,"integration"]) include("0_bench_setup.jl") include("1_bench_sim_model.jl")