def test_lcp(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2, 'x') M = np.array([[1, 3], [4, 1]]) q = np.array([-16, -15]) binding = prog.AddLinearComplementarityConstraint(M, q, x) result = mp.Solve(prog) self.assertTrue(result.is_success()) self.assertIsInstance(binding.evaluator(), mp.LinearComplementarityConstraint)
def test_equality_between_polynomials(self): prog = mp.MathematicalProgram() x = prog.NewIndeterminates(1, "x") a = prog.NewContinuousVariables(2, "a") prog.AddEqualityConstraintBetweenPolynomials(sym.Polynomial( 2 * a[0] * x[0] + a[1] + 2, x), sym.Polynomial(2 * x[0] + 4, x)) result = mp.Solve(prog) a_val = result.GetSolution(a) self.assertAlmostEqual(a_val[0], 1) self.assertAlmostEqual(a_val[1], 2)
def test_AddPolyhedronConstraint(self): p_GP = np.array([[0.2, -0.4], [0.9, 0.2], [-0.1, 1]]) A = np.array([[0.5, 1., 0.1, 0.2, 0.5, 1.5]]) b = np.array([10.]) self.ik_two_bodies.AddPolyhedronConstraint( frameF=self.body1_frame, frameG=self.body2_frame, p_GP=p_GP, A=A, b=b) result = mp.Solve(self.prog) self.assertTrue(result.is_success())
def test_log_determinant(self): # Find the minimal ellipsoid that covers some given points. prog = mp.MathematicalProgram() X = prog.NewSymmetricContinuousVariables(2) pts = np.array([[1, 1], [1, -1], [-1, 1]]) for i in range(3): pt = pts[i, :] prog.AddLinearConstraint(pt.dot(X.dot(pt)) <= 1) prog.AddMaximizeLogDeterminantSymmetricMatrixCost(X) result = mp.Solve(prog) self.assertTrue(result.is_success())
def test_symbolic_qp(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2, "x") prog.AddConstraint(x[0], 1., 100.) prog.AddConstraint(x[1] >= 1) prog.AddQuadraticCost(x[0]**2 + x[1]**2) 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_AddGazeTargetConstraint(self): p_AS = np.array([0.1, 0.2, 0.3]) n_A = np.array([0.3, 0.5, 1.2]) p_BT = np.array([1.1, 0.2, 1.5]) cone_half_angle = 0.2 * math.pi self.ik_two_bodies.AddGazeTargetConstraint( frameA=self.body1_frame, p_AS=p_AS, n_A=n_A, frameB=self.body2_frame, p_BT=p_BT, cone_half_angle=cone_half_angle) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater( p_ST_W.dot(n_W), np.linalg.norm(p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6) with catch_drake_warnings(expected_count=2): self.assertEqual(self.prog.Solve(), mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater( p_ST_W.dot(n_W), np.linalg.norm(p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6)
def test_module_level_solve_function_and_result_accessors(self): qp = TestQP() x_expected = np.array([1, 1]) result = mp.Solve(qp.prog) self.assertTrue(result.is_success()) self.assertTrue(np.allclose(result.get_x_val(), x_expected)) self.assertEqual(result.get_solution_result(), mp.SolutionResult.kSolutionFound) self.assertEqual(result.get_optimal_cost(), 3.0) self.assertTrue(result.get_solver_id().name()) self.assertEqual(result.GetSolution(qp.x[0]), 1.0) self.assertTrue(np.allclose(result.GetSolution(qp.x), x_expected))
def test_maximize_geometric_mean(self): # Find the smallest axis-algined ellipsoid that covers some given # points. prog = mp.MathematicalProgram() a = prog.NewContinuousVariables(2) pts = np.array([[1, 1], [1, -1], [-1, 1]]) for i in range(3): pt = pts[i, :] prog.AddLinearConstraint(pt.dot(a * pt) <= 1) prog.AddMaximizeGeometricMeanCost(a, 1) result = mp.Solve(prog) self.assertTrue(result.is_success())
def decompose(zonotope, dimensions): """ @author: kasra Decompising a given set into bunch of fewer dimensional sets such that the Cartesian product of those sets is a subset of the given set. """ assert (sum(dimensions) == len( zonotope.G)), "ValueError: sum of the given dimensions has \ to be equal to the dimension of the input set" #number_of_sets = len(dimensions) prog = MP.MathematicalProgram() #defining varibales x_i = [prog.NewContinuousVariables(i) for i in dimensions] G_i = [prog.NewContinuousVariables(i, i) for i in dimensions] #non-symmetric G_i #G_i = [prog.NewSymmetricContinuousVariables(i) for i in dimensions ] #symmentric G_i #inbody_zonotope inbody_x = np.concatenate(x_i) inbody_G = block_diag(*G_i) inbody_zonotope = pp.zonotope(inbody_G, inbody_x) #Defining Constraints prog.AddPositiveSemidefiniteConstraint(inbody_G) pp.subset(prog, inbody_zonotope, zonotope) # ASSUMPTION for PSD of inbody_G # ASSUMPTION for SYMMENTRICITY of G_i #Defining the cost function prog.AddMaximizeLogDeterminantSymmetricMatrixCost(inbody_G) #Solving result = MP.Solve(prog) print(f"Is optimization successful? {result.is_success()}") print(f"Solution to x_i: {result.GetSolution(x_i[0])}") print(f"Solution to G_i: {result.GetSolution(G_i[0])}") print(f"optimal cost: {result.get_optimal_cost()}") print('solver is: ', result.get_solver_id().name()) x_i_result = [result.GetSolution(x_i[i]) for i in range(len(dimensions))] G_i_result = [result.GetSolution(G_i[i]) for i in range(len(dimensions))] list_zon = [ pp.zonotope(G=G_i_result[i], x=x_i_result[i]) for i in range(len(dimensions)) ] return list_zon
def test_eval_binding(self): qp = TestQP() prog = qp.prog x = qp.x x_expected = np.array([1., 1.]) costs = qp.costs cost_values_expected = [2., 1.] constraints = qp.constraints constraint_values_expected = [1., 1., 2., 3.] result = mp.Solve(prog) self.assertTrue(np.allclose(result.GetSolution(x), x_expected)) enum = zip(constraints, constraint_values_expected) for (constraint, value_expected) in enum: value = result.EvalBinding(constraint) self.assertTrue(np.allclose(value, value_expected)) value = prog.EvalBinding(constraint, x_expected) self.assertTrue(np.allclose(value, value_expected)) value = prog.EvalBindingVectorized( constraint, np.vstack((x_expected, x_expected)).T) a = np.vstack((value_expected, value_expected)).T self.assertTrue( np.allclose(value, np.vstack((value_expected, value_expected)).T)) enum = zip(costs, cost_values_expected) for (cost, value_expected) in enum: value = result.EvalBinding(cost) self.assertTrue(np.allclose(value, value_expected)) value = prog.EvalBinding(cost, x_expected) self.assertTrue(np.allclose(value, value_expected)) value = prog.EvalBindingVectorized( cost, np.vstack((x_expected, x_expected)).T) self.assertTrue( np.allclose(value, np.vstack((value_expected, value_expected)).T)) self.assertIsInstance(result.EvalBinding(costs[0]), np.ndarray) # Bindings for `Eval`. x_list = (float(1.), AutoDiffXd(1.), sym.Variable("x")) T_y_list = (float, AutoDiffXd, sym.Expression) evaluator = costs[0].evaluator() for x_i, T_y_i in zip(x_list, T_y_list): y_i = evaluator.Eval(x=[x_i, x_i]) self.assertIsInstance(y_i[0], T_y_i)
def test_infeasible_constraints(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(1) result = mp.Solve(prog) 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_pycost_and_pyconstraint(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(1, 'x') def cost(x): return (x[0]-1.)*(x[0]-1.) def constraint(x): return x prog.AddCost(cost, vars=x) prog.AddConstraint(constraint, lb=[0.], ub=[2.], vars=x) result = mp.Solve(prog) self.assertAlmostEqual(result.GetSolution(x)[0], 1.)
def test_max_geometric_mean_trivial(self): # Solve the trivial problem. # max (2x+3)*(3x+2) # s.t 2x+3 >= 0 # 3x+2 >= 0 # x <= 10 prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(1) prog.AddLinearConstraint(x[0] <= 10) A = np.array([2, 3]) b = np.array([3, 2]) prog.AddMaximizeGeometricMeanCost(A, b, x) result = mp.Solve(prog) self.assertTrue(result.is_success())
def test_matrix_variables(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2, 2, "x") for i in range(2): for j in range(2): prog.AddLinearConstraint(x[i, j] == 2 * i + j) result = mp.Solve(prog) xval = result.GetSolution(x) for i in range(2): for j in range(2): self.assertAlmostEqual(xval[i, j], 2 * i + j) self.assertEqual(xval[i, j], result.GetSolution(x[i, j])) # Just check spelling. y = prog.NewIndeterminates(2, 2, "y")
def test_sdp(self): prog = mp.MathematicalProgram() S = prog.NewSymmetricContinuousVariables(3, "S") prog.AddLinearConstraint(S[0, 1] >= 1) prog.AddPositiveSemidefiniteConstraint(S) prog.AddPositiveSemidefiniteConstraint(S + S) prog.AddLinearCost(np.trace(S)) result = mp.Solve(prog) self.assertTrue(result.is_success()) S = result.GetSolution(S) eigs = np.linalg.eigvals(S) tol = 1e-8 self.assertTrue(np.all(eigs >= -tol)) self.assertTrue(S[0, 1] >= -tol)
def test_AddOrientationConstraint(self): theta_bound = 0.2 * math.pi R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5)) R_BbarB = RotationMatrix( quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3)) self.ik_two_bodies.AddOrientationConstraint( frameAbar=self.body1_frame, R_AbarA=R_AbarA, frameBbar=self.body2_frame, R_BbarB=R_BbarB, theta_bound=theta_bound) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat) R_AB = R_AbarA.matrix().transpose().dot( R_AbarBbar.dot(R_BbarB.matrix())) self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat) R_AB = R_AbarA.matrix().transpose().dot( R_AbarBbar.dot(R_BbarB.matrix())) self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
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_qp(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2, "x") # N.B. Scalar-wise logical ops work for Expression, but array ops need # the workaround overloads from `pydrake.math`. prog.AddLinearConstraint(ge(x, 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 check_non_empty(Q, tol=10**-5, solver="gurobi"): Q = pp.to_AH_polytope(Q) prog = MP.MathematicalProgram() zeta = prog.NewContinuousVariables(Q.P.H.shape[1], 1, "zeta") prog.AddLinearConstraint(A=Q.P.H, ub=Q.P.h + tol, lb=-np.inf * np.ones((Q.P.h.shape[0], 1)), vars=zeta) if solver == "gurobi": result = gurobi_solver.Solve(prog, None, None) elif solver == "osqp": prog.AddQuadraticCost(np.eye(zeta.shape[0]), np.zeros(zeta.shape), zeta) result = OSQP_solver.Solve(prog, None, None) else: result = MP.Solve(prog) return result.is_success()
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(indeterminates=sym.Variables(x), degree=2) y = prog.NewIndeterminates(1, "y") (poly, binding) = prog.NewSosPolynomial(monomial_basis=(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 = mp.Solve(prog) self.assertTrue(result.is_success()) # Test SubstituteSolution(sym.Expression) with catch_drake_warnings(action='ignore'): prog.Solve() # TODO(eric.cousineau): Expose `SymbolicTestCase` so that other # tests can use the assertion utilities. self.assertEqual( prog.SubstituteSolution(d[0] + d[1]).Evaluate(), prog.GetSolution(d[0]) + prog.GetSolution(d[1])) # Test SubstituteSolution(sym.Polynomial) poly = d[0] * x.dot(x) poly_sub_actual = prog.SubstituteSolution( sym.Polynomial(poly, sym.Variables(x))) poly_sub_expected = sym.Polynomial( prog.SubstituteSolution(d[0]) * x.dot(x), sym.Variables(x)) # TODO(soonho): At present, these must be converted to `Expression` # to compare, because as `Polynomial`s the comparison fails with # `0*x(0)^2` != `0`, which indicates that simplification is not # happening somewhere. self.assertTrue( poly_sub_actual.ToExpression().EqualTo( poly_sub_expected.ToExpression()), "{} != {}".format(poly_sub_actual, poly_sub_expected))
def _do_test_direct_transcription(self, use_deprecated_solve): # Integrator. plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.], time_period=0.1) context = plant.CreateDefaultContext() dirtran = DirectTranscription(plant, context, num_time_samples=21) # Spell out most of the methods, regardless of whether they make sense # as a consistent optimization. The goal is to check the bindings, # not the implementation. t = dirtran.time() dt = dirtran.fixed_timestep() x = dirtran.state() x2 = dirtran.state(2) x0 = dirtran.initial_state() xf = dirtran.final_state() u = dirtran.input() u2 = dirtran.input(2) dirtran.AddRunningCost(x.dot(x)) dirtran.AddConstraintToAllKnotPoints(u[0] == 0) dirtran.AddFinalCost(2*x.dot(x)) initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21], np.zeros((1, 2))) initial_x = PiecewisePolynomial() dirtran.SetInitialTrajectory(initial_u, initial_x) if use_deprecated_solve: with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) dirtran.Solve() times = dirtran.GetSampleTimes() inputs = dirtran.GetInputSamples() states = dirtran.GetStateSamples() input_traj = dirtran.ReconstructInputTrajectory() state_traj = dirtran.ReconstructStateTrajectory() self.assertEqual(len(w), 6) else: result = mp.Solve(dirtran) times = dirtran.GetSampleTimes(result) inputs = dirtran.GetInputSamples(result) states = dirtran.GetStateSamples(result) input_traj = dirtran.ReconstructInputTrajectory(result) state_traj = dirtran.ReconstructStateTrajectory(result)
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_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_AddOrientationConstraint(self): theta_bound = 0.2 * math.pi R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5)) R_BbarB = RotationMatrix(quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3)) self.ik_two_bodies.AddOrientationConstraint(frameAbar=self.body1_frame, R_AbarA=R_AbarA, frameBbar=self.body2_frame, R_BbarB=R_BbarB, theta_bound=theta_bound) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat) R_AB = R_AbarA.matrix().transpose().dot( R_AbarBbar.dot(R_BbarB.matrix())) self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) self.assertEqual(self.prog.Solve(), mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat) R_AB = R_AbarA.matrix().transpose().dot( R_AbarBbar.dot(R_BbarB.matrix())) self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6) self.assertEqual(len(w), 2)
def test_pycost_and_pyconstraint(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(1, 'x') def cost(x): return (x[0]-1.)*(x[0]-1.) def constraint(x): return x cost_binding = prog.AddCost(cost, vars=x) constraint_binding = prog.AddConstraint( constraint, lb=[0.], ub=[2.], vars=x) result = mp.Solve(prog) xstar = result.GetSolution(x) self.assertAlmostEqual(xstar[0], 1.) # Verify that they can be evaluated. self.assertAlmostEqual(cost_binding.evaluator().Eval(xstar), 0.) self.assertAlmostEqual(constraint_binding.evaluator().Eval(xstar), 1.)
def point_membership(Q, x, tol=10**-5, solver="gurobi"): if type(Q).__name__ == "H_polytope": return Q.if_inside(x, tol) else: Q = pp.to_AH_polytope(Q) prog = MP.MathematicalProgram() zeta = prog.NewContinuousVariables(Q.P.H.shape[1], 1, "zeta") prog.AddLinearConstraint(A=Q.P.H, ub=Q.P.h + tol, lb=-np.inf * np.ones((Q.P.h.shape[0], 1)), vars=zeta) prog.AddLinearEqualityConstraint(Q.T, x - Q.t, zeta) if solver == "gurobi": result = gurobi_solver.Solve(prog, None, None) elif solver == "osqp": prog.AddQuadraticCost(np.eye(zeta.shape[0]), np.zeros(zeta.shape), zeta) result = OSQP_solver.Solve(prog, None, None) else: result = MP.Solve(prog) return result.is_success()
def test_module_level_solve_function_and_result_accessors(self): qp = TestQP() x_expected = np.array([1, 1]) result = mp.Solve(qp.prog) self.assertTrue(result.is_success()) self.assertTrue(np.allclose(result.get_x_val(), x_expected)) self.assertEqual(result.get_solution_result(), mp.SolutionResult.kSolutionFound) self.assertEqual(result.get_optimal_cost(), 3.0) self.assertTrue(result.get_solver_id().name()) self.assertTrue(np.allclose(result.GetSolution(), x_expected)) self.assertAlmostEqual(result.GetSolution(qp.x[0]), 1.0) self.assertTrue(np.allclose(result.GetSolution(qp.x), x_expected)) self.assertTrue(result.GetSolution(sym.Expression(qp.x[0])).EqualTo( result.GetSolution(qp.x[0]))) m = np.array([sym.Expression(qp.x[0]), sym.Expression(qp.x[1])]) self.assertTrue(result.GetSolution(m)[1, 0].EqualTo( result.GetSolution(qp.x[1]))) x_val_new = np.array([1, 2]) result.set_x_val(x_val_new) np.testing.assert_array_equal(x_val_new, result.get_x_val())
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 # The default initial guess is [0, 0, 0]. This initial guess is bad # because LorentzConeConstraint with eval_type=kConvex is not # differentiable at [0, 0, 0]. Use initial guess [0.5, 0.5, 0.5] # instead. result = mp.Solve(prog, [0.5, 0.5, 0.5]) 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_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. # d(0) + d(1) = 1 prog = mp.MathematicalProgram() x = prog.NewIndeterminates(1, "x") poly = prog.NewFreePolynomial(sym.Variables(x), 1) (poly, binding) = prog.NewSosPolynomial( indeterminates=sym.Variables(x), degree=2) y = prog.NewIndeterminates(1, "y") (poly, binding) = prog.NewSosPolynomial( monomial_basis=(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])]) prog.AddLinearEqualityConstraint(d[0] + d[1] == 1) result = mp.Solve(prog) self.assertTrue(result.is_success())
def test_AddMinimumDistanceConstraint(self): ik = self.ik_two_bodies W = self.plant.world_frame() B1 = self.body1_frame B2 = self.body2_frame min_distance = 0.1 tol = 1e-2 radius1 = 0.1 radius2 = 0.2 ik.AddMinimumDistanceConstraint(minimal_distance=min_distance) context = self.plant.CreateDefaultContext() R_I = np.eye(3) self.plant.SetFreeBodyPose(context, B1.body(), Isometry3(R_I, [0, 0, 0.01])) self.plant.SetFreeBodyPose(context, B2.body(), Isometry3(R_I, [0, 0, -0.01])) def get_min_distance_actual(): X = partial(self.plant.CalcRelativeTransform, context) distance = norm(X(W, B1).translation() - X(W, B2).translation()) return distance - radius1 - radius2 self.assertLess(get_min_distance_actual(), min_distance - tol) self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context)) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(ik.q()) self.plant.SetPositions(context, q_val) self.assertGreater(get_min_distance_actual(), min_distance - tol) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) self.assertEqual(self.prog.Solve(), mp.SolutionResult.kSolutionFound) self.assertTrue(np.allclose(self.prog.GetSolution(ik.q()), q_val)) self.assertEqual(len(w), 2)