diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 09e6e4de2..efd9055eb 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -64,14 +64,16 @@ for this transcription method. struct MultipleShooting <: ShootingMethod end @doc raw""" - TrapezoidalCollocation() + TrapezoidalCollocation(h::Int=0) -Construct an implicit trapezoidal [`TranscriptionMethod`](@ref). +Construct an implicit trapezoidal [`TranscriptionMethod`](@ref) with `h`th order hold. This is the simplest collocation method. It supports continuous-time [`NonLinModel`](@ref)s only. The decision variables are the same as for [`MultipleShooting`](@ref), hence similar -computational costs. It currently assumes piecewise constant manipulated inputs (or zero- -order hold) between the samples, but linear interpolation will be added soon. +computational costs. The `h` argument is `0` or `1`, for piecewise constant or linear +manipulated inputs ``\mathbf{u}`` (`h=1` is slightly less expensive). Note that the various +[`DiffSolver`](@ref) here assume zero-order hold, so `h=1` will induce a plant-model +mismatch if the plant is simulated with these solvers. This transcription computes the predictions by calling the continuous-time model in the equality constraint function and by using the implicit trapezoidal rule. It can handle @@ -92,10 +94,14 @@ transcription method. for more details. """ struct TrapezoidalCollocation <: CollocationMethod + h::Int nc::Int - function TrapezoidalCollocation() + function TrapezoidalCollocation(h::Int=0) + if !(h == 0 || h == 1) + throw(ArgumentError("h argument must be 0 or 1 for TrapezoidalCollocation.")) + end nc = 2 # 2 collocation points per interval for trapezoidal rule - return new(nc) + return new(h, nc) end end @@ -1376,11 +1382,11 @@ extracted from the decision variables `Z̃`. The ``\mathbf{k}`` coefficients are evaluated from the continuous-time function `model.f!` and: ```math \begin{aligned} -\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d_0}(k) \Big) \\ -\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0}(k+1), \mathbf{û_0}(k), \mathbf{d_0}(k+1)\Big) +\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d_0}(k) \Big) \\ +\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0}(k+1), \mathbf{û_0}(k+h), \mathbf{d_0}(k+1)\Big) \end{aligned} ``` -and the input of the augmented model is: +in which ``h`` is the hold order `transcription.h` and the disturbed input is: ```math \mathbf{û_0}(k) = \mathbf{u_0}(k) + \mathbf{C_{s_u} x_s}(k) ``` @@ -1391,7 +1397,7 @@ function con_nonlinprogeq!( mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, 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 Ts, p = model.Ts, model.p @@ -1401,30 +1407,64 @@ function con_nonlinprogeq!( X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] x̂0 = @views mpc.estim.x̂0[1:nx̂] d0 = @views mpc.d0[1:nd] + if !iszero(h) + k1, u0, û0 = @views K0[1:nx], U0[1:nu], Û0[1:nu] + x0, xs = @views x̂0[1:nx], x̂0[nx+1:end] + mul!(û0, Cs_u, xs) + û0 .+= u0 + model.f!(k1, x0, û0, d0, p) + lastk2 = k1 + end #TODO: allow parallel for loop or threads? for j=1:Hp - u0 = @views U0[(1 + nu*(j-1)):(nu*j)] - û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] k0 = @views K0[(1 + nk*(j-1)):(nk*j)] d0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] - xd, xs = @views x̂0[1:nx], x̂0[nx+1:end] - xdnext_Z̃, xsnext_Z̃ = @views x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end] + x0, xs = @views x̂0[1:nx], x̂0[nx+1:end] + x0next_Z̃, xsnext_Z̃ = @views x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end] sdnext, ssnext = @views ŝnext[1:nx], ŝnext[nx+1:end] - mul!(û0, Cs_u, xs) # ys_u = Cs_u*xs - û0 .+= u0 # û0 = u0 + ys_u - k1, k2 = @views k0[1:nx], k0[nx+1:2*nx] - model.f!(k1, xd, û0, d0, p) - model.f!(k2, xdnext_Z̃, û0, d0next, p) # assuming ZOH on manipulated inputs u + k1, k2 = @views k0[1:nx], k0[nx+1:2*nx] + # ----------------- stochastic defects ----------------------------------------- xsnext = @views x̂0next[nx+1:end] mul!(xsnext, As, xs) - sdnext .= @. xd - xdnext_Z̃ + (Ts/2)*(k1 + k2) ssnext .= @. xsnext - xsnext_Z̃ + # ----------------- deterministic defects -------------------------------------- + if iszero(h) # piecewise constant manipulated inputs u: + u0 = @views U0[(1 + nu*(j-1)):(nu*j)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + mul!(û0, Cs_u, xs) # ys_u(k) = Cs_u*xs(k) + û0 .+= u0 # û0(k) = u0(k) + ys_u(k) + model.f!(k1, x0, û0, d0, p) + model.f!(k2, x0next_Z̃, û0, d0next, p) + else # piecewise linear manipulated inputs u: + k1 .= lastk2 + j == Hp && break # special case, treated after the loop + u0next = @views U0[(1 + nu*j):(nu*(j+1))] + û0next = @views Û0[(1 + nu*j):(nu*(j+1))] + mul!(û0next, Cs_u, xsnext_Z̃) # ys_u(k+1) = Cs_u*xs(k+1) + û0next .+= u0next # û0(k+1) = u0(k+1) + ys_u(k+1) + model.f!(k2, x0next_Z̃, û0next, d0next, p) + lastk2 = k2 + end + sdnext .= @. x0 - x0next_Z̃ + 0.5*Ts*(k1 + k2) x̂0 = x̂0next_Z̃ # using states in Z̃ for next iteration (allow parallel for) d0 = d0next end + if !iszero(h) + # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc ≤ Hp implies Δu(k+Hp)=0 + x̂0, x̂0next_Z̃ = @views X̂0_Z̃[end-2nx̂+1:end-nx̂], X̂0_Z̃[end-nx̂+1:end] + k1, k2 = @views K0[end-2nx+1:end-nx], K0[end-nx+1:end] # k1 already filled + d0next = @views D̂0[end-nd+1:end] + û0next, u0next = @views Û0[end-nu+1:end], U0[end-nu+1:end] # correspond to u(k+Hp-1) + x0, x0next_Z̃, xsnext_Z̃ = @views x̂0[1:nx], x̂0next_Z̃[1:nx], x̂0next_Z̃[nx+1:end] + sdnext = @views geq[end-nx̂+1:end-nx̂+nx] # ssnext already filled + mul!(û0next, Cs_u, xsnext_Z̃) + û0next .+= u0next + model.f!(k2, x0next_Z̃, û0next, d0next, p) + sdnext .= @. x0 - x0next_Z̃ + (Ts/2)*(k1 + k2) + end return geq end diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index ab244a8db..2991faf35 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -728,6 +728,7 @@ end @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, gc = (_,_,_,_)->[0.0], nc=1) @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, gc! = (_,_,_,_)->[0.0], nc=1) @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation()) + @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation(2)) @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, JE=(Ue,_,_,_)->Ue) @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, gc=(Ue,_,_,_,_)->Ue, nc=0) @@ -806,6 +807,10 @@ end preparestate!(nmpc5, [0.0]) u = moveinput!(nmpc5, [1/0.001]) @test u ≈ [1.0] atol=5e-2 + nmpc5_1 = NonLinMPC(nonlinmodel_c, Nwt=[0], Hp=100, Hc=1, transcription=TrapezoidalCollocation(1)) + preparestate!(nmpc5_1, [0.0]) + u = moveinput!(nmpc5_1, [1/0.001]) + @test u ≈ [1.0] atol=5e-2 nmpc6 = NonLinMPC(linmodel3, Hp=10) preparestate!(nmpc6, [0]) @test moveinput!(nmpc6, [0]) ≈ [0.0] atol=5e-2