debug: getinfo! correct output predictions MPC

franckgaga · franckgaga · commit af8de3b52445 · 2025-02-26T18:10:07.000-05:00
diff --git a/src/controller/execute.jl b/src/controller/execute.jl
@@ -124,12 +124,13 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     Ȳ, Ū        = similar(mpc.Yop), similar(mpc.Uop)
     ΔŨ          = Vector{NT}(undef, nΔŨ)
     Ŷe, Ue      = Vector{NT}(undef, nŶe), Vector{NT}(undef, nUe)
-    Ŷ0, x̂0end  = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, transcription, Z̃)
-    ΔŨ, Ŷe, Ue = nonlinprog_vectors!(ΔŨ, Ŷe, Ue, Ū, mpc, Ŷ0, Z̃)
-    J         = obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, ΔŨ, Z̃)
+    # careful, 1st arg is Ȳ in NonLinMPC to save memory, but Ŷ0 in this func for clarity:
+    Ŷ0, x̂0end   = predict!(Ŷ0, x̂0next, x̂0, u0, û0, mpc, model, transcription, Z̃)
+    ΔŨ, Ŷe, Ue  = nonlinprog_vectors!(ΔŨ, Ŷe, Ue, Ū, mpc, Ŷ0, Z̃)
+    J           = obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, ΔŨ, Z̃)
     U, Ŷ = Ū, Ȳ
-    U   .= mul!(U, mpc.P̃u, mpc.Z̃) .+ mpc.Tu_lastu 
-    Ŷ   .= Ŷ0 .+ mpc.Yop
+    U .= mul!(U, mpc.P̃u, Z̃) .+ mpc.Tu_lastu 
+    Ŷ .= Ŷ0 .+ mpc.Yop
     predictstoch!(Ŷs, mpc, mpc.estim)
     info[:ΔU]   = Z̃[1:mpc.Hc*model.nu]
     info[:ϵ]    = mpc.nϵ == 1 ? mpc.Z̃[end] : zero(NT)
diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl
@@ -193,11 +193,14 @@ The solution is ``\mathbf{Z̃ = - H̃^{-1} q̃}``, see [`init_quadprog`](@ref).
 optim_objective!(mpc::ExplicitMPC) = lmul!(-1, ldiv!(mpc.Z̃, mpc.H̃_chol, mpc.q̃))
 
 "Compute the predictions but not the terminal states if `mpc` is an [`ExplicitMPC`](@ref)."
-function predict!(Ŷ, x̂, _ , _ , _ , mpc::ExplicitMPC, ::LinModel, ::TranscriptionMethod, Z̃)
+function predict!(
+    Ŷ0, x̂0next, _ , _ , _ , mpc::ExplicitMPC, ::LinModel, ::TranscriptionMethod, Z̃
+)
     # in-place operations to reduce allocations :
-    Ŷ .= mul!(Ŷ, mpc.Ẽ, Z̃) .+ mpc.F
-    x̂ .= NaN
-    return Ŷ, x̂
+    Ŷ0    .= mul!(Ŷ0, mpc.Ẽ, Z̃) .+ mpc.F
+    x̂0end  = x̂0next
+    x̂0end .= NaN 
+    return Ŷ0, x̂0end
 end
 
 "`ExplicitMPC` does not support custom nonlinear constraint, return `true`."
diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl
@@ -551,7 +551,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
             x̂0, x̂0next = get_tmp(x̂0_cache, Z̃1), get_tmp(x̂0next_cache, Z̃1)
             u0, û0     = get_tmp(u0_cache, Z̃1), get_tmp(û0_cache, Z̃1)
             gc, g      = get_tmp(gc_cache, Z̃1), get_tmp(g_cache, Z̃1)
-            Ŷ0, x̂0end  = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, transcription, Z̃)
+            Ŷ0, x̂0end  = predict!(Ȳ, x̂0next, x̂0, u0, û0, mpc, model, transcription, Z̃)
             ΔŨ, Ŷe, Ue = nonlinprog_vectors!(ΔŨ, Ŷe, Ue, Ū, mpc, Ŷ0, Z̃)
             ϵ = (nϵ ≠ 0) ? Z̃[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation)
             gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ)
diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl
@@ -607,32 +607,32 @@ end
 
 @doc raw"""
     predict!(
-        Ŷ0, x̂0, _, _, _, 
+        Ŷ0, x̂0next, _, _, _, 
         mpc::PredictiveController, model::LinModel, transcription::TranscriptionMethod, 
         Z̃
     ) -> Ŷ0, x̂0end
 
 Compute the predictions `Ŷ0` and terminal states `x̂0end` if model is a [`LinModel`](@ref).
 
-The method mutates `Ŷ0` and `x̂0` vector arguments. The `x̂end` vector is used for
+The method mutates `Ŷ0` and `x̂0next` vector arguments. The `x̂end` vector is used for
 the terminal constraints applied on ``\mathbf{x̂}_{k-1}(k+H_p)``. The computations are
 identical for any [`TranscriptionMethod`](@ref).
 """
 function predict!(
-    Ŷ0, x̂0, _ , _ , _ , 
+    Ŷ0, x̂0next, _ , _ , _ , 
     mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, 
     Z̃
 )
     # in-place operations to reduce allocations :
-    Ŷ0 .= mul!(Ŷ0, mpc.Ẽ, Z̃) .+ mpc.F
-    x̂0 .= mul!(x̂0, mpc.con.ẽx̂, Z̃) .+ mpc.con.fx̂
-    x̂0end = x̂0
+    Ŷ0    .= mul!(Ŷ0, mpc.Ẽ, Z̃) .+ mpc.F
+    x̂0end  = x̂0next
+    x̂0end .= mul!(x̂0end, mpc.con.ẽx̂, Z̃) .+ mpc.con.fx̂
     return Ŷ0, x̂0end
 end
 
 @doc raw"""
     predict!(
-        Ŷ0, x̂0, x̂0next, u0, û0, 
+        Ŷ0, x̂0next, x̂0, u0, û0, 
         mpc::PredictiveController, model::NonLinModel, transcription::SingleShooting,
         Z̃
     ) -> Ŷ0, x̂0end
@@ -642,7 +642,7 @@ Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`SingleShooting`]
 The method mutates `Ŷ0`, `x̂0`, `x̂0next`, `u0` and `û0` arguments.
 """
 function predict!(
-    Ŷ0, x̂0, x̂0next, u0, û0, 
+    Ŷ0, x̂0next, x̂0, u0, û0, 
     mpc::PredictiveController, model::NonLinModel, ::SingleShooting,
     Z̃
 )
@@ -663,37 +663,38 @@ function predict!(
         ŷ0  = @views Ŷ0[(1 + ny*(j-1)):(ny*j)]
         ĥ!(ŷ0, mpc.estim, model, x̂0, d0)
     end
-    Ŷ0 .+= mpc.F # F = Ŷs if mpc.estim is an InternalModel, else F = 0.
-    x̂0end = x̂0
+    Ŷ0    .+= mpc.F # F = Ŷs if mpc.estim is an InternalModel, else F = 0.
+    x0end   = x0next
     return Ŷ0, x̂0end
 end
 
 @doc raw"""
     predict!(
-        Ŷ0, x̂0, _ , _ , _ , 
+        Ŷ0, x̂0next, _ , _ , _ , 
         mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting,
         Z̃
     ) -> Ŷ0, x̂0end
 
 Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`MultipleShooting`](@ref).
     
-The method mutates `Ŷ0` and `x̂0` arguments.
+The method mutates `Ŷ0` and `x̂0next` arguments.
 """
 function predict!(
-    Ŷ0, x̂0, x̂0next, u0, û0,
+    Ŷ0, x̂0next, _, _, _,
     mpc::PredictiveController, model::NonLinModel, ::MultipleShooting,
     Z̃
 )
     nu, ny, nd, nx̂, Hp, Hc = model.nu, model.ny, model.nd, mpc.estim.nx̂, mpc.Hp, mpc.Hc
     X̂0 = @views Z̃[(nu*Hc+1):(nu*Hc+nx̂*Hp)] # Z̃ = [ΔU; X̂0; ϵ]
     D̂0 = mpc.D̂0
     for j=1:Hp
-        x̂0 .= @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)]
-        d0  = @views D̂0[(1 + nd*(j-1)):(nd*j)]
-        ŷ0  = @views Ŷ0[(1 + ny*(j-1)):(ny*j)]
+        x̂0 = @views X̂0[(1 +  nx̂*(j-1)):(nx̂*j)]
+        d0 = @views D̂0[(1 +  nd*(j-1)):(nd*j)]
+        ŷ0 = @views Ŷ0[(1 +  ny*(j-1)):(ny*j)]
         ĥ!(ŷ0, mpc.estim, model, x̂0, d0)
     end
-    Ŷ0 .+= mpc.F # F = Ŷs if mpc.estim is an InternalModel, else F = 0.
-    x̂0end = x̂0
+    Ŷ0    .+= mpc.F # F = Ŷs if mpc.estim is an InternalModel, else F = 0.
+    x̂0end   = x̂0next
+    x̂0end  .= x̂0
     return Ŷ0, x̂0end
 end
diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl
@@ -616,6 +616,7 @@ end
     h = (x,d,_)   -> linmodel2.C*x + linmodel2.Dd*d
     nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing)
     nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=1000, Hc=1)
+    # if d=[0.1], the output will eventually reach 7*0.1=0.7, no action needed (u=0):
     d = [0.1]
     preparestate!(nmpc2, [0], d)
     u = moveinput!(nmpc2, 7d, d)
