@@ -356,35 +356,35 @@ void OsqpSolver::DoSolve2(const MathematicalProgram& prog,
356
356
solver_details.run_time = work->info ->run_time ;
357
357
solver_details.rho_updates = work->info ->rho_updates ;
358
358
359
+ // We set the primal and dual variable no matter the status of the solver.
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
+
359
383
switch (work->info ->status_val ) {
360
384
case OSQP_SOLVED:
361
385
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
-
377
386
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 );
380
387
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
-
388
388
break ;
389
389
}
390
390
case OSQP_PRIMAL_INFEASIBLE:
0 commit comments