Solving optimal-control problems
Pendulum swing-up
In this example, we will solve an open-loop optimal-control problem (sometimes called trajectory optimization). The problem we will consider is to swing up a pendulum attached to a cart. A very similar tutorial that is using ModelingToolkit to build the model is available here Optimal control using ModelingToolkit models.
We start by defining the dynamics:
using JuliaSimControl
using JuliaSimControl.MPC
using JuliaSimControl.Symbolics
using LinearAlgebra
using StaticArrays
function cartpole(x, u, p, _=0)
T = promote_type(eltype(x), eltype(u))
mc, mp, l, g = 1.0, 0.2, 0.5, 9.81
q = x[SA[1, 2]]
qd = x[SA[3, 4]]
s = sin(q[2])
c = cos(q[2])
H = @SMatrix [mc+mp mp*l*c; mp*l*c mp*l^2]
C = @SMatrix [0 -mp*qd[2]*l*s; 0 0]
G = @SVector [0, mp * g * l * s]
B = @SVector [1, 0]
if T <: Symbolics.Num
qdd = Matrix(-H) \ Vector(C * qd + G - B * u[1])
return [qd; qdd]
else
qdd = -H \ (C * qd + G - B * u[1])
return [qd; qdd]
end
end
nu = 1 # number of controls
nx = 4 # number of states
Ts = 0.04 # sample time
N = 80 # Optimization horizon (number of time steps)
x0 = zeros(nx) # Initial state
r = [0, π, 0, 0] # Reference state is given by the pendulum upright (π rad)
discrete_dynamics = MPC.rk4(cartpole, Ts) # Discretize the dynamics
measurement = (x,u,p,t) -> x # The entire state is available for measurement
dynamics = FunctionSystem(discrete_dynamics, measurement, Ts; x=[:p, :α, :v, :ω], u=:u^nu, y=:y^nx)The next step is to define a cost function and constraints. We will use a quadratic StageCost and a TerminalStateConstraint to force the solution to end up in the desired terminal state: a stationary upright pendulum and a stationary cart in the origin. We also constraints the control input to have magnitude less than 10.
# Create objective
Q1 = Diagonal(@SVector ones(nx)) # state cost matrix
Q2 = 0.01Diagonal(@SVector ones(nu)) # control cost matrix
p = (; Q1, Q2) # Parameter vector
running_cost = StageCost() do si, p, t
Q1, Q2 = p.Q1, p.Q2
e = si.x .- si.r
u = si.u
dot(e, Q1, e) + dot(u, Q2, u)
end
terminal_constraint = TerminalStateConstraint(r, r) do ti, p, t
ti.x
end
objective = Objective(running_cost)
# Create objective input
x = zeros(nx, N+1)
u = zeros(nu, N)
oi = ObjectiveInput(x, u, r)
# Create constraints
control_constraint = StageConstraint([-10], [10]) do si, p, t
u = (si.u)[]
SA[
u
]
endWe will use the GenericMPCProblem structure to create the optimal-control problem. As the name implies, this technically defines an MPC problem, but an MPC problem is nothing more than a repeatedly solved optimal-control problem! When we create the problem, we pass presolve = true to have the problem solved immediately in the constructor.
# Create observer solver and problem
observer = StateFeedback(dynamics, x0)
solver = IpoptSolver(;
verbose = false,
tol = 1e-4,
acceptable_tol = 1e-2,
max_iter = 2000,
max_cpu_time = 60.0,
max_wall_time = 60.0,
constr_viol_tol = 1e-4,
acceptable_constr_viol_tol = 1e-2,
acceptable_iter = 100,
)
prob = GenericMPCProblem(
dynamics;
N,
observer,
objective,
constraints = [control_constraint, terminal_constraint],
p,
objective_input = oi,
solver,
xr = r,
presolve = true,
);With the problem solved, we may extract the optimal trajectories an plot them
using Plots
x_sol, u_sol = get_xu(prob)
fig = plot(
plot(x_sol', title="States", lab=permutedims(state_names(dynamics))),
plot(u_sol', title="Control signal", lab=permutedims(input_names(dynamics))),
)
hline!([π], ls=:dash, c=2, sp=1, lab="α = π")
fig
Rocket launch control
This example follows that of Optimal rocket control with JuMP, and highlights the differences in interface between JuliaSimControl and JuMP.jl, which is a general-purpose modeling language for optimization.
The control problem to solve is to optimize a thrust trajectory for a rocket that aims at maximizing the achieved altitude. The model of the rocket is a simple three-state model where we have the height $h$, the velocity $v$ and the mass $m$ as states. Due to the burning of fuel, the mass decreases during launch.
We start by defining the dynamics, all constants are normalized to be unitless:
using JuliaSimControl
using JuliaSimControl.MPC
using JuliaSimControl.Symbolics
using LinearAlgebra
using StaticArrays
const h_0 = 1 # Initial height
const v_0 = 0 # Initial velocity
const m_0 = 1 # Initial mass
const g_0 = 1 # Gravity at the surface
const T_c = 3.5 # Used for thrust
const m_c = 0.6 # Fraction of initial mass left at end
const m_f = m_c * m_0 # Final mass
const T_max = T_c * g_0 * m_0 # Maximum thrust
function rocket(x, u, p, _=0)
h_c = 500 # Used for drag
v_c = 620 # Used for drag
c = 0.5 * sqrt(g_0 * h_0) # Thrust-to-fuel mass
D_c = 0.5 * v_c * m_0 / g_0 # Drag scaling
h, v, m = x
T = u[] # Thrust (control signal)
drag = D_c * v^2 * exp(-h_c * (h - h_0) / h_0)
grav = g_0 * (h_0 / h)^2
SA[
v
(T - drag - m * grav) / m
-T/c
]
end
nu = 1 # number of control inputs
nx = 3 # number of states
N = 200 # Optimization horizon (number of time steps)
Ts = 0.001 # sample time
x0 = Float64[h_0, v_0, m_0] # Initial state
r = zeros(nx)
measurement = (x,u,p,t) -> x # The entire state is available for measurement
dynamics = FunctionSystem(rocket, measurement; x=[:h, :v, :m], u=:T, y=:y^nx)
discrete_dynamics = MPC.rk4(dynamics, Ts; supersample=3)Next, we define constraints on the states and inputs.
lb = [h_0, v_0, m_f, 0]
ub = [Inf, Inf, m_0, T_max]
stage_constraint = StageConstraint(lb, ub) do si, p, t
u = (si.u)[]
h,v,m = si.x
SA[h, v, m, u]
end
terminal_constraint = TerminalStateConstraint([m_f], [m_f]) do ti, p, t
SA[ti.x[3]] # The final mass must be m_f
end
terminal_cost = TerminalCost() do ti, p, t
h = ti.x[1]
-h # Maximize the terminal altitude
end
objective = Objective(terminal_cost)We also define an initial guess, we create an input-signal trajectory that renders the initial state rollout feasible.
using Plots
u = [0.7T_max * ones(nu, N÷5) T_max / 5 * ones(nu, 4N÷5) ]
x, u = MPC.rollout(discrete_dynamics, x0, u, 0, 0)
oi = ObjectiveInput(x, u, r)
plot(x', layout=3)
We are now ready to create to optimal-control problem:
observer = StateFeedback(discrete_dynamics, x0)
solver = IpoptSolver(;
verbose = true,
tol = 1e-8,
acceptable_tol = 1e-5,
constr_viol_tol = 1e-8,
acceptable_constr_viol_tol = 1e-5,
acceptable_iter = 10,
)
prob = GenericMPCProblem(
dynamics;
N,
observer,
Ts,
objective,
solver,
constraints = [stage_constraint, terminal_constraint],
objective_input = oi,
xr = r,
presolve = true,
verbose = false,
disc = Trapezoidal(; dyn=dynamics),
)This is Ipopt version 3.14.17, running with linear solver MUMPS 5.7.3.
Number of nonzeros in equality constraint Jacobian...: 3202
Number of nonzeros in inequality constraint Jacobian.: 800
Number of nonzeros in Lagrangian Hessian.............: 1407
Total number of variables............................: 803
variables with only lower bounds: 0
variables with lower and upper bounds: 0
variables with only upper bounds: 0
Total number of equality constraints.................: 604
Total number of inequality constraints...............: 800
inequality constraints with only lower bounds: 400
inequality constraints with lower and upper bounds: 400
inequality constraints with only upper bounds: 0
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 -1.0075766e+00 2.00e-02 1.00e+00 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 -1.0063530e+00 1.95e-02 9.76e-01 -1.7 1.15e+02 - 7.24e-03 2.41e-02h 1
2 -1.0088337e+00 1.88e-02 9.40e-01 -1.7 4.27e+01 - 2.88e-02 3.71e-02h 1
3 -1.0058541e+00 1.67e-02 8.37e-01 -1.7 2.62e+01 - 1.25e-02 1.09e-01h 1
4 -1.0074212e+00 1.59e-02 7.94e-01 -1.7 6.66e+01 - 9.24e-02 5.16e-02f 1
5 -1.0078096e+00 1.13e-02 4.19e+00 -1.7 1.18e+01 - 1.00e+00 2.91e-01h 1
6 -1.0077920e+00 1.13e-04 6.43e+00 -1.7 1.10e+00 - 3.61e-01 9.90e-01h 1
7 -1.0079742e+00 1.92e-06 8.66e+00 -1.7 7.41e-01 - 1.00e+00 9.90e-01h 1
8 -1.0079449e+00 7.05e-08 3.15e-03 -1.7 2.27e-01 - 1.00e+00 1.00e+00h 1
9 -1.0079474e+00 3.11e-11 6.88e-03 -3.8 1.20e-03 - 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 -1.0082841e+00 5.49e-07 9.16e-05 -3.8 1.46e-01 - 1.00e+00 1.00e+00f 1
11 -1.0086115e+00 5.54e-07 6.92e+02 -5.7 1.33e-01 - 9.53e-01 1.00e+00h 1
12 -1.0117591e+00 1.09e-04 5.27e+01 -5.7 9.45e-01 - 9.24e-01 1.00e+00h 1
13 -1.0123881e+00 4.01e-05 5.65e-03 -5.7 7.38e-01 - 1.00e+00 9.62e-01h 1
14 -1.0123459e+00 5.90e-06 6.18e-05 -5.7 4.52e-01 - 1.00e+00 1.00e+00f 1
15 -1.0123499e+00 2.31e-07 9.08e-07 -5.7 1.35e-01 - 1.00e+00 1.00e+00h 1
16 -1.0127975e+00 1.90e-05 2.30e+01 -8.6 4.46e-01 - 8.59e-01 9.84e-01h 1
17 -1.0128318e+00 1.49e-05 6.91e+00 -8.6 9.55e-01 - 7.24e-01 9.08e-01h 1
18 -1.0128344e+00 5.25e-06 2.01e+00 -8.6 9.15e-01 - 7.19e-01 9.72e-01h 1
19 -1.0128344e+00 2.90e-06 1.54e-01 -8.6 5.48e-01 - 9.22e-01 4.65e-01f 2
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 -1.0128344e+00 9.22e-07 2.10e-06 -8.6 5.96e-01 - 1.00e+00 1.00e+00h 1
21 -1.0128344e+00 4.81e-08 6.13e-08 -8.6 1.96e-01 - 1.00e+00 1.00e+00h 1
22 -1.0128344e+00 7.19e-10 4.05e-10 -8.6 2.27e-02 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 22
(scaled) (unscaled)
Objective...............: -1.0128344330149079e+00 -1.0128344330149079e+00
Dual infeasibility......: 4.0502327647466422e-10 4.0502327647466422e-10
Constraint violation....: 7.1898759168576021e-10 7.1898759168576021e-10
Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00
Complementarity.........: 2.5116524833972208e-09 2.5116524833972208e-09
Overall NLP error.......: 2.5116524833972208e-09 2.5116524833972208e-09
Number of objective function evaluations = 24
Number of objective gradient evaluations = 23
Number of equality constraint evaluations = 24
Number of inequality constraint evaluations = 24
Number of equality constraint Jacobian evaluations = 23
Number of inequality constraint Jacobian evaluations = 23
Number of Lagrangian Hessian evaluations = 22
Total seconds in IPOPT = 34.569
EXIT: Optimal Solution Found.When the problem is solved, we may plot the optimal trajectory
using Plots
x_sol, u_sol = get_xu(prob)
plot(
plot(x_sol', title="States", lab=permutedims(state_names(dynamics)), layout=(nx,1)),
plot(u_sol', title="Control signal", lab=permutedims(input_names(dynamics))),
)
using Test
@test x_sol[1, end] > 1.012 # Test that the rocket reached high enoughTest Passed