# Solving MPC using JuMP and Gurobi

I am attempting to translate the MPC implemented in python and cvxpy here in Julia using JuMP and Gurobi. However when attempting to solve the first iteration of the optimization I set up I am getting an unconstrained/infeasible error. I am fairly new to quadradic optimization and think I made an error in one of these areas:

1. translation of cvxpy.quad_form(A,B) to the julia transpose(A)BA
for example:

Python:

``````cost += cvxpy.quad_form(u[:, t], R)
``````

Julia:

``````cost += transpose(u[:,t])*R*u[:,t]
``````
1. translation of LQR formulation (or really any of the Matrix contraints) where I need to use element wise equality in Julia (I don’t think this would be wrong but maybe?)

python:

``````constraints += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C]
``````

Julia

``````@constraint(model, x[:, t + 1] .== A*x[:,t] + B * u[:,t] + C)
``````
1. contraint and objective vs NLcontraint and NLobjective. the quadradic terms are not linear but the jump documentation states that the solver can tell and @contraint is preferred for quadradic constraints.

I would appreciate any help on this issue. Full code posted below. reference python code from PythonRobotics Linked above.

``````using LinearAlgebra
using JuMP
import DataFrames
import GLPK
using SparseArrays
using Plots
using Gurobi

NX = 4  # x = x, y, v, yaw
NU = 2  # a = [accel, steer]
T = 5  # horizon length

# mpc parameters
R = Diagonal([0.01, 0.01])  # input cost matrix
Rd = Diagonal([0.01, 1.0])  # input difference cost matrix
Q = Diagonal([1.0, 1.0, 0.5, 0.5])  # state cost matrix
Qf = Q  # state final matrix
GOAL_DIS = 1.5  # goal distance
STOP_SPEED = 0.5 / 3.6  # stop speed
MAX_TIME = 500.0  # max simulation time

# iterative paramter
MAX_ITER = 3  # Max iteration
DU_TH = 0.1  # iteration finish param

TARGET_SPEED = 10.0 / 3.6  # [m/s] target speed
N_IND_SEARCH = 10  # Search index number

DT = 0.2  # [s] time tick

# Vehicle parameters
LENGTH = 4.5  # [m]
WIDTH = 2.0  # [m]
BACKTOWHEEL = 1.0  # [m]
WHEEL_LEN = 0.3  # [m]
WHEEL_WIDTH = 0.2  # [m]
WB = 2.5  # [m]

MAX_SPEED = 55.0 / 3.6  # maximum speed [m/s]
MIN_SPEED = -20.0 / 3.6  # minimum speed [m/s]
MAX_ACCEL = 1.0  # maximum accel [m/ss]

struct State
x::Float64
y::Float64
yaw::Float64
v::Float64
predelta
end

function pi_2_pi(angle)
while(angle > pi)
angle = angle - 2.0*pi
end
while(angle < -pi)
angle = angle + 2.0*pi
end
return angle
end

function get_linear_model_matrix(v, phi, delta)
A = zeros(NX,NX)
A[1, 1] = 1.0
A[2, 2] = 1.0
A[3, 3] = 1.0
A[4, 4] = 1.0
A[1, 3] = cos(phi) * DT
A[1, 4] = -v * sin(phi) * DT
A[2, 3] = sin(phi) * DT
A[2, 4] = v * cos(phi) * DT
A[4, 3] = (tan(delta)/WB) * DT

B = zeros(NX, NU)
B[3, 1] = DT
B[4, 2] = (v/(WB*(cos(delta)^2))) * DT

C = zeros(NX)
C = v * sin(phi) * phi * DT
C = -v * cos(phi) * phi * DT
C = -(DT * v * delta)/(WB * (cos(delta)^2))

return A, B, C
end

function update_state(state, a , delta)
if delta >= MAX_STEER
delta = MAX_STEER
elseif delta <= -MAX_STEER
delta = -MAX_STEER
end

state.x = state.x + state.v * cos(state.yaw) * DT
state.y = state.y + state.v * sin(state.yaw) * DT
state.yaw = state.yaw + state.v / WB * tan(delta) * DT
state.v = state.v + a * DT

if state.v > MAX_SPEED
state.v = MAX_SPEED
elseif state.v < MIN_SPEED
state.v = MIN_SPEED
end

return state
end

function predict_motion(x0, oa, od, xref)
#why would you multiply this by 0?
xbar = xref * 0.0
for i in 1:length(x0)
xbar[i,1] = x0[i]
end

state = State(x0, x0, x0, x0, nothing)
for (ai, di, i) in zip(oa, od, 2:length(T+1))
state = update_state(state, ai, di)
xbar[1, i] = state.x0
xbar[2, i] = state.y
xbar[3, i] = state.v
xbar[4, i] = state.yaw
end

return xbar
end

function iterative_linear_mpc_control(xref,x0, dref, oa, od)
if isnothing(oa) || isnothing(od)
oa = zeros(T)
od = zeros(T)
end

for i in 1:MAX_ITER
xbar = predict_motion(x0, oa, od, xref)
poa = copy(oa)
pod = copy(od)
oa, od, ox, oy, oyaw, ov = linear_mpc_control(xref, xbar, x0, dref)
du = sum(abs(oa - poa)) + sum(abs(od - pod))
if du <= DU_TH
break
end
end

return oa, od, ox, oy, oyaw, ov
end

"""
linear_mpc_control(xref, xbar, x0, dref)

run the mimiziation problem given the constraints

xref: reference point
xbar: operational point
x0: initial state
dref: reference steer angle
"""
function linear_mpc_control(xref, xbar, x0, dref)
model = Model(Gurobi.Optimizer)
# generate variable matrix for states
@variable(model, x[1:NX, 1:(T+1)])
# generate variable matrix for controls
@variable(model, u[1:NU, 1:(T)])

cost = 0.0
for t in 1:T
# im pretty sure cvxpy.quad_form(u,r) is the same thing as u^T*r*u
#Pthon implimentation: cost += cvxpy.quad_form(u[:, t], R
cost += transpose(u[:,t])*R*u[:,t]

if t != 1
#python implimentation: cvxpy.quad_form(xref[:, t] - x[:, t], Q)
cost += transpose(xref[:, t] - x[:, t]) * Q * (xref[:, t] - x[:, t])
end

A,B,C = get_linear_model_matrix(xbar[3,t],xbar[4,t], dref[1,t])
@constraint(model, x[:, t + 1] .== A*x[:,t] + B * u[:,t] + C)

if t < (T - 1)
cost += transpose(u[:, t + 1] - u[:, t]) * Rd * (u[:, t + 1] - u[:, t])
# display(u[2, t + 1] - u[2, t])
#cant take the absolute value of a contraint
#I think we just add 2 contriants then
# @constraint(model, abs(u[2, t + 1] - u[2, t]) <= MAX_DSTEER * DT)
@constraint(model, u[2, t + 1] - u[2, t] <= MAX_DSTEER * DT)
@constraint(model, u[2, t + 1] - u[2, t] >= -MAX_DSTEER * DT)
end
end

cost += transpose(xref[:, T] - x[:, T]) * Qf * (xref[:, T] - x[:, T])

@constraint(model, c1, x[:,1] .== x0)
@constraint(model, c2, x[3,:] .<= MAX_SPEED)
@constraint(model, c3, x[3,:] .>= MIN_SPEED)
#cant do abs in this
# @constraint(model, c4, abs.(u[1,:]) <= MAX_ACCEL)
@constraint(model, c4, u[1,:] .<= MAX_ACCEL)
@constraint(model, c5, u[1,:] .>= -MAX_ACCEL)
# @constraint(model, c5, abs.(u[2,:]) >= MAX_STEER)
@constraint(model, c6, (u[2,:]) .>= MAX_STEER)
@constraint(model, c7, (u[2,:]) .<= -MAX_STEER)
JuMP.abs2

#set objective function
@objective(model, Min, cost)

print(model)
optimize!(model)
if termination_status(model) == MOI.OPTIMAL
#solution is optimal
solution_summary(model)
# display(value(x))
# display(value(u))

#get optimzal values from variables
ox = value(x[1,:])
oy = value(x[2,:])
ov = value(x[3,:])
oyaw = value(x[4,:])
oa = value(u[1,:])
odelta = value(i[2,:])
else
print("\n\ncannot solve\n\n\n")
ox = nothing
oy = nothing
ov = nothing
oyaw = nothing
oa = nothing
odelta = nothing
end

return oa, odelta, ox, oy, oyaw, ov
end

function calc_ref_trajectory(state, cx, cy, cyaw, sp, dl, pind)
xref = zeros(NX, T + 1)
dref = zeros(1, T + 1)
ncourse = length(cx) #cx is the course x position list

ind, _ = calc_nearest_index(state, cx, cy, cyaw, pind)

if pind >= ind
ind = pind
end

xref[1, 1] = cx[ind]
xref[2, 1] = cy[ind]
xref[3, 1] = sp[ind]
xref[4, 1] = cyaw[ind]
dref[1, 1] = 0.0  # steer operational point should be 0

travel = 0.0

for i in 1:T+1
travel += abs(state.v) * DT
dind = convert(Int32,round(travel/dl))

if (ind + dind) < ncourse
xref[1, i] = cx[ind + dind]
xref[2, i] = cy[ind + dind]
xref[3, i] = sp[ind + dind]
xref[4, i] = cyaw[ind + dind]
dref[1, i] = 0.0
else #the minus 1's here might mess it up
xref[1, i] = cx[ncourse - 1]
xref[2, i] = cy[ncourse - 1]
xref[3, i] = sp[ncourse - 1]
xref[4, i] = cyaw[ncourse - 1]
dref[1, i] = 0.0
end
end

return xref, ind, dref
end

function calc_nearest_index(state, cx, cy, cyaw, pind)
dx = [state.x - icx for icx in cx[pind:(pind + N_IND_SEARCH)]]
dy = [state.y - icy for icy in cy[pind:(pind + N_IND_SEARCH)]]

d = [idx^2 + idy^2 for (idx, idy) in zip(dx, dy)]

mind = minimum(d)

ind = indexin(mind,d) + (pind - 1)

mind = sqrt(mind)

dxl = cx[ind] - state.x
dyl = cy[ind] - state.yaw

angle = pi_2_pi(cyaw[ind] - atan(dyl, dxl))
if angle < 0
mind = mind * -1
end

return ind, mind
end

"""
do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state)

cx: course x position list
cy: course y position list
cy: course yaw position list
ck: course curvature list
sp: speed profile
dl: course tick [m]
"""
function do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state)
goal = [last(cx), last(cy)]

state = initial_state

if state.yaw - cyaw >= pi
state.yaw -= math.pi * 2.0
elseif state.yaw - cyaw <= -pi
state.yaw += math.pi * 2.0
end

time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
d = [0.0]
a = [0.0]
target_ind, _ = calc_nearest_index(state, cx, cy, cyaw, 0)

odelta = nothing
oa = nothing

cyaw = smooth_yaw(yaw)e

end

function smooth_yaw(yaw)
for i in 1:yaw-1
dyaw = yaw[i + 1] - yaw

while dyaw >= pi / 2.0
yaw[i + 1] -= pi * 2.0
dyaw = yaw[i + 1] - yaw[i]
end

while dyaw <= -pi / 2.0
yaw[i + 1] += pi * 2.0
dyaw = yaw[i + 1] - yaw[i]
end
end

return yaw
end

function calc_speed_profile(cx, cy, cyaw, target_speed)

speed_profile = zeros(length(cx))
speed_profile = speed_profile .+ target_speed
direction = 1.0

#set a stop point
for i in 1:length(cx)-1
dx = cx[i + 1] - cx[i]
dy = cy[i + 1] - cy[i]

move_direction = atan(dy,dx)

if dx != 0 && dy != 0
dangle = abs(pi_2_pi(move_direction - cyaw[i]))
if dangle >= pi/4
direction = -1.0
else
direction = 1.0
end
end
if direction != 1.0
speed_profile[i] = -target_speed
else
speed_profile[i] = target_speed
end
end
speed_profile[lastindex(speed_profile)] = 0.0

return speed_profile

end

function get_CLF_course()
# equation is x(t) = -1(1-0.64t)(1+ 0.64t)
# y(t) = −(1 − 0.64t)(1 − 0.2t − 0.25t2).
#we will just hardcode 50 slices in
T = range(0, pi/2, 50)
ax = []
ay = []
cyaw = []
for t in T
append!(ax,-(1-0.64*t)*(1 + 0.64*t) * 10)
append!(ay, -(1 - 0.64*t)*(1 - 0.2*t - 0.25*t^2) * 10)
append!(cyaw, pi/2 - t)
end
return ax, ay, cyaw

end

############
#start running
#######
print("starting MPC")

#generate the CLF course
cx, cy, cyaw = get_CLF_course()
dl = 1.0 #course tick

#generate the target speeds at each point
sp = calc_speed_profile(cx,cy,cyaw, TARGET_SPEED)

#set the intial state of the vehicle
initial_state = State(cx, cy, cyaw, 0.0, nothing)

#generate optimal acceleration(oa) and steering(od) arrays
oa = zeros(T)
od = zeros(T)

#the final argument of this is just the previous nearest index, so it doesnt have to search every point

target_ind, _ = calc_nearest_index(initial_state, cx, cy, cyaw, 1)
xref, target_ind, dref = calc_ref_trajectory(initial_state, cx, cy, cyaw, sp, dl, target_ind)

x0 = [initial_state.x, initial_state.y, initial_state.v, initial_state.yaw]

xbar = predict_motion(x0, oa, od, xref)

display(xref)
print("\n",xref)
print("\n",x0)
print("\n",dref)
print("\n",oa)
print("\n",od)

oa, od, ox, oy, oyaw, ov = iterative_linear_mpc_control(xref, x0, dref, oa, od)

display(display(plot(cx,cy)))
``````

edit: added optimality check and return variables in `linear_mpc_control()`

I couldn’t run your code so I don’t know the reason for the infeasibility. Shouldn’t the function `linear_mps_control` return something?

To get the most help out of posts on discourse, it helps to simplify the problem into a minimal reproducible example. Take a read of Please read: make it easier to help you.

You probably have something wrong with your data or there might be a typo somewhere. Try simplifying the problem until you’re confident that it works, then add complexity. For example, just try running `linear_mpc_control`. Do the outputs match what you expect?

I’d also write your JuMP model slightly differently. Here’s how I would write it:

``````function linear_mpc_control(xref, xbar, x0, dref)
model = Model(Gurobi.Optimizer)
@variable(model, x[1:NX, 1:(T+1)])
@variable(model, u[1:NU, 1:T])
for i in 1:NX
fix(x[i, 1], x0[i])
end
for t in 1:(T+1)
set_upper_bound(x[3, t], MAX_SPEED)
set_lower_bound(x[3, t], MIN_SPEED)
set_upper_bound(u[1, t], MAX_ACCEL)
set_lower_bound(u[1, t], -MAX_ACCEL)
set_upper_bound(u[2, t], MAX_STEER)
set_lower_bound(u[2, t], -MAX_STEER)
end
for t in 1:T
A, B, C = get_linear_model_matrix(xbar[3, t], xbar[4, t], dref[1, t])
@constraint(model, x[:, t + 1] .== A * x[:, t] + B * u[:, t] + C)
end
for t in 1:(T-2)
@constraint(model, u[2, t + 1] - u[2, t] <= MAX_DSTEER * DT)
@constraint(model, u[2, t + 1] - u[2, t] >= -MAX_DSTEER * DT)
end
@objective(
model,
Min,
sum(u[:, t]' * R * u[:, t] for t in 1:T) +
sum((xref[:, t] - x[:, t])' * Q * (xref[:, t] - x[:, t]) for t in 2:T) +
sum((u[:, t+1] - u[:, t])' * Rd * (u[:, t+1] - u[:, t]) for t in 1:(T-2)) +
(xref[:, T] - x[:, T])' * Qf * (xref[:, T] - x[:, T]),
)
optimize!(model)
solution_summary(model)
end
``````
1 Like

Thanks for the input, yea I forgot to return the optimal values of x and u from `linear_mpc_control` I will update the code above.

I will spend some time reformatting and looking at your recommendations and post a solution or follow up question. Thanks again!

Updating the constraints as you showed above (with a couple tweaks) helped me find my typo and it is working now. Here is the final `linear_mpc_control()` function. I will post the rest of the code for others looking for a simple MPC example when I finish the iteration code.

Thanks again for the quick response!

``````function linear_mpc_control(xref, xbar, x0, dref)
model = Model(Gurobi.Optimizer)
# generate variable matrix for states
@variable(model, x[1:NX, 1:(T+1)])
# generate variable matrix for controls
@variable(model, u[1:NU, 1:(T)])

#fix variable x[:,1] to intial state
for i in 1:NX
fix(x[i,1], x0[i])
end

#set upper and lower bound for each state
#cant set contraint for first x since it is fixed above
for t in 2:(T+1)
set_upper_bound(x[3, t], MAX_SPEED)
set_lower_bound(x[3, t], MIN_SPEED)
end
for t in 1:T
set_upper_bound(u[1, t], MAX_ACCEL)
set_lower_bound(u[1, t], -MAX_ACCEL)
set_upper_bound(u[2, t], MAX_STEER)
set_lower_bound(u[2, t], -MAX_STEER)
end

#get linear model matrix for each t and add the LQR contraint
for t in 1:T
A, B, C = get_linear_model_matrix(xbar[3, t], xbar[4, t], dref[1, t])
@constraint(model, x[:, t + 1] .== A * x[:, t] + B * u[:, t] + C)
end

#add constraints for maximum steering rate
#pretty sure this should be to T-1, not 2
for t in 1:(T-1)
@constraint(model, u[2, t + 1] - u[2, t] <= MAX_DSTEER * DT)
@constraint(model, u[2, t + 1] - u[2, t] >= -MAX_DSTEER * DT)
end
#generate objective (cost) function
@objective(
model, Min,
sum(u[:, t]' * R * u[:, t] for t in 1:T) +
sum((xref[:, t] - x[:, t])' * Q * (xref[:, t] - x[:, t]) for t in 2:T) +
sum((u[:, t+1] - u[:, t])' * Rd * (u[:, t+1] - u[:, t]) for t in 1:(T-2)) +
(xref[:, T] - x[:, T])' * Qf * (xref[:, T] - x[:, T]),
)

optimize!(model)
if termination_status(model) == MOI.OPTIMAL
#solution is optimal
solution_summary(model)
# display(value(x))
# display(value(u))

#get optimzal values from variables
ox = value.(x[1,:])
oy = value.(x[2,:])
ov = value.(x[3,:])
oyaw = value.(x[4,:])
oa = value.(u[1,:])
odelta = value.(u[2,:])
else
print("\n\ncannot solve\n\n\n")
ox = nothing
oy = nothing
ov = nothing
oyaw = nothing
oa = nothing
odelta = nothing
end

return oa, odelta, ox, oy, oyaw, ov
end
``````
1 Like