From e9a8294cb69240ce9ddba918964efd02074dbf75 Mon Sep 17 00:00:00 2001 From: Pierre Martinon Date: Tue, 30 Apr 2024 13:47:30 +0200 Subject: [PATCH] discrete continuation for parametric ocp, rho and final times (#85) * discrete continuation for parametric ocp, rho and final times * allow to pass solution directly as initial guess for solve * added CI test for continuation * updated tutorial for continuation --- docs/src/continuation.md | 126 ++++++++++++-------- src/solve.jl | 9 +- test/Project.toml | 1 + test/suite/abstract_ocp.jl | 2 +- test/suite/continuation.jl | 119 +++++++++++++++++++ test/test_continuation.jl | 231 ++++++++++++++++++++++++------------- 6 files changed, 358 insertions(+), 130 deletions(-) create mode 100644 test/suite/continuation.jl diff --git a/docs/src/continuation.md b/docs/src/continuation.md index f11e8c5..555a9c3 100644 --- a/docs/src/continuation.md +++ b/docs/src/continuation.md @@ -1,8 +1,12 @@ # Discrete continuation Using the warm start option, it is easy to implement a basic discrete continuation method, where a sequence of problems is solved using each solution as initial guess for the next problem. -This usually gives better and faster convergence than solving each problem with the same initial guess. -We illustrate this on the Goddard problem already presented in the tutorial. +This usually gives better and faster convergence than solving each problem with the same initial guess, and is a way to handle problems that require a good initial guess. + + +## Continuation on parametric OCP + +The most compact syntax to perform a discrete continuation is to use a function that returns the OCP for a given value of the continuation parameter, and solve a sequence of these problems. We illustrate this on a very basic double integrator with increasing fixed final time. ```@setup main using Printf @@ -10,10 +14,53 @@ using Statistics using Plots ``` +First we write a function that returns the OCP for a given final time. + ```@example main using CTDirect using CTBase +function ocp_T(T) + @def ocp begin + t ∈ [ 0, T ], time + x ∈ R², state + u ∈ R, control + q = x₁ + v = x₂ + q(0) == 0 + v(0) == 0 + q(T) == 1 + v(T) == 0 + ẋ(t) == [ v(t), u(t) ] + ∫(u(t)^2) → min + end + return ocp +end +nothing # hide +``` + +Then we perform the continuation with a simple *for* loop, using each solution to initialize the next problem. + +```@example main +init1 = OptimalControlInit() +iter_list = [] +for T=1:5 + ocp1 = ocp_T(T) + sol1 = solve(ocp1, print_level=0, init=init1) + global init1 = OptimalControlInit(sol1) + @printf("T %.2f objective %.6f iterations %d\n", T, sol1.objective, sol1.iterations) + push!(iter_list, sol1.iterations) +end +``` + +## Continuation on global variable + +As a second example, we show how to avoid redefining a new OCP each time, and modify the original one instead. +More precisely we now solve a Goddard problem for a decreasing maximal thrust. If we store the value for *Tmax* in a global variable, we can simply modify this variable and keep the same OCP problem during the continuation. + +Let us first define the Goddard problem (note that the formulation below illustrates all the possible constraints types, and the problem could be defined in a more compact way). + +```@example main Cd = 310 Tmax = 3.5 β = 500 @@ -47,10 +94,38 @@ sol = sol0 @printf("Objective for reference solution %.6f\n", sol0.objective) ``` -## Continuation on speed constraint +Then we perform the continuation on the maximal thrust. + +```@example main +Tmax_list = [] +obj_list = [] +for Tmax_local=3.5:-0.5:1 + global Tmax = Tmax_local + global sol = solve(ocp, print_level=0, init=OptimalControlInit(sol)) + @printf("Tmax %.2f objective %.6f iterations %d\n", Tmax, sol.objective, sol.iterations) + push!(Tmax_list, Tmax) + push!(obj_list, sol.objective) +end +``` + +We plot now the objective w.r.t the maximal thrust, as well as both solutions for *Tmax*=3.5 and *Tmax*=1. + +```@example main +pobj = plot(Tmax_list, obj_list, label="r(tf)",seriestype=:scatter) +xlabel!("Maximal thrust (Tmax)") +ylabel!("Maximal altitude r(tf)") +plot(sol0) +p = plot!(sol) +plot(pobj, p, layout=2) +``` + -Let us solve a sequence of problems with an increasingly strict constraint on the maximal speed. +## Manual constraint redefinition + +Here we illustrate a slightly more involved way of modifying the OCP problem during the continuation. +Instead of just updating a global variable as before, we now remove and redefine one of the constraints (maximal speed). ```@example main +global Tmax = 3.5 vmax_list = [] obj_list = [] iter_list = [] @@ -67,7 +142,7 @@ end @printf("\nAverage iterations %d\n", mean(iter_list)) ``` -We now plot the objective with respect to the speed limit, as well as a comparison of the solutions for the unconstrained case and the vmax=0.05 case. +We now plot the objective with respect to the speed limit, as well as a comparison of the solutions for the unconstrained case and the *vmax*=0.05 case. ```@example main pobj = plot(vmax_list, obj_list, label="r(tf)",seriestype=:scatter) @@ -90,43 +165,4 @@ for vmax=0.15:-0.01:0.05 push!(iter_list, sol.iterations) end @printf("\nAverage iterations %d\n", mean(iter_list)) -``` - -## Continuation on maximal thrust - -As a second example, we now solve the problem for a decreasing maximal thrust Tmax. first we reset the speed constraint. - -```@example main -remove_constraint!(ocp, :speed_limit) -vmax = 0.1 -constraint!(ocp, :state, Index(2), 0, vmax, :speed_limit) -sol = solve(ocp, print_level=0) -sol0 = sol -nothing # hide -``` - -Then we perform the continuation - -```@example main -Tmax_list = [] -obj_list = [] -print("Tmax ") -for Tmax_local=3.5:-0.5:1 - global Tmax = Tmax_local - print(Tmax," ") - global sol = solve(ocp, print_level=0, init=OptimalControlInit(sol)) - push!(Tmax_list, Tmax) - push!(obj_list, sol.objective) -end -``` - -And plot the results as before - -```@example main -pobj = plot(Tmax_list, obj_list, label="r(tf)",seriestype=:scatter) -xlabel!("Maximal thrust (Tmax)") -ylabel!("Maximal altitude r(tf)") -plot(sol0) -p = plot!(sol) -plot(pobj, p, layout=2) -``` +``` \ No newline at end of file diff --git a/src/solve.jl b/src/solve.jl index e6cf1a1..fb448e1 100644 --- a/src/solve.jl +++ b/src/solve.jl @@ -94,17 +94,22 @@ Solve an optimal control problem OCP by direct method """ function solve(ocp::OptimalControlModel, description...; - init::OptimalControlInit=OptimalControlInit(), + init::Union{OptimalControlInit, OptimalControlSolution}=OptimalControlInit(), grid_size::Integer=__grid_size_direct(), display::Bool=__display(), print_level::Integer=__print_level_ipopt(), mu_strategy::String=__mu_strategy_ipopt(), kwargs...) + # build init if needed + if init isa OptimalControlSolution + init = OptimalControlInit(init) + end # build discretized OCP docp = directTranscription(ocp, description, init=init, grid_size=grid_size) # solve DOCP and retrieve OCP solution ocp_solution = solve(docp; display=display, print_level=print_level, mu_strategy=mu_strategy, kwargs...) return ocp_solution -end \ No newline at end of file +end + diff --git a/test/Project.toml b/test/Project.toml index a2cd2f8..5533bba 100644 --- a/test/Project.toml +++ b/test/Project.toml @@ -1,3 +1,4 @@ [deps] LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" diff --git a/test/suite/abstract_ocp.jl b/test/suite/abstract_ocp.jl index b009efc..d4b4646 100644 --- a/test/suite/abstract_ocp.jl +++ b/test/suite/abstract_ocp.jl @@ -80,7 +80,7 @@ end r(tf) → max end -@testset verbose = true showtiming = true ":double_integrator :min_tf :abstract :constr" begin +@testset verbose = true showtiming = true ":goddard :max_rf :abstract :constr" begin sol3 = solve(ocp3, grid_size=100, print_level=0, tol=1e-12) @test sol3.objective ≈ 1.0125 rtol=1e-2 end \ No newline at end of file diff --git a/test/suite/continuation.jl b/test/suite/continuation.jl new file mode 100644 index 0000000..f33fea6 --- /dev/null +++ b/test/suite/continuation.jl @@ -0,0 +1,119 @@ +using CTDirect +using Statistics + +println("Test: discrete continuation") + +# continuation on final time with parametric ocp definition +function ocp_T(T) + @def ocp begin + t ∈ [ 0, T ], time + x ∈ R², state + u ∈ R, control + q = x₁ + v = x₂ + q(0) == 0 + v(0) == 0 + q(T) == 1 + v(T) == 0 + ẋ(t) == [ v(t), u(t) ] + ∫(u(t)^2) → min + end + return ocp +end +@testset verbose = true showtiming = true ":parametric_ocp :warm_start" begin + init = OptimalControlInit() + obj_list = [] + iter_list = [] + for T=1:5 + ocp = ocp_T(T) + sol = solve(ocp, print_level=0, init=init) + init = OptimalControlInit(sol) + push!(obj_list, sol.objective) + push!(iter_list, sol.iterations) + end + @test last(obj_list) ≈ 0.096038 rtol=1e-2 + @test mean(iter_list) ≈ 2.8 rtol=1e-2 +end + + +# continuation with parametric definition of the ocp +relu(x) = max(0, x) +μ = 10 +p_relu(x) = log(abs(1 + exp(μ*x)))/μ +f(x) = 1-x +m(x) = (p_relu∘f)(x) +T = 2 +function myocp(ρ) + @def ocp begin + τ ∈ R, variable + s ∈ [ 0, 1 ], time + x ∈ R², state + u ∈ R², control + x₁(0) == 0 + x₂(0) == 1 + x₁(1) == 1 + ẋ(s) == [τ*(u₁(s)+2), (T-τ)*u₂(s)] + -1 ≤ u₁(s) ≤ 1 + -1 ≤ u₂(s) ≤ 1 + 0 ≤ τ ≤ T + -(x₂(1)-2)^3 - ∫( ρ * ( τ*m(x₁(s))^2 + (T-τ)*m(x₂(s))^2 ) ) → min + end + return ocp +end +@testset verbose = true showtiming = true ":parametric_ocp :warm_start" begin + init = OptimalControlInit() + obj_list = [] + iter_list = [] + for ρ in [0.1, 5, 10, 30, 100] + ocp = myocp(ρ) + sol = solve(ocp, print_level=0, init=init) + init = OptimalControlInit(sol) + push!(obj_list, sol.objective) + push!(iter_list, sol.iterations) + end + @test last(obj_list) ≈ -148.022367 rtol=1e-2 + @test mean(iter_list) ≈ 43.8 rtol=1e-2 +end + +# goddard +Cd = 310 +Tmax = 3.5 +β = 500 +b = 2 +function F00(x) + r, v, m = x + D = Cd * v^2 * exp(-β*(r - 1)) + return [ v, -D/m - 1/r^2, 0 ] +end +function F01(x) + r, v, m = x + return [ 0, Tmax/m, -b*Tmax ] +end +ocp = Model(variable=true) +state!(ocp, 3) +control!(ocp, 1) +variable!(ocp, 1) +time!(ocp, 0, Index(1)) +constraint!(ocp, :initial, [1,0,1], :initial_constraint) +constraint!(ocp, :final, Index(3), 0.6, :final_constraint) +constraint!(ocp, :state, 1:2:3, [1,0.6], [1.2,1], :state_box) +constraint!(ocp, :control, Index(1), 0, 1, :control_box) +constraint!(ocp, :variable, Index(1), 0.01, Inf, :variable_box) +constraint!(ocp, :state, Index(2), 0, Inf, :speed_limit) +objective!(ocp, :mayer, (x0, xf, v) -> xf[1], :max) +dynamics!(ocp, (x, u, v) -> F00(x) + u*F01(x) ) +sol0 = solve(ocp, print_level=0) + +@testset verbose = true showtiming = true ":global_variable :warm_start" begin + sol = sol0 + obj_list = [] + iter_list = [] + for Tmax_local=3.5:-0.5:1 + global Tmax = Tmax_local + sol = solve(ocp, print_level=0, init=sol) + push!(obj_list, sol.objective) + push!(iter_list, sol.iterations) + end + @test last(obj_list) ≈ 1.00359 rtol=1e-2 + @test mean(iter_list) ≈ 17 rtol=1e-2 +end diff --git a/test/test_continuation.jl b/test/test_continuation.jl index 3847f16..e004c1f 100644 --- a/test/test_continuation.jl +++ b/test/test_continuation.jl @@ -6,70 +6,179 @@ using Plots; pyplot() test1 = true test2 = true -test3 = false +test3 = true +test4 = true + +# continuation on fixed final time +# NB. time! can be called only once, so we redefine the ocp +if test1 + println("\nDiscrete continuation on final time") + # parametric ocp + function ocp_T(T) + @def ocp begin + t ∈ [ 0, T ], time + x ∈ R², state + u ∈ R, control + q = x₁ + v = x₂ + q(0) == 0 + v(0) == 0 + q(T) == 1 + v(T) == 0 + ẋ(t) == [ v(t), u(t) ] + ∫(u(t)^2) → min + end + return ocp + end + + # continuation on final time + init1 = OptimalControlInit() + iter_list = [] + for T=1:5 + ocp1 = ocp_T(T) + sol1 = solve(ocp1, print_level=0, init=init1) + global init1 = OptimalControlInit(sol1) + @printf("T %.2f objective %.6f iterations %d\n", T, sol1.objective, sol1.iterations) + push!(iter_list, sol1.iterations) + end + @printf("Average iterations %d\n", mean(iter_list)) +end + + +# continuation with parametric definition of the ocp +if test2 + println("\nDiscrete continuation on parametric ocp") + # definition of the parametric OCP + relu(x) = max(0, x) + μ = 10 + p_relu(x) = log(abs(1 + exp(μ*x)))/μ + f(x) = 1-x + m(x) = (p_relu∘f)(x) + T = 2 + + function myocp(ρ) + @def ocp begin + τ ∈ R, variable + s ∈ [ 0, 1 ], time + x ∈ R², state + u ∈ R², control + x₁(0) == 0 + x₂(0) == 1 + x₁(1) == 1 + ẋ(s) == [τ*(u₁(s)+2), (T-τ)*u₂(s)] + -1 ≤ u₁(s) ≤ 1 + -1 ≤ u₂(s) ≤ 1 + 0 ≤ τ ≤ T + -(x₂(1)-2)^3 - ∫( ρ * ( τ*m(x₁(s))^2 + (T-τ)*m(x₂(s))^2 ) ) → min + end + return ocp + end + + # continuation on rho + init2 = OptimalControlInit() + iter_list = [] + ρs = [0.1, 5, 10, 30, 100] + for ρ in ρs + ocp2 = myocp(ρ) + sol2 = solve(ocp2, print_level=0, init=init2) + global init2 = OptimalControlInit(sol2) + @printf("Rho %.2f objective %.6f iterations %d\n", ρ, sol2.objective, sol2.iterations) + push!(iter_list, sol2.iterations) + end + @printf("Average iterations %d\n", mean(iter_list)) +end + -################################################# # goddard max final altitude -println("Test: discrete continuation (goddard)") - -Cd = 310 -Tmax = 3.5 -β = 500 -b = 2 -function F0(x) - r, v, m = x - D = Cd * v^2 * exp(-β*(r - 1)) - return [ v, -D/m - 1/r^2, 0 ] +if (test3 || test4) + Cd = 310 + Tmax = 3.5 + β = 500 + b = 2 + function F0(x) + r, v, m = x + D = Cd * v^2 * exp(-β*(r - 1)) + return [ v, -D/m - 1/r^2, 0 ] + end + function F1(x) + r, v, m = x + return [ 0, Tmax/m, -b*Tmax ] + end + + ocp = Model(variable=true) + state!(ocp, 3) + control!(ocp, 1) + variable!(ocp, 1) + time!(ocp, 0, Index(1)) + constraint!(ocp, :initial, [1,0,1], :initial_constraint) + constraint!(ocp, :final, Index(3), 0.6, :final_constraint) + constraint!(ocp, :state, 1:2:3, [1,0.6], [1.2,1], :state_box) + constraint!(ocp, :control, Index(1), 0, 1, :control_box) + constraint!(ocp, :variable, Index(1), 0.01, Inf, :variable_box) + constraint!(ocp, :state, Index(2), 0, Inf, :speed_limit) + objective!(ocp, :mayer, (x0, xf, v) -> xf[1], :max) + dynamics!(ocp, (x, u, v) -> F0(x) + u*F1(x) ) + + # solve unconstrained problem + sol0 = solve(ocp, print_level=0) + @printf("\nObjective for goddard reference solution %.6f", sol0.objective) end -function F1(x) - r, v, m = x - return [ 0, Tmax/m, -b*Tmax ] + + +# using a global variable in ocp definition +if test3 + print("\nDiscrete continuation on maximal thrust\nTmax ") + # continuation on Tmax (using default init is slower) + Tmax_list = [] + obj_list = [] + iter_list = [] + sol3 = sol0 + for Tmax_local=3.5:-0.5:1 + global Tmax = Tmax_local + print(Tmax," ") + global sol3 = solve(ocp, print_level=0, init=sol3) + push!(Tmax_list, Tmax) + push!(obj_list, sol3.objective) + push!(iter_list, sol3.iterations) + end + @printf("\nAverage iterations %d\n", mean(iter_list)) + + # plot obj(vmax) + pobj = plot(Tmax_list, obj_list, label="r(tf)", xlabel="Maximal thrust (Tmax)", ylabel="Maximal altitude r(tf)",seriestype=:scatter) + # plot multiple solutions + plot(sol0) + p = plot!(sol3) + display(plot(pobj, p, layout=2, reuse=false)) end -ocp = Model(variable=true) -state!(ocp, 3) -control!(ocp, 1) -variable!(ocp, 1) -time!(ocp, 0, Index(1)) -constraint!(ocp, :initial, [1,0,1], :initial_constraint) -constraint!(ocp, :final, Index(3), 0.6, :final_constraint) -constraint!(ocp, :state, 1:2:3, [1,0.6], [1.2,1], :state_box) -constraint!(ocp, :control, Index(1), 0, 1, :control_box) -constraint!(ocp, :variable, Index(1), 0.01, Inf, :variable_box) -constraint!(ocp, :state, Index(2), 0, Inf, :speed_limit) -objective!(ocp, :mayer, (x0, xf, v) -> xf[1], :max) -dynamics!(ocp, (x, u, v) -> F0(x) + u*F1(x) ) - -# solve unconstrained problem -sol0 = solve(ocp, print_level=0) -@printf("Objective for reference solution %.6f\n", sol0.objective) -sol = sol0 -if test1 +# manually edit a constraint in ocp +if test4 + # reset Tmax + global Tmax = 3.5 # default init - print("\nSolve for different speed limits, default initial guess\nvmax ") + print("\nSolve goddard for different speed limits, default initial guess\nvmax ") iter_list = [] for vmax=0.15:-0.01:0.05 print(vmax," ") remove_constraint!(ocp, :speed_limit) constraint!(ocp, :state, Index(2), 0, vmax, :speed_limit) - global sol = solve(ocp, print_level=0) - #@printf("vmax %.2f objective %.6f iterations %d\n",vmax,sol.objective, sol.iterations) + global sol = solve(ocp, print_level=0) push!(iter_list, sol.iterations) end @printf("\nAverage iterations %d\n", mean(iter_list)) # warm start - print("\nDiscrete continuation on speed limit, with warm start\nvmax ") + print("Discrete continuation on speed limit, with warm start\nvmax ") vmax_list = [] obj_list = [] iter_list = [] + sol = sol0 for vmax=0.15:-0.01:0.05 print(vmax," ") remove_constraint!(ocp, :speed_limit) constraint!(ocp, :state, Index(2), 0, vmax, :speed_limit) - global sol = solve(ocp, print_level=0, init=OptimalControlInit(sol)) - #@printf("vmax %.2f objective %.6f iterations %d\n",vmax,sol.objective, sol.iterations) + global sol = solve(ocp, print_level=0, init=sol) push!(vmax_list, vmax) push!(obj_list, sol.objective) push!(iter_list, sol.iterations) @@ -78,50 +187,8 @@ if test1 # plot obj(vmax) pobj = plot(vmax_list, obj_list, label="r(tf)", xlabel="Speed limit (vmax)", ylabel="Maximal altitude r(tf)",seriestype=:scatter) - # plot multiple solutions plot(sol0) p = plot!(sol) - display(plot(pobj, p, layout=2)) end - -if test2 - print("\nDiscrete continuation on maximal thrust\nTmax ") - # reset speed constraint - remove_constraint!(ocp, :speed_limit) - vmax = 0.1 - constraint!(ocp, :state, Index(2), 0, vmax, :speed_limit) - sol = solve(ocp, print_level=0) - sol0 = sol - - # continuation on Tmax (using default init is slower) - Tmax_list = [] - obj_list = [] - for Tmax_local=3.5:-0.5:1 - global Tmax = Tmax_local - print(Tmax," ") - global sol = solve(ocp, print_level=0, init=OptimalControlInit(sol)) - push!(Tmax_list, Tmax) - push!(obj_list, sol.objective) - #print(Tmax, " ", sol.objective, " ", sol.iterations) - end - - # plot obj(vmax) - pobj = plot(Tmax_list, obj_list, label="r(tf)", xlabel="Maximal thrust (Tmax)", ylabel="Maximal altitude r(tf)",seriestype=:scatter) - - # plot multiple solutions - plot(sol0) - p = plot!(sol) - - display(plot(pobj, p, layout=2, reuse=false)) - -end - - -# currently only one call to time is allowed... -if test3 - time!(ocp, 0, 0.1) - sol = solve(ocp, print_level=0) - println(sol.objective) -end \ No newline at end of file