Merge pull request #315 from JuliaControl/custom_linconstraint_Cgen

added: C codegen support for `LinMPC` custom linear inequality constraints

Author: franckgaga

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]

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,17 +36,18 @@ 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
         issoft(C) = any(x -> x > 0, C)
         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

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