Skip to content

Commit bfe67a2

Browse files
committed
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.
1 parent ed0b95b commit bfe67a2

File tree

5 files changed

+57
-44
lines changed

5 files changed

+57
-44
lines changed

solvers/BUILD.bazel

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2031,8 +2031,8 @@ drake_cc_googletest(
20312031
deps = [
20322032
":cost",
20332033
":mathematical_program_result",
2034-
":osqp_solver",
20352034
":snopt_solver",
2035+
":solve",
20362036
"//common/test_utilities:eigen_matrix_compare",
20372037
"//common/test_utilities:expect_throws_message",
20382038
"//common/test_utilities:symbolic_test_util",

solvers/osqp_solver.cc

Lines changed: 24 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -356,35 +356,35 @@ void OsqpSolver::DoSolve2(const MathematicalProgram& prog,
356356
solver_details.run_time = work->info->run_time;
357357
solver_details.rho_updates = work->info->rho_updates;
358358

359+
// We set the primal and dual variables as long as osqp_solve() is finished.
360+
const Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> osqp_sol(
361+
work->solution->x, prog.num_vars());
362+
363+
// Scale solution back if `scale_map` is not empty.
364+
const auto& scale_map = prog.GetVariableScaling();
365+
if (!scale_map.empty()) {
366+
drake::VectorX<double> scaled_sol = osqp_sol.cast<double>();
367+
for (const auto& [index, scale] : scale_map) {
368+
scaled_sol(index) *= scale;
369+
}
370+
result->set_x_val(scaled_sol);
371+
} else {
372+
result->set_x_val(osqp_sol.cast<double>());
373+
}
374+
solver_details.y =
375+
Eigen::Map<Eigen::VectorXd>(work->solution->y, work->data->m);
376+
SetDualSolution(prog.linear_constraints(), solver_details.y,
377+
constraint_start_row, result);
378+
SetDualSolution(prog.linear_equality_constraints(), solver_details.y,
379+
constraint_start_row, result);
380+
SetDualSolution(prog.bounding_box_constraints(), solver_details.y,
381+
constraint_start_row, result);
382+
359383
switch (work->info->status_val) {
360384
case OSQP_SOLVED:
361385
case OSQP_SOLVED_INACCURATE: {
362-
const Eigen::Map<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> osqp_sol(
363-
work->solution->x, prog.num_vars());
364-
365-
// Scale solution back if `scale_map` is not empty.
366-
const auto& scale_map = prog.GetVariableScaling();
367-
if (!scale_map.empty()) {
368-
drake::VectorX<double> scaled_sol = osqp_sol.cast<double>();
369-
for (const auto& [index, scale] : scale_map) {
370-
scaled_sol(index) *= scale;
371-
}
372-
result->set_x_val(scaled_sol);
373-
} else {
374-
result->set_x_val(osqp_sol.cast<double>());
375-
}
376-
377386
result->set_optimal_cost(work->info->obj_val + constant_cost_term);
378-
solver_details.y =
379-
Eigen::Map<Eigen::VectorXd>(work->solution->y, work->data->m);
380387
solution_result = SolutionResult::kSolutionFound;
381-
SetDualSolution(prog.linear_constraints(), solver_details.y,
382-
constraint_start_row, result);
383-
SetDualSolution(prog.linear_equality_constraints(), solver_details.y,
384-
constraint_start_row, result);
385-
SetDualSolution(prog.bounding_box_constraints(), solver_details.y,
386-
constraint_start_row, result);
387-
388388
break;
389389
}
390390
case OSQP_PRIMAL_INFEASIBLE:

solvers/osqp_solver.h

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,13 @@ Drake uses OSQP's default values for all options except following:
4848
output deterministic (upstream default is `0`, which uses non-deterministic
4949
timing measurements to establish the interval). N.B. Generally the interval
5050
should be an integer multiple of `check_termination`, so if you choose to
51-
override either option you should probably override both at once. */
51+
override either option you should probably override both at once.
52+
53+
At the end of OsqpSolver::Solve() function, we always return the value of the
54+
primal and dual variables inside the OSQP solver, regardless of the solver
55+
status (whether it is optimal, infeasible, unbounded, etc), except when the
56+
problem has an invalid input.
57+
*/
5258
class OsqpSolver final : public SolverBase {
5359
public:
5460
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(OsqpSolver);

solvers/test/mathematical_program_result_test.cc

Lines changed: 10 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,8 @@
88
#include "drake/common/test_utilities/expect_throws_message.h"
99
#include "drake/common/test_utilities/symbolic_test_util.h"
1010
#include "drake/solvers/cost.h"
11-
#include "drake/solvers/osqp_solver.h"
1211
#include "drake/solvers/snopt_solver.h"
12+
#include "drake/solvers/solve.h"
1313

1414
namespace drake {
1515
namespace solvers {
@@ -223,19 +223,15 @@ GTEST_TEST(TestMathematicalProgramResult, InfeasibleProblem) {
223223
prog.AddLinearConstraint(x(1) >= 1);
224224
prog.AddQuadraticCost(x.dot(x.cast<symbolic::Expression>()));
225225

226-
MathematicalProgramResult result;
227-
OsqpSolver osqp_solver;
228-
if (osqp_solver.available()) {
229-
const Eigen::VectorXd x_guess = Eigen::Vector2d::Zero();
230-
osqp_solver.Solve(prog, x_guess, {}, &result);
231-
EXPECT_TRUE(CompareMatrices(
232-
result.GetSolution(x),
233-
Eigen::Vector2d::Constant(std::numeric_limits<double>::quiet_NaN())));
234-
EXPECT_TRUE(std::isnan(result.GetSolution(x(0))));
235-
EXPECT_TRUE(std::isnan(result.GetSolution(x(1))));
236-
EXPECT_EQ(result.get_optimal_cost(),
237-
MathematicalProgram::kGlobalInfeasibleCost);
238-
}
226+
const Eigen::VectorXd x_guess = Eigen::Vector2d::Zero();
227+
const MathematicalProgramResult result = Solve(prog, x_guess, {});
228+
EXPECT_EQ(result.get_solution_result(),
229+
SolutionResult::kInfeasibleConstraints);
230+
const auto x_sol = result.GetSolution(x);
231+
EXPECT_EQ(x_sol.size(), 2);
232+
233+
EXPECT_EQ(result.get_optimal_cost(),
234+
MathematicalProgram::kGlobalInfeasibleCost);
239235
}
240236

241237
GTEST_TEST(TestMathematicalProgramResult, GetInfeasibleConstraintNames) {

solvers/test/osqp_solver_test.cc

Lines changed: 15 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,8 @@ GTEST_TEST(QPtest, TestUnbounded) {
8383
if (solver.available()) {
8484
auto result = solver.Solve(prog, {}, {});
8585
EXPECT_EQ(result.get_solution_result(), SolutionResult::kDualInfeasible);
86+
EXPECT_TRUE(result.GetSolution(x).array().isFinite().all());
87+
EXPECT_EQ(result.get_solver_details<OsqpSolver>().y.size(), 0);
8688
}
8789

8890
// Add a constraint
@@ -99,9 +101,9 @@ GTEST_TEST(QPtest, TestInfeasible) {
99101
auto x = prog.NewContinuousVariables<2>();
100102

101103
prog.AddQuadraticCost(x(0) * x(0) + 2 * x(1) * x(1));
102-
prog.AddLinearConstraint(x(0) + 2 * x(1) == 2);
103-
prog.AddLinearConstraint(x(0) >= 1);
104-
prog.AddLinearConstraint(x(1) >= 2);
104+
auto constraint0 = prog.AddLinearConstraint(x(0) + 2 * x(1) == 2);
105+
auto constraint1 = prog.AddLinearConstraint(x(0) >= 1);
106+
auto constraint2 = prog.AddLinearConstraint(x(1) >= 2);
105107

106108
OsqpSolver solver;
107109
// The program is infeasible.
@@ -111,7 +113,16 @@ GTEST_TEST(QPtest, TestInfeasible) {
111113
SolutionResult::kInfeasibleConstraints);
112114
EXPECT_EQ(result.get_optimal_cost(),
113115
MathematicalProgram::kGlobalInfeasibleCost);
114-
EXPECT_EQ(result.get_solver_details<OsqpSolver>().y.rows(), 0);
116+
117+
EXPECT_EQ(result.get_solver_details<OsqpSolver>().y.rows(), 3);
118+
// Primal solution is not NAN or inf.
119+
EXPECT_TRUE(result.GetSolution(x).array().isFinite().all());
120+
// Dual solution is not NAN or inf.
121+
EXPECT_TRUE(
122+
result.get_solver_details<OsqpSolver>().y.array().isFinite().all());
123+
EXPECT_TRUE(result.GetDualSolution(constraint0).array().isFinite().all());
124+
EXPECT_TRUE(result.GetDualSolution(constraint1).array().isFinite().all());
125+
EXPECT_TRUE(result.GetDualSolution(constraint2).array().isFinite().all());
115126
// In OSQP's default upstream settings, time-based adaptive rho is enabled
116127
// by default (i.e., adaptive_rho_interval=0). However, in our OsqpSolver
117128
// wrapper, we've changed the default to be non-zero so that solver results

0 commit comments

Comments
 (0)