Direct and indirect methods for the Goddard problem
Introduction
For this example, we consider the well-known Goddard problem[1] [2] which models the ascent of a rocket through the atmosphere, and we restrict here ourselves to vertical (one dimensional) trajectories. The state variables are the altitude $r$, speed $v$ and mass $m$ of the rocket during the flight, for a total dimension of 3. The rocket is subject to gravity $g$, thrust $u$ and drag force $D$ (function of speed and altitude). The final time $t_f$ is free, and the objective is to reach a maximal altitude with a bounded fuel consumption.
We thus want to solve the optimal control problem in Mayer form
\[ r(t_f) \to \max\]
subject to the controlled dynamics
\[ \dot{r} = v, \quad \dot{v} = \frac{T_{\max}\,u - D(r,v)}{m} - g, \quad \dot{m} = -u,\]
and subject to the control constraint $u(t) \in [0,1]$ and the state constraint $v(t) \leq v_{\max}$. The initial state is fixed while only the final mass is prescribed.
The Hamiltonian is affine with respect to the control, so singular arcs may occur, as well as constrained arcs due to the path constraint on the velocity (see below).
We import the OptimalControl.jl package to define the optimal control problem and NLPModelsIpopt.jl to solve it. We import the Plots.jl package to plot the solution. The OrdinaryDiffEq.jl package is used to define the shooting function for the indirect method and the MINPACK.jl package permits to solve the shooting equation.
using BenchmarkTools
using OptimalControl # to define the optimal control problem and more
using NLPModelsIpopt # to solve the problem via a direct method
using OrdinaryDiffEq # to get the Flow function from OptimalControl
using MINPACK # NLE solver: use to solve the shooting equation
using Plots # to plot the solutionOptimal control problem
We define the problem
const t0 = 0 # initial time
const r0 = 1 # initial altitude
const v0 = 0 # initial speed
const m0 = 1 # initial mass
const vmax = 0.1 # maximal authorized speed
const 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, (1)
0 ≤ u(t) ≤ 1
r(t) ≥ r0
0 ≤ v(t) ≤ vmax
ẋ(t) == F0(x(t)) + u(t) * F1(x(t))
-r(tf) → min
end
# Dynamics
const Cd = 310
const Tmax = 3.5
const β = 500
const 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]
endDirect method
We then solve it
direct_sol = solve(ocp; grid_size=250)▫ This is OptimalControl version v1.1.3 running with: direct, adnlp, ipopt.
▫ The optimal control problem is solved with CTDirect version v0.17.3.
┌─ The NLP is modelled with ADNLPModels and solved with NLPModelsIpopt.
│
├─ Number of time steps⋅: 250
└─ Discretisation scheme: midpoint
▫ This is Ipopt version 3.14.19, running with linear solver MUMPS 5.8.1.
Number of nonzeros in equality constraint Jacobian...: 4504
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 5259
Total number of variables............................: 1004
variables with only lower bounds: 251
variables with lower and upper bounds: 501
variables with only upper bounds: 0
Total number of equality constraints.................: 754
Total number of inequality constraints...............: 0
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 0
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 -1.0100000e+00 9.00e-01 2.00e+00 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 -1.0091553e+00 8.99e-01 4.39e+01 1.1 1.19e+02 - 4.10e-03 8.29e-04f 1
2 -1.0076086e+00 8.85e-01 3.41e+01 0.4 8.46e+00 - 2.02e-02 1.54e-02f 1
3 -1.0015744e+00 7.58e-01 6.11e+02 0.5 6.85e+00 - 4.82e-01 1.44e-01f 1
4 -1.0040637e+00 6.90e-01 1.28e+04 1.6 7.40e+00 - 6.88e-01 8.85e-02h 1
5 -1.0054084e+00 6.10e-01 3.48e+04 -4.6 1.49e+00 - 1.82e-01 1.17e-01h 1
6 -1.0056836e+00 5.93e-01 3.47e+04 1.7 7.88e+00 - 2.25e-01 2.72e-02f 1
7 -1.0069013e+00 5.45e-01 1.39e+05 0.7 1.49e+00 - 2.86e-01 8.07e-02f 1
8 -1.0092896e+00 4.27e-01 1.02e+05 -0.2 1.36e+00 - 2.79e-01 2.16e-01f 1
9 -1.0120862e+00 6.66e-02 1.23e+05 -0.1 7.25e-01 - 3.54e-01 8.44e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 -1.0082072e+00 1.19e-02 2.72e+04 -0.1 3.68e-01 - 6.09e-01 8.21e-01h 1
11 -1.0072406e+00 1.32e-04 1.56e+04 -0.3 6.22e-01 - 7.62e-01 9.90e-01h 1
12 -1.0071725e+00 1.27e-06 6.02e+02 -1.3 5.52e-02 - 9.75e-01 9.91e-01f 1
13 -1.0071706e+00 1.30e-10 4.30e+02 -7.1 2.43e-04 - 9.89e-01 1.00e+00h 1
14 -1.0071744e+00 2.57e-09 1.04e+03 -9.1 2.33e-03 - 9.59e-01 1.00e+00h 1
15 -1.0072577e+00 1.17e-06 8.03e+02 -10.5 5.42e-02 - 2.29e-01 1.00e+00h 1
16 -1.0077011e+00 4.13e-05 5.10e+02 -5.9 3.76e-01 - 2.60e-01 1.00e+00h 1
17 -1.0078865e+00 4.11e-05 4.36e+02 -5.3 1.07e+00 - 3.14e-01 1.11e-01h 1
18 -1.0083156e+00 2.01e-04 4.24e+03 -4.3 1.01e+01 - 1.00e+00 7.10e-02f 1
19 -1.0088455e+00 1.37e-05 1.62e+03 -10.4 6.80e-02 - 6.57e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 -1.0106159e+00 5.71e-04 3.36e+02 -4.9 6.11e-01 - 1.00e+00 7.44e-01h 1
21 -1.0099324e+00 1.58e-04 2.79e-03 -4.6 2.67e-01 - 1.00e+00 1.00e+00h 1
22 -1.0107923e+00 9.18e-06 3.51e-03 -5.4 6.19e-02 - 1.00e+00 1.00e+00h 1
23 -1.0122090e+00 3.87e-05 7.30e+01 -6.2 1.13e-01 - 1.00e+00 7.90e-01h 1
24 -1.0124950e+00 5.99e-06 1.10e+00 -6.4 1.02e-01 - 1.00e+00 9.89e-01h 1
25 -1.0125526e+00 3.20e-06 3.84e-05 -6.9 7.60e-02 - 1.00e+00 1.00e+00h 1
26 -1.0125709e+00 1.92e-06 1.48e-05 -7.5 7.82e-02 - 1.00e+00 1.00e+00h 1
27 -1.0125736e+00 6.35e-07 1.68e-01 -7.9 8.44e-02 - 1.00e+00 8.93e-01h 1
28 -1.0125762e+00 3.57e-07 1.66e-06 -8.6 7.16e-02 - 1.00e+00 1.00e+00h 1
29 -1.0125766e+00 2.93e-07 2.73e-07 -9.7 7.43e-02 - 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 -1.0125767e+00 5.86e-08 3.10e-08 -10.9 4.30e-02 - 1.00e+00 1.00e+00h 1
31 -1.0125767e+00 1.86e-09 3.65e-10 -11.0 1.01e-02 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 31
(scaled) (unscaled)
Objective...............: -1.0125766794048943e+00 -1.0125766794048943e+00
Dual infeasibility......: 3.6466813327921060e-10 3.6466813327921060e-10
Constraint violation....: 1.0544083384189662e-09 1.8595087136752399e-09
Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00
Complementarity.........: 2.1516659028753746e-11 2.1516659028753746e-11
Overall NLP error.......: 1.0544083384189662e-09 1.8595087136752399e-09
Number of objective function evaluations = 32
Number of objective gradient evaluations = 32
Number of equality constraint evaluations = 32
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 32
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 31
Total seconds in IPOPT = 2.408
EXIT: Optimal Solution Found.and plot the solution
plt = plot(direct_sol, label="direct", size=(800, 800))
Structure of the solution
We first determine visually the structure of the optimal solution which is composed of a bang arc with maximal control, followed by a singular arc, then by a boundary arc and the final arc is with zero control. Note that the switching function vanishes along the singular and boundary arcs.
t = time_grid(direct_sol) # the time grid as a vector
x = state(direct_sol) # the state as a function of time
u = control(direct_sol) # the control as a function of time
p = costate(direct_sol) # the costate as a function of time
H1 = Lift(F1) # H1(x, p) = p' * F1(x)
φ(t) = H1(x(t), p(t)) # switching function
g(x) = vmax - x[2] # state constraint v ≤ vmax
u_plot = plot(t, u, linewidth=2, label = "u(t)")
H1_plot = plot(t, φ, linewidth=2, label = "H₁(x(t), p(t))")
g_plot = plot(t, g ∘ x, linewidth=2, label = "g(x(t))")
plot(u_plot, H1_plot, g_plot, layout=(3,1), size=(600, 600))
We are now in position to solve the problem by an indirect shooting method. We first define the four control laws in feedback form and their associated flows. For this we need to compute some Lie derivatives, namely Poisson brackets of Hamiltonians (themselves obtained as lifts to the cotangent bundle of vector fields), or derivatives of functions along a vector field. For instance, the control along the minimal order singular arcs is obtained as the quotient
\[u_s = -\frac{H_{001}}{H_{101}}\]
of length three Poisson brackets:
\[H_{001} = \{H_0,\{H_0,H_1\}\}, \quad H_{101} = \{H_1,\{H_0,H_1\}\}\]
where, for two Hamiltonians $H$ and $G$,
\[\{H,G\} := (\nabla_p H|\nabla_x G) - (\nabla_x H|\nabla_p G).\]
While the Lie derivative of a function $f$ wrt. a vector field $X$ is simply obtained as
\[(X \cdot f)(x) := f'(x) \cdot X(x),\]
and is used to the compute the control along the boundary arc,
\[u_b(x) = -(F_0 \cdot g)(x) / (F_1 \cdot g)(x),\]
as well as the associated multiplier for the order one state constraint on the velocity:
\[\mu(x, p) = H_{01}(x, p) / (F_1 \cdot g)(x).\]
The Poisson bracket $\{H,G\}$ is also given by the Lie derivative of $G$ along the Hamiltonian vector field $X_H = (\nabla_p H, -\nabla_x H)$ of $H$, that is
\[ \{H,G\} = X_H \cdot G\]
which is the reason why we use the @Lie macro to compute Poisson brackets below.
With the help of differential geometry primitives, these expressions are straightforwardly translated into Julia code:
# Controls
const u0 = 0 # off control
const u1 = 1 # bang control
H0 = Lift(F0) # H0(x, p) = p' * F0(x)
H01 = @Lie {H0, H1}
H001 = @Lie {H0, H01}
H101 = @Lie {H1, H01}
us(x, p) = -H001(x, p) / H101(x, p) # singular control
ub(x) = -(F0⋅g)(x) / (F1⋅g)(x) # boundary control
μ(x, p) = H01(x, p) / (F1⋅g)(x) # multiplier associated to the state constraint g
# Flows
f0 = Flow(ocp, (x, p, tf) -> u0)
f1 = Flow(ocp, (x, p, tf) -> u1)
fs = Flow(ocp, (x, p, tf) -> us(x, p))
fb = Flow(ocp, (x, p, tf) -> ub(x), (x, u, tf) -> g(x), (x, p, tf) -> μ(x, p))Shooting function
Then, we define the shooting function according to the optimal structure we have determined, that is a concatenation of four arcs.
x0 = [r0, v0, m0] # initial state
function shoot!(s, p0, t1, t2, t3, tf)
x1, p1 = f1(t0, x0, p0, t1)
x2, p2 = fs(t1, x1, p1, t2)
x3, p3 = fb(t2, x2, p2, t3)
xf, pf = f0(t3, x3, p3, tf)
s[1] = xf[3] - mf # final mass constraint
s[2:3] = pf[1:2] - [1, 0] # transversality conditions
s[4] = H1(x1, p1) # H1 = H01 = 0
s[5] = H01(x1, p1) # at the entrance of the singular arc
s[6] = g(x2) # g = 0 when entering the boundary arc
s[7] = H0(xf, pf) # since tf is free
endInitial guess
To solve the problem by an indirect shooting method, we then need a good initial guess, that is a good approximation of the initial costate, the three switching times and the final time.
η = 1e-3
t13 = t[ abs.(φ.(t)) .≤ η ]
t23 = t[ 0 .≤ (g ∘ x).(t) .≤ η ]
p0 = p(t0)
t1 = min(t13...)
t2 = min(t23...)
t3 = max(t23...)
tf = t[end]
println("p0 = ", p0)
println("t1 = ", t1)
println("t2 = ", t2)
println("t3 = ", t3)
println("tf = ", tf)
# Norm of the shooting function at solution
using LinearAlgebra: norm
s = similar(p0, 7)
shoot!(s, p0, t1, t2, t3, tf)
println("\nNorm of the shooting function: ‖s‖ = ", norm(s), "\n")p0 = [3.9468316195476345, 0.1489103074249966, 0.05396551620088598]
t1 = 0.020197543560152603
t2 = 0.059784728938051694
t3 = 0.10179561954316911
tf = 0.201975435601526
Norm of the shooting function: ‖s‖ = 1.89153053767089Indirect shooting
We aggregate the data to define the initial guess vector.
ξ_guess = [p0..., t1, t2, t3, tf] # initial guess7-element Vector{Float64}:
3.9468316195476345
0.1489103074249966
0.05396551620088598
0.020197543560152603
0.059784728938051694
0.10179561954316911
0.201975435601526MINPACK.jl
We can use the MINPACK.jl package to solve the shooting equation. To compute the Jacobian of the shooting function we use the DifferentiationInterface.jl package with ForwardDiff.jl backend.
using DifferentiationInterface
import ForwardDiff
backend = AutoForwardDiff()Let us define the problem to solve.
# auxiliary function with aggregated inputs
shoot!(s, ξ) = shoot!(s, ξ[1:3], ξ[4], ξ[5], ξ[6], ξ[7])
# Jacobian of the (auxiliary) shooting function
jshoot!(js, ξ) = jacobian!(shoot!, similar(ξ), js, backend, ξ)We are now in position to solve the problem with the hybrj solver from MINPACK.jl through the fsolve function, providing the Jacobian. Let us solve the problem and retrieve the initial costate solution.
# resolution of S(ξ) = 0
indirect_sol = fsolve(shoot!, jshoot!, ξ_guess, show_trace=true)
# we retrieve the costate solution together with the times
ξ = indirect_sol.x
p0, t1, t2, t3, tf = ξ[1:3], ξ[4:end]...
println("MINPACK results:")
println("p0 = ", p0)
println("t1 = ", t1)
println("t2 = ", t2)
println("t3 = ", t3)
println("tf = ", tf)
# Norm of the shooting function at solution
s = similar(p0, 7)
shoot!(s, p0, t1, t2, t3, tf)
println("\nNorm of the shooting function: ‖s‖ = ", norm(s), "\n")Iter f(x) inf-norm Step 2-norm Step time
------ -------------- -------------- --------------
1 1.889076e+00 0.000000e+00 0.248024
2 6.350508e-02 3.864675e-03 72.540791
3 2.263138e-02 1.051893e-03 0.053435
4 8.512761e-03 7.573554e-04 0.029803
5 1.089271e-03 1.621993e-07 0.029991
6 1.731769e-03 6.487972e-07 0.031889
7 5.837735e-05 1.459778e-06 0.030409
8 7.127584e-05 6.305374e-08 0.035442
9 2.930423e-05 2.262040e-09 0.029401
10 5.289525e-07 8.272193e-11 0.029307
11 6.800537e-07 2.274797e-12 0.036999
12 6.249775e-08 4.748231e-14 0.029703
13 1.955008e-09 2.216391e-15 0.035354
14 1.653990e-09 3.439269e-17 0.029245
15 2.799487e-10 2.120811e-19 0.029116
MINPACK results:
p0 = [3.945764658782372, 0.15039559623313814, 0.053712712940267554]
t1 = 0.023509684043467996
t2 = 0.059737380895502214
t3 = 0.10157134842357175
tf = 0.20204744057153257
Norm of the shooting function: ‖s‖ = 2.806026133827754e-10NonlinearSolve.jl
Alternatively, we can use the NonlinearSolve.jl package to solve the shooting equation. The code is similar, but we use the solve function instead of fsolve. Let us define the problem.
shoot!(s, ξ, λ) = shoot!(s, ξ[1:3], ξ[4], ξ[5], ξ[6], ξ[7])
prob = NonlinearProblem(shoot!, ξ_guess)Now, let us solve the problem and retrieve the initial costate and times.
# resolution of S(ξ) = 0
sol = solve(prob; show_trace=Val(true))
# we retrieve the costate solution together with the times
ξ = sol.u
p0, t1, t2, t3, tf = ξ[1:3], ξ[4:end]...
println("\nNonlinearSolve results:")
println("p0 = ", p0)
println("t1 = ", t1)
println("t2 = ", t2)
println("t3 = ", t3)
println("tf = ", tf)
# Norm of the shooting function at solution
s = similar(p0, 7)
shoot!(s, p0, t1, t2, t3, tf)
println("\nNorm of the shooting function: ‖s‖ = ", norm(s), "\n")
Algorithm: NewtonRaphson(
descent = NewtonDescent(),
autodiff = AutoForwardDiff(),
vjp_autodiff = AutoReverseDiff(
compile = false
),
jvp_autodiff = AutoForwardDiff(),
concrete_jac = Val{false}()
)
---- ------------- -----------
Iter f(u) inf-norm Step 2-norm
---- ------------- -----------
0 1.88907616e+00 0.00000000e+00
1 6.35050789e-02 6.21665070e-02
2 5.18764339e-03 6.17289944e-02
3 4.50670873e-07 6.84174864e-04
4 4.72311079e-12 6.54262099e-08
5 7.30415728e-13 6.06824069e-13
6 3.06532577e-13 7.65303292e-13
7 8.83959572e-13 4.28044027e-13
8 3.24407168e-13 6.19707926e-14
9 7.24864613e-13 4.21693185e-13
10 3.44280160e-13 3.00746471e-13
11 9.36029032e-13 2.83305329e-13
12 1.71673786e-12 3.01997890e-13
13 9.81770221e-13 3.83119254e-13
14 2.05457873e-12 1.33989338e-13
15 1.84785520e-12 1.42924398e-14
16 9.50128864e-13 2.02172381e-13
17 1.34281475e-12 6.14221970e-13
18 9.29478716e-13 6.53045424e-13
19 6.51700915e-13 8.74552815e-13
20 1.25122135e-13 9.84958197e-13
Final 1.25122135e-13
----------------------
NonlinearSolve results:
p0 = [3.9457646586891744, 0.15039559623165552, 0.05371271293970545]
t1 = 0.023509684041879215
t2 = 0.059737380899876
t3 = 0.10157134842432228
tf = 0.20204744057100849
Norm of the shooting function: ‖s‖ = 1.455748787884998e-13Benchmarking
The results found for by the two solvers are extremely close, so now, lets benchmark these two resolutions to compare their performances.
@benchmark fsolve(shoot!, jshoot!, ξ_guess; tol=1e-8, show_trace=false) #MINPACKBenchmarkTools.Trial: 8 samples with 1 evaluation per sample.
Range (min … max): 613.801 ms … 694.902 ms ┊ GC (min … max): 19.34% … 4.83%
Time (median): 646.999 ms ┊ GC (median): 21.25%
Time (mean ± σ): 649.526 ms ± 25.604 ms ┊ GC (mean ± σ): 19.31% ± 5.99%
█ █ ██ █ █ █ █
█▁▁▁▁▁▁▁▁█▁▁▁▁▁▁▁██▁▁▁▁▁▁▁▁▁▁▁▁█▁▁▁█▁▁▁▁█▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁█ ▁
614 ms Histogram: frequency by time 695 ms <
Memory estimate: 1.51 GiB, allocs estimate: 1647598.@benchmark solve(prob; abstol=1e-8, reltol=1e-8, show_trace=Val(false)) # NonlinearSolveBenchmarkTools.Trial: 5 samples with 1 evaluation per sample.
Range (min … max): 1.110 s … 1.196 s ┊ GC (min … max): 19.76% … 9.73%
Time (median): 1.114 s ┊ GC (median): 19.78%
Time (mean ± σ): 1.134 s ± 36.924 ms ┊ GC (mean ± σ): 17.88% ± 4.63%
█ ▁ ▁ ▁
█▁█▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁█▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁█ ▁
1.11 s Histogram: frequency by time 1.2 s <
Memory estimate: 3.02 GiB, allocs estimate: 1271604.According to the NonlinearSolve documentation, for small nonlinear systems, it could be faster to use the SimpleNewtonRaphson() descent algorithm.
@benchmark solve(prob, SimpleNewtonRaphson(); abstol=1e-8, reltol=1e-8, show_trace=Val(false)) # NonlinearSolveBenchmarkTools.Trial: 5 samples with 1 evaluation per sample.
Range (min … max): 1.053 s … 1.127 s ┊ GC (min … max): 19.73% … 8.91%
Time (median): 1.065 s ┊ GC (median): 19.81%
Time (mean ± σ): 1.077 s ± 30.867 ms ┊ GC (mean ± σ): 17.91% ± 5.11%
██ █ █ █
██▁▁▁▁▁▁▁█▁▁▁▁▁▁▁▁▁▁▁▁▁▁█▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁█ ▁
1.05 s Histogram: frequency by time 1.13 s <
Memory estimate: 2.87 GiB, allocs estimate: 1065652.MINPACK.jl (fsolve) is much faster than NonlinearSolve.jl (solve) on this problem. It also uses less memory. Both methods have a similar GC overhead, but MINPACK.jl is overall more efficient here.
Plot of the solution
We plot the solution of the indirect solution (in red) over the solution of the direct method (in blue).
f = f1 * (t1, fs) * (t2, fb) * (t3, f0) # concatenation of the flows
flow_sol = f((t0, tf), x0, p0) # compute the solution: state, costate, control...
plot!(plt, flow_sol; label="indirect", color=2)
References
- 1R.H. Goddard. A Method of Reaching Extreme Altitudes, volume 71(2) of Smithsonian Miscellaneous Collections. Smithsonian institution, City of Washington, 1919.
- 2H. Seywald and E.M. Cliff. Goddard problem in presence of a dynamic pressure limit. Journal of Guidance, Control, and Dynamics, 16(4):776–781, 1993.