Skip to content

Commit f2a50ea

Browse files
committed
added: MultipleShooting with NonLinModel is working 🍾
1 parent cf21adb commit f2a50ea

File tree

4 files changed

+180
-103
lines changed

4 files changed

+180
-103
lines changed

src/controller/construct.jl

Lines changed: 20 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -114,6 +114,8 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}}
114114
Aeq ::Matrix{NT}
115115
# b vector for the linear equality constraints:
116116
beq ::Vector{NT}
117+
# nonlinear equality constraints:
118+
neq ::Int
117119
# constraint softness parameter vectors for the nonlinear inequality constraints:
118120
C_ymin ::Vector{NT}
119121
C_ymax ::Vector{NT}
@@ -404,9 +406,9 @@ end
404406
model::LinModel, transcription::TranscriptionMethod, nc::Int,
405407
i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max,
406408
args...
407-
) -> i_b, i_g, A, Aeq
409+
) -> i_b, i_g, A, Aeq, neq
408410
409-
Init `i_b`, `i_g`, `A` and `Aeq` matrices for the linear and nonlinear constraints.
411+
Init `i_b`, `i_g`, `neq`, and `A` and `Aeq` matrices for the all the MPC constraints.
410412
411413
The linear and nonlinear constraints are respectively defined as:
412414
```math
@@ -420,25 +422,27 @@ The linear and nonlinear constraints are respectively defined as:
420422
The argument `nc` is the number of custom nonlinear inequality constraints in
421423
``\mathbf{g_c}``. `i_b` is a `BitVector` including the indices of ``\mathbf{b}`` that are
422424
finite numbers. `i_g` is a similar vector but for the indices of ``\mathbf{g}``. The method
423-
also returns the ``\mathbf{A, A_{eq}}`` matrices if `args` is provided. In such a case,
424-
`args` needs to contain all the inequality and equality constraint matrices:
425-
`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ`.
425+
also returns the ``\mathbf{A, A_{eq}}`` matrices and `neq` if `args` is provided. In such a
426+
case, `args` needs to contain all the inequality and equality constraint matrices:
427+
`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ`. The integer `neq`
428+
is the number of nonlinear equality constraints in ``\mathbf{g_{eq}}``.
426429
"""
427430
function init_matconstraint_mpc(
428431
::LinModel{NT}, ::TranscriptionMethod, nc::Int,
429432
i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max,
430433
args...
431434
) where {NT<:Real}
432435
if isempty(args)
433-
A, Aeq = nothing, nothing
436+
A, Aeq, neq = nothing, nothing, nothing
434437
else
435438
A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args
436439
A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max]
437440
Aeq = A_ŝ
441+
neq = 0
438442
end
439443
i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_x̂min; i_x̂max]
440444
i_g = trues(nc)
441-
return i_b, i_g, A, Aeq
445+
return i_b, i_g, A, Aeq, neq
442446
end
443447

444448
"Init `i_b` without output constraints if [`NonLinModel`](@ref) & [`MultipleShooting`](@ref)."
@@ -448,15 +452,17 @@ function init_matconstraint_mpc(
448452
args...
449453
) where {NT<:Real}
450454
if isempty(args)
451-
A, Aeq = nothing, nothing
455+
A, Aeq, neq = nothing, nothing, nothing
452456
else
453457
A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args
454458
A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max]
455459
Aeq = A_ŝ
460+
nΔŨ, nZ̃ = size(A_ΔŨmin)
461+
neq = nZ̃ - nΔŨ
456462
end
457463
i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_x̂min; i_x̂max]
458464
i_g = [i_Ymin; i_Ymax; trues(nc)]
459-
return i_b, i_g, A, Aeq
465+
return i_b, i_g, A, Aeq, neq
460466
end
461467

462468
"Init `i_b` without output & terminal constraints if [`NonLinModel`](@ref) & [`SingleShooting`](@ref)."
@@ -466,15 +472,16 @@ function init_matconstraint_mpc(
466472
args...
467473
) where {NT<:Real}
468474
if isempty(args)
469-
A, Aeq = nothing, nothing
475+
A, Aeq, neq = nothing, nothing, nothing
470476
else
471477
A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args
472478
A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max]
473479
Aeq = A_ŝ
480+
neq = 0
474481
end
475482
i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax]
476483
i_g = [i_Ymin; i_Ymax; i_x̂min; i_x̂max; trues(nc)]
477-
return i_b, i_g, A, Aeq
484+
return i_b, i_g, A, Aeq, neq
478485
end
479486

480487

@@ -603,7 +610,7 @@ function init_defaultcon_mpc(
603610
i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax)
604611
i_Ymin, i_Ymax = .!isinf.(Y0min), .!isinf.(Y0max)
605612
i_x̂min, i_x̂max = .!isinf.(x̂0min), .!isinf.(x̂0max)
606-
i_b, i_g, A, Aeq = init_matconstraint_mpc(
613+
i_b, i_g, A, Aeq, neq = init_matconstraint_mpc(
607614
model, transcription, nc,
608615
i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max,
609616
A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂max, A_x̂min,
@@ -619,6 +626,7 @@ function init_defaultcon_mpc(
619626
A , b , i_b ,
620627
A_ŝ ,
621628
Aeq , beq ,
629+
neq ,
622630
C_ymin , C_ymax , c_x̂min , c_x̂max , i_g,
623631
gc! , nc
624632
)

src/controller/execute.jl

Lines changed: 16 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -330,44 +330,36 @@ function linconstraint!(mpc::PredictiveController, ::SimModel)
330330
end
331331

332332
"""
333-
nonlinprog_vectors!(ΔŨ, Ŷe, Ue, Ū, mpc::PredictiveController, Ŷ0, Z̃) -> ΔŨ, Ŷe, Ue
333+
extended_vectors!(Ue, Ŷe, mpc::PredictiveController, U0, Ŷ0) -> Ue, Ŷe
334334
335-
Compute the `ΔŨ`, `Ŷe` and `Ue` vectors for nonlinear programming using `Ŷ0` and ``.
335+
Compute the extended `Ue` and `Ŷe` vectors for nonlinear programming using `U0` and `Ŷ0`.
336336
337-
See [`NonLinMPC`](@ref) for the definition of the vectors. The function mutates `ΔŨ`, `Ŷe`,
338-
`Ue` and `` in arguments, without assuming any initial values for them. Using
337+
See [`NonLinMPC`](@ref) for the definition of the vectors. The function mutates `Ue` and
338+
and `Ŷe` in arguments, without assuming any initial values for them. Using
339339
`nocustomfcts = mpc.weights.iszero_E && mpc.con.nc == 0`, there are three special cases in
340-
which ΔŨ, `Ŷe`, `Ue` and `` are not mutated:
340+
which `Ue` and `Ŷe` are not mutated:
341341
342-
- If `mpc.weights.iszero_Ñ_Hc[]`, the `ΔŨ` vector is not computed to reduce the burden in
343-
the optimization problem.
344342
- If `mpc.weights.iszero_M_Hp[] && nocustomfcts`, the `Ŷe` vector is not computed for the
345343
same reason as above.
346344
- If `mpc.weights.iszero_L_Hp[] && nocustomfcts`, the `Ue` vector is not computed for the
347345
same reason as above.
348346
"""
349-
function nonlinprog_vectors!(ΔŨ, Ŷe, Ue, Ū, mpc::PredictiveController, Ŷ0, Z̃)
347+
function extended_vectors!(Ue, Ŷe, mpc::PredictiveController, U0, Ŷ0)
350348
model = mpc.estim.model
351349
ny, nu = model.ny, model.nu
352350
nocustomfcts = (mpc.weights.iszero_E && iszero_nc(mpc))
353-
# --- augmented input increments ΔŨ = [ΔU; ϵ] ---
354-
if !(mpc.weights.iszero_Ñ_Hc[])
355-
ΔŨ .= mul!(ΔŨ, mpc.P̃Δu, Z̃)
351+
# --- extended manipulated inputs Ue = [U; u(k+Hp-1)] ---
352+
if !(mpc.weights.iszero_L_Hp[] && nocustomfcts)
353+
Ue[1:end-nu] .= U0 .+ mpc.Uop
354+
# u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp):
355+
Ue[end-nu+1:end] .= @views U[end-nu+1:end]
356356
end
357357
# --- extended output predictions Ŷe = [ŷ(k); Ŷ] ---
358358
if !(mpc.weights.iszero_M_Hp[] && nocustomfcts)
359359
Ŷe[1:ny] .= mpc.
360360
Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop
361361
end
362-
# --- extended manipulated inputs Ue = [U; u(k+Hp-1)] ---
363-
if !(mpc.weights.iszero_L_Hp[] && nocustomfcts)
364-
U =
365-
U .= mul!(U, mpc.P̃u, Z̃) .+ mpc.Tu_lastu
366-
Ue[1:end-nu] .= U
367-
# u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp):
368-
Ue[end-nu+1:end] .= @views U[end-nu+1:end]
369-
end
370-
return ΔŨ, Ŷe, Ue
362+
return Ue, Ŷe
371363
end
372364

373365
"Verify if the custom nonlinear constraint has zero elements."
@@ -391,23 +383,23 @@ function obj_nonlinprog!(
391383
end
392384

393385
"""
394-
obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ, Z̃)
386+
obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ)
395387
396388
Nonlinear programming objective method when `model` is not a [`LinModel`](@ref). The
397389
function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This method mutates
398390
`Ȳ` and `Ū` arguments, without assuming any initial values (it recuperates the values in
399391
`Ŷe` and `Ue` arguments).
400392
"""
401393
function obj_nonlinprog!(
402-
Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ, ::AbstractVector{NT}
394+
Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ::AbstractVector{NT}
403395
) where NT<:Real
404396
nu, ny = model.nu, model.ny
405397
# --- output setpoint tracking term ---
406398
if mpc.weights.iszero_M_Hp[]
407399
JR̂y = zero(NT)
408400
else
409401
Ȳ .= @views Ŷe[ny+1:end]
410-
Ȳ .= mpc.R̂y .-
402+
Ȳ .= .- mpc.R̂y
411403
JR̂y = dot(Ȳ, mpc.weights.M_Hp, Ȳ)
412404
end
413405
# --- move suppression and slack variable term ---
@@ -421,7 +413,7 @@ function obj_nonlinprog!(
421413
JR̂u = zero(NT)
422414
else
423415
Ū .= @views Ue[1:end-nu]
424-
Ū .= mpc.R̂u .-
416+
Ū .= .- mpc.R̂u
425417
JR̂u = dot(Ū, mpc.weights.L_Hp, Ū)
426418
end
427419
# --- economic term ---

0 commit comments

Comments
 (0)