コード例 #1
0
 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))
コード例 #2
0
ファイル: utils.py プロジェクト: xyyeh/pympc
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]
コード例 #3
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))