update osqp interface (autowarefoundation#730)

isamu-takagi · Sep 25, 2020 · 5e51f8a · 5e51f8a
1 parent a2096da
commit 5e51f8a
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 13 deletions.
diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.h b/common/osqp_interface/include/osqp_interface/osqp_interface.h
@@ -25,7 +25,8 @@
 namespace osqp
 {
 struct CSC_Matrix;
-}
+const c_float INF = OSQP_INFTY;
+}  // namespace osqp
 
 namespace osqp
 {
@@ -82,7 +83,7 @@ class OSQPInterface
   bool problem_in_memory = false;
 
   // Runs the solver on the stored problem.
-  std::tuple<std::vector<double>, std::vector<double>, int> solve();
+  std::tuple<std::vector<double>, std::vector<double>, int, int> solve();
 
   /*****************************
    * DATA CONVERSION FUNCTIONS
@@ -162,7 +163,7 @@ class OSQPInterface
   //        std::vector<float> param = std::get<0>(result);
   //        double x_0 = param[0];
   //        double x_1 = param[1];
-  std::tuple<std::vector<double>, std::vector<double>, int> optimize();
+  std::tuple<std::vector<double>, std::vector<double>, int, int> optimize();
 
   // Solves convex quadratic programs (QPs) using the OSQP solver.
   //
@@ -181,7 +182,7 @@ class OSQPInterface
   //        std::vector<float> param = std::get<0>(result);
   //        double x_0 = param[0];
   //        double x_1 = param[1];
-  std::tuple<std::vector<double>, std::vector<double>, int> optimize(
+  std::tuple<std::vector<double>, std::vector<double>, int, int> optimize(
     const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
     const std::vector<double> & l, const std::vector<double> & u);
 
@@ -218,10 +219,11 @@ class OSQPInterface
   void updateEpsAbs(const double eps_abs);
   void updateEpsRel(const double eps_rel);
   void updateMaxIter(const int iter);
+  void updateVerbose(const bool verbose);
 
   int getTakenIter();
 };
 
 }  // namespace osqp
 
-#endif  // OSQP_INTERFACE_H
+#endif  // OSQP_INTERFACE_H
diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp
@@ -190,9 +190,11 @@ void OSQPInterface::updateEpsRel(const double eps_rel) { osqp_update_eps_rel(wor
 
 void OSQPInterface::updateMaxIter(const int max_iter) { osqp_update_max_iter(work, max_iter); }
 
+void OSQPInterface::updateVerbose(const bool is_verbose) { osqp_update_verbose(work, is_verbose); }
+
 int OSQPInterface::getTakenIter() { return work->info->iter; }
 
-std::tuple<std::vector<double>, std::vector<double>, int> OSQPInterface::solve()
+std::tuple<std::vector<double>, std::vector<double>, int, int> OSQPInterface::solve()
 {
   // Solve Problem
   osqp_solve(work);
@@ -206,29 +208,31 @@ std::tuple<std::vector<double>, std::vector<double>, int> OSQPInterface::solve()
   std::vector<double> sol_lagrange_multiplier(sol_y, sol_y + static_cast<int>(param_n));
   // Solver polish status
   int status_polish = work->info->status_polish;
+  // Solver solution status
+  int status_solution = work->info->status_val;
   // Result tuple
-  std::tuple<std::vector<double>, std::vector<double>, int> result =
-    std::make_tuple(sol_primal, sol_lagrange_multiplier, status_polish);
+  std::tuple<std::vector<double>, std::vector<double>, int, int> result =
+    std::make_tuple(sol_primal, sol_lagrange_multiplier, status_polish, status_solution);
 
   return result;
 }
 
-std::tuple<std::vector<double>, std::vector<double>, int> OSQPInterface::optimize()
+std::tuple<std::vector<double>, std::vector<double>, int, int> OSQPInterface::optimize()
 {
   // Run the solver on the stored problem representation.
-  std::tuple<std::vector<double>, std::vector<double>, int> result = solve();
+  std::tuple<std::vector<double>, std::vector<double>, int, int> result = solve();
   return result;
 }
 
-std::tuple<std::vector<double>, std::vector<double>, int> OSQPInterface::optimize(
+std::tuple<std::vector<double>, std::vector<double>, int, int> OSQPInterface::optimize(
   const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<double> & q,
   const std::vector<double> & l, const std::vector<double> & u)
 {
   // Allocate memory for problem
   initializeProblem(P, A, q, l, u);
 
   // Run the solver on the stored problem representation.
-  std::tuple<std::vector<double>, std::vector<double>, int> result = solve();
+  std::tuple<std::vector<double>, std::vector<double>, int, int> result = solve();
 
   // Free allocated memory for problem
   osqp_cleanup(work);
@@ -243,4 +247,4 @@ inline bool OSQPInterface::isEqual(double x, double y)
 
 c_int OSQPInterface::getExitFlag(void) { return exitflag; }
 
-}  // namespace osqp
+}  // namespace osqp