# Solving MPC using JuMP and Gurobi

**URL:** <https://discourse.julialang.org/t/solving-mpc-using-jump-and-gurobi/75962>\
**Category:** Optimization (Mathematical)\
**Tags:** jump\
**Created:** [February 7, 2022, 6:23pm UTC](https://discourse.julialang.org/t/solving-mpc-using-jump-and-gurobi/75962 "2022-02-07T18:23:08Z")\
**Posts on this page:** 4\
**Page:** 1

<div class="post-metadata">

**Author:** ![alre5639](https://avatars.discourse-cdn.com/v4/letter/a/eada6e/32.png) [@alre5639](https://discourse.julialang.org/u/alre5639)\
**Post date:** [February 7, 2022, 6:23pm UTC](https://discourse.julialang.org/t/solving-mpc-using-jump-and-gurobi/75962/1 "2022-02-07T18:23:08Z")

</div>

I am attempting to translate the MPC implemented in python and cvxpy [here](https://pythonrobotics.readthedocs.io/en/latest/modules/path_tracking/path_tracking.html#model-predictive-speed-and-steering-control) 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)_B_A  
for example:

Python:

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

```

Julia:

```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:

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

```

Julia

```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.

```julia
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]
TREAD = 0.7 # [m]
WB = 2.5 # [m]

MAX_STEER = deg2rad(45.0) # maximum steering angle [rad]
MAX_DSTEER = deg2rad(30.0) # maximum steering speed [rad/s]
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[1] = v * sin(phi) * phi * DT
    C[2] = -v * cos(phi) * phi * DT
    C[4] = -(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[1], x0[2], x0[4], x0[3], 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])
    
    #add more contraints
    @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)[1] + (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[1] >= pi
        state.yaw -= math.pi * 2.0
    elseif state.yaw - cyaw[1] <= -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[1], cy[1], cyaw[1], 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()`

---

<div class="post-metadata">

**Author:** ![odow](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/odow/32/28685_2.png) [@odow](https://discourse.julialang.org/u/odow)\
**Post date:** [February 7, 2022, 7:28pm UTC](https://discourse.julialang.org/t/solving-mpc-using-jump-and-gurobi/75962/2 "2022-02-07T19:28:03Z")

</div>

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](https://discourse.julialang.org/t/please-read-make-it-easier-to-help-you/14757).

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:

```julia
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

```

---

<div class="post-metadata">

**Author:** ![alre5639](https://avatars.discourse-cdn.com/v4/letter/a/eada6e/32.png) [@alre5639](https://discourse.julialang.org/u/alre5639)\
**Post date:** [February 7, 2022, 8:38pm UTC](https://discourse.julialang.org/t/solving-mpc-using-jump-and-gurobi/75962/3 "2022-02-07T20:38:12Z")

</div>

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!

---

<div class="post-metadata">

**Author:** ![alre5639](https://avatars.discourse-cdn.com/v4/letter/a/eada6e/32.png) [@alre5639](https://discourse.julialang.org/u/alre5639)\
**Post date:** [February 7, 2022, 9:55pm UTC](https://discourse.julialang.org/t/solving-mpc-using-jump-and-gurobi/75962/4 "2022-02-07T21:55:06Z")

</div>

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!

```julia
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

```