Harmonic oscillator problem
This example considers a minimum time problem for the harmonic oscillator with a loss control region. Unlike the classical harmonic oscillator problem (without loss control region), optimal trajectories spiral around the origin and are expected to visit the loss control region multiple times. At each visit, the constant control value can be modified, which is a key feature of loss control regions.
The main challenge here is that the trajectory spirals in a finite number of times before reaching the target, requiring careful handling of multiple visits to the loss control region.
Problem statement
\[ \left\{ \begin{array}{l} \displaystyle \min\, T, \\[0.5em] \dot{x}_1(t) = x_2(t), \; t\in [0,T]\\[0.5em] \dot{x}_2(t) = u(t)-x_1(t), t\in [0,T] \\[0.5em] u(t) \in [-1, 1], \; t\in [0,T]\\[0.5em] x(0) = (4.2,0) , \quad x(T) = 0_{\mathrm{R}^2}, \\[0.5em] \{x \mid x_2 < 0\} \text{ is a control loss region.} \end{array} \right.\]
The partition of $\mathbb{R}^2$ consists of:
- Control region: $X_1 = \{x \in \mathbb{R}^2 \mid x_2 > 0\}$ (where control can change at any time)
- Loss control region: $X_2 = \{x \in \mathbb{R}^2 \mid x_2 < 0\}$ (where control must remain constant)
Note that the final time $T$ is free in this problem, which can be handled using standard augmentation techniques.
Reformulation for the direct method
\[ \left\{ \begin{array}{l} \displaystyle \min\, T + \varepsilon \int_0^T v^2(t)\, \mathrm{d}t + \int_0^T f_{NC}(x_2(t))u^2(t)\, \mathrm{d}t, \\[0.5em] \dot{x}_1(t) = x_2(t), \; t\in [0,T]\\[0.5em] \dot{x}_2(t) =f_{C}(x_2(t))(u(t) - x_1(t)) + f_{NC}(x_2(t))(\lambda(t) - x_1(t)) , t\in [0,T] \\[0.5em] \dot{\lambda}(t) = f_C(x_2(t))v(t),\; t\in [0,T]\\[0.5em] u(t) \in [-1, 1] \; t\in [0,T]\\[0.5em] x(0) = (4.2,0) , \quad x(T) = 0_{\mathrm{R}^2}. \end{array} \right.\]
using Plots
using Plots.PlotMeasures
using OptimalControl
using NLPModelsIpopt
include("smooth.jl")fNC(x) = fNC_unboundedminus(x, 0, 0.018)
plot(fNC, -1, 1, label="fNC")
const ε = 1e-3
@def ocp begin
tf ∈ R, variable
t ∈ [ 0, tf ], time
q = [ x1, x2, λ ] ∈ R^3, state
ω = [u, v] ∈ R^2, control
tf ≥ 0.
x1(0) == 2.5
x2(0) == 4
x1(tf) == 0
x2(tf) == 0
-1 ≤ u(t) ≤ 1
-1 ≤ λ(t) ≤ 1, (1)
-5 ≤ x1(t) ≤ 5, (2)
-5 ≤ x2(t) ≤ 5, (3)
q̇(t) == [
x2(t),
(1-fNC(x2(t)))*u(t) + fNC(x2(t))*λ(t) - x1(t),
(1-fNC(x2(t)))*v(t),
]
tf + ∫(ε*(v(t))^2 +fNC(x2(t))*(u(t))^2) → min
endsol = (
state = t -> [0.1, 0.1, 1],
control = [-1, 0],
variable = 15,
)
for N in [50, 100, 500, 1000]
global sol = solve(ocp, :direct, :adnlp, :ipopt;
disc_method=:gauss_legendre_3,
grid_size=N,
init=sol,
tol=1e-8,
display = N == 1000,
)
end
N = 1000▫ This is OptimalControl version v1.1.6 running with: direct, adnlp, ipopt.
▫ The optimal control problem is solved with CTDirect version v0.17.4.
┌─ The NLP is modelled with ADNLPModels and solved with NLPModelsIpopt.
│
├─ Number of time steps⋅: 1000
└─ Discretisation scheme: gauss_legendre_3
▫ This is Ipopt version 3.14.19, running with linear solver MUMPS 5.8.1.
Number of nonzeros in equality constraint Jacobian...: 99004
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 49001
Total number of variables............................: 14004
variables with only lower bounds: 1
variables with lower and upper bounds: 4003
variables with only upper bounds: 0
Total number of equality constraints.................: 12004
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 9.6393035e+00 5.40e+00 5.17e-03 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 9.8655412e+00 2.14e+00 1.63e+02 -5.5 5.65e+01 - 6.82e-01 1.00e+00h 1
2 9.6984721e+00 3.75e+00 1.01e+01 -2.1 3.66e+01 - 6.88e-01 1.00e+00f 1
3 9.7125477e+00 1.10e+01 1.51e+00 -2.2 2.11e+01 - 7.26e-01 1.00e+00f 1
4 9.9539617e+00 1.13e+02 1.16e+01 -1.2 4.77e+03 -4.5 3.59e-03 2.54e-02f 1
5 9.9928476e+00 1.13e+02 1.16e+01 -1.2 9.18e+02 -2.7 3.60e-03 3.58e-03f 1
6 1.0034886e+01 1.10e+02 1.13e+01 -1.2 6.45e+01 -2.3 3.81e-02 2.69e-02f 1
7 1.0075397e+01 1.08e+02 1.10e+01 -1.2 1.18e+02 -2.8 2.16e-02 2.02e-02f 1
8 1.0118572e+01 1.05e+02 1.08e+01 -1.2 1.84e+02 -3.3 3.11e-02 2.42e-02f 1
9 1.0156786e+01 1.04e+02 1.07e+01 -1.2 1.92e+03 -4.2 6.93e-03 6.65e-03f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 1.0205065e+01 1.00e+02 1.03e+01 -1.2 2.71e+02 - 4.81e-02 3.99e-02f 1
11 1.0308156e+01 9.71e+01 9.96e+00 -1.2 1.98e+03 - 3.15e-02 2.97e-02f 1
12 1.0346210e+01 9.46e+01 9.70e+00 -1.2 1.80e+02 -4.7 2.69e-02 2.63e-02f 1
13 1.0410160e+01 8.78e+01 9.01e+00 -1.2 5.51e+02 - 9.13e-02 7.15e-02f 1
14 1.0500184e+01 7.96e+01 8.16e+00 -1.2 6.27e+02 - 9.98e-02 9.38e-02f 1
15 1.0544247e+01 7.11e+01 7.29e+00 -1.2 4.81e+01 -3.4 1.13e-01 1.06e-01h 1
16 1.0592133e+01 5.04e+01 5.17e+00 -1.2 4.30e+01 -2.9 3.38e-01 2.92e-01h 1
17 1.0590461e+01 3.23e+00 1.03e+00 -1.2 3.06e+01 -2.5 1.00e+00 9.36e-01h 1
18 1.0567474e+01 1.66e-01 6.74e-01 -1.2 2.15e+00 -2.1 1.00e+00 1.00e+00h 1
19 1.0538041e+01 7.46e-01 3.88e-01 -2.2 1.70e+00 -2.6 9.97e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 1.0504449e+01 1.83e+00 3.74e-01 -3.7 2.46e+00 -2.6 8.03e-01 1.00e+00h 1
21 1.0363142e+01 1.16e+01 3.69e-01 -3.3 1.49e+01 -3.1 9.85e-01 1.00e+00h 1
22 1.0361825e+01 4.69e-01 2.13e-01 -3.4 1.16e+01 -3.1 7.40e-01 1.00e+00h 1
23 1.0367077e+01 4.13e-01 5.18e-01 -2.4 1.79e+00 -3.2 1.00e+00 1.00e+00h 1
24 1.0238738e+01 2.80e+01 8.94e-01 -2.8 3.00e+01 -4.1 3.03e-01 4.79e-01h 1
25 1.0278217e+01 5.35e+00 4.77e-01 -2.6 2.80e+01 -3.7 7.03e-01 1.00e+00h 1
26 1.0266998e+01 2.27e+00 5.49e-01 -2.6 5.48e+00 -3.8 1.00e+00 1.00e+00h 1
27 1.0197791e+01 1.57e+01 5.01e-01 -3.3 2.34e+01 -4.7 8.12e-01 1.00e+00h 1
28 1.0166099e+01 1.95e+00 1.09e-01 -3.1 1.58e+01 -3.9 7.73e-01 1.00e+00h 1
29 1.0158784e+01 3.91e-01 1.64e-01 -4.1 2.83e+00 -3.0 9.40e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 1.0157309e+01 3.46e-02 4.99e-01 -5.5 1.69e-01 -0.8 1.00e+00 1.00e+00h 1
31 1.0156813e+01 1.93e-03 1.09e-01 -6.9 3.75e-02 -1.3 1.00e+00 1.00e+00h 1
32 1.0155364e+01 1.48e-02 2.49e-02 -8.4 1.93e-01 -1.7 1.00e+00 1.00e+00h 1
33 1.0154721e+01 3.19e-03 3.28e-03 -10.2 6.67e-02 -1.3 1.00e+00 1.00e+00h 1
34 1.0154483e+01 4.20e-04 3.25e-03 -11.0 2.48e-02 -0.9 1.00e+00 1.00e+00h 1
35 1.0153783e+01 3.46e-03 3.45e-03 -11.0 7.90e-02 -1.4 1.00e+00 1.00e+00h 1
36 1.0153506e+01 5.46e-04 3.06e-03 -11.0 2.63e-02 -0.9 1.00e+00 1.00e+00h 1
37 1.0152694e+01 4.43e-03 3.23e-03 -11.0 8.32e-02 -1.4 1.00e+00 1.00e+00h 1
38 1.0144801e+01 3.23e-01 9.50e-02 -11.0 7.73e-01 -2.4 1.00e+00 1.00e+00h 1
39 1.0135817e+01 3.15e-01 9.70e-02 -9.0 1.68e+00 -2.8 1.00e+00 5.44e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 1.0079696e+01 1.47e+01 5.96e-01 -8.7 6.72e+00 -3.3 1.00e+00 1.00e+00h 1
41 1.0095301e+01 5.99e+00 3.73e+01 -8.8 9.65e+00 -2.0 2.74e-01 6.33e-01h 1
42 1.0096074e+01 4.00e-01 1.20e+01 -8.8 2.81e+00 -1.6 2.76e-08 1.00e+00h 1
43 1.0094023e+01 3.29e-03 1.99e-01 -9.4 1.76e-01 -2.0 1.00e+00 1.00e+00h 1
44 1.0093597e+01 5.35e-04 1.47e-03 -10.8 6.04e-02 -1.6 1.00e+00 1.00e+00h 1
45 1.0092568e+01 4.81e-03 1.47e-03 -11.0 1.81e-01 -2.1 1.00e+00 1.00e+00h 1
46 1.0092212e+01 6.75e-04 1.39e-03 -11.0 6.46e-02 -1.7 1.00e+00 1.00e+00h 1
47 1.0089270e+01 5.30e-02 5.70e-03 -11.0 5.79e-01 -2.6 1.00e+00 1.00e+00h 1
48 1.0081141e+01 3.86e-01 9.00e-03 -11.0 1.57e+00 -3.1 1.00e+00 1.00e+00h 1
49 1.0073446e+01 4.53e-01 4.45e-02 -11.0 2.10e+00 -3.1 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
50 1.0066155e+01 3.76e-01 5.79e-02 -9.0 1.66e+00 -3.2 1.00e+00 1.00e+00h 1
51 1.0064046e+01 1.81e-02 1.65e-02 -9.5 6.94e-01 -2.8 1.00e+00 1.00e+00h 1
52 1.0063177e+01 3.97e-03 4.35e-03 -10.9 2.13e-01 -2.3 1.00e+00 1.00e+00h 1
53 1.0062760e+01 4.24e-03 3.78e-03 -8.9 6.36e-01 -2.8 1.00e+00 1.60e-01h 1
54 1.0062518e+01 3.49e-03 2.89e-03 -7.6 2.38e-01 -2.4 1.00e+00 2.48e-01h 1
55 1.0061775e+01 5.39e-03 2.47e-03 -7.0 7.12e-01 -2.9 1.00e+00 2.58e-01h 1
56 1.0060910e+01 8.24e-03 2.73e-03 -7.2 7.13e+00 -3.8 3.50e-01 3.55e-02h 1
57 1.0060189e+01 8.80e-03 2.66e-03 -7.9 4.87e+00 -3.9 3.96e-01 3.73e-02h 1
58 1.0053540e+01 1.94e-01 2.51e-02 -5.8 5.33e+00 -3.9 1.00e+00 4.34e-01h 1
59 1.0052721e+01 1.25e-01 1.64e-02 -11.0 8.32e-01 -3.1 8.77e-01 3.73e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
60 1.0051073e+01 2.45e-02 7.82e-03 -5.7 8.22e-01 -3.1 1.00e+00 1.00e+00h 1
61 1.0039745e+01 1.08e+00 7.15e-02 -6.0 7.56e+00 -4.1 4.85e-01 8.28e-01h 1
62 1.0033685e+01 5.22e-01 3.07e-02 -6.4 4.37e+00 -3.7 1.00e+00 1.00e+00h 1
63 1.0026025e+01 8.52e-01 8.94e-02 -7.5 6.05e+00 -3.7 7.91e-01 8.85e-01h 1
64 9.9987632e+00 1.17e+01 3.14e-01 -8.1 4.49e+01 -3.8 4.35e-01 3.51e-01h 1
65 9.9999290e+00 2.21e-02 3.89e-02 -7.3 1.17e+01 -2.9 1.00e+00 1.00e+00h 1
66 9.9941290e+00 3.43e-01 1.28e-01 -7.7 3.14e+00 -3.0 1.00e+00 4.35e-01h 1
67 9.9907974e+00 4.21e-01 1.31e-01 -6.4 2.87e+01 -3.0 1.00e+00 1.63e-02h 1
68 9.9878493e+00 6.89e-02 1.37e-01 -7.0 5.24e-01 -2.2 1.00e+00 1.00e+00h 1
69 9.9785280e+00 5.47e-01 5.34e-02 -7.7 4.77e+00 -3.1 4.83e-01 4.08e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
70 9.9731554e+00 1.61e-01 4.79e-02 -5.4 8.73e-01 -2.3 1.00e+00 9.51e-01h 1
71 1.0000769e+01 1.24e+00 1.10e+00 -3.5 1.78e+00 -2.3 1.00e+00 1.00e+00f 1
72 9.9947854e+00 1.23e-01 1.68e-01 -3.6 7.74e-01 -1.9 1.00e+00 1.00e+00h 1
73 9.9947186e+00 5.93e-04 1.01e-02 -3.6 1.15e-01 -1.5 1.00e+00 1.00e+00h 1
74 9.9909928e+00 1.54e-02 8.93e-02 -4.9 3.01e-01 -1.9 1.00e+00 1.00e+00h 1
75 9.9440589e+00 5.83e+00 2.80e-01 -5.2 1.37e+01 -2.9 4.00e-01 5.45e-01h 1
76 9.9319829e+00 4.41e+00 2.09e-01 -5.1 5.82e+00 -2.9 4.21e-02 2.44e-01h 1
77 9.9135696e+00 3.83e+00 1.77e-01 -5.0 1.78e+01 -3.0 1.06e-01 1.31e-01h 1
78 9.9095495e+00 2.07e-01 1.46e-01 -5.1 3.83e+00 -2.6 1.00e+00 9.46e-01h 1
79 9.9064995e+00 3.21e-02 2.66e-02 -4.8 3.81e-01 -2.1 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
80 9.8892642e+00 1.31e+00 1.67e-01 -5.0 2.39e+00 -2.6 9.92e-01 1.00e+00h 1
81 9.8902618e+00 7.00e-02 5.87e-02 -4.7 1.33e+00 -2.2 1.00e+00 1.00e+00h 1
82 9.8882421e+00 1.28e-01 1.23e-01 -4.1 6.23e-01 -2.7 1.00e+00 1.00e+00h 1
83 9.9017518e+00 4.66e-01 1.03e-01 -3.3 6.19e+00 -3.1 1.00e+00 1.77e-01h 1
84 9.9082275e+00 2.83e-01 6.38e-02 -3.4 1.00e+00 -2.7 1.00e+00 1.00e+00h 1
85 9.9055210e+00 6.50e-01 7.15e-02 -3.4 1.75e+00 -3.2 7.95e-01 1.00e+00h 1
86 9.9024048e+00 1.46e-01 1.50e-02 -3.4 5.60e-01 -2.8 1.00e+00 1.00e+00h 1
87 9.8982246e+00 1.01e-01 1.92e-02 -3.4 5.02e-01 -2.8 1.00e+00 1.00e+00h 1
88 9.8904591e+00 2.35e-01 1.48e-02 -3.4 1.03e+00 -3.3 1.00e+00 1.00e+00h 1
89 9.8810196e+00 5.85e-01 2.24e-02 -3.4 2.98e+00 -3.4 5.92e-01 5.00e-01h 2
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
90 9.8635121e+00 2.53e-01 5.00e-02 -4.7 9.13e-01 -2.5 8.68e-01 1.00e+00h 1
91 9.8621571e+00 5.83e-03 2.90e-02 -5.4 2.36e-01 -1.7 1.00e+00 1.00e+00h 1
92 9.8423546e+00 4.32e-01 1.43e-01 -5.7 1.01e+00 -2.6 3.95e-01 9.52e-01h 1
93 9.8386117e+00 3.71e-01 1.25e-01 -11.0 1.84e+00 -2.7 2.54e-01 1.42e-01h 1
94 9.8310825e+00 2.68e-01 9.76e-02 -5.2 2.83e+00 -2.7 1.00e+00 2.78e-01h 1
95 9.8212952e+00 6.15e-01 1.13e-01 -5.6 1.86e+01 -2.8 8.95e-02 5.65e-02h 1
96 9.8084840e+00 6.80e-01 1.24e-01 -5.9 6.85e+00 -2.8 1.50e-01 1.67e-01h 1
97 9.8119744e+00 8.60e-02 5.23e-02 -4.4 6.81e-01 -2.0 1.00e+00 1.00e+00h 1
98 9.7908455e+00 1.35e+00 1.78e-01 -4.5 1.96e+00 -2.9 3.33e-01 9.00e-01h 1
99 9.7965199e+00 2.64e-01 1.12e-01 -3.9 1.35e+00 -2.1 8.22e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
100 9.7857762e+00 9.20e-01 1.11e-01 -4.3 1.20e+02 -3.0 3.27e-03 1.38e-02h 2
101 9.7827138e+00 1.91e-02 1.93e-02 -4.4 9.17e-01 -2.2 8.32e-01 1.00e+00h 1
102 9.7618481e+00 8.09e-01 1.68e-01 -4.5 1.08e+00 -3.1 3.60e-01 1.00e+00h 1
103 9.7626740e+00 5.88e-02 4.73e-02 -4.9 4.25e-01 -1.8 9.91e-01 1.00e+00h 1
104 9.7514368e+00 2.77e-01 1.78e-01 -4.9 7.20e-01 -2.7 4.01e-01 1.00e+00h 1
105 9.7518364e+00 2.87e-03 2.13e-02 -5.9 2.83e-01 -1.4 1.00e+00 1.00e+00h 1
106 9.7512709e+00 2.79e-04 6.43e-03 -7.1 2.84e-02 -1.0 1.00e+00 1.00e+00h 1
107 9.7504515e+00 1.11e-03 2.50e-03 -5.9 6.94e-02 -1.5 1.00e+00 7.21e-01h 1
108 9.7483076e+00 1.49e-02 6.34e-03 -6.3 1.51e-01 -1.9 1.00e+00 1.00e+00h 1
109 9.7475939e+00 1.34e-03 1.80e-03 -7.6 5.88e-02 -1.5 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
110 9.7473431e+00 2.04e-04 1.73e-03 -8.3 2.12e-02 -1.1 1.00e+00 1.00e+00h 1
111 9.7466271e+00 1.75e-03 1.68e-03 -9.2 6.17e-02 -1.6 1.00e+00 1.00e+00h 1
112 9.7454300e+00 5.47e-03 1.64e-03 -7.2 1.78e-01 -2.0 1.00e+00 6.01e-01h 1
113 9.7414762e+00 7.77e-02 1.25e-02 -5.6 1.41e+00 -3.0 1.00e+00 2.73e-01h 1
114 9.7408147e+00 1.01e-03 4.23e-03 -6.4 8.25e-02 -1.7 1.00e+00 1.00e+00h 1
115 9.7366499e+00 9.25e-02 2.08e-02 -6.8 5.26e-01 -2.6 4.88e-01 7.82e-01h 1
116 9.7325286e+00 1.92e-01 2.55e-02 -5.4 3.81e+00 -3.6 1.16e-01 1.39e-01h 1
117 9.7311964e+00 1.97e-01 2.27e-02 -4.5 2.43e+00 -3.6 1.00e+00 1.24e-01h 1
118 9.7177398e+00 2.74e+00 6.47e-02 -4.8 3.65e+00 -3.7 3.60e-01 7.73e-01h 1
119 9.7119798e+00 2.50e+00 6.86e-02 -4.3 3.07e+01 -4.6 4.93e-02 1.34e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
120 9.7127028e+00 6.90e-02 2.08e-02 -4.8 8.24e-01 -2.9 6.67e-01 1.00e+00h 1
121 9.7097493e+00 5.24e-02 4.85e-03 -5.8 6.72e-01 -2.9 4.20e-01 1.00e+00h 1
122 9.7094081e+00 5.69e-04 7.43e-04 -6.1 8.89e-02 -2.1 9.35e-01 1.00e+00h 1
123 9.7092460e+00 1.36e-04 8.32e-04 -7.2 3.73e-02 -1.7 1.00e+00 1.00e+00h 1
124 9.7092020e+00 1.91e-05 8.31e-04 -8.6 1.40e-02 -1.2 1.00e+00 1.00e+00h 1
125 9.7091598e+00 2.41e-05 8.32e-04 -10.5 1.57e-02 -1.3 9.80e-01 1.00e+00h 1
126 9.7087967e+00 1.84e-03 8.39e-04 -11.0 1.43e-01 -2.2 1.00e+00 1.00e+00h 1
127 9.7085525e+00 2.50e-03 8.37e-04 -9.0 1.25e+00 -3.2 1.00e+00 9.01e-02h 1
128 9.7066790e+00 3.40e-02 3.53e-03 -7.0 8.65e+00 -4.1 1.00e+00 1.14e-01h 1
129 9.7031330e+00 1.44e-01 1.51e-02 -11.0 9.52e+00 -4.2 1.14e-01 2.17e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
130 9.7007263e+00 1.70e-01 1.83e-02 -6.7 1.06e+01 -4.2 9.31e-01 1.41e-01h 1
131 9.7006688e+00 1.88e-04 4.69e-03 -8.0 1.36e-01 -2.0 1.00e+00 1.00e+00h 1
132 9.6999466e+00 9.84e-03 2.10e-02 -7.6 5.47e-01 -3.0 1.00e+00 7.15e-01h 1
133 9.6999432e+00 4.74e-07 1.09e-03 -9.5 5.86e-03 -0.7 1.00e+00 1.00e+00h 1
134 9.6999252e+00 6.86e-06 6.79e-04 -9.6 9.97e-03 -1.2 1.00e+00 9.60e-01h 1
135 9.6998593e+00 9.41e-05 6.02e-04 -8.7 9.01e-02 -2.2 1.00e+00 3.99e-01h 1
136 9.6997627e+00 2.37e-04 6.09e-04 -8.4 1.01e-01 -2.2 1.00e+00 5.28e-01h 1
137 9.6994629e+00 1.89e-03 6.06e-04 -7.4 8.80e-01 -3.2 1.00e+00 1.96e-01h 1
138 9.6992409e+00 9.96e-04 5.99e-04 -8.0 1.24e-01 -2.3 1.00e+00 1.00e+00h 1
139 9.6986276e+00 7.72e-03 5.91e-04 -7.4 1.08e+00 -3.3 1.00e+00 3.28e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
140 9.6977881e+00 1.78e-02 6.47e-04 -7.2 1.23e+00 -3.3 1.00e+00 4.13e-01h 1
141 9.6966006e+00 3.46e-02 5.89e-04 -6.9 1.40e+00 -3.4 1.00e+00 5.39e-01h 1
142 9.6964999e+00 1.88e-04 1.48e-03 -8.0 7.68e-02 -2.0 1.00e+00 1.00e+00h 1
143 9.6959357e+00 6.33e-03 2.99e-03 -9.4 5.95e-01 -3.0 7.56e-01 6.12e-01h 1
144 9.6954350e+00 8.06e-03 2.41e-03 -7.9 6.72e-01 -3.0 1.00e+00 4.96e-01h 1
145 9.6936822e+00 5.84e-02 4.89e-03 -7.6 6.16e+00 -4.0 9.33e-02 2.00e-01h 1
146 9.6912759e+00 1.46e-01 8.34e-03 -7.6 7.42e+00 -4.1 4.91e-01 2.31e-01h 1
147 9.6879118e+00 2.93e-01 1.52e-02 -7.9 9.26e+00 -4.1 1.62e-02 2.51e-01h 1
148 9.6838083e+00 5.07e-01 3.10e-02 -7.3 1.58e+01 -4.2 2.45e-01 1.97e-01h 1
149 9.6820211e+00 2.46e-01 1.22e-03 -7.3 2.11e+00 -3.3 1.00e+00 6.47e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
150 9.6790848e+00 1.90e-01 1.64e-02 -8.4 2.40e+00 -3.4 8.59e-01 8.89e-01h 1
151 9.6788836e+00 1.82e-01 2.60e-02 -6.0 3.02e+00 -3.4 1.00e+00 4.57e-02h 1
152 9.6782755e+00 1.66e-01 1.43e-02 -5.4 3.36e+00 -3.5 3.55e-01 1.34e-01h 1
153 9.6783317e+00 1.43e-01 1.01e-02 -4.7 1.71e+00 -3.5 1.80e-01 1.59e-01h 1
154 9.6757782e+00 2.19e-01 2.20e-02 -5.4 4.99e+00 -3.6 1.00e+00 3.41e-01h 1
155 9.6739943e+00 5.30e-02 1.20e-02 -6.6 6.02e-01 -2.7 9.47e-01 1.00e+00h 1
156 9.6708741e+00 2.99e-01 2.27e-01 -6.0 1.39e+01 -3.7 1.00e+00 1.16e-01h 1
157 9.6681362e+00 2.22e-01 1.24e-02 -6.6 9.53e-01 -2.8 1.00e+00 8.63e-01h 1
158 9.6677731e+00 2.09e-01 5.88e-02 -7.0 1.17e+00 -2.9 1.00e+00 7.14e-02h 1
159 9.6672932e+00 6.55e-03 6.23e-02 -6.0 9.94e-02 -1.5 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
160 9.6671149e+00 1.50e-04 7.00e-03 -6.9 1.49e-02 -1.1 1.00e+00 1.00e+00h 1
161 9.6669937e+00 2.65e-04 6.25e-03 -7.6 6.04e-02 -1.6 1.00e+00 3.77e-01h 1
162 9.6668628e+00 1.81e-04 1.59e-03 -8.7 2.30e-02 -1.2 1.00e+00 1.00e+00h 1
163 9.6666847e+00 4.87e-04 4.89e-03 -9.3 6.85e-02 -1.6 1.00e+00 4.96e-01h 1
164 9.6665800e+00 6.35e-04 8.44e-02 -7.2 5.59e-01 -2.6 1.00e+00 3.90e-02h 1
165 9.6665306e+00 3.26e-04 6.06e-02 -6.9 2.12e-02 -1.3 1.00e+00 5.75e-01h 1
166 9.6664095e+00 6.47e-04 4.06e-02 -6.3 5.37e-02 -1.7 1.00e+00 7.31e-01h 1
167 9.6662462e+00 2.61e-04 1.26e-03 -7.2 2.59e-02 -1.3 1.00e+00 1.00e+00h 1
168 9.6659290e+00 1.56e-03 4.55e-03 -7.5 7.50e-02 -1.8 1.00e+00 8.42e-01h 1
169 9.6653117e+00 7.78e-03 8.76e-02 -6.3 5.77e-01 -2.7 1.00e+00 2.24e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
170 9.6633204e+00 5.81e-02 2.07e-01 -4.9 8.79e+00 -3.7 1.00e+00 4.96e-02h 1
171 9.6647371e+00 1.01e-01 8.95e-02 -4.5 3.11e+00 -3.7 5.24e-01 5.07e-01h 1
172 9.6649174e+00 8.30e-04 5.36e-03 -4.7 8.67e-02 -1.5 1.00e+00 1.00e+00h 1
173 9.6628885e+00 5.73e-03 4.83e-02 -5.8 9.35e-02 -2.0 1.00e+00 7.75e-01h 1
174 9.6620776e+00 5.97e-04 1.72e-03 -5.8 3.63e-02 -1.6 1.00e+00 1.00e+00h 1
175 9.6610123e+00 4.40e-03 1.11e-03 -6.2 1.18e-01 -2.0 1.00e+00 9.93e-01h 1
176 9.6602777e+00 7.90e-03 4.99e-02 -7.0 1.18e+00 -3.0 6.18e-01 1.15e-01h 1
177 9.6590409e+00 1.73e-02 4.69e-02 -6.0 3.98e-01 -2.6 1.00e+00 6.26e-01h 1
178 9.6584521e+00 5.61e-03 9.46e-03 -6.1 1.44e-01 -2.1 1.00e+00 9.00e-01h 1
179 9.6570168e+00 2.99e-02 4.35e-02 -5.8 9.70e-01 -3.1 1.00e+00 3.76e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
180 9.6554248e+00 3.41e-02 1.15e-02 -6.2 4.02e-01 -2.7 1.00e+00 1.00e+00h 1
181 9.6548986e+00 8.35e-03 5.63e-03 -6.9 1.47e-01 -2.2 1.00e+00 8.59e-01h 1
182 9.6547040e+00 5.89e-04 7.83e-04 -7.0 5.22e-02 -1.8 1.00e+00 1.00e+00h 1
183 9.6546097e+00 2.39e-03 1.16e-03 -5.9 1.15e-01 -2.3 1.00e+00 1.00e+00h 1
184 9.6556172e+00 5.95e-03 1.47e-02 -5.1 2.25e-01 -2.8 8.67e-01 8.31e-01h 1
185 9.6524728e+00 1.18e-01 3.76e-03 -5.3 6.32e+01 - 1.17e-03 2.30e-02h 1
186 9.6512936e+00 1.31e-01 3.70e-03 -4.9 2.01e+01 - 8.36e-02 2.89e-02f 1
187 9.6515178e+00 1.07e-01 4.33e-03 -4.6 1.84e+01 - 2.85e-01 1.58e-01h 1
188 9.6492689e+00 1.41e-01 3.62e-03 -5.1 1.55e+01 - 2.09e-01 1.59e-01h 1
189 9.6479013e+00 1.36e-01 3.09e-03 -5.1 1.30e+01 - 2.41e-01 1.46e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
190 9.6466607e+00 1.28e-01 2.67e-03 -5.3 1.11e+01 - 3.59e-01 1.45e-01h 1
191 9.6455711e+00 1.17e-01 2.67e-03 -5.3 9.52e+00 - 7.77e-01 1.52e-01h 1
192 9.6445093e+00 1.04e-01 2.56e-03 -5.4 1.04e+01 - 1.00e+00 1.63e-01h 1
193 9.6395686e+00 2.42e-01 8.24e-03 -5.8 6.76e+00 - 4.35e-01 9.15e-01h 1
194 9.6393860e+00 2.19e-01 7.44e-03 -6.4 1.55e+01 - 3.62e-02 9.67e-02h 1
195 9.6392983e+00 3.04e-04 3.33e-05 -5.7 6.98e-01 - 9.92e-01 1.00e+00h 1
196 9.6385668e+00 1.55e-03 1.72e-04 -6.4 1.13e+00 - 6.45e-01 6.29e-01h 1
197 9.6383474e+00 3.25e-04 1.17e-05 -6.2 1.12e-01 - 1.00e+00 1.00e+00h 1
198 9.6379993e+00 7.71e-04 3.89e-05 -6.6 1.60e-01 - 1.00e+00 1.00e+00h 1
199 9.6378823e+00 1.90e-04 5.24e-06 -7.1 1.16e-01 - 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
200 9.6378255e+00 7.56e-05 3.31e-06 -7.9 9.25e-02 - 1.00e+00 1.00e+00h 1
201 9.6378173e+00 6.71e-06 1.04e-06 -8.7 5.29e-02 - 1.00e+00 1.00e+00h 1
202 9.6378165e+00 6.66e-07 2.05e-07 -8.8 2.63e-02 - 1.00e+00 1.00e+00h 1
203 9.6378156e+00 1.73e-07 6.32e-08 -9.5 1.48e-02 - 1.00e+00 1.00e+00h 1
204 9.6378154e+00 1.72e-08 1.91e-08 -11.0 8.59e-03 - 1.00e+00 1.00e+00h 1
205 9.6378154e+00 4.54e-10 3.32e-09 -11.0 3.77e-03 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 205
(scaled) (unscaled)
Objective...............: 9.6378153647125764e+00 9.6378153647125764e+00
Dual infeasibility......: 3.3232221418609757e-09 3.3232221418609757e-09
Constraint violation....: 4.5399022735592709e-10 4.5399022735592709e-10
Variable bound violation: 7.1729133743758666e-09 7.1729133743758666e-09
Complementarity.........: 7.1346915166717995e-11 7.1346915166717995e-11
Overall NLP error.......: 3.3232221418609757e-09 3.3232221418609757e-09
Number of objective function evaluations = 213
Number of objective gradient evaluations = 206
Number of equality constraint evaluations = 213
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 206
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 205
Total seconds in IPOPT = 81.875
EXIT: Optimal Solution Found.plot(sol; layout=:group, size=(800, 300))
tf = variable(sol)
tt = (0:N+1) * (tf/(N+1))
x1(t) = state(sol)(t)[1]
x2(t) = state(sol)(t)[2]
λ(t) = state(sol)(t)[3]
u(t) = control(sol)(t)[1]
p1(t) = costate(sol)(t)[1]
p2(t) = costate(sol)(t)[2]
a = λ(tf)plot(x1, x2, 0, tf, label="optimal trajectory", color="blue", linewidth=2)
plot!([-4, 5], [0, 0], color=:black, label=false, linewidth=2)
plot(tt, u, label="optimal control", color="red", linewidth=2)
plot!(tt, λ, label="state λ", color="green", linewidth=2)
# Find the crossing times based on conditions for x1
t1_index = findfirst(t -> x2(t) ≤ 0, tt)
t2_index = nothing
t3_index = nothing
# If t1 is found, find the next crossing times
if t1_index !== nothing
t2_index = findfirst(t -> x2(t) ≥ 0, tt[t1_index+1:end])
t2_index = t2_index !== nothing ? t2_index + t1_index : nothing
end
if t2_index !== nothing
t3_index = findfirst(t -> x2(t) ≤ 0, tt[t2_index+1:end])
t3_index = t3_index !== nothing ? t3_index + t2_index : nothing
end
if t3_index !== nothing
t4_index = findfirst(t -> x2(t) ≥ 0, tt[t3_index+1:end])
t4_index = t4_index !== nothing ? t4_index + t3_index : nothing
end
# Convert indices to times
t1 = t1_index !== nothing ? tt[t1_index] : "No such t1 found"
t2 = t2_index !== nothing ? tt[t2_index] : "No such t2 found"
t3 = t3_index !== nothing ? tt[t3_index] : "No such t3 found"
t4 = tt[end]
println("First crossing time: ", t1)
println("Second crossing time: ", t2)
println("Third crossing time: ", t3)
println("Fourth crossing/final time: ", t4)First crossing time: 0.8564042088068693
Second crossing time: 4.0029679872321084
Third crossing time: 6.495200460052099
Fourth crossing/final time: 9.63214171927726d = diff(u.(tt))
tstar = tt[findfirst(abs.(d) .> 0.9)[]]
println("the switching time: ", tstar)the switching time: 4.897862272839286jmp1 = p2(t1+0.1) - p2(t1-0.1)
jmp2 = p2(t2+0.1) - p2(t2-0.1)
println("p2(t1+) - p2(t1-) = ", jmp1)
println("p2(t2+) - p2(t2-) = ", jmp2)p2(t1+) - p2(t1-) = -0.05503863941787823
p2(t2+) - p2(t2-) = -0.0670837791741189Analysis of the direct method results
The direct method reveals that the optimal trajectory visits the loss control region $X_2$ twice:
- First visit: $\lambda = 1$ (an extremal value of the control set $[-1,1]$)
- Second visit: $\lambda \in (-1,1)$ (an interior value)
Additionally, the control exhibits a bang-bang structure with a single switching time during the second visit to the control region. This is characteristic of time-optimal control problems.
Indirect Method
Based on the direct method results, we deduce that the optimal solution $(x^*, u^*)$ consists of:
- Three consecutive bang arcs: the control switches from $-1$ to $+1$ at the first crossing time $\tau_1^*$, then from $+1$ to $-1$ at a switching time $\sigma^* \in (\tau_2^*, \tau_3^*)$
- One constant arc: from $-1$ to a constant value in $(-1,1)$ at the crossing time $\tau_3^*$
An important observation is that an extremal value ($u_1^* = 1$) is possible when visiting a loss control region, which must satisfy the averaged Hamiltonian gradient condition (as an inequality). However, for the second visit, the interior value $u_2^* \approx 0.89$ requires an additional equation in the shooting function.
using NonlinearSolve
using OrdinaryDiffEq
using Animations# Dynamics
function F0(x)
return [x[2], -x[1]]
end
function F1(x)
return [0, 1]
end
H0(x, p) = p' * F0(x)
H1(x, p) = p' * F1(x)
# Hamiltonians:
H(x, p, u) = H0(x, p) + u*H1(x,p) # pseudo-Hamiltonian
up(x, p) = 1
um(x, p) = -1
Hp(x, p) = H(x, p, up(x, p))
Hm(x, p) = H(x, p, um(x, p))
# Hamiltonians: control loss region 2
H2(x, b, y, p) = H0(x, p) + b*H1(x, p) - y*p[2] # pseudo-Hamiltonian
Hcl(X, P) = H2(X[1:2], X[3], X[4], P[1:2]) # control loss 2
# Flows
fp = Flow(Hamiltonian(Hp))
fm = Flow(Hamiltonian(Hm))
fcl = Flow(Hamiltonian(Hcl))# parameters
const t0 = 0
const x0 = [2.5; 4.0]# Shooting function
function shoot(p0, tt1, tt2, ttstar, tt3, b1, jump1, jump2, TT)
pb0 = 0
py0 = 0
x1, p1 = fm(t0 , x0, p0, tt1)
x2, p2 = fp(tt1, x1, p1 - [0, jump1], tt2)
x3, p3 = fp(tt2, x2, p2, ttstar)
x4, p4 = fm(ttstar, x3, p3, tt3)
X5, P5 = fcl(tt3, [x4 ; b1 ; 0], [p4 - [0, jump2]; pb0 ; py0], TT)
s = zeros(eltype(p0), 10)
s[1:2] = X5[1:2] - [ 0 , 0 ] # target
s[3] = H1(x3, p3) # switching
s[4] = x1[2] - 0 # first crossing
s[5] = x2[2] - 0 # second crossing
s[6] = x4[2] - 0 # third crossing
s[7] = jump1 - p1[2]*(1 + 1)/(1 - x1[1]) # jump1
s[8] = jump2 - p4[2]*(b1 + 1)/(b1 - x4[1]) # jump2
s[9] = Hm(x0, p0) - 1 # free final time
s[10] = P5[4] # averaged gradient condition
return s
end# auxiliary function with aggregated inputs
nle = (ξ, λ) -> shoot(ξ[1:2], ξ[3], ξ[4], ξ[5],ξ[6],ξ[7],ξ[8], ξ[9], ξ[10])
# initial guess
ξ_guess = [p1(0) , p2(0), t1, t2, tstar , t3, a, jmp1, jmp2, t4]
prob = NonlinearProblem(nle, ξ_guess)indirect_sol = solve(prob, SimpleNewtonRaphson(); abstol=1e-8, reltol=1e-8, show_trace=Val(true))# Retrieves solution
pp0 = indirect_sol[1:2]
tt1 = indirect_sol[3]
tt2 = indirect_sol[4]
ttstar = indirect_sol[5]
tt3 = indirect_sol[6]
b11 = indirect_sol[7]
jmp1 = indirect_sol[8]
jmp2 = indirect_sol[9]
T1 = indirect_sol[10]ode_sol = fm((t0, tt1), x0, pp0, saveat=0.1)
ttt1 = ode_sol.t
xx1 = [ ode_sol[1:2, j] for j in 1:size(ttt1, 1) ]
pp1 = [ ode_sol[3:4, j] for j in 1:size(ttt1, 1) ]
uu1 = um.(xx1, pp1)
ode_sol = fp((tt1, tt2), xx1[end], pp1[end] - [0, jmp1], saveat=0.1)
ttt2 = ode_sol.t
xx2 = [ ode_sol[1:2, j] for j in 1:size(ttt2, 1) ]
pp2 = [ ode_sol[3:4, j] for j in 1:size(ttt2, 1) ]
uu2 = up.(xx2, pp2)
ode_sol = fp((tt2, ttstar), xx2[end], pp2[end] , saveat=0.1)
ttt3 = ode_sol.t
xx3 = [ ode_sol[1:2, j] for j in 1:size(ttt3, 1) ]
pp3 = [ ode_sol[3:4, j] for j in 1:size(ttt3, 1) ]
uu3 = up.(xx3, pp3)
ode_sol = fm((ttstar, tt3), xx3[end], pp3[end], saveat=0.1)
ttt4 = ode_sol.t
xx4 = [ ode_sol[1:2, j] for j in 1:size(ttt4, 1) ]
pp4 = [ ode_sol[3:4, j] for j in 1:size(ttt4, 1) ]
uu4 = um.(xx4, pp4)
ode_sol = fcl((tt3, T1), [xx4[end] ; b11 ; 0], [pp4[end] - [0, jmp2]; 0 ; 0], saveat=0.1)
ttt5 = ode_sol.t
xx5 = [ ode_sol[1:2, j] for j in 1:size(ttt5, 1) ]
pp5 = [ ode_sol[5:6, j] for j in 1:size(ttt5, 1) ]
uu5 = b11.*ones(length(ttt5))tt0 = [ ttt1 ; ttt2 ; ttt3 ; ttt4 ; ttt5 ]
xx = [ xx1 ; xx2 ; xx3 ; xx4 ; xx5 ]
pp = [ pp1 ; pp2 ; pp3 ; pp4 ; pp5 ]
uu = [ uu1 ; uu2 ; uu3 ; uu4 ; uu5 ]
m = length(tt0)
x11 = [ xx[i][1] for i=1:m ]
x22 = [ xx[i][2] for i=1:m ]
p11 = [ pp[i][1] for i=1:m ]
p22 = [ pp[i][2] for i=1:m ]plot(x11, x22, label="optimal trajectory", linecolor=:blue , linewidth=2)
plot!([-4, 5], [0, 0], color=:black, label=false, linewidth=2)
plot(tt0, uu, label="optimal control", linecolor=:red , linewidth=2)
plot(tt0, p11, label="costate p1", linecolor=:purple , linewidth=2)
plot!(tt0, p22, label="costate p2", linecolor=:violet , linewidth=2)
# create an animation
animx = @animate for i = 1:length(tt0)
plot(x11[1:i], x22[1:i], xlim=(-3.,5.), ylim=(-4.,4.3), label="optimal trajectory",
linecolor=:blue, linewidth=2, legend=:topleft)
scatter!([x11[i]], [x22[i]], markersize=4, marker=:circle, color=:black, label=false)
plot!([-4, 5], [0, 0], color=:black, label=false, linewidth=2)
end
animu = @animate for i = 1:length(tt0)
plot(tt0[1:i], uu[1:i], xlim=(0.,tt0[end]), ylim=(-1.2,1.2), label="optimal control",
linecolor=:red, linewidth=2)
end
animp = @animate for i = 1:length(tt0)
plot(tt0[1:i], p11[1:i], xlim=(0.,tt0[end]), ylim=(-1.3, 0.5), label="costate p1",
linecolor=:purple, linewidth=2)
plot!(tt0[1:i], p22[1:i], xlim=(0.,tt0[end]), ylim=(-1.5,1.3), label="costate p2",
linecolor=:violet, linewidth=2, legend=:topleft)
end# display the animation
gif(animx, "ho_x.gif", fps = 10)
gif(animu, "ho_u.gif", fps = 10)
gif(animp, "ho_p.gif", fps = 10)