Merge pull request #14 from control-toolbox/uptuto

Update of the tutorials

Author: ocots

docs/Project.toml

@@ -1,28 +1,36 @@
 [deps]
+BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf"
+DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0"
 DifferentiationInterface = "a0c0ee7d-e4b9-4e03-894e-1c5f64a51d63"
 Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4"
 ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 MINPACK = "4854310b-de5a-5eb6-a2a5-c1dee2bd17f9"
 MadNLP = "2621e9c9-9eb4-46b1-8089-e8c72242dfb6"
+NLPModels = "a4795742-8479-5a88-8948-cc11e1c8c1a6"
 NLPModelsIpopt = "f4238b75-b362-5c4c-b852-0801c9a21d71"
 NonlinearSolve = "8913a72c-1f9b-4ce2-8d82-65094dcecaec"
 OptimalControl = "5f98b655-cc9a-415a-b60e-744165666948"
 OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
+Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7"
 Suppressor = "fd094767-a336-5f1f-9728-57cf17d0bbfb"
 
 [compat]
+BenchmarkTools = "1.6"
+DataFrames = "1.7"
 DifferentiationInterface = "0.7"
 Documenter = "1.8"
 ForwardDiff = "0.10, 1"
 LinearAlgebra = "1"
 MINPACK = "1.3"
 MadNLP = "0.8"
+NLPModels = "0.21"
 NLPModelsIpopt = "0.10"
 NonlinearSolve = "4.6"
 OptimalControl = "1.0"
 OrdinaryDiffEq = "6.93"
 Plots = "1.40"
+Printf = "1"
 Suppressor = "0.2"
-julia = "1"
+julia = "1.10"

docs/extras/disc.jl

@@ -0,0 +1,131 @@
+using OptimalControl  # to define the optimal control problem and more
+using NLPModelsIpopt  # to solve the problem via a direct method
+using Plots           # to plot the solution
+
+t0 = 0      # initial time
+r0 = 1      # initial altitude
+v0 = 0      # initial speed
+m0 = 1      # initial mass
+vmax = 0.1  # maximal authorized speed
+mf = 0.6    # final mass to target
+
+ocp = @def begin # definition of the optimal control problem
+
+    tf ∈ R, variable
+    t ∈ [t0, tf], time
+    x = (r, v, m) ∈ R³, state
+    u ∈ R, control
+
+    x(t0) == [r0, v0, m0]
+    m(tf) == mf
+    0 ≤ u(t) ≤ 1
+    r(t) ≥ r0
+    0 ≤ v(t) ≤ vmax
+
+    ẋ(t) == F0(x(t)) + u(t) * F1(x(t))
+
+    r(tf) → max
+
+end;
+
+# Dynamics
+Cd = 310
+Tmax = 3.5
+β = 500
+b = 2
+
+F0(x) = begin
+    r, v, m = x
+    D = Cd * v^2 * exp(-β*(r - 1)) # Drag force
+    return [ v, -D/m - 1/r^2, 0 ]
+end
+
+F1(x) = begin
+    r, v, m = x
+    return [ 0, Tmax/m, -b*Tmax ]
+end
+
+# Solve the problem with different discretization methods
+solutions = []
+schemes = [:trapeze, :midpoint, :euler, :euler_implicit, :gauss_legendre_2, :gauss_legendre_3]
+for scheme in schemes
+    sol = solve(ocp; disc_method=scheme, tol=1e-8, display=false)
+    push!(solutions, (scheme, sol))
+end
+
+# Plot the results
+x_style = (legend=:none,)
+p_style = (legend=:none,)
+styles = (state_style=x_style, costate_style=p_style)
+
+scheme, sol = solutions[1]
+plt = plot(sol; label=string(scheme), styles...)
+for (scheme, sol) in solutions[2:end]
+    plt = plot!(sol; label=string(scheme), styles...)
+end
+plot(plt; size=(800, 800))
+
+# Benchmark the problem with different AD backends
+using NLPModels
+using DataFrames
+using BenchmarkTools
+
+# DataFrame to store the results
+data = DataFrame(
+    Backend=Symbol[],
+    NNZO=Int[],
+    NNZJ=Int[],
+    NNZH=Int[],
+    PrepTime=Float64[],
+    ExecTime=Float64[],
+    Objective=Float64[], 
+    Iterations=Int[]
+)
+
+# The different AD backends to test
+backends = [:optimized, :manual]
+
+# Loop over the backends
+for adnlp_backend in backends
+
+    println("Testing backend: ", adnlp_backend)
+
+    # Discretize the problem with a large grid size and Gauss-Legendre method
+    bt = @btimed direct_transcription($ocp; 
+        disc_method=:euler, 
+        grid_size=1000, 
+        adnlp_backend=$adnlp_backend,
+    )
+    docp, nlp = bt.value
+    prepa_time = bt.time
+
+    # Get the number of non-zero elements
+    nnzo = get_nnzo(nlp) # Gradient of the Objective
+    nnzj = get_nnzj(nlp) # Jacobian of the constraints
+    nnzh = get_nnzh(nlp) # Hessian of the Lagrangian
+
+    # Solve the problem
+    bt = @btimed ipopt($nlp; 
+        print_level=0, 
+        mu_strategy="adaptive", 
+        tol=1e-8,
+        sb="yes",
+    )
+    nlp_sol = bt.value
+    exec_time = bt.time
+
+    # Store the results in the DataFrame
+    push!(data, 
+        (
+            Backend=adnlp_backend, 
+            NNZO=nnzo, 
+            NNZJ=nnzj, 
+            NNZH=nnzh, 
+            PrepTime=prepa_time, 
+            ExecTime=exec_time, 
+            Objective=-nlp_sol.objective, 
+            Iterations=nlp_sol.iter
+        )
+    )
+end
+println(data)

docs/extras/iss.jl

@@ -0,0 +1,131 @@
+using OptimalControl    # to define the optimal control problem and its flow
+using OrdinaryDiffEq    # to get the Flow function from OptimalControl
+using MINPACK           # NLE solver: use to solve the shooting equation
+using Plots             # to plot the solution
+
+const t0 = 0
+const tf = 1
+const x0 = -1
+const xf = 0
+const α  = 1.5
+ocp = @def begin
+
+    t ∈ [t0, tf], time
+    x ∈ R, state
+    u ∈ R, control
+
+    x(t0) == x0
+    x(tf) == xf
+
+    ẋ(t) == -x(t) + α * x(t)^2 + u(t)
+
+    ∫( 0.5u(t)^2 ) → min
+    
+end
+nothing # hide
+
+u(x, p) = p
+φ = Flow(ocp, u)
+
+π((x, p)) = x
+
+S(p0) = π( φ(t0, x0, p0, tf) ) - xf    # shooting function
+
+ξ = [0.1]    # initial guess
+
+using MINPACK
+function fsolve(f, j, x; kwargs...)
+    try
+        MINPACK.fsolve(f, j, x; kwargs...)
+    catch e
+        println("Erreur using MINPACK")
+        println(e)
+        println("hybrj not supported. Replaced by hybrd even if it is not visible on the doc.")
+        MINPACK.fsolve(f, x; kwargs...)
+    end
+end
+
+using DifferentiationInterface
+import ForwardDiff
+backend = AutoForwardDiff()
+
+nle!  = ( s, ξ) -> s[1] = S(ξ[1])                                 # auxiliary function
+jnle! = (js, ξ) -> jacobian!(nle!, similar(ξ), js, backend, ξ)    # 
+
+
+indirect_sol = fsolve(nle!, jnle!, ξ; show_trace=true)    # resolution of S(p0) = 0
+p0_sol = indirect_sol.x[1]                                # costate solution
+println("\ncostate:    p0 = ", p0_sol)
+println("shoot: |S(p0)| = ", abs(S(p0_sol)), "\n")
+
+sol = φ((t0, tf), x0, p0_sol)
+plot(sol)
+
+using Plots.PlotMeasures 
+function Plots.plot(S::Function, p0::Float64; Np0=20, kwargs...) 
+ 
+    # times for wavefronts
+    times = range(t0, tf, length=3)
+
+    # times for trajectories
+    tspan = range(t0, tf, length=100)
+
+    # interval of initial covector
+    p0_min = -0.5 
+    p0_max = 2 
+
+    # covector solution
+    p0_sol = p0 
+ 
+    # plot of the flow in phase space
+    plt_flow = plot() 
+    p0s = range(p0_min, p0_max, length=Np0) 
+    for i ∈ eachindex(p0s) 
+        sol = φ((t0, tf), x0, p0s[i])
+        x = state(sol).(tspan)
+        p = costate(sol).(tspan)
+        label = i==1 ? "extremals" : false 
+        plot!(plt_flow, x, p, color=:blue, label=label) 
+    end 
+ 
+    # plot of wavefronts in phase space 
+    p0s = range(p0_min, p0_max, length=200) 
+    xs  = zeros(length(p0s), length(times)) 
+    ps  = zeros(length(p0s), length(times)) 
+    for i ∈ eachindex(p0s) 
+        sol = φ((t0, tf), x0, p0s[i], saveat=times)
+        xs[i, :] .= state(sol).(times) 
+        ps[i, :] .= costate(sol).(times) 
+    end 
+    for j ∈ eachindex(times) 
+        label = j==1 ? "flow at times" : false 
+        plot!(plt_flow, xs[:, j], ps[:, j], color=:green, linewidth=2, label=label) 
+    end 
+ 
+    #  
+    plot!(plt_flow, xlims=(-1.1, 1), ylims=(p0_min, p0_max)) 
+    plot!(plt_flow, [0, 0], [p0_min, p0_max], color=:black, xlabel="x", ylabel="p", label="x=xf") 
+     
+    # solution 
+    sol = φ((t0, tf), x0, p0_sol)
+    x = state(sol).(tspan)
+    p = costate(sol).(tspan)
+    plot!(plt_flow, x, p, color=:red, linewidth=2, label="extremal solution") 
+    plot!(plt_flow, [x[end]], [p[end]], seriestype=:scatter, color=:green, label=false) 
+ 
+    # plot of the shooting function  
+    p0s = range(p0_min, p0_max, length=200) 
+    plt_shoot = plot(xlims=(p0_min, p0_max), ylims=(-2, 4), xlabel="p₀", ylabel="y") 
+    plot!(plt_shoot, p0s, S, linewidth=2, label="S(p₀)", color=:green) 
+    plot!(plt_shoot, [p0_min, p0_max], [0, 0], color=:black, label="y=0") 
+    plot!(plt_shoot, [p0_sol, p0_sol], [-2, 0], color=:black, label="p₀ solution", linestyle=:dash) 
+    plot!(plt_shoot, [p0_sol], [0], seriestype=:scatter, color=:green, label=false) 
+ 
+    # final plot 
+    plot(plt_flow, plt_shoot; layout=(1,2), leftmargin=15px, bottommargin=15px, kwargs...) 
+ 
+end
+
+p0_sol
+
+plot(S, p0_sol; size=(800, 450))

docs/extras/lqr.jl

@@ -0,0 +1,69 @@
+using Pkg
+Pkg.activate("docs") 
+
+using OptimalControl
+using NLPModelsIpopt
+using Plots
+
+function lqr(tf)
+
+    x0 = [ 0
+        1 ]
+
+    A = [  0 1
+        -1 0 ]
+
+    B = [ 0
+        1 ]
+
+    Q = [ 1 0
+        0 1 ]
+
+    R = 1
+
+    ocp = @def begin
+        t ∈ [0, tf], time
+        x ∈ R², state
+        u ∈ R, control
+        x(0) == x0
+        ẋ(t) == [x₂(t), - x₁(t) + u(t)]
+        0.5∫( x₁(t)^2 + x₂(t)^2 + u(t)^2 ) → min
+    end
+
+    return ocp
+end
+
+solutions = []   # empty list of solutions
+tfs = [3, 5, 30]
+
+for tf ∈ tfs
+    solution = solve(lqr(tf); display=false)
+    push!(solutions, solution)
+end
+
+plt = plot(solutions[1], time=:normalize)
+for sol ∈ solutions[2:end]
+    plot!(plt, sol, time=:normalize)
+end
+
+# we plot only the state and control variables and we add the legend
+N = length(tfs)
+px1 = plot(plt[1], legend=false, xlabel="s", ylabel="x₁")
+px2 = plot(plt[2], label=reshape(["tf = $tf" for tf ∈ tfs], (1, N)), xlabel="s", ylabel="x₂")
+pu  = plot(plt[5], legend=false, xlabel="s", ylabel="u")
+
+using Plots.PlotMeasures # for leftmargin, bottommargin
+plot(px1, px2, pu, layout=(1, 3), size=(800, 300), leftmargin=5mm, bottommargin=5mm)
+
+
+####
+
+plt = plot(solutions[1], :state, :control; time=:normalize, label="tf = $(tfs[1])")
+for (tf, sol) ∈ zip(tfs[2:end], solutions[2:end])
+    plot!(plt, sol, :state, :control; time=:normalize, label="tf = $tf")
+end
+
+px1 = plot(plt[1], legend=false, xlabel="s", ylabel="x₁")
+px2 = plot(plt[2], legend=true, xlabel="s", ylabel="x₂")
+pu  = plot(plt[3], legend=false, xlabel="s", ylabel="u")
+plot(px1, px2, pu, layout=(1, 3), size=(800, 300), leftmargin=5mm, bottommargin=5mm)