Skip to content

Instantly share code, notes, and snippets.

@mfalt
Created Nov 19, 2020
Embed
What would you like to do?
NLOptControl
using Ipopt # Solver
using NLOptControl # Problem definition and discretization
using Plots # Custom plots
# =========== PROBLEM DEFINITION ==============================
# Constraints on states
XL = [-Inf,-Inf,-Inf,-Inf]
XU = [Inf,Inf,Inf,Inf]
# Constraints on control, we will add ux^2+uy^2 <= 1, so these won't be active
CL = [-1.0, -1.0]
CU = [1.0, 1.0]
# Initial and final constraints on states
X0 = [-1.5, 0, 0, 0]
XF = [1.5, 0, 0, 0]
# Create problem ===============================================================
prob = define(numStates = 4, numControls=2,
X0=X0, XF=XF, CL=CL, CU=CU, XL=XL, XU=XU)
# Give name to states and controls
states!(prob, [:x, :vx, :y, :vy],
descriptions=["x(t)", "vx(t)", "y(t)", "vy(t))"])
controls!(prob, [:ux, :uy], descriptions=["ux", "uy"])
# Setup model equations as julia **expressions** ===============================
dx = Array{Expr,1}(undef,4)
dx[1] = :( vx[j] )
dx[2] = :( ux[j] )
dx[3] = :( vy[j] )
dx[4] = :( uy[j] )
## Add dynamics to model =======================================================
dynamics!(prob, dx)
# The problem is now defined as "prob"
# Obstracle constraints
expr1 = :( ux[j]^2 + uy[j]^2 <= 1 )
expr2 = :( y[j] >= cos(x[j]) - 0.2 )
# Try with both [expr1] and [expr1, expr2]
constraints!(prob, [expr1, expr2])
# Number of grid points in discretization
N = 700
# Final time fixed, backward Euler discretization
configure!(prob; finalTimeDV=false, tf=4.5, integrationScheme=:bkwEuler, N=N)
# Set objective
obj = integrate!(prob, :( ux[j]^2 + uy[j]^2) )
# Set to minimize obj
@NLobjective(prob.ocp.mdl, Min, obj)
@time optimize!(prob)
# This plot from "PrettyPlots" plots all states
# allPlots(prob)
# Get the internal data
data = prob.r.ocp.dfs[1]
# Plot x, vx, ux
p1 = plot(data[:t], data[:x], lab="x(t)", xlab="t");
plot!(p1,data[:t], data[:vx], lab="vx(t)");
plot!(p1,data[:t], data[:ux], lab="ux(t)");
display(p1)
# Plot x,y plane
p2 = plot(data[:x], data[:y], xlims=(-1.5,1.5), ylims=(-1.5, 1.5),
label="trajectory", xlabel="x", ylabel="y", reuse=false)
# reuse=false means use new plot window
# Plot constraint
xs = -1.5:0.1:1.5
plot!(p2,xs, cos.(xs).-0.2, label="constraint")
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment