Skip to content

Commit dcf5839

Browse files
franckgagafranckgaga
committed
debug: add dispatch on transcription for linconstraint 
1 parent c246ddb commit dcf5839

File tree

3 files changed

+94
-62
lines changed

3 files changed

+94
-62
lines changed

‎src/controller/execute.jl‎

Lines changed: 1 addition & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ function moveinput!(
6666
end
6767
validate_args(mpc, ry, d, D̂, R̂y, R̂u)
6868
initpred!(mpc, mpc.estim.model, d, D̂, R̂y, R̂u)
69-
linconstraint!(mpc, mpc.estim.model)
69+
linconstraint!(mpc, mpc.estim.model, mpc.transcription)
7070
linconstrainteq!(mpc, mpc.estim.model, mpc.transcription)
7171
Z̃ = optim_objective!(mpc)
7272
return getinput(mpc, Z̃)
@@ -269,66 +269,6 @@ end
269269
"Fill `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)."
270270
predictstoch!(Ŷs, mpc::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing)
271271

272-
@doc raw"""
273-
linconstraint!(mpc::PredictiveController, model::LinModel)
274-
275-
Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ b}``).
276-
277-
Also init ``\mathbf{f_x̂} = \mathbf{g_x̂ d_0}(k) + \mathbf{j_x̂ D̂_0} + \mathbf{k_x̂ x̂_0}(k) +
278-
\mathbf{v_x̂ u_0}(k-1) + \mathbf{b_x̂}`` vector for the terminal constraints, see
279-
[`init_predmat`](@ref).
280-
"""
281-
function linconstraint!(mpc::PredictiveController, model::LinModel)
282-
nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min)
283-
nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂
284-
fx̂ .= mpc.con.bx̂
285-
mul!(fx̂, mpc.con.kx̂, mpc.estim.x̂0, 1, 1)
286-
mul!(fx̂, mpc.con.vx̂, mpc.estim.lastu0, 1, 1)
287-
if model.nd ≠ 0
288-
mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1)
289-
mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1)
290-
end
291-
n = 0
292-
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0
293-
n += nU
294-
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.Tu_lastu0
295-
n += nU
296-
mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
297-
n += nΔŨ
298-
mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax
299-
n += nΔŨ
300-
mpc.con.b[(n+1):(n+nY)] .= @. -mpc.con.Y0min + mpc.F
301-
n += nY
302-
mpc.con.b[(n+1):(n+nY)] .= @. +mpc.con.Y0max - mpc.F
303-
n += nY
304-
mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min + fx̂
305-
n += nx̂
306-
mpc.con.b[(n+1):(n+nx̂)] .= @. +mpc.con.x̂0max - fx̂
307-
if any(mpc.con.i_b)
308-
lincon = mpc.optim[:linconstraint]
309-
JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b])
310-
end
311-
return nothing
312-
end
313-
314-
"Set `b` excluding predicted output constraints when `model` is not a [`LinModel`](@ref)."
315-
function linconstraint!(mpc::PredictiveController, ::SimModel)
316-
nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin)
317-
n = 0
318-
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0
319-
n += nU
320-
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.Tu_lastu0
321-
n += nU
322-
mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
323-
n += nΔŨ
324-
mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax
325-
if any(mpc.con.i_b)
326-
lincon = mpc.optim[:linconstraint]
327-
@views JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b])
328-
end
329-
return nothing
330-
end
331-
332272
"""
333273
extended_vectors!(Ue, Ŷe, mpc::PredictiveController, U0, Ŷ0) -> Ue, Ŷe
334274

‎src/controller/explicitmpc.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -182,7 +182,7 @@ function Base.show(io::IO, mpc::ExplicitMPC)
182182
print_estim_dim(io, mpc.estim, n)
183183
end
184184

185-
linconstraint!(::ExplicitMPC, ::LinModel) = nothing
185+
linconstraint!(::ExplicitMPC, ::LinModel, ::TranscriptionMethod) = nothing
186186

187187
@doc raw"""
188188
optim_objective!(mpc::ExplicitMPC) -> Z̃

‎src/controller/transcription.jl‎

Lines changed: 92 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -742,6 +742,98 @@ function set_nonlincon!(
742742
return nothing
743743
end
744744

745+
@doc raw"""
746+
linconstraint!(mpc::PredictiveController, model::LinModel)
747+
748+
Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ b}``).
749+
750+
Also init ``\mathbf{f_x̂} = \mathbf{g_x̂ d_0}(k) + \mathbf{j_x̂ D̂_0} + \mathbf{k_x̂ x̂_0}(k) +
751+
\mathbf{v_x̂ u_0}(k-1) + \mathbf{b_x̂}`` vector for the terminal constraints, see
752+
[`init_predmat`](@ref).
753+
"""
754+
function linconstraint!(mpc::PredictiveController, model::LinModel, ::TranscriptionMethod)
755+
nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min)
756+
nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂
757+
fx̂ .= mpc.con.bx̂
758+
mul!(fx̂, mpc.con.kx̂, mpc.estim.x̂0, 1, 1)
759+
mul!(fx̂, mpc.con.vx̂, mpc.estim.lastu0, 1, 1)
760+
if model.nd ≠ 0
761+
mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1)
762+
mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1)
763+
end
764+
n = 0
765+
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0
766+
n += nU
767+
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.Tu_lastu0
768+
n += nU
769+
mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
770+
n += nΔŨ
771+
mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax
772+
n += nΔŨ
773+
mpc.con.b[(n+1):(n+nY)] .= @. -mpc.con.Y0min + mpc.F
774+
n += nY
775+
mpc.con.b[(n+1):(n+nY)] .= @. +mpc.con.Y0max - mpc.F
776+
n += nY
777+
mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min + fx̂
778+
n += nx̂
779+
mpc.con.b[(n+1):(n+nx̂)] .= @. +mpc.con.x̂0max - fx̂
780+
if any(mpc.con.i_b)
781+
lincon = mpc.optim[:linconstraint]
782+
JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b])
783+
end
784+
return nothing
785+
end
786+
787+
"Set `b` excluding predicted output constraints for `NonLinModel` and not `SingleShooting`."
788+
function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod)
789+
nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min)
790+
nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂
791+
fx̂ .= mpc.con.bx̂
792+
mul!(fx̂, mpc.con.kx̂, mpc.estim.x̂0, 1, 1)
793+
mul!(fx̂, mpc.con.vx̂, mpc.estim.lastu0, 1, 1)
794+
if model.nd ≠ 0
795+
mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1)
796+
mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1)
797+
end
798+
n = 0
799+
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0
800+
n += nU
801+
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.Tu_lastu0
802+
n += nU
803+
mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
804+
n += nΔŨ
805+
mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax
806+
n += nΔŨ
807+
mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min + fx̂
808+
n += nx̂
809+
mpc.con.b[(n+1):(n+nx̂)] .= @. +mpc.con.x̂0max - fx̂
810+
if any(mpc.con.i_b)
811+
lincon = mpc.optim[:linconstraint]
812+
JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b])
813+
end
814+
end
815+
816+
"Also exclude terminal constraints for `NonLinModel` and `SingleShooting`."
817+
function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooting)
818+
nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin)
819+
n = 0
820+
mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0
821+
n += nU
822+
mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.Tu_lastu0
823+
n += nU
824+
mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
825+
n += nΔŨ
826+
mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax
827+
if any(mpc.con.i_b)
828+
lincon = mpc.optim[:linconstraint]
829+
@views JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b])
830+
end
831+
return nothing
832+
end
833+
834+
835+
836+
745837
@doc raw"""
746838
linconstrainteq!(
747839
mpc::PredictiveController, model::LinModel, transcription::MultipleShooting

0 commit comments

Comments
 (0)