feat: warn when solver fails.

hungpham2511 · Sep 13, 2023 · 1a5dfb2 · 1a5dfb2
1 parent c2f2be4
commit 1a5dfb2
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 8 deletions.
diff --git a/cpp/src/toppra/algorithm/toppra.cpp b/cpp/src/toppra/algorithm/toppra.cpp
@@ -29,7 +29,7 @@ ReturnCode TOPPRA::computeForwardPass(value_type vel_start) {
     solver_ret = m_solver->solveStagewiseOptim(i, H, g_upper, x, x_next, solution);
     if (!solver_ret) {
       ret = ReturnCode::ERR_FAIL_FORWARD_PASS;
-      TOPPRA_LOG_DEBUG("Fail: forward pass, idx: " << i);
+      TOPPRA_LOG_WARN("Fail: forward pass, idx: " << i);
       break;
     }
     /// \todo This can be optimized further by solving a 1D problem instead of 2D

diff --git a/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp b/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp
@@ -1,12 +1,15 @@
+#include <toppra/geometric_path/piecewise_poly_path.hpp>
+
 #include <iostream>
+#include <iomanip>
 #include <ostream>
 #include <stdexcept>
 #include <string>
-#include <toppra/geometric_path/piecewise_poly_path.hpp>
-#include <toppra/toppra.hpp>
-#include <Eigen/Dense>
 #include <array>
 
+#include <Eigen/Dense>
+#include <toppra/toppra.hpp>
+
 #ifdef TOPPRA_OPT_MSGPACK
 #include <msgpack.hpp>
 #endif
@@ -238,10 +241,11 @@ Vectors PiecewisePolyPath::eval(const Vector &positions, int order) const {
 }
 
 size_t PiecewisePolyPath::findSegmentIndex(value_type pos) const {
-  if (pos > m_breakpoints[m_breakpoints.size()-1] || pos < m_breakpoints[0]) {
+  if (pos > m_breakpoints.back() || pos < m_breakpoints[0]) {
+    constexpr auto max_precision {std::numeric_limits<value_type>::digits10 + 1};
     std::ostringstream oss;
-    oss << "Position " << pos << " is outside of range [ " << m_breakpoints[0]
-      << ", " << m_breakpoints[m_breakpoints.size()-1] << ']';
+    oss << std::setprecision(max_precision) << "Position " << pos <<
+      " is outside of range [ " << m_breakpoints[0] << ", " << m_breakpoints.back() << ']';
     throw std::runtime_error(oss.str());
   }
   auto it = std::upper_bound(m_breakpoints.begin(), m_breakpoints.end(), pos);

diff --git a/cpp/src/toppra/solver/seidel.cpp b/cpp/src/toppra/solver/seidel.cpp
@@ -342,7 +342,13 @@ bool Seidel::solveStagewiseOptim(std::size_t i,
         active_c[k] = m_index_map[lpsol.active_c[k]];
     return true;
   }
-  TOPPRA_LOG_DEBUG("Seidel: solver fails (upper ? " << upper << ')');
+  TOPPRA_LOG_WARN("Seidel: solver fails"
+      "\ni = " << i <<
+      "\ng = " << g.transpose() <<
+      "\nx = " << x.transpose() <<
+      "\nxNext = " << xNext.transpose() <<
+      "\n"
+  );
   return false;
 }