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

@@ -1,19 +1,66 @@
 # 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
 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)
-```
+```

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
+end
+

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"

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

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