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

Even when the problem is infeasible/unbounded/numerical_error.

Author: hongkai-dai

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 variable no matter the status of the solver.
+    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,12 @@ 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).
+  */
 class OsqpSolver final : public SolverBase {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(OsqpSolver);

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