Provide GetInfeasibleConstraints (RobotLocomotion#11103)

This is a long-missed feature by former developers of mathematical programs in the Matlab versions of drake.
Resolves RobotLocomotion#3130

bindings/pydrake/solvers/mathematicalprogram_py.cc

@@ -838,7 +838,11 @@ PYBIND11_MODULE(mathematicalprogram, m) {
     py::class_<Class, std::shared_ptr<EvaluatorBase>> cls(m, "EvaluatorBase");
     cls  // BR
         .def("num_outputs", &Class::num_outputs, cls_doc.num_outputs.doc)
-        .def("num_vars", &Class::num_vars, cls_doc.num_vars.doc);
+        .def("num_vars", &Class::num_vars, cls_doc.num_vars.doc)
+        .def("get_description", &Class::get_description,
+            cls_doc.get_description.doc)
+        .def("set_description", &Class::set_description,
+            cls_doc.set_description.doc);
     auto bind_eval = [&cls, &cls_doc](auto dummy_x, auto dummy_y) {
       using T_x = decltype(dummy_x);
       using T_y = decltype(dummy_y);
@@ -1002,7 +1006,10 @@ PYBIND11_MODULE(mathematicalprogram, m) {
               const optional<Eigen::VectorXd>&, const optional<SolverOptions>&>(
               &solvers::Solve),
           py::arg("prog"), py::arg("initial_guess") = py::none(),
-          py::arg("solver_options") = py::none(), doc.Solve.doc_3args);
+          py::arg("solver_options") = py::none(), doc.Solve.doc_3args)
+      .def("GetInfeasibleConstraints", &solvers::GetInfeasibleConstraints,
+          py::arg("prog"), py::arg("result"), py::arg("tol") = nullopt,
+          doc.GetInfeasibleConstraints.doc);
 }  // NOLINT(readability/fn_size)
 
 }  // namespace pydrake

bindings/pydrake/solvers/test/mathematicalprogram_test.py

@@ -248,6 +248,9 @@ def test_constraint_api(self):
         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))
@@ -557,3 +560,11 @@ def test_solver_options(self):
         # For now, just make sure the constructor exists.  Once we bind more
         # accessors, we can test them here.
         options_object = SolverOptions()
+
+    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)

solvers/BUILD.bazel

@@ -603,6 +603,7 @@ drake_cc_library(
     deps = [
         ":choose_best_solver",
         ":mathematical_program_lite",
+        "//common:nice_type_name",
     ],
 )
 

solvers/solve.cc

@@ -2,6 +2,7 @@
 
 #include <memory>
 
+#include "drake/common/nice_type_name.h"
 #include "drake/solvers/choose_best_solver.h"
 #include "drake/solvers/solver_interface.h"
 
@@ -27,5 +28,38 @@ MathematicalProgramResult Solve(
 MathematicalProgramResult Solve(const MathematicalProgram& prog) {
   return Solve(prog, {}, {});
 }
+
+std::vector<std::string> GetInfeasibleConstraints(
+    const MathematicalProgram& prog, const MathematicalProgramResult&
+    result, optional<double> tolerance) {
+  std::vector<std::string> descriptions;
+
+  if (!tolerance) {
+    // TODO(russt): Extract the constraint tolerance from the solver.  This
+    // value was used successfully for some time in MATLAB Drake, so I've
+    // ported it as the default here.
+    tolerance = 1e-4;
+  }
+
+  for (const auto& binding : prog.GetAllConstraints()) {
+    const Eigen::VectorXd val = result.EvalBinding(binding);
+    const std::shared_ptr<Constraint>& constraint = binding.evaluator();
+    std::string d = constraint->get_description();
+    if (d.empty()) {
+      d = NiceTypeName::Get(*constraint);
+    }
+    for (int i = 0; i < val.rows(); i++) {
+      if (val[i] < constraint->lower_bound()[i] - *tolerance ||
+          val[i] > constraint->upper_bound()[i] + *tolerance) {
+        descriptions.push_back(
+            d + "[" + std::to_string(i) +
+            "]: " + std::to_string(constraint->lower_bound()[i]) +
+            " <= " + std::to_string(val[i]) +
+            " <= " + std::to_string(constraint->upper_bound()[i]));
+      }
+    }
+  }
+  return descriptions;
+}
 }  // namespace solvers
 }  // namespace drake

solvers/solve.h

@@ -1,5 +1,8 @@
 #pragma once
 
+#include <string>
+#include <vector>
+
 #include "drake/solvers/mathematical_program.h"
 
 namespace drake {
@@ -28,5 +31,28 @@ MathematicalProgramResult Solve(
     const Eigen::Ref<const Eigen::VectorXd>& initial_guess);
 
 MathematicalProgramResult Solve(const MathematicalProgram& prog);
+
+/** Some solvers (e.g. SNOPT) provide a "best-effort solution" even when they
+ * determine that a problem is infeasible.  This method will return the
+ * descriptions corresponding to the constraints for which `CheckSatisfied`
+ * evaluates to false given the reported solution.  This can be very useful
+ * for debugging.
+ *
+ * @param prog A MathematicalProgram
+ * @param result A MathematicalProgramResult obtained by solving @p prog.
+ * @param tolerance A positive tolerance to check the constraint violation.
+ * If no tolerance is provided, this method will attempt to obtain the
+ * constraint tolerance from the solver, or insert a conservative default
+ * tolerance.
+ *
+ * Note: Currently most constraints have the empty string as the
+ * description, so the NiceTypeName of the Constraint is used instead.  Use
+ * e.g.
+ * `prog.AddConstraint(x == 1).evaluator().set_description(str)`
+ * to make this method more specific/useful. */
+std::vector<std::string> GetInfeasibleConstraints(
+    const MathematicalProgram& prog, const MathematicalProgramResult& result,
+    optional<double> tolerance = nullopt);
+
 }  // namespace solvers
 }  // namespace drake

solvers/test/solve_test.cc

@@ -1,11 +1,14 @@
 #include "drake/solvers/solve.h"
 
+#include <regex>
+
 #include <gtest/gtest.h>
 
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/solvers/choose_best_solver.h"
 #include "drake/solvers/gurobi_solver.h"
 #include "drake/solvers/linear_system_solver.h"
+#include "drake/solvers/snopt_solver.h"
 
 namespace drake {
 namespace solvers {
@@ -50,5 +53,38 @@ GTEST_TEST(SolveTest, TestInitialGuessAndOptions) {
   }
 }
 
+GTEST_TEST(SolveTest, GetInfeasibleConstraints) {
+  if (SnoptSolver::is_available()) {
+    MathematicalProgram prog;
+    auto x = prog.NewContinuousVariables<1>();
+    auto b0 = prog.AddBoundingBoxConstraint(0, 0, x);
+    auto b1 = prog.AddBoundingBoxConstraint(1, 1, x);
+
+    SnoptSolver solver;
+    MathematicalProgramResult result = solver.Solve(prog, {}, {});
+    EXPECT_FALSE(result.is_success());
+
+    std::vector<std::string> infeasible =
+        GetInfeasibleConstraints(prog, result);
+    EXPECT_EQ(infeasible.size(), 1);
+
+    // If no description is set, we should see the NiceTypeName of the
+    // Constraint.
+    auto matcher = [](const std::string& s, const std::string& re) {
+      return std::regex_match(s, std::regex(re));
+    };
+    EXPECT_PRED2(matcher, infeasible[0],
+                 "drake::solvers::BoundingBoxConstraint.*");
+
+    // If a description for the constraint has been set, then that description
+    // should be returned instead. There is no reason a priori for b0 or b1 to
+    // be the infeasible one, so set both descriptions.
+    b0.evaluator()->set_description("Test");
+    b1.evaluator()->set_description("Test");
+    infeasible = GetInfeasibleConstraints(prog, result);
+    EXPECT_PRED2(matcher, infeasible[0], "Test.*");
+  }
+}
+
 }  // namespace solvers
 }  // namespace drake