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)
