1+ using ModelingToolkit, InfiniteOpt, JuMP, Ipopt
2+ using ModelingToolkit: D_nounits as D, t_nounits as t, varmap_to_vars
3+
4+ @mtkmodel Pendulum begin
5+ @parameters begin
6+ g = 9.8
7+ L = 0.4
8+ K = 1.2
9+ m = 0.3
10+ end
11+ @variables begin
12+ θ (t) # state
13+ ω (t) # state
14+ τ (t) = 0 # input
15+ y (t) # output
16+ end
17+ @equations begin
18+ D (θ) ~ ω
19+ D (ω) ~ - g/ L* sin (θ) - K/ m* ω + τ/ m/ L^ 2
20+ y ~ θ * 180 / π
21+ end
22+ end
23+ @named model = Pendulum ()
24+ model = complete (model)
25+
26+ inputs = [model. τ]
27+ (f_oop, f_ip), dvs, psym, io_sys = ModelingToolkit. generate_control_function (model, inputs, split= false )
28+
29+ outputs = [model. y]
30+ f_obs = ModelingToolkit. build_explicit_observed_function (io_sys, outputs; inputs = inputs)
31+
32+ expected_state_order = [model. θ, model. ω]
33+ permutation = [findfirst (isequal (x), expected_state_order) for x in dvs] # This maps our expected state order to the actual state order
34+
35+ # #
36+
37+ ub = varmap_to_vars ([model. θ => 2pi , model. ω => 10 ], dvs)
38+ lb = varmap_to_vars ([model. θ => - 2pi , model. ω => - 10 ], dvs)
39+ xf = varmap_to_vars ([model. θ => pi , model. ω => 0 ], dvs)
40+ nx = length (dvs)
41+ nu = length (inputs)
42+ ny = length (outputs)
43+
44+ # #
45+ m = InfiniteModel (optimizer_with_attributes (Ipopt. Optimizer,
46+ " print_level" => 0 , " acceptable_tol" => 1e-3 , " constr_viol_tol" => 1e-5 , " max_iter" => 1000 ,
47+ " tol" => 1e-5 , " mu_strategy" => " monotone" " nlp_scaling_method" => " gradient-based"
48+ " alpha_for_y" => " safer-min-dual-infeas" " bound_mult_init_method" => " mu-based" " print_user_options" => " yes"
49+
50+
51+ @infinite_parameter (m, τ in [0 , 1 ], num_supports = 51 , derivative_method = OrthogonalCollocation (4 )) # Time variable
52+ guess_xs = [t -> pi , t -> 0.1 ][permutation]
53+ guess_us = [t -> 0.1 ]
54+ InfiniteOpt. @variables (m, begin
55+ # state variables
56+ (lb[i] <= x[i = 1 : nx] <= ub[i], Infinite (τ), start = guess_xs[i]) # state variables
57+ - 10 <= u[i = 1 : nu] <= 10 , Infinite (τ), (start = guess_us[i]) # control variables
58+ 0 <= tf <= 10 , (start = 5 ) # Final time
59+ 0.2 <= L <= 0.6 , (start = 0.4 ) # Length parameter
60+ end )
61+
62+ # Trace the dynamics
63+ x0, p = ModelingToolkit. get_u0_p (io_sys, [model. θ => 0 , model. ω => 0 ], [model. L => L])
64+
65+ xp = f_oop (x, u, p, τ)
66+ cp = f_obs (x, u, p, τ) # Test that it's possible to trace through an observed function
67+
68+ @objective (m, Min, tf)
69+ @constraint (m, [i = 1 : nx], x[i](0 ) == x0[i]) # Initial condition
70+ @constraint (m, [i = 1 : nx], x[i](1 ) == xf[i]) # Terminal state
71+
72+ x_scale = varmap_to_vars ([
73+ model. θ => 1
74+ model. ω => 1
75+ ], dvs)
76+
77+
78+ # Add dynamics constraints
79+ @constraint (m, [i = 1 : nx], (∂ (x[i], τ) - tf* xp[i]) / x_scale[i] == 0 )
80+
81+ optimize! (m)
82+
83+ # Extract the optimal solution
84+ opt_tf = value (tf)
85+ opt_time = opt_tf * value (τ)
86+ opt_x = [value (x[i]) for i in permutation]
87+ opt_u = [value (u[i]) for i in 1 : nu]
88+ opt_L = value (L)
89+
90+ # Plot the results
91+ # using Plots
92+ # plot(opt_time, opt_x[1], label = "θ", xlabel = "Time [s]", layout=3)
93+ # plot!(opt_time, opt_x[2], label = "ω", sp=2)
94+ # plot!(opt_time, opt_u[1], label = "τ", sp=3)
95+
96+
97+ using Test
98+ @test opt_x[1 ][end ] ≈ pi atol= 1e-3
99+ @test opt_x[2 ][end ] ≈ 0 atol= 1e-3
100+
101+ @test opt_x[1 ][1 ] ≈ 0 atol= 1e-3
102+ @test opt_x[2 ][1 ] ≈ 0 atol= 1e-3
103+
104+ @test opt_L ≈ 0.2 atol= 1e-3 # Smallest permissible length is optimal
0 commit comments