In [1]:

    

using JuMP
using NLopt
using Winston


    

In [1]:

    

m = Model(solver=NLoptSolver(algorithm=:LD_SLSQP))

nodes = 40

# state variables for for direct transcription
@defVar(m, l_x[1:3,1:nodes])
# input decision variables -- u[1] is actually u[0] 
@defVar(m, b_u[1:nodes])

b_dx = 80.0/3.6
b_dy = 0
dt = 0.5;
println("Constant forward body speed of [", b_dx, ", ", b_dy,"] [m/s]")

# Initial conditions
l_x0 = 30.
l_y0 = -60.
l_th0 = 3.14159265358979323

umax = 0.7;

# Set the objective function
@setNLObjective(m, Min, sum{l_x[1,i]^2 + l_x[2,i]^2 + 10.0*(b_u[i])^2, i = 1:nodes} - (b_u[nodes])^2)

for n in 1:nodes
    #initialize variables
    setValue(l_x[1,n],0.)
    setValue(l_x[2,n],0.)
    setValue(l_x[3,n],0.)
    setValue(b_u[n],0.)
end

# initial condition constraints on dynamics
@addNLConstraint(m, b_u[1] <= umax)
@addNLConstraint(m, -b_u[1] <= umax)
@addNLConstraint(m, l_x[1,1] == l_x0 + dt*(cos(l_th0)*b_dx))
@addNLConstraint(m, l_x[2,1] == l_y0 + dt*(sin(l_th0)*b_dx))
@addNLConstraint(m, l_x[3,1] == l_th0 + dt*b_u[1])

for n in 1:(nodes-1)
    # dynamics constraints
    @addNLConstraint(m, l_x[1,n+1] == l_x[1,n] + dt*(cos(l_x[3,n])*b_dx))
    @addNLConstraint(m, l_x[2,n+1] == l_x[2,n] + dt*(sin(l_x[3,n])*b_dx))
    @addNLConstraint(m, l_x[3,n+1] == l_x[3,n] + dt*b_u[n+1])
    @addNLConstraint(m, b_u[n+1] <= umax)
    @addNLConstraint(m, -b_u[n+1] <= umax)
    
end

toc = @elapsed status = solve(m)

# get optimal values in general numeric array
x = zeros(3,nodes)
u = zeros(nodes)
for i in 1:nodes, j in 1:3
    x[j,i] = getValue(l_x[j,i])
    u[i] = getValue(b_u[i])
end

println(toc)
plot(x[1,:],x[2,:])


    

    




Model not defined
while loading In[1], in expression starting on line 1

In [4]:

    

plot(u)


    

    
Out[4]:

In [28]:

    

println(toc)
println("got ", getObjectiveValue(m), " at ", [getValue(l_x[1,1]), getValue(l_x[2,1]), getValue(b_u[1])])


    

    




3.814518798
got 2.0667222697890685e6 at [200.0,211.11111111111111,-0.6999997372936334]

In [ ]: