Solving MPC using JuMP and Gurobi

alre5639 · February 7, 2022, 6:23pm

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:

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]

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)

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]
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()

odow · February 7, 2022, 7:28pm

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

alre5639 · February 7, 2022, 8:38pm

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!

alre5639 · February 7, 2022, 9:55pm

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