From 00ac589d15c307597c5efc9f2af951045497aa5c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 5 Sep 2025 15:01:41 -0400 Subject: [PATCH 1/6] wip : foh trapezoidal --- src/controller/transcription.jl | 38 ++++++++++++++++++++++----------- 1 file changed, 26 insertions(+), 12 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 09e6e4de2..5ca2df87e 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -64,14 +64,14 @@ for this transcription method. struct MultipleShooting <: ShootingMethod end @doc raw""" - TrapezoidalCollocation() + TrapezoidalCollocation(nh::Int=0) -Construct an implicit trapezoidal [`TranscriptionMethod`](@ref). +Construct an implicit trapezoidal [`TranscriptionMethod`](@ref) with `nh`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 `nh` argument is `0` or `1`, for piecewise constant or linear +manipulated inputs ``\mathbf{u}`` (`1` is slightly less computationally expensive). 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 +92,14 @@ transcription method. for more details. """ struct TrapezoidalCollocation <: CollocationMethod + nh::Int nc::Int - function TrapezoidalCollocation() + function TrapezoidalCollocation(nh::Int=0) + if !(nh == 0 || nh == 1) + throw(ArgumentError("nh argument must be 0 or 1 for TrapezoidalCollocation.")) + end nc = 2 # 2 collocation points per interval for trapezoidal rule - return new(nc) + return new(nh, nc) end end @@ -1403,8 +1407,6 @@ function con_nonlinprogeq!( d0 = @views mpc.d0[1:nd] #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)] @@ -1413,11 +1415,23 @@ function con_nonlinprogeq!( 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] 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 + if iszero(transcription.nh) # 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, xd, û0, d0, p) + model.f!(k2, xdnext_Z̃, û0, d0next, p) + else # piecewise linear manipulated inputs u: + u0next = @views U0[(1 + nu*j):(nu*(j+1))] + û0next = @views U0[(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, xdnext_Z̃, û0next, d0next, p) + k1 .= lastk2 + lastk2 = k2 + end xsnext = @views x̂0next[nx+1:end] mul!(xsnext, As, xs) sdnext .= @. xd - xdnext_Z̃ + (Ts/2)*(k1 + k2) From 43d324ba1c5bbde059d5aae0b7534bcea2f4b979 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 5 Sep 2025 16:15:44 -0400 Subject: [PATCH 2/6] added: first order hold for `TrapezoidalCollocation` --- src/controller/transcription.jl | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 5ca2df87e..298a41de1 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1405,6 +1405,14 @@ 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(transcription.nh) + k1, u0, û0 = @views K0[1:nx], U0[1:nu], Û0[1:nu] + xd, xs = @views x̂0[1:nx], x̂0[nx+1:end] + mul!(û0, Cs_u, xs) + û0 .+= u0 + model.f!(k1, xd, û0, d0, p) + lastk2 = k1 + end #TODO: allow parallel for loop or threads? for j=1:Hp k0 = @views K0[(1 + nk*(j-1)):(nk*j)] @@ -1424,12 +1432,17 @@ function con_nonlinprogeq!( model.f!(k1, xd, û0, d0, p) model.f!(k2, xdnext_Z̃, û0, d0next, p) else # piecewise linear manipulated inputs u: - u0next = @views U0[(1 + nu*j):(nu*(j+1))] - û0next = @views U0[(1 + nu*j):(nu*(j+1))] + if j < Hp + u0next = @views U0[(1 + nu*j):(nu*(j+1))] + û0next = @views Û0[(1 + nu*j):(nu*(j+1))] + else # u(k+Hp) = u(k+Hp-1) since Hc ≤ Hp implies that Δu(k+Hp)=0 + u0next = @views U0[(1 + nu*(j-1)):(nu*j)] + û0next = @views Û0[(1 + nu*(j-1)):(nu*j)] + end 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, xdnext_Z̃, û0next, d0next, p) k1 .= lastk2 + model.f!(k2, xdnext_Z̃, û0next, d0next, p) lastk2 = k2 end xsnext = @views x̂0next[nx+1:end] From d7638b46ce00b1090714e3aeb625291e931909ec Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 5 Sep 2025 16:26:24 -0400 Subject: [PATCH 3/6] doc: clarification on possible plant-model mismatch with `nh=1` --- src/controller/transcription.jl | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 298a41de1..53d57170f 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -71,7 +71,9 @@ Construct an implicit trapezoidal [`TranscriptionMethod`](@ref) with `nh`th orde 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. The `nh` argument is `0` or `1`, for piecewise constant or linear -manipulated inputs ``\mathbf{u}`` (`1` is slightly less computationally expensive). +manipulated inputs ``\mathbf{u}`` (`nh=1` is slightly less expensive). Note that the various +[`DiffSolver`](@ref) assume zero-order hold, so `nh=1` will induce a mismatch if the plant +is simulated using 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 From 632b9092d1722b7bb2db9976e05ff124784cecd9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 5 Sep 2025 16:29:35 -0400 Subject: [PATCH 4/6] test: new tests with `TrapezoidalCollocation(1)` --- test/3_test_predictive_control.jl | 5 +++++ 1 file changed, 5 insertions(+) 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 From 4b0c849e24cced4f4b3da66474ab9786ed1b1902 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 6 Sep 2025 15:31:36 -0400 Subject: [PATCH 5/6] debug: last `sdnext` after loop for `nh=1` (special case) --- src/controller/transcription.jl | 59 +++++++++++++++++++-------------- 1 file changed, 35 insertions(+), 24 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 53d57170f..c320418c7 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1397,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, nh = model.nu, mpc.estim.nx̂, model.nd, model.nx, transcription.nh Hp, Hc = mpc.Hp, mpc.Hc nΔU, nX̂ = nu*Hc, nx̂*Hp Ts, p = model.Ts, model.p @@ -1407,12 +1407,12 @@ 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(transcription.nh) + if !iszero(nh) k1, u0, û0 = @views K0[1:nx], U0[1:nu], Û0[1:nu] - xd, xs = @views x̂0[1:nx], x̂0[nx+1:end] + x0, xs = @views x̂0[1:nx], x̂0[nx+1:end] mul!(û0, Cs_u, xs) û0 .+= u0 - model.f!(k1, xd, û0, d0, p) + model.f!(k1, x0, û0, d0, p) lastk2 = k1 end #TODO: allow parallel for loop or threads? @@ -1422,38 +1422,49 @@ function con_nonlinprogeq!( 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] - k1, k2 = @views k0[1:nx], k0[nx+1:2*nx] - if iszero(transcription.nh) # piecewise constant 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) + ssnext .= @. xsnext - xsnext_Z̃ + # ----------------- deterministic defects -------------------------------------- + if iszero(nh) # 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, xd, û0, d0, p) - model.f!(k2, xdnext_Z̃, û0, d0next, p) + model.f!(k1, x0, û0, d0, p) + model.f!(k2, x0next_Z̃, û0, d0next, p) else # piecewise linear manipulated inputs u: - if j < Hp - u0next = @views U0[(1 + nu*j):(nu*(j+1))] - û0next = @views Û0[(1 + nu*j):(nu*(j+1))] - else # u(k+Hp) = u(k+Hp-1) since Hc ≤ Hp implies that Δu(k+Hp)=0 - u0next = @views U0[(1 + nu*(j-1)):(nu*j)] - û0next = @views Û0[(1 + nu*(j-1)):(nu*j)] - end + 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) - k1 .= lastk2 - model.f!(k2, xdnext_Z̃, û0next, d0next, p) + model.f!(k2, x0next_Z̃, û0next, d0next, p) lastk2 = k2 - end - xsnext = @views x̂0next[nx+1:end] - mul!(xsnext, As, xs) - sdnext .= @. xd - xdnext_Z̃ + (Ts/2)*(k1 + k2) - ssnext .= @. xsnext - xsnext_Z̃ + 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(nh) + # 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 From 02bcdb56edae271f10f4f64f6d0f4bec419f6008 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 6 Sep 2025 15:42:05 -0400 Subject: [PATCH 6/6] changed: hold order `nh` renamed to `h` --- src/controller/transcription.jl | 36 ++++++++++++++++----------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index c320418c7..efd9055eb 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -64,16 +64,16 @@ for this transcription method. struct MultipleShooting <: ShootingMethod end @doc raw""" - TrapezoidalCollocation(nh::Int=0) + TrapezoidalCollocation(h::Int=0) -Construct an implicit trapezoidal [`TranscriptionMethod`](@ref) with `nh`th order hold. +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. The `nh` argument is `0` or `1`, for piecewise constant or linear -manipulated inputs ``\mathbf{u}`` (`nh=1` is slightly less expensive). Note that the various -[`DiffSolver`](@ref) assume zero-order hold, so `nh=1` will induce a mismatch if the plant -is simulated using these solvers. +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 @@ -94,14 +94,14 @@ transcription method. for more details. """ struct TrapezoidalCollocation <: CollocationMethod - nh::Int + h::Int nc::Int - function TrapezoidalCollocation(nh::Int=0) - if !(nh == 0 || nh == 1) - throw(ArgumentError("nh argument must be 0 or 1 for 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(nh, nc) + return new(h, nc) end end @@ -1382,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) ``` @@ -1397,7 +1397,7 @@ function con_nonlinprogeq!( mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, U0, Z̃ ) - nu, nx̂, nd, nx, nh = model.nu, mpc.estim.nx̂, model.nd, model.nx, transcription.nh + 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 @@ -1407,7 +1407,7 @@ 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(nh) + 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) @@ -1431,7 +1431,7 @@ function con_nonlinprogeq!( mul!(xsnext, As, xs) ssnext .= @. xsnext - xsnext_Z̃ # ----------------- deterministic defects -------------------------------------- - if iszero(nh) # piecewise constant manipulated inputs u: + 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) @@ -1452,7 +1452,7 @@ function con_nonlinprogeq!( x̂0 = x̂0next_Z̃ # using states in Z̃ for next iteration (allow parallel for) d0 = d0next end - if !iszero(nh) + 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