Skip to content

Commit b5a1fd9

Browse files
shawn-tao01shawn-tao01
committed
Add DOP solving tutorial to documentation
1 parent ba94203 commit b5a1fd9

File tree

2 files changed

+235
-0
lines changed

2 files changed

+235
-0
lines changed

‎docs/make.jl‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,10 @@ makedocs(;
1616
),
1717
pages=[
1818
"Home" => "index.md",
19+
"Tutorials" => [
20+
"Solving a DOP" => "Tutorials/solving_a_dop.md",
21+
]
22+
1923
],
2024
)
2125

‎docs/src/Tutorials/solving_a_dop.md‎

Lines changed: 231 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,231 @@
1+
```@meta
2+
CurrentModule = JuDO
3+
```
4+
5+
```julia
6+
using Interesso
7+
using JuMP
8+
using Plots
9+
using JuDO
10+
using DynOptInterface
11+
12+
13+
const g = 9.81
14+
const l = 0.5
15+
const m_1 = 1.0
16+
const m_2 = 0.3
17+
const t_0 = 0.0
18+
const t_f = 2.0
19+
const u_max = 20.0
20+
const r_max = 2.0
21+
22+
dop = DynModel(Interesso.Optimizer)
23+
24+
struct LinearInterpolant <: DynOptInterface.AbstractDynamicSolution
25+
y_a::Float64
26+
y_b::Float64
27+
end
28+
(li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0)
29+
30+
@phase(dop, t)
31+
@constraint(dop, initial(t) == 0)
32+
@constraint(dop, final(t) == 2)
33+
34+
@variable(dop, -u_max <= u <= u_max, DefinedOn(t))
35+
@variable(dop, 0 <= r <= r_max, DefinedOn(t))
36+
37+
@variable(dop, θ, DefinedOn(t))
38+
@variable(dop, ν, DefinedOn(t))
39+
@variable(dop, ω, DefinedOn(t))
40+
41+
@constraint(dop, initial(r) == 0)
42+
@constraint(dop, initial(θ) == 0)
43+
@constraint(dop, initial(ν) == 0)
44+
@constraint(dop, initial(ω) == 0)
45+
46+
@constraint(dop, final(r) == 1)
47+
@constraint(dop, final(θ) == pi)
48+
@constraint(dop, final(ν) == 0)
49+
@constraint(dop, final(ω) == 0)
50+
51+
@constraint(dop, derivative(r) == ν)
52+
@constraint(dop, derivative(ν) == (l*m_2*sin(θ)*ω^2 + u + m_2*g*cos(θ)*sin(θ))/(m_1 + m_2*sin(θ)^2))
53+
54+
@constraint(dop, derivative(θ) == ω)
55+
@constraint(dop, derivative(ω) == (-l*m_2*cos(θ)*sin(θ)*ω^2 - u*cos(θ) - (m_1 + m_2)*g*sin(θ))/(l*(m_1 + m_2*sin(θ)^2)))
56+
57+
@objective(dop, Min, integral(u^2))
58+
59+
JuDO.warmstart!(dop, LinearInterpolant(0.0, 1.0), r)
60+
JuDO.warmstart!(dop, LinearInterpolant(0.0, pi), θ)
61+
62+
63+
JuDO.optimize!(dop)
64+
rsol=dyn_value(dop, r)
65+
thetasol=dyn_value(dop, θ)
66+
nusol=dyn_value(dop, ν)
67+
omegasol=dyn_value(dop, ω)
68+
69+
time_points = collect(range(t_0, t_f, length=100))
70+
r_values = [omegasol(t) for t in time_points]
71+
open("rsol_judo.txt", "w") do io
72+
println(io, "time,r_value")
73+
for (t, r_val) in zip(time_points, r_values)
74+
println(io, "$(t),$(r_val)")
75+
end
76+
end
77+
# plot trajectory with labels, title, no legend
78+
p = plot(time_points, r_values;
79+
xlabel = "Time (s)",
80+
ylabel = "Angular Velocity (rad/s)",
81+
legend = false,
82+
xlims = (t_0, t_f),
83+
lw = 1,
84+
grid = true,
85+
lc = :black)
86+
87+
display(p)
88+
savefig(p, "cartpole_omega.png")
89+
90+
91+
thetasol_values = [thetasol(t) for t in time_points]
92+
q = plot(time_points, thetasol_values;
93+
xlabel = "Time (s)",
94+
ylabel = "Theta (rad)",
95+
legend = false,
96+
xlims = (t_0, t_f),
97+
lw = 1,
98+
grid = true,
99+
lc = :black)
100+
101+
display(q)
102+
savefig(q, "cartpole_theta.png")
103+
```
104+
105+
"""julia
106+
using Interesso, JuMP, JuDO, DynOptInterface
107+
using Plots
108+
dop = DynModel(Interesso.Optimizer)
109+
110+
const m = 203000 / 32.174
111+
const ρ_0, h_r, R_e = 0.002378, 23800.0, 20902900.0
112+
const μ = 0.14076539e17
113+
const a_0, a_1 = -0.20704, 0.029244
114+
const b_0, b_1, b_2 = 0.07854, -0.61592e-2, 0.621408e-3
115+
const S = 2690
116+
117+
struct LinearInterpolant <: DynOptInterface.AbstractDynamicSolution
118+
y_a::Float64
119+
y_b::Float64
120+
end
121+
(li::LinearInterpolant)(t::Real) = li.y_a + (t - 0) * (li.y_b - li.y_a) / (2500 - 0)
122+
123+
# Phase
124+
@phase(dop, t)
125+
@constraint(dop, initial(t) == 0)
126+
@constraint(dop, final(t) ≤ 2500)
127+
128+
# States
129+
@variable(dop, 0 ≤ h, DefinedOn(t))
130+
@variable(dop, deg2rad(-89) ≤ θ ≤ deg2rad(89), DefinedOn(t))
131+
@variable(dop, Φ, DefinedOn(t))
132+
@variable(dop, 1e-4 ≤ v, DefinedOn(t))
133+
@variable(dop, deg2rad(-89) ≤ γ ≤ deg2rad(89), DefinedOn(t))
134+
@variable(dop, ψ, DefinedOn(t))
135+
136+
# Controls
137+
@variable(dop, deg2rad(-90) ≤ α ≤ deg2rad(90), DefinedOn(t))
138+
@variable(dop, deg2rad(-90) ≤ β ≤ deg2rad(1), DefinedOn(t))
139+
140+
# Boundary conditions
141+
@constraint(dop, initial(h) == 2.6e5)
142+
@constraint(dop, final(h) == 0.8e5)
143+
@constraint(dop, initial(θ) == 0)
144+
@constraint(dop, initial(Φ) == 0)
145+
@constraint(dop, initial(v) == 25600)
146+
@constraint(dop, final(v) == 2500)
147+
@constraint(dop, initial(γ) == deg2rad(1))
148+
@constraint(dop, final(γ) == deg2rad(-5))
149+
@constraint(dop, initial(ψ) == deg2rad(90))
150+
151+
# Expressions
152+
@expression(dop, r, R_e + h)
153+
@expression(dop, g, μ / r^2)
154+
@expression(dop, ρ, ρ_0 * exp(-h / h_r))
155+
@expression(dop, α_deg, (180 / pi) * α)
156+
@expression(dop, L, 0.5 * S * ρ * v^2 * (a_0 + a_1 * α_deg))
157+
@expression(dop, D, 0.5 * S * ρ * v^2 * (b_0 + b_1 * α_deg + b_2 * α_deg))
158+
159+
# Differential equations
160+
@constraint(dop, derivative(h) == v * sin(γ))
161+
@constraint(dop, derivative(θ) == v * cos(γ) * cos(ψ) / r)
162+
@constraint(dop, derivative(Φ) == v * cos(γ) * sin(ψ) / (r * cos(θ)))
163+
@constraint(dop, derivative(v) == -D / m - g * sin(γ))
164+
@constraint(dop, derivative(γ) == L * cos(β) / (m * v) + cos(γ) * (v / r - g / v))
165+
@constraint(dop, derivative(ψ) == L * sin(β) / (m * v * cos(γ)) + v * cos(γ) * sin(ψ) * sin(θ) / (r * cos(θ)))
166+
167+
JuDO.warmstart!(dop, LinearInterpolant(2.6e5, 0.8e5), h)
168+
JuDO.warmstart!(dop, LinearInterpolant(0.0, deg2rad(90)), θ)
169+
JuDO.warmstart!(dop, LinearInterpolant(0.0, deg2rad(50)), Φ)
170+
JuDO.warmstart!(dop, LinearInterpolant(25600.0, 2500.0), v)
171+
JuDO.warmstart!(dop, LinearInterpolant(deg2rad(1), deg2rad(-5)), γ)
172+
JuDO.warmstart!(dop, LinearInterpolant(deg2rad(90),deg2rad(20) ), ψ)
173+
174+
# Objective
175+
@objective(dop, Max, final(θ))
176+
177+
JuDO.optimize!(dop)
178+
179+
h_sol = dyn_value(dop, h)
180+
θ_sol = dyn_value(dop, θ)
181+
Φ_sol = dyn_value(dop, Φ)
182+
v_sol = dyn_value(dop, v)
183+
γ_sol = dyn_value(dop, γ)
184+
ψ_sol = dyn_value(dop, ψ)
185+
α_sol = dyn_value(dop, α)
186+
β_sol = dyn_value(dop, β)
187+
188+
# --- 2. Extract Final Time & Time Grid ---
189+
tf = phase_final(t)
190+
ts = collect(range(0, tf, length=500))
191+
192+
# --- 3. Define Plotting Configuration ---
193+
# Format: (SolutionObject, Title, Y-Label, ScalingFunction, Filename)
194+
plot_configs = [
195+
(h_sol, "Altitude", "Altitude (km)", y -> y/1000, "altitude.png"),
196+
(θ_sol, "Latitude", "Latitude (deg)", y -> rad2deg(y), "latitude.png"),
197+
(Φ_sol, "Longitude", "Longitude (deg)", y -> rad2deg(y), "longitude.png"),
198+
(v_sol, "Velocity", "Velocity (km/s)", y -> y/1000, "velocity.png"),
199+
(γ_sol, "Flight Path Angle", "Flight Path Angle (deg)", y -> rad2deg(y), "flight_path.png"),
200+
(ψ_sol, "Azimuth", "Azimuth (deg)", y -> rad2deg(y), "Azimuth.png"),
201+
(α_sol, "Angle of Attack", "Angle of Attack (deg)", y -> rad2deg(y), "alpha.png"),
202+
(β_sol, "Bank Angle", "Bank Angle (deg)", y -> rad2deg(y), "beta.png")
203+
]
204+
traj = plot(
205+
rad2deg.(Φ_sol.(ts)),
206+
rad2deg.(θ_sol.(ts)),
207+
h_sol.(ts)./1000;
208+
linewidth = 1,
209+
legend = nothing,
210+
xlabel = "Longitude (deg)",
211+
ylabel = "Latitude (deg)",
212+
zlabel = "Altitude (km)",
213+
lc = :black
214+
)
215+
216+
savefig(traj,"3-d-trajectory.png")
217+
# --- 4. Loop, Plot, and Save ---
218+
for (sol, title_text, ylab, scale_fn, fname) in plot_configs
219+
# Create the plot
220+
p = plot(ts, t -> scale_fn(sol(t)),
221+
legend = false,
222+
ylabel = ylab,
223+
xlabel = "Time (s)",
224+
lw = 1,
225+
lc = :black
226+
)
227+
228+
# Save the figure
229+
savefig(p, fname)
230+
end
231+
"""

0 commit comments

Comments
 (0)