Set the x_val and optimal cost to nan by default in MathematicalProgr…

…amResult (RobotLocomotion#11707) Set the x_val and optimal cost to nan by default in MathematicalProgramResult.
miquelramirez · Jun 24, 2019 · f93935d · f93935d
1 parent 2733f21
commit f93935d
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 1 deletion.
diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel
@@ -1691,6 +1691,7 @@ drake_cc_googletest(
     deps = [
         ":cost",
         ":mathematical_program_result",
+        ":osqp_solver",
         "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_throws_message",
     ],

diff --git a/solvers/mathematical_program_result.h b/solvers/mathematical_program_result.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <limits>
 #include <memory>
 #include <typeinfo>
 #include <unordered_map>
@@ -85,11 +86,14 @@ class MathematicalProgramResult final {
   /**
    * Sets decision_variable_index mapping, that maps each decision variable to
    * its index in the aggregated vector containing all decision variables in
-   * MathematicalProgram.
+   * MathematicalProgram. Initialize x_val to NAN.
    */
   void set_decision_variable_index(
       std::unordered_map<symbolic::Variable::Id, int> decision_variable_index) {
     decision_variable_index_ = std::move(decision_variable_index);
+    x_val_ =
+        Eigen::VectorXd::Constant(decision_variable_index_->size(),
+                                  std::numeric_limits<double>::quiet_NaN());
   }
 
   /** Sets SolutionResult. */

diff --git a/solvers/solver_base.cc b/solvers/solver_base.cc
@@ -1,5 +1,6 @@
 #include "drake/solvers/solver_base.h"
 
+#include <limits>
 #include <utility>
 
 #include <fmt/format.h>

diff --git a/solvers/test/mathematical_program_result_test.cc b/solvers/test/mathematical_program_result_test.cc
@@ -1,10 +1,13 @@
 #include "drake/solvers/mathematical_program_result.h"
 
+#include <limits>
+
 #include <gtest/gtest.h>
 
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/common/test_utilities/expect_throws_message.h"
 #include "drake/solvers/cost.h"
+#include "drake/solvers/osqp_solver.h"
 
 namespace drake {
 namespace solvers {
@@ -37,6 +40,10 @@ TEST_F(MathematicalProgramResultTest, DefaultConstructor) {
 TEST_F(MathematicalProgramResultTest, Setters) {
   MathematicalProgramResult result;
   result.set_decision_variable_index(decision_variable_index_);
+  EXPECT_TRUE(CompareMatrices(
+      result.get_x_val(),
+      Eigen::VectorXd::Constant(decision_variable_index_.size(),
+                                std::numeric_limits<double>::quiet_NaN())));
   result.set_solution_result(SolutionResult::kSolutionFound);
   const Eigen::Vector2d x_val(0, 1);
   result.set_x_val(x_val);
@@ -116,6 +123,30 @@ TEST_F(MathematicalProgramResultTest, EvalBinding) {
                                  Vector1<symbolic::Variable>(x1_)};
   EXPECT_TRUE(CompareMatrices(result.EvalBinding(cost), Vector1d(2)));
 }
+
+GTEST_TEST(TestMathematicalProgramResult, InfeasibleProblem) {
+  // Test if we can query the information in the result when the problem is
+  // infeasible.
+  MathematicalProgram prog;
+  auto x = prog.NewContinuousVariables<2>();
+  prog.AddLinearConstraint(x(0) + x(1) <= 1);
+  prog.AddLinearConstraint(x(0) >= 1);
+  prog.AddLinearConstraint(x(1) >= 1);
+  prog.AddQuadraticCost(x.dot(x.cast<symbolic::Expression>()));
+
+  MathematicalProgramResult result;
+  OsqpSolver osqp_solver;
+  const Eigen::VectorXd x_guess = Eigen::Vector2d::Zero();
+  osqp_solver.Solve(prog, x_guess, {}, &result);
+  EXPECT_TRUE(CompareMatrices(
+      result.GetSolution(x),
+      Eigen::Vector2d::Constant(std::numeric_limits<double>::quiet_NaN())));
+  EXPECT_TRUE(std::isnan(result.GetSolution(x(0))));
+  EXPECT_TRUE(std::isnan(result.GetSolution(x(1))));
+  EXPECT_EQ(result.get_optimal_cost(),
+            MathematicalProgram::kGlobalInfeasibleCost);
+}
+
 }  // namespace
 }  // namespace solvers
 }  // namespace drake