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.270727
     2     3.077457e+00     9.233870e+00         0.223057
     3     9.829505e-09     2.367685e+00         0.000467
     4     6.491184e-09     2.415479e-17         0.000437
     5     5.237606e-08     9.132615e-17         0.000475
     6     5.775412e-09     7.229585e-17         0.000420
     7     5.237605e-08     7.229558e-17         0.000470
     8     5.201817e-09     5.864837e-17         0.000423
     9     5.237605e-08     5.864847e-17         0.000468
    10     4.731865e-09     4.853008e-17         0.000421
    11     5.237606e-08     4.853029e-17         0.000474
    12     4.339792e-09     4.082120e-17         0.000412
    13     5.237607e-08     4.082138e-17         0.000479
    14     4.007716e-09     3.481324e-17         0.000423
    15     5.237604e-08     3.481267e-17         0.000484
    16     3.722853e-09     3.003963e-17         0.000418
    17     5.237606e-08     3.004001e-17         0.000480
    18     3.475794e-09     2.618526e-17         0.000415
    19     5.237604e-08     2.618500e-17         0.000496
    20     3.259488e-09     2.302731e-17         0.000436
    21     5.237605e-08     2.302742e-17         0.000465
    22     3.068525e-09     2.040827e-17         0.000437
    23     5.237604e-08     2.040814e-17         0.000471
    24     2.898702e-09     1.821171e-17         0.000426
    25     5.237605e-08     1.821185e-17         0.000465
    26     2.746691e-09     1.635181e-17         0.000429
    27     5.237610e-08     1.635236e-17         0.000469
    28     2.609823e-09     1.476333e-17         0.000427
    29     2.090234e-09     5.851517e-19         0.000763
    30     5.237606e-08     9.469727e-18         0.000475
    31     2.010020e-09     8.756841e-18         0.000424
    32     1.609840e-09     3.470936e-19         0.000773
    33     5.237604e-08     5.616972e-18         0.000486
    34     1.561837e-09     5.286974e-18         0.000419
    35     1.250892e-09     2.095646e-19         0.000772
    36     5.237606e-08     3.391488e-18         0.000478
    37     1.221712e-09     3.235115e-18         0.000436
    38     9.784849e-10     1.282284e-19         0.000775
    39     5.237607e-08     2.075235e-18         0.000489
    40     9.605377e-10     1.999817e-18         0.000419
    41     3.304990e-08     7.926384e-20         0.000807
    42     9.551371e-10     7.484981e-20         0.000464
    43     5.237611e-08     1.977570e-18         0.000548
    44     9.380301e-10     1.907369e-18         0.000455
    45     5.237606e-08     1.907158e-18         0.000491
    46     3.289768e-08     1.840638e-18         0.000483
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.515446 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.007900
     2     5.000000e+00     1.000000e+04         0.063682
     3     5.000000e+00     3.906250e+03         0.000143
     4     5.000000e+00     3.906250e+03         0.000696
     5     5.000000e+00     3.906250e+03         0.000076
     6     5.000000e+00     5.493164e+02         0.000066
     7     5.000000e+00     7.724762e+01         0.000068
     8     9.550782e-01     1.086295e+01         0.000494
     9     5.144214e-08     2.280436e-01         0.000372
    10     3.266365e-09     6.615735e-16         0.000417
    11     2.081820e-09     3.041236e-18         0.000434
    12     5.237605e-08     9.393622e-18         0.000454
    13     2.002238e-09     8.689149e-18         0.000400
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.075216 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.958904
     2     2.553513e-15     2.250000e+00         0.010058
     3     7.771561e-16     1.774937e-30         0.000199
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.969188 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