Notebook source code: examples/homotopy-julia/FGS.ipynb
Run the notebook yourself on binder Binder badge

Turnpike and homotopy: an example

Definition of the optimal control problem

\[\begin{split}\left\{ \begin{array}{l} \displaystyle \min \frac{1}{2}\int_{0}^{1}\left((y_1(t)-1)^2+(y_2(t)-1)^2+(u(t)-2)^2\right) \mathrm{d}t,\\[1.0em] \dot{y}_1(t) = t_f\, y_2(t)\\ \dot{y}_2(t) = t_f\, \left(1-y_1(t)+y_2^3(t)+u(t)\right)\\[1.0em] y(0) = (1, 1),\quad y(1) = (3, 0). \end{array} \right.\end{split}\]

Preliminaries

Packages

[1]:
# Standard packages
using LinearAlgebra
using NLsolve
using Plots

# Own files
include("Flow.jl");      # To generate flows of Hamiltonian system
include("Homotopy.jl")   # To compute homotopies
include("Plottings.jl"); # To plot solutions

Parameters

[2]:
y0 = [1.; 1.] # Initial point
yf = [3.; 0.] # Final point

ybar = [2.; 0.]    # Solution to the static problem
qbar = [-1.; -1.]; # Lagrange multiplier from the static problem

Hamiltonian

[3]:
function H(y, q, u, tf) # Pseudo-Hamiltonian
    y1 = y[1]; y2 = y[2]
    q1 = q[1]; q2 = q[2]
    return (-0.5*((y1-1)^2 + (y2-1)^2 + (u-2)^2) + y2*q1 + (1 - y1 + y2^3 + u)*q2)*tf
end

u(y, q)  = 2. + q[2] # Maximizing control law

H(y, q, tf) = H(y, q, u(y, q), tf); # maximized Hamiltonian

Flow of the Hamiltonian

[4]:
f = Flow(H);

Shooting

[5]:
# The shooting function
function shoot(q0, q1, λ, tf ; optionsODE = Dict())

    # Integration from 0 to 0.5
    z0 = [λ*y0+(1-λ)*ybar ; q0]
    zm1 = f(0., z0, 0.5, λ=tf, optionsODE=optionsODE)

    # Integration from 1. to 0.5
    z1 = [λ*yf+(1-λ)*ybar ; q1]
    zm2 = f(1., z1, 0.5, λ=tf, optionsODE=optionsODE)

    # Matching at the middle point
    s = zm1 - zm2

    return s
end

# test : lambda = 0, tf = 40
λ  = 0.
tf = 40.
println("shoot(qbar, qbar, λ, tf) = ", shoot(qbar, qbar, λ, tf))
shoot(qbar, qbar, λ, tf) = [0.0, 0.0, 0.0, 0.0]
[6]:
# Resolution of the shooting equation

λ_shoot  = 0.05
tf_shoot = 10.

S(ξ) = shoot(ξ[1:2], ξ[3:4], λ_shoot, tf_shoot)

nl_sol = nlsolve(S, [qbar; qbar]; xtol=1e-8, method=:trust_region, show_trace=true); q0_sol = nl_sol.zero[1:2]; q1_sol = nl_sol.zero[3:4];

display(nl_sol)
Iter     f(x) inf-norm    Step 2-norm
------   --------------   --------------
     0     1.009507e+00              NaN
     1     2.812356e+00     1.110223e-16
     2     2.812356e+00     0.000000e+00
     3     2.812356e+00     0.000000e+00
     4     2.812356e+00     0.000000e+00
     5     2.812356e+00     0.000000e+00
     6     7.539201e-01     8.133038e-02
     7     1.979988e+00     0.000000e+00
     8     4.440810e-01     9.147134e-02
     9     3.166153e-01     5.088333e-02
    10     2.012772e-02     2.070249e-02
    11     1.624971e-04     1.280979e-03
    12     7.209311e-09     1.139099e-05
Results of Nonlinear Solver Algorithm
 * Algorithm: Trust-region with dogleg and autoscaling
 * Starting Point: [-1.0, -1.0, -1.0, -1.0]
 * Zero: [-0.9226235194681129, -1.0498486216329852, -0.9027010765185398, -1.0205351599839694]
 * Inf-norm of residuals: 0.000000
 * Iterations: 12
 * Convergence: true
   * |x - x'| < 1.0e-08: false
   * |f(x)| < 1.0e-08: true
 * Function Calls (f): 9
 * Jacobian Calls (df/dx): 7
[7]:
# Display the solution
plot_shoot(y0, q0_sol, yf, q1_sol, ybar, λ_shoot, tf_shoot, f, size=(900, 600))
../_images/homotopy-julia_FGS_12_0.svg
../_images/homotopy-julia_FGS_12_1.svg

Homotopy on the boundary conditions

[8]:
opt     = Dict([(:algo,Tsit5()),(:reltol,1.e-12),(:abstol,1.e-12)])
tf_bc   = 10.0
S(ξ, λ) = shoot(ξ[1:2], ξ[3:4], λ, tf_bc, optionsODE=opt)
p       = Path(S);
[9]:
λ₀ = 0.
λ₁ = 1.
path_bc = p([qbar; qbar], λ₀, λ₁; saveat=0.01);

     Calls  |f(x,pars)|     |x|             Homotopic param

         1  0.00000000e+00  2.00000000e+00  0.00000000e+00
         2  1.59036614e-13  1.99527339e+00  4.63092298e-03
         3  5.64576445e-12  1.98211393e+00  1.77903580e-02
         4  4.60305781e-11  1.96498879e+00  3.56281208e-02
         5  1.67829384e-10  1.94614348e+00  5.65042591e-02
         6  5.59479307e-10  1.92591008e+00  8.10485666e-02
         7  1.44018063e-09  1.90634378e+00  1.08226374e-01
         8  3.26225158e-09  1.88901298e+00  1.37812114e-01
         9  6.42469775e-09  1.87594054e+00  1.69053449e-01
        10  1.14432903e-08  1.86892139e+00  2.01497254e-01
        11  1.25973256e-12  1.86959923e+00  2.34589586e-01
        12  9.57242331e-09  1.87925651e+00  2.67917770e-01
        13  1.32907332e-12  1.89874406e+00  3.01099219e-01
        14  1.46471304e-12  1.92845027e+00  3.33842892e-01
        15  1.67079071e-12  1.96831177e+00  3.65916196e-01
        16  2.01621236e-12  2.01788884e+00  3.97152178e-01
        17  2.53524158e-12  2.07645468e+00  4.27434890e-01
        18  2.82923812e-12  2.14310030e+00  4.56692459e-01
        19  2.29650192e-12  2.21682364e+00  4.84886300e-01
        20  2.37069642e-12  2.29660808e+00  5.12004905e-01
        21  2.47722617e-12  2.38270347e+00  5.38416167e-01
        22  2.74427937e-12  2.47451153e+00  5.64137863e-01
        23  2.93729730e-12  2.57168516e+00  5.89244621e-01
        24  3.27133833e-12  2.67380287e+00  6.13771320e-01
        25  3.48571067e-12  2.78054448e+00  6.37763433e-01
        26  3.69625402e-12  2.89159354e+00  6.61253696e-01
        27  3.96713633e-12  3.00668552e+00  6.84274993e-01
        28  4.33210511e-12  3.12557732e+00  7.06854884e-01
        29  4.43359225e-12  3.24805845e+00  7.29019084e-01
        30  4.70189089e-12  3.37394016e+00  7.50790312e-01
        31  4.99411779e-12  3.50305641e+00  7.72189354e-01
        32  5.23803023e-12  3.63525895e+00  7.93234928e-01
        33  5.42555892e-12  3.77041596e+00  8.13944099e-01
        34  5.64660168e-12  3.90840918e+00  8.34332365e-01
        35  6.19558315e-12  4.04913234e+00  8.54413876e-01
        36  6.44422456e-12  4.19248923e+00  8.74201546e-01
        37  6.74707843e-12  4.33839237e+00  8.93707201e-01
        38  6.73365760e-12  4.48676164e+00  9.12941681e-01
        39  7.31072661e-12  4.63752329e+00  9.31914949e-01
        40  7.96850252e-12  4.79060895e+00  9.50636179e-01
        41  8.68142860e-12  4.94595491e+00  9.69113835e-01
        42  8.55131057e-12  5.10350140e+00  9.87355749e-01
        43  8.95047041e-12  5.21516339e+00  1.00000000e+00

 Results of the path solver method:
 xf            = [1.8380780571396904, -4.524744727246664, 1.159776180023536, -1.414567989884903]
 parsf         = 0.9999999999852779
 |F(xf,parsf)| = 8.950470413235674e-12
 steps         = 570
 status        = Terminated
[10]:
plot_path_bc(path_bc, size=(800, 600))
┌ Warning: To maintain consistency with solution indexing, keyword argument vars will be removed in a future version. Please use keyword argument idxs instead.
│   caller = ip:0x0
└ @ Core :-1
[10]:
../_images/homotopy-julia_FGS_16_1.svg
[11]:
plot_traj_bc(path_bc, y0, yf, ybar, tf_bc, f, size=(800, 1200))
┌ Info: Saved animation to
│   fn = /Users/ocots/Boulot/recherche/Logiciels/dev/ct/gallery/examples/homotopy-julia/traj_bc.mp4
└ @ Plots /Users/ocots/.julia/packages/Plots/Ra8fG/src/animation.jl:126
┌ Info: Saved animation to
│   fn = /Users/ocots/Boulot/recherche/Logiciels/dev/ct/gallery/examples/homotopy-julia/traj_bc.gif
└ @ Plots /Users/ocots/.julia/packages/Plots/Ra8fG/src/animation.jl:126
[11]:

Homotopy on the final time

[12]:
S(ξ, tf) = shoot(ξ[1:2], ξ[3:4], λ₁, tf, optionsODE=opt)
p = Path(S);
[13]:
ξ₀  = path_bc[1:4, end]
tf₀ = tf_bc
tf₁ = 60.
path_tf = p(ξ₀, tf₀, tf₁; saveat=1.0);

     Calls  |f(x,pars)|     |x|             Homotopic param

         1  2.66269047e-08  5.21516339e+00  1.00000000e+01
         2  2.66263527e-08  5.21514408e+00  1.00189728e+01
         3  2.66258934e-08  5.21506014e+00  1.01094485e+01
         4  2.66323549e-08  5.21495482e+00  1.02506644e+01
         5  2.66466621e-08  5.21486898e+00  1.04132906e+01
         6  2.67138436e-08  5.21480798e+00  1.06172659e+01
         7  2.68675121e-08  5.21479019e+00  1.08503628e+01
         8  2.72448665e-08  5.21482088e+00  1.11197978e+01
         9  2.79826050e-08  5.21489747e+00  1.14226261e+01
        10  2.94331549e-08  5.21500967e+00  1.17651761e+01
        11  3.23248024e-08  5.21514013e+00  1.21551302e+01
        12  3.92838206e-08  5.21526803e+00  1.26119239e+01
        13  4.72675361e-08  5.21535865e+00  1.30955899e+01
        14  5.35329063e-08  5.21540433e+00  1.35843876e+01
        15  5.97255682e-08  5.21541507e+00  1.40993107e+01
        16  6.51952565e-08  5.21540279e+00  1.46420352e+01
        17  7.06331320e-08  5.21538043e+00  1.52310418e+01
        18  7.90629848e-08  5.21535893e+00  1.58938772e+01
        19  5.35098383e-11  5.21534589e+00  1.66964053e+01
        20  5.21700620e-08  5.21534416e+00  1.74501942e+01
        21  9.85721547e-11  5.21534722e+00  1.82805435e+01
        22  1.28740824e-10  5.21535062e+00  1.91877122e+01
        23  1.67612625e-10  5.21535208e+00  2.03280251e+01
        24  2.09091935e-10  5.21535174e+00  2.14489348e+01
        25  3.73694608e-10  5.21535125e+00  2.27221200e+01
        26  7.45687148e-10  5.21535119e+00  2.43470647e+01
        27  1.13575283e-09  5.21535126e+00  2.59804509e+01
        28  2.08623478e-09  5.21535126e+00  2.81287902e+01
        29  6.37281134e-09  5.21535126e+00  3.04987538e+01
        30  1.54819197e-08  5.21535126e+00  3.40910756e+01
        31  6.63832697e-08  5.21535126e+00  3.77487606e+01
        32  3.28564259e-08  5.21535126e+00  4.38959813e+01
        33  1.49624505e-07  5.21535126e+00  5.22986634e+01
        34  1.44837659e-06  5.21535126e+00  6.00194297e+01

 Results of the path solver method:
 xf            = [1.8360048424669169, -4.525775316049591, 1.1603158042301727, -1.4142135623728809]
 parsf         = 60.01942966061689
 |F(xf,parsf)| = 1.4483765886137437e-6
 steps         = 86
 status        = Terminated
[14]:
plot_path_tf(path_tf, size=(800, 600))
[14]:
../_images/homotopy-julia_FGS_21_0.svg
[15]:
plot_traj_tf(path_tf, y0, yf, ybar, λ₁, f, size=(800, 1200))
┌ Info: Saved animation to
│   fn = /Users/ocots/Boulot/recherche/Logiciels/dev/ct/gallery/examples/homotopy-julia/traj_tf.mp4
└ @ Plots /Users/ocots/.julia/packages/Plots/Ra8fG/src/animation.jl:126
┌ Info: Saved animation to
│   fn = /Users/ocots/Boulot/recherche/Logiciels/dev/ct/gallery/examples/homotopy-julia/traj_tf.gif
└ @ Plots /Users/ocots/.julia/packages/Plots/Ra8fG/src/animation.jl:126
[15]:

thumbnail

Annexes

Flow

Back to Flow

[16]:
;pygmentize Flow.jl
# Flow
using LinearAlgebra
using ForwardDiff
using DifferentialEquations
#using ODEInterfaceDiffEq
#using Sundials

grad(f, x) = ForwardDiff.gradient(f, x)

jac(f, x) = ForwardDiff.jacobian(f, x)

function Flow(h)
    """
    f = Flow(h)
    Build the Hamiltonian flow
    
    Input:
    h : Hamiltonian, function
        trueH = h(x,p;par=[])
        Input:
            x : state, Float(n)
            p : co-state, Float(n)
            par : parameters
        Output:
            trueH : value of the Hamiltonian, Float
    Output
    f : flow, function
        sol = f(tspan, z0; λ=[], optionsODE = Dict()))
        Input:
            tspan : tuple of initial and final time (t0, tf)
            z0 : initial point, float(2n)
        Optional input:
            λ : parameters
            optionsODE : dictionary of options for the numerical integration
        Output:
            sol : solution of the (IVP)
       
        sol = f(t0, z0, tf; λ=[], optionsODE = Dict()))
        Output:
            sol : solution of the (IVP) at tf
    """

    function hvfun(z, λ, t)
        """
            Hvec = hvfun(z, λ, t)
            Compute the second membre of the hamiltonan system
            
            Input:
                z : state and co-state [x ; p], Float(2n)
                λ : parameters
                t : time, Float
            Output:
                Hvec : second member , Float(2n)
        """
        n = size(z, 1)÷2
        if λ==[]
            foo = z -> h(z[1:n], z[n+1:2*n])
        else
            foo = z -> h(z[1:n], z[n+1:2*n],λ)
        end
        dh = grad(foo, z)                      # Automatic differentiation

        return [dh[n+1:2n] ; -dh[1:n]]
    end
    
      function f(tspan, z0; λ=[], optionsODE = Dict())
        
        ode = ODEProblem(hvfun, z0, tspan, λ)
        algo = get(optionsODE,:algo,Tsit5())
        RelTol = get(optionsODE,:reltol,1.e-8)
        AbsTol = get(optionsODE,:abstol,1.e-12)
        saveat = get(optionsODE,:saveat,[])
        sol = solve(ode, algo, reltol = RelTol, abstol = AbsTol, saveat = saveat)
        return sol
    end
    
    function f(t0, z0, tf; λ=[], optionsODE = Dict())
        sol = f((t0, tf), z0, λ=λ, optionsODE = optionsODE)
        deux_n = size(z0, 1)
        return sol[1:deux_n, end]
    end
    
    return f

end

Homotopy

Back to Homotopy

[17]:
;pygmentize Homotopy.jl
# Path following,
# Input
# homotopy and jacobian
#
using LinearAlgebra
using DifferentialEquations
using ForwardDiff
using Plots
using Printf

function Path(F)

    jac(f, x) = ForwardDiff.jacobian(f, x)
    """
    path = Path(F)
    Build the path following 
    
    Input:
    F : Homotopy
        val = F(x,λ)
        Input:
            x : Float(n)
            λ : homotopic parameter Float
        Output:
            val : value of the Homotopy, Float(n)
    Output
        H : pathfollowing function
        sol = H(x0, λ0, λf; sf=1e4, abstol=1e-8, reltol=1e-8, display=true)
            Input:
                x0 : initial point, Foat(n)
                λ0 : initial value of the homotopic parameter, Float
                λf : final value of the homotopic parameter, Float

            Optional input
                sf : max value of sf, Float
                abstol : Absolute tolerance for the numerical integration, Float
                reltol : Relative tolerance for the numerical integration, Float
                display : print result at each integration, Boolean
    
            Output:
                sol : path, solution of the (IVP)
    """       
    
    
    function H(x0, λ0, λf; sf=1e4, abstol=1e-8, reltol=1e-8, display=true, saveat=[])
        n = size(x0, 1)
        first = true
        #old_dc = Array{typeof(x0[1])}(undef, n+1)
        old_dc = zeros(n+1)

        function rhs!(dc, c, par, s)
        """
           Compute de right hand side of the IVP
           rhs!(dc, c, par, s)
           Input
               c : state, Float(n)
               par : parameter (λ0,λf)
               s : independant parameter
           Output
               dc : tangent vector, Float(n)   
        """
            λ0, λf = par
            n     = size(c, 1)-1
            g(c)  = F(c[1:n], c[n+1])
            dF    = jac(g, c)      # dF is the Jacobian Matrix of F

            Q, R  = qr(dF')        # QR decomposation

            dc[:] = Q[:,n+1]       # dc[:] is a vector of norm 1 of the kernel
            # \dot{\lambda} must be of the same signe than (\lambda_f-\lambda0)) the fisrt time
            if first                                
                dc[:] = sign((λf-λ0)*dc[end])*dc
                first = false
            # test the direction of the tangent vector for taking the good one
            else
                dc[:] = sign(dot(old_dc,dc))*dc   
            end
            old_dc[:] = dc
        
        end
    
        function rhsu!(du, u, p, t)
            rhs!(du, u, p, t)
        end
        
        # todo: 
        # vector of parameters : pars(lambda) = (1-lambda) pars0 + lambda parsf
        
        # ode problem
        c0  = [x0; λ0]
        ode = ODEProblem{true}(rhsu!, c0, (0., sf), [λ0; λf])
        
        # callback: termination
        condition(c,t,integrator) = c[end]-λf
        affect!(integrator) = terminate!(integrator)
        cbt  = ContinuousCallback(condition,affect!)

        # callback: projector
        function g!(out,c,p,t)
            out[1:n] = F(c[1:n], c[n+1])
        end
        cbp = ManifoldProjection(g!)
        
        # callback print
        iter = 1
        function cb_display(c, t, integrator)
            @printf("%10d", iter)
            @printf("%16.8e", norm(F(c[1:n], c[n+1])))
            @printf("%16.8e", norm(c[1:n]))
            @printf("%16.8e", c[n+1])
            println()
            iter = iter + 1
        end
        cbd = FunctionCallingCallback(cb_display)
        
        #
        if display
            cb = CallbackSet(cbt, cbp, cbd)
            
            # init print
            println("\n     Calls  |f(x,pars)|     |x|             Homotopic param \n")
        
        else
            cb = CallbackSet(cbt, cbp)
        end
        
        # resolution
        if saveat==[]
            sol = solve(ode, Tsit5(), abstol=abstol, reltol=reltol, callback=cb)
        else
            sol = solve(ode, Tsit5(), abstol=abstol, reltol=reltol, callback=cb, saveat=saveat)
        end
        
        if display
            cf = sol[:,end]
            print("\n Results of the path solver method:\n")
            println(" xf            = ", cf[1:n]);
            println(" parsf         = ", cf[1+n]);
            println(" |F(xf,parsf)| = ", norm(F(cf[1:n], cf[1+n])));
            println(" steps         = ", size(sol.t, 1));
            println(" status        = ", sol.retcode);
        end
        
        return sol
    end
        
    return H
  
end
[18]:
;pygmentize Plottings.jl
using Plots.PlotMeasures

function __size_shoot()
    return (900,600)
end

function __size_path()
    return (800,600)
end

function __size_traj()
    return (800,1200)
end

function plot_shoot(y0, q0, yf, q1, ybar, λ, tf, f; size=__size_shoot())
    
    z0 = *y0+(1-λ)*ybar ; q0]
    ode_sol = f((0.,0.5), z0, λ=tf)
    T1 = ode_sol.t
    X1 = ode_sol[1:2,:]
    P1 = ode_sol[3:4,:]

    z1 = *yf+(1-λ)*ybar ; q1]
    ode_sol = f((1.,0.5), z1, λ=tf)
    T2 = ode_sol.t;      T2 = T2[end:-1:1]
    X2 = ode_sol[1:2,:]; X2 = X2[:,end:-1:1]
    P2 = ode_sol[3:4,:]; P2 = P2[:,end:-1:1]

    T = [T1;T2]
    X = [X1 X2]
    P = [P1 P2]

    px1 = plot(T,X[1,:], xlabel = "t", ylabel = "y₁", legend=false)
    px2 = plot(T,X[2,:], xlabel = "t", ylabel = "y₂", legend=false)
    pp1 = plot(T,P[1,:], xlabel = "t", ylabel = "q₁", legend=false)
    pp2 = plot(T,P[2,:], xlabel = "t", ylabel = "q₂", legend=false)
    p_phase = plot(X[1,:], X[2,:], xlabel = "y₁", ylabel = "y₂", legend=false)
    plot!(p_phase, [ybar[1]], [ybar[2]], color = :blue, seriestype=:scatter, markersize = 5, markerstrokewidth=0)

    display(plot(px1, px2, pp1, pp2, layout=(2,2), size=size))
    display(plot(p_phase, size=size))
    
end

function plot_path_bc(path; size=__size_path())
    # Plots
    px = plot(path, vars=[(1,5) (2,5)], lw=2.0, xlabel = "q", ylabel = "λ", label = ["q₁(0)" "q₂(0)"], legend=:bottomright)
    pp = plot(path, vars=[(3,5) (4,5)], lw=2.0, xlabel = "q", ylabel = "λ", label = ["q₁(1)" "q₂(1)"], legend=:bottomright)

    plot(px, pp, layout = (2,1), size=size)
end

function plot_traj_bc(path, y0, yf, ybar, tf, f, Ind; size=__size_traj())
    
    # Graphics
    ex = 5e-2
    ey = 1e-1
    py1 = plot(xlabel = "t", ylabel = "y₁", legend=false, xlims=(0-ex,1+ex), ylims=(1-ey, 3+ey))
    py2 = plot(xlabel = "t", ylabel = "y₂", legend=false, xlims=(0-ex,1+ex), ylims=(-0.1, 1.05))
    pq1 = plot(xlabel = "t", ylabel = "q₁", legend=false, xlims=(0-ex,1+ex), ylims=(-1.25, 2.0))
    pq2 = plot(xlabel = "t", ylabel = "q₂", legend=false, xlims=(0-ex,1+ex), ylims=(-5, 0))
    p_phase = plot(xlabel = "y₁", ylabel = "y₂", legend=false, xlims=(1,3), ylims=(-0.1, 1.05))
    
    plot!(py1, [0], [y0[1]], color = :black, seriestype=:scatter, markersize = 4, markerstrokewidth=0)
    plot!(py1, [1], [yf[1]], color = :black, seriestype=:scatter, markersize = 4, markerstrokewidth=0)
    plot!(py2, [0], [y0[2]], color = :black, seriestype=:scatter, markersize = 4, markerstrokewidth=0)
    plot!(py2, [1], [yf[2]], color = :black, seriestype=:scatter, markersize = 4, markerstrokewidth=0)
    plot!(p_phase, [ybar[1]], [ybar[2]], color = :blue, seriestype=:scatter, markersize = 5, markerstrokewidth=0)
    
    for i in Ind
        qq_sol = path[1:4,i]
        λ = path[5,i]
        z0 = *y0+(1-λ)*ybar ; qq_sol[1:2]]
        ode_sol = f((0.,0.5), z0, λ=tf, optionsODE=opt)
        T1 = ode_sol.t
        X1 = ode_sol[1:2,:]
        P1 = ode_sol[3:4,:]

        #U1 = 2 .+ P1[2,:]
        z1 = *yf+(1-λ)*ybar ; qq_sol[3:4]]
        ode_sol = f((1.,0.5), z1, λ=tf, optionsODE=opt)
        T2 = ode_sol.t
        X2 = ode_sol[1:2,:]
        P2 = ode_sol[3:4,:]
        T2 = T2[end:-1:1]
        X2 = X2[:,end:-1:1]
        P2 = P2[:,end:-1:1]
        #U2 = 2 .+ P2[2,:]

        T = [T1;T2]
        X = [X1 X2]
        P = [P1 P2]

        #U = [U1 ; U2]
        plot!(py1, T, X[1,:], color=:blue)
        plot!(py2, T, X[2,:], color=:blue)
        plot!(pq1, T, P[1,:], color=:blue)
        plot!(pq2, T, P[2,:], color=:blue)
        plot!(p_phase, X[1,:], X[2,:], color=:blue)
        
        txt = @sprintf("λ=%2.2f", λ)
        plot!(py1, annotation=((0.5,  1.5, txt)), annotationcolor=:red, annotationfontsize=12, annotationhalign=:center)
        plot!(py2, annotation=((0.5,  0.5, txt)), annotationcolor=:red, annotationfontsize=12, annotationhalign=:center)
        plot!(pq1, annotation=((0.5,  0.0, txt)), annotationcolor=:red, annotationfontsize=12, annotationhalign=:center)
        plot!(pq2, annotation=((0.5, -3.0, txt)), annotationcolor=:red, annotationfontsize=12, annotationhalign=:center)
        plot!(p_phase, annotation=((2.0, 0.5, txt)), annotationcolor=:red, annotationfontsize=12, annotationhalign=:center)
        
    end

    p_traj = plot(py1, py2, pq1, pq2, layout = (2,2))
    p_phas = plot(p_phase)
    plot(p_traj, p_phas, layout=(2,1), size=size, left_margin=5mm) #, dpi=200)
    
end

function plot_traj_bc(path, y0, yf, ybar, tf, f; size=__size_traj())
    
    n_path, m_path = Base.size(path[:,:])
    nn = min(m_path, 50)

    vals = range(path[5,1], path[5,end], length=nn)
    Ind = zeros(Int, nn)
    for j in 1:nn
        Ind[j] = argmin(abs.(path[5,:].-vals[j]))
    end
    
    nFrame = length(Ind)
    anim = @animate for i  1:nFrame
        plot_traj_bc(path, y0, yf, ybar, tf, f, Ind[i:i], size=size)
    end
    
    # enregistrement
    duree = 15.0
    fps = nFrame/duree
    gif(anim, "traj_bc.mp4", fps=fps);
    gif(anim, "traj_bc.gif", fps=fps);
    
end

function plot_path_tf(path; size=__size_path())
    py1 = plot(path, vars=[(1,5)], lw=2.0, xlabel = "q", ylabel = "λ", label = "q₁(0)", xlims=(1.8325, 1.84))
    py2 = plot(path, vars=[(2,5)], lw=2.0, xlabel = "q", ylabel = "λ", label = "q₂(0)", xlims=(-4.53, -4.52))
    pq1 = plot(path, vars=[(3,5)], lw=2.0, xlabel = "q", ylabel = "λ", label = "q₁(1)", xlims=(1.159, 1.161))
    pq2 = plot(path, vars=[(4,5)], lw=2.0, xlabel = "q", ylabel = "λ", label = "q₂(1)", xlims=(-1.415, -1.413))

    plot(py1, py2, pq1, pq2, layout = (2,2), size=size)
end

function plot_traj_tf(path, y0, yf, ybar, λ, f, Ind; size=__size_traj())
    
    ex = 5e-2
    ey = 1e-1
    py1 = plot(xlabel = "t", ylabel = "y₁", legend=false, xlims=(0-ex,1+ex), ylims=(1-ey, 3+ey))
    py2 = plot(xlabel = "t", ylabel = "y₂", legend=false, xlims=(0-ex,1+ex), ylims=(-0.1, 1.05))
    pq1 = plot(xlabel = "t", ylabel = "q₁", legend=false, xlims=(0-ex,1+ex), ylims=(-1.25, 2.0))
    pq2 = plot(xlabel = "t", ylabel = "q₂", legend=false, xlims=(0-ex,1+ex), ylims=(-5, 0))
    p_phase = plot(xlabel = "y₁", ylabel = "y₂", legend=false, xlims=(1,3), ylims=(-0.1, 1.05))
    
    for i in Ind
        
        qq_sol = path[1:4,i]
        tf = path[5,i]
        z0 = *y0+(1-λ)*ybar ; qq_sol[1:2]]
        ode_sol = f((0.,0.5), z0, λ=tf, optionsODE=opt)
        T1 = ode_sol.t
        X1 = ode_sol[1:2,:]
        P1 = ode_sol[3:4,:]

        #U1 = 2 .+ P1[2,:]
        z1 = *yf+(1-λ)*ybar ; qq_sol[3:4]]
        ode_sol = f((1.,0.5), z1, λ=tf, optionsODE=opt)
        T2 = ode_sol.t
        X2 = ode_sol[1:2,:]
        P2 = ode_sol[3:4,:]
        T2 = T2[end:-1:1]
        X2 = X2[:,end:-1:1]
        P2 = P2[:,end:-1:1]
        #U2 = 2 .+ P2[2,:]

        T = [T1;T2]
        X = [X1 X2]
        P = [P1 P2]

        #U = [U1 ; U2]
        plot!(py1, T, X[1,:], color=:blue)
        plot!(py2, T, X[2,:], color=:blue)
        plot!(pq1, T, P[1,:], color=:blue)
        plot!(pq2, T, P[2,:], color=:blue)
        plot!(p_phase, X[1,:], X[2,:], color=:blue)
        
        txt = @sprintf("tf=%2.2f", tf)
        plot!(py1, annotation=((0.5,  1.5, txt)), annotationcolor=:red, annotationfontsize=12, annotationhalign=:center)
        plot!(py2, annotation=((0.5,  0.5, txt)), annotationcolor=:red, annotationfontsize=12, annotationhalign=:center)
        plot!(pq1, annotation=((0.5,  0.0, txt)), annotationcolor=:red, annotationfontsize=12, annotationhalign=:center)
        plot!(pq2, annotation=((0.5, -3.0, txt)), annotationcolor=:red, annotationfontsize=12, annotationhalign=:center)
        plot!(p_phase, annotation=((2.0, 0.5, txt)), annotationcolor=:red, annotationfontsize=12, annotationhalign=:center)
        
    end

    plot!(py1, [0], [y0[1]], color = :black, seriestype=:scatter, markersize = 4, markerstrokewidth=0)
    plot!(py1, [1], [yf[1]], color = :black, seriestype=:scatter, markersize = 4, markerstrokewidth=0)
    plot!(py2, [0], [y0[2]], color = :black, seriestype=:scatter, markersize = 4, markerstrokewidth=0)
    plot!(py2, [1], [yf[2]], color = :black, seriestype=:scatter, markersize = 4, markerstrokewidth=0)
    plot!(p_phase, [ybar[1]], [ybar[2]], color = :blue, seriestype=:scatter, markersize = 2, markerstrokewidth=0)
    
    p_traj = plot(py1, py2, pq1, pq2, layout = (2,2))
    p_phas = plot(p_phase)
    plot(p_traj, p_phas, layout=(2,1), size=size, left_margin=5mm) #, dpi=200)
    
end

function plot_traj_tf(path, y0, yf, ybar, λ, f; size=__size_traj())
    
    n_path, m_path = Base.size(path[:,:])
    nn = min(m_path, 50)

    vals = range(path[5,1], path[5,end], length=nn)
    Ind = zeros(Int, nn)
    for j in 1:nn
        Ind[j] = argmin(abs.(path[5,:].-vals[j]))
    end
    
    nFrame = length(Ind)
    anim = @animate for i  1:nFrame
        plot_traj_tf(path, y0, yf, ybar, λ, f, Ind[i:i], size=size)
    end
    
    # enregistrement
    duree = 15.0
    fps = nFrame/duree
    gif(anim, "traj_tf.mp4", fps=fps);
    gif(anim, "traj_tf.gif", fps=fps);
    
end