From e9c527e90131096b76f1bdab7bf8791276d45e33 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 15:49:39 -0500 Subject: [PATCH 01/10] wip: support for custom linear constraint C codegen --- ext/LinearMPCext.jl | 52 ++++++++++++++++++++++++++++++--------------- 1 file changed, 35 insertions(+), 17 deletions(-) diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl index 595df0be9..cbeb79420 100644 --- a/ext/LinearMPCext.jl +++ b/ext/LinearMPCext.jl @@ -9,7 +9,7 @@ import ModelPredictiveControl: isblockdiag function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) model, estim, weights = mpc.estim.model, mpc.estim, mpc.weights - nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ + nu, ny, nx̂, nw = model.nu, model.ny, estim.nx̂, mpc.con.nw Hp, Hc = mpc.Hp, mpc.Hc nΔU = Hc * nu validate_compatibility(mpc) @@ -36,7 +36,7 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) R = weights.L_Hp[1:nu, 1:nu] LinearMPC.set_objective!(newmpc; Q, Rr, R, Qf) # --- Custom move blocking --- - LinearMPC.move_block!(newmpc, mpc.nb) # un-comment when debugged + LinearMPC.move_block!(newmpc, mpc.nb) # ---- Constraint softening --- only_hard = weights.isinf_C if !only_hard @@ -44,9 +44,10 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) C_u = -mpc.con.A_Umin[:, end] C_Δu = -mpc.con.A_ΔŨmin[1:nΔU, end] C_y = -mpc.con.A_Ymin[:, end] + C_w = -mpc.con.A_Wmin[:, end] c_x̂ = -mpc.con.A_x̂min[:, end] if sum(mpc.con.i_b) > 1 # ignore the slack variable ϵ bound - if issoft(C_u) || issoft(C_Δu) || issoft(C_y) || issoft(C_x̂) + if issoft(C_u) || issoft(C_Δu) || issoft(C_y) || issoft(C_w) || issoft(C_x̂) @warn "The LinearMPC conversion is approximate for the soft constraints.\n"* "You may need to adjust the soft_weight field of the "* "LinearMPC.MPC object to replicate behaviors." @@ -61,6 +62,7 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) C_u = zeros(nu*Hp) C_Δu = zeros(nu*Hc) C_y = zeros(ny*Hp) + C_w = zeros(nw*(Hp+1)) c_x̂ = zeros(nx̂) end # --- Manipulated inputs constraints --- @@ -72,7 +74,7 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) Umax_finals = reshape(Umax[nu*(Hc-1)+1:end], nu, :) umin_end = mapslices(maximum, Umin_finals; dims=2) umax_end = mapslices(minimum, Umax_finals; dims=2) - for k in 0:Hc-1 + for k = 0:Hc-1 if k < Hc - 1 umin_k, umax_k = Umin[k*nu+1:(k+1)*nu], Umax[k*nu+1:(k+1)*nu] else @@ -91,7 +93,7 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) # --- Input increment constraints --- ΔUmin, ΔUmax = mpc.con.ΔŨmin[1:nΔU], mpc.con.ΔŨmax[1:nΔU] I_Δu = Matrix{Float64}(I, nu, nu) - for k in 0:Hc-1 + for k = 0:Hc-1 Δumin_k, Δumax_k = ΔUmin[k*nu+1:(k+1)*nu], ΔUmax[k*nu+1:(k+1)*nu] c_Δu_k = C_Δu[k*nu+1:(k+1)*nu] ks = [k + 1] # a `1` in ks argument corresponds to the present time step k+0 @@ -105,7 +107,7 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) end # --- Output constraints --- Y0min, Y0max = mpc.con.Y0min, mpc.con.Y0max - for k in 1:Hp + for k = 1:Hp ymin_k, ymax_k = Y0min[(k-1)*ny+1:k*ny], Y0max[(k-1)*ny+1:k*ny] c_y_k = C_y[(k-1)*ny+1:k*ny] ks = [k + 1] # a `1` in ks argument corresponds to the present time step k+0 @@ -116,6 +118,19 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) Ax, Ad = C[i:i, :], Dd[i:i, :] LinearMPC.add_constraint!(newmpc; Ax, Ad, lb, ub, ks, soft) end + end + # --- Custom linear constraints --- + Wmin, Wmax = mpc.con.Wmin, mpc.con.Wmax + for k in 1:Hp+1 + wmin_k, wmax_k = Wmin[(k-1)*nw+1:k*nw], Wmax[(k-1)*nw+1:k*nw] + c_w_k = C_w[(k-1)*nw+1:k*nw] + ks = [k + 1] + for i in 1:nw + lb = isfinite(wmin_k[i]) ? [wmin_k[i]] : zeros(0) + ub = isfinite(wmax_k[i]) ? [wmax_k[i]] : zeros(0) + soft = !only_hard && c_w_k[i] > 0 + + end # --- Terminal constraints --- x̂0min, x̂0max = mpc.con.x̂0min, mpc.con.x̂0max @@ -180,29 +195,32 @@ end function validate_constraints(mpc::ModelPredictiveControl.LinMPC) nΔU = mpc.Hc * mpc.estim.model.nu - mpc.con.nw > 0 && error("Conversion of custom linear inequality constraints is not supported for now.") mpc.weights.isinf_C && return nothing # only hard constraints are entirely supported C_umin, C_umax = -mpc.con.A_Umin[:, end], -mpc.con.A_Umax[:, end] C_Δumin, C_Δumax = -mpc.con.A_ΔŨmin[1:nΔU, end], -mpc.con.A_ΔŨmax[1:nΔU, end] C_ymin, C_ymax = -mpc.con.A_Ymin[:, end], -mpc.con.A_Ymax[:, end] + C_wmin, C_wmax = -mpc.con.A_Wmin[:, end], -mpc.con.A_Wmax[:, end] C_x̂min, C_x̂max = -mpc.con.A_x̂min[:, end], -mpc.con.A_x̂max[:, end] + if ( + !isapprox(C_umin, C_umax) || + !isapprox(C_Δumin, C_Δumax) || + !isapprox(C_ymin, C_ymax) || + !isapprox(C_wmin, C_wmax) || + !isapprox(C_x̂min, C_x̂max) + ) + error("LinearMPC only supports identical softness parameters for lower and upper bounds.") + end is0or1(C) = all(x -> x ≈ 0 || x ≈ 1, C) if ( !is0or1(C_umin) || !is0or1(C_umax) || !is0or1(C_Δumin) || !is0or1(C_Δumax) || - !is0or1(C_ymin) || !is0or1(C_ymax) || + !is0or1(C_ymin) || !is0or1(C_ymax) || + !is0or1(C_wmin) || !is0or1(C_wmax) || !is0or1(C_x̂min) || !is0or1(C_x̂max) ) - error("LinearMPC only supports softness parameters c = 0 or 1.") - end - if ( - !isapprox(C_umin, C_umax) || - !isapprox(C_Δumin, C_Δumax) || - !isapprox(C_ymin, C_ymax) || - !isapprox(C_x̂min, C_x̂max) - ) - error("LinearMPC only supports identical softness parameters for lower and upper bounds.") + @warn "LinearMPC only supports softness parameters c = 0 or 1.\n"* + "All constraints with c > 0 will be considered soft." end return nothing end From f5baa5f4fdbb86c8df298a7fa49dcbaa250ceff1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 17:18:35 -0500 Subject: [PATCH 02/10] wip: idem --- ext/LinearMPCext.jl | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl index cbeb79420..1d941b76d 100644 --- a/ext/LinearMPCext.jl +++ b/ext/LinearMPCext.jl @@ -9,7 +9,7 @@ import ModelPredictiveControl: isblockdiag function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) model, estim, weights = mpc.estim.model, mpc.estim, mpc.weights - nu, ny, nx̂, nw = model.nu, model.ny, estim.nx̂, mpc.con.nw + nu, ny, nd, nx̂, nw = model.nu, model.ny, model.nd, estim.nx̂, mpc.con.nw Hp, Hc = mpc.Hp, mpc.Hc nΔU = Hc * nu validate_compatibility(mpc) @@ -121,16 +121,31 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) end # --- Custom linear constraints --- Wmin, Wmax = mpc.con.Wmin, mpc.con.Wmax + Wy = mpc.con.W̄y[1:nw, 1:ny] + Wu = mpc.con.W̄u[1:nw, 1:nu] + Wd = mpc.con.W̄d[1:nw, 1:nd] + Wr = mpc.con.W̄r[1:nw, 1:ny] for k in 1:Hp+1 wmin_k, wmax_k = Wmin[(k-1)*nw+1:k*nw], Wmax[(k-1)*nw+1:k*nw] c_w_k = C_w[(k-1)*nw+1:k*nw] ks = [k + 1] for i in 1:nw - lb = isfinite(wmin_k[i]) ? [wmin_k[i]] : zeros(0) - ub = isfinite(wmax_k[i]) ? [wmax_k[i]] : zeros(0) + Wy_i, Wu_i, Wd_i, Wr_i = Wy[i:i, :], Wu[i:i, :], Wd[i:i, :], Wr[i:i, :] + + lb_k_i = wmin_k[i:i] - Wy_i*yoff + ub_k_i = wmax_k[i:i] - Wy_i*yoff + @show lb_k_i + @show ub_k_i + lb = isfinite(lb_k_i[]) ? lb_k_i : zeros(0) + ub = isfinite(ub_k_i[]) ? ub_k_i : zeros(0) soft = !only_hard && c_w_k[i] > 0 + Ax = Wy_i*C + Ad = Wy_i*Dd + @show Ax + @show Ad + LinearMPC.add_constraint!(newmpc; Ax, Ad, lb, ub, ks, soft) - + end end # --- Terminal constraints --- x̂0min, x̂0max = mpc.con.x̂0min, mpc.con.x̂0max From d3278ed8db9fb7fc9045590c82bbc8e53825d898 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 17:31:38 -0500 Subject: [PATCH 03/10] added: custom constraint conversion now works --- ext/LinearMPCext.jl | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl index 1d941b76d..55dc23aaf 100644 --- a/ext/LinearMPCext.jl +++ b/ext/LinearMPCext.jl @@ -134,17 +134,14 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) lb_k_i = wmin_k[i:i] - Wy_i*yoff ub_k_i = wmax_k[i:i] - Wy_i*yoff - @show lb_k_i - @show ub_k_i lb = isfinite(lb_k_i[]) ? lb_k_i : zeros(0) ub = isfinite(ub_k_i[]) ? ub_k_i : zeros(0) soft = !only_hard && c_w_k[i] > 0 Ax = Wy_i*C - Ad = Wy_i*Dd - @show Ax - @show Ad - LinearMPC.add_constraint!(newmpc; Ax, Ad, lb, ub, ks, soft) - + Au = Wu_i + Ad = Wy_i*Dd + Wd_i + Ar = Wr_i + LinearMPC.add_constraint!(newmpc; Ax, Au, Ad, Ar, lb, ub, ks, soft) end end # --- Terminal constraints --- From 84ae4a078848061bc9600d80c542b92eb0cf089c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 17:32:26 -0500 Subject: [PATCH 04/10] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 55bb3680f..7e60c0bc2 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "1.15.0" +version = "1.16.0" authors = ["Francis Gagnon"] [deps] From 0af9dd19b05a4dc06b006808788ca5a74a6efbbe Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 5 Feb 2026 17:38:48 -0500 Subject: [PATCH 05/10] removed: blank line --- ext/LinearMPCext.jl | 1 - 1 file changed, 1 deletion(-) diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl index 55dc23aaf..0f99168a1 100644 --- a/ext/LinearMPCext.jl +++ b/ext/LinearMPCext.jl @@ -131,7 +131,6 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) ks = [k + 1] for i in 1:nw Wy_i, Wu_i, Wd_i, Wr_i = Wy[i:i, :], Wu[i:i, :], Wd[i:i, :], Wr[i:i, :] - lb_k_i = wmin_k[i:i] - Wy_i*yoff ub_k_i = wmax_k[i:i] - Wy_i*yoff lb = isfinite(lb_k_i[]) ? lb_k_i : zeros(0) From 7a0a8a0fe7b616c51fa6fb6a2fa0a4bb791f224a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 6 Feb 2026 13:39:25 -0500 Subject: [PATCH 06/10] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 7e60c0bc2..11d88959d 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "1.16.0" +version = "1.16.1" authors = ["Francis Gagnon"] [deps] From 6e1409ef7eb2a4e2c88af4fa5d2fb0d23029cc7b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 6 Feb 2026 14:31:44 -0500 Subject: [PATCH 07/10] removed: `k>H_c` logic for C codegen This is no longer necessary for recent LinearMPC.jl versions --- ext/LinearMPCext.jl | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl index 0f99168a1..b3531dc9b 100644 --- a/ext/LinearMPCext.jl +++ b/ext/LinearMPCext.jl @@ -68,18 +68,8 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) # --- Manipulated inputs constraints --- Umin, Umax = mpc.con.U0min + mpc.Uop, mpc.con.U0max + mpc.Uop I_u = Matrix{Float64}(I, nu, nu) - # add_constraint! does not support u bounds pass the control horizon Hc - # so we compute the extremum bounds from k=Hc-1 to Hp, and apply them at k=Hc-1 - Umin_finals = reshape(Umin[nu*(Hc-1)+1:end], nu, :) - Umax_finals = reshape(Umax[nu*(Hc-1)+1:end], nu, :) - umin_end = mapslices(maximum, Umin_finals; dims=2) - umax_end = mapslices(minimum, Umax_finals; dims=2) - for k = 0:Hc-1 - if k < Hc - 1 - umin_k, umax_k = Umin[k*nu+1:(k+1)*nu], Umax[k*nu+1:(k+1)*nu] - else - umin_k, umax_k = umin_end, umax_end - end + for k = 0:Hp-1 + umin_k, umax_k = Umin[k*nu+1:(k+1)*nu], Umax[k*nu+1:(k+1)*nu] c_u_k = C_u[k*nu+1:(k+1)*nu] ks = [k + 1] # a `1` in ks argument corresponds to the present time step k+0 for i in 1:nu From 2c37f6f717a4557da1727dc6473dc3e2299f610d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 6 Feb 2026 14:37:40 -0500 Subject: [PATCH 08/10] debug: C codegen correct timestep for custom constraints --- ext/LinearMPCext.jl | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl index b3531dc9b..ee84ac17d 100644 --- a/ext/LinearMPCext.jl +++ b/ext/LinearMPCext.jl @@ -115,9 +115,9 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) Wu = mpc.con.W̄u[1:nw, 1:nu] Wd = mpc.con.W̄d[1:nw, 1:nd] Wr = mpc.con.W̄r[1:nw, 1:ny] - for k in 1:Hp+1 - wmin_k, wmax_k = Wmin[(k-1)*nw+1:k*nw], Wmax[(k-1)*nw+1:k*nw] - c_w_k = C_w[(k-1)*nw+1:k*nw] + for k in 0:Hp + wmin_k, wmax_k = Wmin[k*nw+1:(k+1)*nw], Wmax[k*nw+1:(k+1)*nw] + c_w_k = C_w[k*nw+1:(k+1)*nw] ks = [k + 1] for i in 1:nw Wy_i, Wu_i, Wd_i, Wr_i = Wy[i:i, :], Wu[i:i, :], Wd[i:i, :], Wr[i:i, :] @@ -130,6 +130,7 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) Au = Wu_i Ad = Wy_i*Dd + Wd_i Ar = Wr_i + Au |> display LinearMPC.add_constraint!(newmpc; Ax, Au, Ad, Ar, lb, ub, ks, soft) end end From b3f236f552a7501748fa946027104b3d604b9ff7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 6 Feb 2026 14:38:17 -0500 Subject: [PATCH 09/10] removed: useless print --- ext/LinearMPCext.jl | 1 - 1 file changed, 1 deletion(-) diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl index ee84ac17d..756109570 100644 --- a/ext/LinearMPCext.jl +++ b/ext/LinearMPCext.jl @@ -130,7 +130,6 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) Au = Wu_i Ad = Wy_i*Dd + Wd_i Ar = Wr_i - Au |> display LinearMPC.add_constraint!(newmpc; Ax, Au, Ad, Ar, lb, ub, ks, soft) end end From 4c6f86eaa4033306de8bc1bde39b4cbfcc98bb49 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 6 Feb 2026 16:20:38 -0500 Subject: [PATCH 10/10] test: `LinearMPC.MPC` comparisons with custom linear constraints --- test/5_test_extensions.jl | 128 +++++++++++++++++++++++++++++++++++++- 1 file changed, 127 insertions(+), 1 deletion(-) diff --git a/test/5_test_extensions.jl b/test/5_test_extensions.jl index 68e74c6ce..cf0cabfc7 100644 --- a/test/5_test_extensions.jl +++ b/test/5_test_extensions.jl @@ -1,4 +1,4 @@ -@testitem "LinearMPCext extension" setup=[SetupMPCtests] begin +@testitem "LinearMPCext general" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, DAQP import LinearMPC model = LinModel(sys, Ts, i_u=1:2) @@ -50,4 +50,130 @@ "OSQP.\nThe results in closed-loop may be different."), LinearMPC.MPC(mpc_osqp) ) + +end + +@testitem "LinearMPCext with Wy weight" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, DAQP + import LinearMPC + model = LinModel(tf([2], [10, 1]), 3.0) + model = setop!(model, yop=[50], uop=[20]) + optim = JuMP.Model(DAQP.Optimizer) + mpc1 = LinMPC(model, Hp=20, Hc=5, Wy=[1], optim=optim) + mpc1 = setconstraint!(mpc1, wmax=[55]) + mpc2 = LinearMPC.MPC(mpc1) + function sim_wy(model, mpc1, mpc2, N) + r = [60.0] + u1 = [20.0] + u2 = [20.0] + model.x0 .= 0 + u_data1, u_data2 = zeros(1, N), zeros(1, N) + for k in 0:N-1 + y = model() + x̂ = preparestate!(mpc1, y) + u1 = moveinput!(mpc1, r, lastu=u1) + u2 = LinearMPC.compute_control(mpc2, x̂, r=r, uprev=u2) + u_data1[:, k+1], u_data2[:, k+1] = u1, u2 + updatestate!(model, u1) + updatestate!(mpc1, u1, y) + end + return u_data1, u_data2 + end + N = 30 + u_data1, u_data2 = sim_wy(model, mpc1, mpc2, N) + @test u_data1 ≈ u_data2 atol=1e-2 rtol=1e-2 +end + +@testitem "LinearMPCext with Wu weight" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, DAQP + import LinearMPC + model = LinModel(tf([2], [10, 1]), 3.0) + model = setop!(model, uop=[20], yop=[50]) + optim = JuMP.Model(DAQP.Optimizer) + mpc1 = LinMPC(model, Nwt=[0], Hp=250, Hc=1, Wu=[1], optim=optim) + mpc1 = setconstraint!(mpc1, wmin=[19.0]) + mpc2 = LinearMPC.MPC(mpc1) + function sim_wu(model, mpc1, mpc2, N) + r = [40.0] + u1 = [20.0] + u2 = [20.0] + model.x0 .= 0 + u_data1, u_data2 = zeros(1, N), zeros(1, N) + for k in 0:N-1 + y = model() + x̂ = preparestate!(mpc1, y) + u1 = moveinput!(mpc1, r, lastu=u1) + u2 = LinearMPC.compute_control(mpc2, x̂, r=r, uprev=u2) + u_data1[:, k+1], u_data2[:, k+1] = u1, u2 + updatestate!(model, u1) + updatestate!(mpc1, u1, y) + end + return u_data1, u_data2 + end + N = 30 + u_data1, u_data2 = sim_wu(model, mpc1, mpc2, N) + @test u_data1 ≈ u_data2 atol=1e-2 rtol=1e-2 +end + +@testitem "LinearMPCext with Wd weight" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, DAQP + import LinearMPC + model = LinModel([tf([2], [10, 1]) tf(0.1, [7, 1])], 3.0, i_d=[2]) + model = setop!(model, uop=[25], dop=[30], yop=[50]) + optim = JuMP.Model(DAQP.Optimizer) + mpc1 = LinMPC(model, Nwt=[0], Hp=250, Hc=1, Wd=[1], Wu=[1], optim=optim) + mpc1 = setconstraint!(mpc1, wmax=[60]) + mpc2 = LinearMPC.MPC(mpc1) + function sim_wd(model, mpc1, mpc2, N) + r = [80.0] + d = [30.0] + u1 = [25.0] + u2 = [25.0] + model.x0 .= 0 + u_data1, u_data2 = zeros(1, N), zeros(1, N) + for k in 0:N-1 + y = model(d) + x̂ = preparestate!(mpc1, y, d) + u1 = moveinput!(mpc1, r, d, lastu=u1) + u2 = LinearMPC.compute_control(mpc2, x̂, r=r, d=d, uprev=u2) + u_data1[:, k+1], u_data2[:, k+1] = u1, u2 + updatestate!(model, u1, d) + updatestate!(mpc1, u1, y, d) + end + return u_data1, u_data2 + end + N = 30 + u_data1, u_data2 = sim_wd(model, mpc1, mpc2, N) + @test u_data1 ≈ u_data2 atol=1e-2 rtol=1e-2 +end + +@testitem "LinearMPCext with Wr weight" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, DAQP + import LinearMPC + model = LinModel(tf([2], [10, 1]), 3.0) + model = setop!(model, yop=[50], uop=[20]) + optim = JuMP.Model(DAQP.Optimizer) + mpc1 = LinMPC(model, Hp=20, Hc=5, Wy=[1], Wr=[1], optim=optim) + mpc1 = setconstraint!(mpc1, wmin=[85]) + mpc2 = LinearMPC.MPC(mpc1) + function sim_wr(model, mpc1, mpc2, N) + r = [40.0] + u1 = [20.0] + u2 = [20.0] + model.x0 .= 0 + u_data1, u_data2 = zeros(1, N), zeros(1, N) + for k in 0:N-1 + y = model() + x̂ = preparestate!(mpc1, y) + u1 = moveinput!(mpc1, r, lastu=u1) + u2 = LinearMPC.compute_control(mpc2, x̂, r=r, uprev=u2) + u_data1[:, k+1], u_data2[:, k+1] = u1, u2 + updatestate!(model, u1) + updatestate!(mpc1, u1, y) + end + return u_data1, u_data2 + end + N = 30 + u_data1, u_data2 = sim_wr(model, mpc1, mpc2, N) + @test u_data1 ≈ u_data2 atol=1e-2 rtol=1e-2 end