MathematicalProgram reports optimal cost when the problem is infeasib…

…le or unbounded. (RobotLocomotion#7688)

* Set the optimal cost appropriately when the problem is infeasible or unbounded.

* test nlopt and ipopt.

* Bug fix.

* Address Sean's comments.

* Use constexpr instead of const

* lint

solvers/BUILD.bazel

@@ -897,6 +897,21 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "nlopt_solver_test",
+    srcs = [
+        "test/nlopt_solver_test.cc",
+    ],
+    deps = [
+        ":linear_program_examples",
+        ":mathematical_program",
+        ":mathematical_program_test_util",
+        ":optimization_examples",
+        ":quadratic_program_examples",
+        "//drake/common/test_utilities:eigen_matrix_compare",
+    ],
+)
+
 drake_cc_googletest(
     name = "non_convex_optimization_util_test",
     srcs = [

solvers/equality_constrained_qp_solver.cc

@@ -247,12 +247,22 @@ SolutionResult EqualityConstrainedQPSolver::Solve(
 
   prog.SetDecisionVariableValues(x);
   double optimal_cost{};
-  if (solver_result == SolutionResult::kSolutionFound) {
-    optimal_cost = 0.5 * x.dot(G * x) + c.dot(x) + constant_term;
-  } else if (solver_result == SolutionResult::kUnbounded) {
-    optimal_cost = -std::numeric_limits<double>::infinity();
-  } else {
-    optimal_cost = NAN;
+  switch (solver_result.value()) {
+    case SolutionResult::kSolutionFound: {
+      optimal_cost = 0.5 * x.dot(G * x) + c.dot(x) + constant_term;
+      break;
+    }
+    case SolutionResult::kUnbounded: {
+      optimal_cost = MathematicalProgram::kUnboundedCost;
+      break;
+    }
+    case SolutionResult::kInfeasibleConstraints: {
+      optimal_cost = MathematicalProgram::kGlobalInfeasibleCost;
+      break;
+    }
+    default: {
+      optimal_cost = NAN;
+    }
   }
   prog.SetOptimalCost(optimal_cost);
   prog.SetSolverId(id());

solvers/gurobi_solver.cc

@@ -779,10 +779,12 @@ SolutionResult GurobiSolver::Solve(MathematicalProgram& prog) const {
           break;
         }
         case GRB_UNBOUNDED: {
+          prog.SetOptimalCost(MathematicalProgram::kUnboundedCost);
           result = SolutionResult::kUnbounded;
           break;
         }
         case GRB_INFEASIBLE: {
+          prog.SetOptimalCost(MathematicalProgram::kGlobalInfeasibleCost);
           result = SolutionResult::kInfeasibleConstraints;
           break;
         }

solvers/ipopt_solver.cc

@@ -397,6 +397,11 @@ class IpoptSolver_NLP : public Ipopt::TNLP {
         result_ = SolutionResult::kInfeasibleConstraints;
         break;
       }
+      case Ipopt::DIVERGING_ITERATES: {
+        result_ = SolutionResult::kUnbounded;
+        obj_value = MathematicalProgram::kUnboundedCost;
+        break;
+      }
       case Ipopt::MAXITER_EXCEEDED: {
         result_ = SolutionResult::kIterationLimit;
         drake::log()->warn(

solvers/linear_system_solver.cc

@@ -1,6 +1,7 @@
 #include "drake/solvers/linear_system_solver.h"
 
 #include <cstring>
+#include <limits>
 #include <memory>
 #include <vector>
 
@@ -49,12 +50,13 @@ SolutionResult LinearSystemSolver::Solve(MathematicalProgram& prog) const {
   const Eigen::VectorXd least_square_sol =
       Aeq.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(beq);
   prog.SetDecisionVariableValues(least_square_sol);
-  prog.SetOptimalCost(0.);
 
   prog.SetSolverId(id());
   if (beq.isApprox(Aeq * least_square_sol)) {
+    prog.SetOptimalCost(0.);
     return SolutionResult::kSolutionFound;
   } else {
+    prog.SetOptimalCost(MathematicalProgram::kGlobalInfeasibleCost);
     return SolutionResult::kInfeasibleConstraints;
   }
 }

solvers/mathematical_program.cc

@@ -115,6 +115,9 @@ enum {
 // to int so it is odr-used (see
 // https://gcc.gnu.org/wiki/VerboseDiagnostics#missing_static_const_definition)
 
+constexpr double MathematicalProgram::kGlobalInfeasibleCost;
+constexpr double MathematicalProgram::kUnboundedCost;
+
 MathematicalProgram::MathematicalProgram()
     : x_initial_guess_(
           static_cast<Eigen::Index>(INITIAL_VARIABLE_ALLOCATION_NUM)),

solvers/mathematical_program.h

@@ -290,6 +290,13 @@ class MathematicalProgram {
 
   using VarType = symbolic::Variable::Type;
 
+  /// The optimal cost is +∞ when the problem is globally infeasible.
+  static constexpr double kGlobalInfeasibleCost =
+    std::numeric_limits<double>::infinity();
+  /// The optimal cost is -∞ when the problem is unbounded.
+  static constexpr double kUnboundedCost =
+    -std::numeric_limits<double>::infinity();
+
   MathematicalProgram();
   virtual ~MathematicalProgram() {}
 
@@ -2016,8 +2023,14 @@ class MathematicalProgram {
   optional<SolverId> GetSolverId() const { return solver_id_; }
 
   /**
-   * Getter for optimal cost at the solution. Will return NaN if there has
-   * been no successful solution.
+   * Getter for optimal cost at the solution.
+   * If the solver finds an optimal solution, then we return the cost evaluated
+   * at this solution.
+   * If the program is unbounded, then the optimal cost is -∞.
+   * If the program is globally infeasible, then the optimal cost is +∞.
+   * If the program is locally infeasible, then the solver (e.g. SNOPT) might
+   * return some finite value as the optimal cost.
+   * Otherwise, the optimal cost is NaN.
    */
   double GetOptimalCost() const { return optimal_cost_; }
 

solvers/nlopt_solver.cc

@@ -29,8 +29,8 @@ Eigen::VectorXd MakeEigenVector(const std::vector<double>& x) {
 }
 
 AutoDiffVecXd MakeInputAutoDiffVec(const MathematicalProgram& prog,
-                               const Eigen::VectorXd& xvec,
-                               const VectorXDecisionVariable& vars) {
+                                   const Eigen::VectorXd& xvec,
+                                   const VectorXDecisionVariable& vars) {
   const int num_vars = vars.rows();
 
   auto tx = math::initializeAutoDiff(xvec);
@@ -294,6 +294,24 @@ void WrapConstraint(const MathematicalProgram& prog, const Binding<C>& binding,
   }
 }
 
+template <typename Binding>
+bool IsVectorOfConstraintsSatisfiedAtSolution(
+    const MathematicalProgram& prog, const std::vector<Binding>& bindings,
+    double tol) {
+  for (const auto& binding : bindings) {
+    const Eigen::VectorXd constraint_val = prog.EvalBindingAtSolution(binding);
+    const int num_constraint = constraint_val.rows();
+    if (((constraint_val - binding.constraint()->lower_bound()).array() <
+         -Eigen::ArrayXd::Constant(num_constraint, tol))
+            .any() ||
+        ((constraint_val - binding.constraint()->upper_bound()).array() >
+         Eigen::ArrayXd::Constant(num_constraint, tol))
+            .any()) {
+      return false;
+    }
+  }
+  return true;
+}
 }  // anonymous namespace
 
 bool NloptSolver::available() const { return true; }
@@ -378,27 +396,87 @@ SolutionResult NloptSolver::Solve(MathematicalProgram& prog) const {
   SolutionResult result = SolutionResult::kSolutionFound;
 
   double minf = 0;
+  const double kUnboundedTol = -1E30;
   try {
     const nlopt::result nlopt_result = opt.optimize(x, minf);
-    unused(nlopt_result);
+    if (nlopt_result == nlopt::SUCCESS ||
+        nlopt_result == nlopt::STOPVAL_REACHED ||
+        nlopt_result == nlopt::XTOL_REACHED ||
+        nlopt_result == nlopt::FTOL_REACHED ||
+        nlopt_result == nlopt::MAXEVAL_REACHED ||
+        nlopt_result == nlopt::MAXTIME_REACHED) {
+      Eigen::VectorXd sol(x.size());
+      for (int i = 0; i < nx; i++) {
+        sol(i) = x[i];
+      }
+      prog.SetDecisionVariableValues(sol);
+    }
+    switch (nlopt_result) {
+      case nlopt::SUCCESS:
+      case nlopt::STOPVAL_REACHED: {
+        result = SolutionResult::kSolutionFound;
+        break;
+      }
+      case nlopt::FTOL_REACHED:
+      case nlopt::XTOL_REACHED: {
+        // Now check if the constraints are violated.
+        // TODO(hongkai.dai) Allow the user to set this tolerance.
+        const double constraint_tol = 1E-6;
+        bool all_constraints_satisfied = true;
+        auto constraint_test = [&prog, constraint_tol,
+                                &all_constraints_satisfied](auto constraints) {
+          all_constraints_satisfied &= IsVectorOfConstraintsSatisfiedAtSolution(
+              prog, constraints, constraint_tol);
+        };
+        constraint_test(prog.generic_constraints());
+        constraint_test(prog.bounding_box_constraints());
+        constraint_test(prog.linear_constraints());
+        constraint_test(prog.linear_equality_constraints());
+        constraint_test(prog.lorentz_cone_constraints());
+        constraint_test(prog.rotated_lorentz_cone_constraints());
+
+        if (!all_constraints_satisfied) {
+          result = SolutionResult::kInfeasibleConstraints;
+        }
+        break;
+      }
+      case nlopt::MAXTIME_REACHED:
+      case nlopt::MAXEVAL_REACHED: {
+        result = SolutionResult::kIterationLimit;
+        break;
+      }
+      case nlopt::INVALID_ARGS: {
+        result = SolutionResult::kInvalidInput;
+        break;
+      }
+      case nlopt::ROUNDOFF_LIMITED: {
+        if (minf < kUnboundedTol) {
+          result = SolutionResult::kUnbounded;
+          minf = -std::numeric_limits<double>::infinity();
+        } else {
+          result = SolutionResult::kUnknownError;
+        }
+        break;
+      }
+      default: { result = SolutionResult::kUnknownError; }
+    }
   } catch (std::invalid_argument&) {
     result = SolutionResult::kInvalidInput;
   } catch (std::bad_alloc&) {
     result = SolutionResult::kUnknownError;
   } catch (nlopt::roundoff_limited) {
-    result = SolutionResult::kUnknownError;
+    if (minf < kUnboundedTol) {
+      result = SolutionResult::kUnbounded;
+      minf = MathematicalProgram::kUnboundedCost;
+    } else {
+      result = SolutionResult::kUnknownError;
+    }
   } catch (nlopt::forced_stop) {
     result = SolutionResult::kUnknownError;
-  } catch (std::runtime_error&) {
+  } catch (std::runtime_error) {
     result = SolutionResult::kUnknownError;
   }
 
-  Eigen::VectorXd sol(x.size());
-  for (int i = 0; i < nx; i++) {
-    sol(i) = x[i];
-  }
-
-  prog.SetDecisionVariableValues(sol);
   prog.SetOptimalCost(minf);
   prog.SetSolverId(id());
   return result;

solvers/scs_solver.cc

@@ -650,9 +650,11 @@ SolutionResult ScsSolver::Solve(MathematicalProgram& prog) const {
   } else if (scs_status == SCS_UNBOUNDED ||
              scs_status == SCS_UNBOUNDED_INACCURATE) {
     sol_result = SolutionResult::kUnbounded;
+    prog.SetOptimalCost(MathematicalProgram::kUnboundedCost);
   } else if (scs_status == SCS_INFEASIBLE ||
              scs_status == SCS_INFEASIBLE_INACCURATE) {
     sol_result = SolutionResult::kInfeasibleConstraints;
+    prog.SetOptimalCost(MathematicalProgram::kGlobalInfeasibleCost);
   }
   if (scs_status != SCS_SOLVED) {
     drake::log()->info("SCS returns code {}, with message \"{}\".\n",

solvers/snopt_solver.cc

@@ -498,8 +498,7 @@ void UpdateLinearConstraint(const MathematicalProgram& prog,
            ++it) {
         tripletList->emplace_back(
             *linear_constraint_index + it.row(),
-            prog.FindDecisionVariableIndex(binding.variables()(k)),
-            it.value());
+            prog.FindDecisionVariableIndex(binding.variables()(k)), it.value());
       }
     }
 
@@ -728,6 +727,9 @@ SolutionResult SnoptSolver::Solve(MathematicalProgram& prog) const {
     drake::log()->debug("Snopt returns code {}\n", info);
     if (info >= 11 && info <= 16) {
       return SolutionResult::kInfeasibleConstraints;
+    } else if (info >= 20 && info <= 22) {
+      prog.SetOptimalCost(MathematicalProgram::kUnboundedCost);
+      return SolutionResult::kUnbounded;
     } else if (info == 91) {
       return SolutionResult::kInvalidInput;
     }