diff --git a/Project.toml b/Project.toml index 9645a412b0..098757e3b9 100644 --- a/Project.toml +++ b/Project.toml @@ -73,6 +73,7 @@ DeepDiffs = "ab62b9b5-e342-54a8-a765-a90f495de1a6" FMI = "14a09403-18e3-468f-ad8a-74f8dda2d9ac" InfiniteOpt = "20393b10-9daf-11e9-18c9-8db751c92c57" LabelledArrays = "2ee39098-c373-598a-b85f-a56591580800" +Pyomo = "0e8e1daf-01b5-4eba-a626-3897743a3816" [extensions] MTKBifurcationKitExt = "BifurcationKit" @@ -81,6 +82,7 @@ MTKDeepDiffsExt = "DeepDiffs" MTKFMIExt = "FMI" MTKInfiniteOptExt = "InfiniteOpt" MTKLabelledArraysExt = "LabelledArrays" +MTKPyomoDynamicOptExt = "Pyomo" [compat] ADTypes = "1.14.0" @@ -140,6 +142,7 @@ OrdinaryDiffEqCore = "1.15.0" OrdinaryDiffEqDefault = "1.2" OrdinaryDiffEqNonlinearSolve = "1.5.0" PrecompileTools = "1" +Pyomo = "0.1.0" REPL = "1" RecursiveArrayTools = "3.26" Reexport = "0.2, 1" diff --git a/docs/Project.toml b/docs/Project.toml index ef6af51d79..314a1d7f84 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -5,11 +5,14 @@ BifurcationKit = "0f109fa4-8a5d-4b75-95aa-f515264e7665" CairoMakie = "13f3f980-e62b-5c42-98c6-ff1f3baf88f0" ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" DataInterpolations = "82cc6244-b520-54b8-b5a6-8a565e85f1d0" +DiffEqDevTools = "f3b72e0c-5b89-59e1-b016-84e28bfd966d" Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" DynamicQuantities = "06fc5a27-2a28-4c7c-a15d-362465fb6821" FMI = "14a09403-18e3-468f-ad8a-74f8dda2d9ac" FMIZoo = "724179cf-c260-40a9-bd27-cccc6fe2f195" +InfiniteOpt = "20393b10-9daf-11e9-18c9-8db751c92c57" +Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78" ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739" diff --git a/docs/src/API/dynamic_opt.md b/docs/src/API/dynamic_opt.md new file mode 100644 index 0000000000..1a7081154c --- /dev/null +++ b/docs/src/API/dynamic_opt.md @@ -0,0 +1,41 @@ +### Solvers + +Currently 4 backends are exposed for solving dynamic optimization problems using collocation: JuMP, InfiniteOpt, CasADi, and Pyomo. + +Please note that there are differences in how to construct the collocation solver for the different cases. For example, the Python based ones, CasADi and Pyomo, expect the solver to be passed in as a string (CasADi and Pyomo come pre-loaded with Ipopt, but other solvers may need to be manually installed using `pip` or `conda`), while JuMP/InfiniteOpt expect the optimizer object to be passed in directly: + +``` +JuMPCollocation(Ipopt.Optimizer, constructRK4()) +CasADiCollocation("ipopt", constructRK4()) +``` + +**JuMP** and **CasADi** collocation require an ODE tableau to be passed in. These can be constructed by calling the `constructX()` functions from DiffEqDevTools. The list of tableaus can be found [here](https://docs.sciml.ai/DiffEqDevDocs/dev/internals/tableaus/). If none is passed in, both solvers will default to using Radau second-order with five collocation points. + +**Pyomo** and **InfiniteOpt** each have their own built-in collocation methods. + + 1. **InfiniteOpt**: The list of InfiniteOpt collocation methods can be found [in the table on this page](https://infiniteopt.github.io/InfiniteOpt.jl/stable/guide/derivative/). If none is passed in, the solver defaults to `FiniteDifference(Backward())`, which is effectively implicit Euler. + 2. **Pyomo**: The list of Pyomo collocation methods can be found [at the bottom of this page](https://github.com/SciML/Pyomo.jl). If none is passed in, the solver defaults to a `LagrangeRadau(3)`. + +Some examples of the latter two collocations: + +```julia +PyomoCollocation("ipopt", LagrangeRadau(2)) +InfiniteOptCollocation(Ipopt.Optimizer, OrthogonalCollocation(3)) +``` + +```@docs; canonical = false +JuMPCollocation +InfiniteOptCollocation +CasADiCollocation +PyomoCollocation +solve(::AbstractDynamicOptProblem) +``` + +### Problem constructors + +```@docs; canonical = false +JuMPDynamicOptProblem +InfiniteOptDynamicOptProblem +CasADiDynamicOptProblem +PyomoDynamicOptProblem +``` diff --git a/docs/src/tutorials/dynamic_optimization.md b/docs/src/tutorials/dynamic_optimization.md new file mode 100644 index 0000000000..ff1ffab3dc --- /dev/null +++ b/docs/src/tutorials/dynamic_optimization.md @@ -0,0 +1,128 @@ +# Solving Dynamic Optimization Problems + +Systems in ModelingToolkit.jl can be directly converted to dynamic optimization or optimal control problems. In such systems, one has one or more input variables that are externally controlled to control the dynamics of the system. A dynamic optimization solves for the optimal time trajectory of the input variables in order to maximize or minimize a desired objective function. For example, a car driver might like to know how to step on the accelerator if the goal is to finish a race while using the least gas. + +To begin, let us take a rocket launch example. The input variable here is the thrust exerted by the engine. The rocket state is described by its current height, mass, and velocity. The mass decreases as the rocket loses fuel while thrusting. + +```@example dynamic_opt +using ModelingToolkit +t = ModelingToolkit.t_nounits +D = ModelingToolkit.D_nounits + +@parameters h_c m₀ h₀ g₀ D_c c Tₘ m_c +@variables begin + h(..) + v(..) + m(..), [bounds = (m_c, 1)] + T(..), [input = true, bounds = (0, Tₘ)] +end + +drag(h, v) = D_c * v^2 * exp(-h_c * (h - h₀) / h₀) +gravity(h) = g₀ * (h₀ / h) + +eqs = [D(h(t)) ~ v(t), + D(v(t)) ~ (T(t) - drag(h(t), v(t))) / m(t) - gravity(h(t)), + D(m(t)) ~ -T(t) / c] + +(ts, te) = (0.0, 0.2) +costs = [-h(te)] +cons = [T(te) ~ 0, m(te) ~ m_c] + +@named rocket = System(eqs, t; costs, constraints = cons) +rocket = mtkcompile(rocket, inputs = [T(t)]) + +u0map = [h(t) => h₀, m(t) => m₀, v(t) => 0] +pmap = [ + g₀ => 1, m₀ => 1.0, h_c => 500, c => 0.5 * √(g₀ * h₀), D_c => 0.5 * 620 * m₀ / g₀, + Tₘ => 3.5 * g₀ * m₀, T(t) => 0.0, h₀ => 1, m_c => 0.6] +``` + +What we would like to optimize here is the final height of the rocket. We do this by providing a vector of expressions corresponding to the costs. By default, the sense of the optimization is to minimize the provided cost. So to maximize the rocket height at the final time, we write `-h(te)` as the cost. + +Now we can construct a problem and solve it. Let us use JuMP as our backend here. Note that the package trigger is actually [InfiniteOpt](https://infiniteopt.github.io/InfiniteOpt.jl/stable/), and not JuMP - this package includes JuMP but is designed for optimization on function spaces. Additionally we need to load the solver package - we will use [Ipopt](https://github.com/jump-dev/Ipopt.jl) here (a good choice in general). + +Here we have also loaded DiffEqDevTools because we will need to construct the ODE tableau. This is only needed if one desires a custom ODE tableau for the collocation - by default the solver will use RadauIIA5. + +```@example dynamic_opt +using InfiniteOpt, Ipopt, DiffEqDevTools +jprob = JuMPDynamicOptProblem(rocket, [u0map; pmap], (ts, te); dt = 0.001) +jsol = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructRadauIIA5())); +``` + +The solution has three fields: `jsol.sol` is the ODE solution for the states, `jsol.input_sol` is the ODE solution for the inputs, and `jsol.model` is the wrapped model that we can use to query things like objective and constraint residuals. + +Let's plot the final solution and the controller here: + +```@example dynamic_opt +using CairoMakie +fig = Figure(resolution = (800, 400)) +ax1 = Axis(fig[1, 1], title = "Rocket trajectory", xlabel = "Time") +ax2 = Axis(fig[1, 2], title = "Control trajectory", xlabel = "Time") + +for u in unknowns(rocket) + lines!(ax1, jsol.sol.t, jsol.sol[u], label = string(u)) +end +lines!(ax2, jsol.input_sol, label = "Thrust") +axislegend(ax1) +axislegend(ax2) +fig +``` + +### Free final time problems + +There are additionally a class of dynamic optimization problems where we would like to know how to control our system to achieve something in the least time. Such problems are called free final time problems, since the final time is unknown. To model these problems in ModelingToolkit, we declare the final time as a parameter. + +Below we have a model system called the double integrator. We control the acceleration of a block in order to reach a desired destination in the least time. + +```@example dynamic_opt +@variables begin + x(..) + v(..) + u(..), [bounds = (-1.0, 1.0), input = true] +end + +@parameters tf + +constr = [v(tf) ~ 0, x(tf) ~ 0] +cost = [tf] # Minimize time + +@named block = System( + [D(x(t)) ~ v(t), D(v(t)) ~ u(t)], t; costs = cost, constraints = constr) + +block = mtkcompile(block; inputs = [u(t)]) + +u0map = [x(t) => 1.0, v(t) => 0.0] +tspan = (0.0, tf) +parammap = [u(t) => 0.0, tf => 1.0] +``` + +The `tf` mapping in the parameter map is treated as an initial guess. + +Please note that, at the moment, free final time problems cannot support constraints defined at definite time values, like `x(3) ~ 2`. + +!!! warning + + The Pyomo collocation methods (LagrangeRadau, LagrangeLegendre) currently are bugged for free final time problems. Strongly suggest using BackwardEuler() for such problems when using Pyomo as the backend. + +When declaring the problem in this case we need to provide the number of steps, since dt can't be known in advanced. Let's solve plot our final solution and the controller for the block, using InfiniteOpt as the backend: + +```@example dynamic_opt +iprob = InfiniteOptDynamicOptProblem(block, [u0map; parammap], tspan; steps = 100) +isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer)); +``` + +Let's plot the final solution and the controller here: + +```@example dynamic_opt +fig = Figure(resolution = (800, 400)) +ax1 = Axis(fig[1, 1], title = "Block trajectory", xlabel = "Time") +ax2 = Axis(fig[1, 2], title = "Control trajectory", xlabel = "Time") + +for u in unknowns(block) + lines!(ax1, isol.sol.t, isol.sol[u], label = string(u)) +end +lines!(ax2, isol.input_sol, label = "Acceleration") +axislegend(ax1) +axislegend(ax2) +fig +``` diff --git a/ext/MTKCasADiDynamicOptExt.jl b/ext/MTKCasADiDynamicOptExt.jl index 8f6b2c345e..73e4a77ed4 100644 --- a/ext/MTKCasADiDynamicOptExt.jl +++ b/ext/MTKCasADiDynamicOptExt.jl @@ -6,10 +6,8 @@ using UnPack using NaNMath const MTK = ModelingToolkit -# NaNMath for ff in [acos, log1p, acosh, log2, asin, tan, atanh, cos, log, sin, log10, sqrt] f = nameof(ff) - # These need to be defined so that JuMP can trace through functions built by Symbolics @eval NaNMath.$f(x::CasadiSymbolicObject) = Base.$f(x) end @@ -19,12 +17,19 @@ struct MXLinearInterpolation t::Vector{Float64} dt::Float64 end +Base.getindex(m::MXLinearInterpolation, i...) = length(i) == length(size(m.u)) ? m.u[i...] : m.u[i..., :] -struct CasADiModel - opti::Opti +mutable struct CasADiModel + model::Opti U::MXLinearInterpolation V::MXLinearInterpolation tₛ::MX + is_free_final::Bool + solver_opti::Union{Nothing, Opti} + + function CasADiModel(opti, U, V, tₛ, is_free_final, solver_opti = nothing) + new(opti, U, V, tₛ, is_free_final, solver_opti) + end end struct CasADiDynamicOptProblem{uType, tType, isinplace, P, F, K} <: @@ -33,7 +38,7 @@ struct CasADiDynamicOptProblem{uType, tType, isinplace, P, F, K} <: u0::uType tspan::tType p::P - model::CasADiModel + wrapped_model::CasADiModel kwargs::K function CasADiDynamicOptProblem(f, u0, tspan, p, model, kwargs...) @@ -48,244 +53,94 @@ function (M::MXLinearInterpolation)(τ) Δ = nt - i + 1 (i > length(M.t) || i < 1) && error("Cannot extrapolate past the tspan.") + colons = ntuple(_ -> (:), length(size(M.u)) - 1) if i < length(M.t) - M.u[:, i] + Δ * (M.u[:, i + 1] - M.u[:, i]) + M.u[colons..., i] + Δ*(M.u[colons..., i+1] - M.u[colons..., i]) else - M.u[:, i] + M.u[colons..., i] end end -""" - CasADiDynamicOptProblem(sys::System, u0, tspan, p; dt, steps) - -Convert an System representing an optimal control system into a CasADi model -for solving using optimization. Must provide either `dt`, the timestep between collocation -points (which, along with the timespan, determines the number of points), or directly -provide the number of points as `steps`. - -The optimization variables: -- a vector-of-vectors U representing the unknowns as an interpolation array -- a vector-of-vectors V representing the controls as an interpolation array - -The constraints are: -- The set of user constraints passed to the System via `constraints` -- The solver constraints that encode the time-stepping used by the solver -""" -function MTK.CasADiDynamicOptProblem(sys::System, u0map, tspan, pmap; +function MTK.CasADiDynamicOptProblem(sys::System, op, tspan; dt = nothing, steps = nothing, guesses = Dict(), kwargs...) - MTK.warn_overdetermined(sys, u0map) - _u0map = has_alg_eqs(sys) ? MTK.to_varmap(u0map, unknowns(sys)) : - merge(Dict(u0map), Dict(guesses)) - pmap = MTK.to_varmap(pmap, parameters(sys)) - f, u0, p = MTK.process_SciMLProblem(ODEInputFunction, sys, merge(_u0map, pmap); - t = tspan !== nothing ? tspan[1] : tspan, output_type = MX, kwargs...) + prob, _ = MTK.process_DynamicOptProblem(CasADiDynamicOptProblem, CasADiModel, sys, op, tspan; dt, steps, guesses, kwargs...) + prob +end - pmap = MTK.recursive_unwrap(MTK.AnyDict(pmap)) - MTK.evaluate_varmap!(pmap, keys(pmap)) - steps, is_free_t = MTK.process_tspan(tspan, dt, steps) - model = init_model(sys, tspan, steps, u0map, pmap, u0; is_free_t) +MTK.generate_internal_model(::Type{CasADiModel}) = CasADi.Opti() +MTK.generate_time_variable!(opti::Opti, args...) = nothing - CasADiDynamicOptProblem(f, u0, tspan, p, model, kwargs...) +function MTK.generate_state_variable!(model::Opti, u0, ns, tsteps) + nt = length(tsteps) + U = CasADi.variable!(model, ns, nt) + set_initial!(model, U, DM(repeat(u0, 1, nt))) + MXLinearInterpolation(U, tsteps, tsteps[2] - tsteps[1]) end -function init_model(sys, tspan, steps, u0map, pmap, u0; is_free_t = false) - ctrls = MTK.unbound_inputs(sys) - states = unknowns(sys) - opti = CasADi.Opti() +function MTK.generate_input_variable!(model::Opti, c0, nc, tsteps) + nt = length(tsteps) + V = CasADi.variable!(model, nc, nt) + !isempty(c0) && set_initial!(model, V, DM(repeat(c0, 1, nt))) + MXLinearInterpolation(V, tsteps, tsteps[2] - tsteps[1]) +end +function MTK.generate_timescale!(model::Opti, guess, is_free_t) if is_free_t - (ts_sym, te_sym) = tspan - MTK.symbolic_type(ts_sym) !== MTK.NotSymbolic() && - error("Free initial time problems are not currently supported in CasADiDynamicOptProblem.") - tₛ = variable!(opti) - set_initial!(opti, tₛ, pmap[te_sym]) - subject_to!(opti, tₛ >= ts_sym) - hasbounds(te_sym) && begin - lo, hi = getbounds(te_sym) - subject_to!(opti, tₛ >= lo) - subject_to!(opti, tₛ >= hi) - end - pmap[te_sym] = tₛ - tsteps = LinRange(0, 1, steps) + tₛ = variable!(model) + set_initial!(model, tₛ, guess) + subject_to!(model, tₛ >= 0) + tₛ else - tₛ = MX(1) - tsteps = LinRange(tspan[1], tspan[2], steps) - end - - U = CasADi.variable!(opti, length(states), steps) - V = CasADi.variable!(opti, length(ctrls), steps) - set_initial!(opti, U, DM(repeat(u0, 1, steps))) - c0 = MTK.value.([pmap[c] for c in ctrls]) - !isempty(c0) && set_initial!(opti, V, DM(repeat(c0, 1, steps))) - - U_interp = MXLinearInterpolation(U, tsteps, tsteps[2] - tsteps[1]) - V_interp = MXLinearInterpolation(V, tsteps, tsteps[2] - tsteps[1]) - for (i, ct) in enumerate(ctrls) - pmap[ct] = V[i, :] + MX(1) end - - model = CasADiModel(opti, U_interp, V_interp, tₛ) - - set_casadi_bounds!(model, sys, pmap) - add_cost_function!(model, sys, tspan, pmap; is_free_t) - add_user_constraints!(model, sys, tspan, pmap; is_free_t) - - stidxmap = Dict([v => i for (i, v) in enumerate(states)]) - u0map = Dict([MTK.default_toterm(MTK.value(k)) => v for (k, v) in u0map]) - u0_idxs = has_alg_eqs(sys) ? collect(1:length(states)) : - [stidxmap[MTK.default_toterm(k)] for (k, v) in u0map] - add_initial_constraints!(model, u0, u0_idxs) - - model end -function set_casadi_bounds!(model, sys, pmap) - @unpack opti, U, V = model - for (i, u) in enumerate(unknowns(sys)) - if MTK.hasbounds(u) - lo, hi = MTK.getbounds(u) - subject_to!(opti, Symbolics.fast_substitute(lo, pmap) <= U.u[i, :]) - subject_to!(opti, U.u[i, :] <= Symbolics.fast_substitute(hi, pmap)) - end - end - for (i, v) in enumerate(MTK.unbound_inputs(sys)) - if MTK.hasbounds(v) - lo, hi = MTK.getbounds(v) - subject_to!(opti, Symbolics.fast_substitute(lo, pmap) <= V.u[i, :]) - subject_to!(opti, V.u[i, :] <= Symbolics.fast_substitute(hi, pmap)) - end +function MTK.add_constraint!(m::CasADiModel, expr) + if expr isa Equation + subject_to!(m.model, expr.lhs - expr.rhs == 0) + elseif expr.relational_op === Symbolics.geq + subject_to!(m.model, expr.lhs - expr.rhs ≥ 0) + else + subject_to!(m.model, expr.lhs - expr.rhs ≤ 0) end end -function add_initial_constraints!(model::CasADiModel, u0, u0_idxs) - @unpack opti, U = model +MTK.set_objective!(m::CasADiModel, expr) = minimize!(m.model, MX(expr)) + +function MTK.add_initial_constraints!(m::CasADiModel, u0, u0_idxs, args...) + @unpack model, U = m for i in u0_idxs - subject_to!(opti, U.u[i, 1] == u0[i]) + subject_to!(model, U.u[i, 1] == u0[i]) end end -function add_user_constraints!(model::CasADiModel, sys, tspan, pmap; is_free_t) - @unpack opti, U, V, tₛ = model - - iv = MTK.get_iv(sys) - jconstraints = MTK.get_constraints(sys) - (isnothing(jconstraints) || isempty(jconstraints)) && return nothing - - stidxmap = Dict([v => i for (i, v) in enumerate(unknowns(sys))]) - ctidxmap = Dict([v => i for (i, v) in enumerate(MTK.unbound_inputs(sys))]) - cons_dvs, cons_ps = MTK.process_constraint_system( - jconstraints, Set(unknowns(sys)), parameters(sys), iv; validate = false) - - auxmap = Dict([u => MTK.default_toterm(MTK.value(u)) for u in cons_dvs]) - jconstraints = substitute_casadi_vars(model, sys, pmap, jconstraints; is_free_t, auxmap) - # Manually substitute fixed-t variables - for (i, cons) in enumerate(jconstraints) - consvars = MTK.vars(cons) - for st in consvars - MTK.iscall(st) || continue - x = MTK.operation(st) - t = only(MTK.arguments(st)) - MTK.symbolic_type(t) === MTK.NotSymbolic() || continue - if haskey(stidxmap, x(iv)) - idx = stidxmap[x(iv)] - cv = U - else - idx = ctidxmap[x(iv)] - cv = V - end - cons = Symbolics.substitute(cons, Dict(x(t) => cv(t)[idx])) - end - - if cons isa Equation - subject_to!(opti, cons.lhs - cons.rhs == 0) - elseif cons.relational_op === Symbolics.geq - subject_to!(opti, cons.lhs - cons.rhs ≥ 0) - else - subject_to!(opti, cons.lhs - cons.rhs ≤ 0) - end - end +function MTK.lowered_var(m::CasADiModel, uv, i, t) + X = getfield(m, uv) + t isa Union{Num, Symbolics.Symbolic} ? X.u[i, :] : X(t)[i] end -function add_cost_function!(model::CasADiModel, sys, tspan, pmap; is_free_t) - @unpack opti, U, V, tₛ = model - jcosts = cost(sys) - if Symbolics._iszero(jcosts) - minimize!(opti, MX(0)) - return - end - - iv = MTK.get_iv(sys) - stidxmap = Dict([v => i for (i, v) in enumerate(unknowns(sys))]) - ctidxmap = Dict([v => i for (i, v) in enumerate(MTK.unbound_inputs(sys))]) - - jcosts = substitute_casadi_vars(model, sys, pmap, [jcosts]; is_free_t)[1] - # Substitute fixed-time variables. - costvars = MTK.vars(jcosts) - for st in costvars - MTK.iscall(st) || continue - x = operation(st) - t = only(arguments(st)) - MTK.symbolic_type(t) === MTK.NotSymbolic() || continue - if haskey(stidxmap, x(iv)) - idx = stidxmap[x(iv)] - cv = U - else - idx = ctidxmap[x(iv)] - cv = V +function MTK.lowered_integral(model::CasADiModel, expr, lo, hi) + total = MX(0) + dt = model.U.t[2] - model.U.t[1] + for (i, t) in enumerate(model.U.t) + if lo < t < hi + Δt = min(dt, t - lo) + total += (0.5*Δt*(expr[i] + expr[i-1])) + elseif t >= hi && (t - dt < hi) + Δt = hi - t + dt + total += (0.5*Δt*(expr[i] + expr[i-1])) end - jcosts = Symbolics.substitute(jcosts, Dict(x(t) => cv(t)[idx])) - end - - dt = U.t[2] - U.t[1] - intmap = Dict() - for int in MTK.collect_applied_operators(jcosts, Symbolics.Integral) - op = MTK.operation(int) - arg = only(arguments(MTK.value(int))) - lo, hi = (op.domain.domain.left, op.domain.domain.right) - !isequal((lo, hi), tspan) && - error("Non-whole interval bounds for integrals are not currently supported for CasADiDynamicOptProblem.") - # Approximate integral as sum. - intmap[int] = dt * tₛ * sum(arg) - end - jcosts = Symbolics.substitute(jcosts, intmap) - jcosts = MTK.value(jcosts) - minimize!(opti, MX(jcosts)) -end - -function substitute_casadi_vars( - model::CasADiModel, sys, pmap, exprs; auxmap::Dict = Dict(), is_free_t) - @unpack opti, U, V, tₛ = model - iv = MTK.get_iv(sys) - sts = unknowns(sys) - cts = MTK.unbound_inputs(sys) - - x_ops = [MTK.operation(MTK.unwrap(st)) for st in sts] - c_ops = [MTK.operation(MTK.unwrap(ct)) for ct in cts] - - exprs = map(c -> Symbolics.fast_substitute(c, auxmap), exprs) - exprs = map(c -> Symbolics.fast_substitute(c, Dict(pmap)), exprs) - # tf means different things in different contexts; a [tf] in a cost function - # should be tₛ, while a x(tf) should translate to x[1] - if is_free_t - free_t_map = Dict([[x(tₛ) => U.u[i, end] for (i, x) in enumerate(x_ops)]; - [c(tₛ) => V.u[i, end] for (i, c) in enumerate(c_ops)]]) - exprs = map(c -> Symbolics.fast_substitute(c, free_t_map), exprs) end - - # for variables like x(t) - whole_interval_map = Dict([[v => U.u[i, :] for (i, v) in enumerate(sts)]; - [v => V.u[i, :] for (i, v) in enumerate(cts)]]) - exprs = map(c -> Symbolics.fast_substitute(c, whole_interval_map), exprs) - exprs + model.tₛ * total end -function add_solve_constraints(prob, tableau) +function add_solve_constraints!(prob::CasADiDynamicOptProblem, tableau) @unpack A, α, c = tableau - @unpack model, f, p = prob - @unpack opti, U, V, tₛ = model - solver_opti = copy(opti) + @unpack wrapped_model, f, p = prob + @unpack model, U, V, tₛ = wrapped_model + solver_opti = copy(model) tsteps = U.t dt = tsteps[2] - tsteps[1] @@ -312,7 +167,7 @@ function add_solve_constraints(prob, tableau) for k in 1:(length(tsteps) - 1) τ = tsteps[k] Kᵢ = variable!(solver_opti, nᵤ, length(α)) - ΔUs = A * Kᵢ' # the stepsize at each stage of the implicit method + ΔUs = A * Kᵢ' for (i, h) in enumerate(c) ΔU = ΔUs[i, :]' Uₙ = U.u[:, k] + ΔU * dt @@ -326,57 +181,54 @@ function add_solve_constraints(prob, tableau) solver_opti end -""" - solve(prob::CasADiDynamicOptProblem, casadi_solver, ode_solver; plugin_options, solver_options, silent) - -`plugin_options` and `solver_options` get propagated to the Opti object in CasADi. - -NOTE: the solver should be passed in as a string to CasADi. "ipopt" -""" -function DiffEqBase.solve( - prob::CasADiDynamicOptProblem, solver::Union{String, Symbol} = "ipopt", - tableau_getter = MTK.constructDefault; plugin_options::Dict = Dict(), - solver_options::Dict = Dict(), silent = false) - @unpack model, u0, p, tspan, f = prob - tableau = tableau_getter() - @unpack opti, U, V, tₛ = model +struct CasADiCollocation <: AbstractCollocation + solver::Union{String, Symbol} + tableau::DiffEqBase.ODERKTableau +end - opti = add_solve_constraints(prob, tableau) - silent && (solver_options["print_level"] = 0) - solver!(opti, "$solver", plugin_options, solver_options) +MTK.CasADiCollocation(solver, tableau = MTK.constructDefault()) = CasADiCollocation(solver, tableau) - failed = false - value_getter = nothing - sol = nothing +function MTK.prepare_and_optimize!(prob::CasADiDynamicOptProblem, solver::CasADiCollocation; verbose = false, solver_options = Dict(), plugin_options = Dict(), kwargs...) + solver_opti = add_solve_constraints!(prob, solver.tableau) + verbose || (solver_options["print_level"] = 0) + solver!(solver_opti, "$(solver.solver)", plugin_options, solver_options) try - sol = CasADi.solve!(opti) - value_getter = x -> CasADi.value(sol, x) + CasADi.solve!(solver_opti) catch ErrorException - value_getter = x -> CasADi.debug_value(opti, x) - failed = true end + prob.wrapped_model.solver_opti = solver_opti + prob.wrapped_model +end - ts = value_getter(tₛ) * U.t - U_vals = value_getter(U.u) +function MTK.get_U_values(model::CasADiModel) + value_getter = MTK.successful_solve(model) ? CasADi.debug_value : CasADi.value + (nu, nt) = size(model.U.u) + U_vals = value_getter(model.solver_opti, model.U.u) size(U_vals, 2) == 1 && (U_vals = U_vals') - U_vals = [[U_vals[i, j] for i in 1:size(U_vals, 1)] for j in 1:length(ts)] - ode_sol = DiffEqBase.build_solution(prob, tableau_getter, ts, U_vals) + U_vals = [[U_vals[i, j] for i in 1:nu] for j in 1:nt] +end - input_sol = nothing - if prod(size(V.u)) != 0 - V_vals = value_getter(V.u) +function MTK.get_V_values(model::CasADiModel) + value_getter = MTK.successful_solve(model) ? CasADi.debug_value : CasADi.value + (nu, nt) = size(model.V.u) + if nu*nt != 0 + V_vals = value_getter(model.solver_opti, model.V.u) size(V_vals, 2) == 1 && (V_vals = V_vals') - V_vals = [[V_vals[i, j] for i in 1:size(V_vals, 1)] for j in 1:length(ts)] - input_sol = DiffEqBase.build_solution(prob, tableau_getter, ts, V_vals) + V_vals = [[V_vals[i, j] for i in 1:nu] for j in 1:nt] + else + nothing end +end - if failed - ode_sol = SciMLBase.solution_new_retcode( - ode_sol, SciMLBase.ReturnCode.ConvergenceFailure) - !isnothing(input_sol) && (input_sol = SciMLBase.solution_new_retcode( - input_sol, SciMLBase.ReturnCode.ConvergenceFailure)) - end +function MTK.get_t_values(model::CasADiModel) + value_getter = MTK.successful_solve(model) ? CasADi.debug_value : CasADi.value + ts = value_getter(model.solver_opti, model.tₛ) .* model.U.t +end +MTK.objective_value(model::CasADiModel) = CasADi.pyconvert(Float64, model.solver_opti.py.value(model.solver_opti.py.f)) - DynamicOptSolution(model, ode_sol, input_sol) +function MTK.successful_solve(m::CasADiModel) + isnothing(m.solver_opti) && return false + retcode = CasADi.return_status(m.solver_opti) + retcode == "Solve_Succeeded" || retcode == "Solved_To_Acceptable_Level" end end diff --git a/ext/MTKInfiniteOptExt.jl b/ext/MTKInfiniteOptExt.jl index 40eb6f5264..8150cd06f7 100644 --- a/ext/MTKInfiniteOptExt.jl +++ b/ext/MTKInfiniteOptExt.jl @@ -4,17 +4,26 @@ using InfiniteOpt using DiffEqBase using LinearAlgebra using StaticArrays +using UnPack import SymbolicUtils import NaNMath const MTK = ModelingToolkit +struct InfiniteOptModel + model::InfiniteModel + U::Vector{<:AbstractVariableRef} + V::Vector{<:AbstractVariableRef} + tₛ::AbstractVariableRef + is_free_final::Bool +end + struct JuMPDynamicOptProblem{uType, tType, isinplace, P, F, K} <: AbstractDynamicOptProblem{uType, tType, isinplace} f::F u0::uType tspan::tType p::P - model::InfiniteModel + wrapped_model::InfiniteOptModel kwargs::K function JuMPDynamicOptProblem(f, u0, tspan, p, model, kwargs...) @@ -29,7 +38,7 @@ struct InfiniteOptDynamicOptProblem{uType, tType, isinplace, P, F, K} <: u0::uType tspan::tType p::P - model::InfiniteModel + wrapped_model::InfiniteOptModel kwargs::K function InfiniteOptDynamicOptProblem(f, u0, tspan, p, model, kwargs...) @@ -38,275 +47,90 @@ struct InfiniteOptDynamicOptProblem{uType, tType, isinplace, P, F, K} <: end end -""" - JuMPDynamicOptProblem(sys::System, u0, tspan, p; dt) +MTK.generate_internal_model(m::Type{InfiniteOptModel}) = InfiniteModel() +MTK.generate_time_variable!(m::InfiniteModel, tspan, tsteps) = @infinite_parameter(m, t in [tspan[1], tspan[2]], num_supports = length(tsteps)) +MTK.generate_state_variable!(m::InfiniteModel, u0::Vector, ns, ts) = @variable(m, U[i = 1:ns], Infinite(m[:t]), start=u0[i]) +MTK.generate_input_variable!(m::InfiniteModel, c0, nc, ts) = @variable(m, V[i = 1:nc], Infinite(m[:t]), start=c0[i]) -Convert a System representing an optimal control system into a JuMP model -for solving using optimization. Must provide either `dt`, the timestep between collocation -points (which, along with the timespan, determines the number of points), or directly -provide the number of points as `steps`. +function MTK.generate_timescale!(m::InfiniteModel, guess, is_free_t) + @variable(m, tₛ ≥ 0, start = guess) + if !is_free_t + fix(tₛ, 1, force=true) + set_start_value(tₛ, 1) + end + tₛ +end -The optimization variables: -- a vector-of-vectors U representing the unknowns as an interpolation array -- a vector-of-vectors V representing the controls as an interpolation array +function MTK.add_constraint!(m::InfiniteOptModel, expr::Union{Equation, Inequality}) + if expr isa Equation + @constraint(m.model, expr.lhs - expr.rhs == 0) + elseif expr.relational_op === Symbolics.geq + @constraint(m.model, expr.lhs - expr.rhs ≥ 0) + else + @constraint(m.model, expr.lhs - expr.rhs ≤ 0) + end +end +MTK.set_objective!(m::InfiniteOptModel, expr) = @objective(m.model, Min, expr) -The constraints are: -- The set of user constraints passed to the System via `constraints` -- The solver constraints that encode the time-stepping used by the solver -""" -function MTK.JuMPDynamicOptProblem(sys::System, u0map, tspan, pmap; +function MTK.JuMPDynamicOptProblem(sys::System, op, tspan; dt = nothing, steps = nothing, guesses = Dict(), kwargs...) - MTK.warn_overdetermined(sys, u0map) - _u0map = has_alg_eqs(sys) ? MTK.to_varmap(u0map, unknowns(sys)) : - merge(Dict(u0map), Dict(guesses)) - pmap = MTK.to_varmap(pmap, parameters(sys)) - f, u0, p = MTK.process_SciMLProblem(ODEInputFunction, sys, merge(_u0map, pmap); - t = tspan !== nothing ? tspan[1] : tspan, kwargs...) - - pmap = MTK.recursive_unwrap(MTK.AnyDict(pmap)) - MTK.evaluate_varmap!(pmap, keys(pmap)) - steps, is_free_t = MTK.process_tspan(tspan, dt, steps) - model = init_model(sys, tspan, steps, u0map, pmap, u0; is_free_t) - - JuMPDynamicOptProblem(f, u0, tspan, p, model, kwargs...) + prob, _ = MTK.process_DynamicOptProblem(JuMPDynamicOptProblem, InfiniteOptModel, sys, op, tspan; dt, steps, guesses, kwargs...) + prob end -""" - InfiniteOptDynamicOptProblem(sys::System, u0map, tspan, pmap; dt) - -Convert System representing an optimal control system into a InfiniteOpt model -for solving using optimization. Must provide `dt` for determining the length -of the interpolation arrays. - -Related to `JuMPDynamicOptProblem`, but directly adds the differential equations -of the system as derivative constraints, rather than using a solver tableau. -""" -function MTK.InfiniteOptDynamicOptProblem(sys::System, u0map, tspan, pmap; +function MTK.InfiniteOptDynamicOptProblem(sys::System, op, tspan; dt = nothing, steps = nothing, guesses = Dict(), kwargs...) - MTK.warn_overdetermined(sys, u0map) - _u0map = has_alg_eqs(sys) ? MTK.to_varmap(u0map, unknowns(sys)) : - merge(Dict(u0map), Dict(guesses)) - pmap = MTK.to_varmap(pmap, parameters(sys)) - f, u0, p = MTK.process_SciMLProblem(ODEInputFunction, sys, merge(_u0map, pmap); - t = tspan !== nothing ? tspan[1] : tspan, kwargs...) - - pmap = MTK.recursive_unwrap(MTK.AnyDict(pmap)) - MTK.evaluate_varmap!(pmap, keys(pmap)) - steps, is_free_t = MTK.process_tspan(tspan, dt, steps) - model = init_model(sys, tspan, steps, u0map, pmap, u0; is_free_t) - - add_infopt_solve_constraints!(model, sys, pmap; is_free_t) - InfiniteOptDynamicOptProblem(f, u0, tspan, p, model, kwargs...) -end - -# Initialize InfiniteOpt model. -function init_model(sys, tspan, steps, u0map, pmap, u0; is_free_t = false) - ctrls = MTK.unbound_inputs(sys) - states = unknowns(sys) - model = InfiniteModel() - - if is_free_t - (ts_sym, te_sym) = tspan - MTK.symbolic_type(ts_sym) !== MTK.NotSymbolic() && - error("Free initial time problems are not currently supported.") - @variable(model, tf, start=pmap[te_sym]) - set_lower_bound(tf, ts_sym) - hasbounds(te_sym) && begin - lo, hi = getbounds(te_sym) - set_lower_bound(tf, lo) - set_upper_bound(tf, hi) - end - pmap[te_sym] = model[:tf] - tspan = (0, 1) - end - - @infinite_parameter(model, t in [tspan[1], tspan[2]], num_supports=steps) - @variable(model, U[i = 1:length(states)], Infinite(t), start=u0[i]) - c0 = MTK.value.([pmap[c] for c in ctrls]) - @variable(model, V[i = 1:length(ctrls)], Infinite(t), start=c0[i]) - for (i, ct) in enumerate(ctrls) - pmap[ct] = model[:V][i] - end - - set_jump_bounds!(model, sys, pmap) - add_jump_cost_function!(model, sys, (tspan[1], tspan[2]), pmap; is_free_t) - add_user_constraints!(model, sys, pmap; is_free_t) - - stidxmap = Dict([v => i for (i, v) in enumerate(states)]) - u0map = Dict([MTK.default_toterm(MTK.value(k)) => v for (k, v) in u0map]) - u0_idxs = has_alg_eqs(sys) ? collect(1:length(states)) : - [stidxmap[MTK.default_toterm(k)] for (k, v) in u0map] - add_initial_constraints!(model, u0, u0_idxs, tspan[1]) - return model -end - -""" -Modify the pmap by replacing controls with V[i](t), and tf with the model's final time variable for free final time problems. -""" -function modified_pmap(model, sys, pmap) - pmap = Dict{Any, Any}(pmap) + prob, pmap = MTK.process_DynamicOptProblem(InfiniteOptDynamicOptProblem, InfiniteOptModel, sys, op, tspan; dt, steps, guesses, kwargs...) + MTK.add_equational_constraints!(prob.wrapped_model, sys, pmap, tspan) + prob end -function set_jump_bounds!(model, sys, pmap) - U = model[:U] - for (i, u) in enumerate(unknowns(sys)) - if MTK.hasbounds(u) - lo, hi = MTK.getbounds(u) - set_lower_bound(U[i], Symbolics.fast_substitute(lo, pmap)) - set_upper_bound(U[i], Symbolics.fast_substitute(hi, pmap)) - end - end - - V = model[:V] - for (i, v) in enumerate(MTK.unbound_inputs(sys)) - if MTK.hasbounds(v) - lo, hi = MTK.getbounds(v) - set_lower_bound(V[i], Symbolics.fast_substitute(lo, pmap)) - set_upper_bound(V[i], Symbolics.fast_substitute(hi, pmap)) - end - end -end - -function add_jump_cost_function!(model::InfiniteModel, sys, tspan, pmap; is_free_t = false) - jcosts = cost(sys) - if Symbolics._iszero(jcosts) - @objective(model, Min, 0) - return - end - jcosts = substitute_jump_vars(model, sys, pmap, [jcosts]; is_free_t)[1] - tₛ = is_free_t ? model[:tf] : 1 - - # Substitute integral - iv = MTK.get_iv(sys) +MTK.lowered_integral(model::InfiniteOptModel, expr, lo, hi) = model.tₛ * InfiniteOpt.∫(expr, model.model[:t], lo, hi) +MTK.lowered_derivative(model::InfiniteOptModel, i) = ∂(model.U[i], model.model[:t]) - intmap = Dict() - for int in MTK.collect_applied_operators(jcosts, Symbolics.Integral) - op = MTK.operation(int) - arg = only(arguments(MTK.value(int))) - lo, hi = (op.domain.domain.left, op.domain.domain.right) - lo = MTK.value(lo) - hi = haskey(pmap, hi) ? 1 : MTK.value(hi) - intmap[int] = tₛ * InfiniteOpt.∫(arg, model[:t], lo, hi) +function MTK.process_integral_bounds(model::InfiniteOptModel, integral_span, tspan) + if MTK.is_free_final(model) && isequal(integral_span, tspan) + integral_span = (0, 1) + elseif MTK.is_free_final(model) + error("Free final time problems cannot handle partial timespans.") + else + integral_span end - jcosts = Symbolics.substitute(jcosts, intmap) - @objective(model, Min, MTK.value(jcosts)) end -function add_user_constraints!(model::InfiniteModel, sys, pmap; is_free_t = false) - jconstraints = MTK.get_constraints(sys) - (isnothing(jconstraints) || isempty(jconstraints)) && return nothing - cons_dvs, cons_ps = MTK.process_constraint_system( - jconstraints, Set(unknowns(sys)), parameters(sys), MTK.get_iv(sys); validate = false) - - if is_free_t - for u in cons_dvs - x = MTK.operation(u) - t = only(arguments(u)) - if (MTK.symbolic_type(t) === MTK.NotSymbolic()) - error("Provided specific time constraint in a free final time problem. This is not supported by the JuMP/InfiniteOpt collocation solvers. The offending variable is $u. Specific-time constraints can only be specified at the beginning or end of the timespan.") - end - end +function MTK.add_initial_constraints!(m::InfiniteOptModel, u0, u0_idxs, ts) + for i in u0_idxs + fix(m.U[i](0), u0[i], force = true) end - - auxmap = Dict([u => MTK.default_toterm(MTK.value(u)) for u in cons_dvs]) - jconstraints = substitute_jump_vars(model, sys, pmap, jconstraints; auxmap, is_free_t) - - # Substitute to-term'd variables - for (i, cons) in enumerate(jconstraints) - if cons isa Equation - @constraint(model, cons.lhs - cons.rhs==0, base_name="user[$i]") - elseif cons.relational_op === Symbolics.geq - @constraint(model, cons.lhs - cons.rhs≥0, base_name="user[$i]") - else - @constraint(model, cons.lhs - cons.rhs≤0, base_name="user[$i]") - end - end -end - -function add_initial_constraints!(model::InfiniteModel, u0, u0_idxs, ts) - U = model[:U] - @constraint(model, initial[i in u0_idxs], U[i](ts)==u0[i]) end -function substitute_jump_vars(model, sys, pmap, exprs; auxmap = Dict(), is_free_t = false) - iv = MTK.get_iv(sys) - sts = unknowns(sys) - cts = MTK.unbound_inputs(sys) - U = model[:U] - V = model[:V] - x_ops = [MTK.operation(MTK.unwrap(st)) for st in sts] - c_ops = [MTK.operation(MTK.unwrap(ct)) for ct in cts] - - exprs = map(c -> Symbolics.fast_substitute(c, auxmap), exprs) - exprs = map(c -> Symbolics.fast_substitute(c, Dict(pmap)), exprs) - if is_free_t - tf = model[:tf] - free_t_map = Dict([[x(tf) => U[i](1) for (i, x) in enumerate(x_ops)]; - [c(tf) => V[i](1) for (i, c) in enumerate(c_ops)]]) - exprs = map(c -> Symbolics.fast_substitute(c, free_t_map), exprs) - end - - # for variables like x(t) - whole_interval_map = Dict([[v => U[i] for (i, v) in enumerate(sts)]; - [v => V[i] for (i, v) in enumerate(cts)]]) - exprs = map(c -> Symbolics.fast_substitute(c, whole_interval_map), exprs) - - # for variables like x(1.0) - fixed_t_map = Dict([[x_ops[i] => U[i] for i in 1:length(U)]; - [c_ops[i] => V[i] for i in 1:length(V)]]) - - exprs = map(c -> Symbolics.fast_substitute(c, fixed_t_map), exprs) - exprs +function MTK.lowered_var(m::InfiniteOptModel, uv, i, t) + X = getfield(m, uv) + t isa Union{Num, Symbolics.Symbolic} ? X[i] : X[i](t) end -function add_infopt_solve_constraints!(model::InfiniteModel, sys, pmap; is_free_t = false) - # Differential equations - U = model[:U] - t = model[:t] - D = Differential(MTK.get_iv(sys)) - diffsubmap = Dict([D(U[i]) => ∂(U[i], t) for i in 1:length(U)]) - tₛ = is_free_t ? model[:tf] : 1 - - diff_eqs = substitute_jump_vars(model, sys, pmap, diff_equations(sys)) - diff_eqs = map(e -> Symbolics.substitute(e, diffsubmap), diff_eqs) - @constraint(model, D[i = 1:length(diff_eqs)], diff_eqs[i].lhs==tₛ * diff_eqs[i].rhs) - - # Algebraic equations - alg_eqs = substitute_jump_vars(model, sys, pmap, alg_equations(sys)) - @constraint(model, A[i = 1:length(alg_eqs)], alg_eqs[i].lhs==alg_eqs[i].rhs) -end - -function add_jump_solve_constraints!(prob, tableau; is_free_t = false) - A = tableau.A - α = tableau.α - c = tableau.c - model = prob.model - f = prob.f - p = prob.p - +function add_solve_constraints!(prob::JuMPDynamicOptProblem, tableau) + @unpack A, α, c = tableau + @unpack wrapped_model, f, p = prob + @unpack tₛ, U, V, model = wrapped_model t = model[:t] tsteps = supports(t) - tmax = tsteps[end] - pop!(tsteps) - tₛ = is_free_t ? model[:tf] : 1 dt = tsteps[2] - tsteps[1] - U = model[:U] - V = model[:V] nᵤ = length(U) nᵥ = length(V) if MTK.is_explicit(tableau) K = Any[] - for τ in tsteps + for τ in tsteps[1:end-1] for (i, h) in enumerate(c) ΔU = sum([A[i, j] * K[j] for j in 1:(i - 1)], init = zeros(nᵤ)) Uₙ = [U[i](τ) + ΔU[i] * dt for i in 1:nᵤ] Vₙ = [V[i](τ) for i in 1:nᵥ] - Kₙ = tₛ * f(Uₙ, Vₙ, p, τ + h * dt) # scale the time + Kₙ = tₛ * f(Uₙ, Vₙ, p, τ + h * dt) push!(K, Kₙ) end ΔU = dt * sum([α[i] * K[i] for i in 1:length(α)]) @@ -315,37 +139,37 @@ function add_jump_solve_constraints!(prob, tableau; is_free_t = false) empty!(K) end else - @variable(model, K[1:length(α), 1:nᵤ], Infinite(t)) + K = @variable(model, K[1:length(α), 1:nᵤ], Infinite(model[:t])) ΔUs = A * K ΔU_tot = dt * (K' * α) - for τ in tsteps + for τ in tsteps[1:end-1] for (i, h) in enumerate(c) ΔU = @view ΔUs[i, :] - Uₙ = U + ΔU * h * dt + Uₙ = U + ΔU * dt @constraint(model, [j = 1:nᵤ], K[i, j]==(tₛ * f(Uₙ, V, p, τ + h * dt)[j]), DomainRestrictions(t => τ), base_name="solve_K$i($τ)") end - @constraint(model, [n = 1:nᵤ], U[n](τ) + ΔU_tot[n]==U[n](min(τ + dt, tmax)), + @constraint(model, [n = 1:nᵤ], U[n](τ) + ΔU_tot[n]==U[n](min(τ + dt, tsteps[end])), DomainRestrictions(t => τ), base_name="solve_U($τ)") end end end -""" -Solve JuMPDynamicOptProblem. Arguments: -- prob: a JumpDynamicOptProblem -- jump_solver: a LP solver such as HiGHS -- tableau_getter: Takes in a function to fetch a tableau. Tableau loaders look like `constructRK4` and may be found at https://docs.sciml.ai/DiffEqDevDocs/stable/internals/tableaus/. If this argument is not passed in, the solver will default to Radau second order. -- silent: set the model silent (suppress model output) +struct JuMPCollocation <: AbstractCollocation + solver::Any + tableau::DiffEqBase.ODERKTableau +end +MTK.JuMPCollocation(solver, tableau = MTK.constructDefault()) = JuMPCollocation(solver, tableau) -Returns a DynamicOptSolution, which contains both the model and the ODE solution. -""" -function DiffEqBase.solve( - prob::JuMPDynamicOptProblem, jump_solver, tableau_getter = MTK.constructDefault; silent = false) - model = prob.model - tableau = tableau_getter() - silent && set_silent(model) +struct InfiniteOptCollocation <: AbstractCollocation + solver::Any + derivative_method::InfiniteOpt.AbstractDerivativeMethod +end +MTK.InfiniteOptCollocation(solver, derivative_method = InfiniteOpt.FiniteDifference(InfiniteOpt.Backward())) = InfiniteOptCollocation(solver, derivative_method) +function MTK.prepare_and_optimize!(prob::JuMPDynamicOptProblem, solver::JuMPCollocation; verbose = false, kwargs...) + model = prob.wrapped_model.model + verbose || set_silent(model) # Unregister current solver constraints for con in all_constraints(model) if occursin("solve", JuMP.name(con)) @@ -360,53 +184,47 @@ function DiffEqBase.solve( delete(model, var) end end - add_jump_solve_constraints!(prob, tableau; is_free_t = haskey(model, :tf)) - _solve(prob, jump_solver, tableau_getter) + add_solve_constraints!(prob, solver.tableau) + set_optimizer(model, solver.solver) + optimize!(model) + model end -""" -`derivative_method` kwarg refers to the method used by InfiniteOpt to compute derivatives. The list of possible options can be found at https://infiniteopt.github.io/InfiniteOpt.jl/stable/guide/derivative/. Defaults to FiniteDifference(Backward()). -""" -function DiffEqBase.solve(prob::InfiniteOptDynamicOptProblem, jump_solver; - derivative_method = InfiniteOpt.FiniteDifference(Backward()), silent = false) - model = prob.model - silent && set_silent(model) - set_derivative_method(model[:t], derivative_method) - _solve(prob, jump_solver, derivative_method) +function MTK.prepare_and_optimize!(prob::InfiniteOptDynamicOptProblem, solver::InfiniteOptCollocation; verbose = false, kwargs...) + model = prob.wrapped_model.model + verbose || set_silent(model) + set_derivative_method(model[:t], solver.derivative_method) + set_optimizer(model, solver.solver) + optimize!(model) + model end -function _solve(prob::AbstractDynamicOptProblem, jump_solver, solver) - model = prob.model - set_optimizer(model, jump_solver) - optimize!(model) +function MTK.get_V_values(m::InfiniteModel) + nt = length(supports(m[:t])) + if !isempty(m[:V]) + V_vals = value.(m[:V]) + V_vals = [[V_vals[i][j] for i in 1:length(V_vals)] for j in 1:nt] + else + nothing + end +end +function MTK.get_U_values(m::InfiniteModel) + nt = length(supports(m[:t])) + U_vals = value.(m[:U]) + U_vals = [[U_vals[i][j] for i in 1:length(U_vals)] for j in 1:nt] +end +MTK.get_t_values(m::InfiniteModel) = value(m[:tₛ]) * supports(m[:t]) +MTK.objective_value(m::InfiniteModel) = InfiniteOpt.objective_value(m) +function MTK.successful_solve(model::InfiniteModel) tstatus = termination_status(model) pstatus = primal_status(model) !has_values(model) && error("Model not solvable; please report this to github.com/SciML/ModelingToolkit.jl with a MWE.") - tf = haskey(model, :tf) ? value(model[:tf]) : 1 - ts = tf * supports(model[:t]) - U_vals = value.(model[:U]) - U_vals = [[U_vals[i][j] for i in 1:length(U_vals)] for j in 1:length(ts)] - sol = DiffEqBase.build_solution(prob, solver, ts, U_vals) - - input_sol = nothing - if !isempty(model[:V]) - V_vals = value.(model[:V]) - V_vals = [[V_vals[i][j] for i in 1:length(V_vals)] for j in 1:length(ts)] - input_sol = DiffEqBase.build_solution(prob, solver, ts, V_vals) - end - - if !(pstatus === FEASIBLE_POINT && + pstatus === FEASIBLE_POINT && (tstatus === OPTIMAL || tstatus === LOCALLY_SOLVED || tstatus === ALMOST_OPTIMAL || - tstatus === ALMOST_LOCALLY_SOLVED)) - sol = SciMLBase.solution_new_retcode(sol, SciMLBase.ReturnCode.ConvergenceFailure) - !isnothing(input_sol) && (input_sol = SciMLBase.solution_new_retcode( - input_sol, SciMLBase.ReturnCode.ConvergenceFailure)) - end - - DynamicOptSolution(model, sol, input_sol) + tstatus === ALMOST_LOCALLY_SOLVED) end import InfiniteOpt: JuMP, GeneralVariableRef diff --git a/ext/MTKPyomoDynamicOptExt.jl b/ext/MTKPyomoDynamicOptExt.jl new file mode 100644 index 0000000000..cb0aafc432 --- /dev/null +++ b/ext/MTKPyomoDynamicOptExt.jl @@ -0,0 +1,230 @@ +module MTKPyomoDynamicOptExt +using ModelingToolkit +using Pyomo +using DiffEqBase +using UnPack +using NaNMath +using Setfield +const MTK = ModelingToolkit + +const SPECIAL_FUNCTIONS_DICT = Dict([acos => Pyomo.py_acos, + acosh => Pyomo.py_acosh, + asin => Pyomo.py_asin, + tan => Pyomo.py_tan, + atanh => Pyomo.py_atanh, + cos => Pyomo.py_cos, + log => Pyomo.py_log, + sin => Pyomo.py_sin, + sqrt => Pyomo.py_sqrt, + exp => Pyomo.py_exp]) + +struct PyomoDynamicOptModel + model::ConcreteModel + U::PyomoVar + V::PyomoVar + tₛ::PyomoVar + is_free_final::Bool + solver_model::Union{Nothing, ConcreteModel} + dU::PyomoVar + model_sym::Union{Num, Symbolics.BasicSymbolic} + t_sym::Union{Num, Symbolics.BasicSymbolic} + dummy_sym::Union{Num, Symbolics.BasicSymbolic} + + function PyomoDynamicOptModel(model, U, V, tₛ, is_free_final) + @variables MODEL_SYM::Symbolics.symstruct(ConcreteModel) T_SYM DUMMY_SYM + model.dU = dae.DerivativeVar(U, wrt = model.t, initialize = 0) + new(model, U, V, tₛ, is_free_final, nothing, + PyomoVar(model.dU), MODEL_SYM, T_SYM, DUMMY_SYM) + end +end + +struct PyomoDynamicOptProblem{uType, tType, isinplace, P, F, K} <: + AbstractDynamicOptProblem{uType, tType, isinplace} + f::F + u0::uType + tspan::tType + p::P + wrapped_model::PyomoDynamicOptModel + kwargs::K + + function PyomoDynamicOptProblem(f, u0, tspan, p, model, kwargs...) + new{typeof(u0), typeof(tspan), SciMLBase.isinplace(f, 5), + typeof(p), typeof(f), typeof(kwargs)}(f, u0, tspan, p, model, kwargs) + end +end + +function pysym_getproperty(s::Union{Num, Symbolics.Symbolic}, name::Symbol) + Symbolics.wrap(SymbolicUtils.term( + _getproperty, Symbolics.unwrap(s), Val{name}(), type = Symbolics.Struct{PyomoVar})) +end +_getproperty(s, name::Val{fieldname}) where {fieldname} = getproperty(s, fieldname) + +function MTK.PyomoDynamicOptProblem(sys::System, op, tspan; + dt = nothing, steps = nothing, + guesses = Dict(), kwargs...) + prob, + pmap = MTK.process_DynamicOptProblem(PyomoDynamicOptProblem, PyomoDynamicOptModel, + sys, op, tspan; dt, steps, guesses, kwargs...) + conc_model = prob.wrapped_model.model + MTK.add_equational_constraints!(prob.wrapped_model, sys, pmap, tspan) + prob +end + +function MTK.generate_internal_model(m::Type{PyomoDynamicOptModel}) + ConcreteModel(pyomo.ConcreteModel()) +end + +function MTK.generate_time_variable!(m::ConcreteModel, tspan, tsteps) + m.steps = length(tsteps) + m.t = dae.ContinuousSet(initialize = tsteps, bounds = tspan) + m.time = pyomo.Var(m.t) +end + +function MTK.generate_state_variable!(m::ConcreteModel, u0, ns, ts) + m.u_idxs = pyomo.RangeSet(1, ns) + init_f = Pyomo.pyfunc((m, i, t) -> (u0[Pyomo.pyconvert(Int, i)])) + m.U = pyomo.Var(m.u_idxs, m.t, initialize = init_f) + PyomoVar(m.U) +end + +function MTK.generate_input_variable!(m::ConcreteModel, c0, nc, ts) + m.v_idxs = pyomo.RangeSet(1, nc) + init_f = Pyomo.pyfunc((m, i, t) -> (c0[Pyomo.pyconvert(Int, i)])) + m.V = pyomo.Var(m.v_idxs, m.t, initialize = init_f) + PyomoVar(m.V) +end + +function MTK.generate_timescale!(m::ConcreteModel, guess, is_free_t) + m.tₛ = is_free_t ? pyomo.Var(initialize = guess, bounds = (0, Inf)) : Pyomo.Py(1) + PyomoVar(m.tₛ) +end + +function MTK.add_constraint!(pmodel::PyomoDynamicOptModel, cons; n_idxs = 1) + @unpack model, model_sym, t_sym, dummy_sym = pmodel + expr = if cons isa Equation + cons.lhs - cons.rhs == 0 + elseif cons.relational_op === Symbolics.geq + cons.lhs - cons.rhs ≥ 0 + else + cons.lhs - cons.rhs ≤ 0 + end + expr = Symbolics.substitute(Symbolics.unwrap(expr), SPECIAL_FUNCTIONS_DICT, fold = false) + + cons_sym = Symbol("cons", hash(cons)) + if occursin(Symbolics.unwrap(t_sym), expr) + f = eval(Symbolics.build_function(expr, model_sym, t_sym)) + setproperty!(model, cons_sym, pyomo.Constraint(model.t, rule = Pyomo.pyfunc(f))) + else + f = eval(Symbolics.build_function(expr, model_sym, dummy_sym)) + setproperty!(model, cons_sym, pyomo.Constraint(rule = Pyomo.pyfunc(f))) + end +end + +function MTK.set_objective!(pmodel::PyomoDynamicOptModel, expr) + @unpack model, model_sym, t_sym, dummy_sym = pmodel + expr = Symbolics.substitute(expr, SPECIAL_FUNCTIONS_DICT, fold = false) + if occursin(Symbolics.unwrap(t_sym), expr) + f = eval(Symbolics.build_function(expr, model_sym, t_sym)) + model.obj = pyomo.Objective(model.t, rule = Pyomo.pyfunc(f)) + else + f = eval(Symbolics.build_function(expr, model_sym, dummy_sym)) + model.obj = pyomo.Objective(rule = Pyomo.pyfunc(f)) + end +end + +function MTK.add_initial_constraints!(model::PyomoDynamicOptModel, u0, u0_idxs, ts) + for i in u0_idxs + model.U[i, 0].fix(u0[i]) + end +end + +function MTK.lowered_integral(m::PyomoDynamicOptModel, arg, lo, hi) + @unpack model, model_sym, t_sym, dummy_sym = m + total = 0 + dt = Pyomo.pyconvert(Float64, (model.t.at(-1) - model.t.at(1))/(model.steps - 1)) + f = Symbolics.build_function(arg, model_sym, t_sym, expression = false) + for (i, t) in enumerate(model.t) + if Bool(lo < t) && Bool(t < hi) + t_p = model.t.at(i-1) + Δt = min(t - lo, t - t_p) + total += 0.5*Δt*(f(model, t) + f(model, t_p)) + elseif Bool(t >= hi) && Bool(t - dt < hi) + t_p = model.t.at(i-1) + Δt = hi - t + dt + total += 0.5*Δt*(f(model, t) + f(model, t_p)) + end + end + PyomoVar(model.tₛ * total) +end + +function MTK.lowered_derivative(m::PyomoDynamicOptModel, i) + mdU = Symbolics.value(pysym_getproperty(m.model_sym, :dU)) + Symbolics.unwrap(mdU[i, m.t_sym]) +end + +function MTK.lowered_var(m::PyomoDynamicOptModel, uv, i, t) + X = Symbolics.value(pysym_getproperty(m.model_sym, uv)) + var = t isa Union{Num, Symbolics.Symbolic} ? X[i, m.t_sym] : X[i, t] + Symbolics.unwrap(var) +end + +struct PyomoCollocation <: AbstractCollocation + solver::Union{String, Symbol} + derivative_method::Pyomo.DiscretizationMethod +end + +function MTK.PyomoCollocation(solver, derivative_method = LagrangeRadau(5)) + PyomoCollocation(solver, derivative_method) +end + +function MTK.prepare_and_optimize!( + prob::PyomoDynamicOptProblem, collocation; verbose, kwargs...) + solver_m = prob.wrapped_model.model.clone() + dm = collocation.derivative_method + discretizer = TransformationFactory(dm) + if MTK.is_free_final(prob.wrapped_model) && !Pyomo.is_finite_difference(dm) + error("The Lagrange-Radau and Lagrange-Legendre collocations currently cannot be used for free final problems.") + end + ncp = Pyomo.is_finite_difference(dm) ? 1 : dm.np + discretizer.apply_to(solver_m, wrt = solver_m.t, nfe = solver_m.steps - 1, + scheme = Pyomo.scheme_string(dm)) + + solver = SolverFactory(string(collocation.solver)) + results = solver.solve(solver_m, tee = true) + PyomoOutput(results, solver_m) +end + +struct PyomoOutput + result::Pyomo.Py + model::Pyomo.Py +end + +function MTK.get_U_values(output::PyomoOutput) + m = output.model + [[Pyomo.pyconvert(Float64, pyomo.value(m.U[i, t])) for i in m.u_idxs] for t in m.t] +end +function MTK.get_V_values(output::PyomoOutput) + m = output.model + [[Pyomo.pyconvert(Float64, pyomo.value(m.V[i, t])) for i in m.v_idxs] for t in m.t] +end +function MTK.get_t_values(output::PyomoOutput) + m = output.model + Pyomo.pyconvert(Float64, pyomo.value(m.tₛ)) * [Pyomo.pyconvert(Float64, t) for t in m.t] +end + +function MTK.objective_value(output::PyomoOutput) + Pyomo.pyconvert(Float64, pyomo.value(output.model.obj)) +end + +function MTK.successful_solve(output::PyomoOutput) + r = output.result + ss = r.solver.status + tc = r.solver.termination_condition + if Bool(ss == opt.SolverStatus.ok) && (Bool(tc == opt.TerminationCondition.optimal) || + Bool(tc == opt.TerminationCondition.locallyOptimal)) + return true + else + return false + end +end +end diff --git a/src/ModelingToolkit.jl b/src/ModelingToolkit.jl index 0e9962c241..255c5ade48 100644 --- a/src/ModelingToolkit.jl +++ b/src/ModelingToolkit.jl @@ -356,7 +356,9 @@ function FMIComponent end include("systems/optimal_control_interface.jl") export AbstractDynamicOptProblem, JuMPDynamicOptProblem, InfiniteOptDynamicOptProblem, - CasADiDynamicOptProblem + CasADiDynamicOptProblem, PyomoDynamicOptProblem +export AbstractCollocation, JuMPCollocation, InfiniteOptCollocation, + CasADiCollocation, PyomoCollocation export DynamicOptSolution @public apply_to_variables diff --git a/src/systems/optimal_control_interface.jl b/src/systems/optimal_control_interface.jl index 2b460f77a2..ea19afea86 100644 --- a/src/systems/optimal_control_interface.jl +++ b/src/systems/optimal_control_interface.jl @@ -1,6 +1,8 @@ abstract type AbstractDynamicOptProblem{uType, tType, isinplace} <: SciMLBase.AbstractODEProblem{uType, tType, isinplace} end +abstract type AbstractCollocation end + struct DynamicOptSolution model::Any sol::ODESolution @@ -16,15 +18,85 @@ function Base.show(io::IO, sol::DynamicOptSolution) print("\n\nPlease query the model using sol.model, the solution trajectory for the system using sol.sol, or the solution trajectory for the controllers using sol.input_sol.") end +""" + JuMPDynamicOptProblem(sys::System, u0, tspan, p; dt, steps, guesses, kwargs...) + +Convert an System representing an optimal control system into a JuMP model +for solving using optimization. Must provide either `dt`, the timestep between collocation +points (which, along with the timespan, determines the number of points), or directly +provide the number of points as `steps`. + +To construct the problem, please load InfiniteOpt along with ModelingToolkit. +""" function JuMPDynamicOptProblem end +""" + InfiniteOptDynamicOptProblem(sys::System, op, tspan; dt) + +Convert an System representing an optimal control system into a InfiniteOpt model +for solving using optimization. Must provide `dt` for determining the length +of the interpolation arrays. + +Related to `JuMPDynamicOptProblem`, but directly adds the differential equations +of the system as derivative constraints, rather than using a solver tableau. + +To construct the problem, please load InfiniteOpt along with ModelingToolkit. +""" function InfiniteOptDynamicOptProblem end +""" + CasADiDynamicOptProblem(sys::System, u0, tspan, p; dt, steps, guesses, kwargs...) + +Convert an System representing an optimal control system into a CasADi model +for solving using optimization. Must provide either `dt`, the timestep between collocation +points (which, along with the timespan, determines the number of points), or directly +provide the number of points as `steps`. + +To construct the problem, please load CasADi along with ModelingToolkit. +""" function CasADiDynamicOptProblem end +""" + PyomoDynamicOptProblem(sys::System, u0, tspan, p; dt, steps) -function warn_overdetermined(sys, u0map) +Convert an System representing an optimal control system into a Pyomo model +for solving using optimization. Must provide either `dt`, the timestep between collocation +points (which, along with the timespan, determines the number of points), or directly +provide the number of points as `steps`. + +To construct the problem, please load Pyomo along with ModelingToolkit. +""" +function PyomoDynamicOptProblem end + +### Collocations +""" +JuMP Collocation solver. +- solver: a optimization solver such as Ipopt +- tableau: An ODE RK tableau. Load a tableau by calling a function like `constructRK4` and may be found at https://docs.sciml.ai/DiffEqDevDocs/stable/internals/tableaus/. If this argument is not passed in, the solver will default to Radau second order. +""" +function JuMPCollocation end +""" +InfiniteOpt Collocation solver. +- solver: an optimization solver such as Ipopt +- `derivative_method`: the method used by InfiniteOpt to compute derivatives. The list of possible options can be found at https://infiniteopt.github.io/InfiniteOpt.jl/stable/guide/derivative/. Defaults to FiniteDifference(Backward()). +""" +function InfiniteOptCollocation end +""" +CasADi Collocation solver. +- solver: an optimization solver such as Ipopt. Should be given as a string or symbol in all lowercase, e.g. "ipopt" +- tableau: An ODE RK tableau. Load a tableau by calling a function like `constructRK4` and may be found at https://docs.sciml.ai/DiffEqDevDocs/stable/internals/tableaus/. If this argument is not passed in, the solver will default to Radau second order. +""" +function CasADiCollocation end +""" +Pyomo Collocation solver. +- solver: an optimization solver such as Ipopt. Should be given as a string or symbol in all lowercase, e.g. "ipopt" +- derivative_method: a derivative method from Pyomo. The choices here are ForwardEuler, BackwardEuler, MidpointEuler, LagrangeRadau, or LagrangeLegendre. The last two should additionally have a number indicating the number of collocation points per timestep, e.g. PyomoCollocation("ipopt", LagrangeRadau(3)). Defaults to LagrangeRadau(5). +""" +function PyomoCollocation end + +function warn_overdetermined(sys, op) cstrs = constraints(sys) + init_conds = filter(x -> value(x) ∈ Set(unknowns(sys)), [k for (k,v) in op]) if !isempty(cstrs) - (length(cstrs) + length(u0map) > length(unknowns(sys))) && - @warn "The control problem is overdetermined. The total number of conditions (# constraints + # fixed initial values given by u0map) exceeds the total number of states. The solvers will default to doing a nonlinear least-squares optimization." + (length(cstrs) + length(init_conds) > length(unknowns(sys))) && + @warn "The control problem is overdetermined. The total number of conditions (# constraints + # fixed initial values given by op) exceeds the total number of states. The solvers will default to doing a nonlinear least-squares optimization." end end @@ -131,6 +203,9 @@ end # returns the JuMP timespan, the number of steps, and whether it is a free time problem. function process_tspan(tspan, dt, steps) is_free_time = false + symbolic_type(tspan[1]) !== NotSymbolic() && + error("Free initial time problems are not currently supported by the collocation solvers.") + if isnothing(dt) && isnothing(steps) error("Must provide either the dt or the number of intervals to the collocation solvers (JuMP, InfiniteOpt, CasADi).") elseif symbolic_type(tspan[1]) === ScalarSymbolic() || @@ -140,11 +215,273 @@ function process_tspan(tspan, dt, steps) isnothing(dt) || @warn "Specified dt for free final time problem. This will be ignored; dt will be determined by the number of timesteps." - return steps, true + return (0, 1), steps, true else isnothing(steps) || @warn "Specified number of steps for problem with concrete tspan. This will be ignored; number of steps will be determined by dt." - return length(tspan[1]:dt:tspan[2]), false + return tspan, length(tspan[1]:dt:tspan[2]), false + end +end + +########################## +### MODEL CONSTRUCTION ### +########################## +function process_DynamicOptProblem(prob_type::Type{<:AbstractDynamicOptProblem}, model_type, sys::System, op, tspan; + dt = nothing, + steps = nothing, + guesses = Dict(), kwargs...) + + warn_overdetermined(sys, op) + ctrls = unbound_inputs(sys) + states = unknowns(sys) + + stidxmap = Dict([v => i for (i, v) in enumerate(states)]) + op = Dict([default_toterm(value(k)) => v for (k, v) in op]) + u0_idxs = has_alg_eqs(sys) ? collect(1:length(states)) : + [stidxmap[default_toterm(k)] for (k, v) in op if haskey(stidxmap, k)] + + _op = has_alg_eqs(sys) ? op : merge(Dict(op), Dict(guesses)) + f, u0, p = process_SciMLProblem(ODEInputFunction, sys, _op; + t = tspan !== nothing ? tspan[1] : tspan, kwargs...) + model_tspan, steps, is_free_t = process_tspan(tspan, dt, steps) + warn_overdetermined(sys, op) + + pmap = filter(p -> (first(p) ∉ Set(unknowns(sys))), op) + pmap = recursive_unwrap(AnyDict(pmap)) + evaluate_varmap!(pmap, keys(pmap)) + c0 = value.([pmap[c] for c in ctrls]) + + tsteps = LinRange(model_tspan[1], model_tspan[2], steps) + model = generate_internal_model(model_type) + generate_time_variable!(model, model_tspan, tsteps) + U = generate_state_variable!(model, u0, length(states), tsteps) + V = generate_input_variable!(model, c0, length(ctrls), tsteps) + tₛ = generate_timescale!(model, get(pmap, tspan[2], tspan[2]), is_free_t) + fullmodel = model_type(model, U, V, tₛ, is_free_t) + + set_variable_bounds!(fullmodel, sys, pmap, tspan[2]) + add_cost_function!(fullmodel, sys, tspan, pmap) + add_user_constraints!(fullmodel, sys, tspan, pmap) + add_initial_constraints!(fullmodel, u0, u0_idxs, model_tspan[1]) + + prob_type(f, u0, tspan, p, fullmodel, kwargs...), pmap +end + +function generate_time_variable! end +function generate_internal_model end +function generate_state_variable! end +function generate_input_variable! end +function generate_timescale! end +function add_initial_constraints! end +function add_constraint! end + +function set_variable_bounds!(m, sys, pmap, tf) + @unpack model, U, V, tₛ = m + t = get_iv(sys) + for (i, u) in enumerate(unknowns(sys)) + var = lowered_var(m, :U, i, t) + if hasbounds(u) + lo, hi = getbounds(u) + add_constraint!(m, var ≳ Symbolics.fixpoint_sub(lo, pmap)) + add_constraint!(m, var ≲ Symbolics.fixpoint_sub(hi, pmap)) + end + end + for (i, v) in enumerate(unbound_inputs(sys)) + var = lowered_var(m, :V, i, t) + if hasbounds(v) + lo, hi = getbounds(v) + add_constraint!(m, var ≳ Symbolics.fixpoint_sub(lo, pmap)) + add_constraint!(m, var ≲ Symbolics.fixpoint_sub(hi, pmap)) + end + end + if symbolic_type(tf) === ScalarSymbolic() && hasbounds(tf) + lo, hi = getbounds(tf) + set_lower_bound(tₛ, Symbolics.fixpoint_sub(lo, pmap)) + set_upper_bound(tₛ, Symbolics.fixpoint_sub(hi, pmap)) + end +end + +is_free_final(model) = model.is_free_final + +function add_cost_function!(model, sys, tspan, pmap) + jcosts = cost(sys) + if Symbolics._iszero(jcosts) + set_objective!(model, 0) + return + end + + jcosts = substitute_model_vars(model, sys, [jcosts], tspan) + jcosts = substitute_params(pmap, jcosts) + jcosts = substitute_integral(model, only(jcosts), tspan) + set_objective!(model, value(jcosts)) +end + +""" +Substitute integrals. For an integral from (ts, te): +- Free final time problems should transcribe this to (0, 1) in the case that (ts, te) is the original timespan. Free final time problems cannot handle partial timespans. +- CasADi cannot handle partial timespans, even for non-free-final time problems. +time problems and unchanged otherwise. +""" +function substitute_integral(model, expr, tspan) + intmap = Dict() + for int in collect_applied_operators(expr, Symbolics.Integral) + op = operation(int) + arg = only(arguments(value(int))) + lo, hi = value.((op.domain.domain.left, op.domain.domain.right)) + lo, hi = process_integral_bounds(model, (lo, hi), tspan) + intmap[int] = lowered_integral(model, arg, lo, hi) + end + Symbolics.substitute(expr, intmap) +end + +function process_integral_bounds(model, integral_span, tspan) + if is_free_final(model) && isequal(integral_span, tspan) + integral_span = (0, 1) + elseif is_free_final(model) + error("Free final time problems cannot handle partial timespans.") + else + (lo, hi) = integral_span + (lo < tspan[1] || hi > tspan[2]) && error("Integral bounds are beyond the timespan.") + integral_span + end +end + +"""Substitute variables like x(1.5), x(t), etc. with the corresponding model variables.""" +function substitute_model_vars(model, sys, exprs, tspan) + x_ops = [operation(unwrap(st)) for st in unknowns(sys)] + c_ops = [operation(unwrap(ct)) for ct in unbound_inputs(sys)] + t = get_iv(sys) + + exprs = map(c -> Symbolics.fast_substitute(c, whole_t_map(model, t, x_ops, c_ops)), exprs) + + (ti, tf) = tspan + if symbolic_type(tf) === ScalarSymbolic() + _tf = model.tₛ + ti + exprs = map(c -> Symbolics.fast_substitute(c, free_t_map(model, tf, x_ops, c_ops)), exprs) + exprs = map(c -> Symbolics.fast_substitute(c, Dict(tf => _tf)), exprs) + end + exprs = map(c -> Symbolics.fast_substitute(c, fixed_t_map(model, x_ops, c_ops)), exprs) + exprs +end + +"""Mappings for variables that depend on the final time parameter, x(tf).""" +function free_t_map(m, tf, x_ops, c_ops) + Dict([[x(tf) => lowered_var(m, :U, i, 1) for (i, x) in enumerate(x_ops)]; + [c(tf) => lowered_var(m, :V, i, 1) for (i, c) in enumerate(c_ops)]]) +end + +"""Mappings for variables that cover the whole timespan, x(t).""" +function whole_t_map(m, t, x_ops, c_ops) + Dict([[v(t) => lowered_var(m, :U, i, t) for (i, v) in enumerate(x_ops)]; + [v(t) => lowered_var(m, :V, i, t) for (i, v) in enumerate(c_ops)]]) +end + +"""Mappings for variables that cover the whole timespan, x(t).""" +function fixed_t_map(m, x_ops, c_ops) + Dict([[v => (t -> lowered_var(m, :U, i, t)) for (i, v) in enumerate(x_ops)]; + [v => (t -> lowered_var(m, :V, i, t)) for (i, v) in enumerate(c_ops)]]) +end + +function process_integral_bounds end +function lowered_integral end +function lowered_derivative end +function lowered_var end +function fixed_t_map end + +function add_user_constraints!(model, sys, tspan, pmap) + jconstraints = get_constraints(sys) + (isnothing(jconstraints) || isempty(jconstraints)) && return nothing + cons_dvs, cons_ps = process_constraint_system(jconstraints, Set(unknowns(sys)), parameters(sys), get_iv(sys); validate = false) + + is_free_final(model) && check_constraint_vars(cons_dvs) + + jconstraints = substitute_toterm(cons_dvs, jconstraints) + jconstraints = substitute_model_vars(model, sys, jconstraints, tspan) + jconstraints = substitute_params(pmap, jconstraints) + + for c in jconstraints + add_constraint!(model, c) + end +end + +function add_equational_constraints!(model, sys, pmap, tspan) + diff_eqs = substitute_model_vars(model, sys, diff_equations(sys), tspan) + diff_eqs = substitute_params(pmap, diff_eqs) + diff_eqs = substitute_differentials(model, sys, diff_eqs) + for eq in diff_eqs + add_constraint!(model, eq.lhs ~ eq.rhs * model.tₛ) + end + + alg_eqs = substitute_model_vars(model, sys, alg_equations(sys), tspan) + alg_eqs = substitute_params(pmap, alg_eqs) + for eq in alg_eqs + add_constraint!(model, eq.lhs ~ eq.rhs) + end +end + +function set_objective! end +objective_value(sol::DynamicOptSolution) = objective_value(sol.model) + +function substitute_differentials(model, sys, eqs) + t = get_iv(sys) + D = Differential(t) + diffsubmap = Dict([D(lowered_var(model, :U, i, t)) => lowered_derivative(model, i) for i in 1:length(unknowns(sys))]) + eqs = map(c -> Symbolics.substitute(c, diffsubmap), eqs) +end + +function substitute_toterm(vars, exprs) + toterm_map = Dict([u => default_toterm(value(u)) for u in vars]) + exprs = map(c -> Symbolics.fast_substitute(c, toterm_map), exprs) +end + +function substitute_params(pmap, exprs) + exprs = map(c -> Symbolics.fixpoint_sub(c, Dict(pmap)), exprs) +end + +function check_constraint_vars(vars) + for u in vars + x = operation(u) + t = only(arguments(u)) + if (symbolic_type(t) === NotSymbolic()) + error("Provided specific time constraint in a free final time problem. This is not supported by the collocation solvers at the moment. The offending variable is $u. Specific-time user constraints can only be specified at the end of the timespan.") + end + end +end + +######################## +### SOLVER UTILITIES ### +######################## +""" +Add the solve constraints, set the solver (Ipopt, e.g.) and solver options, optimize the model. +""" +function prepare_and_optimize! end +function get_t_values end +function get_U_values end +function get_V_values end +function successful_solve end + +""" + solve(prob::AbstractDynamicOptProblem, solver::AbstractCollocation; verbose = false, kwargs...) + +- kwargs are used for other options. For example, the `plugin_options` and `solver_options` will propagated to the Opti object in CasADi. +""" +function DiffEqBase.solve(prob::AbstractDynamicOptProblem, solver::AbstractCollocation; verbose = false, kwargs...) + solved_model = prepare_and_optimize!(prob, solver; verbose, kwargs...) + + ts = get_t_values(solved_model) + Us = get_U_values(solved_model) + Vs = get_V_values(solved_model) + is_free_final(prob.wrapped_model) && (ts .+ prob.tspan[1]) + + ode_sol = DiffEqBase.build_solution(prob, solver, ts, Us) + input_sol = isnothing(Vs) ? nothing : DiffEqBase.build_solution(prob, solver, ts, Vs) + + if !successful_solve(solved_model) + ode_sol = SciMLBase.solution_new_retcode( + ode_sol, SciMLBase.ReturnCode.ConvergenceFailure) + !isnothing(input_sol) && (input_sol = SciMLBase.solution_new_retcode( + input_sol, SciMLBase.ReturnCode.ConvergenceFailure)) end + DynamicOptSolution(solved_model, ode_sol, input_sol) end diff --git a/test/extensions/Project.toml b/test/extensions/Project.toml index 9f43e6f4a4..6a8d01a7b8 100644 --- a/test/extensions/Project.toml +++ b/test/extensions/Project.toml @@ -20,6 +20,7 @@ OrdinaryDiffEqNonlinearSolve = "127b3ac7-2247-4354-8eb6-78cf4e7c58e8" OrdinaryDiffEqSDIRK = "2d112036-d095-4a1e-ab9a-08536f3ecdbf" OrdinaryDiffEqTsit5 = "b1df2697-797e-41e3-8120-5422d3b24e4a" OrdinaryDiffEqVerner = "79d7bb75-1356-48c1-b8c0-6832512096c2" +Pyomo = "0e8e1daf-01b5-4eba-a626-3897743a3816" SciMLSensitivity = "1ed8b502-d754-442c-8d5d-10ac956f44a1" SciMLStructures = "53ae85a6-f571-4167-b2af-e1d143709226" SimpleDiffEq = "05bca326-078c-5bf0-a5bf-ce7c7982d7fd" diff --git a/test/extensions/dynamic_optimization.jl b/test/extensions/dynamic_optimization.jl index dd2368b558..3884bc1aa9 100644 --- a/test/extensions/dynamic_optimization.jl +++ b/test/extensions/dynamic_optimization.jl @@ -1,11 +1,12 @@ using ModelingToolkit -import JuMP, InfiniteOpt +import InfiniteOpt using DiffEqDevTools, DiffEqBase using SimpleDiffEq using OrdinaryDiffEqSDIRK, OrdinaryDiffEqVerner, OrdinaryDiffEqTsit5, OrdinaryDiffEqFIRK using Ipopt using DataInterpolations using CasADi +using Pyomo import DiffEqBase: solve const M = ModelingToolkit @@ -26,28 +27,27 @@ const M = ModelingToolkit parammap = [α => 1.5, β => 1.0, γ => 3.0, δ => 1.0] # Test explicit method. - jprob = JuMPDynamicOptProblem(sys, u0map, tspan, parammap, dt = 0.01) - @test JuMP.num_constraints(jprob.model) == 2 # initials - jsol = solve(jprob, Ipopt.Optimizer, constructRK4, silent = true) + jprob = JuMPDynamicOptProblem(sys, [u0map; parammap], tspan, dt = 0.01) + jsol = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructRK4())) oprob = ODEProblem(sys, [u0map; parammap], tspan) osol = solve(oprob, SimpleRK4(), dt = 0.01) - cprob = CasADiDynamicOptProblem(sys, u0map, tspan, parammap, dt = 0.01) - csol = solve(cprob, "ipopt", constructRK4) + cprob = CasADiDynamicOptProblem(sys, [u0map; parammap], tspan, dt = 0.01) + csol = solve(cprob, CasADiCollocation("ipopt", constructRK4())) @test jsol.sol.u ≈ osol.u @test csol.sol.u ≈ osol.u # Implicit method. - jsol2 = solve(jprob, Ipopt.Optimizer, constructImplicitEuler, silent = true) # 63.031 ms, 26.49 MiB - osol2 = solve(oprob, ImplicitEuler(), dt = 0.01, adaptive = false) # 129.375 μs, 61.91 KiB - jsol2 = solve(jprob, Ipopt.Optimizer, constructImplicitEuler, silent = true) # 63.031 ms, 26.49 MiB + osol2 = solve(oprob, ImplicitEuler(), dt = 0.01, adaptive = false) + jsol2 = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructImplicitEuler())) @test ≈(jsol2.sol.u, osol2.u, rtol = 0.001) - iprob = InfiniteOptDynamicOptProblem(sys, u0map, tspan, parammap, dt = 0.01) - isol = solve(iprob, Ipopt.Optimizer, - derivative_method = InfiniteOpt.FiniteDifference(InfiniteOpt.Backward()), - silent = true) # 11.540 ms, 4.00 MiB + iprob = InfiniteOptDynamicOptProblem(sys, [u0map; parammap], tspan, dt = 0.01) + isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer)) @test ≈(isol.sol.u, osol2.u, rtol = 0.001) - csol2 = solve(cprob, "ipopt", constructImplicitEuler, silent = true) + csol2 = solve(cprob, CasADiCollocation("ipopt", constructImplicitEuler())) @test ≈(csol2.sol.u, osol2.u, rtol = 0.001) + pprob = PyomoDynamicOptProblem(sys, [u0map; parammap], tspan, dt = 0.01) + psol = solve(pprob, PyomoCollocation("ipopt", BackwardEuler())) + @test all([≈(psol.sol(t), osol2(t), rtol = 1e-2) for t in 0.:0.01:1.]) # With a constraint u0map = Pair[] @@ -55,22 +55,26 @@ const M = ModelingToolkit constr = [x(0.6) ~ 3.5, x(0.3) ~ 7.0] @mtkcompile lksys = System(eqs, t; constraints = constr) - jprob = JuMPDynamicOptProblem(lksys, u0map, tspan, parammap; guesses = guess, dt = 0.01) - @test JuMP.num_constraints(jprob.model) == 2 - jsol = solve(jprob, Ipopt.Optimizer, constructTsitouras5, silent = true) # 12.190 s, 9.68 GiB + jprob = JuMPDynamicOptProblem(lksys, [u0map; parammap], tspan; guesses = guess, dt = 0.01) + jsol = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructTsitouras5())) @test jsol.sol(0.6; idxs = x(t)) ≈ 3.5 @test jsol.sol(0.3; idxs = x(t)) ≈ 7.0 cprob = CasADiDynamicOptProblem( - lksys, u0map, tspan, parammap; guesses = guess, dt = 0.01) - csol = solve(cprob, "ipopt", constructTsitouras5, silent = true) + lksys, [u0map; parammap], tspan; guesses = guess, dt = 0.01) + csol = solve(cprob, CasADiCollocation("ipopt", constructTsitouras5())) @test csol.sol(0.6; idxs = x(t)) ≈ 3.5 @test csol.sol(0.3; idxs = x(t)) ≈ 7.0 + pprob = PyomoDynamicOptProblem( + lksys, [u0map; parammap], tspan; guesses = guess, dt = 0.01) + psol = solve(pprob, PyomoCollocation("ipopt", LagrangeLegendre(3))) + @test psol.sol(0.6; idxs = x(t)) ≈ 3.5 + @test psol.sol(0.3; idxs = x(t)) ≈ 7.0 + iprob = InfiniteOptDynamicOptProblem( - lksys, u0map, tspan, parammap; guesses = guess, dt = 0.01) - isol = solve(iprob, Ipopt.Optimizer, - derivative_method = InfiniteOpt.OrthogonalCollocation(3), silent = true) # 48.564 ms, 9.58 MiB + lksys, [u0map; parammap], tspan; guesses = guess, dt = 0.01) + isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer, InfiniteOpt.OrthogonalCollocation(3))) # 48.564 ms, 9.58 MiB sol = isol.sol @test sol(0.6; idxs = x(t)) ≈ 3.5 @test sol(0.3; idxs = x(t)) ≈ 7.0 @@ -79,18 +83,21 @@ const M = ModelingToolkit constr = [x(t) ≳ 1, y(t) ≳ 1] @mtkcompile lksys = System(eqs, t; constraints = constr) iprob = InfiniteOptDynamicOptProblem( - lksys, u0map, tspan, parammap; guesses = guess, dt = 0.01) - isol = solve(iprob, Ipopt.Optimizer, - derivative_method = InfiniteOpt.OrthogonalCollocation(3), silent = true) + lksys, [u0map; parammap], tspan; guesses = guess, dt = 0.01) + isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer, InfiniteOpt.OrthogonalCollocation(3))) @test all(u -> u > [1, 1], isol.sol.u) - jprob = JuMPDynamicOptProblem(lksys, u0map, tspan, parammap; guesses = guess, dt = 0.01) - jsol = solve(jprob, Ipopt.Optimizer, constructRadauIA3, silent = true) # 12.190 s, 9.68 GiB + jprob = JuMPDynamicOptProblem(lksys, [u0map; parammap], tspan; guesses = guess, dt = 0.01) + jsol = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructRadauIA3())) @test all(u -> u > [1, 1], jsol.sol.u) + pprob = PyomoDynamicOptProblem(lksys, [u0map; parammap], tspan; guesses = guess, dt = 0.01) + psol = solve(pprob, PyomoCollocation("ipopt", MidpointEuler())) + @test all(u -> u > [1, 1], psol.sol.u) + cprob = CasADiDynamicOptProblem( - lksys, u0map, tspan, parammap; guesses = guess, dt = 0.01) - csol = solve(cprob, "ipopt", constructRadauIA3, silent = true) + lksys, [u0map; parammap], tspan; guesses = guess, dt = 0.01) + csol = solve(cprob, CasADiCollocation("ipopt", constructRadauIA3())) @test all(u -> u > [1, 1], csol.sol.u) end @@ -103,16 +110,16 @@ function is_bangbang(input_sol, lbounds, ubounds, rtol = 1e-4) end function ctrl_to_spline(inputsol, splineType) - us = reduce(vcat, inputsol.u) - ts = reduce(vcat, inputsol.t) - splineType(us, ts) + us = reduce(vcat, inputsol.u) + ts = reduce(vcat, inputsol.t) + splineType(us, ts) end @testset "Linear systems" begin # Double integrator t = M.t_nounits D = M.D_nounits - @variables x(..) [bounds = (0.0, 0.25)] v(..) + @variables x(..) v(..) @variables u(..) [bounds = (-1.0, 1.0), input = true] constr = [v(1.0) ~ 0.0] cost = [-x(1.0)] # Maximize the final distance. @@ -123,16 +130,15 @@ end u0map = [x(t) => 0.0, v(t) => 0.0] tspan = (0.0, 1.0) parammap = [u(t) => 0.0] - jprob = JuMPDynamicOptProblem(block, u0map, tspan, parammap; dt = 0.01) - jsol = solve(jprob, Ipopt.Optimizer, constructVerner8, silent = true) + jprob = JuMPDynamicOptProblem(block, [u0map; parammap], tspan; dt = 0.01) + jsol = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructVerner8()), verbose = true) # Linear systems have bang-bang controls @test is_bangbang(jsol.input_sol, [-1.0], [1.0]) # Test reached final position. @test ≈(jsol.sol[x(t)][end], 0.25, rtol = 1e-5) - cprob = CasADiDynamicOptProblem(block, u0map, tspan, parammap; dt = 0.01) - csol = solve(cprob, "ipopt", constructVerner8, silent = true) - # Linear systems have bang-bang controls + cprob = CasADiDynamicOptProblem(block, [u0map; parammap], tspan; dt = 0.01) + csol = solve(cprob, CasADiCollocation("ipopt", constructVerner8())) @test is_bangbang(csol.input_sol, [-1.0], [1.0]) # Test reached final position. @test ≈(csol.sol[x(t)][end], 0.25, rtol = 1e-5) @@ -146,12 +152,20 @@ end @test ≈(jsol.sol.u, osol.u, rtol = 0.05) @test ≈(csol.sol.u, osol.u, rtol = 0.05) - iprob = InfiniteOptDynamicOptProblem(block, u0map, tspan, parammap; dt = 0.01) - isol = solve(iprob, Ipopt.Optimizer; silent = true) + iprob = InfiniteOptDynamicOptProblem(block, [u0map; parammap], tspan; dt = 0.01) + isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer)) @test is_bangbang(isol.input_sol, [-1.0], [1.0]) @test ≈(isol.sol[x(t)][end], 0.25, rtol = 1e-5) - osol = solve(oprob, ImplicitEuler(); dt = 0.01, adaptive = false) + + pprob = PyomoDynamicOptProblem(block, [u0map; parammap], tspan; dt = 0.01) + psol = solve(pprob, PyomoCollocation("ipopt", BackwardEuler())) + @test is_bangbang(psol.input_sol, [-1.0], [1.0]) + @test ≈(psol.sol[x(t)][end], 0.25, rtol = 1e-3) + + spline = ctrl_to_spline(isol.input_sol, ConstantInterpolation) + oprob = ODEProblem(block_ode, [u0map; u_interp => spline], tspan) @test ≈(isol.sol.u, osol.u, rtol = 0.05) + @test all([≈(psol.sol(t), osol(t), rtol = 0.05) for t in 0.:0.01:1.]) ################### ### Bee example ### @@ -170,15 +184,18 @@ end u0map = [w(t) => 40, q(t) => 2] pmap = [b => 1, c => 1, μ => 1, s => 1, ν => 1, α => 1] - jprob = JuMPDynamicOptProblem(beesys, u0map, tspan, pmap, dt = 0.01) - jsol = solve(jprob, Ipopt.Optimizer, constructTsitouras5, silent = true) + jprob = JuMPDynamicOptProblem(beesys, [u0map; pmap], tspan, dt = 0.01) + jsol = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructTsitouras5())) @test is_bangbang(jsol.input_sol, [0.0], [1.0]) - iprob = InfiniteOptDynamicOptProblem(beesys, u0map, tspan, pmap, dt = 0.01) - isol = solve(iprob, Ipopt.Optimizer; silent = true) + iprob = InfiniteOptDynamicOptProblem(beesys, [u0map; pmap], tspan, dt = 0.01) + isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer)) @test is_bangbang(isol.input_sol, [0.0], [1.0]) - cprob = CasADiDynamicOptProblem(beesys, u0map, tspan, pmap; dt = 0.01) - csol = solve(cprob, "ipopt", constructTsitouras5, silent = true) + cprob = CasADiDynamicOptProblem(beesys, [u0map; pmap], tspan; dt = 0.01) + csol = solve(cprob, CasADiCollocation("ipopt", constructTsitouras5())) @test is_bangbang(csol.input_sol, [0.0], [1.0]) + pprob = PyomoDynamicOptProblem(beesys, [u0map; pmap], tspan, dt = 0.01) + psol = solve(pprob, PyomoCollocation("ipopt", BackwardEuler())) + @test is_bangbang(psol.input_sol, [0.0], [1.0]) @parameters (α_interp::LinearInterpolation)(..) eqs = [D(w(t)) ~ -μ * w(t) + b * s * α_interp(t) * w(t), @@ -193,6 +210,7 @@ end @test ≈(osol.u, csol.sol.u, rtol = 0.01) osol2 = solve(oprob, ImplicitEuler(); dt = 0.01, adaptive = false) @test ≈(osol2.u, isol.sol.u, rtol = 0.01) + @test all([≈(psol.sol(t), osol2(t), rtol = 0.01) for t in 0.:0.01:4.]) end @testset "Rocket launch" begin @@ -218,19 +236,22 @@ end pmap = [ g₀ => 1, m₀ => 1.0, h_c => 500, c => 0.5 * √(g₀ * h₀), D_c => 0.5 * 620 * m₀ / g₀, Tₘ => 3.5 * g₀ * m₀, T(t) => 0.0, h₀ => 1, m_c => 0.6] - jprob = JuMPDynamicOptProblem(rocket, u0map, (ts, te), pmap; dt = 0.001, cse = false) - jsol = solve(jprob, Ipopt.Optimizer, constructRadauIIA5, silent = true) + jprob = JuMPDynamicOptProblem(rocket, [u0map; pmap], (ts, te); dt = 0.001, cse = false) + jsol = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructRadauIIA5())) @test jsol.sol[h(t)][end] > 1.012 - cprob = CasADiDynamicOptProblem(rocket, u0map, (ts, te), pmap; dt = 0.001, cse = false) - csol = solve(cprob, "ipopt"; silent = true) + cprob = CasADiDynamicOptProblem(rocket, [u0map; pmap], (ts, te); dt = 0.001, cse = false) + csol = solve(cprob, CasADiCollocation("ipopt")) @test csol.sol[h(t)][end] > 1.012 - iprob = InfiniteOptDynamicOptProblem( - rocket, u0map, (ts, te), pmap; dt = 0.001) - isol = solve(iprob, Ipopt.Optimizer, silent = true) + iprob = InfiniteOptDynamicOptProblem(rocket, [u0map; pmap], (ts, te); dt = 0.001) + isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer)) @test isol.sol[h(t)][end] > 1.012 + pprob = PyomoDynamicOptProblem(rocket, [u0map; pmap], (ts, te); dt = 0.001, cse = false) + psol = solve(pprob, PyomoCollocation("ipopt", LagrangeRadau(4))) + @test psol.sol[h(t)][end] > 1.012 + # Test solution @parameters (T_interp::CubicSpline)(..) eqs = [D(h(t)) ~ v(t), @@ -247,6 +268,11 @@ end oprob1 = ODEProblem(rocket_ode, merge(Dict(u0map), Dict(pmap), interpmap1), (ts, te)) osol1 = solve(oprob1, ImplicitEuler(); adaptive = false, dt = 0.001) @test ≈(isol.sol.u, osol1.u, rtol = 0.01) + + interpmap2 = Dict(T_interp => ctrl_to_spline(psol.input_sol, CubicSpline)) + oprob2 = ODEProblem(rocket_ode, merge(Dict(u0map), Dict(pmap), interpmap2), (ts, te)) + osol2 = solve(oprob2, RadauIIA5(); adaptive = false, dt = 0.001) + @test all([≈(psol.sol(t), osol2(t), rtol = 0.01) for t in 0:0.001:0.2]) end @testset "Free final time problems" begin @@ -264,17 +290,25 @@ end u0map = [x(t) => 17.5] pmap = [u(t) => 0.0, tf => 8] - jprob = JuMPDynamicOptProblem(rocket, u0map, (0, tf), pmap; steps = 201) - jsol = solve(jprob, Ipopt.Optimizer, constructTsitouras5, silent = true) + jprob = JuMPDynamicOptProblem(rocket, [u0map; pmap], (0, tf); steps = 201) + jsol = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructTsitouras5())) @test isapprox(jsol.sol.t[end], 10.0, rtol = 1e-3) + @test ≈(M.objective_value(jsol), -92.75, atol = 0.25) - cprob = CasADiDynamicOptProblem(rocket, u0map, (0, tf), pmap; steps = 201) - csol = solve(cprob, "ipopt", constructTsitouras5, silent = true) + cprob = CasADiDynamicOptProblem(rocket, [u0map; pmap], (0, tf); steps = 201) + csol = solve(cprob, CasADiCollocation("ipopt", constructTsitouras5())) @test isapprox(csol.sol.t[end], 10.0, rtol = 1e-3) + @test ≈(M.objective_value(csol), -92.75, atol = 0.25) - iprob = InfiniteOptDynamicOptProblem(rocket, u0map, (0, tf), pmap; steps = 200) - isol = solve(iprob, Ipopt.Optimizer, silent = true) + iprob = InfiniteOptDynamicOptProblem(rocket, [u0map; pmap], (0, tf); steps = 200) + isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer)) @test isapprox(isol.sol.t[end], 10.0, rtol = 1e-3) + @test ≈(M.objective_value(isol), -92.75, atol = 0.25) + + pprob = PyomoDynamicOptProblem(rocket, [u0map; pmap], (0, tf); steps = 201) + psol = solve(pprob, PyomoCollocation("ipopt", BackwardEuler())) + @test isapprox(psol.sol.t[end], 10.0, rtol = 1e-3) + @test ≈(M.objective_value(psol), -92.75, atol = 0.1) @variables x(..) v(..) @variables u(..) [bounds = (-1.0, 1.0), input = true] @@ -288,18 +322,22 @@ end u0map = [x(t) => 1.0, v(t) => 0.0] tspan = (0.0, tf) - parammap = [u(t) => 0.0, tf => 1.0] - jprob = JuMPDynamicOptProblem(block, u0map, tspan, parammap; steps = 51) - jsol = solve(jprob, Ipopt.Optimizer, constructVerner8, silent = true) + parammap = [u(t) => 1.0, tf => 1.0] + jprob = JuMPDynamicOptProblem(block, [u0map; parammap], tspan; steps = 51) + jsol = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructVerner8())) @test isapprox(jsol.sol.t[end], 2.0, atol = 1e-5) - cprob = CasADiDynamicOptProblem(block, u0map, (0, tf), parammap; steps = 51) - csol = solve(cprob, "ipopt", constructVerner8, silent = true) + cprob = CasADiDynamicOptProblem(block, [u0map; parammap], (0, tf); steps = 51) + csol = solve(cprob, CasADiCollocation("ipopt", constructVerner8())) @test isapprox(csol.sol.t[end], 2.0, atol = 1e-5) - iprob = InfiniteOptDynamicOptProblem(block, u0map, tspan, parammap; steps = 51) - isol = solve(iprob, Ipopt.Optimizer, silent = true) + iprob = InfiniteOptDynamicOptProblem(block, [u0map; parammap], tspan; steps = 51) + isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer), verbose = true) @test isapprox(isol.sol.t[end], 2.0, atol = 1e-5) + + pprob = PyomoDynamicOptProblem(block, [u0map; parammap], tspan; steps = 51) + psol = solve(pprob, PyomoCollocation("ipopt", BackwardEuler())) + @test isapprox(psol.sol.t[end], 2.0, atol = 1e-5) end @testset "Cart-pole problem" begin @@ -331,15 +369,19 @@ end u0map = [D(x(t)) => 0.0, D(θ(t)) => 0.0, θ(t) => 0.0, x(t) => 0.0] pmap = [mₖ => 1.0, mₚ => 0.2, l => 0.5, g => 9.81, u => 0] - jprob = JuMPDynamicOptProblem(cartpole, u0map, tspan, pmap; dt = 0.04) - jsol = solve(jprob, Ipopt.Optimizer, constructRK4, silent = true) + jprob = JuMPDynamicOptProblem(cartpole, [u0map; pmap], tspan; dt = 0.04) + jsol = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructRK4())) @test jsol.sol.u[end] ≈ [π, 0, 0, 0] - cprob = CasADiDynamicOptProblem(cartpole, u0map, tspan, pmap; dt = 0.04) - csol = solve(cprob, "ipopt", constructRK4, silent = true) + cprob = CasADiDynamicOptProblem(cartpole, [u0map; pmap], tspan; dt = 0.04) + csol = solve(cprob, CasADiCollocation("ipopt", constructRK4())) @test csol.sol.u[end] ≈ [π, 0, 0, 0] - iprob = InfiniteOptDynamicOptProblem(cartpole, u0map, tspan, pmap; dt = 0.04) - isol = solve(iprob, Ipopt.Optimizer, silent = true) + iprob = InfiniteOptDynamicOptProblem(cartpole, [u0map; pmap], tspan; dt = 0.04) + isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer, InfiniteOpt.OrthogonalCollocation(2))) @test isol.sol.u[end] ≈ [π, 0, 0, 0] + + pprob = PyomoDynamicOptProblem(cartpole, [u0map; pmap], tspan; dt = 0.04) + psol = solve(pprob, PyomoCollocation("ipopt", LagrangeLegendre(4))) + @test psol.sol.u[end] ≈ [π, 0, 0, 0] end