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)