OSQP always reports primal and dual values regardless of the solver status.

Even when the problem is infeasible/unbounded/numerical_error. As long
as osqp_solve() is finished.

Author: hongkai-dai

solvers/BUILD.bazel

@@ -2031,8 +2031,8 @@ drake_cc_googletest(
     deps = [
         ":cost",
         ":mathematical_program_result",
-        ":osqp_solver",
         ":snopt_solver",
+        ":solve",
         "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_throws_message",
         "//common/test_utilities:symbolic_test_util",

solvers/osqp_solver.cc

@@ -356,35 +356,35 @@ void OsqpSolver::DoSolve2(const MathematicalProgram& prog,
     solver_details.run_time = work->info->run_time;
     solver_details.rho_updates = work->info->rho_updates;
 
+    // We set the primal and dual variables as long as osqp_solve() is finished.
+    const Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> osqp_sol(
+        work->solution->x, prog.num_vars());
+
+    // Scale solution back if `scale_map` is not empty.
+    const auto& scale_map = prog.GetVariableScaling();
+    if (!scale_map.empty()) {
+      drake::VectorX<double> scaled_sol = osqp_sol.cast<double>();
+      for (const auto& [index, scale] : scale_map) {
+        scaled_sol(index) *= scale;
+      }
+      result->set_x_val(scaled_sol);
+    } else {
+      result->set_x_val(osqp_sol.cast<double>());
+    }
+    solver_details.y =
+        Eigen::Map<Eigen::VectorXd>(work->solution->y, work->data->m);
+    SetDualSolution(prog.linear_constraints(), solver_details.y,
+                    constraint_start_row, result);
+    SetDualSolution(prog.linear_equality_constraints(), solver_details.y,
+                    constraint_start_row, result);
+    SetDualSolution(prog.bounding_box_constraints(), solver_details.y,
+                    constraint_start_row, result);
+
     switch (work->info->status_val) {
       case OSQP_SOLVED:
       case OSQP_SOLVED_INACCURATE: {
-        const Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> osqp_sol(
-            work->solution->x, prog.num_vars());
-
-        // Scale solution back if `scale_map` is not empty.
-        const auto& scale_map = prog.GetVariableScaling();
-        if (!scale_map.empty()) {
-          drake::VectorX<double> scaled_sol = osqp_sol.cast<double>();
-          for (const auto& [index, scale] : scale_map) {
-            scaled_sol(index) *= scale;
-          }
-          result->set_x_val(scaled_sol);
-        } else {
-          result->set_x_val(osqp_sol.cast<double>());
-        }
-
         result->set_optimal_cost(work->info->obj_val + constant_cost_term);
-        solver_details.y =
-            Eigen::Map<Eigen::VectorXd>(work->solution->y, work->data->m);
         solution_result = SolutionResult::kSolutionFound;
-        SetDualSolution(prog.linear_constraints(), solver_details.y,
-                        constraint_start_row, result);
-        SetDualSolution(prog.linear_equality_constraints(), solver_details.y,
-                        constraint_start_row, result);
-        SetDualSolution(prog.bounding_box_constraints(), solver_details.y,
-                        constraint_start_row, result);
-
         break;
       }
       case OSQP_PRIMAL_INFEASIBLE:

solvers/osqp_solver.h

@@ -48,7 +48,13 @@ Drake uses OSQP's default values for all options except following:
   output deterministic (upstream default is `0`, which uses non-deterministic
   timing measurements to establish the interval). N.B. Generally the interval
   should be an integer multiple of `check_termination`, so if you choose to
-  override either option you should probably override both at once. */
+  override either option you should probably override both at once.
+
+At the end of OsqpSolver::Solve() function, we always return the value of the
+primal and dual variables inside the OSQP solver, regardless of the solver
+status (whether it is optimal, infeasible, unbounded, etc), except when the
+problem has an invalid input.
+  */
 class OsqpSolver final : public SolverBase {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(OsqpSolver);

solvers/test/mathematical_program_result_test.cc

@@ -8,8 +8,8 @@
 #include "drake/common/test_utilities/expect_throws_message.h"
 #include "drake/common/test_utilities/symbolic_test_util.h"
 #include "drake/solvers/cost.h"
-#include "drake/solvers/osqp_solver.h"
 #include "drake/solvers/snopt_solver.h"
+#include "drake/solvers/solve.h"
 
 namespace drake {
 namespace solvers {
@@ -223,19 +223,15 @@ GTEST_TEST(TestMathematicalProgramResult, InfeasibleProblem) {
   prog.AddLinearConstraint(x(1) >= 1);
   prog.AddQuadraticCost(x.dot(x.cast<symbolic::Expression>()));
 
-  MathematicalProgramResult result;
-  OsqpSolver osqp_solver;
-  if (osqp_solver.available()) {
-    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);
-  }
+  const Eigen::VectorXd x_guess = Eigen::Vector2d::Zero();
+  const MathematicalProgramResult result = Solve(prog, x_guess, {});
+  EXPECT_EQ(result.get_solution_result(),
+            SolutionResult::kInfeasibleConstraints);
+  const auto x_sol = result.GetSolution(x);
+  EXPECT_EQ(x_sol.size(), 2);
+
+  EXPECT_EQ(result.get_optimal_cost(),
+            MathematicalProgram::kGlobalInfeasibleCost);
 }
 
 GTEST_TEST(TestMathematicalProgramResult, GetInfeasibleConstraintNames) {

solvers/test/osqp_solver_test.cc

@@ -83,6 +83,8 @@ GTEST_TEST(QPtest, TestUnbounded) {
   if (solver.available()) {
     auto result = solver.Solve(prog, {}, {});
     EXPECT_EQ(result.get_solution_result(), SolutionResult::kDualInfeasible);
+    EXPECT_TRUE(result.GetSolution(x).array().isFinite().all());
+    EXPECT_EQ(result.get_solver_details<OsqpSolver>().y.size(), 0);
   }
 
   // Add a constraint
@@ -99,9 +101,9 @@ GTEST_TEST(QPtest, TestInfeasible) {
   auto x = prog.NewContinuousVariables<2>();
 
   prog.AddQuadraticCost(x(0) * x(0) + 2 * x(1) * x(1));
-  prog.AddLinearConstraint(x(0) + 2 * x(1) == 2);
-  prog.AddLinearConstraint(x(0) >= 1);
-  prog.AddLinearConstraint(x(1) >= 2);
+  auto constraint0 = prog.AddLinearConstraint(x(0) + 2 * x(1) == 2);
+  auto constraint1 = prog.AddLinearConstraint(x(0) >= 1);
+  auto constraint2 = prog.AddLinearConstraint(x(1) >= 2);
 
   OsqpSolver solver;
   // The program is infeasible.
@@ -111,7 +113,16 @@ GTEST_TEST(QPtest, TestInfeasible) {
               SolutionResult::kInfeasibleConstraints);
     EXPECT_EQ(result.get_optimal_cost(),
               MathematicalProgram::kGlobalInfeasibleCost);
-    EXPECT_EQ(result.get_solver_details<OsqpSolver>().y.rows(), 0);
+
+    EXPECT_EQ(result.get_solver_details<OsqpSolver>().y.rows(), 3);
+    // Primal solution is not NAN or inf.
+    EXPECT_TRUE(result.GetSolution(x).array().isFinite().all());
+    // Dual solution is not NAN or inf.
+    EXPECT_TRUE(
+        result.get_solver_details<OsqpSolver>().y.array().isFinite().all());
+    EXPECT_TRUE(result.GetDualSolution(constraint0).array().isFinite().all());
+    EXPECT_TRUE(result.GetDualSolution(constraint1).array().isFinite().all());
+    EXPECT_TRUE(result.GetDualSolution(constraint2).array().isFinite().all());
     // In OSQP's default upstream settings, time-based adaptive rho is enabled
     // by default (i.e., adaptive_rho_interval=0). However, in our OsqpSolver
     // wrapper, we've changed the default to be non-zero so that solver results