Hey people,
I want to optimize the path of a vehicle consisting on an array/vector of states of the form [x_i,y_i,theta_i] and my control inputs are [kappa_i,dS_i]. As I am dealing with a MINLP problem (MI because I perform collision avoidance) I have to set as a constraint that each [x_i,y_i] pair is collision free, which adds a lot of constraints and variables to my problem.
My objective is to optimize the control inputs to make them as small as possible while still reaching a goal pose [x_goal, y_goal, theta_goal].
I have to use a current non-optimal path as initialization because it has been demanded to me to not start the optimization from scratch. The problem is that adding the Mixed Integer problem adds a lot of constraints, as each state needs to be collision free, and it runs forever (and I am just considering one obstacle). Do you know any way I can optimize my current code in order to make it converge faster?
Pkg.add("JuMP")
Pkg.add("Ipopt")
Pkg.add("Juniper")
Pkg.add("Cbc")
Pkg.add("LinearAlgebra")
Pkg.add("NPZ")
Pkg.add("Plots")
using Juniper
using JuMP
using Ipopt
using Cbc
using LinearAlgebra # for the dot product
using NPZ
using Plots
optimizer = Juniper.Optimizer
params = Dict{Symbol,Any}()
params[:nl_solver] = with_optimizer(Ipopt.Optimizer,print_level=0)
params[:mip_solver] = with_optimizer(Cbc.Optimizer,logLevel=0)
m = Model(with_optimizer(optimizer, params))
kappa = npzread("../../../Processing/control.npy") # Initial guess for my controls obtained from A*
dS = npzread("../../../Processing/dS.npy")
kappa = kappa[1:1500]
dS = dS[1:1500]
x0 = 3.999999999994130917e+01 # Initial pose
y0 = 2.014000000000001123e+00
theta0 = 1.570796326790000030e+00
x_coord = zeros(Float64, length(kappa)+1)
y_coord = zeros(Float64, length(kappa)+1)
theta_coord = zeros(Float64, length(kappa)+1)
@variable(m, kappa_var[1:length(kappa)])
@variable(m, dS_var[1:length(kappa)])
set_start_value.(kappa_var, kappa) # Initialize controls
set_start_value.(dS_var, dS)
x_coord[1] = x0
y_coord[1] = y0
theta_coord[1] = theta0
for i = 2:length(x_coord)
x_coord[i] = x_coord[i-1] + dS[i-1]*cos(theta_coord[i-1]) # Kinematics of the vehicle
y_coord[i] = y_coord[i-1] + dS[i-1]*sin(theta_coord[i-1])
theta_coord[i] = theta_coord[i-1] + dS[i-1]*kappa[i-1]
end
@variable(m, x[1:length(x_coord)])
@variable(m, y[1:length(x_coord)])
@variable(m, theta[1:length(x_coord)])
set_start_value.(x, x_coord) # Initialize poses
set_start_value.(y, y_coord)
set_start_value.(theta, theta_coord)
@objective(m, Min, dot(kappa_var,kappa_var) + dot(dS_var,dS_var))
for i = 2:length(x_coord)
@NLconstraint(m, x[i-1]+dS_var[i-1]*cos(theta[i-1]) == x[i]) # Kinematic constraints
@NLconstraint(m, y[i-1]+dS_var[i-1]*sin(theta[i-1]) == y[i])
@NLconstraint(m, theta[i-1]+dS_var[i-1]*kappa_var[i-1] == theta[i])
end
@constraint(m, x_init, x[1]==x0) # Fixed initial position
@constraint(m, y_init, y[1] == y0)
@constraint(m, theta_init, theta[1] == theta0)
@constraint(m, x_end_max, x[length(x)]<=33.20) # Fixed Final position
@constraint(m, y_end_max, y[length(y)]<=41.29)
@constraint(m, theta_end_max,theta[length(theta)]<=-0.58)
@constraint(m, x_end_min, x[length(x)]>=33.17)
@constraint(m, y_end_min, y[length(y)]>=41.24)
@constraint(m, theta_end_min,theta[length(theta)]>=-0.586)
@constraint(m, [i = 1:length(dS)], dS_var[i] <= 0.20)
@constraint(m, [i = 1:length(dS)], dS_var[i] >= -0.20)
# Obstacle Avoidance for just ONE obstacle with rectangular shape
@variable(m, alpha[1:4], Bin)
M = 110
@NLconstraint(m, [i = 1:length(x)], x[i] <= 40+alpha[1]*M)
@NLconstraint(m, [i = 1:length(x)], -x[i] <= -45+alpha[2]*M)
@NLconstraint(m, [i = 1:length(y)], y[i] <= 20+alpha[3]*M)
@NLconstraint(m, [i = 1:length(y)], -y[i] <= -30+alpha[4]*M)
optimize!(m)