docs: dynamic optimization docs

Author: vyudu

docs/src/tutorials/dynamic_optimization.md

@@ -0,0 +1,105 @@
+# 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 and velocity.
+```julia
+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 = ODESystem(eqs, t; costs, constraints = cons)
+rocket, input_idxs = structural_simplify(rocket, ([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.
+```julia
+jprob = JuMPDynamicOptProblem(rocket, u0map, (ts, te), pmap; dt = 0.001, cse = false)
+jsol = solve(jprob, JuMPCollocation(Ipopt.Optimizer, constructRadauIIA5()))
+```
+
+Let's plot our final solution and the controller here:
+```julia
+```
+
+###### 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.
+
+```julia
+@variables x(..) v(..)
+@variables u(..) [bounds = (-1.0, 1.0), input = true]
+@parameters tf
+
+constr = [v(tf) ~ 0, x(tf) ~ 0]
+cost = [tf] # Minimize time
+
+@named block = ODESystem(
+    [D(x(t)) ~ v(t), D(v(t)) ~ u(t)], t; costs = cost, constraints = constr)
+
+block, input_idxs = structural_simplify(block, ([u(t)], []))
+
+u0map = [x(t) => 1.0, v(t) => 0.0]
+tspan = (0.0, tf)
+parammap = [u(t) => 0.0, tf => 1.0]
+```
+
+Please note that, at the moment, free final time problems cannot support constraints defined at definite time values, like `x(3) ~ 2`.
+
+Let's plot our final solution and the controller for the block:
+```julia
+```
+
+### 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 certain solvers, 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. 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 [here](). If none is passed in, the solver defaults to a `LagrangeRadau(3)`.
+
+```@docs; canonical = false
+JuMPCollocation
+InfiniteOptCollocation
+CasADiCollocation
+PyomoCollocation
+solve(::AbstractDynamicOptProblem)
+```
+
+### Problem constructors
+```@docs; canonical = false
+JuMPDynamicOptProblem
+InfiniteOptDynamicOptProblem
+CasADiDynamicOptProblem
+PyomoDynamicOptProblem
+```

ext/MTKCasADiDynamicOptExt.jl

@@ -120,7 +120,20 @@ function MTK.lowered_var(m::CasADiModel, uv, i, t)
     t isa Union{Num, Symbolics.Symbolic} ? X.u[i, :] : X(t)[i]
 end
 
-MTK.lowered_integral(model::CasADiModel, expr, args...) = model.tₛ * (model.U.t[2] - model.U.t[1]) * sum(expr)
+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
+    end
+    model.tₛ * total
+end
 
 function add_solve_constraints!(prob::CasADiDynamicOptProblem, tableau)
     @unpack A, α, c = tableau
@@ -210,6 +223,7 @@ 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))
 
 function MTK.successful_solve(m::CasADiModel) 
     isnothing(m.solver_opti) && return false

ext/MTKInfiniteOptExt.jl

@@ -91,7 +91,7 @@ end
 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])
 
-function MTK.process_integral_bounds(model, integral_span, tspan)
+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)
@@ -213,6 +213,7 @@ function MTK.get_U_values(m::InfiniteModel)
     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)

ext/MTKPyomoDynamicOptExt.jl

@@ -8,16 +8,13 @@ using Setfield
 const MTK = ModelingToolkit
 
 SPECIAL_FUNCTIONS_DICT = Dict([acos => Pyomo.py_acos,
-                               log1p => Pyomo.py_log1p,
                                acosh => Pyomo.py_acosh,
-                               log2 => Pyomo.py_log2,
                                asin => Pyomo.py_asin,
                                tan => Pyomo.py_tan,
                                atanh => Pyomo.py_atanh,
                                cos => Pyomo.py_cos,
                                log => Pyomo.py_log,
                                sin => Pyomo.py_sin,
-                               log10 => Pyomo.py_log10,
                                sqrt => Pyomo.py_sqrt,
                                exp => Pyomo.py_exp])
 
@@ -36,10 +33,21 @@ struct PyomoDynamicOptModel
     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)
+        #add_time_equation!(model, MODEL_SYM, T_SYM)
         new(model, U, V, tₛ, is_free_final, nothing, PyomoVar(model.dU), MODEL_SYM, T_SYM, DUMMY_SYM)
     end
 end
 
+function add_time_equation!(model::ConcreteModel, model_sym, t_sym)
+    model.dtime = dae.DerivativeVar(model.time)
+
+    mdt = Symbolics.value(pysym_getproperty(model_sym, :dtime))
+    mts = Symbolics.value(pysym_getproperty(model_sym, :tₛ))
+    expr = mdt[t_sym] - mts == 0
+    f = Pyomo.pyfunc(eval(Symbolics.build_function(expr, model_sym, t_sym)))
+    model.time_eq = pyomo.Constraint(model.t, rule = f)
+end
+
 struct PyomoDynamicOptProblem{uType, tType, isinplace, P, F, K} <:
        AbstractDynamicOptProblem{uType, tType, isinplace}
     f::F
@@ -72,6 +80,7 @@ MTK.generate_internal_model(m::Type{PyomoDynamicOptModel}) = ConcreteModel(pyomo
 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) 
@@ -116,7 +125,6 @@ end
 
 function MTK.set_objective!(pmodel::PyomoDynamicOptModel, expr)
     @unpack model, model_sym, t_sym, dummy_sym = pmodel
-    @show expr
     expr = Symbolics.substitute(expr, SPECIAL_FUNCTIONS_DICT)
     if occursin(Symbolics.unwrap(t_sym), expr)
         f = eval(Symbolics.build_function(expr, model_sym, t_sym))
@@ -134,15 +142,24 @@ function MTK.add_initial_constraints!(model::PyomoDynamicOptModel, u0, u0_idxs,
 end
 
 function MTK.lowered_integral(m::PyomoDynamicOptModel, arg, lo, hi)
-    @unpack model, model_sym, t_sym = m
-    arg_f = eval(Symbolics.build_function(arg, model_sym, t_sym))
-    int_sym = Symbol(:int, hash(arg))
-    setproperty!(model, int_sym, dae.Integral(model.t, wrt = model.t, rule=Pyomo.pyfunc(arg_f)))
-    PyomoVar(model.tₛ * model.int_sym)
+    @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
 
-MTK.process_integral_bounds(model, integral_span, tspan) = integral_span
-
 function MTK.lowered_derivative(m::PyomoDynamicOptModel, i)
     mdU = Symbolics.value(pysym_getproperty(m.model_sym, :dU))
     Symbolics.unwrap(mdU[i, m.t_sym])
@@ -167,6 +184,7 @@ function MTK.prepare_and_optimize!(prob::PyomoDynamicOptProblem, collocation; ve
     discretizer = TransformationFactory(dm)
     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)
@@ -190,10 +208,11 @@ function MTK.get_t_values(output::PyomoOutput)
     Pyomo.pyconvert(Float64, pyomo.value(m.tₛ)) * [Pyomo.pyconvert(Float64, t) for t in m.t]
 end
 
+MTK.objective_value(output::PyomoOutput) = Pyomo.pyconvert(Float64, pyomo.value(output.model.obj))
+
 function MTK.successful_solve(output::PyomoOutput)
     r = output.result
     ss = r.solver.status
-    Main.xx[] = ss
     tc = r.solver.termination_condition
     if Bool(ss == opt.SolverStatus.ok) && (Bool(tc == opt.TerminationCondition.optimal) || Bool(tc == opt.TerminationCondition.locallyOptimal))
         return true

src/systems/optimal_control_interface.jl

@@ -331,6 +331,18 @@ function substitute_integral(model, exprs, tspan)
     exprs = map(c -> Symbolics.substitute(c, intmap), value.(exprs))
 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)]
@@ -405,6 +417,7 @@ function add_equational_constraints!(model, sys, pmap, tspan)
 end
 
 function set_objective! end
+objective_value(sol::DynamicOptSolution) = objective_value(sol.model) 
 
 function substitute_differentials(model, sys, eqs)
     t = get_iv(sys)

test/extensions/dynamic_optimization.jl

@@ -47,7 +47,7 @@ const M = ModelingToolkit
     @test ≈(csol2.sol.u, osol2.u, rtol = 0.001)
     pprob = PyomoDynamicOptProblem(sys, u0map, tspan, parammap, dt = 0.01)
     psol = solve(pprob, PyomoCollocation("ipopt", BackwardEuler()))
-    @test all([≈(psol.sol(t), osol2(t), rtol = 1e-3) for t in 0.:0.01:1.])
+    @test all([≈(psol.sol(t), osol2(t), rtol = 1e-2) for t in 0.:0.01:1.])
 
     # With a constraint
     u0map = Pair[]
@@ -111,9 +111,9 @@ 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
@@ -163,7 +163,8 @@ end
     @test is_bangbang(psol.input_sol, [-1.0], [1.0])
     @test ≈(psol.sol.u[end][2], 0.25, rtol = 1e-3)
 
-    osol = solve(oprob, ImplicitEuler(); dt = 0.01, adaptive = false)
+    spline = ctrl_to_spline(isol.input_sol, ConstantInterpolation)
+    oprob = ODEProblem(block_ode, u0map, tspan, [u_interp => spline])
     @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.])
 
@@ -250,7 +251,7 @@ end
     @test isol.sol[h(t)][end] > 1.012
 
     pprob = PyomoDynamicOptProblem(rocket, u0map, (ts, te), pmap; dt = 0.001, cse = false)
-    psol = solve(pprob, PyomoCollocation("ipopt", MidpointEuler()))
+    psol = solve(pprob, PyomoCollocation("ipopt", LagrangeRadau(4)))
     @test psol.sol.u[end][1] > 1.012
 
     # Test solution
@@ -273,7 +274,7 @@ end
     interpmap2 = Dict(T_interp => ctrl_to_spline(psol.input_sol, CubicSpline))
     oprob2 = ODEProblem(rocket_ode, u0map, (ts, te), merge(Dict(pmap), interpmap2))
     osol2 = solve(oprob2, RadauIIA5(); adaptive = false, dt = 0.001)
-    @test ≈(psol.sol.u, osol2.u, rtol = 0.01)
+    @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
@@ -294,18 +295,22 @@ end
     jprob = JuMPDynamicOptProblem(rocket, u0map, (0, tf), pmap; 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, 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, 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, (0, tf), pmap; steps = 201)
-    psol = solve(pprob, PyomoCollocation("ipopt"))
+    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]
@@ -375,7 +380,7 @@ end
     @test csol.sol.u[end] ≈ [π, 0, 0, 0]
 
     iprob = InfiniteOptDynamicOptProblem(cartpole, u0map, tspan, pmap; dt = 0.04)
-    isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer))
+    isol = solve(iprob, InfiniteOptCollocation(Ipopt.Optimizer, InfiniteOpt.OrthogonalCollocation(2)))
     @test isol.sol.u[end] ≈ [π, 0, 0, 0]
 
     pprob = PyomoDynamicOptProblem(cartpole, u0map, tspan, pmap; dt = 0.04)