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