feat: warn when solver fails.

HISTORY.md

@@ -5,6 +5,7 @@
 ### Changed
 
 - [cpp]: fix numerical issue in Seidel solver for 1D problems. (#232)
+- [cpp]: warn when solver fails. (#241)
 
 ## 0.6.0 (Mar 08 2023)
 

cpp/src/toppra/algorithm.cpp

@@ -28,7 +28,6 @@ ReturnCode PathParametrizationAlgorithm::computePathParametrization(value_type v
 ReturnCode PathParametrizationAlgorithm::computeControllableSets(
     const Bound &vel_ends) {
   TOPPRA_LOG_DEBUG("computeControllableSets");
-  ReturnCode ret = ReturnCode::OK;
   bool solver_ret;
   Vector g_upper{2}, g_lower{2}, solution;
   g_upper << 1e-9, -1;
@@ -43,9 +42,14 @@ ReturnCode PathParametrizationAlgorithm::computeControllableSets(
     solver_ret = m_solver->solveStagewiseOptim(i, H, g_upper, x, x_next, solution);
 
     if (!solver_ret) {
-      ret = ReturnCode::ERR_FAIL_CONTROLLABLE;
       TOPPRA_LOG_DEBUG("Fail: controllable, upper problem, idx: " << i);
-      break;
+      m_errorStream <<
+        "Failed to compute controllable set, upper problem, at idx " << i <<
+        "\ng_upper = " << g_upper.transpose() <<
+        "\nx = " << x <<
+        "\nx_next = " << x_next <<
+        '\n';
+      return ReturnCode::ERR_FAIL_CONTROLLABLE;
     }
 
     m_data.controllable_sets(i, 1) = solution[1];
@@ -54,22 +58,26 @@ ReturnCode PathParametrizationAlgorithm::computeControllableSets(
 
     TOPPRA_LOG_DEBUG("down: " << solution.transpose());
     if (!solver_ret) {
-      ret = ReturnCode::ERR_FAIL_CONTROLLABLE;
       TOPPRA_LOG_DEBUG("Fail: controllable, lower problem, idx: " << i);
-      break;
+      m_errorStream <<
+        "Failed to compute controllable set, lower problem, at idx " << i <<
+        "\ng_lower = " << g_lower.transpose() <<
+        "\nx = " << x <<
+        "\nx_next = " << x_next <<
+        '\n';
+      return ReturnCode::ERR_FAIL_CONTROLLABLE;
     }
 
     // For whatever reason, sometimes the solver return negative
     // solution despite having a set of positve bounds. This readjust
     // if the solution is negative.
     m_data.controllable_sets(i, 0) = std::max(0.0, solution[1]);
   }
-  return ret;
+  return ReturnCode::OK;
 }
 
 ReturnCode PathParametrizationAlgorithm::computeFeasibleSets() {
   initialize();
-  ReturnCode ret = ReturnCode::OK;
   bool solver_ret;
   Vector g_upper{2}, g_lower{2}, solution;
   g_upper << 1e-9, -1;
@@ -83,24 +91,34 @@ ReturnCode PathParametrizationAlgorithm::computeFeasibleSets() {
     solver_ret = m_solver->solveStagewiseOptim(i, H, g_upper, x, x_next, solution);
 
     if (!solver_ret) {
-      ret = ReturnCode::ERR_FAIL_FEASIBLE;
-      TOPPRA_LOG_DEBUG("Fail: controllable, upper problem, idx: " << i);
-      break;
+      TOPPRA_LOG_DEBUG("Fail: feasible, upper problem, idx: " << i);
+      m_errorStream <<
+        "Failed to compute feasible set, upper problem, at idx " << i <<
+        "\ng_upper = " << g_upper.transpose() <<
+        "\nx = " << x <<
+        "\nx_next = " << x_next <<
+        '\n';
+      return ReturnCode::ERR_FAIL_FEASIBLE;
     }
 
     m_data.feasible_sets(i, 1) = solution[1];
 
     solver_ret = m_solver->solveStagewiseOptim(i, H, g_lower, x, x_next, solution);
 
     if (!solver_ret) {
-      ret = ReturnCode::ERR_FAIL_FEASIBLE;
-      TOPPRA_LOG_DEBUG("Fail: controllable, lower problem, idx: " << i);
-      break;
+      TOPPRA_LOG_DEBUG("Fail: feasible, lower problem, idx: " << i);
+      m_errorStream <<
+        "Failed to compute feasible set, lower problem, at idx " << i <<
+        "\ng_lower = " << g_lower.transpose() <<
+        "\nx = " << x <<
+        "\nx_next = " << x_next <<
+        '\n';
+      return ReturnCode::ERR_FAIL_FEASIBLE;
     }
 
     m_data.feasible_sets(i, 0) = solution[1];
   }
-  return ret;
+  return ReturnCode::OK;
 }
 
 void PathParametrizationAlgorithm::setGridpoints(const Vector& gridpoints)
@@ -118,6 +136,7 @@ void PathParametrizationAlgorithm::setGridpoints(const Vector& gridpoints)
 }
 
 void PathParametrizationAlgorithm::initialize() {
+  m_errorStream = std::stringstream();
   if (m_initialized) return;
   if (!m_solver)
     throw std::logic_error("You must set a solver first.");

cpp/src/toppra/algorithm.hpp

@@ -2,6 +2,7 @@
 #define TOPPRA_ALGORITHM_HPP
 
 #include <stdexcept>
+#include <sstream>
 #include <toppra/constraint.hpp>
 #include <toppra/geometric_path.hpp>
 #include <toppra/solver.hpp>
@@ -90,7 +91,8 @@ class PathParametrizationAlgorithm {
    *
    * \param vel_start
    * \param vel_end
-   * \return Return code.
+   * \return Return code. When not ReturnCode::OK,
+   *         check PathParametrizationAlgorithm::getErrorMessage
    */
   virtual ReturnCode computePathParametrization(value_type vel_start = 0,
                                                 value_type vel_end = 0);
@@ -107,6 +109,13 @@ class PathParametrizationAlgorithm {
     m_initXBound = xbound;
   }
 
+  /** Get the error message when PathParametrizationAlgorithm::computePathParametrization
+   * failed.
+   */
+  std::string getErrorMessage() const {
+    return m_errorStream.str();
+  }
+
   virtual ~PathParametrizationAlgorithm() {}
 
  protected:
@@ -132,6 +141,8 @@ class PathParametrizationAlgorithm {
   GeometricPathPtr m_path;
   SolverPtr m_solver;
 
+  std::stringstream m_errorStream;
+
   /// Struct containing algorithm output.
   ParametrizationData m_data;
 

cpp/src/toppra/algorithm/toppra.cpp

@@ -14,7 +14,6 @@ TOPPRA::TOPPRA(LinearConstraintPtrs constraints, const GeometricPathPtr &path)
 
 ReturnCode TOPPRA::computeForwardPass(value_type vel_start) {
   TOPPRA_LOG_DEBUG("computeForwardPass");
-  ReturnCode ret = ReturnCode::OK;
   bool solver_ret;
   Vector g_upper{2}, solution{2};
   Matrix H;
@@ -28,9 +27,14 @@ ReturnCode TOPPRA::computeForwardPass(value_type vel_start) {
     x_next = m_data.controllable_sets.row(i + 1);
     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);
-      break;
+      TOPPRA_LOG_WARN("Fail: forward pass, idx: " << i);
+      m_errorStream <<
+        "Forward pass failed at idx " << i <<
+        "\ng_upper = " << g_upper.transpose() <<
+        "\nx = " << x <<
+        "\nx_next = " << x_next <<
+        '\n';
+      return ReturnCode::ERR_FAIL_FORWARD_PASS;
     }
     /// \todo This can be optimized further by solving a 1D problem instead of 2D
     // Claim the output to be within the controllable sets.
@@ -42,7 +46,7 @@ ReturnCode TOPPRA::computeForwardPass(value_type vel_start) {
                               << "]=" << m_data.parametrization(i + 1));
   }
 
-  return ret;
+  return ReturnCode::OK;
 };
 
 }  // namespace algorithm

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);

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;
 }
 

cpp/tests/test_algorithm.cpp

@@ -201,3 +201,43 @@ TYPED_TEST(ProblemInstance, ParametrizeOutputTrajectory) {
               this->path->eval_single(this->path->pathInterval()[1])[i]);
   }
 }
+
+class FakeFailingSolver : public toppra::Solver {
+  public:
+    bool solveStagewiseOptim(std::size_t i,
+        const toppra::Matrix& H, const toppra::Vector& g,
+        const toppra::Bound& x, const toppra::Bound& xNext,
+        toppra::Vector& solution) {
+      return false;
+    }
+};
+
+TEST(ErrorMessageWhenFailing, ControllableSets) {
+  int nDof = 2;
+  toppra::Matrix coeff0{4, 2}, coeff1{4, 2}, coeff2{4, 2};
+  coeff0 << -0.500000, -0.500000, 1.500000, 0.500000, 0.000000, 3.000000, 0.000000,
+      0.000000;
+  coeff1 << -0.500000, -0.500000, 0.000000, -1.000000, 1.500000, 2.500000, 1.000000,
+      3.000000;
+  coeff2 << -0.500000, -0.500000, -1.500000, -2.500000, 0.000000, -1.000000, 2.000000,
+      4.000000;
+  toppra::Matrices coefficents = {coeff0, coeff1, coeff2};
+  auto path = std::make_shared<toppra::PiecewisePolyPath>(coefficents, std::vector<double>{0, 1, 2, 3});
+  auto v = toppra::LinearConstraintPtrs{
+      std::make_shared<toppra::constraint::LinearJointVelocity>(
+          -toppra::Vector::Ones(nDof), toppra::Vector::Ones(nDof)),
+      std::make_shared<toppra::constraint::LinearJointAcceleration>(
+          -0.2 * toppra::Vector::Ones(nDof), 0.2 * toppra::Vector::Ones(nDof))};
+  // Change the discretiation type to collocation to make it similar to 
+  // the python impl outputs
+  for (auto vi: v)
+    vi->discretizationType(toppra::DiscretizationType::Collocation);
+
+  toppra::algorithm::TOPPRA problem{v, path};
+  problem.setN(50);
+  problem.solver(std::make_shared<FakeFailingSolver>());
+  auto ret_code = problem.computePathParametrization();
+
+  std::string starts_with {"Failed to compute controllable set"};
+  ASSERT_EQ(problem.getErrorMessage().substr(0, starts_with.size()), starts_with);
+}