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))