def test_qp(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)
# Redundant cost just to check the spelling.
prog.AddQuadraticErrorCost(vars=x, Q=np.eye(2),
x_desired=np.zeros(2))
prog.AddL2NormCost(A=np.eye(2), b=np.zeros(2), vars=x)
result = mp.Solve(prog)
self.assertTrue(result.is_success())
x_expected = np.array([1, 1])
self.assertTrue(np.allclose(result.GetSolution(x), x_expected))
def test_initial_guess(self):
prog = mp.MathematicalProgram()
count = 6
shape = (2, 3)
x = prog.NewContinuousVariables(count, 'x')
x_matrix = x.reshape(shape)
x0 = np.arange(count)
x0_matrix = x0.reshape(shape)
all_nan = np.full(x.shape, np.nan)
self.assertTrue(np.isnan(prog.GetInitialGuess(x)).all())
def check_and_reset():
self.assertTrue((prog.GetInitialGuess(x) == x0).all())
self.assertTrue(
(prog.GetInitialGuess(x_matrix) == x0_matrix).all())
prog.SetInitialGuess(x, all_nan)
self.assertTrue(np.isnan(prog.GetInitialGuess(x)).all())
# Test setting individual variables
for i in range(count):
prog.SetInitialGuess(x[i], x0[i])
self.assertEqual(prog.GetInitialGuess(x[i]), x0[i])
check_and_reset()
# Test setting matrix values using both
# 1d and 2d np arrays.
prog.SetInitialGuess(x, x0)
check_and_reset()
prog.SetInitialGuess(x_matrix, x0_matrix)
check_and_reset()
# Test setting all values at once.
prog.SetInitialGuessForAllVariables(x0)
np.testing.assert_array_equal(prog.initial_guess(), x0)
check_and_reset()
# Check an extrinsic guess. We sanity check changes to the guess using
# loose "any" and "all" predicates rather than specific indices because
# we should not presume how variables map into indices.
guess = np.ndarray(count)
guess.fill(np.nan)
self.assertTrue(all([np.isnan(i) for i in guess]))
prog.SetDecisionVariableValueInVector(x[0], x0[0], guess)
self.assertFalse(all([np.isnan(i) for i in guess]))
self.assertTrue(any([np.isnan(i) for i in guess]))
prog.SetDecisionVariableValueInVector(x_matrix, x0_matrix, guess)
self.assertFalse(any([np.isnan(i) for i in guess]))
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))
self.assertGreater(result.get_solver_details().optimizer_time, 0.)
self.assertEqual(result.get_solver_details().error_code, 0)
self.assertEqual(result.get_solver_details().optimization_status, 2)
self.assertTrue(np.isnan(result.get_solver_details().objective_bound))
def test_inequality_elimination(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
prog.AddLinearConstraint(x[0] >= 0)
prog.AddLinearConstraint(x[1] >= 0)
prog.AddLinearConstraint(x[0] + x[1] <= 1)
prog.AddLinearConstraint(x[0] + x[1] <= 2)
qp = mqp.SimpleQuadraticProgram.from_mathematicalprogram(prog)
reduced = qp.eliminate_redundant_inequalities()
# Sort the rows of A. Kind of gross.
order = sorted(range(reduced.A.shape[0]), key=lambda i: tuple(reduced.A[i, :]))
self.assertTrue(np.allclose(reduced.A[order, :],
np.array([[-1, 0],
[0, -1],
[1 / np.sqrt(2), 1 / np.sqrt(2)]])))
self.assertTrue(np.allclose(reduced.b[order], np.array([0, 0, 1 / np.sqrt(2)])))
def test_dummy_solver_interface(self):
solver = DummySolverInterface()
self.assertTrue(solver.available())
self.assertTrue(solver.enabled())
self.assertEqual(solver.solver_id().name(), "dummy")
self.assertIsInstance(solver, SolverInterface)
prog = mp.MathematicalProgram()
with self.assertRaisesRegex(Exception, "Dummy.*cannot solve"):
unused_result = mp.MathematicalProgramResult()
solver.Solve(prog, None, None, unused_result)
with self.assertRaisesRegex(Exception, "Dummy.*cannot solve"):
_ = solver.Solve(prog)
self.assertFalse(solver.AreProgramAttributesSatisfied(prog))
solver.can_solve = True
self.assertTrue(solver.AreProgramAttributesSatisfied(prog))
result = solver.Solve(prog)
self.assertIsInstance(result, mp.MathematicalProgramResult)
def alpha(X, Y, Theta):
X2 = pp.to_AH_polytope(X)
Y2 = pp.to_AH_polytope(Y)
prog = MP.MathematicalProgram()
alpha = prog.NewContinuousVariables(1, "alpha")
t = prog.NewContinuousVariables(X2.n, 1, "t")
Y2.P.h = Y2.P.h * alpha
X2.t = X2.t + t
subset(prog, X2, Y2, Theta=Theta)
prog.AddLinearCost(np.eye(1), np.zeros((1)), alpha)
result = gurobi_solver.Solve(prog, None, None)
if result.is_success():
# print("alpha test successful")
# print(result.GetSolution(alpha),result.GetSolution(t))
return 1 / result.GetSolution(alpha)[0]
else:
print("not a subset")
def test_snopt_solver(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
prog.AddLinearConstraint(x[0] + x[1] == 1)
prog.AddBoundingBoxConstraint(0, 1, x[1])
prog.AddLinearCost(x[0])
solver = SnoptSolver()
if solver.available():
self.assertEqual(solver.solver_type(), mp.SolverType.kSnopt)
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
numpy_compare.assert_float_allclose(result.GetSolution(x),
[0., 1.],
atol=1E-7)
self.assertEqual(result.get_solver_details().info, 1)
def test_add_lorentz_cone_constraint(self):
# Call AddLorentzConeConstraint, make sure no error is thrown.
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(3)
prog.AddLorentzConeConstraint(
v=np.array([x[0]+1, x[1]+x[2], 2*x[1]]))
prog.AddLorentzConeConstraint(
linear_expression=x[0] + x[1] + 1,
quadratic_expression=x[0]*x[0] + x[1] * x[1] + 2 * x[0] * x[1] + 1,
tol=0.)
A = np.array([[1, 0], [0, 1], [1, 0], [0, 0]])
b = np.array([1, 1, 0, 2])
constraint = prog.AddLorentzConeConstraint(A=A, b=b, vars=x[:2])
np.testing.assert_allclose(
constraint.evaluator().A().todense(), A)
np.testing.assert_allclose(constraint.evaluator().b(), b)
def test_mosek_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 = MosekSolver()
# Mosek prints output to the terminal.
solver.set_stream_logging(True, "")
self.assertTrue(solver.available())
self.assertEqual(solver.solver_type(), mp.SolverType.kMosek)
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))
self.assertEqual(result.get_solver_details().solution_status, 1)
self.assertEqual(result.get_solver_details().rescode, 0)
self.assertGreater(result.get_solver_details().optimizer_time, 0.)
def test_mixed_integer_optimization(self):
prog = mp.MathematicalProgram()
x = prog.NewBinaryVariables(3, "x")
c = np.array([-1.0, -1.0, -2.0])
prog.AddLinearCost(c.dot(x))
a = np.array([1.0, 2.0, 3.0])
prog.AddLinearConstraint(a.dot(x) <= 4)
prog.AddLinearConstraint(x[0] + x[1], 1, np.inf)
result = prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
# Test that we got the right solution for all x
x_expected = np.array([1.0, 0.0, 1.0])
self.assertTrue(np.all(np.isclose(prog.GetSolution(x), x_expected)))
# Also test by asking for the value of each element of x
for i in range(3):
self.assertAlmostEqual(prog.GetSolution(x[i]), x_expected[i])
def test_infeasible_constraints(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1)
result = mp.Solve(prog)
with catch_drake_warnings(expected_count=1):
infeasible = mp.GetInfeasibleConstraints(prog=prog, result=result,
tol=1e-4)
self.assertEqual(len(infeasible), 0)
infeasible = result.GetInfeasibleConstraints(prog)
self.assertEqual(len(infeasible), 0)
infeasible = result.GetInfeasibleConstraints(prog, tol=1e-4)
self.assertEqual(len(infeasible), 0)
infeasible_names = result.GetInfeasibleConstraintNames(
prog=prog, tol=1e-4)
self.assertEqual(len(infeasible_names), 0)
def test_nlopt_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)
x_expected = np.array([1., 1.])
solver = NloptSolver()
self.assertTrue(solver.available())
self.assertEqual(solver.solver_id().name(), "NLopt")
self.assertEqual(solver.SolverName(), "NLopt")
self.assertEqual(solver.solver_type(), mp.SolverType.kNlopt)
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
numpy_compare.assert_float_allclose(
result.GetSolution(x), x_expected, atol=1E-7)
self.assertEqual(result.get_solver_details().status, 4)
def test_scaled_double_sided_ineq(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
prog.AddLinearConstraint(x[0] + 2 * x[1] >= 2)
prog.AddLinearConstraint(-3 * x[0] - 6 * x[1] >= -6)
qp = mqp.SimpleQuadraticProgram.from_mathematicalprogram(prog)
self.assertEqual(qp.A.shape, (2, 2))
self.assertEqual(qp.C.shape, (0, 2))
qp = qp.convert_double_inequalities_to_equalities()
self.assertEqual(qp.A.shape, (0, 2))
self.assertEqual(qp.b.shape, (0,))
self.assertEqual(qp.C.shape, (1, 2))
self.assertEqual(qp.d.shape, (1,))
s = np.sign(qp.C[0, 0])
self.assertTrue(np.allclose(qp.C * s, np.array([[1.0 / 3.0, 2.0 / 3.0]])))
self.assertTrue(np.allclose(qp.d * s, np.array([2.0 / 3.0])))
def test_recenter(self):
np.random.seed(2)
for i in range(20):
x_goal = np.random.rand(3) * 20 - 10
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(3, "x")
prog.AddLinearConstraint(x[0] >= 1)
prog.AddLinearConstraint(x[0] <= 10)
prog.AddLinearConstraint(x[0] + 5 * x[1] <= 11)
prog.AddLinearConstraint(-x[1] + 5 * x[0] <= 5)
prog.AddLinearConstraint(x[2] == x[0] + -2 * x[1])
prog.AddQuadraticCost(np.sum(np.power(x - x_goal, 2)))
qp = mqp.SimpleQuadraticProgram.from_mathematicalprogram(prog)
recentered = qp.transform_goal_to_origin()
self.assertFalse(np.allclose(qp.f, 0))
self.assertTrue(np.allclose(recentered.f, 0))
self.assertTrue(np.allclose(recentered.solve(), qp.solve(), atol=1e-7))
def test_lorentz_cone_constraint(self):
# Set Up Mathematical Program
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
z = prog.NewContinuousVariables(1, "z")
prog.AddCost(z[0])
# Add LorentzConeConstraints
prog.AddLorentzConeConstraint(np.array([0*x[0]+1, x[0]-1, x[1]-1]))
prog.AddLorentzConeConstraint(np.array([z[0], x[0], x[1]]))
# Test result
result = mp.Solve(prog)
self.assertTrue(result.is_success())
# Check answer
x_expected = np.array([1-2**(-0.5), 1-2**(-0.5)])
self.assertTrue(np.allclose(result.GetSolution(x), x_expected))
def test_ibex_solver(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
prog.AddBoundingBoxConstraint(-1., 1., x[0])
prog.AddBoundingBoxConstraint(-1., 1., x[1])
prog.AddCost(x[0] - x[1])
solver = IbexSolver()
self.assertEqual(solver.solver_id(), IbexSolver.id())
self.assertTrue(solver.available())
self.assertEqual(solver.solver_id().name(), "IBEX")
self.assertEqual(solver.SolverName(), "IBEX")
self.assertEqual(solver.solver_type(), mp.SolverType.kIbex)
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
numpy_compare.assert_float_allclose(result.GetSolution(x), [-1.0, 1.0],
atol=1E-7)
def test(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2)
b = prog.NewBinaryVariables(2)
prog.AddLinearConstraint(x[0] + 2 * x[1] + b[0] == 2.)
prog.AddLinearConstraint(x[0] - 3.1 * b[1] >= 1)
prog.AddLinearConstraint(b[1] + 1.2 * x[1] - b[0] <= 5)
prog.AddQuadraticCost(x[0] * x[0])
dut = bnb.MixedIntegerBranchAndBound(prog, OsqpSolver().solver_id())
solution_result = dut.Solve()
self.assertEqual(solution_result, mp.SolutionResult.kSolutionFound)
self.assertAlmostEqual(dut.GetOptimalCost(), 1.)
self.assertAlmostEqual(dut.GetSubOptimalCost(0), 1.)
self.assertAlmostEqual(dut.GetSolution(x[0], 0), 1.)
self.assertAlmostEqual(dut.GetSolution(x[0], 1), 1.)
np.testing.assert_allclose(dut.GetSolution(x, 0), [1., 0.], atol=1e-12)
def test_linear_constraints(self):
# TODO(eric.cousineau): Add more general tests
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, 'x')
lb = [0., 0.]
ub = [1., 1.]
prog.AddBoundingBoxConstraint(lb, ub, x)
prog.AddBoundingBoxConstraint(0., 1., x[0])
prog.AddBoundingBoxConstraint(0., 1., x)
prog.AddLinearConstraint(A=np.eye(2), lb=np.zeros(2), ub=np.ones(2),
vars=x)
prog.AddLinearConstraint(e=x[0], lb=0, ub=1)
prog.AddLinearConstraint(v=x, lb=[0, 0], ub=[1, 1])
prog.AddLinearConstraint(f=(x[0] == 0))
prog.AddLinearEqualityConstraint(np.eye(2), np.zeros(2), x)
prog.AddLinearEqualityConstraint(x[0] == 1)
prog.AddLinearEqualityConstraint(x[0] + x[1], 1)
def test_scs_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 = ScsSolver()
self.assertTrue(solver.available())
self.assertEqual(solver.solver_id().name(), "SCS")
self.assertEqual(solver.SolverName(), "SCS")
self.assertEqual(solver.solver_type(), mp.SolverType.kScs)
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
numpy_compare.assert_float_allclose(
result.GetSolution(x), [1.0, 1.0], atol=1E-7)
numpy_compare.assert_float_allclose(
result.get_solver_details().primal_objective, 1.0, atol=1E-7)
def test_sos(self):
# Find a,b,c,d subject to
# a(0) + a(1)*x,
# b(0) + 2*b(1)*x + b(2)*x^2 is SOS,
# c(0)*x^2 + 2*c(1)*x*y + c(2)*y^2 is SOS,
# d(0)*x^2 is SOS.
# d(1)*x^2 is SOS.
prog = mp.MathematicalProgram()
x = prog.NewIndeterminates(1, "x")
poly = prog.NewFreePolynomial(sym.Variables(x), 1)
(poly, binding) = prog.NewSosPolynomial(sym.Variables(x), 2)
y = prog.NewIndeterminates(1, "y")
(poly, binding) = prog.NewSosPolynomial(
(sym.Monomial(x[0]), sym.Monomial(y[0])))
d = prog.NewContinuousVariables(2, "d")
prog.AddSosConstraint(d[0] * x.dot(x))
prog.AddSosConstraint(d[1] * x.dot(x), [sym.Monomial(x[0])])
result = prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
