1+ module LinearMPCext
2+
3+ using ModelPredictiveControl
4+ using LinearAlgebra, SparseArrays
5+ using JuMP
6+
7+ import LinearMPC
8+ import ModelPredictiveControl: isblockdiag
9+
10+ function Base. convert (:: Type{LinearMPC.MPC} , mpc:: ModelPredictiveControl.LinMPC )
11+ model, estim, weights = mpc. estim. model, mpc. estim, mpc. weights
12+ nu, ny, nx̂ = model. nu, model. ny, estim. nx̂
13+ Hp, Hc = mpc. Hp, mpc. Hc
14+ nΔU = Hc * nu
15+ validate_compatibility (mpc)
16+ # --- Model parameters ---
17+ F, G, Gd = estim. Â, estim. B̂u, estim. B̂d
18+ C, Dd = estim. Ĉ, estim. D̂d
19+ Np = Hp
20+ Nc = Hc
21+ newmpc = LinearMPC. MPC (F, G; Gd, C, Dd, Np, Nc)
22+ # --- Operating points ---
23+ uoff = model. uop
24+ doff = model. dop
25+ yoff = model. yop
26+ xoff = estim. x̂op
27+ foff = estim. f̂op
28+ LinearMPC. set_offset! (newmpc; uo= uoff, ho= yoff, doff= doff, xo= xoff, fo= foff)
29+ # --- State observer parameters ---
30+ Q, R = estim. cov. Q̂, estim. cov. R̂
31+ LinearMPC. set_state_observer! (newmpc; C= estim. Ĉm, Q, R)
32+ # --- Objective function weights ---
33+ Q = weights. M_Hp[1 : ny, 1 : ny]
34+ Qf = weights. M_Hp[end - ny+ 1 : end , end - ny+ 1 : end ]
35+ Rr = weights. Ñ_Hc[1 : nu, 1 : nu]
36+ R = weights. L_Hp[1 : nu, 1 : nu]
37+ LinearMPC. set_objective! (newmpc; Q, Rr, R, Qf)
38+ # --- Custom move blocking ---
39+ LinearMPC. move_block! (newmpc, mpc. nb) # un-comment when debugged
40+ # ---- Constraint softening ---
41+ only_hard = weights. isinf_C
42+ if ! only_hard
43+ issoft (C) = any (x -> x > 0 , C)
44+ C_u = - mpc. con. A_Umin[:, end ]
45+ C_Δu = - mpc. con. A_ΔŨmin[1 : nΔU, end ]
46+ C_y = - mpc. con. A_Ymin[:, end ]
47+ c_x̂ = - mpc. con. A_x̂min[:, end ]
48+ if sum (mpc. con. i_b) > 1 # ignore the slack variable ϵ bound
49+ if issoft (C_u) || issoft (C_Δu) || issoft (C_y) || issoft (C_x̂)
50+ @warn " The LinearMPC conversion is approximate for the soft constraints.\n " *
51+ " You may need to adjust the soft_weight field of the " *
52+ " LinearMPC.MPC object to replicate behaviors."
53+ end
54+ end
55+ # LinearMPC relies on a different softening mechanism (new implicit slacks for each
56+ # softened bounds), so we apply an approximate conversion factor on the Cwt weight:
57+ Cwt = weights. Ñ_Hc[end , end ]
58+ nsoft = sum ((mpc. con. A[:,end ] .< 0 ) .& (mpc. con. i_b)) - 1
59+ newmpc. settings. soft_weight = 10 * sqrt (nsoft* Cwt)
60+ else
61+ C_u = zeros (nu* Hp)
62+ C_Δu = zeros (nu* Hc)
63+ C_y = zeros (ny* Hp)
64+ c_x̂ = zeros (nx̂)
65+ end
66+ # --- Manipulated inputs constraints ---
67+ Umin, Umax = mpc. con. U0min + mpc. Uop, mpc. con. U0max + mpc. Uop
68+ I_u = Matrix {Float64} (I, nu, nu)
69+ # add_constraint! does not support u bounds pass the control horizon Hc
70+ # so we compute the extremum bounds from k=Hc-1 to Hp, and apply them at k=Hc-1
71+ Umin_finals = reshape (Umin[nu* (Hc- 1 )+ 1 : end ], nu, :)
72+ Umax_finals = reshape (Umax[nu* (Hc- 1 )+ 1 : end ], nu, :)
73+ umin_end = mapslices (maximum, Umin_finals; dims= 2 )
74+ umax_end = mapslices (minimum, Umax_finals; dims= 2 )
75+ for k in 0 : Hc- 1
76+ if k < Hc - 1
77+ umin_k, umax_k = Umin[k* nu+ 1 : (k+ 1 )* nu], Umax[k* nu+ 1 : (k+ 1 )* nu]
78+ else
79+ umin_k, umax_k = umin_end, umax_end
80+ end
81+ c_u_k = C_u[k* nu+ 1 : (k+ 1 )* nu]
82+ ks = [k + 1 ] # a `1` in ks argument corresponds to the present time step k+0
83+ for i in 1 : nu
84+ lb = isfinite (umin_k[i]) ? [umin_k[i]] : zeros (0 )
85+ ub = isfinite (umax_k[i]) ? [umax_k[i]] : zeros (0 )
86+ soft = ! only_hard && c_u_k[i] > 0
87+ Au = I_u[i: i, :]
88+ LinearMPC. add_constraint! (newmpc; Au, lb, ub, ks, soft)
89+ end
90+ end
91+ # --- Input increment constraints ---
92+ ΔUmin, ΔUmax = mpc. con. ΔŨmin[1 : nΔU], mpc. con. ΔŨmax[1 : nΔU]
93+ I_Δu = Matrix {Float64} (I, nu, nu)
94+ for k in 0 : Hc- 1
95+ Δumin_k, Δumax_k = ΔUmin[k* nu+ 1 : (k+ 1 )* nu], ΔUmax[k* nu+ 1 : (k+ 1 )* nu]
96+ c_Δu_k = C_Δu[k* nu+ 1 : (k+ 1 )* nu]
97+ ks = [k + 1 ] # a `1` in ks argument corresponds to the present time step k+0
98+ for i in 1 : nu
99+ lb = isfinite (Δumin_k[i]) ? [Δumin_k[i]] : zeros (0 )
100+ ub = isfinite (Δumax_k[i]) ? [Δumax_k[i]] : zeros (0 )
101+ soft = ! only_hard && c_Δu_k[i] > 0
102+ Au, Aup = I_Δu[i: i, :], - I_Δu[i: i, :]
103+ LinearMPC. add_constraint! (newmpc; Au, Aup, lb, ub, ks, soft)
104+ end
105+ end
106+ # --- Output constraints ---
107+ Y0min, Y0max = mpc. con. Y0min, mpc. con. Y0max
108+ for k in 1 : Hp
109+ ymin_k, ymax_k = Y0min[(k- 1 )* ny+ 1 : k* ny], Y0max[(k- 1 )* ny+ 1 : k* ny]
110+ c_y_k = C_y[(k- 1 )* ny+ 1 : k* ny]
111+ ks = [k + 1 ] # a `1` in ks argument corresponds to the present time step k+0
112+ for i in 1 : ny
113+ lb = isfinite (ymin_k[i]) ? [ymin_k[i]] : zeros (0 )
114+ ub = isfinite (ymax_k[i]) ? [ymax_k[i]] : zeros (0 )
115+ soft = ! only_hard && c_y_k[i] > 0
116+ Ax, Ad = C[i: i, :], Dd[i: i, :]
117+ LinearMPC. add_constraint! (newmpc; Ax, Ad, lb, ub, ks, soft)
118+ end
119+ end
120+ # --- Terminal constraints ---
121+ x̂0min, x̂0max = mpc. con. x̂0min, mpc. con. x̂0max
122+ I_x̂ = Matrix {Float64} (I, nx̂, nx̂)
123+ ks = [Hp + 1 ] # a `1` in ks argument corresponds to the present time step k+0
124+ for i in 1 : nx̂
125+ lb = isfinite (x̂0min[i]) ? [x̂0min[i]] : zeros (0 )
126+ ub = isfinite (x̂0max[i]) ? [x̂0max[i]] : zeros (0 )
127+ soft = ! only_hard && c_x̂[i] > 0
128+ Ax = I_x̂[i: i, :]
129+ LinearMPC. add_constraint! (newmpc; Ax, lb, ub, ks, soft)
130+ end
131+ return newmpc
132+ end
133+
134+ function validate_compatibility (mpc:: ModelPredictiveControl.LinMPC )
135+ if mpc. transcription isa MultipleShooting
136+ error (" LinearMPC only supports SingleShooting transcription."
137+ end
138+ if ! (mpc. estim isa SteadyKalmanFilter) || ! mpc. estim. direct
139+ error (" LinearMPC only supports SteadyKalmanFilter with direct=true option."
140+ end
141+ if JuMP. solver_name (mpc. optim) != " DAQP"
142+ @warn " LinearMPC relies on DAQP, and the solver in the mpc object " *
143+ " is currently $(JuMP. solver_name (mpc. optim)) .\n " *
144+ " The results in closed-loop may be different."
145+ end
146+ validate_weights (mpc)
147+ validate_constraints (mpc)
148+ return nothing
149+ end
150+
151+ function validate_weights (mpc:: ModelPredictiveControl.LinMPC )
152+ ny, nu = mpc. estim. model. ny, mpc. estim. model. nu
153+ Hp, Hc = mpc. Hp, mpc. Hc
154+ nΔU = Hc * nu
155+ M_Hp, N_Hc, L_Hp = mpc. weights. M_Hp, mpc. weights. Ñ_Hc[1 : nΔU, 1 : nΔU], mpc. weights. L_Hp
156+ M_1, N_1, L_1 = M_Hp[1 : ny, 1 : ny], N_Hc[1 : nu, 1 : nu], L_Hp[1 : nu, 1 : nu]
157+ for i in 2 : mpc. Hp- 1 # last block is terminal weight, can be different
158+ M_i = M_Hp[(i- 1 )* ny+ 1 : i* ny, (i- 1 )* ny+ 1 : i* ny]
159+ if ! isapprox (M_i, M_1)
160+ error (" LinearMPC only supports identical weights for each stages in M_Hp."
161+ end
162+ end
163+ isblockdiag (M_Hp, ny, Hp) || error (" M_Hp must be block diagonal."
164+ for i in 2 : mpc. Hc
165+ N_i = N_Hc[(i- 1 )* nu+ 1 : i* nu, (i- 1 )* nu+ 1 : i* nu]
166+ if ! isapprox (N_i, N_1)
167+ error (" LinearMPC only supports identical weights for each stages in Ñ_Hc."
168+ end
169+ end
170+ isblockdiag (N_Hc, nu, Hc) || error (" Ñ_Hc must be block diagonal."
171+ for i in 2 : mpc. Hp
172+ L_i = L_Hp[(i- 1 )* nu+ 1 : i* nu, (i- 1 )* nu+ 1 : i* nu]
173+ if ! isapprox (L_i, L_1)
174+ error (" LinearMPC only supports identical weights for each stages in L_Hp."
175+ end
176+ end
177+ isblockdiag (L_Hp, nu, Hp) || error (" L_Hp must be block diagonal."
178+ return nothing
179+ end
180+
181+ function validate_constraints (mpc:: ModelPredictiveControl.LinMPC )
182+ nΔU = mpc. Hc * mpc. estim. model. nu
183+ mpc. weights. isinf_C && return nothing # only hard constraints are entirely supported
184+ C_umin, C_umax = - mpc. con. A_Umin[:, end ], - mpc. con. A_Umax[:, end ]
185+ C_Δumin, C_Δumax = - mpc. con. A_ΔŨmin[1 : nΔU, end ], - mpc. con. A_ΔŨmax[1 : nΔU, end ]
186+ C_ymin, C_ymax = - mpc. con. A_Ymin[:, end ], - mpc. con. A_Ymax[:, end ]
187+ C_x̂min, C_x̂max = - mpc. con. A_x̂min[:, end ], - mpc. con. A_x̂max[:, end ]
188+ is0or1 (C) = all (x -> x ≈ 0 || x ≈ 1 , C)
189+ if (
190+ ! is0or1 (C_umin) || ! is0or1 (C_umax) ||
191+ ! is0or1 (C_Δumin) || ! is0or1 (C_Δumax) ||
192+ ! is0or1 (C_ymin) || ! is0or1 (C_ymax) ||
193+ ! is0or1 (C_x̂min) || ! is0or1 (C_x̂max)
194+
195+ )
196+ error (" LinearMPC only supports softness parameters c = 0 or 1."
197+ end
198+ if (
199+ ! isapprox (C_umin, C_umax) ||
200+ ! isapprox (C_Δumin, C_Δumax) ||
201+ ! isapprox (C_ymin, C_ymax) ||
202+ ! isapprox (C_x̂min, C_x̂max)
203+ )
204+ error (" LinearMPC only supports identical softness parameters for lower and upper bounds."
205+ end
206+ return nothing
207+ end
208+
209+ @doc raw """
210+ LinearMPC.MPC(mpc::LinMPC)
211+
212+ Convert a `ModelPredictiveControl.LinMPC` object to a `LinearMPC.MPC` object.
213+
214+ The `LinearMPC` package needs to be installed and available in the activated Julia
215+ environment. The converted object can be used to generate lightweight C-code for embedded
216+ applications using the `LinearMPC.codegen` function. Note that not all features of [`LinMPC`](@ref)
217+ are supported, including these restrictions:
218+
219+ - the solver is limited to [`DAQP`](https://darnstrom.github.io/daqp/).
220+ - the transcription method must be [`SingleShooting`](@ref).
221+ - the state estimator must be a [`SteadyKalmanFilter`](@ref) with `direct=true`.
222+ - only block-diagonal weights are allowed.
223+ - the constraint relaxation mechanism is different, so a 1-on-1 conversion of the soft
224+ constraints is impossible (use `Cwt=Inf` to disable relaxation).
225+
226+ But the package has also several exclusive functionalities, such as pre-stabilization,
227+ constrained explicit MPC, and binary manipulated inputs. See the [`LinearMPC.jl`](@extref LinearMPC)
228+ documentation for more details on the supported features and how to generate code.
229+
230+ # Examples
231+ ```jldoctest
232+ julia> import LinearMPC, JuMP, DAQP;
233+
234+ julia> mpc1 = LinMPC(LinModel(tf(2, [10, 1]), 1.0); optim=JuMP.Model(DAQP.Optimizer));
235+
236+ julia> preparestate!(mpc1, [1.0]);
237+
238+ julia> u = moveinput!(mpc1, [10.0]); round.(u, digits=6)
239+ 1-element Vector{Float64}:
240+ 17.577311
241+
242+ julia> mpc2 = LinearMPC.MPC(mpc1);
243+
244+ julia> x̂ = LinearMPC.correct_state!(mpc2, [1.0]);
245+
246+ julia> u = LinearMPC.compute_control(mpc2, x̂, r=[10.0]); round.(u, digits=6)
247+ 1-element Vector{Float64}:
248+ 17.577311
249+ ```
250+ """
251+ LinearMPC. MPC (mpc:: ModelPredictiveControl.LinMPC ) = convert (LinearMPC. MPC, mpc)
252+
253+
254+ end # LinearMPCext
0 commit comments