diff --git a/benchmark/2_bench_state_estim.jl b/benchmark/2_bench_state_estim.jl index a05afe808..25dc87042 100644 --- a/benchmark/2_bench_state_estim.jl +++ b/benchmark/2_bench_state_estim.jl @@ -335,21 +335,21 @@ JuMP.set_attribute(mhe_pendulum_madnlp_pred.optim, "tol", 1e-7) samples, evals, seconds = 25, 1, 15*60 CASE_ESTIM["Pendulum"]["MovingHorizonEstimator"]["Ipopt"]["Current form"] = @benchmarkable( - sim!($mhe_pendulum_ipopt_curr, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + sim!($mhe_pendulum_ipopt_curr, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_ESTIM["Pendulum"]["MovingHorizonEstimator"]["Ipopt"]["Prediction form"] = @benchmarkable( - sim!($mhe_pendulum_ipopt_pred, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + sim!($mhe_pendulum_ipopt_pred, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_ESTIM["Pendulum"]["MovingHorizonEstimator"]["MadNLP"]["Current form"] = @benchmarkable( - sim!($mhe_pendulum_madnlp_curr, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + sim!($mhe_pendulum_madnlp_curr, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_ESTIM["Pendulum"]["MovingHorizonEstimator"]["MadNLP"]["Prediction form"] = @benchmarkable( - sim!($mhe_pendulum_madnlp_pred, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + sim!($mhe_pendulum_madnlp_pred, $N, $u; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) \ No newline at end of file diff --git a/benchmark/3_bench_predictive_control.jl b/benchmark/3_bench_predictive_control.jl index 8a624a654..427f2e7e6 100644 --- a/benchmark/3_bench_predictive_control.jl +++ b/benchmark/3_bench_predictive_control.jl @@ -46,10 +46,18 @@ nmpc_nonlin_ss = NonLinMPC( nonlinmodel, transcription=SingleShooting(), Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 ) +nmpc_nonlin_ss_hess = NonLinMPC( + nonlinmodel, transcription=SingleShooting(), hessian=true, + Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 +) nmpc_nonlin_ms = NonLinMPC( nonlinmodel, transcription=MultipleShooting(), Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 ) +nmpc_nonlin_ms_hess = NonLinMPC( + nonlinmodel, transcription=MultipleShooting(), hessian=true, + Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 +) nmpc_nonlin_tc = NonLinMPC( nonlinmodel_c, transcription=TrapezoidalCollocation(), Mwt=[1], Nwt=[0.1], Lwt=[0.1], Hp=10 @@ -74,12 +82,24 @@ UNIT_MPC["NonLinMPC"]["moveinput!"]["NonLinModel"]["SingleShooting"] = setup=preparestate!($nmpc_nonlin_ss, $y, $d), samples=samples, evals=evals, seconds=seconds ) +UNIT_MPC["NonLinMPC"]["moveinput!"]["NonLinModel"]["SingleShootingHessian"] = + @benchmarkable( + moveinput!($nmpc_nonlin_ss, $y, $d), + setup=preparestate!($nmpc_nonlin_ss_hess, $y, $d), + samples=samples, evals=evals, seconds=seconds + ) UNIT_MPC["NonLinMPC"]["moveinput!"]["NonLinModel"]["MultipleShooting"] = @benchmarkable( moveinput!($nmpc_nonlin_ms, $y, $d), setup=preparestate!($nmpc_nonlin_ms, $y, $d), samples=samples, evals=evals, seconds=seconds ) +UNIT_MPC["NonLinMPC"]["moveinput!"]["NonLinModel"]["MultipleShootingHessian"] = + @benchmarkable( + moveinput!($nmpc_nonlin_ms, $y, $d), + setup=preparestate!($nmpc_nonlin_ms_hess, $y, $d), + samples=samples, evals=evals, seconds=seconds + ) UNIT_MPC["NonLinMPC"]["moveinput!"]["NonLinModel"]["TrapezoidalCollocation"] = @benchmarkable( moveinput!($nmpc_nonlin_tc, $y_c, $d_c), @@ -258,12 +278,24 @@ nmpc_ipopt_ss = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) nmpc_ipopt_ss = setconstraint!(nmpc_ipopt_ss; umin, umax) JuMP.unset_time_limit_sec(nmpc_ipopt_ss.optim) +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription, hessian = SingleShooting(), true +nmpc_ipopt_ss_hess = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription, hessian) +nmpc_ipopt_ss_hess = setconstraint!(nmpc_ipopt_ss_hess; umin, umax) +JuMP.unset_time_limit_sec(nmpc_ipopt_ss_hess.optim) + optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) transcription = MultipleShooting() nmpc_ipopt_ms = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) nmpc_ipopt_ms = setconstraint!(nmpc_ipopt_ms; umin, umax) JuMP.unset_time_limit_sec(nmpc_ipopt_ms.optim) +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription, hessian = MultipleShooting(), true +nmpc_ipopt_ms_hess = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription, hessian) +nmpc_ipopt_ms_hess = setconstraint!(nmpc_ipopt_ms_hess; umin, umax) +JuMP.unset_time_limit_sec(nmpc_ipopt_ms_hess.optim) + optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) transcription = MultipleShooting(f_threads=true) nmpc_ipopt_mst = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) @@ -305,32 +337,42 @@ JuMP.unset_time_limit_sec(nmpc_madnlp_ss.optim) samples, evals, seconds = 100, 1, 15*60 CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["SingleShooting"] = @benchmarkable( - sim!($nmpc_ipopt_ss, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + sim!($nmpc_ipopt_ss, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), + samples=samples, evals=evals, seconds=seconds + ) +CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["SingleShooting (Hessian)"] = + @benchmarkable( + sim!($nmpc_ipopt_ss_hess, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["MultipleShooting"] = @benchmarkable( - sim!($nmpc_ipopt_ms, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + sim!($nmpc_ipopt_ms, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), + samples=samples, evals=evals, seconds=seconds + ) +CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["MultipleShooting (Hessian)"] = + @benchmarkable( + sim!($nmpc_ipopt_ms_hess, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["MultipleShooting (threaded)"] = @benchmarkable( - sim!($nmpc_ipopt_mst, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + sim!($nmpc_ipopt_mst, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["TrapezoidalCollocation"] = @benchmarkable( - sim!($nmpc_ipopt_tc, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + sim!($nmpc_ipopt_tc, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["TrapezoidalCollocation (threaded)"] = @benchmarkable( - sim!($nmpc_ipopt_tct, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + sim!($nmpc_ipopt_tct, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["MadNLP"]["SingleShooting"] = @benchmarkable( - sim!($nmpc_madnlp_ss, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + sim!($nmpc_madnlp_ss, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) @@ -376,22 +418,22 @@ JuMP.unset_time_limit_sec(empc_madnlp_ss.optim) samples, evals, seconds = 100, 1, 15*60 CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["Ipopt"]["SingleShooting"] = @benchmarkable( - sim!($empc_ipopt_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + sim!($empc_ipopt_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["Ipopt"]["MultipleShooting"] = @benchmarkable( - sim!($empc_ipopt_ms, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + sim!($empc_ipopt_ms, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["Ipopt"]["TrapezoidalCollocation"] = @benchmarkable( - sim!($empc_ipopt_tc, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + sim!($empc_ipopt_tc, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["MadNLP"]["SingleShooting"] = @benchmarkable( - sim!($empc_madnlp_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + sim!($empc_madnlp_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) @@ -442,17 +484,17 @@ JuMP.unset_time_limit_sec(nmpc2_ipopt_tc.optim) samples, evals, seconds = 100, 1, 15*60 CASE_MPC["Pendulum"]["NonLinMPC"]["Custom constraints"]["Ipopt"]["SingleShooting"] = @benchmarkable( - sim!($nmpc2_ipopt_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + sim!($nmpc2_ipopt_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_MPC["Pendulum"]["NonLinMPC"]["Custom constraints"]["Ipopt"]["MultipleShooting"] = @benchmarkable( - sim!($nmpc2_ipopt_ms, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + sim!($nmpc2_ipopt_ms, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) CASE_MPC["Pendulum"]["NonLinMPC"]["Custom constraints"]["Ipopt"]["TrapezoidalCollocation"] = @benchmarkable( - sim!($nmpc2_ipopt_tc, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + sim!($nmpc2_ipopt_tc, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0, progress=false), samples=samples, evals=evals, seconds=seconds ) diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index b57b4c0e9..4e8e74d17 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -24,7 +24,8 @@ ModelPredictiveControl.relaxterminal ModelPredictiveControl.init_quadprog ModelPredictiveControl.init_stochpred ModelPredictiveControl.init_matconstraint_mpc -ModelPredictiveControl.get_nonlinops(::NonLinMPC, ::ModelPredictiveControl.GenericModel) +ModelPredictiveControl.get_nonlinobj_op(::NonLinMPC, ::ModelPredictiveControl.GenericModel) +ModelPredictiveControl.get_nonlincon_oracle(::NonLinMPC, ::ModelPredictiveControl.GenericModel) ``` ## Update Quadratic Optimization diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index ec246c121..423475c5f 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -6,9 +6,11 @@ using Random: randn using RecipesBase -using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff, AutoSparse -using DifferentiationInterface: gradient!, jacobian!, prepare_gradient, prepare_jacobian -using DifferentiationInterface: value_and_gradient!, value_and_jacobian! +using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff +using DifferentiationInterface: AutoSparse, SecondOrder +using DifferentiationInterface: gradient!, value_and_gradient!, prepare_gradient +using DifferentiationInterface: jacobian!, value_and_jacobian!, prepare_jacobian +using DifferentiationInterface: hessian!, value_gradient_and_hessian!, prepare_hessian using DifferentiationInterface: Constant, Cache using SparseConnectivityTracer: TracerSparsityDetector using SparseMatrixColorings: GreedyColoringAlgorithm, sparsity_pattern diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 66473eebb..cabafde4c 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -7,6 +7,7 @@ const DEFAULT_NONLINMPC_JACSPARSE = AutoSparse( sparsity_detector=TracerSparsityDetector(), coloring_algorithm=GreedyColoringAlgorithm(), ) +const DEFAULT_NONLINMPC_HESSIAN = DEFAULT_NONLINMPC_JACSPARSE struct NonLinMPC{ NT<:Real, @@ -15,7 +16,8 @@ struct NonLinMPC{ TM<:TranscriptionMethod, JM<:JuMP.GenericModel, GB<:AbstractADType, - JB<:AbstractADType, + JB<:AbstractADType, + HB<:Union{AbstractADType, Nothing}, PT<:Any, JEfunc<:Function, GCfunc<:Function @@ -28,6 +30,7 @@ struct NonLinMPC{ con::ControllerConstraint{NT, GCfunc} gradient::GB jacobian::JB + hessian::HB oracle::Bool Z̃::Vector{NT} ŷ::Vector{NT} @@ -68,7 +71,7 @@ struct NonLinMPC{ estim::SE, Hp, Hc, nb, weights::CW, JE::JEfunc, gc!::GCfunc, nc, p::PT, transcription::TM, optim::JM, - gradient::GB, jacobian::JB, oracle + gradient::GB, jacobian::JB, hessian::HB, oracle ) where { NT<:Real, SE<:StateEstimator, @@ -77,6 +80,7 @@ struct NonLinMPC{ JM<:JuMP.GenericModel, GB<:AbstractADType, JB<:AbstractADType, + HB<:Union{AbstractADType, Nothing}, PT<:Any, JEfunc<:Function, GCfunc<:Function, @@ -116,9 +120,9 @@ struct NonLinMPC{ nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ Z̃ = zeros(NT, nZ̃) buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) - mpc = new{NT, SE, CW, TM, JM, GB, JB, PT, JEfunc, GCfunc}( + mpc = new{NT, SE, CW, TM, JM, GB, JB, HB, PT, JEfunc, GCfunc}( estim, transcription, optim, con, - gradient, jacobian, oracle, + gradient, jacobian, hessian, oracle, Z̃, ŷ, Hp, Hc, nϵ, nb, weights, @@ -218,6 +222,9 @@ This controller allocates memory at each time step for the optimization. function, see [`DifferentiationInterface` doc](@extref DifferentiationInterface List). - `jacobian=default_jacobian(transcription)` : an `AbstractADType` backend for the Jacobian of the nonlinear constraints, see `gradient` above for the options (default in Extended Help). +- `hessian=false` : an `AbstractADType` backend for the Hessian of the Lagrangian, see + `gradient` above for the options. The default `false` skip it and use the quasi-Newton + method of `optim`, which is always the case if `oracle=false` (see Extended Help). - `oracle=JuMP.solver_name(optim)=="Ipopt"`: use the efficient [`VectorNonlinearOracle`](@extref MathOptInterface MathOptInterface.VectorNonlinearOracle) for the nonlinear constraints (not supported by most optimizers for now). - additional keyword arguments are passed to [`UnscentedKalmanFilter`](@ref) constructor @@ -235,6 +242,7 @@ NonLinMPC controller with a sample time Ts = 10.0 s: ├ transcription: MultipleShooting ├ gradient: AutoForwardDiff ├ jacobian: AutoSparse (AutoForwardDiff, TracerSparsityDetector, GreedyColoringAlgorithm) +├ hessian: nothing └ dimensions: ├ 20 prediction steps Hp ├ 10 control steps Hc @@ -276,9 +284,9 @@ NonLinMPC controller with a sample time Ts = 10.0 s: the `gc` argument (both `gc` and `gc!` accepts non-mutating and mutating functions). By default, the optimization relies on dense [`ForwardDiff`](@extref ForwardDiff) - automatic differentiation (AD) to compute the objective and constraint derivatives. One - exception: if `transcription` is not a [`SingleShooting`](@ref), the `jacobian` argument - defaults to this [sparse backend](@extref DifferentiationInterface AutoSparse-object): + automatic differentiation (AD) to compute the objective and constraint derivatives. Two + exceptions: if `transcription` is not a [`SingleShooting`](@ref), the `jacobian` + argument defaults to this [sparse backend](@extref DifferentiationInterface AutoSparse-object): ```julia AutoSparse( AutoForwardDiff(); @@ -286,6 +294,10 @@ NonLinMPC controller with a sample time Ts = 10.0 s: coloring_algorithm = GreedyColoringAlgorithm() ) ``` + This is also the sparse backend selected for the Hessian of the Lagrangian function if + `oracle=true` and `hessian=true`, which is the second exception. Second order + derivatives are only supported with `oracle=true` option. + Optimizers generally benefit from exact derivatives like AD. However, the [`NonLinModel`](@ref) state-space functions must be compatible with this feature. See [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) for common mistakes when writing these functions. @@ -314,6 +326,7 @@ function NonLinMPC( optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), gradient::AbstractADType = DEFAULT_NONLINMPC_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), + hessian::Union{AbstractADType, Bool, Nothing} = false, oracle::Bool = JuMP.solver_name(optim)=="Ipopt", kwargs... ) @@ -321,7 +334,7 @@ function NonLinMPC( return NonLinMPC( estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, - transcription, optim, gradient, jacobian, oracle + transcription, optim, gradient, jacobian, hessian, oracle ) end @@ -343,10 +356,11 @@ julia> mpc = NonLinMPC(estim, Hp=20, Cwt=1e6) NonLinMPC controller with a sample time Ts = 10.0 s: ├ estimator: UnscentedKalmanFilter ├ model: NonLinModel -├ optimizer: Ipopt +├ optimizer: Ipopt ├ transcription: SingleShooting ├ gradient: AutoForwardDiff ├ jacobian: AutoForwardDiff +├ hessian: nothing └ dimensions: ├ 20 prediction steps Hp ├ 2 control steps Hc @@ -379,6 +393,7 @@ function NonLinMPC( optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), gradient::AbstractADType = DEFAULT_NONLINMPC_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), + hessian::Union{AbstractADType, Bool, Nothing} = false, oracle::Bool = JuMP.solver_name(optim)=="Ipopt" ) where { NT<:Real, @@ -394,9 +409,10 @@ function NonLinMPC( validate_JE(NT, JE) gc! = get_mutating_gc(NT, gc) weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) + hessian = validate_hessian(hessian, gradient, oracle) return NonLinMPC{NT}( estim, Hp, Hc, nb, weights, JE, gc!, nc, p, - transcription, optim, gradient, jacobian, oracle + transcription, optim, gradient, jacobian, hessian, oracle ) end @@ -496,6 +512,34 @@ function test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, return nothing end +""" + validate_hessian(hessian, gradient, oracle) -> backend + +Validate `hessian` argument and return the differentiation backend. +""" +function validate_hessian(hessian, gradient, oracle) + if hessian == true + backend = DEFAULT_NONLINMPC_HESSIAN + elseif hessian == false || isnothing(hessian) + backend = nothing + else + backend = hessian + end + if oracle == false && !isnothing(backend) + error("Second order derivatives are only supported with oracle=true.") + end + if oracle == true && !isnothing(backend) + hess = dense_backend(backend) + grad = dense_backend(gradient) + if hess != grad + @info "The objective function gradient will be computed with the hessian "* + "backend ($(backend_str(hess)))\n instead of the one in gradient "* + "argument ($(backend_str(grad))) for efficiency." + end + end + return backend +end + """ addinfo!(info, mpc::NonLinMPC) -> info @@ -547,8 +591,9 @@ function init_optimization!( end end if mpc.oracle - g_oracle, geq_oracle, J_op = get_nonlinops(mpc, optim) - optim[:J_op] = J_op + J_op = get_nonlinobj_op(mpc, optim) + g_oracle, geq_oracle = get_nonlincon_oracle(mpc, optim) + else J_func, ∇J_func!, g_funcs, ∇g_funcs!, geq_funcs, ∇geq_funcs! = get_optim_functions( mpc, optim @@ -574,7 +619,7 @@ Re-construct nonlinear constraints and add them to `mpc.optim`. """ function reset_nonlincon!(mpc::NonLinMPC) if mpc.oracle - g_oracle, geq_oracle = get_nonlinops(mpc, mpc.optim) + g_oracle, geq_oracle = get_nonlincon_oracle(mpc, mpc.optim) set_nonlincon!(mpc, mpc.optim, g_oracle, geq_oracle) else set_nonlincon_leg!(mpc, mpc.estim.model, mpc.transcription, mpc.optim) @@ -582,30 +627,22 @@ function reset_nonlincon!(mpc::NonLinMPC) end """ - get_nonlinops(mpc::NonLinMPC, optim) -> g_oracle, geq_oracle, J_op + get_nonlincon_oracle(mpc::NonLinMPC, optim) -> g_oracle, geq_oracle -Return the operators for the nonlinear optimization of `mpc` [`NonLinMPC`](@ref) controller. +Return the nonlinear constraint oracles for [`NonLinMPC`](@ref) `mpc`. Return `g_oracle` and `geq_oracle`, the inequality and equality [`VectorNonlinearOracle`](@extref MathOptInterface MathOptInterface.VectorNonlinearOracle) for the two respective constraints. Note that `g_oracle` only includes the non-`Inf` -inequality constraints, thus it must be re-constructed if they change. Also return `J_op`, -the [`NonlinearOperator`](@extref JuMP NonlinearOperator) for the objective function, based -on the splatting syntax. This method is really intricate and that's because of 3 elements: - -- These functions are used inside the nonlinear optimization, so they must be type-stable - and as efficient as possible. All the function outputs and derivatives are cached and - updated in-place if required to use the efficient [`value_and_jacobian!`](@extref DifferentiationInterface DifferentiationInterface.value_and_jacobian!). -- The splatting syntax for objective functions implies the use of `Vararg{T,N}` (see the [performance tip](@extref Julia Be-aware-of-when-Julia-avoids-specializing)) - and memoization to avoid redundant computations. This is already complex, but it's even - worse knowing that the automatic differentiation tools do not support splatting. -- The signature of gradient and hessian functions is not the same for univariate (`nZ̃ == 1`) - and multivariate (`nZ̃ > 1`) operators in `JuMP`. Both must be defined. +inequality constraints, thus it must be re-constructed if they change. This method is really +intricate because the oracles are used inside the nonlinear optimization, so they must be +type-stable and as efficient as possible. All the function outputs and derivatives are +ached and updated in-place if required to use the efficient [`value_and_jacobian!`](@extref DifferentiationInterface DifferentiationInterface.value_and_jacobian!). """ -function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT<:Real +function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real # ----------- common cache for all functions ---------------------------------------- model = mpc.estim.model transcription = mpc.transcription - grad, jac = mpc.gradient, mpc.jacobian + jac, hess = mpc.jacobian, mpc.hessian nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ nk = get_nk(model, transcription) Hp, Hc = mpc.Hp, mpc.Hc @@ -616,7 +653,6 @@ function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT< nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny strict = Val(true) myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) - J::Vector{JNT} = zeros(JNT, 1) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) x̂0end::Vector{JNT} = zeros(JNT, nx̂) K0::Vector{JNT} = zeros(JNT, nK) @@ -625,20 +661,39 @@ function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT< Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nX̂) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) gi::Vector{JNT}, geq::Vector{JNT} = zeros(JNT, ngi), zeros(JNT, neq) + λi::Vector{JNT}, λeq::Vector{JNT} = ones(JNT, ngi), ones(JNT, neq) # -------------- inequality constraint: nonlinear oracle ----------------------------- function gi!(gi, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) gi .= @views g[i_g] return nothing end - Z̃_∇gi = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call + function ℓ_gi(Z̃, λi, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g, gi) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + gi .= @views g[i_g] + return dot(λi, gi) + end + Z̃_∇gi = fill(myNaN, nZ̃) # NaN to force update at first call ∇gi_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) - ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_context...; strict) - ∇gi = init_diffmat(JNT, jac, ∇gi_prep, nZ̃, ngi) + ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_context...; strict) + ∇gi = init_diffmat(JNT, jac, ∇gi_prep, nZ̃, ngi) + ∇gi_structure = init_diffstructure(∇gi) + if !isnothing(hess) + ∇²gi_context = ( + Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), + Cache(Û0), Cache(K0), Cache(X̂0), + Cache(gc), Cache(geq), Cache(g), Cache(gi) + ) + ∇²gi_prep = prepare_hessian( + ℓ_gi, hess, Z̃_∇gi, Constant(λi), ∇²gi_context...; strict + ) + ∇²ℓ_gi = init_diffmat(JNT, hess, ∇²gi_prep, nZ̃, nZ̃) + ∇²gi_structure = lowertriangle_indices(init_diffstructure(∇²ℓ_gi)) + end function update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg) if isdifferent(Z̃_arg, Z̃_∇gi) Z̃_∇gi .= Z̃_arg @@ -651,33 +706,57 @@ function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT< return gi_arg .= gi end function ∇gi_func!(∇gi_arg, Z̃_arg) - update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg) - return diffmat2vec!(∇gi_arg, ∇gi) + update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg) + return diffmat2vec!(∇gi_arg, ∇gi, ∇gi_structure) + end + function ∇²gi_func!(∇²ℓ_arg, Z̃_arg, λ_arg) + Z̃_∇gi .= Z̃_arg + λi .= λ_arg + hessian!(ℓ_gi, ∇²ℓ_gi, ∇²gi_prep, hess, Z̃_∇gi, Constant(λi), ∇²gi_context...) + return diffmat2vec!(∇²ℓ_arg, ∇²ℓ_gi, ∇²gi_structure) end gi_min = fill(-myInf, ngi) gi_max = zeros(JNT, ngi) - ∇gi_structure = init_diffstructure(∇gi) g_oracle = MOI.VectorNonlinearOracle(; dimension = nZ̃, l = gi_min, u = gi_max, eval_f = gi_func!, jacobian_structure = ∇gi_structure, - eval_jacobian = ∇gi_func! + eval_jacobian = ∇gi_func!, + hessian_lagrangian_structure = isnothing(hess) ? Tuple{Int,Int}[] : ∇²gi_structure, + eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²gi_func! ) # ------------- equality constraints : nonlinear oracle ------------------------------ function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) return nothing end - Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call + function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + return dot(λeq, geq) + end + Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update at first call ∇geq_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), Cache(gc), Cache(g) ) ∇geq_prep = prepare_jacobian(geq!, geq, jac, Z̃_∇geq, ∇geq_context...; strict) - ∇geq = init_diffmat(JNT, jac, ∇geq_prep, nZ̃, neq) + ∇geq = init_diffmat(JNT, jac, ∇geq_prep, nZ̃, neq) + ∇geq_structure = init_diffstructure(∇geq) + if !isnothing(hess) + ∇²geq_context = ( + Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), + Cache(Û0), Cache(K0), Cache(X̂0), + Cache(gc), Cache(geq), Cache(g) + ) + ∇²geq_prep = prepare_hessian( + ℓ_geq, hess, Z̃_∇geq, Constant(λeq), ∇²geq_context...; strict + ) + ∇²ℓ_geq = init_diffmat(JNT, hess, ∇²geq_prep, nZ̃, nZ̃) + ∇²geq_structure = lowertriangle_indices(init_diffstructure(∇²ℓ_geq)) + end function update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg) if isdifferent(Z̃_arg, Z̃_∇geq) Z̃_∇geq .= Z̃_arg @@ -691,54 +770,150 @@ function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT< end function ∇geq_func!(∇geq_arg, Z̃_arg) update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃_arg) - return diffmat2vec!(∇geq_arg, ∇geq) + return diffmat2vec!(∇geq_arg, ∇geq, ∇geq_structure) + end + function ∇²geq_func!(∇²ℓ_arg, Z̃_arg, λ_arg) + Z̃_∇geq .= Z̃_arg + λeq .= λ_arg + hessian!(ℓ_geq, ∇²ℓ_geq, ∇²geq_prep, hess, Z̃_∇geq, Constant(λeq), ∇²geq_context...) + return diffmat2vec!(∇²ℓ_arg, ∇²ℓ_geq, ∇²geq_structure) end geq_min = geq_max = zeros(JNT, neq) - ∇geq_structure = init_diffstructure(∇geq) geq_oracle = MOI.VectorNonlinearOracle(; dimension = nZ̃, l = geq_min, u = geq_max, eval_f = geq_func!, jacobian_structure = ∇geq_structure, - eval_jacobian = ∇geq_func! + eval_jacobian = ∇geq_func!, + hessian_lagrangian_structure = isnothing(hess) ? Tuple{Int,Int}[] : ∇²geq_structure, + eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²geq_func! ) - # ------------- objective function: splatting syntax --------------------------------- + return g_oracle, geq_oracle +end + +""" + get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) -> J_op + +Return the nonlinear operator for the objective function of `mpc` [`NonLinMPC`](@ref). + +It is based on the splatting syntax. This method is really intricate and that's because of: + +- These functions are used inside the nonlinear optimization, so they must be type-stable + and as efficient as possible. All the function outputs and derivatives are cached and + updated in-place if required to use the efficient [`value_and_gradient!`](@extref DifferentiationInterface DifferentiationInterface.value_and_jacobian!). +- The splatting syntax for objective functions implies the use of `Vararg{T,N}` (see the [performance tip](@extref Julia Be-aware-of-when-Julia-avoids-specializing)) + and memoization to avoid redundant computations. This is already complex, but it's even + worse knowing that the automatic differentiation tools do not support splatting. +- The signature of gradient and hessian functions is not the same for univariate (`nZ̃ == 1`) + and multivariate (`nZ̃ > 1`) operators in `JuMP`. Both must be defined. +""" +function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT<:Real + model = mpc.estim.model + transcription = mpc.transcription + grad, hess = mpc.gradient, mpc.hessian + nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ + nk = get_nk(model, transcription) + Hp, Hc = mpc.Hp, mpc.Hc + ng = length(mpc.con.i_g) + nc, neq = mpc.con.nc, mpc.con.neq + nZ̃, nU, nŶ, nX̂, nK = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk + nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny + strict = Val(true) + myNaN = convert(JNT, NaN) + J::Vector{JNT} = zeros(JNT, 1) + ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) + x̂0end::Vector{JNT} = zeros(JNT, nx̂) + K0::Vector{JNT} = zeros(JNT, nK) + Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) + U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) + Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nX̂) + gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) + geq::Vector{JNT} = zeros(JNT, neq) function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq) update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end - Z̃_∇J = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call - ∇J_context = ( + Z̃_J = fill(myNaN, nZ̃) # NaN to force update at first call + J_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), ) - ∇J_prep = prepare_gradient(J!, grad, Z̃_∇J, ∇J_context...; strict) - ∇J = Vector{JNT}(undef, nZ̃) - function update_objective!(J, ∇J, Z̃_∇J, Z̃_arg) - if isdifferent(Z̃_arg, Z̃_∇J) - Z̃_∇J .= Z̃_arg - J[], _ = value_and_gradient!(J!, ∇J, ∇J_prep, grad, Z̃_∇J, ∇J_context...) + ∇J_prep = prepare_gradient(J!, grad, Z̃_J, J_context...; strict) + ∇J = Vector{JNT}(undef, nZ̃) + if !isnothing(hess) + ∇²J_prep = prepare_hessian(J!, hess, Z̃_J, J_context...; strict) + ∇²J = init_diffmat(JNT, hess, ∇²J_prep, nZ̃, nZ̃) + end + update_objective! = if !isnothing(hess) + function (J, ∇J, ∇²J, Z̃_J, Z̃_arg) + if isdifferent(Z̃_arg, Z̃_J) + Z̃_J .= Z̃_arg + J[], _ = value_gradient_and_hessian!(J!, ∇J, ∇²J, hess, Z̃_J, J_context...) + end + end + else + update_objective! = function (J, ∇J, Z̃_∇J, Z̃_arg) + if isdifferent(Z̃_arg, Z̃_∇J) + Z̃_∇J .= Z̃_arg + J[], _ = value_and_gradient!(J!, ∇J, ∇J_prep, grad, Z̃_∇J, J_context...) + end + end + end + J_func = if !isnothing(hess) + function (Z̃_arg::Vararg{T, N}) where {N, T<:Real} + update_objective!(J, ∇J, ∇²J, Z̃_J, Z̃_arg) + return J[]::T + end + else + function (Z̃_arg::Vararg{T, N}) where {N, T<:Real} + update_objective!(J, ∇J, Z̃_J, Z̃_arg) + return J[]::T end - end - function J_func(Z̃_arg::Vararg{T, N}) where {N, T<:Real} - update_objective!(J, ∇J, Z̃_∇J, Z̃_arg) - return J[]::T end ∇J_func! = if nZ̃ == 1 # univariate syntax (see JuMP.@operator doc): + if !isnothing(hess) + function (Z̃_arg) + update_objective!(J, ∇J, ∇²J, Z̃_J, Z̃_arg) + return ∇J[] + end + else + function (Z̃_arg) + update_objective!(J, ∇J, Z̃_J, Z̃_arg) + return ∇J[] + end + end + else # multivariate syntax (see JuMP.@operator doc): + if !isnothing(hess) + function (∇J_arg::AbstractVector{T}, Z̃_arg::Vararg{T, N}) where {N, T<:Real} + update_objective!(J, ∇J, ∇²J, Z̃_J, Z̃_arg) + return ∇J_arg .= ∇J + end + else + function (∇J_arg::AbstractVector{T}, Z̃_arg::Vararg{T, N}) where {N, T<:Real} + update_objective!(J, ∇J, Z̃_J, Z̃_arg) + return ∇J_arg .= ∇J + end + end + end + ∇²J_func! = if nZ̃ == 1 # univariate syntax (see JuMP.@operator doc): function (Z̃_arg) - update_objective!(J, ∇J, Z̃_∇J, Z̃_arg) - return ∇J[] + update_objective!(J, ∇J, ∇²J, Z̃_J, Z̃_arg) + return ∇²J[] end else # multivariate syntax (see JuMP.@operator doc): - function (∇J_arg::AbstractVector{T}, Z̃_arg::Vararg{T, N}) where {N, T<:Real} - update_objective!(J, ∇J, Z̃_∇J, Z̃_arg) - return ∇J_arg .= ∇J + function (∇²J_arg::AbstractMatrix{T}, Z̃_arg::Vararg{T, N}) where {N, T<:Real} + update_objective!(J, ∇J, ∇²J, Z̃_J, Z̃_arg) + return fill_lowertriangle!(∇²J_arg, ∇²J) end end - J_op = JuMP.add_nonlinear_operator(optim, nZ̃, J_func, ∇J_func!, name=:J_op) - return g_oracle, geq_oracle, J_op + if !isnothing(hess) + @operator(optim, J_op, nZ̃, J_func, ∇J_func!, ∇²J_func!) + else + @operator(optim, J_op, nZ̃, J_func, ∇J_func!) + end + return J_op end """ @@ -808,4 +983,5 @@ end function print_backends(io::IO, mpc::NonLinMPC) println(io, "├ gradient: $(backend_str(mpc.gradient))") println(io, "├ jacobian: $(backend_str(mpc.jacobian))") + println(io, "├ hessian: $(backend_str(mpc.hessian))") end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index e8f63e4b5..40474f426 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1372,6 +1372,7 @@ function get_nonlinops( ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_context...; strict) estim.Nk[] = 0 ∇gi = init_diffmat(JNT, jac, ∇gi_prep, nZ̃, ngi) + ∇gi_structure = init_diffstructure(∇gi) function update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg) if isdifferent(Z̃_arg, Z̃_∇gi) Z̃_∇gi .= Z̃_arg @@ -1385,11 +1386,10 @@ function get_nonlinops( end function ∇gi_func!(∇gi_vec, Z̃_arg) update_con!(gi, ∇gi, Z̃_∇gi, Z̃_arg) - return diffmat2vec!(∇gi_vec, ∇gi) + return diffmat2vec!(∇gi_vec, ∇gi, ∇gi_structure) end gi_min = fill(-myInf, ngi) gi_max = zeros(JNT, ngi) - ∇gi_structure = init_diffstructure(∇gi) g_oracle = MOI.VectorNonlinearOracle(; dimension = nZ̃, l = gi_min, diff --git a/src/general.jl b/src/general.jl index 7dec4709b..b608bb415 100644 --- a/src/general.jl +++ b/src/general.jl @@ -69,19 +69,46 @@ function init_diffstructure(A::AbstractSparseMatrix) I, J = findnz(A) return collect(zip(I, J)) end -init_diffstructure(A::AbstractMatrix)= Tuple.(CartesianIndices(A))[:] +init_diffstructure(A::AbstractMatrix) = Tuple.(CartesianIndices(A))[:] -"Store the differentiation matrix `A` in the vector `v` as required by `JuMP.jl.`" -diffmat2vec!(v::AbstractVector, A::AbstractSparseMatrix) = v .= nonzeros(A) -diffmat2vec!(v::AbstractVector, A::AbstractMatrix) = v[:] = A +"Get the lower-triangular indices from the differentiation matrix structure `i_vec`." +function lowertriangle_indices(i_vec::Vector{Tuple{Int, Int}}) + return [(i,j) for (i,j) in i_vec if i ≥ j] +end + +"Fill the lower triangular part of A in-place with the corresponding part in B." +function fill_lowertriangle!(A::AbstractMatrix, B::AbstractMatrix) + for j in axes(A, 2), i in axes(A, 1) + (i ≥ j) && (A[i, j] = B[i, j]) + end + return A +end + +"Store the diff. matrix `A` in the vector `v` with list of nonzero indices `i_vec`" +function diffmat2vec!(v::AbstractVector, A::AbstractMatrix, i_vec::Vector{Tuple{Int, Int}}) + for i in eachindex(v) + i_A, j_A = i_vec[i] + v[i] = A[i_A, j_A] + end + return v +end backend_str(backend::AbstractADType) = string(nameof(typeof(backend))) +backend_str(backend::Nothing) = "nothing" function backend_str(backend::AutoSparse) str = "AutoSparse ($(nameof(typeof(backend.dense_ad))),"* " $(nameof(typeof(backend.sparsity_detector))),"* " $(nameof(typeof(backend.coloring_algorithm))))" return str end +function backend_str(backend::SecondOrder) + str = "SecondOrder ($(nameof(typeof(backend.outer))),"* + " $(nameof(typeof(backend.inner))))" + return str +end +dense_backend(backend::AbstractADType) = backend +dense_backend(backend::AutoSparse) = backend.dense_ad +dense_backend(backend::SecondOrder) = backend.inner "Verify that x and y elements are different using `!==`." isdifferent(x, y) = any(xi !== yi for (xi, yi) in zip(x, y)) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index a7f9edb28..db8ed3721 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -724,10 +724,18 @@ end @test size(nmpc17.con.Aeq, 1) == nmpc17.estim.nx̂*nmpc17.Hp nmpc18 = NonLinMPC(nonlinmodel, Hp=10, gradient=AutoFiniteDiff(), - jacobian=AutoFiniteDiff() + jacobian=AutoFiniteDiff(), + hessian=AutoFiniteDiff() ) @test nmpc18.gradient == AutoFiniteDiff() @test nmpc18.jacobian == AutoFiniteDiff() + @test nmpc18.hessian == AutoFiniteDiff() + nonlinmodel_simple = NonLinModel((x,u,_,_)->x+u,(x,_,_)->x, 1, 1, 1, 1, solver=nothing) + nmpc19 = NonLinMPC(nonlinmodel_simple, Hc=1, Hp=10, Cwt=Inf, + hessian=SecondOrder(AutoForwardDiff(), AutoForwardDiff()) + ) + @test nmpc19.hessian == SecondOrder(AutoForwardDiff(), AutoForwardDiff()) + @test_nowarn repr(nmpc19) # printing SecondOrder backends, for coverage nonlinmodel2 = NonLinModel{Float32}(f, h, Ts, 2, 4, 2, 1, solver=nothing) nmpc15 = NonLinMPC(nonlinmodel2, Hp=15) @@ -742,6 +750,7 @@ end @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_throws ErrorException NonLinMPC(linmodel1, oracle=false, hessian=AutoFiniteDiff()) @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, JE=(Ue,_,_,_)->Ue) @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, gc=(Ue,_,_,_,_)->Ue, nc=0) @@ -856,7 +865,8 @@ end nmpc10 = setconstraint!(NonLinMPC( nonlinmodel, Nwt=[0], Hp=100, Hc=1, gradient=AutoFiniteDiff(), - jacobian=AutoFiniteDiff()), + jacobian=AutoFiniteDiff(), + hessian=true), ymax=[100], ymin=[-100] ) preparestate!(nmpc10, [0], [0])