Merge pull request #331 from JuliaControl/ortho_colloc_foh

added: `h=1` support for `OrthogonalCollocation` + misc. improvements

Author: franckgaga

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]

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

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,25 +1402,21 @@ 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)
             model.f!(k̇1, x0_Z̃, û0, d̂0, p)
         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

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,