Minimum-time swing-up: a worked solution¶
The worked solution to exercise 1 of tutorial 2. Attempt it there first.
The pendulum of tutorial 2 has a motor at the pivot, capped at
u_max = 10. Instead of minimizing control effort over a fixed five-second
horizon, we ask for the fastest swing-up from hanging down to upright:
$$ \begin{aligned} \min_{\theta, \omega, u,\, \Delta t}\quad & N \Delta t \\ \text{s.t.}\quad & \theta_i - \theta_{i-1} - \tfrac{\Delta t}{2}(\omega_i + \omega_{i-1}) = 0, && i = 1, \dots, N, \\ & \omega_i - \omega_{i-1} - \tfrac{\Delta t}{2}\big(f(\theta_i,\omega_i,u_i) + f(\theta_{i-1},\omega_{i-1},u_{i-1})\big) = 0, && i = 1, \dots, N, \\ & -u_{\max} \le u_i \le u_{\max}, && i = 0, \dots, N, \\ & \theta_0 = 0,\quad \omega_0 = 0,\quad \theta_N = \pi,\quad \omega_N = 0 . \end{aligned} $$
The horizon enters through $\Delta t$, the step length, which is now a decision variable rather than a constant. This is the same device Goddard's rocket uses for its free final time: minimizing the step minimizes the horizon, and the step multiplies the states inside every dynamics row, so those constraints become nonlinear in a variable they did not previously contain. The generators are written exactly as before.
In [1]:
using CUDA, CUDSS, ExaModels, MadNLP, MadNLPGPU
using Plots
gr(fmt = :svg)
function swingup_mintime(N; u_max = 10.0, backend = nothing)
c = ExaCore(; backend = backend)
@add_var(c, dt, 1; start = 5.0 / N, lvar = 0.0)
@add_var(c, θ, 0:N; start = [π * i / N for i = 0:N])
@add_var(c, ω, 0:N; start = fill(π / 5.0, N + 1))
@add_var(c, u, 0:N; lvar = -u_max, uvar = u_max, start = zeros(N + 1))
@add_obj(c, dt[1])
@add_con(c, θ[i] - θ[i-1] - dt[1] / 2 * (ω[i] + ω[i-1]) for i = 1:N)
@add_con(c,
ω[i] - ω[i-1] - dt[1] / 2 *
((u[i] - 9.81 * sin(θ[i]) - 0.1 * ω[i]) +
(u[i-1] - 9.81 * sin(θ[i-1]) - 0.1 * ω[i-1])) for i = 1:N)
@add_con(c, θ[0])
@add_con(c, ω[0])
@add_con(c, θ[N] - π)
@add_con(c, ω[N])
return ExaModel(c)
end
N = 1000
model_mt = swingup_mintime(N; backend = CUDABackend())
result_mt = madnlp(model_mt; max_iter = 1000, tol = 1e-8)
(status = result_mt.status, minimum_time_s = N * result_mt.objective)
This is MadNLP version v0.10.1, running with cuDSS v0.8.0 Number of nonzeros in constraint Jacobian............: 12004 Number of nonzeros in Lagrangian Hessian.............: 34000 Total number of variables............................: 3004 variables with only lower bounds: 1 variables with lower and upper bounds: 1001 variables with only upper bounds: 0 Total number of equality constraints.................: 0 Total number of inequality constraints...............: 2004 inequality constraints with only lower bounds: 0 inequality constraints with lower and upper bounds: 2004 inequality constraints with only upper bounds: 0 iter objective inf_pr inf_du inf_compl lg(mu) lg(rg) alpha_pr ir ls 0 9.9999900e-03 6.28e-01 0.00e+00 1.00e+01 -1.0 - 0.00e+00 0 0 1 9.9999851e-03 6.28e-01 1.62e-01 3.85e-03 -1.0 - 2.00e-07 2 2h 2 5.2572598e-04 4.97e-02 7.09e-01 5.74e-04 -1.7 - 1.00e+00 4 1h 3 5.4278432e-04 4.90e-02 6.02e+00 5.97e-04 -1.7 - 1.22e-02 3 1h 4 6.7880343e-04 4.45e-02 1.86e+02 5.71e-04 -1.7 - 8.75e-02 3 1h 5 1.2695441e-03 2.80e-02 2.88e+02 2.74e-04 -1.7 - 3.71e-01 3 1h 6 1.3906611e-03 1.59e-02 1.59e+02 1.08e-04 -1.7 - 4.32e-01 3 1h 7 1.5052025e-03 3.69e-04 2.43e+01 3.54e-05 -1.7 - 1.00e+00 3 1h 8 1.5214465e-03 1.44e-05 2.52e-01 3.44e-06 -1.7 - 1.00e+00 3 1h 9 2.5740849e-03 1.75e-02 4.32e-01 3.40e-06 -1.7 - 6.97e-01 10 1f iter objective inf_pr inf_du inf_compl lg(mu) lg(rg) alpha_pr ir ls 10 1.9798645e-03 6.22e-03 5.78e-01 6.98e-06 -1.7 -4.0 1.00e+00 2 1h 11 3.7627186e-03 1.64e-02 1.72e-01 3.57e-06 -1.7 -4.5 1.00e+00 2 1h 12 4.4689073e-03 1.49e-02 1.70e-01 3.30e-06 -2.5 -3.1 8.60e-02 1 1h 13 4.1863799e-03 1.63e-03 4.12e-01 3.86e-06 -2.5 -2.7 1.00e+00 1 1h 14 5.9680293e-03 1.72e-02 2.84e-01 5.70e-05 -2.5 -3.2 1.00e+00 1 1h 15 6.5082146e-03 9.06e-04 1.20e+00 9.90e-06 -2.5 -1.9 1.00e+00 2 1h 16 6.1839375e-03 2.79e-03 1.23e+00 3.00e-06 -2.5 -2.3 1.00e+00 2 1h 17 6.4177876e-03 8.90e-05 1.07e-02 3.01e-06 -2.5 -2.8 1.00e+00 2 1h 18 6.4114118e-03 7.83e-07 1.63e-02 3.27e-06 -3.8 -3.3 1.00e+00 1 1h 19 6.3596570e-03 1.62e-05 9.17e-05 3.02e-06 -3.8 -3.8 1.00e+00 2 1h iter objective inf_pr inf_du inf_compl lg(mu) lg(rg) alpha_pr ir ls 20 6.1820047e-03 8.53e-05 2.81e-01 5.88e-06 -5.7 -4.3 1.00e+00 1 1h 21 5.6126847e-03 8.10e-04 1.11e-02 2.17e-06 -5.7 -4.7 1.00e+00 4 1h 22 4.6508774e-03 2.57e-03 1.19e-01 1.84e-06 -5.7 -5.2 1.00e+00 4 1h 23 3.8027465e-03 3.03e-03 1.09e-01 1.84e-06 -5.7 -5.7 1.00e+00 3 1h 24 3.1771586e-03 2.52e-03 2.46e-02 1.84e-06 -5.7 -6.2 1.00e+00 4 1h 25 2.7038203e-03 2.94e-03 9.40e-02 1.86e-06 -5.7 -6.6 1.00e+00 5 1h 26 2.3233886e-03 6.12e-03 2.96e-01 1.94e-06 -5.7 -7.1 1.00e+00 7 1h 27 2.2867486e-03 5.84e-03 2.20e-01 1.93e-06 -5.7 -7.6 3.17e-01 7 2h 28 2.2325431e-03 2.29e-03 2.47e-02 3.09e-06 -5.7 -7.2 1.00e+00 8 1h 29 2.1960614e-03 8.05e-04 5.00e-03 1.85e-06 -5.7 -7.6 1.00e+00 10 1h iter objective inf_pr inf_du inf_compl lg(mu) lg(rg) alpha_pr ir ls 30 2.2022929e-03 6.62e-05 4.15e-04 1.87e-06 -5.7 -7.2 1.00e+00 9 1h 31 2.2121360e-03 1.14e-04 1.16e-03 1.84e-06 -5.7 -7.7 1.00e+00 9 1h 32 2.2216377e-03 6.88e-05 8.69e-05 1.84e-06 -5.7 -8.2 1.00e+00 9 1h 33 2.2245137e-03 4.74e-06 1.18e-05 1.84e-06 -5.7 -8.7 1.00e+00 10 1h 34 1.9108841e-03 1.36e-03 3.49e-02 6.81e-07 -8.6 - 1.00e+00 10 1h 35 1.7689045e-03 1.58e-03 2.19e-02 4.28e-07 -8.6 - 5.42e-01 10 1h 36 1.7095304e-03 1.23e-03 1.97e-02 3.41e-07 -8.6 -9.1 2.13e-01 10 1h 37 1.6787892e-03 1.22e-03 1.54e-02 3.38e-07 -8.6 -9.6 6.57e-03 10 1h 38 1.6224201e-03 4.08e-04 8.35e-03 1.34e-07 -8.6 -7.4 6.55e-01 5 1h 39 1.5782538e-03 3.20e-04 8.11e-03 9.12e-08 -8.6 -7.9 3.32e-01 4 1h iter objective inf_pr inf_du inf_compl lg(mu) lg(rg) alpha_pr ir ls 40 1.5497715e-03 1.86e-04 6.70e-03 5.29e-08 -8.6 -7.4 4.22e-01 3 1h 41 1.5228869e-03 2.71e-04 6.14e-03 4.86e-08 -8.6 -7.9 8.57e-02 3 1h 42 1.5058606e-03 2.24e-04 6.35e-03 4.43e-08 -8.6 -7.5 1.98e-01 2 1h 43 1.4832655e-03 9.27e-05 5.42e-03 1.98e-08 -8.6 -7.1 5.85e-01 2 1h 44 1.4653039e-03 9.05e-05 5.53e-03 2.14e-08 -8.6 -7.5 2.17e-01 2 1h 45 1.4355077e-03 1.52e-04 1.17e-02 2.11e-08 -8.6 -8.0 1.73e-02 2 1h 46 1.4244270e-03 1.39e-04 1.12e-02 2.85e-08 -8.6 -7.6 1.06e-01 2 1h 47 1.3889473e-03 2.07e-04 1.92e-02 2.74e-08 -8.6 -8.1 5.33e-02 2 1h 48 1.3764581e-03 1.90e-04 1.75e-02 3.27e-08 -8.6 -7.6 1.27e-01 2 1h 49 1.3461763e-03 2.53e-04 2.05e-02 2.96e-08 -8.6 -8.1 1.01e-01 2 1h iter objective inf_pr inf_du inf_compl lg(mu) lg(rg) alpha_pr ir ls 50 1.3226736e-03 3.43e-04 2.34e-02 2.92e-08 -8.6 -8.6 2.11e-02 2 1h 51 1.3175305e-03 3.48e-04 2.32e-02 2.68e-08 -8.6 -9.1 1.30e-02 2 1h 52 1.3124888e-03 3.43e-04 2.28e-02 2.49e-08 -8.6 - 1.68e-02 2 1h 53 1.3045748e-03 3.25e-04 2.15e-02 2.21e-08 -8.6 - 5.10e-02 2 1h 54 1.3004180e-03 2.87e-04 1.89e-02 1.95e-08 -8.6 - 1.17e-01 2 1h 55 1.2977883e-03 2.35e-04 1.55e-02 1.64e-08 -8.6 - 1.81e-01 2 1h 56 1.2949243e-03 1.57e-04 1.04e-02 1.18e-08 -8.6 - 3.31e-01 2 1h 57 1.2906632e-03 2.31e-05 9.49e-04 3.58e-09 -8.6 - 9.05e-01 2 1h 58 1.2903998e-03 4.98e-07 3.31e-07 2.61e-09 -8.6 - 1.00e+00 2 1h 59 1.2903998e-03 3.32e-12 8.67e-12 2.51e-09 -8.6 - 1.00e+00 1 1h Number of Iterations....: 59 (scaled) (unscaled) Objective...............: 1.2903998125995703e-03 1.2903998125995703e-03 Dual infeasibility......: 8.6706197777175475e-12 8.6706197777175475e-12 Constraint violation....: 3.3249965958146237e-12 3.3249965958146237e-12 Complementarity.........: 2.5066038582115816e-09 2.5066038582115816e-09 Overall NLP error.......: 2.5066038582115816e-09 2.5066038582115816e-09 Number of objective function evaluations = 63 Number of objective gradient evaluations = 60 Number of constraint evaluations = 63 Number of constraint Jacobian evaluations = 60 Number of Lagrangian Hessian evaluations = 59 Number of KKT factorizations = 114 Number of KKT backsolves = 257 Total wall secs in initialization = 3.114 s Total wall secs in linear solver = unavailable Total wall secs in NLP function evaluations = 1.366 s Total wall secs in solver (w/o init./fun./lin. alg.) = unavailable Total wall secs = 13.782 s EXIT: Optimal Solution Found (tol = 1.0e-08).
Out[1]:
(status = MadNLP.SOLVE_SUCCEEDED, minimum_time_s = 1.2903998125995704)
The trajectory and the torque:
In [2]:
θ_mt = Array(solution(result_mt, model_mt.refs.θ))
u_mt = Array(solution(result_mt, model_mt.refs.u))
t_mt = range(0, N * result_mt.objective; length = N + 1)
plot(t_mt, [θ_mt u_mt];
layout = (2, 1), legend = false, lw = 2, size = (760, 460),
left_margin = 4Plots.mm, bottom_margin = 4Plots.mm,
title = ["minimum-time swing-up: θ(t)" "torque u(t), |u| <= 10"],
xlabel = ["" "t [s]"])
Out[2]:
The torque sits at its limit and switches sign sharply, where minimizing effort spread a gentler torque over the full five seconds. Minimizing time pushes the actuator against its bounds; minimizing effort keeps it away from them.
This notebook was generated using Literate.jl.