Simplify MINLP problem to speed up results

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)

x-ref: https://github.com/lanl-ansi/Juniper.jl/issues/169#issuecomment-567974709

You can use @NLexpression. (Note: for simplicity, everything here is linear. If you actually have linear constraints and expressions, you should use @constraint and @expression instead.)

For example, instead of:

model = Model()
x0 = 0
@variable(model, x[t = 1:5])
@variable(model, u[t = 1:5] >= 0)
@NLconstraint(model, x[1] == x0 + u[1])
@NLconstraint(model, [t = 2:5], x[t] == x[t-1]  + u[t])
@NLconstraint(model, x[5] == 1)

Use

model = Model()
x0 = 0
@variable(model, u[t = 1:5] >= 0)
x = [@NLexpression(model, x0 + u[1])]
for t = 2:5
    push!(x, @NLexpression(model, x[t - 1] + u[t]))
end
@NLconstraint(model, x[5] == 1)

Do you know why when I implement your suggestion I receive:

#Variables: 3003
#IntBinVar: 2003
#Constraints: 3006
#Linear Constraints: 1000
#Quadratic Constraints: 0
#NonLinear Constraints: 2006
Obj Sense: Min


Julia has exited.
Press Enter to start a new session.

My updated code:

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")
N = 500
kappa = kappa[1:N]
dS = dS[1:N]

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 = [@NLexpression(m, x0 + dS[1]*cos(theta0))]
y = [@NLexpression(m, y0 + dS[1]*sin(theta0))]
theta = [@NLexpression(m, theta0 + dS[1]*kappa[1])]

x_coord[1] = x0
y_coord[1] = y0
theta_coord[1] = theta0
@variable(m, alpha[1:4*length(x_coord)-1], Bin)
M = 110
for i = 2:length(x_coord)
    push!(x, @NLexpression(m, x[i - 1] + dS[i-1]*cos(theta[i-1])))
    push!(y, @NLexpression(m, y[i - 1] + dS[i-1]*sin(theta[i-1])))
    push!(theta, @NLexpression(m, theta[i - 1] + dS[i-1]*kappa[i-1]))
    # Obstacle Avoidance for just ONE obstacle with rectangular shape
    @NLconstraint(m, x[i] <= 40+alpha[4*(i-1)]*M)
    @NLconstraint(m, -x[i] <= -45+alpha[4*(i-1)+1]*M)
    @NLconstraint(m, y[i] <= 20+alpha[4*(i-1)+2]*M)
    @NLconstraint(m, -y[i] <= -30+alpha[4*(i-1)+3]*M)
end

@objective(m, Min, dot(kappa_var,kappa_var) + dot(dS_var,dS_var))

@NLconstraint(m, x_end_max, x[length(x_coord)]<=x[N]+0.05) # Fixed Final position
@NLconstraint(m, y_end_max, y[length(x_coord)]<=y[N]+0.05)
@NLconstraint(m, theta_end_max,theta[length(x_coord)]<=theta[N]+0.05)

@NLconstraint(m, x_end_min, x[length(x_coord)]>=x[N]-0.05)
@NLconstraint(m, y_end_min, y[length(x_coord)]>=y[N]-0.05)
@NLconstraint(m, theta_end_min,theta[length(x_coord)]>=theta[N]-0.05)

@constraint(m, [i = 1:length(dS)], dS_var[i] <= 0.20)
@constraint(m, [i = 1:length(dS)], dS_var[i] >= -0.20)

optimize!(m)

No idea without a full stack trace of the error. I assume this is in Juno/Atom? Try running Julia from a terminal and include("/path/to/file.jl") your file.

You should also read Please read: make it easier to help you. It’s worth spending time simplifying the example, but removing unneeded packages (e.g., Plots) and parameters and providing dummy data instead of the files.

Also, some things to improve your performance. Variable bounds (e.g., the last two constraints) are better specified as

@variable(m, -0.2 <= dS_var[1:N] <= 0.2)

Thanks a lot @odow ,
Have you ever experienced errors where the solver just exits without outputing any message? Now I run it through the terminal version (you can access julia from the JuliaPro package) and still outputs:

nl_solver   : OptimizerFactory(Ipopt.Optimizer, (), Base.Iterators.Pairs(:print_level=>0))
mip_solver  : OptimizerFactory(Cbc.Optimizer, (), Base.Iterators.Pairs(:logLevel=>0))
log_levels  : Symbol[:Options, :Table, :Info]

#Variables: 3003
#IntBinVar: 2003
#Constraints: 2006
#Linear Constraints: 0
#Quadratic Constraints: 0
#NonLinear Constraints: 2006
Obj Sense: Min

Killed

Without further information

Try simplifying your example to create a minimum working example, and then post back here.

What platform/versions are you using?