def test_gurobi_solver(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
prog.AddLinearConstraint(x[0] >= 1)
prog.AddLinearConstraint(x[1] >= 1)
prog.AddQuadraticCost(np.eye(2), np.zeros(2), x)
solver = GurobiSolver()
self.assertTrue(solver.available())
self.assertEqual(solver.solver_type(), mp.SolverType.kGurobi)
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
x_expected = np.array([1, 1])
self.assertTrue(np.allclose(result.GetSolution(x), x_expected))
class HybridModelPredictiveController(object):
def __init__(self, S, N, Q, R, P, X_N):
# store inputs
self.S = S
self.N = N
self.Q = Q
self.R = R
self.P = P
self.X_N = X_N
# mpMIQP
self.build_mpmiqp()
def build_mpmiqp(self):
# express the constrained dynamics as a list of polytopes in the (x,u,x+)-space
P = graph_representation(self.S)
m = big_m(P)
# initialize program
self.prog = MathematicalProgram()
self.x = []
self.u = []
self.d = []
obj = 0.
self.binaries_lower_bound = []
# initial conditions (set arbitrarily to zero in the building phase)
self.x.append(self.prog.NewContinuousVariables(self.S.nx))
self.initial_condition = []
for k in range(self.S.nx):
self.initial_condition.append(self.prog.AddLinearConstraint(self.x[0][k] == 0.).evaluator())
# loop over time
for t in range(self.N):
# create input, mode and next state variables
self.u.append(self.prog.NewContinuousVariables(self.S.nu))
self.d.append(self.prog.NewBinaryVariables(self.S.nm))
self.x.append(self.prog.NewContinuousVariables(self.S.nx))
# enforce constrained dynamics (big-m methods)
xux = np.concatenate((self.x[t], self.u[t], self.x[t+1]))
for i in range(self.S.nm):
mi_sum = np.sum([m[i][j] * self.d[t][j] for j in range(self.S.nm) if j != i], axis=0)
for k in range(P[i].A.shape[0]):
self.prog.AddLinearConstraint(P[i].A[k].dot(xux) <= P[i].b[k] + mi_sum[k])
# SOS1 on the binaries
self.prog.AddLinearConstraint(sum(self.d[t]) == 1.)
# stage cost to the objective
obj += .5 * self.u[t].dot(self.R).dot(self.u[t])
obj += .5 * self.x[t].dot(self.Q).dot(self.x[t])
# terminal constraint
for k in range(self.X_N.A.shape[0]):
self.prog.AddLinearConstraint(self.X_N.A[k].dot(self.x[self.N]) <= self.X_N.b[k])
# terminal cost
obj += .5 * self.x[self.N].dot(self.P).dot(self.x[self.N])
self.objective = self.prog.AddQuadraticCost(obj)
# set solver
self.solver = GurobiSolver()
self.prog.SetSolverOption(self.solver.solver_type(), 'OutputFlag', 1)
def set_initial_condition(self, x0):
for k, c in enumerate(self.initial_condition):
c.UpdateLowerBound(x0[k:k+1])
c.UpdateUpperBound(x0[k:k+1])
def feedforward(self, x0):
# overwrite initial condition
self.set_initial_condition(x0)
# solve MIQP
result = self.solver.Solve(self.prog)
# check feasibility
if result != SolutionResult.kSolutionFound:
return None, None, None, None
# get cost
obj = self.prog.EvalBindingAtSolution(self.objective)[0]
# store argmin in list of vectors
u = [self.prog.GetSolution(ut) for ut in self.u]
x = [self.prog.GetSolution(xt) for xt in self.x]
d = [self.prog.GetSolution(dt) for dt in self.d]
# retrieve mode sequence and check integer feasibility
ms = [np.argmax(dt) for dt in d]
return u, x, ms, obj
def feedback(self, x0):
# get feedforward and extract first input
u_feedforward = self.feedforward(x0)[0]
if u_feedforward is None:
return None
return u_feedforward[0]
# Create M (TODO: calculate this value)
M = 100
# Constrain the points to the regions
for i in range(num_regions):
for j in range(A[i].shape[0]):
prog.AddLinearConstraint(A[i][j][0]*x[0]+A[i][j][1]*x[1]+A[i][j][2]*x[2] + M*z[i] <= b[i][j] + M)
# Add objective
prog.AddQuadraticCost((x[0]-x_goal[0])**2 + (x[1]-x_goal[1])**2 + (x[2]-x_goal[2])**2) # distance of x to the goal point
# Solve problem
solver = GurobiSolver()
assert(solver.available())
assert(solver.solver_type()==mp.SolverType.kGurobi)
result = solver.Solve(prog)
assert(result == mp.SolutionResult.kSolutionFound)
print("Goal: " + str(x_goal))
finalx = prog.GetSolution(x)
print("Final Solution: " + str(finalx))
# ********* GRAPH PROBLEM *********
# Create figure
fig = plt.figure(1, (20, 10))
ax = fig.add_subplot(111, projection='3d')
plt.title("Minimize distance of point within " + str(num_regions) + " " + str(dim) + "-D Polytopes to Goal Point")
# Plot regions
for j in range(num_regions):
print("Region " + str(j))
