Merge branch 'hungpham2511:develop' into dev/varying_accel

.bumpversion.cfg

@@ -1,7 +1,8 @@
 [bumpversion]
-current_version = 0.5.1
+current_version = 0.6.2
 commit = True
 tag = True
+message = chore: Bump toppra version: '{current_version}' --> '{new_version}'
 
 [bumpversion:file:VERSION]
 

.github/workflows/integrate.yml

@@ -9,7 +9,7 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
       matrix:
-        python-version: [3.6, 3.7, 3.8, 3.9]
+        python-version: [3.7, 3.8, 3.9, "3.10"]
 
     steps:
       - uses: actions/checkout@v2

HISTORY.md

@@ -1,5 +1,33 @@
 # Changelog
 
+## 0.6.2 (Sept 19 2023)
+
+### Changed
+
+- [cpp]: warn when solver fails. (#241)
+
+## 0.6.1
+
+### Changed
+
+- [cpp]: fix numerical issue in Seidel solver for 1D problems. (#232)
+
+## 0.6.0 (Mar 08 2023)
+
+### Added
+- [python] seidelWrapper solves 1d LP instead of 2d LP when x_min == x_max and solve_1d > 0
+- [python] Fixed planning_utils.py to follow the latest api of toppra.algorithm.ParameterizationAlgorithm
+- [cpp] add initial gridpoints to proposeGridpoints method.
+
+### Changed
+- fix: the f-string bug when hot_qpoases solver failed (#208)
+- feat(solver): Make seidel solver(cython) solves 1D LP when possible (#223)
+- feat(cpp,constraint): Make Interpolation the default method (#225)
+- fix(util): toppra.compute_trajectory() does not return aux data. (#224)
+
+## 0.5.2 (Nov 19 2022)
+- [cpp] always define all installed symbols.
+
 ## 0.5.0 (July 08 2021)
 
 - [cpp] Minor PR to make the cpp part compiles on windows64 (msvc). Thanks @ahoarau.

README.md

@@ -52,8 +52,8 @@ your proposal!
 # Credits
 
 `toppra` was originally developed by [Hung
-Pham](https://hungpham2511.github.com/) (Eureka Robotics, former CRI
-Group) and [Phạm Quang Cưong](https://personal.ntu.edu.sg/cuong/)
+Pham](https://hungpham2511.github.com/) ([Eureka Robotics](https://eurekarobotics.com/), former [CRI
+Group](https://personal.ntu.edu.sg/cuong/)) and [Phạm Quang Cường](https://personal.ntu.edu.sg/cuong/)
 (Eureka Robotics, CRI Group) with major contributions from talented
 contributors:
 - [Joseph Mirabel](https://github.com/jmirabel) (C++ API)

VERSION

@@ -1 +1 @@
-0.5.1
+0.6.2

cpp/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.5)
 
 project(toppra
-  VERSION 0.5.1
+  VERSION 0.6.2
   # Disable because not available in CMake 3.5
   #DESCRIPTION "Library computing the time-optimal path parameterization."
   LANGUAGES CXX)

cpp/bindings/paths.cpp

@@ -44,6 +44,13 @@ void exposePaths(py::module m)
             ss << b.cast<std::string>();
             p.deserialize(ss);
           })
+      .def("proposeGridpoints", &GeometricPath::proposeGridpoints,
+          py::arg("maxErrThreshold") = 1e-4,
+          py::arg("maxIteration") = 100,
+          py::arg("maxSegLength") = 0.05,
+          py::arg("minNbPoints") = 100,
+          py::arg("initialGridpoints") = Vector()
+       )
 
       .def_property_readonly("dof", &GeometricPath::dof)
       .def_property_readonly("path_interval", &GeometricPath::pathInterval)

cpp/bindings/toppra_int.cpp

@@ -44,12 +44,17 @@ PYBIND11_MODULE(toppra_int, m) {
       .def_readwrite("feasible_sets", &toppra::ParametrizationData::feasible_sets)
       .def_readwrite("ret_code", &toppra::ParametrizationData::ret_code);
 
-  py::class_<algorithm::TOPPRA>(m, "TOPPRA")
-      .def(py::init<LinearConstraintPtrs, const GeometricPathPtr &>())
+  py::class_<PathParametrizationAlgorithm>(m, "PathParametrizationAlgorithm")
+      .def("setN", &algorithm::TOPPRA::setN)
+      .def("setGridpoints", &PathParametrizationAlgorithm::setGridpoints)
+      .def("solver", &PathParametrizationAlgorithm::solver)
+      .def("setInitialXBounds", &PathParametrizationAlgorithm::setInitialXBounds)
       .def("computePathParametrization", &algorithm::TOPPRA::computePathParametrization,
            py::arg("vel_start") = 0, py::arg("vel_end") = 0)
-      .def("setN", &algorithm::TOPPRA::setN)
       .def_property_readonly("parametrizationData", &algorithm::TOPPRA::getParameterizationData);
+
+  py::class_<algorithm::TOPPRA, PathParametrizationAlgorithm>(m, "TOPPRA")
+      .def(py::init<LinearConstraintPtrs, const GeometricPathPtr &>());
 }
 }  // namespace python
 }  // namespace toppra

cpp/package.xml

@@ -1,7 +1,7 @@
 <?xml version='1.0'?>
 <package format='2'>
  <name>toppra</name>
- <version>0.5.1</version>
+ <version>0.6.2</version>
  <description>Time-Optimal Path Parameterization</description>
 
  <maintainer email="[email protected]">Hung Pham</maintainer>

cpp/src/CMakeLists.txt

@@ -9,6 +9,8 @@ set(SOURCE_FILES
 
   toppra/solver.cpp
   toppra/solver/seidel.cpp
+  toppra/solver/qpOASES-wrapper.cpp
+  toppra/solver/glpk-wrapper.cpp
 
   toppra/geometric_path.cpp
   toppra/geometric_path/piecewise_poly_path.cpp
@@ -18,15 +20,7 @@ set(SOURCE_FILES
 
   toppra/algorithm.cpp
   toppra/algorithm/toppra.cpp
-  )
-if(BUILD_WITH_qpOASES)
-    list(APPEND SOURCE_FILES
-        toppra/solver/qpOASES-wrapper.cpp)
-endif()
-if(BUILD_WITH_GLPK)
-    list(APPEND SOURCE_FILES
-        toppra/solver/glpk-wrapper.cpp)
-endif()
+)
 
 add_library(toppra ${SOURCE_FILES})
 set_property(TARGET toppra PROPERTY POSITION_INDEPENDENT_CODE ON)

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/constraint.hpp

@@ -145,7 +145,7 @@ class LinearConstraint {
      * */
     LinearConstraint(Eigen::Index k, Eigen::Index m, bool constantF,
         bool uBound, bool xBound)
-      : m_discretizationType (Collocation)
+      : m_discretizationType (Interpolation)
       , m_k (k), m_m (m)
       , m_constantF (constantF)
       , m_hasUbounds (uBound)