Possibility to reduce ammount of variables

[hect1995]

Hey people, sorry to bother again, but I want to optimize the path of a vehicle consisting on an array/vector of state states of the form [x_i,y_i,theta_i] and my control inputs are [kappa_i,dS_i]. As I am dealing with Mixed Integer representation (for obstacle 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 question is, as I know how x_i,y_i,theta_i evolve based on f(kappa_i,dSi), i.e. x(i+1) = x_i + dS_i*cos(theta_i), I was trying to remove x_i, y_i and theta_i from being @variables and represent all the obstacle avoidance steps and final position (my final position is fixed) based on my initial position and just the control inputs. The problem is that in the for loop I am trying to equal a JuMP variable into a Julia variable, which throws the error: ERROR: LoadError: UndefVarError: new_x not defined My code is attached below. I would appreciate if you could tell me any trick to reduce the amount of variables while still considering obstacle avoidance and a fixed final position (without having to consider the array of x_i,y_i,theta_i as @variables of the problem which makes it extremely slow or any other way to speed up the optimization (consider gradients?)).

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")
dS = npzread("../../../Processing/dS.npy")
kapp = kappa[1:1500] # Deal with 1500 timesteps
dS = dS[1:1500]

x0 = 3.999999999994130917e+01 # Initial pose of the vehicle
y0 = 2.014000000000001123e+00
theta0 = 1.570796326790000030e+00

@variable(m, kappa_var[1:length(kappa)])
@variable(m, dS_var[1:length(kappa)])

set_start_value.(kappa_var, control)
set_start_value.(dS_var, dS)

@objective(m, Min, dot(kappa_var,kappa_var) + dot(dS_var,dS_var))
@variable(m, alpha[1:4], Bin)
M = 110
new_x = x0
new_y = y0
new_theta = theta0
for i = 2:length(dS_var)
    new_x = new_x + dS_var[i-1]*cos(new_theta) # x_i+1 = f(kappa_i,dS_i)
    new_y = new_y + dS_var[i-1]*sin(new_theta) # y_i+1 = f(kappa_i,dS_i)
    new_theta = new_theta + dS_var[i-1]*control[i-1]  # theta_i+1 = f(kappa_i,dS_i)
    @NLconstraint(m, new_x <= 40+alpha[1]*M) # Binary variables to perform obstacle avoidance
    @NLconstraint(m, -new_x <= -45+alpha[2]*M)
    @NLconstraint(m, new_y <= 20+alpha[3]*M)
    @NLconstraint(m, -new_y <= -30+alpha[4]*M)
end
@constraint(m, x_end_max, new_x<=33.20) # Last position is fixed
@constraint(m, y_end_max, new_y<=41.29)
@constraint(m, theta_end_max,new_theta<=-0.58)
@constraint(m, x_end_min, new_x>=33.17)
@constraint(m, y_end_min, new_y>=41.24)
@constraint(m, theta_end_min,new_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)

optimize!(m)

My original working but infite slow solution was this:

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))
control = npzread("../../../Processing/control.npy")
dS = npzread("../../../Processing/dS.npy")
control = control[1:1500]
dS = dS[1:1500]

x0 = 3.999999999994130917e+01
y0 = 2.014000000000001123e+00
theta0 = 1.570796326790000030e+00
x_coord = zeros(Float64, length(control)+1)
y_coord = zeros(Float64, length(control)+1)
theta_coord = zeros(Float64, length(control))
@variable(m, control_var[1:length(control))
@variable(m, dS_var[1:length(control))

set_start_value.(control_var, control)
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])
    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]*control[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)
set_start_value.(y, y_coord)
set_start_value.(theta, theta_coord)

@objective(m, Min, dot(control_var,control_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])
    @NLconstraint(m, y[i-1]+dS_var[i-1]*sin(theta[i-1]) == y[i])
    @NLconstraint(m, theta[i-1]+dS_var[i-1]*control_var[i-1] == theta[i])
end
@constraint(m, x_init, x[1]==x0)
@constraint(m, y_init, y[1] == y0)
@constraint(m, theta_init, theta[1] == theta0)

@constraint(m, x_end_max, x[length(x)]<=33.20)
@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
@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)

[odow]

These questions are better posted on our user forum: https://discourse.julialang.org/c/domain/opt/13

You're looking for @NLexpression: https://www.juliaopt.org/JuMP.jl/v0.20.0/expressions/#Nonlinear-expressions-1

The Rocket Control tutorial is a good example to follow of nonlinear MPC.

[hect1995]

Thanks a lot, Ill ask over there