def test_qp(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
# N.B. Scalar-wise logical ops work for Expression, but array ops need
# the workaround overloads from `pydrake.math`.
prog.AddLinearConstraint(ge(x, 1))
prog.AddQuadraticCost(np.eye(2), np.zeros(2), x)
# Redundant cost just to check the spelling.
prog.AddQuadraticErrorCost(vars=x, Q=np.eye(2), x_desired=np.zeros(2))
prog.AddL2NormCost(A=np.eye(2), b=np.zeros(2), vars=x)
result = mp.Solve(prog)
self.assertTrue(result.is_success())
x_expected = np.array([1, 1])
self.assertTrue(np.allclose(result.GetSolution(x), x_expected))
def test_qp(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
# N.B. Scalar-wise logical ops work for Expression, but array ops need
# the workaround overloads from `pydrake.math`.
prog.AddLinearConstraint(ge(x, 1))
prog.AddQuadraticCost(np.eye(2), np.zeros(2), x)
# Redundant cost just to check the spelling.
prog.AddQuadraticErrorCost(vars=x, Q=np.eye(2),
x_desired=np.zeros(2))
prog.AddL2NormCost(A=np.eye(2), b=np.zeros(2), vars=x)
result = mp.Solve(prog)
self.assertTrue(result.is_success())
x_expected = np.array([1, 1])
self.assertTrue(np.allclose(result.GetSolution(x), x_expected))
