Goddard examples plot

[jbcaillau]

• [x] use plot facilities on ODE sol to simplify the code (no need for [ x[i][1] … ] stuff

[jbcaillau]

@ocots

• define a subtype of ODE solution plots to take into account controls and plot them [^*]
• probably needs a proper subtype of flows when the flow comes from an ocp (with a provided maximising control)
• for such flows, take into account controls when concatenating

See also: plot for ODE sol

[^*]: a minima pour avoir (comme en direct avec le plot(ocp)) un truc du style

[ocots]

For the moment, I think that it would be simpler and more coherent to have all flows in the same package, including flows from ocp with control. Hence, I need to rename the package HamiltonianFlows since the name is not correct for such a purpose. It could be ControlFlows if we think that this package may be used independently from OptimalControl or CTFlows if it would be just an internal package (to be consistent with other CT packages).

[ocots]

For the moment, a flow is given by the structure

# to specify D, U and T is useful to insure coherence for instance when concatenating two flows
struct CTFlow{D, U, T}
    f::Function      # the mere function which depends on the kind of flow (Hamiltonian or classical) 
                     # this function takes a right and side as input
    rhs!::Function   # the right and side of the form: rhs!(du::D, u::U, p, t::T)
    tstops::Times    # specific times where the integrator must stop
                     # useful when the rhs is not smooth at such times
    # constructors
    CTFlow{D, U, T}(f, rhs!) where {D, U, T} = new{D, U, T}(f, rhs!, Vector{Time}())
    CTFlow{D, U, T}(f, rhs!, tstops) where {D, U, T} = new{D, U, T}(f, rhs!, tstops)
end

and a call to a flow is given by

(F::CTFlow)(args...; kwargs...) = F.f(args...; _t_stops_interne=F.tstops, DiffEqRHS=F.rhs!, kwargs...)

[ocots]

For the moment, in the [Goddard problem](), the plot of the solution of a flow is made like this:

f1sb0 = f1 * (t1, fs) * (t2, fb) * (t3, f0) # concatenation of the Hamiltonian flows
flow_sol = f1sb0((t0, tf), x0, p0)
r_plot = plot(flow_sol, idxs=(0, 1), xlabel="t", label="r")
v_plot = plot(flow_sol, idxs=(0, 2), xlabel="t", label="v")
m_plot = plot(flow_sol, idxs=(0, 3), xlabel="t", label="m")
pr_plot = plot(flow_sol, idxs=(0, 4), xlabel="t", label="p_r")
pv_plot = plot(flow_sol, idxs=(0, 5), xlabel="t", label="p_v")
pm_plot = plot(flow_sol, idxs=(0, 6), xlabel="t", label="p_m")
plot(r_plot, pr_plot, v_plot, pv_plot, m_plot, pv_plot, layout=(3, 2), size=(600, 300))

The goal is to make a simple call like this and we must have also the plot of the control function:

f1sb0 = f1 * (t1, fs) * (t2, fb) * (t3, f0) # concatenation of the Hamiltonian flows
flow_sol = f1sb0((t0, tf), x0, p0)
plot(flow_sol)

Assumptions.

• we assume that the package is internal and is called CTFlows.
• we assume that we have all the needed information in flow_sol.

Then, the function plot can be simply given by:

function CTFlows.plot(sol::OptimalControlFlowSolution)
   ocp_sol = OptimalControlSolution(sol) # from a flow (from ocp and control) solution to an OptimalControlSolution
   CTBase.plot(ocp_sol)
end

To simulate the fact that the OptimalControlFlowSolution is a subtype of a DifferentialEquations solution, recalling that there is no inheritance, then we can also define

function CTFlows.plot(sol::OptimalControlFlowSolution, args...; kwargs...)
   ode_sol = DifferentialEquationsSolution(sol) 
   Plots.plot(ode_sol, args...; kwargs...)
end

[ocots]

We assume here that to achieve our goal, we aren't going to make a code too generic, that is we are allowed to make some redundancy in the code.

First, we consider a new structure for flows from ocp with control:

# to specify D, U and T is useful to insure coherence for instance when concatenating two flows
struct OptimalControlFlow{D, U, T}
    # 
    f::Function      # the mere function which depends on the kind of flow (Hamiltonian or classical) 
                     # this function takes a right and side as input
    rhs!::Function   # the right and side of the form: rhs!(du::D, u::U, p, t::T)
    tstops::Times    # specific times where the integrator must stop
                     # useful when the rhs is not smooth at such times
    feedback_control::ControlFunction # the control law in feedback form, that is u(t, x, p)
    control_dimension::Dimension
    control_labels::Vector{String}
    state_dimension::Dimension
    state_labels::Vector{String}

    # constructors
    function OptimalControlFlow{D, U, T}(f::Function, rhs!::Function, tstops::Times=Vector{Time}(), 
       u::Function, m::Dimension, u_labels::Vector{String}, n::Dimension, x_labels::Vector{String}) where {D, U, T} 
       return new{D, U, T}(f, rhs!, tstops, u, m, u_labels, n, x_labels)
    end

end

We still need:

• [x] a new way to call the flow which returns a new structure containing all the needed information for plot
• [x] a new structure representing a solution of an OptimalControlFlow
• [x] to take into account the concatenation of two OptimalControlFlow
• [x] to redefine the function Flow(ocp, u)
• [x] a functor from an OptimalControlFlowSolution to an OptimalControlSolution.

[ocots]

Remark. When this is done, I can replace the structure CTFlow by ClassicalFlow and create also a HamiltonianFlow and a corresponding plot which plots the state, the adjoint vector and the Hamiltonian. We need again some labels. We could have a phase portrait plot if the dimension of the state is 1.

[ocots]

An OptimalControlFlowSolution must contain the solution of the DifferentialEquations package and the other useful information:

struct OptimalControlFlowSolution
    # 
    ode_sol
    feedback_control::ControlFunction # the control law in feedback form, that is u(t, x, p)
    control_dimension::Dimension
    control_labels::Vector{String}
    state_dimension::Dimension
    state_labels::Vector{String}
end

[ocots]

The concatenation must concatenate also the control in feedback form:

# concatenate two flows with a prescribed switching time
function concatenate(F:: OptimalControlFlow{D, U, T}, g::Tuple{MyNumber, OptimalControlFlow{D, U, T}}) where {D, U, T}

    # concatenation of the right and sides
    t_switch, G = g
    function rhs!(du::D, u::U, p, t::T)
        t < t_switch ? F.rhs!(du, u, p, t) : G.rhs!(du, u, p, t)
    end

    # time to break integration 
    tstops = F.tstops
    append!(tstops, G.tstops)
    append!(tstops, t_switch)
    tstops = unique(sort(tstops))

    # control law in feedback form: must be a ControlFunction
    # nonautonomous and vectorial usage for this function which only redirect the call
    function _feedback_control(t, x, u)
        t < t_switch ? F.feedback_control(t, x, u) : G.feedback_control(t, x, u)
    end
    feedback_control = ControlFunction{:nonautonomous, :vectorial}(_feedback_control)

    # we choose default values of F
    return OptimalControlFlow{D, U, T}(F.f, rhs!, tstops, feedback_control, 
       F.control_dimension, F.control_labels, F.state_dimension, F.state_labels)

end

*(F:: OptimalControlFlow{D, U, T}, g::Tuple{MyNumber, OptimalControlFlow{D, U, T}}) where {D, U, T} = concatenate(F, g)

[ocots]

The construction of an OptimalControlFlow can be done this way:

function Flow(ocp::OptimalControlModel{time_dependence}, u_::Function; alg=__alg(), abstol=__abstol(), 
    reltol=__reltol(), saveat=__saveat(), kwargs_Flow...) where {time_dependence}

    #  data
    p⁰ = -1.
    f  = dynamics(ocp)
    f⁰ = lagrange(ocp)
    s  = ismin(ocp) ? 1.0 : -1.0 #

    # construction of the Hamiltonian
    if f ≠ nothing
        u = ControlFunction{time_dependence}(u_) # coherence is needed on the time dependence
        h = Hamiltonian{:nonautonomous}(f⁰ ≠ nothing ? makeH(f, u, f⁰, p⁰, s) : makeH(f, u))
    else 
        error("no dynamics in ocp")
    end

    # right and side: same as for a flow from a Hamiltonian
    rhs! = rhs(h)

    # flow function
    f = __Hamiltonian_Flow(alg, abstol, reltol, saveat; kwargs_Flow...)

    # construction of the OptimalControlFlow
    return OptimalControlFlow{D, U, T}(f, rhs!, u,   # no tstops, so value by default
       control_dimension(ocp), control_labels(ocp), state_dimension(ocp), state_labels(ocp))

end

where

function rhs(h:Hamiltonian)
    function rhs!(dz::DCoTangent, z::CoTangent, λ, t::Time)
        n = size(z, 1) ÷ 2
        foo = isempty(λ) ? (z -> h(t, z[1:n], z[n+1:2*n])) : (z -> h(t, z[1:n], z[n+1:2*n], λ...))
        dh = ctgradient(foo, z)
        dz[1:n] = dh[n+1:2n]
        dz[n+1:2n] = -dh[1:n]
    end
    return rhs!
end

We also need a flow from an ocp, a control and a constraint with its Lagrange multiplier.

function Flow(ocp::OptimalControlModel{time_dependence}, u_::Function, g_::Function, μ_::Function; alg=__alg(), 
abstol=__abstol(), reltol=__reltol(), saveat=__saveat(), kwargs_Flow...) where {time_dependence}

    # data
    p⁰ = -1.
    f  = dynamics(ocp)
    f⁰ = lagrange(ocp)
    s  = ismin(ocp) ? 1.0 : -1.0 #

    # construction of the Hamiltonian
    if f ≠ nothing
        u = ControlFunction{time_dependence}(u_) # coherence is needed on the time dependence
        g = MixedConstraintFunction{time_dependence}(g_)
        μ = MultiplierFunction{time_dependence}(μ_)
        h = Hamiltonian{:nonautonomous}(f⁰ ≠ nothing ? makeH(f, u, f⁰, p⁰, s, g, μ) : makeH(f, u, g, μ))
    else 
        error("no dynamics in ocp")
    end

    # right and side: same as for a flow from a Hamiltonian
    rhs! = rhs(h)

    # flow function
    f = __Hamiltonian_Flow(alg, abstol, reltol, saveat; kwargs_Flow...)

    # construction of the OptimalControlFlow
    return OptimalControlFlow{D, U, T}(f, rhs!, u,   # no tstops, so value by default
       control_dimension(ocp), control_labels(ocp), state_dimension(ocp), state_labels(ocp))

end

[ocots]

The functor:

function OptimalControlSolution(ocfs::OptimalControlFlowSolution)
    n = ocfs.state_dimension
    T = ocfs.ode_sol.t
    x(t) = ocfs.ode_sol(t)[1:n]
    p(t) = ocfs.ode_sol(t)[1+n:2n]
    u(t) = ocfs.feedback_control(t, x(t), p(t))
    sol = OptimalControlSolution()
    sol.state_dimension = n
    sol.control_dimension = ocfs.control_dimension
    sol.times = T
    sol.state = t -> x(t)
    sol.state_labels = ocfs.state_labels
    sol.adjoint = t -> p(t)
    sol.control = t -> u(t)
    sol.control_labels = ocfs.control_labels
    return sol
end

[ocots]

The new call to an OptimalControlFlow:

(F::OptimalControlFlow)(args...; kwargs...) = F.f(args...; _t_stops_interne=F.tstops, DiffEqRHS=F.rhs!, kwargs...)

function (F::OptimalControlFlow)(tspan::Tuple{Time,Time}, args...; kwargs...) 
    ode_sol = F.f(tspan, args...; _t_stops_interne=F.tstops, DiffEqRHS=F.rhs!, kwargs...)
    ocfs = OptimalControlFlowSolution(ode_sol, F.feedback_control, F.control_dimension,
            F.control_labels, F.state_dimension, F.state_labels)
    return ocfs
end

[ocots]

Done! See: https://github.com/control-toolbox/OptimalControl.jl/blob/main/examples/goddard_f.ipynb

But!

Direct

Indirect

[ocots]

The problem existed already: https://ct.gitlabpages.inria.fr/gallery/goddard-j/goddard.html

[ocots]

Okidoki, that's because the constraint on the mass is not handled the same between the direct and indirect method. No problemo. Actually, to get the same adjoint, we should add a state constraint and so a multiplier in the Hamiltonian.