Merge pull request #365 from JuliaControl/nonlinmpc_quadobj_linmodel

added: quadratic objective for `NonLinMPC` with `LinModel` and `Ewt=0`

Author: franckgaga

src/controller/execute.jl

@@ -494,10 +494,9 @@ If first warm-starts the solver with [`set_warmstart!`](@ref). It then calls
 """
 function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
     model, optim = mpc.estim.model, mpc.optim
-    nu, Hc = model.nu, mpc.Hc 
     Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var]
     Z̃s = set_warmstart!(mpc, mpc.transcription, Z̃var)
-    set_objective_linear_coef!(mpc, Z̃var)
+    set_objective_linear_coef!(mpc, model, Z̃var)
     try
         JuMP.optimize!(optim)
     catch err
@@ -534,8 +533,14 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
     return mpc.Z̃
 end
 
-"By default, no need to modify the objective function."
-set_objective_linear_coef!(::PredictiveController, _ ) = nothing
+"By default, no need to update the objective function."
+set_objective_linear_coef!(::PredictiveController, ::SimModel, _ ) = nothing
+
+"Update the linear coefficients of the quadratic objective with `mpc.q̃` if applicable."
+function set_objective_linear_coef!(mpc::PredictiveController, ::LinModel, Z̃var)
+    mpc.weights.iszero_E && JuMP.set_objective_coefficient(mpc.optim, Z̃var, mpc.q̃)
+    return nothing
+end
 
 """
     preparestate!(mpc::PredictiveController, ym, d=[]) -> x̂
@@ -794,9 +799,15 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
     JuMP.delete(optim, optim[:linconstrainteq])
     JuMP.unregister(optim, :linconstrainteq)
     @constraint(optim, linconstrainteq, con.Aeq*Z̃var .== con.beq)
-    set_objective_hessian!(mpc, Z̃var)
+    set_objective_hessian!(mpc, model, Z̃var)
     return nothing
 end
 
-"No need to set the objective Hessian by default (only needed for quadratic optimization)."
-set_objective_hessian!(::PredictiveController, _ ) = nothing
+"No need to set the objective Hessian by default (only needed for quadratic objective)."
+set_objective_hessian!(::PredictiveController, ::SimModel, _ ) = nothing
+
+"Set the objective Hessian with `mpc.H̃` if the objective is quadratic."
+function set_objective_hessian!(mpc::PredictiveController, ::LinModel, Z̃var)
+    mpc.weights.iszero_E && @objective(mpc.optim, Min, obj_quadprog(Z̃var, mpc.H̃, mpc.q̃))
+    return nothing
+end

src/controller/linmpc.jl

@@ -329,18 +329,6 @@ function init_optimization!(mpc::LinMPC, model::LinModel, optim::JuMP.GenericMod
     Aeq = con.Aeq
     beq = con.beq
     @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq)
-    set_objective_hessian!(mpc, Z̃var)
-    return nothing
-end
-
-"For [`LinMPC`](@ref), set the QP linear coefficient `q̃` just before optimization."
-function set_objective_linear_coef!(mpc::LinMPC, Z̃var)
-    JuMP.set_objective_coefficient(mpc.optim, Z̃var, mpc.q̃)
-    return nothing
-end
-
-"Update the quadratic objective function for [`LinMPC`](@ref) controllers."
-function set_objective_hessian!(mpc::LinMPC, Z̃var)
-    @objective(mpc.optim, Min, obj_quadprog(Z̃var, mpc.H̃, mpc.q̃))
+    set_objective_hessian!(mpc, model, Z̃var)
     return nothing
 end

src/controller/nonlinmpc.jl

@@ -733,9 +733,7 @@ end
 
 Init the nonlinear optimization for [`NonLinMPC`](@ref) controllers.
 """
-function init_optimization!(
-    mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel{JNT}
-)  where JNT<:Real
+function init_optimization!(mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel)  
     # --- variables and linear constraints ---
     con = mpc.con
     nZ̃ = length(mpc.Z̃)
@@ -759,6 +757,36 @@ function init_optimization!(
     return nothing
 end
 
+function init_optimization!(mpc::NonLinMPC, model::LinModel, optim::JuMP.GenericModel) 
+    # --- variables and linear constraints ---
+    con = mpc.con
+    nZ̃ = length(mpc.Z̃)
+    JuMP.num_variables(optim) == 0 || JuMP.empty!(optim)
+    JuMP.set_silent(optim)
+    limit_solve_time(mpc.optim, model.Ts)
+    @variable(optim, Z̃var[1:nZ̃])
+    A = con.A[con.i_b, :]
+    b = con.b[con.i_b]
+    @constraint(optim, linconstraint, A*Z̃var .≤ b)
+    Aeq = con.Aeq
+    beq = con.beq
+    @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq)
+    C = mpc.nϵ > 0 ? mpc.weights.Ñ_Hc[end, end] : Inf
+    set_scaling_gradient!(optim, C)
+    if mpc.weights.iszero_E
+        set_objective_hessian!(mpc, model, Z̃var)
+    else
+        # --- nonlinear optimization init for the custom NL objective ---
+        Jop = get_nonlinobj_op(mpc, optim)
+        @objective(optim, Min, Jop(Z̃var...))
+    end
+    if con.nc > 0
+        g_oracle, geq_oracle = get_nonlincon_oracle(mpc, optim)
+        set_nonlincon!(mpc, optim, g_oracle, geq_oracle)
+    end
+    return nothing
+end
+
 """
     reset_nonlincon!(mpc::NonLinMPC)
 

src/estimator/mhe/construct.jl

@@ -1499,6 +1499,7 @@ function init_optimization!(
 )
     C, con = estim.C, estim.con
     nZ̃ = length(estim.Z̃)
+    # --- variables and linear constraints ---
     JuMP.num_variables(optim) == 0 || JuMP.empty!(optim)
     JuMP.set_silent(optim)
     limit_solve_time(optim, model.Ts)
@@ -1508,7 +1509,9 @@ function init_optimization!(
     @constraint(optim, linconstraint, A*Z̃var .≤ b)
     @objective(optim, Min, obj_quadprog(Z̃var, estim.H̃, estim.q̃))
     if con.nc > 0
+        # --- nonlinear optimization init for the custom NL constraints ---
         set_scaling_gradient!(optim, C)
+        # constraints with vector nonlinear oracle 
         g_oracle = get_nonlincon_oracle(estim, optim)  
         set_nonlincon!(estim, optim, g_oracle)
     end