def test_solver_options(self):
prog = mp.MathematicalProgram()
prog.SetSolverOption(SolverType.kGurobi, "double_key", 1.0)
prog.SetSolverOption(GurobiSolver().solver_id(), "int_key", 2)
prog.SetSolverOption(SolverType.kGurobi, "string_key", "3")
options = prog.GetSolverOptions(SolverType.kGurobi)
self.assertDictEqual(options, {
"double_key": 1.0,
"int_key": 2,
"string_key": "3"
})
options = prog.GetSolverOptions(GurobiSolver().solver_id())
self.assertDictEqual(options, {
"double_key": 1.0,
"int_key": 2,
"string_key": "3"
})
# For now, just make sure the constructor exists. Once we bind more
# accessors, we can test them here.
options_object = SolverOptions()
solver_id = SolverId("dummy")
self.assertEqual(solver_id.name(), "dummy")
options_object.SetOption(solver_id, "double_key", 1.0)
options_object.SetOption(solver_id, "int_key", 2)
options_object.SetOption(solver_id, "string_key", "3")
options = options_object.GetOptions(solver_id)
self.assertDictEqual(options, {
"double_key": 1.0,
"int_key": 2,
"string_key": "3"
})
def test_gurobi_license(self):
# Nominal use case.
with GurobiSolver.AcquireLicense():
pass
# Inspect.
with GurobiSolver.AcquireLicense() as license:
self.assertTrue(license.is_valid())
self.assertFalse(license.is_valid())
def test_write_to_file(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2)
prog.AddLinearConstraint(x[0] + x[1] == 1)
prog.AddQuadraticCost(x[0] * x[0] + x[1] * x[1])
solver = GurobiSolver()
file_name = temp_directory() + "/gurobi.mps"
options = mp.SolverOptions()
options.SetOption(solver.id(), "GRBwrite", file_name)
result = solver.Solve(prog, None, options)
self.assertTrue(os.path.exists(file_name))
def linear_program_drake(f, A, b, C=None, d=None, x_bound=None, solver='gurobi', **kwargs):
"""
Solves the linear program
minimize f^T * x
s. t. A * x <= b
C * x = d
INPUTS:
f: gradient of the cost function (2D numpy array)
A: left hand side of the constraints (2D numpy array)
b: right hand side of the constraints (2D numpy array)
C: left hand side of the equalities (2D numpy array)
d: right hand side of the equalities (2D numpy array)
OUTPUTS:
x_min: argument which minimizes the cost (its elements are nan if unfeasible or unbounded)
cost_min: minimum of the cost function (nan if unfeasible or unbounded)
"""
# program dimensions
n_variables = f.shape[0]
n_constraints = A.shape[0]
# build program
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(n_variables, "x")
for i in range(0, n_constraints):
prog.AddLinearConstraint((A[i,:] + 1e-15).dot(x) <= b[i])
if C is not None and d is not None:
for i in range(C.shape[0]):
prog.AddLinearConstraint(C[i, :].dot(x) == d[i])
prog.AddLinearCost((f.flatten() + 1e-15).dot(x))
# options
if solver == 'gurobi':
for (key, value) in kwargs.items():
prog.SetSolverOption(mp.SolverType.kGurobi, key, value)
# set bounds to the solution
if x_bound is not None:
for i in range(0, n_variables):
prog.AddLinearConstraint(x[i] <= x_bound)
prog.AddLinearConstraint(x[i] >= -x_bound)
# solve
if solver == 'gurobi':
solver = GurobiSolver()
elif solver == 'mosek':
solver = MosekSolver()
result = solver.Solve(prog)
x_min = np.reshape(prog.GetSolution(x), (n_variables,1))
cost_min = f.T.dot(x_min)
return [x_min, cost_min]
def test_compute_iis(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2)
prog.AddBoundingBoxConstraint(1, np.inf, x)
prog.AddLinearConstraint(x[0] + x[1] == 1)
solver = GurobiSolver()
ilp_file_name = temp_directory() + "/gurobi.ilp"
options = mp.SolverOptions()
options.SetOption(solver.id(), "GRBwrite", ilp_file_name)
options.SetOption(solver.id(), "GRBcomputeIIS", 1)
result = solver.Solve(prog, None, options)
self.assertTrue(os.path.exists(ilp_file_name))
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 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))
def test_gurobi_socp_dual(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
constraint = prog.AddLorentzConeConstraint(
[2., 2 * x[0], 3 * x[1] + 1])
prog.AddLinearCost(x[1])
solver = GurobiSolver()
options = mp.SolverOptions()
options.SetOption(solver.solver_id(), "QCPDual", 1)
result = solver.Solve(prog, None, options)
np.testing.assert_allclose(result.GetDualSolution(constraint),
np.array([-1. / 12]),
atol=1e-7)
def test_api(self):
plant = MultibodyPlant(time_step=0.01)
model_instance = Parser(plant).AddModelFromFile(FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"))
plant.Finalize()
context = plant.CreateDefaultContext()
options = ik.GlobalInverseKinematics.Options()
global_ik = ik.GlobalInverseKinematics(plant=plant, options=options)
self.assertIsInstance(global_ik.prog(), mp.MathematicalProgram)
self.assertIsInstance(global_ik.get_mutable_prog(),
mp.MathematicalProgram)
body_index_A = plant.GetBodyIndices(model_instance)[0]
body_index_B = plant.GetBodyIndices(model_instance)[1]
self.assertEqual(
global_ik.body_rotation_matrix(body_index=body_index_A).shape,
(3, 3))
self.assertEqual(
global_ik.body_position(body_index=body_index_A).shape, (3, ))
global_ik.AddWorldPositionConstraint(
body_index=body_index_A,
p_BQ=[0, 0, 0],
box_lb_F=[-np.inf, -np.inf, -np.inf],
box_ub_F=[np.inf, np.inf, np.inf],
X_WF=RigidTransform())
global_ik.AddWorldRelativePositionConstraint(
body_index_B=body_index_B,
p_BQ=[0, 0, 0],
body_index_A=body_index_A,
p_AP=[0, 0, 0],
box_lb_F=[-np.inf, -np.inf, -np.inf],
box_ub_F=[np.inf, np.inf, np.inf],
X_WF=RigidTransform())
global_ik.AddWorldOrientationConstraint(
body_index=body_index_A,
desired_orientation=Quaternion(),
angle_tol=np.inf)
global_ik.AddPostureCost(
q_desired=plant.GetPositions(context),
body_position_cost=[1] * plant.num_bodies(),
body_orientation_cost=[1] * plant.num_bodies())
gurobi_solver = GurobiSolver()
if gurobi_solver.available():
global_ik.SetInitialGuess(q=plant.GetPositions(context))
result = gurobi_solver.Solve(global_ik.prog())
self.assertTrue(result.is_success())
global_ik.ReconstructGeneralizedPositionSolution(result=result)
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_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)
solver = GurobiSolver()
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
# 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(result.GetSolution(x), x_expected)))
# Also test by asking for the value of each element of x
for i in range(3):
self.assertAlmostEqual(result.GetSolution(x[i]), x_expected[i])
def solve(self):
solver = GurobiSolver()
self.prog.SetSolverOption(mp.SolverType.kGurobi, "LogToConsole", 1)
self.prog.SetSolverOption(mp.SolverType.kGurobi, "OutputFlag", 1)
start_time = time.time()
result = solver.Solve(self.prog)
solve_time = time.time() - start_time
assert result == mp.SolutionResult.kSolutionFound
states, inputs, contact = self.prog.extract_solution(
self.robot, self.vars.qcom, self.vars.qlimb, self.vars.contact,
self.vars.contact_force, self.vars.contact_lambda,
self.vars.contact_sequence_array)
ts = states.components[0].breaks
return SolutionData(opt=self,
states=states,
inputs=inputs,
contact_indicator=contact,
ts=ts,
solve_time=solve_time)
def quadratic_program(H, f, A, b, C=None, d=None):
"""
Solves the convex (i.e., H > 0) quadratic program
minimize x^T * H * x + f^T * x
s. t. A * x <= b
C * x = d
INPUTS:
H: Hessian of the cost function (2D numpy array)
f: linear term of the cost function (2D numpy array)
A: left hand side of the inequalities (2D numpy array)
b: right hand side of the inequalities (2D numpy array)
C: left hand side of the equalities (2D numpy array)
d: right hand side of the equalities (2D numpy array)
OUTPUTS:
x_min: argument which minimizes the cost (its elements are nan if unfeasible or unbounded)
cost_min: minimum of the cost function (nan if unfeasible or unbounded)
"""
# program dimensions
n_variables = f.shape[0]
n_constraints = A.shape[0]
# build program
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(n_variables, "x")
for i in range(0, n_constraints):
prog.AddLinearConstraint((A[i,:] + 1e-15).dot(x) <= b[i])
if C is not None and d is not None:
for i in range(C.shape[0]):
prog.AddLinearConstraint(C[i, :].dot(x) == d[i])
prog.AddQuadraticCost(H, f, x)
# solve
solver = GurobiSolver()
result = solver.Solve(prog)
x_min = np.reshape(prog.GetSolution(x), (n_variables,1))
cost_min = .5*x_min.T.dot(H.dot(x_min)) + f.T.dot(x_min)
return [x_min, cost_min]
def test_callback(self):
prog = mp.MathematicalProgram()
b = prog.NewBinaryVariables(4)
prog.AddLinearConstraint(b[0] <= 1 - 0.5 * b[1])
prog.AddLinearConstraint(b[1] <= 1 - 0.5 * b[0])
prog.AddLinearCost(-b[0] - b[1])
prog.SetSolverOption(GurobiSolver.id(), "Presolve", 0)
prog.SetSolverOption(GurobiSolver.id(), "Heuristics", 0.)
prog.SetSolverOption(GurobiSolver.id(), "Cuts", 0)
prog.SetSolverOption(GurobiSolver.id(), "NodeMethod", 2)
b_init = np.array([0, 0., 0., 0.])
prog.SetInitialGuess(b, b_init)
solver = GurobiSolver()
explored_node_count = 0
def node_callback(prog, solver_status_info, x, x_vals):
nonlocal explored_node_count
explored_node_count = solver_status_info.explored_node_count
solver.AddMipNodeCallback(
callback=lambda prog, solver_status_info, x, x_vals: node_callback(
prog, solver_status_info, x, x_vals))
best_objectives = []
def sol_callback(prog, callback_info, objectives):
print(f"explored nodes {callback_info.explored_node_count}")
objectives.append(callback_info.best_objective)
solver.AddMipSolCallback(
callback=lambda prog, callback_info: sol_callback(
prog, callback_info, best_objectives))
result = solver.Solve(prog)
self.assertTrue(result.is_success())
self.assertGreater(explored_node_count, 0)
self.assertGreater(len(best_objectives), 0)
class TestMathematicalProgram(unittest.TestCase):
def test_program_construction(self):
prog = mp.MathematicalProgram()
vars = prog.NewContinuousVariables(5, "x")
self.assertEqual(vars.dtype, sym.Variable)
vars_all = prog.decision_variables()
self.assertEqual(vars_all.shape, (5, ))
def test_program_attributes_and_solver_selection(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
# Add linear equality constraints; make sure the solver works.
prog.AddLinearConstraint(x[0] + x[1] == 0)
prog.AddLinearConstraint(2 * x[0] - x[1] == 1)
solver_id = mp.ChooseBestSolver(prog)
self.assertEqual(solver_id.name(), "Linear system")
solver = mp.MakeSolver(solver_id)
self.assertEqual(solver.solver_id().name(), "Linear system")
self.assertTrue(solver.AreProgramAttributesSatisfied(prog))
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
# With an inequality constraint added, the "Linear system" solver
# doesn't work anymore.
prog.AddLinearConstraint(x[0] >= 0)
self.assertFalse(solver.AreProgramAttributesSatisfied(prog))
with self.assertRaises(ValueError):
solver.Solve(prog, None, None)
# A different solver will work, though. We re-use the result object
# (as a mutable output argument), and make sure that it changes.
solver_id = mp.ChooseBestSolver(prog)
self.assertNotEqual(solver_id.name(), "Linear system")
solver = mp.MakeSolver(solver_id)
solver.Solve(prog, None, None, result)
self.assertTrue(result.is_success())
self.assertEqual(result.get_solver_id().name(), solver_id.name())
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())
# TODO(jwnimmer-tri) MOSEK is also able to solve mixed integer programs;
# perhaps we should test both of them?
@unittest.skipUnless(GurobiSolver().available(), "Requires Gurobi")
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)
prog.AddConstraint(
LinearConstraint(np.array([[1., 1.]]), np.array([1]),
np.array([np.inf])), [x[0], x[1]])
solver = GurobiSolver()
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
# 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(result.GetSolution(x), x_expected)))
# Also test by asking for the value of each element of x
for i in range(3):
self.assertAlmostEqual(result.GetSolution(x[i]), x_expected[i])
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)
prog.AddQuadraticCost(np.eye(2), np.zeros(2), 1, 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_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_bindings(self):
qp = TestQP()
self.assertEqual(str(qp.constraints[0]),
"BoundingBoxConstraint\n1 <= x(0) <= inf\n")
prog = qp.prog
x = qp.x
self.assertEqual(prog.FindDecisionVariableIndices(vars=[x[0], x[1]]),
[0, 1])
self.assertEqual(prog.decision_variable_index()[x[0].get_id()], 0)
self.assertEqual(prog.decision_variable_index()[x[1].get_id()], 1)
for binding in prog.GetAllCosts():
self.assertIsInstance(binding.evaluator(), mp.Cost)
for binding in prog.GetLinearConstraints():
self.assertIsInstance(binding.evaluator(), mp.Constraint)
for binding in prog.GetAllConstraints():
self.assertIsInstance(binding.evaluator(), mp.Constraint)
self.assertTrue(prog.linear_costs())
for (i, binding) in enumerate(prog.linear_costs()):
cost = binding.evaluator()
self.assertTrue(np.allclose(cost.a(), np.ones((1, 2))))
self.assertIsNone(cost.gradient_sparsity_pattern())
self.assertTrue(prog.quadratic_costs())
for (i, binding) in enumerate(prog.quadratic_costs()):
cost = binding.evaluator()
self.assertTrue(np.allclose(cost.Q(), np.eye(2)))
self.assertTrue(np.allclose(cost.b(), np.zeros(2)))
self.assertIsNone(cost.gradient_sparsity_pattern())
self.assertTrue(prog.bounding_box_constraints())
for (i, binding) in enumerate(prog.bounding_box_constraints()):
constraint = binding.evaluator()
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[0]),
prog.FindDecisionVariableIndex(var=x[i]))
self.assertIsNone(constraint.gradient_sparsity_pattern())
num_constraints = constraint.num_constraints()
if num_constraints == 1:
self.assertEqual(constraint.A(), 1)
self.assertEqual(constraint.lower_bound(), 1)
self.assertEqual(constraint.upper_bound(), np.inf)
else:
self.assertTrue(np.allclose(constraint.A(), np.eye(2)))
self.assertTrue(
np.allclose(constraint.lower_bound(), [1, -np.inf]))
self.assertTrue(
np.allclose(constraint.upper_bound(), [np.inf, 2]))
self.assertTrue(prog.linear_constraints())
for (i, binding) in enumerate(prog.linear_constraints()):
constraint = binding.evaluator()
self.assertIsNone(constraint.gradient_sparsity_pattern())
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[0]),
prog.FindDecisionVariableIndex(var=x[0]))
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[1]),
prog.FindDecisionVariableIndex(var=x[1]))
self.assertTrue(np.allclose(constraint.A(), [3, -1]))
self.assertTrue(constraint.lower_bound(), -2)
self.assertTrue(constraint.upper_bound(), np.inf)
self.assertTrue(prog.linear_equality_constraints())
for (i, binding) in enumerate(prog.linear_equality_constraints()):
self.assertIsNone(constraint.gradient_sparsity_pattern())
constraint = binding.evaluator()
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[0]),
prog.FindDecisionVariableIndex(var=x[0]))
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[1]),
prog.FindDecisionVariableIndex(var=x[1]))
self.assertTrue(np.allclose(constraint.A(), [1, 2]))
self.assertTrue(constraint.lower_bound(), 3)
self.assertTrue(constraint.upper_bound(), 3)
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_constraint_api(self):
prog = mp.MathematicalProgram()
x0, = prog.NewContinuousVariables(1, "x")
c = prog.AddLinearConstraint(x0 >= 2).evaluator()
ce = prog.AddLinearEqualityConstraint(2 * x0, 1).evaluator()
self.assertTrue(c.CheckSatisfied([2.], tol=1e-3))
satisfied = c.CheckSatisfiedVectorized(np.array([1., 2., 3.]).reshape(
(1, 3)),
tol=1e-3)
self.assertEqual(satisfied, [False, True, True])
self.assertFalse(c.CheckSatisfied([AutoDiffXd(1.)]))
self.assertIsInstance(c.CheckSatisfied([x0]), sym.Formula)
ce.set_description("my favorite constraint")
self.assertEqual(ce.get_description(), "my favorite constraint")
def check_bounds(c, A, lb, ub):
self.assertTrue(np.allclose(c.A(), A))
self.assertTrue(np.allclose(c.lower_bound(), lb))
self.assertTrue(np.allclose(c.upper_bound(), ub))
check_bounds(c, [1.], [2.], [np.inf])
c.UpdateLowerBound([3.])
check_bounds(c, [1.], [3.], [np.inf])
c.UpdateUpperBound([4.])
check_bounds(c, [1.], [3.], [4.])
c.set_bounds([-10.], [10.])
check_bounds(c, [1.], [-10.], [10.])
c.UpdateCoefficients([10.], [-20.], [-30.])
check_bounds(c, [10.], [-20.], [-30.])
check_bounds(ce, [2.], [1.], [1.])
ce.UpdateCoefficients([10.], [20.])
check_bounds(ce, [10.], [20.], [20.])
def test_cost_api(self):
prog = mp.MathematicalProgram()
x0, = prog.NewContinuousVariables(1, "x")
lc = prog.AddLinearCost([1], 2, [x0]).evaluator()
qc = prog.AddQuadraticCost(0.5 * x0**2 + 2 * x0 + 3).evaluator()
def check_linear_cost(cost, a, b):
self.assertTrue(np.allclose(cost.a(), a))
self.assertTrue(np.allclose(cost.b(), b))
check_linear_cost(lc, [1.], 2.)
lc.UpdateCoefficients([10.])
check_linear_cost(lc, [10.], 0.)
def check_quadratic_cost(cost, Q, b, c):
self.assertTrue(np.allclose(cost.Q(), Q))
self.assertTrue(np.allclose(cost.b(), b))
self.assertTrue(np.allclose(cost.c(), c))
check_quadratic_cost(qc, [1.], [2.], 3.)
qc.UpdateCoefficients([10.], [20.])
check_quadratic_cost(qc, [10.], [20.], 0)
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_get_binding_variable_values(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(3)
binding1 = prog.AddBoundingBoxConstraint(-1, 1, x[0])
binding2 = prog.AddLinearEqualityConstraint(x[1] + 2 * x[2], 2)
x_val = np.array([-2., 1., 2.])
np.testing.assert_allclose(
prog.GetBindingVariableValues(binding1, x_val), np.array([-2]))
np.testing.assert_allclose(
prog.GetBindingVariableValues(binding2, x_val), np.array([1, 2]))
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_nonnegative_polynomial(self):
# Only check if the API works.
prog = mp.MathematicalProgram()
x = prog.NewIndeterminates(3, "x")
(poly1, grammian1) = prog.NewNonnegativePolynomial(
indeterminates=sym.Variables(x),
degree=4,
type=mp.MathematicalProgram.NonnegativePolynomial.kSdsos)
self.assertIsInstance(poly1, sym.Polynomial)
self.assertIsInstance(grammian1, np.ndarray)
grammian2 = prog.NewSymmetricContinuousVariables(2)
poly2 = prog.NewNonnegativePolynomial(
grammian=grammian2,
monomial_basis=(sym.Monomial(x[0]), sym.Monomial(x[1])),
type=mp.MathematicalProgram.NonnegativePolynomial.kDsos)
self.assertIsInstance(grammian2, np.ndarray)
poly3, grammian3 = prog.NewNonnegativePolynomial(
monomial_basis=(sym.Monomial(x[0]), sym.Monomial(x[1])),
type=mp.MathematicalProgram.NonnegativePolynomial.kSos)
self.assertIsInstance(poly3, sym.Polynomial)
self.assertIsInstance(grammian3, np.ndarray)
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")
self.assertEqual(prog.indeterminates_index()[x[0].get_id()], 0)
poly = prog.NewFreePolynomial(sym.Variables(x), 1)
(poly,
binding) = prog.NewSosPolynomial(indeterminates=sym.Variables(x),
degree=2)
even_poly = prog.NewEvenDegreeFreePolynomial(sym.Variables(x), 2)
odd_poly = prog.NewOddDegreeFreePolynomial(sym.Variables(x), 3)
y = prog.NewIndeterminates(1, "y")
self.assertEqual(prog.indeterminates_index()[y[0].get_id()], 1)
(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())
result.GetSolution(poly)
(poly, Q_oo, Q_ee) = prog.NewEvenDegreeSosPolynomial(
indeterminates=sym.Variables(x), degree=2)
(poly, Q_oo, Q_ee) = prog.NewEvenDegreeSdsosPolynomial(
indeterminates=sym.Variables(x), degree=2)
(poly, Q_oo, Q_ee) = prog.NewEvenDegreeDsosPolynomial(
indeterminates=sym.Variables(x), degree=2)
def test_make_polynomial(self):
prog = mp.MathematicalProgram()
x = prog.NewIndeterminates(1, "x")[0]
a = prog.NewContinuousVariables(1, "a")[0]
# e = (a + 1)x² + 2ax + 3a.
e = (a + 1) * (x * x) + (2 * a) * x + 3 * a
# We create a polynomial of `e` via MakePolynomial.
p = prog.MakePolynomial(e)
# Check its indeterminates and decision variables are correctly set,
self.assertEqual(p.indeterminates().size(), 1)
self.assertTrue(p.indeterminates().include(x))
self.assertEqual(p.decision_variables().size(), 1)
self.assertTrue(p.decision_variables().include(a))
# Check if it holds the same expression when converted back to
# symbolic expression.
self.assertTrue(p.ToExpression().EqualTo(e))
def test_reparse(self):
prog = mp.MathematicalProgram()
x = prog.NewIndeterminates(1, "x")[0]
a = prog.NewContinuousVariables(1, "a")[0]
e = (a + 1) * (x * x) + (2 * a) * x + 3 * a
# p = (x^2 + 2x + 3)a + x^2 with indeterminates {a}.
p = sym.Polynomial(e, [a])
self.assertEqual(p.TotalDegree(), 1)
# p = (a + 1)x² + 2ax + 3a with indeterminates {x}.
prog.Reparse(p)
self.assertEqual(p.TotalDegree(), 2)
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_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_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 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_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_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)
prog.AddLinearEqualityConstraint(2 * x[:2] + np.array([0, 1]),
np.array([3, 2]))
def test_constraint_set_bounds(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
def constraint(x):
return x[1]**2
binding = prog.AddConstraint(constraint, [0], [1], vars=x)
self.assertIsInstance(binding.evaluator(), PyFunctionConstraint)
np.testing.assert_array_equal(binding.evaluator().lower_bound(),
np.array([0.]))
np.testing.assert_array_equal(binding.evaluator().upper_bound(),
np.array([1.]))
# Test UpdateLowerBound()
binding.evaluator().UpdateLowerBound(new_lb=[-1.])
np.testing.assert_array_equal(binding.evaluator().lower_bound(),
np.array([-1.]))
np.testing.assert_array_equal(binding.evaluator().upper_bound(),
np.array([1.]))
# Test UpdateLowerBound()
binding.evaluator().UpdateUpperBound(new_ub=[2.])
np.testing.assert_array_equal(binding.evaluator().lower_bound(),
np.array([-1.]))
np.testing.assert_array_equal(binding.evaluator().upper_bound(),
np.array([2.]))
# Test set_bounds()
binding.evaluator().set_bounds(lower_bound=[-3.], upper_bound=[4.])
np.testing.assert_array_equal(binding.evaluator().lower_bound(),
np.array([-3.]))
np.testing.assert_array_equal(binding.evaluator().upper_bound(),
np.array([4.]))
def test_constraint_gradient_sparsity(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
def cost(x):
return x[0]**2
def constraint(x):
return x[1]**2
cost_binding = prog.AddCost(cost, vars=x)
constraint_binding = prog.AddConstraint(constraint, [0], [1], vars=x)
cost_evaluator = cost_binding.evaluator()
constraint_evaluator = constraint_binding.evaluator()
self.assertIsNone(cost_evaluator.gradient_sparsity_pattern())
self.assertIsNone(constraint_evaluator.gradient_sparsity_pattern())
# Now set the sparsity
cost_evaluator.SetGradientSparsityPattern([(0, 0)])
self.assertEqual(cost_evaluator.gradient_sparsity_pattern(), [(0, 0)])
constraint_binding.evaluator().SetGradientSparsityPattern([(0, 1)])
self.assertEqual(constraint_evaluator.gradient_sparsity_pattern(),
[(0, 1)])
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.)
self.assertEqual(len(prog.generic_constraints()), 1)
self.assertEqual(prog.generic_constraints()[0].evaluator(),
constraint_binding.evaluator())
self.assertEqual(len(prog.generic_costs()), 1)
self.assertEqual(prog.generic_costs()[0].evaluator(),
cost_binding.evaluator())
def get_different_scalar_type(self, T):
# Gets U such that U != T.
next_index = SCALAR_TYPES.index(T) + 1
U = SCALAR_TYPES[next_index % len(SCALAR_TYPES)]
self.assertNotEqual(U, T)
return U
def test_pycost_wrap_error(self):
"""Tests for checks using PyFunctionCost::Wrap."""
# TODO(eric.cousineau): It would be nice to not need a
# MathematicalProgram to test these.
def user_cost_bad_shape(x):
# WARNING: This should return a scalar, not a vector!
return x
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1, 'x')
binding_bad_shape = prog.AddCost(user_cost_bad_shape, vars=x)
for T in SCALAR_TYPES:
array_T = np.vectorize(T)
x0 = array_T([0.])
x0_bad = array_T([0., 1.])
# Bad input (before function is called).
if kDrakeAssertIsArmed:
# See note in `WrapUserFunc`.
input_error_cls = SystemExit
input_error_expected = (
"x.rows() == num_vars_ || num_vars_ == Eigen::Dynamic")
else:
input_error_cls = RuntimeError
input_error_expected = (
"PyFunctionCost: Input must be of .ndim = 1 or 2 (vector) "
"and .size = 1. Got .ndim = 1 and .size = 2 instead.")
with self.assertRaises(input_error_cls) as cm:
binding_bad_shape.evaluator().Eval(x0_bad)
self.assertIn(input_error_expected, str(cm.exception))
# Bad output shape.
with self.assertRaises(RuntimeError) as cm:
binding_bad_shape.evaluator().Eval(x0)
self.assertEqual(
str(cm.exception),
"PyFunctionCost: Output must be of .ndim = 0 (scalar) and "
".size = 1. Got .ndim = 1 and .size = 1 instead.")
# Bad output dtype.
U = self.get_different_scalar_type(T)
def user_cost_bad_dtype(x):
# WARNING: This should return the same dtype as x!
return U(0.)
binding_bad_dtype = prog.AddCost(user_cost_bad_dtype, vars=x)
with self.assertRaises(RuntimeError) as cm:
binding_bad_dtype.evaluator().Eval(x0)
self.assertEqual(
str(cm.exception),
f"PyFunctionCost: Output must be of scalar type {T.__name__}. "
f"Got {U.__name__} instead.")
def test_pyconstraint_wrap_error(self):
"""Tests for checks using PyFunctionConstraint::Wrap."""
# TODO(eric.cousineau): It would be nice to not need a
# MathematicalProgram to test these.
def user_constraint_bad_shape(x):
# WARNING: This should return a vector, not a scalar!
return x[0]
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1, 'x')
binding_bad_shape = prog.AddConstraint(user_constraint_bad_shape,
lb=[0.],
ub=[2.],
vars=x)
for T in SCALAR_TYPES:
array_T = np.vectorize(T)
x0 = array_T([0.])
x0_bad = array_T([0., 1.])
# Bad input (before function is called).
if kDrakeAssertIsArmed:
# See note in `WrapUserFunc`.
input_error_cls = SystemExit
input_error_expected = (
"x.rows() == num_vars_ || num_vars_ == Eigen::Dynamic")
else:
input_error_cls = RuntimeError
input_error_expected = (
"PyFunctionConstraint: Input must be of .ndim = 1 or 2 "
"(vector) and .size = 1. Got .ndim = 1 and .size = 2 "
"instead.")
with self.assertRaises(input_error_cls) as cm:
binding_bad_shape.evaluator().Eval(x0_bad)
self.assertIn(input_error_expected, str(cm.exception))
# Bad output.
with self.assertRaises(RuntimeError) as cm:
binding_bad_shape.evaluator().Eval(x0)
self.assertEqual(
str(cm.exception),
"PyFunctionConstraint: Output must be of .ndim = 1 or 2 "
"(vector) and .size = 1. Got .ndim = 0 and .size = 1 instead.")
# Bad output dtype.
U = self.get_different_scalar_type(T)
def user_constraint_bad_dtype(x):
# WARNING: This should return the same dtype as x!
return [U(0.)]
binding_bad_dtype = prog.AddConstraint(user_constraint_bad_dtype,
lb=[0.],
ub=[2.],
vars=x)
with self.assertRaises(RuntimeError) as cm:
binding_bad_dtype.evaluator().Eval(x0)
self.assertEqual(
str(cm.exception),
f"PyFunctionConstraint: Output must be of scalar type "
f"{T.__name__}. Got {U.__name__} instead.")
def test_addcost_symbolic(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1, 'x')
prog.AddCost((x[0] - 1.)**2)
prog.AddConstraint(0 <= x[0])
prog.AddConstraint(x[0] <= 2)
result = mp.Solve(prog)
self.assertAlmostEqual(result.GetSolution(x)[0], 1.)
def test_addconstraint_matrix(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1, 'x')
prog.AddConstraint(np.array([[x[0] <= 2], [x[0] >= -2]]))
result = mp.Solve(prog)
self.assertTrue(result.GetSolution(x)[0] <= 2)
self.assertTrue(result.GetSolution(x)[0] >= -2)
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)
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]))
@unittest.skipIf(SNOPT_NO_GUROBI,
"SNOPT is unable to solve this problem (#10653).")
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_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_add_rotated_lorentz_cone_constraint(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(3)
A = np.array([[1, 0], [1, 1], [1, 0], [0, 1], [0, 0]])
b = np.array([1, 0, 1, 0, 2])
constraint = prog.AddRotatedLorentzConeConstraint(A=A, b=b, vars=x[:2])
np.testing.assert_allclose(constraint.evaluator().A().todense(), A)
np.testing.assert_allclose(constraint.evaluator().b(), b)
prog.AddRotatedLorentzConeConstraint(
v=[x[0] + 1, x[0] + x[1], x[0], x[2] + 1, 2])
constraint = prog.AddRotatedLorentzConeConstraint(
linear_expression1=x[0] + 1,
linear_expression2=x[0] + x[1],
quadratic_expression=x[0] * x[0] + 2 * x[0] + x[1] * x[1] + 5)
def test_solver_options(self):
prog = mp.MathematicalProgram()
prog.SetSolverOption(SolverType.kGurobi, "double_key", 1.0)
prog.SetSolverOption(GurobiSolver().solver_id(), "int_key", 2)
prog.SetSolverOption(SolverType.kGurobi, "string_key", "3")
options = prog.GetSolverOptions(SolverType.kGurobi)
self.assertDictEqual(options, {
"double_key": 1.0,
"int_key": 2,
"string_key": "3"
})
options = prog.GetSolverOptions(GurobiSolver().solver_id())
self.assertDictEqual(options, {
"double_key": 1.0,
"int_key": 2,
"string_key": "3"
})
# For now, just make sure the constructor exists. Once we bind more
# accessors, we can test them here.
options_object = SolverOptions()
solver_id = SolverId("dummy")
self.assertEqual(solver_id.name(), "dummy")
options_object.SetOption(solver_id, "double_key", 1.0)
options_object.SetOption(solver_id, "int_key", 2)
options_object.SetOption(solver_id, "string_key", "3")
options = options_object.GetOptions(solver_id)
self.assertDictEqual(options, {
"double_key": 1.0,
"int_key": 2,
"string_key": "3"
})
prog.SetSolverOptions(options_object)
prog_options = prog.GetSolverOptions(solver_id)
self.assertDictEqual(prog_options, {
"double_key": 1.0,
"int_key": 2,
"string_key": "3"
})
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_add_indeterminates_and_decision_variables(self):
prog = mp.MathematicalProgram()
x0 = sym.Variable("x0")
x1 = sym.Variable("x1")
a0 = sym.Variable("a0")
a1 = sym.Variable("a1")
prog.AddIndeterminates(np.array([x0, x1]))
prog.AddDecisionVariables(np.array([a0, a1]))
numpy_compare.assert_equal(prog.decision_variables()[0], a0)
numpy_compare.assert_equal(prog.decision_variables()[1], a1)
numpy_compare.assert_equal(prog.indeterminates()[0], x0)
numpy_compare.assert_equal(prog.indeterminate(1), x1)
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]
def compute_input(self, x, xd, initial_guess=None):
prog = MathematicalProgram()
# Joint configuration states & Contact forces
q = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='q')
v = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='v')
u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
contact = prog.NewContinuousVariables(rows=self.T,
cols=self.nf,
name='lambda1')
z = prog.NewBinaryVariables(rows=self.T, cols=self.nf, name='z')
# Add Initial Condition Constraint
prog.AddConstraint(eq(q[0], np.array(x[0:3])))
prog.AddConstraint(eq(v[0], np.array(x[3:6])))
# Add Final Condition Constraint
prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))
prog.AddConstraint(z[0, 0] == 0)
prog.AddConstraint(z[0, 1] == 0)
# Add Dynamics Constraints
for t in range(self.T):
# Add Dynamics Constraints
prog.AddConstraint(
eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))
prog.AddConstraint(v[t + 1, 0] == (
v[t, 0] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
prog.AddConstraint(v[t + 1,
1] == (v[t, 1] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 1] +
contact[t, 0] - contact[t, 1])))
prog.AddConstraint(v[t + 1, 2] == (
v[t, 2] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))
# Add Contact Constraints with big M = self.contact
prog.AddConstraint(ge(contact[t], 0))
prog.AddConstraint(contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) >= 0)
prog.AddConstraint(contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) >= 0)
# Mixed Integer Constraints
M = self.contact_max
prog.AddConstraint(contact[t, 0] <= M)
prog.AddConstraint(contact[t, 1] <= M)
prog.AddConstraint(contact[t, 0] <= M * z[t, 0])
prog.AddConstraint(contact[t, 1] <= M * z[t, 1])
prog.AddConstraint(
contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) <= M *
(1 - z[t, 0]))
prog.AddConstraint(
contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) <= M *
(1 - z[t, 1]))
prog.AddConstraint(z[t, 0] + z[t, 1] == 1)
# Add Input Constraints. Contact Constraints already enforced in big-M
# prog.AddConstraint(le(u[t], self.input_max))
# prog.AddConstraint(ge(u[t], -self.input_max))
# Add Costs
prog.AddCost(u[t].dot(u[t]))
# Set Initial Guess as empty. Otherwise, start from last solver iteration.
if (type(initial_guess) == type(None)):
initial_guess = np.empty(prog.num_vars())
# Populate initial guess by linearly interpolating between initial
# and final states
#qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
uinit = np.tile(np.array([0, 0]), (self.T, 1))
finit = np.tile(np.array([0, 0]), (self.T, 1))
prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
prog.SetDecisionVariableValueInVector(contact, finit,
initial_guess)
# Solve the program
if (self.solver == "ipopt"):
solver_id = IpoptSolver().solver_id()
elif (self.solver == "snopt"):
solver_id = SnoptSolver().solver_id()
elif (self.solver == "osqp"):
solver_id = OsqpSolver().solver_id()
elif (self.solver == "mosek"):
solver_id = MosekSolver().solver_id()
elif (self.solver == "gurobi"):
solver_id = GurobiSolver().solver_id()
solver = MixedIntegerBranchAndBound(prog, solver_id)
#result = solver.Solve(prog, initial_guess)
result = solver.Solve()
if result != result.kSolutionFound:
raise ValueError('Infeasible optimization problem')
sol = result.GetSolution()
q_opt = result.GetSolution(q)
v_opt = result.GetSolution(v)
u_opt = result.GetSolution(u)
f_opt = result.GetSolution(contact)
return sol, q_opt, v_opt, u_opt, f_opt
def linear_program(f, A, b, C=None, d=None, tol=1.e-5):
"""
Solves the linear program min_x f^T x s.t. A x <= b, C x = d.
Arguments
----------
f : numpy.ndarray
Gradient of the cost function.
A : numpy.ndarray
Left-hand side of the inequality constraints.
b : numpy.ndarray
Right-hand side of the inequality constraints.
C : numpy.ndarray
Left-hand side of the equality constraints.
d : numpy.ndarray
Right-hand side of the equality constraints.
tol : float
Maximum value of a residual of an inequality to consider the related constraint active.
Returns
----------
sol : dict
Dictionary with the solution of the LP.
Fields
----------
min : float
Minimum of the LP (None if the problem is unfeasible or unbounded).
argmin : numpy.ndarray
Argument that minimizes the LP (None if the problem is unfeasible or unbounded).
active_set : list of int
Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible or unbounded).
multiplier_inequality : numpy.ndarray
Lagrange multipliers for the inequality constraints (None if the problem is unfeasible or unbounded).
multiplier_equality : numpy.ndarray
Lagrange multipliers for the equality constraints (None if the problem is unfeasible or unbounded or without equality constraints).
"""
# check equalities
if (C is None) != (d is None):
raise ValueError('missing C or d.')
# problem size
n_ineq, n_x = A.shape
if C is not None:
n_eq = C.shape[0]
else:
n_eq = 0
# reshape inputs
if len(f.shape) == 2:
f = np.reshape(f, f.shape[0])
# build program
prog = MathematicalProgram()
x = prog.NewContinuousVariables(n_x)
inequalities = []
for i in range(n_ineq):
lhs = A[i, :] + 1.e-20 * np.random.rand(
(n_x)
) # drake raises a RuntimeError if the in the expression x does not appear (e.g.: 0 x <= 1)
rhs = b[i] + 1.e-15 * np.random.rand(
1
) # in case the constraint is 0 x <= 0 the previous trick ends up adding the constraint x <= 0 to the program...
inequalities.append(prog.AddLinearConstraint(lhs.dot(x) <= rhs))
for i in range(n_eq):
prog.AddLinearConstraint(C[i, :].dot(x) == d[i])
prog.AddLinearCost(f.dot(x))
# solve
solver = GurobiSolver()
prog.SetSolverOption(solver.solver_type(), "OutputFlag", 0)
result = prog.Solve()
# initialize output
sol = {
'min': None,
'argmin': None,
'active_set': None,
'multiplier_inequality': None,
'multiplier_equality': None
}
if result == SolutionResult.kSolutionFound:
sol['argmin'] = prog.GetSolution(x).reshape(n_x, 1)
sol['min'] = f.dot(sol['argmin'])[0]
sol['active_set'] = np.where(
A.dot(sol['argmin']) - b > -tol)[0].tolist()
# retrieve multipliers through KKT conditions
M = A[sol['active_set'], :].T
if n_eq > 0:
M = np.hstack((M, C.T))
m = np.linalg.pinv(M).dot(-f.reshape(n_x, 1))
sol['multiplier_inequality'] = np.zeros((n_ineq, 1))
for i, j in enumerate(sol['active_set']):
sol['multiplier_inequality'][j, 0] = m[i, :]
if n_eq > 0:
sol['multiplier_equality'] = m[len(sol['active_set']):, :]
return sol
class TestMathematicalProgram(unittest.TestCase):
def test_program_construction(self):
prog = mp.MathematicalProgram()
vars = prog.NewContinuousVariables(5, "x")
self.assertEqual(vars.dtype, sym.Variable)
vars_all = prog.decision_variables()
self.assertEqual(vars_all.shape, (5,))
def test_program_attributes_and_solver_selection(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
# Add linear equality constraints; make sure the solver works.
prog.AddLinearConstraint(x[0] + x[1] == 0)
prog.AddLinearConstraint(2*x[0] - x[1] == 1)
solver_id = mp.ChooseBestSolver(prog)
self.assertEqual(solver_id.name(), "Linear system")
solver = mp.MakeSolver(solver_id)
self.assertEqual(solver.solver_id().name(), "Linear system")
self.assertTrue(solver.AreProgramAttributesSatisfied(prog))
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
# With an inequality constraint added, the "Linear system" solver
# doesn't work anymore.
prog.AddLinearConstraint(x[0] >= 0)
self.assertFalse(solver.AreProgramAttributesSatisfied(prog))
with self.assertRaises(ValueError):
solver.Solve(prog, None, None)
# A different solver will work, though. We re-use the result object
# (as a mutable output argument), and make sure that it changes.
solver_id = mp.ChooseBestSolver(prog)
self.assertNotEqual(solver_id.name(), "Linear system")
solver = mp.MakeSolver(solver_id)
solver.Solve(prog, None, None, result)
self.assertTrue(result.is_success())
self.assertEqual(result.get_solver_id().name(), solver_id.name())
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))
@unittest.skipUnless(GurobiSolver().available(), "Requires Gurobi")
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)
self.assertIsNone(prog.GetSolverId())
result = prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
self.assertIsNotNone(prog.GetSolverId().name())
# 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_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 = prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
x_expected = np.array([1, 1])
self.assertTrue(np.allclose(prog.GetSolution(x), x_expected))
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 = prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
x_expected = np.array([1, 1])
self.assertTrue(np.allclose(prog.GetSolution(x), x_expected))
def test_bindings(self):
qp = TestQP()
prog = qp.prog
x = qp.x
for binding in prog.GetAllCosts():
self.assertIsInstance(binding.evaluator(), mp.Cost)
for binding in prog.GetLinearConstraints():
self.assertIsInstance(binding.evaluator(), mp.Constraint)
for binding in prog.GetAllConstraints():
self.assertIsInstance(binding.evaluator(), mp.Constraint)
self.assertTrue(prog.linear_costs())
for (i, binding) in enumerate(prog.linear_costs()):
cost = binding.evaluator()
self.assertTrue(np.allclose(cost.a(), np.ones((1, 2))))
self.assertTrue(prog.quadratic_costs())
for (i, binding) in enumerate(prog.quadratic_costs()):
cost = binding.evaluator()
self.assertTrue(np.allclose(cost.Q(), np.eye(2)))
self.assertTrue(np.allclose(cost.b(), np.zeros(2)))
self.assertTrue(prog.bounding_box_constraints())
for (i, binding) in enumerate(prog.bounding_box_constraints()):
constraint = binding.evaluator()
self.assertEqual(
prog.FindDecisionVariableIndex(binding.variables()[0]),
prog.FindDecisionVariableIndex(x[i]))
num_constraints = constraint.num_constraints()
if num_constraints == 1:
self.assertEqual(constraint.A(), 1)
self.assertEqual(constraint.lower_bound(), 1)
self.assertEqual(constraint.upper_bound(), np.inf)
else:
self.assertTrue(np.allclose(constraint.A(), np.eye(2)))
self.assertTrue(np.allclose(constraint.lower_bound(),
[1, -np.inf]))
self.assertTrue(np.allclose(constraint.upper_bound(),
[np.inf, 2]))
self.assertTrue(prog.linear_constraints())
for (i, binding) in enumerate(prog.linear_constraints()):
constraint = binding.evaluator()
self.assertEqual(
prog.FindDecisionVariableIndex(binding.variables()[0]),
prog.FindDecisionVariableIndex(x[0]))
self.assertEqual(
prog.FindDecisionVariableIndex(binding.variables()[1]),
prog.FindDecisionVariableIndex(x[1]))
self.assertTrue(np.allclose(constraint.A(), [3, -1]))
self.assertTrue(constraint.lower_bound(), -2)
self.assertTrue(constraint.upper_bound(), np.inf)
self.assertTrue(prog.linear_equality_constraints())
for (i, binding) in enumerate(prog.linear_equality_constraints()):
constraint = binding.evaluator()
self.assertEqual(
prog.FindDecisionVariableIndex(binding.variables()[0]),
prog.FindDecisionVariableIndex(x[0]))
self.assertEqual(
prog.FindDecisionVariableIndex(binding.variables()[1]),
prog.FindDecisionVariableIndex(x[1]))
self.assertTrue(np.allclose(constraint.A(), [1, 2]))
self.assertTrue(constraint.lower_bound(), 3)
self.assertTrue(constraint.upper_bound(), 3)
result = prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
x_expected = np.array([1, 1])
self.assertTrue(np.allclose(prog.GetSolution(x), x_expected))
# Test deprecated method.
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('once', DrakeDeprecationWarning)
c = binding.constraint()
self.assertEqual(len(w), 1)
def test_constraint_api(self):
prog = mp.MathematicalProgram()
x0, = prog.NewContinuousVariables(1, "x")
c = prog.AddLinearConstraint(x0 >= 2).evaluator()
ce = prog.AddLinearEqualityConstraint(2*x0, 1).evaluator()
self.assertTrue(c.CheckSatisfied([2.], tol=1e-3))
self.assertFalse(c.CheckSatisfied([AutoDiffXd(1.)]))
self.assertIsInstance(c.CheckSatisfied([x0]), sym.Formula)
def check_bounds(c, A, lb, ub):
self.assertTrue(np.allclose(c.A(), A))
self.assertTrue(np.allclose(c.lower_bound(), lb))
self.assertTrue(np.allclose(c.upper_bound(), ub))
check_bounds(c, [1.], [2.], [np.inf])
c.UpdateLowerBound([3.])
check_bounds(c, [1.], [3.], [np.inf])
c.UpdateUpperBound([4.])
check_bounds(c, [1.], [3.], [4.])
c.set_bounds([-10.], [10.])
check_bounds(c, [1.], [-10.], [10.])
c.UpdateCoefficients([10.], [-20.], [-30.])
check_bounds(c, [10.], [-20.], [-30.])
check_bounds(ce, [2.], [1.], [1.])
ce.UpdateCoefficients([10.], [20.])
check_bounds(ce, [10.], [20.], [20.])
def test_cost_api(self):
prog = mp.MathematicalProgram()
x0, = prog.NewContinuousVariables(1, "x")
lc = prog.AddLinearCost(1*x0 + 2).evaluator()
qc = prog.AddQuadraticCost(0.5*x0**2 + 2*x0 + 3).evaluator()
def check_linear_cost(cost, a, b):
self.assertTrue(np.allclose(cost.a(), a))
self.assertTrue(np.allclose(cost.b(), b))
check_linear_cost(lc, [1.], 2.)
lc.UpdateCoefficients([10.])
check_linear_cost(lc, [10.], 0.)
def check_quadratic_cost(cost, Q, b, c):
self.assertTrue(np.allclose(cost.Q(), Q))
self.assertTrue(np.allclose(cost.b(), b))
self.assertTrue(np.allclose(cost.c(), c))
check_quadratic_cost(qc, [1.], [2.], 3.)
qc.UpdateCoefficients([10.], [20.])
check_quadratic_cost(qc, [10.], [20.], 0)
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.]
prog.Solve()
self.assertTrue(np.allclose(prog.GetSolution(x), x_expected))
enum = zip(constraints, constraint_values_expected)
for (constraint, value_expected) in enum:
value = prog.EvalBindingAtSolution(constraint)
self.assertTrue(np.allclose(value, value_expected))
enum = zip(costs, cost_values_expected)
for (cost, value_expected) in enum:
value = prog.EvalBindingAtSolution(cost)
self.assertTrue(np.allclose(value, value_expected))
# Existence check.
self.assertIsInstance(
prog.EvalBinding(costs[0], x_expected), np.ndarray)
self.assertIsInstance(
prog.EvalBindings(prog.GetAllConstraints(), x_expected),
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_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)
prog.Solve()
xval = prog.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], prog.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 = prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
S = prog.GetSolution(S)
eigs = np.linalg.eigvals(S)
tol = 1e-8
self.assertTrue(np.all(eigs >= -tol))
self.assertTrue(S[0, 1] >= -tol)
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)
# Test SubstituteSolution(sym.Expression)
# 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 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 = prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
self.assertIsInstance(binding.evaluator(),
mp.LinearComplementarityConstraint)
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(np.eye(2), np.zeros(2), np.ones(2), x)
prog.AddLinearEqualityConstraint(np.eye(2), np.zeros(2), x)
prog.AddLinearEqualityConstraint(x[0] == 1)
prog.AddLinearEqualityConstraint(x[0] + x[1], 1)
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)
prog.Solve()
self.assertAlmostEqual(prog.GetSolution(x)[0], 1.)
def test_addcost_symbolic(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1, 'x')
prog.AddCost((x[0]-1.)**2)
prog.AddConstraint(0 <= x[0])
prog.AddConstraint(x[0] <= 2)
prog.Solve()
self.assertAlmostEqual(prog.GetSolution(x)[0], 1.)
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)
check_and_reset()
@unittest.skipIf(
SNOPT_NO_GUROBI,
"SNOPT is unable to solve this problem (#10653).")
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 = prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
# Check answer
x_expected = np.array([1-2**(-0.5), 1-2**(-0.5)])
self.assertTrue(np.allclose(prog.GetSolution(x), x_expected))
def test_solver_options(self):
prog = mp.MathematicalProgram()
prog.SetSolverOption(SolverType.kGurobi, "double_key", 1.0)
prog.SetSolverOption(SolverType.kGurobi, "int_key", 2)
prog.SetSolverOption(SolverType.kGurobi, "string_key", "3")
options = prog.GetSolverOptions(SolverType.kGurobi)
self.assertDictEqual(
options, {"double_key": 1.0, "int_key": 2, "string_key": "3"})
# For now, just make sure the constructor exists. Once we bind more
# accessors, we can test them here.
options_object = SolverOptions()
SolverId, SolverInterface)
from functools import partial
import unittest
import warnings
import numpy as np
import pydrake
from pydrake.autodiffutils import AutoDiffXd
from pydrake.common.test_utilities import numpy_compare
from pydrake.forwarddiff import jacobian
from pydrake.math import ge
import pydrake.symbolic as sym
SNOPT_NO_GUROBI = SnoptSolver().available() and not GurobiSolver().available()
class TestQP:
def __init__(self):
# Create a simple QP that uses all deduced linear constraint types,
# along with a quadratic and linear cost.
# The solution should be [1, 1].
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
self.prog = prog
self.x = x
self.constraints = [
# Bounding box
prog.AddLinearConstraint(x[0] >= 1),
# Bounding box
def mixed_integer_quadratic_program(nc, H, f, A, b, C=None, d=None, **kwargs):
"""
Solves the strictly convex (H > 0) mixed-integer quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d.
The first nc variables in x are continuous, the remaining are binaries.
Arguments
----------
nc : int
Number of continuous variables in the problem.
H : numpy.ndarray
Positive definite Hessian of the cost function.
f : numpy.ndarray
Gradient of the cost function.
A : numpy.ndarray
Left-hand side of the inequality constraints.
b : numpy.ndarray
Right-hand side of the inequality constraints.
C : numpy.ndarray
Left-hand side of the equality constraints.
d : numpy.ndarray
Right-hand side of the equality constraints.
Returns
----------
sol : dict
Dictionary with the solution of the MIQP.
Fields
----------
min : float
Minimum of the MIQP (None if the problem is unfeasible).
argmin : numpy.ndarray
Argument that minimizes the MIQP (None if the problem is unfeasible).
"""
# check equalities
if (C is None) != (d is None):
raise ValueError('missing C or d.')
# problem size
n_ineq, n_x = A.shape
if C is not None:
n_eq = C.shape[0]
else:
n_eq = 0
# build program
prog = MathematicalProgram()
x = np.hstack((
prog.NewContinuousVariables(nc),
prog.NewBinaryVariables(n_x - nc)
))
[prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)]
[prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)]
prog.AddQuadraticCost(.5*x.dot(H).dot(x) + f.dot(x))
# solve
solver = GurobiSolver()
prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0)
[prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()]
result = prog.Solve()
# initialize output
sol = {
'min': None,
'argmin': None
}
if result == SolutionResult.kSolutionFound:
sol['argmin'] = prog.GetSolution(x)
sol['min'] = .5*sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin'])
return sol
def __init__(self, frontend: SlamFrontend):
"""
Indices (consistent with Indelman2015, aka "the paper".)
Constants:
k: current time step.
L: planning horizon.
i: time step.
j: landmark.
l \in [k, k+L]: one of the future time steps up to L.
:param X_WB_e_0: np array of shape (self.dim_X). Initial estimated
robot position, used to anchor the entire trajectory.
"""
self.solver = GurobiSolver()
self.X_WB_e_0 = np.zeros(2)
self.dim_l = 2 # dimension of landmarks.
self.dim_X = 2 # dimension of poses.
self.dim_measurements = 2
# current beliefs
self.X_WB_e_dict = {0: self.X_WB_e_0}
self.X_WB_e_var_dict = {}
self.l_xy_e_dict = dict()
# Information matrix of current robot poses and landmarks, which are
# ordered as follows:
# [X_WB_e0, ... X_WB_e_k, l_xy_e0, ... l_xy_eK]
# The landmarks are ordered as self.l_xy_e_dict.keys().
self.I_all = None
# past measurements
self.odometry_measurements = dict()
# key (int): landmark index
# value: dict of {t: {"distance":d, "bearing": b}}.
# t: index of robot poses from which the landmark is visible.
# d: range measurement for the landmark from the pose indexed by t.
# b: bearing measurement for the landmark from the pose indexed by t.
self.landmark_measurements = dict()
# Taking stuff from frontend
self.frontend = frontend
self.vis = frontend.vis
self.l_xy = frontend.l_xy
# TODO: load from config file?
self.sigma_odometry = frontend.sigma_odometry
self.sigma_range = frontend.sigma_range
self.sigma_bearing = frontend.sigma_bearing
self.sigma_dynamics = self.sigma_odometry
self.r_range_max = frontend.r_range_max
self.r_range_min = frontend.r_range_min
# weights/selection matrices for the cost function.
self.M_u = np.ones(self.dim_l) * 0.1
self.beta = 0.15
self.alpha_LB = 0.6
self.is_uncertainty_reduction_only = False
class SlamBackend:
def __init__(self, frontend: SlamFrontend):
"""
Indices (consistent with Indelman2015, aka "the paper".)
Constants:
k: current time step.
L: planning horizon.
i: time step.
j: landmark.
l \in [k, k+L]: one of the future time steps up to L.
:param X_WB_e_0: np array of shape (self.dim_X). Initial estimated
robot position, used to anchor the entire trajectory.
"""
self.solver = GurobiSolver()
self.X_WB_e_0 = np.zeros(2)
self.dim_l = 2 # dimension of landmarks.
self.dim_X = 2 # dimension of poses.
self.dim_measurements = 2
# current beliefs
self.X_WB_e_dict = {0: self.X_WB_e_0}
self.X_WB_e_var_dict = {}
self.l_xy_e_dict = dict()
# Information matrix of current robot poses and landmarks, which are
# ordered as follows:
# [X_WB_e0, ... X_WB_e_k, l_xy_e0, ... l_xy_eK]
# The landmarks are ordered as self.l_xy_e_dict.keys().
self.I_all = None
# past measurements
self.odometry_measurements = dict()
# key (int): landmark index
# value: dict of {t: {"distance":d, "bearing": b}}.
# t: index of robot poses from which the landmark is visible.
# d: range measurement for the landmark from the pose indexed by t.
# b: bearing measurement for the landmark from the pose indexed by t.
self.landmark_measurements = dict()
# Taking stuff from frontend
self.frontend = frontend
self.vis = frontend.vis
self.l_xy = frontend.l_xy
# TODO: load from config file?
self.sigma_odometry = frontend.sigma_odometry
self.sigma_range = frontend.sigma_range
self.sigma_bearing = frontend.sigma_bearing
self.sigma_dynamics = self.sigma_odometry
self.r_range_max = frontend.r_range_max
self.r_range_min = frontend.r_range_min
# weights/selection matrices for the cost function.
self.M_u = np.ones(self.dim_l) * 0.1
self.beta = 0.15
self.alpha_LB = 0.6
self.is_uncertainty_reduction_only = False
def update_landmark_measurements(self, t, idx_visible_l_list,
d_l_measured_list, b_l_measured_list):
"""
inputs are the output of Frontend.get_range_measurements()
:return: None.
"""
for i, i_l in enumerate(idx_visible_l_list):
d_l = d_l_measured_list[i]
b_l = b_l_measured_list[i]
if i_l not in self.landmark_measurements:
self.landmark_measurements[i_l] = dict()
self.landmark_measurements[i_l][t] = \
{"distance": d_l, "bearing": b_l}
def update_odometry_measruements(self, t, odometry_measurement):
"""
:param t: current time step (starting from 0).
:param odometry_measurement: odometry measruement between t and t-1.
:return:
"""
self.odometry_measurements[t] = odometry_measurement
def get_X_WB_belief(self):
n = len(self.X_WB_e_dict)
X_WB_e = np.zeros((n, self.dim_X))
for i in range(n):
X_WB_e[i] = self.X_WB_e_dict[i]
return X_WB_e
def get_l_xy_belief(self):
n = len(self.l_xy_e_dict)
l_xy_e = np.zeros((n, self.dim_l))
for i, k in enumerate(self.l_xy_e_dict.keys()):
l_xy_e[i] = self.l_xy_e_dict[k]
return l_xy_e
def get_odometry_measurements(self):
n_o = len(self.odometry_measurements)
odometry_measurements = np.zeros((n_o, self.dim_X))
for i in range(n_o):
odometry_measurements[i] = self.odometry_measurements[i + 1]
return odometry_measurements
def get_X_WB_initial_guess_array(self):
"""
Use self.t_xy_e_dict to create an initial guess for the next bundle
adjustment.
Let t be the current exploration time step. As t_xy_e_dict is the
estimate for time step t-1, its shape is (t - 1, 3), which does
not include an estimate for the pose of the current time step.
The initial guess for the current time step is computed as
self.t_xy_e_dict[t - 1] + self.odometry_measurements[t].
:return:
"""
n_o = len(self.odometry_measurements)
X_WB_e = np.zeros((n_o + 1, self.dim_X))
for i in range(n_o):
X_WB_e[i] = self.X_WB_e_dict[i]
if n_o > 0:
X_WB_e[n_o] = X_WB_e[n_o - 1] + self.odometry_measurements[n_o]
# First ground truth is given.
X_WB_e[0] = self.X_WB_e_0
return X_WB_e
def get_l_xy_initial_guess_array(self):
nl = len(self.landmark_measurements)
l_xy_e = np.ones((nl, 2))
for i_k, k in enumerate(self.landmark_measurements.keys()):
if k in self.l_xy_e_dict:
l_xy_e[i_k] = self.l_xy_e_dict[k]
return l_xy_e
def marginalize_info_matrix(self, Omega, n_p, n_l):
nX = n_p * self.dim_X
m, n = Omega.shape
assert m == n
assert m == nX + n_l * self.dim_l
A = Omega[:nX, :nX]
B = Omega[:nX, nX:]
C = Omega[nX:, nX:]
return A - B.dot(np.linalg.inv(C)).dot(B.T)
def calc_range_derivatives(self,
J,
i_row,
j_start_x,
j_start_l,
d_xy,
sigma=1.0):
"""
:param i_row:
:param j_start_x:
:param j_start_l:
:param d_xy: X_WB - l_xy
:return:
"""
if torch.is_tensor(J):
norm = torch.norm
else:
norm = np.linalg.norm
d = norm(d_xy)
J[i_row, j_start_x:j_start_x + 2] = d_xy / (d * sigma)
J[i_row, j_start_l:j_start_l + self.dim_l] = -d_xy / (d * sigma)
return d
def calc_bearing_derivatives(self,
J,
i_row,
j_start_x,
j_start_l,
X_WB,
l_xy,
sigma=1.0):
xb = X_WB[0]
yb = X_WB[1]
xl = l_xy[0]
yl = l_xy[1]
dx = xl - xb
dy = yl - yb
d_arctan_D_dx = 1 / (1 + (dy / dx)**2) * (-dy / dx**2)
d_arctan_D_dy = 1 / (1 + (dy / dx)**2) * (1 / dx)
J[i_row, j_start_x] = -d_arctan_D_dx / sigma
J[i_row, j_start_x + 1] = -d_arctan_D_dy / sigma
J[i_row, j_start_l] = d_arctan_D_dx / sigma
J[i_row, j_start_l + 1] = d_arctan_D_dy / sigma
return dx, dy
@staticmethod
def calc_num_landmark_measurements(landmark_measurements):
nl_measurements = 0 # number of landmark measurements
for visible_landmarks_i in landmark_measurements.values():
nl_measurements += len(visible_landmarks_i)
return nl_measurements
def calc_jacobian_and_b(self, X_WB_e, l_xy_e):
"""
:param X_WB_e (n_o + 1, 3)
:param l_xy_e (nl, 2): coordinates of landmarks, ordered the same way as
self.landmark_measurements.
:param landmark_measurements:
:return:
"""
dim_l = self.dim_l # dimension of landmarks.
dim_X = self.dim_X # dimension of poses.
nl = len(self.landmark_measurements) # number of landmarks
n_o = len(self.odometry_measurements) # number of odometries
# number of landmark measurements
nl_measurements = self.calc_num_landmark_measurements(
self.landmark_measurements)
n_rows = (n_o + 1) * dim_X + nl_measurements * self.dim_measurements
n_cols = (n_o + 1) * dim_X + nl * dim_l
J = np.zeros((n_rows, n_cols))
b = np.zeros(n_rows)
sigmas = np.zeros(n_rows)
# prior on first pose.
J[-self.dim_X:, :self.dim_X] = np.eye(self.dim_X)
sigmas[-self.dim_X:] = (self.sigma_odometry / 10)
# odometry
for i in range(n_o):
i0 = i * dim_X
i1 = i0 + dim_X
J[i0:i1, i0:i1] = -np.eye(dim_X)
J[i0:i1, i0 + dim_X:i1 + dim_X] = np.eye(dim_X)
bi = b[i0:i1]
sigmas_i = sigmas[i0:i1]
# displacement
bi[:2] = X_WB_e[i + 1, :2] - X_WB_e[i, :2] - \
self.odometry_measurements[i + 1][:2]
sigmas_i[:2] = self.sigma_odometry
# range and bearing measurements.
i_row = n_o * dim_X
for idx_j, (j, visible_robot_poses) in enumerate(
self.landmark_measurements.items()):
for i in visible_robot_poses.keys():
# i: index of robot poses visible from
d_ik_m = visible_robot_poses[i]["distance"]
b_ik_m = visible_robot_poses[i]["bearing"]
# range.
d_xy = X_WB_e[i, :2] - l_xy_e[idx_j]
j_start_x = i * dim_X
j_start_l = (n_o + 1) * dim_X + idx_j * dim_l
d = self.calc_range_derivatives(J, i_row, j_start_x, j_start_l,
d_xy)
b[i_row] = d - d_ik_m
sigmas[i_row] = self.sigma_range
# bearing.
i_row += 1
dx, dy = self.calc_bearing_derivatives(J, i_row, j_start_x,
j_start_l, X_WB_e[i],
l_xy_e[idx_j])
b[i_row] = calc_angle_diff(b_ik_m, np.arctan2(dy, dx))
sigmas[i_row] = self.sigma_bearing
i_row += 1
return J, b, sigmas
def calc_info_matrix(self, X_WB_e, l_xy_e):
J, b, sigmas = self.calc_jacobian_and_b(X_WB_e, l_xy_e)
Omega = 1 / sigmas**2
I = (J.T * Omega).dot(J)
q = J.T.dot(Omega * b)
c = (b * Omega).dot(b)
return I, q, c
def calc_A_lower_half(self, dX_WB, l_xy_e):
"""
Algorithm 4 in the paper.
:param dX_WB: [l, self.dim_X]. Input for time steps [l : ].
:return:
"""
# X_WB_e: belief. e stands for "estimated".
# X_WB_p: prediction. corresponds to quantities with an overbar.
n_p = len(self.X_WB_e_dict)
l = len(dX_WB)
# future predictions.
X_WB_p = self.calc_pose_predictions(dX_WB)
# Find the visible landmarks for every X_WB_p[i].
# Store in {i: [landmark order indices.]}, i.e. the same order as
# self.landmark_measurements.
# future_landmark_measurements = self.find_visible_landmarks(X_WB_p)
nl = len(self.landmark_measurements) # number of landmarks
# number of new landmark measurements
nl_measurements = l * nl
# Structure of new "state"
# [X_WB_0, ... X_WB_{k},
# l_xy (all landmarks),
# X_WB_{k+1}, ... X_WB_{k+l}]
n_Xk = n_p * self.dim_X + nl * self.dim_l
n_rows_F = l * self.dim_X
n_rows_H = 2 * nl_measurements
n_cols = l * self.dim_X + n_Xk
# F_whitened is devided by sigma_w.
if torch.is_tensor(dX_WB):
F_whitened = torch.zeros((n_rows_F, n_cols))
# H is the original Jacobian, before divided by sigma_v.
H = torch.zeros((n_rows_H, n_cols))
# as defined in Eq. (33)
Omega_v_bar_sqrt = torch.zeros(n_rows_H)
Omega_v_sqrt = torch.zeros(n_rows_H)
I_X = torch.eye(self.dim_X)
l_xy_e = torch.from_numpy(l_xy_e)
norm = torch.norm
vstack = torch.vstack
else:
F_whitened = np.zeros((n_rows_F, n_cols), dtype=dX_WB.dtype)
H = np.zeros((n_rows_H, n_cols), dtype=dX_WB.dtype)
Omega_v_bar_sqrt = np.zeros(n_rows_H)
Omega_v_sqrt = np.zeros(n_rows_H)
I_X = np.eye(self.dim_X)
norm = np.linalg.norm
vstack = np.vstack
# dynamics, F_whitened.
for i in range(l):
i0 = i * self.dim_X
i1 = i0 + self.dim_X
F_whitened[i0:i1, n_Xk + i0:n_Xk + i1] = I_X / self.sigma_dynamics
if i == 0:
j0 = (n_p - 1) * self.dim_X
j1 = n_p * self.dim_X
F_whitened[i0: i1, j0: j1] = \
-I_X / self.sigma_dynamics
else:
j0 = n_Xk + i0 - self.dim_X
j1 = n_Xk + i1 - self.dim_X
F_whitened[i0: i1, j0: j1] = \
-I_X / self.sigma_dynamics
# observations, H.
i_row = 0
for idx_j in range(nl):
for i in range(l):
d_xy = X_WB_p[i] - l_xy_e[idx_j]
j_start_x = n_Xk + i * self.dim_X
j_start_l = n_p * self.dim_X + idx_j * self.dim_l
d = self.calc_range_derivatives(H,
i_row,
j_start_x,
j_start_l,
d_xy,
sigma=1)
self.calc_bearing_derivatives(H,
i_row + 1,
j_start_x,
j_start_l,
X_WB_p[i],
l_xy_e[idx_j],
sigma=1)
if d > 5 * self.r_range_max:
p_visible = 0.
else:
p_visible = 1.
Omega_v_bar_sqrt[i_row] = p_visible / self.sigma_range
Omega_v_bar_sqrt[i_row + 1] = p_visible / self.sigma_bearing
Omega_v_sqrt[i_row] = 1 / self.sigma_range
Omega_v_sqrt[i_row + 1] = 1 / self.sigma_bearing
i_row += 2
# A2 is the lower half of \mathcal{A} in Eq. (32),
# i.e. [F_whitened; H / Omega_v_sqrt].
A2 = vstack([F_whitened, (H.T * Omega_v_bar_sqrt).T])
return {
"A2": A2,
"X_WB_p": X_WB_p,
"H": H,
"F_whitened": F_whitened,
"Omega_v_bar_sqrt": Omega_v_bar_sqrt,
"Omega_v_sqrt": Omega_v_sqrt
}
def calc_inner_layer(self, dX_WB, l_xy_e, I_Xk):
"""
Algorithm 4 in the paper.
:param dX_WB:
:param X_WB_e:
:param l_xy_e:
:param I_Xk: information matrix of current trajetory and landmarks.
:return:
"""
if torch.is_tensor(dX_WB):
I_Xk = torch.from_numpy(I_Xk)
n_Xk = I_Xk.shape[0] # size of states up to now.
def calc_I(A2, dtype):
n_Xl = A2.shape[1] - n_Xk # size of states into the future.
n_X = n_Xk + n_Xl
A21 = A2[:, :n_Xk]
A22 = A2[:, n_Xk:]
# I_X{k+l} = A.T.dot(A)
if torch.is_tensor(A2):
I = torch.zeros((n_X, n_X))
I[:n_Xk, :n_Xk] = I_Xk + torch.mm(A21.T, A21)
I[n_Xk:, n_Xk:] = torch.mm(A22.T, A22)
I[:n_Xk, n_Xk:] = torch.mm(A21.T, A22)
else:
I = np.zeros((n_X, n_X), dtype=dtype)
I[:n_Xk, :n_Xk] = I_Xk + A21.T.dot(A21)
I[n_Xk:, n_Xk:] = A22.T.dot(A22)
I[:n_Xk, n_Xk:] = A21.T.dot(A22)
I[n_Xk:, :n_Xk] = I[:n_Xk, n_Xk:].T
return I
# lower half of A
result = self.calc_A_lower_half(dX_WB, l_xy_e)
# including landmarks.
result["I_e"] = calc_I(result["A2"], dtype=object)
# excluding landmarks.
result["I_p"] = calc_I(result["F_whitened"], dtype=np.float)
return result
def calc_pose_predictions(self, dX_WB):
L = len(dX_WB)
k = len(self.X_WB_e_dict) - 1 # current time step.
# robot configuration for time steps k + 1 : (k + L).
if torch.is_tensor(dX_WB):
X_WB_p = torch.zeros((L, self.dim_X))
X_WB_e_k = torch.from_numpy(self.X_WB_e_dict[k])
else:
X_WB_p = np.zeros((L, self.dim_X), dtype=dX_WB.dtype)
X_WB_e_k = self.X_WB_e_dict[k]
for i in range(L):
if i == 0:
X_WB_p[i] = X_WB_e_k + dX_WB[i]
else:
X_WB_p[i] = X_WB_p[i - 1] + dX_WB[i]
return X_WB_p
@staticmethod
def get_inverse(x):
if torch.is_tensor(x):
return torch.inverse
else:
return inv_pydrake
def calc_objective(self, dX_WB, X_WB_e, l_xy_e, X_WB_g, alpha):
"""
Algorithm 3 in the paper.
:return:
"""
L = len(dX_WB)
I_k, _, _ = self.calc_info_matrix(X_WB_e, l_xy_e)
if torch.is_tensor(dX_WB):
M_u = torch.from_numpy(self.M_u)
X_WB_g = torch.from_numpy(X_WB_g)
else:
M_u = self.M_u
inv = self.get_inverse(dX_WB)
# (a) in eq. (41).
c_a = (dX_WB * M_u * dX_WB).sum()
# (b) in eq. (41).
c_b = 0.
for l in range(L):
result_l = self.calc_inner_layer(dX_WB[:l + 1], l_xy_e, I_k)
Cov_e_l = inv(result_l["I_e"])
c_b += Cov_e_l[-self.dim_X:, -self.dim_X:].diagonal().sum()
Cov_e_L = Cov_e_l
Cov_p_L = inv(result_l["I_p"])
X_WB_p_L = result_l["X_WB_p"]
A2_L = result_l["A2"]
Omega_v_sqrt = result_l["Omega_v_sqrt"]
Omega_v_bar_sqrt = result_l["Omega_v_bar_sqrt"]
# (c) in eq. (41)
c_c1 = ((X_WB_p_L[-1] - X_WB_g)**2).sum()
H_whitened_kL = A2_L[L * self.dim_X:]
H_kL = result_l["H"]
# B.shape = (self.dim_X, m2),
# where m2 is the number of range and bearing measurements of
# the trajectory X_WB_p_L.
if torch.is_tensor(dX_WB):
B = torch.mm(Cov_e_L[-self.dim_X:],
H_whitened_kL.T) * Omega_v_bar_sqrt
Q_kL = torch.mm(B.T, B)
D = torch.mm(torch.mm(H_kL, Cov_p_L), H_kL.T)
else:
B = Cov_e_L[-self.dim_X:].dot(H_whitened_kL.T) * Omega_v_bar_sqrt
Q_kL = B.T.dot(B)
D = H_kL.dot(Cov_p_L).dot(H_kL.T)
D[np.diag_indices_from(D)] += 1 / Omega_v_sqrt**2
if torch.is_tensor(dX_WB):
c_c2 = torch.mm(Q_kL, D).diagonal().sum()
else:
c_c2 = Q_kL.dot(D).diagonal().sum()
c = c_a + alpha * (c_b * 50) + (1 - alpha) * (c_c1 + c_c2)
# c = c_c1
return {
"c":
c,
"c_a":
c_a,
"c_b":
c_b,
"c_c1":
c_c1,
"c_c2":
c_c2,
"predicted_uncertainty_L":
Cov_p_L[-self.dim_X:, -self.dim_X:].diagonal().sum()
}
def calc_alpha(self, dX_WB, X_WB_e, l_xy_e):
inv = self.get_inverse(dX_WB)
I_k, _, _ = self.calc_info_matrix(X_WB_e, l_xy_e)
result_L = self.calc_inner_layer(dX_WB, l_xy_e, I_k)
Cov_p_L = inv(result_L["I_p"])
uncertainty_L = Cov_p_L[-self.dim_X:, -self.dim_X:].diagonal().sum()
alpha = min(1, uncertainty_L / self.beta)
if self.is_uncertainty_reduction_only:
if alpha < self.alpha_LB:
self.is_uncertainty_reduction_only = False
else:
alpha = 1
else:
if alpha == 1:
self.is_uncertainty_reduction_only = True
return float(alpha)
def calc_objective_gradient_finite_difference(self, dX_WB, X_WB_e, l_xy_e,
X_WB_g, alpha):
"""
Algorithm 2 in the paper.
:param dX_WB:
:param X_WB_e:
:param l_xy_e:
:param X_WB_g: goal.
:return:
"""
Dc = np.zeros_like(dX_WB.ravel())
h = 1e-3
for i in range(len(Dc)):
dX_WB_new = dX_WB.copy()
dX_WB_new.ravel()[i] += h
c_new1 = self.calc_objective(dX_WB_new, X_WB_e, l_xy_e, X_WB_g,
alpha)["c"]
dX_WB_new.ravel()[i] -= 2 * h
c_new2 = self.calc_objective(dX_WB_new, X_WB_e, l_xy_e, X_WB_g,
alpha)["c"]
Dc[i] = (c_new1 - c_new2) / h / 2
Dc.resize(dX_WB.shape)
return Dc
def run_gradient_descent(self,
dX_WB0,
X_WB_e,
l_xy_e,
X_WB_g,
alpha=None,
backprop=True):
"""
Algorithm 1 in the paper.
:return:
"""
dX_WB = dX_WB0.copy()
dX_WB_torch = torch.from_numpy(dX_WB)
dX_WB_torch.requires_grad_()
iter_count = 0
a = 0.4
b = 0.5
epsilon = 2e-3
if alpha is None:
alpha = self.calc_alpha(dX_WB, X_WB_e, l_xy_e)
while True:
if backprop:
if dX_WB_torch.grad is not None:
dX_WB_torch.grad.data.zero_()
result = self.calc_objective(dX_WB_torch, X_WB_e, l_xy_e,
X_WB_g, alpha)
c0 = result["c"]
c0.backward()
D_dX_WB = dX_WB_torch.grad.numpy()
result = self.convert_cost_from_torch_tensor_to_float(result)
else:
result = self.calc_objective(dX_WB, X_WB_e, l_xy_e, X_WB_g,
alpha)
D_dX_WB = self.calc_objective_gradient_finite_difference(
dX_WB, X_WB_e, l_xy_e, X_WB_g, alpha)
c0 = result["c"]
print("\nIteration {}, cost \n".format(iter_count), result)
# line search
t = 1
line_search_count = 0
while True:
result = self.calc_objective(dX_WB - t * D_dX_WB, X_WB_e,
l_xy_e, X_WB_g, alpha)
c_t = result["c"]
if c_t < c0 - a * t * (D_dX_WB**
2).sum() or line_search_count > 10:
break
# print("count {}, c_t {}, ".format(line_search_count, c_t))
t *= b
line_search_count += 1
dX_WB -= t * D_dX_WB
print("Line search steps {}, gradient {}".format(
line_search_count, np.linalg.norm(D_dX_WB)))
print(result)
iter_count += 1
# termination
is_cost_reduction_small = abs((c_t - c0) / c_t) < epsilon
is_gradient_small = np.linalg.norm(D_dX_WB) < epsilon
if is_cost_reduction_small or is_gradient_small or iter_count > 200:
break
print("alpha = {}".format(alpha), self.is_uncertainty_reduction_only)
result["alpha"] = alpha
return dX_WB, result
@staticmethod
def convert_cost_from_torch_tensor_to_float(result):
result_no_torch = dict()
for key, value in result.items():
result_no_torch[key] = float(value)
return result_no_torch
def initialize_dX_WB_with_goal(self, X_WB_0, X_WB_g, L, max_step=0.2):
"""
:param X_WB_0: current robot pose.
:param X_WB_g: goal robot pose.
:param L: number of time steps.
:return:
"""
step = (X_WB_g - X_WB_0) / L
step_size = np.linalg.norm(step)
step /= step_size
step *= min(step_size, max_step)
dX_WB = np.zeros((L, X_WB_0.size))
dX_WB[:] = step
return dX_WB
def initialize_dX_WB_with_fixed_input(self, X_WB_0, dX_WB0, L):
"""
:param X_WB_0: current robot pose.
:param dX_WB0: fixed input.
:param L: number of time steps.
:return:
"""
dX_WB = np.zeros((L, X_WB_0.size))
dX_WB[:] = dX_WB0
return dX_WB
def run_bundle_adjustment(self, is_printing=False):
dim_l = self.dim_l # dimension of landmarks.
dim_X = self.dim_X # dimension of poses.
X_WB_e0 = self.get_X_WB_initial_guess_array()
l_xy_e0 = self.get_l_xy_initial_guess_array()
X_WB_e = X_WB_e0.copy()
l_xy_e = l_xy_e0.copy()
n_o = len(self.odometry_measurements)
n_p = n_o + 1 # number of poses.
n_l = len(self.landmark_measurements)
steps_counter = 0
while True:
I_k, q, c = self.calc_info_matrix(X_WB_e, l_xy_e)
prog = mp.MathematicalProgram()
dX_WB_e = prog.NewContinuousVariables(n_p, dim_X, "dX_WB_e")
dl_xy_e = prog.NewContinuousVariables(n_l, dim_l, "dl_xy_e")
dx = np.hstack((dX_WB_e.ravel(), dl_xy_e.ravel()))
nx = len(dx)
prog.AddQuadraticCost(I_k, q, 0.5 * c, dx)
prog.AddQuadraticCost(np.eye(nx) * 2, np.zeros(nx), dx)
result = self.solver.Solve(prog)
optimal_cost = result.get_optimal_cost()
assert result.get_solution_result() == \
mp.SolutionResult.kSolutionFound
dX_WB_e_values = result.GetSolution(dX_WB_e)
dl_xy_e_values = result.GetSolution(dl_xy_e)
dx_values = result.GetSolution(dx)
X_WB_e += dX_WB_e_values
l_xy_e += dl_xy_e_values
steps_counter += 1
if steps_counter > 500 or \
np.linalg.norm(dx_values) / (n_p + n_l) < 5e-3:
break
self.update_beliefs(X_WB_e, l_xy_e, I_k)
if is_printing:
print("\nStep ", n_o)
print("optimal cost: ", optimal_cost)
_, b, _ = self.calc_jacobian_and_b(X_WB_e, l_xy_e)
print("b norm recalculated: ", np.linalg.norm(b))
print("total gradient steps: ", steps_counter)
print("dx norm: ", np.linalg.norm(dx_values))
Omega_pose = self.marginalize_info_matrix(I_k, n_p, n_l)
print("Marginalized covariance diagonal\n",
np.linalg.inv(Omega_pose).diagonal())
Cov = np.linalg.inv(I_k)
print("Sqrt covariance trace\n", np.sqrt(Cov.diagonal().sum()))
print("\n")
return X_WB_e, l_xy_e
def update_beliefs(self, X_WB_e_values, l_xy_e_values, I_all):
Cov = np.linalg.inv(I_all)
for i, X_WB_e in enumerate(X_WB_e_values):
i0 = i * self.dim_X
i1 = i0 + self.dim_X
self.X_WB_e_dict[i] = X_WB_e
Cov_i = Cov[i0:i1, i0:i1]
e_values, e_vectors = np.linalg.eig(Cov_i)
X_WB_ellipsoid = np.eye(4)
X_WB_ellipsoid[:2, :2] = e_vectors
X_WB_ellipsoid[:2, 3] = X_WB_e
sigmas = np.sqrt(e_values)
self.X_WB_e_var_dict[i] = {
"sigmas": np.hstack([np.real(sigmas), 0.01]),
"transform": X_WB_ellipsoid,
"cov": Cov_i
}
self.l_xy_e_dict.clear()
for k, l_xy_e_value in zip(self.landmark_measurements.keys(),
l_xy_e_values):
self.l_xy_e_dict[k] = l_xy_e_value
self.I_all = I_all
def calc_sqrt_postion_cov_trace(self):
postion_cov_trace = 0.
for a in self.X_WB_e_var_dict.values():
postion_cov_trace += a["cov"].diagonal().sum()
return np.sqrt(postion_cov_trace)
def find_visible_landmarks(self, X_WB_p):
future_landmark_measurements = dict()
for i, X_WB in enumerate(X_WB_p):
for j in self.l_xy_e_dict.keys():
d_xy = self.l_xy_e_dict[j] - X_WB
d = np.linalg.norm(d_xy)
if self.r_range_min < d:
if j not in future_landmark_measurements:
future_landmark_measurements[j] = dict()
b = np.arctan2(d_xy[1], d_xy[0])
future_landmark_measurements[j][i] = {
"distance": d,
"bearing": b
}
return future_landmark_measurements
def draw_estimated_path(self):
n_p = len(self.X_WB_e_dict)
t_xy_e = np.zeros((n_p, 2))
for i in range(n_p):
t_xy_e[i] = self.X_WB_e_dict[i][:2]
self.draw_robot_path(t_xy_e,
color=[1, 0, 0],
prefix="robot_poses_e",
idx_segment=0,
size=0.2)
def draw_estimated_path_segment(self,
L: int,
idx_segment: int,
covariance_scale=1.):
n_p = len(self.X_WB_e_dict)
name = "robot_poses_e/{}".format(idx_segment)
material = meshcat.geometry.MeshLambertMaterial(color=0x778899,
opacity=0.5)
if L is None:
L = n_p
points = np.zeros((L, 3))
for i in range(L):
idx = n_p - L + i
# Draw covariance ellipses.
sigmas = self.X_WB_e_var_dict[idx]["sigmas"]
sigmas[:2] *= covariance_scale
self.vis[name]["points"][str(i)].set_object(
meshcat.geometry.Ellipsoid(sigmas), material)
self.vis[name]["points"][str(i)].set_transform(
self.X_WB_e_var_dict[idx]["transform"])
points[i, :2] = self.X_WB_e_dict[idx]
# Draw lines.
self.vis[name]["path"].set_object(
meshcat.geometry.Line(meshcat.geometry.PointsGeometry(points.T),
material))
def draw_estimated_landmarks(self):
nl = len(self.l_xy_e_dict)
l_xy_e = np.zeros((nl, 3))
for i, (k, l_xy_e_i) in enumerate(self.l_xy_e_dict.items()):
l_xy_e[i][:2] = l_xy_e_i
points = np.zeros((2, 3))
points[0, :2] = l_xy_e_i
points[1, :2] = self.l_xy[k]
self.vis["landmarks_e"]["corrspondances"][str(k)].set_object(
meshcat.geometry.Line(meshcat.geometry.PointsGeometry(
points.T)))
# draw landmark
prefix = "landmarks_e/points/{}".format(i)
self.frontend.draw_cylinder(prefix, 0.05, 0.11, 0x00ffff, l_xy_e_i)
def draw_robot_path(self,
X_WBs,
color,
prefix: str,
idx_segment: int,
size=0.2):
t_xy = X_WBs[:, :2]
for i in range(1, len(t_xy)):
name = prefix + "/{}/points/{}".format(idx_segment, i)
self.frontend.draw_box(name, [size, size, 0.15], color, t_xy[i])
points = np.zeros((2, 3))
points[0, :2] = t_xy[i - 1]
points[1, :2] = t_xy[i]
self.vis[prefix][str(idx_segment)]["path"][str(i)].set_object(
meshcat.geometry.Line(meshcat.geometry.PointsGeometry(
points.T)))
def quadratic_program(H, f, A, b, C=None, d=None, tol=1.e-5, **kwargs):
"""
Solves the strictly convex (H > 0) quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d.
Arguments
----------
H : numpy.ndarray
Positive definite Hessian of the cost function.
f : numpy.ndarray
Gradient of the cost function.
A : numpy.ndarray
Left-hand side of the inequality constraints.
b : numpy.ndarray
Right-hand side of the inequality constraints.
C : numpy.ndarray
Left-hand side of the equality constraints.
d : numpy.ndarray
Right-hand side of the equality constraints.
tol : float
Maximum value of a residual of an inequality to consider the related constraint active.
Returns
----------
sol : dict
Dictionary with the solution of the QP.
Fields
----------
min : float
Minimum of the QP (None if the problem is unfeasible).
argmin : numpy.ndarray
Argument that minimizes the QP (None if the problem is unfeasible).
active_set : list of int
Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible).
multiplier_inequality : numpy.ndarray
Lagrange multipliers for the inequality constraints (None if the problem is unfeasible).
multiplier_equality : numpy.ndarray
Lagrange multipliers for the equality constraints (None if the problem is unfeasible or without equality constraints).
"""
# check equalities
if (C is None) != (d is None):
raise ValueError('missing C or d.')
# problem size
n_ineq, n_x = A.shape
if C is not None:
n_eq = C.shape[0]
else:
n_eq = 0
# build program
prog = MathematicalProgram()
x = prog.NewContinuousVariables(n_x)
[prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)]
[prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)]
inequalities = []
prog.AddQuadraticCost(.5*x.dot(H).dot(x) + f.dot(x))
# solve
solver = GurobiSolver()
prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0)
[prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()]
result = prog.Solve()
# initialize output
sol = {
'min': None,
'argmin': None,
'active_set': None,
'multiplier_inequality': None,
'multiplier_equality': None
}
if result == SolutionResult.kSolutionFound:
sol['argmin'] = prog.GetSolution(x)
sol['min'] = .5*sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin'])
sol['active_set'] = np.where(A.dot(sol['argmin']) - b > -tol)[0].tolist()
# retrieve multipliers through KKT conditions
lhs = A[sol['active_set']]
rhs = b[sol['active_set']]
if n_eq > 0:
lhs = np.vstack((lhs, C))
rhs = np.concatenate((rhs, d))
H_inv = np.linalg.inv(H)
M = lhs.dot(H_inv).dot(lhs.T)
m = - np.linalg.inv(M).dot(lhs.dot(H_inv).dot(f) + rhs)
sol['multiplier_inequality'] = np.zeros(n_ineq)
for i, j in enumerate(sol['active_set']):
sol['multiplier_inequality'][j] = m[i]
if n_eq > 0:
sol['multiplier_equality'] = m[len(sol['active_set']):]
return sol
z = prog.NewBinaryVariables(num_regions, "z") # Integer variables that represent the region the point will be in
prog.AddLinearConstraint(np.sum(z) == 1) # only one is set
# 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] + M*z[i] <= b[i][j] + M)
# Add objective
prog.AddQuadraticCost((x[0]-x_goal[0])**2 + (x[1]-x_goal[1])**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))
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):
class TestMathematicalProgram(unittest.TestCase):
def test_program_construction(self):
prog = mp.MathematicalProgram()
vars = prog.NewContinuousVariables(5, "x")
self.assertEqual(vars.dtype, sym.Variable)
vars_all = prog.decision_variables()
self.assertEqual(vars_all.shape, (5, ))
def test_program_attributes_and_solver_selection(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
# Add linear equality constraints; make sure the solver works.
prog.AddLinearConstraint(x[0] + x[1] == 0)
prog.AddLinearConstraint(2 * x[0] - x[1] == 1)
solver_id = mp.ChooseBestSolver(prog)
self.assertEqual(solver_id.name(), "Linear system")
solver = mp.MakeSolver(solver_id)
self.assertEqual(solver.solver_id().name(), "Linear system")
self.assertTrue(solver.AreProgramAttributesSatisfied(prog))
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
# With an inequality constraint added, the "Linear system" solver
# doesn't work anymore.
prog.AddLinearConstraint(x[0] >= 0)
self.assertFalse(solver.AreProgramAttributesSatisfied(prog))
with self.assertRaises(ValueError):
solver.Solve(prog, None, None)
# A different solver will work, though. We re-use the result object
# (as a mutable output argument), and make sure that it changes.
solver_id = mp.ChooseBestSolver(prog)
self.assertNotEqual(solver_id.name(), "Linear system")
solver = mp.MakeSolver(solver_id)
solver.Solve(prog, None, None, result)
self.assertTrue(result.is_success())
self.assertEqual(result.get_solver_id().name(), solver_id.name())
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])))
# TODO(jwnimmer-tri) MOSEK is also able to solve mixed integer programs;
# perhaps we should test both of them?
@unittest.skipUnless(GurobiSolver().available(), "Requires Gurobi")
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)
prog.AddConstraint(
LinearConstraint(np.array([[1., 1.]]), np.array([1]),
np.array([np.inf])), [x[0], x[1]])
solver = GurobiSolver()
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
# 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(result.GetSolution(x), x_expected)))
# Also test by asking for the value of each element of x
for i in range(3):
self.assertAlmostEqual(result.GetSolution(x[i]), x_expected[i])
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)
prog.AddQuadraticCost(np.eye(2), np.zeros(2), 1, 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_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_bindings(self):
qp = TestQP()
prog = qp.prog
x = qp.x
self.assertEqual(prog.FindDecisionVariableIndices(vars=[x[0], x[1]]),
[0, 1])
self.assertEqual(prog.decision_variable_index()[x[0].get_id()], 0)
self.assertEqual(prog.decision_variable_index()[x[1].get_id()], 1)
for binding in prog.GetAllCosts():
self.assertIsInstance(binding.evaluator(), mp.Cost)
for binding in prog.GetLinearConstraints():
self.assertIsInstance(binding.evaluator(), mp.Constraint)
for binding in prog.GetAllConstraints():
self.assertIsInstance(binding.evaluator(), mp.Constraint)
self.assertTrue(prog.linear_costs())
for (i, binding) in enumerate(prog.linear_costs()):
cost = binding.evaluator()
self.assertTrue(np.allclose(cost.a(), np.ones((1, 2))))
self.assertIsNone(cost.gradient_sparsity_pattern())
self.assertTrue(prog.quadratic_costs())
for (i, binding) in enumerate(prog.quadratic_costs()):
cost = binding.evaluator()
self.assertTrue(np.allclose(cost.Q(), np.eye(2)))
self.assertTrue(np.allclose(cost.b(), np.zeros(2)))
self.assertIsNone(cost.gradient_sparsity_pattern())
self.assertTrue(prog.bounding_box_constraints())
for (i, binding) in enumerate(prog.bounding_box_constraints()):
constraint = binding.evaluator()
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[0]),
prog.FindDecisionVariableIndex(var=x[i]))
self.assertIsNone(constraint.gradient_sparsity_pattern())
num_constraints = constraint.num_constraints()
if num_constraints == 1:
self.assertEqual(constraint.A(), 1)
self.assertEqual(constraint.lower_bound(), 1)
self.assertEqual(constraint.upper_bound(), np.inf)
else:
self.assertTrue(np.allclose(constraint.A(), np.eye(2)))
self.assertTrue(
np.allclose(constraint.lower_bound(), [1, -np.inf]))
self.assertTrue(
np.allclose(constraint.upper_bound(), [np.inf, 2]))
self.assertTrue(prog.linear_constraints())
for (i, binding) in enumerate(prog.linear_constraints()):
constraint = binding.evaluator()
self.assertIsNone(constraint.gradient_sparsity_pattern())
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[0]),
prog.FindDecisionVariableIndex(var=x[0]))
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[1]),
prog.FindDecisionVariableIndex(var=x[1]))
self.assertTrue(np.allclose(constraint.A(), [3, -1]))
self.assertTrue(constraint.lower_bound(), -2)
self.assertTrue(constraint.upper_bound(), np.inf)
self.assertTrue(prog.linear_equality_constraints())
for (i, binding) in enumerate(prog.linear_equality_constraints()):
self.assertIsNone(constraint.gradient_sparsity_pattern())
constraint = binding.evaluator()
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[0]),
prog.FindDecisionVariableIndex(var=x[0]))
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[1]),
prog.FindDecisionVariableIndex(var=x[1]))
self.assertTrue(np.allclose(constraint.A(), [1, 2]))
self.assertTrue(constraint.lower_bound(), 3)
self.assertTrue(constraint.upper_bound(), 3)
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_constraint_api(self):
prog = mp.MathematicalProgram()
x0, = prog.NewContinuousVariables(1, "x")
c = prog.AddLinearConstraint(x0 >= 2).evaluator()
ce = prog.AddLinearEqualityConstraint(2 * x0, 1).evaluator()
self.assertTrue(c.CheckSatisfied([2.], tol=1e-3))
self.assertFalse(c.CheckSatisfied([AutoDiffXd(1.)]))
self.assertIsInstance(c.CheckSatisfied([x0]), sym.Formula)
ce.set_description("my favorite constraint")
self.assertEqual(ce.get_description(), "my favorite constraint")
def check_bounds(c, A, lb, ub):
self.assertTrue(np.allclose(c.A(), A))
self.assertTrue(np.allclose(c.lower_bound(), lb))
self.assertTrue(np.allclose(c.upper_bound(), ub))
check_bounds(c, [1.], [2.], [np.inf])
c.UpdateLowerBound([3.])
check_bounds(c, [1.], [3.], [np.inf])
c.UpdateUpperBound([4.])
check_bounds(c, [1.], [3.], [4.])
c.set_bounds([-10.], [10.])
check_bounds(c, [1.], [-10.], [10.])
c.UpdateCoefficients([10.], [-20.], [-30.])
check_bounds(c, [10.], [-20.], [-30.])
check_bounds(ce, [2.], [1.], [1.])
ce.UpdateCoefficients([10.], [20.])
check_bounds(ce, [10.], [20.], [20.])
def test_cost_api(self):
prog = mp.MathematicalProgram()
x0, = prog.NewContinuousVariables(1, "x")
lc = prog.AddLinearCost([1], 2, [x0]).evaluator()
qc = prog.AddQuadraticCost(0.5 * x0**2 + 2 * x0 + 3).evaluator()
def check_linear_cost(cost, a, b):
self.assertTrue(np.allclose(cost.a(), a))
self.assertTrue(np.allclose(cost.b(), b))
check_linear_cost(lc, [1.], 2.)
lc.UpdateCoefficients([10.])
check_linear_cost(lc, [10.], 0.)
def check_quadratic_cost(cost, Q, b, c):
self.assertTrue(np.allclose(cost.Q(), Q))
self.assertTrue(np.allclose(cost.b(), b))
self.assertTrue(np.allclose(cost.c(), c))
check_quadratic_cost(qc, [1.], [2.], 3.)
qc.UpdateCoefficients([10.], [20.])
check_quadratic_cost(qc, [10.], [20.], 0)
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))
enum = zip(costs, cost_values_expected)
for (cost, value_expected) in enum:
value = result.EvalBinding(cost)
self.assertTrue(np.allclose(value, value_expected))
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_get_binding_variable_values(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(3)
binding1 = prog.AddBoundingBoxConstraint(-1, 1, x[0])
binding2 = prog.AddLinearEqualityConstraint(x[1] + 2 * x[2], 2)
x_val = np.array([-2., 1., 2.])
np.testing.assert_allclose(
prog.GetBindingVariableValues(binding1, x_val), np.array([-2]))
np.testing.assert_allclose(
prog.GetBindingVariableValues(binding2, x_val), np.array([1, 2]))
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_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")
self.assertEqual(prog.indeterminates_index()[x[0].get_id()], 0)
poly = prog.NewFreePolynomial(sym.Variables(x), 1)
(poly,
binding) = prog.NewSosPolynomial(indeterminates=sym.Variables(x),
degree=2)
y = prog.NewIndeterminates(1, "y")
self.assertEqual(prog.indeterminates_index()[y[0].get_id()], 1)
(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_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_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_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 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_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_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_constraint_gradient_sparsity(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
def cost(x):
return x[0]**2
def constraint(x):
return x[1]**2
cost_binding = prog.AddCost(cost, vars=x)
constraint_binding = prog.AddConstraint(constraint, [0], [1], vars=x)
cost_evaluator = cost_binding.evaluator()
constraint_evaluator = constraint_binding.evaluator()
self.assertIsNone(cost_evaluator.gradient_sparsity_pattern())
self.assertIsNone(constraint_evaluator.gradient_sparsity_pattern())
# Now set the sparsity
cost_evaluator.SetGradientSparsityPattern([(0, 0)])
self.assertEqual(cost_evaluator.gradient_sparsity_pattern(), [(0, 0)])
constraint_binding.evaluator().SetGradientSparsityPattern([(0, 1)])
self.assertEqual(constraint_evaluator.gradient_sparsity_pattern(),
[(0, 1)])
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 test_addcost_symbolic(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1, 'x')
prog.AddCost((x[0] - 1.)**2)
prog.AddConstraint(0 <= x[0])
prog.AddConstraint(x[0] <= 2)
result = mp.Solve(prog)
self.assertAlmostEqual(result.GetSolution(x)[0], 1.)
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)
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]))
@unittest.skipIf(SNOPT_NO_GUROBI,
"SNOPT is unable to solve this problem (#10653).")
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_solver_options(self):
prog = mp.MathematicalProgram()
prog.SetSolverOption(SolverType.kGurobi, "double_key", 1.0)
prog.SetSolverOption(GurobiSolver().solver_id(), "int_key", 2)
prog.SetSolverOption(SolverType.kGurobi, "string_key", "3")
options = prog.GetSolverOptions(SolverType.kGurobi)
self.assertDictEqual(options, {
"double_key": 1.0,
"int_key": 2,
"string_key": "3"
})
options = prog.GetSolverOptions(GurobiSolver().solver_id())
self.assertDictEqual(options, {
"double_key": 1.0,
"int_key": 2,
"string_key": "3"
})
# For now, just make sure the constructor exists. Once we bind more
# accessors, we can test them here.
options_object = SolverOptions()
solver_id = SolverId("dummy")
self.assertEqual(solver_id.name(), "dummy")
options_object.SetOption(solver_id, "double_key", 1.0)
options_object.SetOption(solver_id, "int_key", 2)
options_object.SetOption(solver_id, "string_key", "3")
options = options_object.GetOptions(solver_id)
self.assertDictEqual(options, {
"double_key": 1.0,
"int_key": 2,
"string_key": "3"
})
prog.SetSolverOptions(options_object)
prog_options = prog.GetSolverOptions(solver_id)
self.assertDictEqual(prog_options, {
"double_key": 1.0,
"int_key": 2,
"string_key": "3"
})
def test_infeasible_constraints(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1)
result = mp.Solve(prog)
infeasible = mp.GetInfeasibleConstraints(prog=prog,
result=result,
tol=1e-4)
self.assertEquals(len(infeasible), 0)
def test_add_indeterminates_and_decision_variables(self):
prog = mp.MathematicalProgram()
x0 = sym.Variable("x0")
x1 = sym.Variable("x1")
a0 = sym.Variable("a0")
a1 = sym.Variable("a1")
prog.AddIndeterminates(np.array([x0, x1]))
prog.AddDecisionVariables(np.array([a0, a1]))
numpy_compare.assert_equal(prog.decision_variables()[0], a0)
numpy_compare.assert_equal(prog.decision_variables()[1], a1)
numpy_compare.assert_equal(prog.indeterminates()[0], x0)
numpy_compare.assert_equal(prog.indeterminate(1), x1)
def solveProgram(self):
if (not self.upToDate):
prog = mp.MathematicalProgram()
M = 100
# SET UP FOOTSTEP VARIABLES
f = []
for fNum in range(FootstepPlanner.MAXFOOTSTEPS):
fnR = prog.NewContinuousVariables(self.dim, "fr" +
str(fNum)) # Right Footstep
fnL = prog.NewContinuousVariables(self.dim, "fl" +
str(fNum)) # Left Footstep
f.append(fnR)
f.append(fnL)
n = prog.NewBinaryVariables(
2 * FootstepPlanner.MAXFOOTSTEPS,
"n") # binary variables for nominal regions
# CONSTRAIN WITH REACHABLE REGIONS
# Start position
prog.AddLinearConstraint(f[0][0] == self.startR[0])
prog.AddLinearConstraint(f[0][1] == self.startR[1])
prog.AddLinearConstraint(f[1][0] == self.startL[0])
prog.AddLinearConstraint(f[1][1] == self.startL[1])
# All other footsteps
for fNum in range(1, FootstepPlanner.MAXFOOTSTEPS):
for i in range(self.reachableA.shape[0]):
# Constrain footsteps
prog.AddLinearConstraint(
self.reachableA[i][0] *
(f[2 * fNum][0] - f[2 * fNum - 1][0]) +
self.reachableA[i][1] *
(f[2 * fNum][1] - (f[2 * fNum - 1][1] - self.yOffset))
<= self.reachableb[i]
) # Right Footstep (2*fNum) to previous left (2*fNum-1)
prog.AddLinearConstraint(
self.reachableA[i][0] *
(f[2 * fNum + 1][0] - f[2 * fNum][0]) +
self.reachableA[i][1] *
(f[2 * fNum + 1][1] -
(f[2 * fNum][1] + self.yOffset)) <= self.reachableb[i]
) # Left Footstep (2*fNum+1) to previous right (2*fNum)
if (self.hasNominal): # Nominal Regions
prog.AddLinearConstraint(
self.reachableA[i][0] *
(f[2 * fNum][0] - f[2 * fNum - 1][0]) +
self.reachableA[i][1] *
(f[2 * fNum][1] -
(f[2 * fNum - 1][1] - self.yOffset)) +
n[2 * fNum] * M <=
self.nominal * self.reachableb[i] + M
) # Right Footstep (2*fNum) to previous left (2*fNum-1)
prog.AddLinearConstraint(
self.reachableA[i][0] *
(f[2 * fNum + 1][0] - f[2 * fNum][0]) +
self.reachableA[i][1] *
(f[2 * fNum + 1][1] -
(f[2 * fNum][1] + self.yOffset)) +
n[2 * fNum + 1] * M <=
self.nominal * self.reachableb[i] + M
) # Left Footstep (2*fNum+1) to previous right (2*fNum)
# CONSTRAIN TO OBSTACLE FREE REGIONS
if (self.hasObstacleFree):
H = []
for fNum in range(1, FootstepPlanner.MAXFOOTSTEPS):
hnR = prog.NewBinaryVariables(self.num_regions, "hr" +
str(fNum)) # Right Footstep
hnL = prog.NewBinaryVariables(self.num_regions, "hl" +
str(fNum)) # Left Footstep
# Constrain each footstep to exactly one convex region
prog.AddLinearConstraint(
np.sum(hnR) == 1) # only one is set
prog.AddLinearConstraint(
np.sum(hnL) == 1) # only one is set
H.append(hnR)
H.append(hnL)
# Constrain the footsteps to the regions
for fNum in range(1, FootstepPlanner.MAXFOOTSTEPS - 1):
for i in range(self.num_regions):
for j in range(self.oA[i].shape[0]):
prog.AddLinearConstraint(
self.oA[i][j][0] * f[2 * fNum][0] +
self.oA[i][j][1] * f[2 * fNum][1] +
M * H[2 * fNum][i] <=
self.ob[i][j] + M) # Right footstep constraint
prog.AddLinearConstraint(
self.oA[i][j][0] * f[2 * fNum + 1][0] +
self.oA[i][j][1] * f[2 * fNum + 1][1] +
M * H[2 * fNum + 1][i] <=
self.ob[i][j] + M) # Left footstep constraint
# OPTIMAL NUMBER OF FOOTSTEPS
z = prog.NewBinaryVariables(FootstepPlanner.MAXFOOTSTEPS, "z")
for fNum in range(FootstepPlanner.MAXFOOTSTEPS - 1):
prog.AddLinearConstraint(z[fNum] <= z[fNum + 1])
prog.AddLinearConstraint(z[FootstepPlanner.MAXFOOTSTEPS - 1] -
z[0] == 1)
for fNum in range(
FootstepPlanner.MAXFOOTSTEPS
): # if z[i], then the ith footstep is the same as the final footstep
prog.AddLinearConstraint(
f[2 * fNum][0] +
M * z[fNum] <= f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][0] +
M)
prog.AddLinearConstraint(
f[2 * fNum][1] +
M * z[fNum] <= f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][1] +
M)
prog.AddLinearConstraint(
-f[2 * fNum][0] + M *
z[fNum] <= -f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][0] + M)
prog.AddLinearConstraint(
-f[2 * fNum][1] + M *
z[fNum] <= -f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][1] + M)
prog.AddLinearConstraint(
f[2 * fNum + 1][0] +
M * z[fNum] <= f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][0] +
M)
prog.AddLinearConstraint(
f[2 * fNum + 1][1] +
M * z[fNum] <= f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][1] +
M)
prog.AddLinearConstraint(
-f[2 * fNum + 1][0] + M *
z[fNum] <= -f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][0] + M)
prog.AddLinearConstraint(
-f[2 * fNum + 1][1] + M *
z[fNum] <= -f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][1] + M)
# ADD COSTS
# Cost of consecutive footsteps (with nominal regions considered)
for fNum in range(1, 2 * FootstepPlanner.MAXFOOTSTEPS - 1):
prog.AddQuadraticCost(((f[fNum][0] - f[fNum + 1][0])**2 +
(f[fNum][1] - f[fNum + 1][1])**2) + 2 *
(1 - n[fNum]))
# Cost of number of footsteps
prog.AddLinearCost(-np.sum(z) * 5)
# Cost of distance of final position to goal
prog.AddQuadraticCost(
1 *
((f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][0] - self.goal[0])**2
+ (f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][1] - self.goal[1])
**2))
prog.AddQuadraticCost(
1 *
((f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][0] - self.goal[0])**2
+ (f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][1] - self.goal[1])
**2))
# SOLVE PROBLEM
solver = GurobiSolver()
assert (solver.available())
assert (solver.solver_type() == mp.SolverType.kGurobi)
result = solver.Solve(prog)
assert (result == mp.SolutionResult.kSolutionFound)
# SAVE SOLUTION
self.footsteps = []
self.numFootsteps = 0
for fNum in range(FootstepPlanner.MAXFOOTSTEPS):
self.numFootsteps += 1
self.footsteps.append(prog.GetSolution(f[2 * fNum]))
self.footsteps.append(prog.GetSolution(f[2 * fNum + 1]))
if (prog.GetSolution(z[fNum]) == 1):
break
self.footsteps = np.array(self.footsteps)
self.upToDate = True
