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] diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl index 595df0be9..756109570 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, 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) @@ -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,23 +62,14 @@ 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 --- 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 in 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 @@ -91,7 +83,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 +97,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 @@ -117,6 +109,30 @@ function Base.convert(::Type{LinearMPC.MPC}, mpc::ModelPredictiveControl.LinMPC) LinearMPC.add_constraint!(newmpc; Ax, Ad, lb, ub, ks, soft) end 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 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, :] + 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) + ub = isfinite(ub_k_i[]) ? ub_k_i : zeros(0) + soft = !only_hard && c_w_k[i] > 0 + Ax = Wy_i*C + 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 --- x̂0min, x̂0max = mpc.con.x̂0min, mpc.con.x̂0max I_x̂ = Matrix{Float64}(I, nx̂, nx̂) @@ -180,29 +196,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 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