diff --git a/Project.toml b/Project.toml index 9b8f4f196..1ebe9d446 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.0.0" +version = "2.1.0" authors = ["Francis Gagnon"] [deps] diff --git a/benchmark/3_bench_predictive_control.jl b/benchmark/3_bench_predictive_control.jl index f2250ee88..fb88d5b43 100644 --- a/benchmark/3_bench_predictive_control.jl +++ b/benchmark/3_bench_predictive_control.jl @@ -395,6 +395,12 @@ nmpc_madnlp_ms_hess = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcripti nmpc_madnlp_ms_hess = setconstraint!(nmpc_madnlp_ms_hess; umin, umax) JuMP.unset_time_limit_sec(nmpc_madnlp_ms_hess.optim) +optim = JuMP.Model(()->UnoSolver.Optimizer(preset="filtersqp"), add_bridges=false) +transcription, hessian = OrthogonalCollocation(), true +nmpc_uno_oc_hess = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription, hessian) +nmpc_uno_oc_hess = setconstraint!(nmpc_uno_oc_hess; umin, umax) +JuMP.unset_time_limit_sec(nmpc_uno_oc_hess.optim) + samples, evals, seconds = 100, 1, 15*60 CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["SingleShooting"] = @benchmarkable( @@ -461,7 +467,11 @@ CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Uno"]["MultipleShooting (Hessi sim!($nmpc_uno_ms_hess, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds, setup=GC.gc() ) - +CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Uno"]["OrthogonalCollocation (Hessian)"] = + @benchmarkable( + sim!($nmpc_uno_oc_hess, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), + samples=samples, evals=evals, seconds=seconds, setup=GC.gc() + ) # ----------------- Case study: Pendulum economic -------------------------------- model2, p = pendulum_model2, pendulum_p2 diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 630c6ca1a..5deecdbf4 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -129,10 +129,9 @@ end Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). Also known as pseudo-spectral method. It supports continuous-time [`NonLinModel`](@ref)s -only. The `h` argument is the hold order for ``\mathbf{u}``, and `no` argument, the number -of collocation points ``n_o``. Only zero-order hold is currently implemented, so `h` must -be `0`. The decision variable is similar to [`MultipleShooting`](@ref), but it also includes -the collocation points: +only. The `h` argument is the hold order for ``\mathbf{u}``, and the `no` argument, the +number of collocation points ``n_o``. The decision variable is similar to [`MultipleShooting`](@ref), +but it also includes the collocation points: ```math \mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} ``` @@ -187,9 +186,6 @@ struct OrthogonalCollocation <: CollocationMethod function OrthogonalCollocation( h::Int=0, no::Int=3; f_threads=false, h_threads=false, roots=:gaussradau ) - if !(h == 0) - throw(ArgumentError("Only the zero-order hold (h=0) is currently implemented.")) - end if roots==:gaussradau x, _ = FastGaussQuadrature.gaussradau(no, COLLOCATION_NODE_TYPE) # we reverse the nodes to include the τ=1.0 node: @@ -1378,6 +1374,11 @@ function con_nonlinprogeq!( nk = get_nk(model, transcription) D̂0 = mpc.D̂0 X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] + for j=0:Hp-1 # prefilling Û0 to avoid race-condition (both û0 and û0next are needed): + x̂0_Z̃ = @views j < 1 ? mpc.estim.x̂0[1:nx̂] : X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] + u0, û0 = @views U0[(1 + nu*j):(nu*(j+1))], Û0[(1 + nu*j):(nu*(j+1))] + f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) + end @threadsif f_threads for j=1:Hp if j < 2 x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] @@ -1401,9 +1402,7 @@ function con_nonlinprogeq!( fs!(x̂0next, mpc.estim, model, x̂0_Z̃) ssnext .= @. xsnext - xsnext_Z̃ # ----------------- deterministic defects: trapezoidal collocation ------------- - u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) if f_threads || h < 1 || j < 2 # we need to recompute k1 with multi-threading, even with h==1, since the # last iteration (j-1) may not be executed (iterations are re-orderable) @@ -1411,15 +1410,13 @@ function con_nonlinprogeq!( else k̇1 .= @views K̇[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1 end - if h < 1 || j ≥ Hp - # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc ≤ Hp implies Δu(k+Hp) = 0 - û0next = û0 + if h < 1 + model.f!(k̇2, x0next_Z̃, û0, d̂0next, p) else - u0next = @views U0[(1 + nu*j):(nu*(j+1))] - û0next = @views Û0[(1 + nu*j):(nu*(j+1))] - f̂_input!(û0next, mpc.estim, model, x̂0next_Z̃, u0next) + # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc≤Hp implies Δu(k+Hp) = 0: + û0next = @views j ≥ Hp ? û0 : Û0[(1 + nu*j):(nu*(j+1))] + model.f!(k̇2, x0next_Z̃, û0next, d̂0next, p) end - model.f!(k̇2, x0next_Z̃, û0next, d̂0next, p) sdnext .= @. x0_Z̃ - x0next_Z̃ + 0.5*Ts*(k̇1 + k̇2) end return geq @@ -1456,16 +1453,21 @@ extracted from the decision variable `Z̃`. The ``\mathbf{x_0}`` vectors are the deterministic state extracted from `Z̃`. The ``\mathbf{k̇}_i`` derivative for the ``i``th collocation point is computed from the continuous-time function `model.f!` and: ```math -\mathbf{k̇}_i(k+j) = \mathbf{f}\Big(\mathbf{k}_i(k+j), \mathbf{û_0}(k+j), \mathbf{d̂}_i(k+j), \mathbf{p}\Big) +\mathbf{k̇}_i(k+j) = \mathbf{f}\Big(\mathbf{k}_i(k+j), \mathbf{û_i}(k+j), \mathbf{d̂}_i(k+j), \mathbf{p}\Big) ``` -The disturbed input ``\mathbf{û_0}(k+j)`` is defined in [`f̂_input!`](@ref). Based on the -normalized time ``τ_i ∈ [0, 1]``, measured disturbances are linearly interpolated: +Based on the normalized time ``τ_i ∈ [0, 1]`` and hold order `transcription.h`, the inputs +and disturbances are piecewise constant or linear: ```math -\mathbf{d̂}_i(k+j) = (1-τ_i)\mathbf{d̂_0}(k+j) + τ_i\mathbf{d̂_0}(k+j+1) +\begin{aligned} +\mathbf{û}_i(k+j) &= \begin{cases} + \mathbf{û_0}(k+1) & h = 0 \\ + (1-τ_i)\mathbf{û_0}(k+j) + τ_i\mathbf{û_0}(k+j+1) & h = 1 \end{cases} \\ +\mathbf{d̂}_i(k+j) &= (1-τ_i)\mathbf{d̂_0}(k+j) + τ_i\mathbf{d̂_0}(k+j+1) +\end{aligned} ``` -The defects for the stochastic states ``\mathbf{s_s}`` are computed as in the -[`TrapezoidalCollocation`](@ref) method, and the ones for the continuity constraint of the -deterministic state trajectories are given by: +The disturbed input ``\mathbf{û_0}(k+j)`` is defined in [`f̂_input!`](@ref). The defects for +the stochastic states ``\mathbf{s_s}`` are computed as the [`TrapezoidalCollocation`](@ref) +method, and the ones for the continuity constraint of the deterministic states are: ```math \mathbf{s_c}(k+j+1) = \mathbf{C_o} \begin{bmatrix} @@ -1483,7 +1485,7 @@ function con_nonlinprogeq!( mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, U0, Z̃ ) - nu, nx̂, nd, nx = model.nu, mpc.estim.nx̂, model.nd, model.nx + nu, nx̂, nd, nx, h = model.nu, mpc.estim.nx̂, model.nd, model.nx, transcription.h Hp, Hc = mpc.Hp, mpc.Hc nΔU, nX̂ = nu*Hc, nx̂*Hp f_threads = transcription.f_threads @@ -1494,8 +1496,12 @@ function con_nonlinprogeq!( nx̂_nk = nx̂ + nk D̂0 = mpc.D̂0 X̂0_Z̃, K_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk*Hp)] - di = mpc.estim.buffer.d - ΔK = similar(K̇) # TODO: remove this allocation + D̂temp = mpc.buffer.D̂ + for j=0:Hp-1 # prefilling Û0 to avoid race-condition (both û0 and û0next are needed): + x̂0_Z̃ = @views j < 1 ? mpc.estim.x̂0[1:nx̂] : X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] + u0, û0 = @views U0[(1 + nu*j):(nu*(j+1))], Û0[(1 + nu*j):(nu*(j+1))] + f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) + end @threadsif f_threads for j=1:Hp if j < 2 x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂] @@ -1505,7 +1511,6 @@ function con_nonlinprogeq!( d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end k̇ = @views K̇[(1 + nk*(j-1)):(nk*j)] - Δk = @views ΔK[(1 + nk*(j-1)):(nk*j)] k_Z̃ = @views K_Z̃[(1 + nk*(j-1)):(nk*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] @@ -1521,18 +1526,30 @@ function con_nonlinprogeq!( fs!(x̂0next, mpc.estim, model, x̂0_Z̃) ssnext .= @. xsnext - xsnext_Z̃ # ----------------- collocation constraint defects ----------------------------- - u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0) + Δk = k̇ + for i=1:no + Δk[(1 + (i-1)*nx):(i*nx)] = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] .- x0_Z̃ + end + mul!(sk, Mo, Δk) + d̂i = @views D̂temp[(1 + nd*(j-1)):(nd*j)] + if h > 0 + ûi = similar(û0) # TODO: remove this allocation + end for i=1:no k̇i = @views k̇[(1 + (i-1)*nx):(i*nx)] - Δki = @views Δk[(1 + (i-1)*nx):(i*nx)] ki_Z̃ = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] - Δki .= @. ki_Z̃ - x0_Z̃ - di .= (1-τ[i]).*d̂0 .+ τ[i].*d̂0next - model.f!(k̇i, ki_Z̃, û0, d̂0, p) + d̂i .= (1-τ[i]).*d̂0 .+ τ[i].*d̂0next + if h < 1 + model.f!(k̇i, ki_Z̃, û0, d̂i, p) + else + # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc≤Hp implies Δu(k+Hp) = 0: + û0next = @views j ≥ Hp ? û0 : Û0[(1 + nu*j):(nu*(j+1))] + ûi .= (1-τ[i]).*û0 .+ τ[i].*û0next + model.f!(k̇i, ki_Z̃, ûi, d̂i, p) + end end - sk .= mul!(sk, Mo, Δk) .- k̇ + sk .-= k̇ # ----------------- continuity constraint defects ------------------------------ scnext .= mul!(scnext, Co, k_Z̃) .+ (λo.*x0_Z̃) .- x0next_Z̃ end diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 75a736a83..98c4469aa 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -955,36 +955,53 @@ end f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d h = (x,d,model) -> model.C*x + model.Dd*d nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) + + f! = (ẋ,x,u,_,_) -> ẋ .= -0.001x .+ u + h! = (y,x,_,_) -> y .= x + nonlinmodel_c = NonLinModel(f!, h!, 500, 1, 1, 1) - nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=100, Hc=1) - preparestate!(nmpc2, [0], [0]) + nmpc1 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=100, Hc=1) + preparestate!(nmpc1, [0], [0]) # if d=[0.1], the output will eventually reach 7*0.1=0.7, no action needed (u=0): d = [0.1] - u = moveinput!(nmpc2, 7d, d) + u = moveinput!(nmpc1, 7d, d) @test u ≈ [0] atol=5e-2 - u = nmpc2(7d, d) + u = nmpc1(7d, d) @test u ≈ [0] atol=5e-2 - info = getinfo(nmpc2) + info = getinfo(nmpc1) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 7d[1] atol=5e-2 - nmpc3 = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=Inf, Hp=100, Hc=1) - preparestate!(nmpc3, [0], [0]) - u = moveinput!(nmpc3, 7d, d) + nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=Inf, Hp=100, Hc=1) + preparestate!(nmpc2, [0], [0]) + u = moveinput!(nmpc2, 7d, d) @test u ≈ [0] atol=5e-2 - nmpc4 = NonLinMPC(nonlinmodel, Hp=15, Mwt=[0], Nwt=[0], Lwt=[1]) - preparestate!(nmpc4, [0], [0]) - u = moveinput!(nmpc4, [0], d, R̂u=fill(12, nmpc4.Hp)) + nmpc3 = NonLinMPC(nonlinmodel, Hp=15, Mwt=[0], Nwt=[0], Lwt=[1]) + preparestate!(nmpc3, [0], [0]) + u = moveinput!(nmpc3, [0], d, R̂u=fill(12, nmpc3.Hp)) @test u ≈ [12] atol=5e-2 + + transcription = MultipleShooting() + nmpc4 = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription) + preparestate!(nmpc4, [0], [0]) + u = moveinput!(nmpc4, [10], [0]) + @test u ≈ [2] atol=5e-2 + info = getinfo(nmpc4) + @test info[:u] ≈ u + @test info[:Ŷ][end] ≈ 10 atol=5e-2 - nmpc5 = NonLinMPC(nonlinmodel, Hp=1, Hc=1, Cwt=Inf, transcription=MultipleShooting()) - nmpc5 = setconstraint!(nmpc5, ymin=[1]) - f! = (ẋ,x,u,_,_) -> ẋ .= -0.001x .+ u - h! = (y,x,_,_) -> y .= x - nonlinmodel_c = NonLinModel(f!, h!, 500, 1, 1, 1) + transcription = MultipleShooting(f_threads=true, h_threads=true) + nmpc4t = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription, hessian=true) + nmpc4t = setconstraint!(nmpc4t, ymax=[100], ymin=[-100]) # coverage of getinfo! Hessians of Lagrangian + preparestate!(nmpc4t, [0], [0]) + u = moveinput!(nmpc4t, [10], [0]) + @test u ≈ [2] atol=5e-2 + info = getinfo(nmpc4t) + @test info[:u] ≈ u + @test info[:Ŷ][end] ≈ 10 atol=5e-2 + transcription = TrapezoidalCollocation(0, f_threads=true, h_threads=true) - nmpc5 = NonLinMPC(nonlinmodel_c; Nwt=[0], Hp=100, Hc=1, transcription) preparestate!(nmpc5, [0.0]) u = moveinput!(nmpc5, [1/0.001]) @@ -997,13 +1014,13 @@ end @test u ≈ [1.0] atol=5e-2 transcription = OrthogonalCollocation(0, 4) - nmpc6 = NonLinMPC(InternalModel(nonlinmodel_c); Nwt=[0], Hp=100, Hc=1, transcription) + nmpc6 = NonLinMPC(nonlinmodel_c; Nwt=[0], Hp=100, Hc=1, transcription) preparestate!(nmpc6, [0.0]) u = moveinput!(nmpc6, [1/0.001]) @test u ≈ [1.0] atol=5e-2 - transcription = OrthogonalCollocation(roots=:gausslegendre) - nmpc6_1 = NonLinMPC(InternalModel(nonlinmodel_c); Nwt=[0], Hp=100, Hc=1, transcription) + transcription = OrthogonalCollocation(1, roots=:gausslegendre) + nmpc6_1 = NonLinMPC(nonlinmodel_c; Nwt=[0], Hp=100, Hc=1, transcription) preparestate!(nmpc6_1, [0.0]) u = moveinput!(nmpc6_1, [1/0.001]) @test u ≈ [1.0] atol=5e-2 @@ -1014,25 +1031,6 @@ end ModelPredictiveControl.h!(y, nonlinmodel2, Float32[0,0], Float32[0], nonlinmodel2.p) preparestate!(nmpc7, [0], [0]) @test moveinput!(nmpc7, [0], [0]) ≈ [0.0] atol=5e-2 - transcription = MultipleShooting() - - nmpc8 = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription) - preparestate!(nmpc8, [0], [0]) - u = moveinput!(nmpc8, [10], [0]) - @test u ≈ [2] atol=5e-2 - info = getinfo(nmpc8) - @test info[:u] ≈ u - @test info[:Ŷ][end] ≈ 10 atol=5e-2 - - transcription = MultipleShooting(f_threads=true, h_threads=true) - nmpc8t = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription, hessian=true) - nmpc8t = setconstraint!(nmpc8t, ymax=[100], ymin=[-100]) # coverage of getinfo! Hessians of Lagrangian - preparestate!(nmpc8t, [0], [0]) - u = moveinput!(nmpc8t, [10], [0]) - @test u ≈ [2] atol=5e-2 - info = getinfo(nmpc8t) - @test info[:u] ≈ u - @test info[:Ŷ][end] ≈ 10 atol=5e-2 nmpc10 = setconstraint!(NonLinMPC( nonlinmodel, Nwt=[0], Hp=100, Hc=1,