GitHub

Indirect method

We introduce the pseudo-Hamiltonian

\[ h(x,p,p^0,u) = p^0 x + p u.\]

For the sake of simplicity, we assume that the BC-extremals associated to the solution of the studied problem is normal, and so we fix $p^0 = -1$. According to the Pontryagin maximum principle, the maximizing control is given by $u(x,p) \to \mathrm{sign}(p)$. This function is non-differentiable, and may lead to numerical issues.

Let's start by defining the problem. 

using OptimalControl
using Plots
using ForwardDiff
using DifferentialEquations
using MINPACK

t0 = 0                                                      # initial time
x0 = 0                                                      # initial state
tf = 5                                                      # final time
xf = 0                                                      # final state

@def ocp begin                                              # problem definition

    t ∈ [ t0, tf ], time
    x ∈ R, state
    u ∈ R, control

    x(t0) == x0
    x(tf) == xf

    ẋ(t) == u(t)

    ∫( x(t) ) → min

end

Thanks to the control-toolbox, the flow $\varphi$ of the (true) Hamiltonian

\[ H(x,p) = h(x,p,-1, u(x,p)) = p^0 x + \lvert p \rvert \]

is given by the function $\texttt{Flow}$. The shooting function $S \colon \mathbb{R} \to \mathbb{R}$ is defined by

\[ S(p_0) = \pi \big( \varphi(t_0, x_0, p_0, t_f) \big) - x_f\]

where $\pi (x,p) = x$ is the classical $x$-space projection.

ϕ = Flow(ocp, (x,p) -> sign(p))                             # flow with maximizing control
π((x,p)) = x;                                               # projection on state space

S(p0) = π( ϕ(t0, x0, p0, tf) ) - xf;                        # shooting function
nle = p0 -> [S(p0[1])]                                      # auxiliary function

# Plot
plot(range(-7, 2, 500), S, xlim = [-7, 2])
plot!([-7,2], [0,0], color = :black)
plot!(xlabel = "p0", ylabel = "S", legend=false)
Example block output

Finite difference method

The main goal now is to find the zero of $S$. To this purpose, we use the numerical solver $\texttt{hybrd1}$ given in the package $\texttt{MINPACK.jl}$. If we don't provide the Jacobian $J_S$ of $S$ to the solver, the finite difference method is used to approximate it. 

ξ = [-1.0]                                                  # initial guess
S!(s, ξ) = (s[:] .= S(ξ[1]); nothing)                       # auxiliary function
p0_sol = fsolve(S!, ξ, show_trace = true)                   # solve
println(p0_sol)
Iter     f(x) inf-norm    Step 2-norm      Step time
------   --------------   --------------   --------------
     1     3.000000e+00     0.000000e+00         0.263597
     2     3.077457e+00     9.233870e+00         0.217593
     3     9.829505e-09     2.367685e+00         0.000455
     4     6.491184e-09     2.415479e-17         0.000424
     5     5.237606e-08     9.132615e-17         0.000455
     6     5.775412e-09     7.229585e-17         0.000405
     7     5.237605e-08     7.229558e-17         0.000462
     8     5.201817e-09     5.864837e-17         0.000398
     9     5.237605e-08     5.864847e-17         0.000455
    10     4.731865e-09     4.853008e-17         0.000397
    11     5.237606e-08     4.853029e-17         0.000475
    12     4.339792e-09     4.082120e-17         0.000415
    13     5.237607e-08     4.082138e-17         0.000457
    14     4.007716e-09     3.481324e-17         0.000395
    15     5.237604e-08     3.481267e-17         0.000434
    16     3.722853e-09     3.003963e-17         0.000404
    17     5.237606e-08     3.004001e-17         0.000441
    18     3.475794e-09     2.618526e-17         0.000406
    19     5.237604e-08     2.618500e-17         0.000445
    20     3.259488e-09     2.302731e-17         0.000392
    21     5.237605e-08     2.302742e-17         0.000445
    22     3.068525e-09     2.040827e-17         0.000389
    23     5.237604e-08     2.040814e-17         0.000443
    24     2.898702e-09     1.821171e-17         0.000392
    25     5.237605e-08     1.821185e-17         0.000449
    26     2.746691e-09     1.635181e-17         0.000393
    27     5.237610e-08     1.635236e-17         0.000437
    28     2.609823e-09     1.476333e-17         0.000404
    29     2.090234e-09     5.851517e-19         0.000741
    30     5.237606e-08     9.469727e-18         0.000461
    31     2.010020e-09     8.756841e-18         0.000437
    32     1.609840e-09     3.470936e-19         0.000769
    33     5.237604e-08     5.616972e-18         0.000516
    34     1.561837e-09     5.286974e-18         0.000435
    35     1.250892e-09     2.095646e-19         0.000780
    36     5.237606e-08     3.391488e-18         0.000447
    37     1.221712e-09     3.235115e-18         0.000417
    38     9.784849e-10     1.282284e-19         0.000745
    39     5.237607e-08     2.075235e-18         0.000443
    40     9.605377e-10     1.999817e-18         0.000392
    41     3.304990e-08     7.926384e-20         0.000785
    42     9.551371e-10     7.484981e-20         0.000412
    43     5.237611e-08     1.977570e-18         0.000469
    44     9.380301e-10     1.907369e-18         0.000399
    45     5.237606e-08     1.907158e-18         0.000457
    46     3.289768e-08     1.840638e-18         0.000440
Results of Nonlinear Solver Algorithm
 * Algorithm: Modified Powell
 * Starting Point: [-1.0]
 * Zero: [-2.499999988327954]
 * Inf-norm of residuals: 0.000000
 * Convergence: true
 * Message: algorithm estimates that the relative error between x and the solution is at most tol
 * Total time: 0.501815 seconds
 * Function Calls: 46
 * Jacobian Calls (df/dx): 6
sol = ϕ((t0, tf), x0, p0_sol.x)                             # get the optimal trajectory
plot(sol)                                                   # plot
Example block output

Automatic differentiation (wrong way)

Now, we want to provide $J_S$ to the solver, thanks to the $\texttt{ForwardDiff.jl}$ package. This Jacobian is computed with the variational equation, and leads to a false result in our case.

Details.

Denoting z0=(x0,p0)z_0 = (x_0,p_0) the initial state costate couple, we have

φ(t0,z0,tf)=z0+∫t0tfH⃗(φ(t0,z0,t)) dt. \varphi(t_0, z_0, t_f) = z_0 + \int_{t_0}^{t_f} \vec H\big(\varphi(t_0, z_0, t)\big) \,\mathrm dt.

If we assume that z0→φ(t0,z0,tf)z_0 \to \varphi(t_0, z_0, t_f) is differentiable, we have

∂φ∂z0(t0,z0,tf)⋅δz0=δz0+∫t0tfH⃗′(φ(t0,z0,t))⋅(∂φ∂z0(t0,z0,t)⋅δz0) dt, \frac{\partial \varphi}{\partial z_0}(t_0, z_0, t_f)\cdot \delta z_0 = \delta z_0 + \int_{t_0}^{t_f} \vec H'\big(\varphi(t_0, z_0, t)\big)\cdot \left( \frac{\partial \varphi}{\partial z_0}(t_0, z_0, t) \cdot \delta z_0 \right) \,\mathrm dt,

and so, z0→∂φ∂z0(t0,z0,tf)⋅δz0z_0 \to \frac{\partial \varphi}{\partial z_0}(t_0, z_0, t_f)\cdot \delta z_0 is solution of the variational equations

∂δz∂t(t)=H⃗′(φ(t0,z0,tf))⋅δz(t),δz(t0)=δz0. \frac{\partial \delta z}{\partial t}(t) = \vec H'\big(\varphi(t_0, z_0, t_f)\big) \cdot \delta z(t), \qquad \delta z(t_0) = \delta z_0.

In the studied optimal control problem, we have

H⃗(x,p)=(sign(p),−1) \vec H(x,p) = (\mathrm{sign}(p), -1)

and so, we have H⃗′(z)=02\vec H'(z) = 0_2 almost everywhere, which implies

∂φ∂z0(t0,z0,tf)⋅δz0=exp((tf−t0)02)⋅δz0=δz0. \frac{\partial \varphi}{\partial z_0}(t_0, z_0, t_f) \cdot \delta z_0 = \mathrm{exp}\big((t_f-t_0) 0_2 \big)\cdot \delta z_0 = \delta z_0.

The Jacobian of the shooting function is then given by

S′(p0)=π(∂φ∂p0(t0,x0,p0,tf))=π(∂φ∂z0(t0,z0,tf)⋅(0,1))=π(0,1)=0. S'(p_0) = \pi \left( \frac{\partial \varphi}{\partial p_0}(t_0, x_0, p_0, t_f) \right) = \pi \left( \frac{\partial \varphi}{\partial z_0}(t_0, z_0, t_f) \cdot (0,1) \right) = \pi(0,1) = 0. 

ξ = [-1.0]                                                  # initial guess
JS(ξ) = ForwardDiff.jacobian(p -> [S(p[1])], ξ)             # compute jacobian by forward differentiation
println("ξ = ", ξ[1])
println("JS(ξ) : ", JS(ξ)[1])
ξ = -1.0
JS(ξ) : 0.0

However, the solver $\texttt{hybrd1}$ uses rank 1 approximations to actualize the Jacobian insted of compute it at each iteration, which imply that it still converges to the solution even if the given Jacobian is completely false.

JS!(js, ξ) = (js[:] .= JS(ξ); nothing)                      # auxiliary function
p0_sol = fsolve(S!, JS!, ξ, show_trace = true)              # solve
println(p0_sol)
Iter     f(x) inf-norm    Step 2-norm      Step time
------   --------------   --------------   --------------
     1     3.000000e+00     0.000000e+00         0.007267
     2     5.000000e+00     1.000000e+04         0.057913
     3     5.000000e+00     3.906250e+03         0.000141
     4     5.000000e+00     3.906250e+03         0.000721
     5     5.000000e+00     3.906250e+03         0.000080
     6     5.000000e+00     5.493164e+02         0.000066
     7     5.000000e+00     7.724762e+01         0.000065
     8     9.550782e-01     1.086295e+01         0.000506
     9     5.144214e-08     2.280436e-01         0.000348
    10     3.266365e-09     6.615735e-16         0.000412
    11     2.081820e-09     3.041236e-18         0.000401
    12     5.237605e-08     9.393622e-18         0.000443
    13     2.002238e-09     8.689149e-18         0.000414
Results of Nonlinear Solver Algorithm
 * Algorithm: Modified Powell (User Jac, Expert)
 * Starting Point: [-1.0]
 * Zero: [-2.499999989894703]
 * Inf-norm of residuals: 0.000000
 * Convergence: true
 * Message: algorithm estimates that the relative error between x and the solution is at most tol
 * Total time: 0.068793 seconds
 * Function Calls: 13
 * Jacobian Calls (df/dx): 2
sol = ϕ((t0, tf), x0, p0_sol.x)                             # get the optimal trajectory
plt = plot(sol)                                             # plot
Example block output

Automatic differentiation (good way)

The goal is to provide the true Jacobian of $S$ by using the $\texttt{ForwardDiff}$ package, and so we need to indicate to the solver that the dynamic of the system change when $p = 0$.

To understand why we need to give this information to the solver, see the following details.

Details.

The problem is that the Hamiltonian HH is not differentiable everywhere due to the maximizing control. This control is bang-bang (u=1u = 1 and u=−1u = -1).

Let now construct the two smooth Hamiltonians associated to these two controls

H+(x,p)=h(x,p,−1,1)=−x+pandH−(x,p)=h(x,p,−1,−1)=−x−p. H^+(x,p) = h(x,p,-1,1) = -x + p \qquad \text{and} \qquad H^-(x,p) = h(x,p,-1,-1) = -x - p.

Their associated vector fields are given by

H⃗+(x,p)=(1,1)andH⃗−(x,p)=(−1,1), \vec H^+(x,p) = (1,1) \qquad \text{and} \qquad \vec H^-(x,p) = (-1, 1),

and their associated flow correspond to

φ+(t0,z0,tf)=z0+(11)(tf−t0)andφ−(t0,z0,tf)=z0+(−1−1)(tf−t0). \varphi^+(t_0, z_0, t_f) = z_0 + \left( \begin{array}{c} 1 \\ 1 \end{array} \right) (t_f -t_0) \qquad \text{and} \qquad \varphi^-(t_0, z_0, t_f) = z_0 + \left( \begin{array}{c} -1 \\ \phantom{-} 1 \end{array} \right) (t_f -t_0).

If we assume that the optimal structure of the problem is negative then positive bangs, then the associated flow is defined by

φ(t0,z0,tf)=φ+(t1(z0),φ−(t0,z0,t1(z0)),tf), \varphi(t_0, z_0, t_f) = \varphi^+ \big( t_1(z_0), \varphi^-\big(t_0, z_0, t_1(z_0)\big), t_f \big),

with the following condition

πp(φ−(t0,z0,t1(z0)))=0, \pi_p \big( \varphi^-(t_0, z_0, t_1(z_0)) \big) = 0,

where πp(x,p)=p\pi_p(x,p) = p is the classical pp-space projection. By devlopping this last condition, an explicit form of the function t1(⋅)t_1(\cdot) is given by

t1(x0,p0)=t0−p0. t_1(x_0, p_0) = t_0 - p_0.

Finally, we have

∂φ∂z0=∂φ+∂t0∂t1∂z0+φ+∂z0(∂φ−∂z0+∂φ−∂tf∂t1∂z0)=(−1−1)(0−1)+(1001)[(1001)+(−1−1)(0−1)]=(0101)+(1001)+(0−10−1)=(1201) \begin{align*} \frac{\partial \varphi}{\partial z_0} &= \frac{\partial \varphi^+}{\partial t_0} \frac{\partial t_1}{\partial z_0} + \frac{\varphi^+}{\partial z_0} \left( \frac{\partial \varphi^-}{\partial z_0} + \frac{\partial \varphi^-}{\partial t_f} \frac{\partial t_1}{\partial z_0} \right) \\ &= \left( \begin{array}{c} -1 \\ -1 \end{array} \right) \left( \begin{array}{cc}0 & -1 \end{array} \right) + \left( \begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array} \right) \left[ \left( \begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array} \right) + \left( \begin{array}{c} -1 \\ \phantom - 1 \end{array} \right) \left( \begin{array}{cc}0 & -1 \end{array} \right) \right] \\ &= \left( \begin{array}{cc} 0 & 1 \\ 0 & 1 \end{array} \right) + \left( \begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array} \right) + \left( \begin{array}{cc} 0 & \phantom -1 \\ 0 & -1 \end{array} \right) \\ &= \left( \begin{array}{cc} 1 & 2 \\ 0 & 1 \end{array} \right) \end{align*}

and so, we have that

S′(p0)=π(∂φ∂p0(t0,x0,p0,tf))=π(∂φ∂z0(t0,z0,tf)⋅(0,1))=π(2,1)=2. S'(p_0) = \pi \left( \frac{\partial \varphi}{\partial p_0}(t_0, x_0, p_0, t_f) \right) = \pi \left( \frac{\partial \varphi}{\partial z_0}(t_0, z_0, t_f) \cdot (0,1) \right) = \pi(2,1) = 2.

To provide this change of dynamic to the solver, we need to use a callback during the integration that will execute the function $\texttt{affect!}$ when $\texttt{condition(x,p)} = 0$.

For us, the condition is given by $(x,p) \to p$. For the $\texttt{affect!}$ function, we use a global parameter $\alpha$. This parameter will be set to $\pm 1$ at the beginning of the integration and it sign will change with the $\texttt{affect!}$ function.

Thanks to the $\texttt{control-toolbox}$ package, the created callback can be easily pass to the integrator throught the $\texttt{Flow}$ function.

global α                                                    # parameter: ̇p(t) = α with α = ±1

function condition(z, t, integrator)                        # event when condition(x,p) == 0
    x,p = z
    return p
end

function affect!(integrator)                                # action when condition == 0
    global α = -α
    nothing
end

cb = ContinuousCallback(condition, affect!)                 # callback

φ_ = Flow(ocp, (x,p) -> α, callback = cb)                   # intermediate flow

function φ(t0, x0, p0, tf; kwargs...)                       # flow
    global α = sign(p0)
    return φ_(t0, x0, p0, tf; kwargs...)
end

function φ((t0, tf), x0, p0; kwargs...)                     # flow for plot
    global α = sign(p0)
    return φ_((t0, tf), x0, p0; kwargs...)
end

Shoot(p0) =  π( φ(t0, x0, p0, tf) ) - xf                    # shooting function

ξ = [-1.0]                                                  # initial guess
JShoot(ξ) = ForwardDiff.jacobian(p -> [Shoot(p[1])], ξ)     # compute jacobian by forward differentiation
println("ξ = ", ξ[1])
println("JS(ξ) : ", JShoot(ξ)[1])
Shoot!(shoot, ξ) = (shoot[:] .= Shoot(ξ[1]); nothing)       # auxiliary function
JShoot!(jshoot, ξ) = (jshoot[:] .= JShoot(ξ); nothing)      # auxiliary function

p0_sol = fsolve(Shoot!, JShoot!, ξ, show_trace = true)      # solve
println(p0_sol)
ξ = -1.0
JS(ξ) : 2.0
Iter     f(x) inf-norm    Step 2-norm      Step time
------   --------------   --------------   --------------
     1     3.000000e+00     0.000000e+00         3.724867
     2     2.553513e-15     2.250000e+00         0.008959
     3     7.771561e-16     1.774937e-30         0.000158
Results of Nonlinear Solver Algorithm
 * Algorithm: Modified Powell (User Jac, Expert)
 * Starting Point: [-1.0]
 * Zero: [-2.5000000000000013]
 * Inf-norm of residuals: 0.000000
 * Convergence: true
 * Message: algorithm estimates that the relative error between x and the solution is at most tol
 * Total time: 3.734005 seconds
 * Function Calls: 3
 * Jacobian Calls (df/dx): 1
# get optimal trajectory
sol = φ((t0, tf), x0, p0_sol.x[1], saveat=range(t0, tf, 500))

# plot
t = time_grid(sol)
x = state(sol)
p = costate(sol)
u = sign ∘ p

plt_x = plot(t, x, label = "x")
plt_p = plot(t, p, label = "p")
plt_u = plot(t, u, label = "u")

plt_xp = plot(plt_x, plt_p, layout=(1, 2))
plot(plt_xp, plt_u, layout = (2, 1))
Example block output