[CPP] Invented a parallel algorithm based on seidel implementation.

.gitignore

@@ -111,4 +111,5 @@ venv.bak/
 cpp/.clangd
 build*
 cpp/doc/CMakeFiles/*
-cpp/doc/doc/*
+cpp/doc/doc/*
+temp/

cpp/CMakeLists.txt

@@ -17,7 +17,7 @@ option(TOPPRA_DEBUG_ON "Set logging mode to on." OFF)
 option(TOPPRA_WARN_ON "Enable warning messages." ON)
 option(OPT_MSGPACK "Serialization using msgpack " OFF)
 # Bindings
-option(PYTHON_BINDINGS "Build bindings for Python" ON)
+option(PYTHON_BINDINGS "Build bindings for Python" OFF)
 set(PYTHON_VERSION 3.7 CACHE STRING "Build bindings for Python version")
 set(BUILD_WITH_PINOCCHIO_PYTHON false CACHE BOOL "Force compile with Pinocchio bindings")
 # This option is needed because sometime the flag
@@ -47,6 +47,13 @@ if(BUILD_WITH_GLPK)
     message(STATUS "Found glpk ${GLPK_LIBRARY} ${GLPK_INCLUDE_DIR}")
 endif(BUILD_WITH_GLPK)
 
+find_package(OpenMP REQUIRED)
+if (OPENMP_FOUND)
+    set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+    set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+endif()
+
 add_subdirectory(src)
 add_subdirectory(doc)
 add_subdirectory(bindings)

cpp/src/CMakeLists.txt

@@ -9,6 +9,7 @@ set(SOURCE_FILES
 
   toppra/solver.cpp
   toppra/solver/seidel.cpp
+  toppra/solver/seidel-parallel.cpp
 
   toppra/geometric_path.cpp
   toppra/geometric_path/piecewise_poly_path.cpp

cpp/src/toppra/algorithm.cpp

@@ -5,6 +5,16 @@
 #include <iostream>
 #include "toppra/toppra.hpp"
 
+#include <ctime>
+#include <sys/time.h>
+
+unsigned long long GetCurrentTimeUsec()
+{
+  struct timeval tv;
+  gettimeofday(&tv,NULL);
+  return ((unsigned long long)tv.tv_sec * 1000000 + (unsigned long long)tv.tv_usec);
+}
+
 namespace toppra {
 
 PathParametrizationAlgorithm::PathParametrizationAlgorithm(
@@ -13,15 +23,24 @@ PathParametrizationAlgorithm::PathParametrizationAlgorithm(
 
 ReturnCode PathParametrizationAlgorithm::computePathParametrization(value_type vel_start,
                                                                     value_type vel_end) {
+  size_t be = GetCurrentTimeUsec();
   initialize();
+  size_t en = GetCurrentTimeUsec();
+  printf("time of init:                             %fms\n", (en - be)/1000.0);
+  be = en;
   m_solver->setupSolver();
   Bound vel_ends;
   vel_ends.setConstant(vel_end);
   m_data.ret_code = computeControllableSets(vel_ends);
+  en = GetCurrentTimeUsec();
+  printf("time of computeControllableSets:          %fms\n", (en - be)/1000.0);
+  be = en;
   if ((int)m_data.ret_code > 0) {
     return m_data.ret_code;
   }
   m_data.ret_code = computeForwardPass(vel_start);
+  en = GetCurrentTimeUsec();
+  printf("time of computeForwardPass:               %fms\n", (en - be)/1000.0);
   return m_data.ret_code;
 };
 
@@ -67,6 +86,100 @@ ReturnCode PathParametrizationAlgorithm::computeControllableSets(
   return ret;
 }
 
+ReturnCode PathParametrizationAlgorithm::computePathParametrizationParallel(value_type vel_start,
+                                                                            value_type vel_end) {
+
+  size_t be = GetCurrentTimeUsec();
+  initialize();
+  size_t en = GetCurrentTimeUsec();
+  printf("time of init:                             %fms\n", (en - be)/1000.0);
+  be = en;
+
+  m_solver->setupSolver();
+  Bound vel_ends;
+  vel_ends.setConstant(vel_end);
+  m_data.ret_code = computeControllableSetsParallel(vel_ends);
+  en = GetCurrentTimeUsec();
+  printf("time of computeControllableSetsParallel:  %fms\n", (en - be)/1000.0);
+  be = en;
+  if ((int)m_data.ret_code > 0) {
+    return m_data.ret_code;
+  }
+
+  m_data.ret_code = computeForwardPass(vel_start);
+  en = GetCurrentTimeUsec();
+  printf("time of computeForwardPass:               %fms\n", (en - be)/1000.0);
+  return m_data.ret_code;
+};
+
+ReturnCode PathParametrizationAlgorithm::computeControllableSetsParallel(
+    const Bound &vel_ends) {
+  ReturnCode ret = ReturnCode::OK;
+  bool solver_ret;
+  Vector g_upper{2}, g_lower{2}, solution;
+  // Vectors  solution_upper, solution_lower;
+  // solution_upper.assign(m_N, Vector::Zero(2, 1));
+  // solution_lower.assign(m_N, Vector::Zero(2, 1));
+
+  g_upper << 1e-9, -1;
+  g_lower << -1e-9, 1;
+  m_data.controllable_sets.row(m_N) = vel_ends.cwiseAbs2();
+
+  size_t be = GetCurrentTimeUsec();
+
+  bool batch_ok = true;
+  printf("omp omp_get_max_threads %d\n", omp_get_max_threads());
+  #pragma omp parallel for schedule(dynamic)
+  for (int i = 0; i < m_N; i++) {
+    // solver_ret = m_solver->solveStagewiseBatch(i, g_upper, solution_upper[i]);
+    solver_ret = m_solver->solveStagewiseBatch(i, g_upper);
+    if (!solver_ret) {
+      ret = ReturnCode::ERR_FAIL_CONTROLLABLE;
+      printf("Fail: solveStagewiseBatch, upper problem, idx: %d\n", i);
+      batch_ok = false;
+    }
+
+    // solver_ret = m_solver->solveStagewiseBatch(i, g_lower, solution_lower[i]);
+    solver_ret = m_solver->solveStagewiseBatch(i, g_lower);
+    if (!solver_ret) {
+      ret = ReturnCode::ERR_FAIL_CONTROLLABLE;
+      printf("Fail: solveStagewiseBatch, lower problem, idx: %d\n", i);
+      batch_ok = false;
+    }
+  }
+  if (!batch_ok) return ret;
+
+  size_t en = GetCurrentTimeUsec();
+  printf("time of Batch:    %fms\n", (en - be)/1000.0);
+  be = en;
+
+  Bound x_next;
+  for (int i = m_N - 1; i != -1; i--) {
+    x_next = m_data.controllable_sets.row(i + 1);
+    solver_ret = m_solver->solveStagewiseBack(i, g_upper, x_next, solution);
+    if (!solver_ret) {
+      ret = ReturnCode::ERR_FAIL_CONTROLLABLE;
+      printf("Fail: solveStagewiseBack, upper problem, idx: %d\n", i);
+      break;
+    }
+    m_data.controllable_sets(i, 1) = solution[1];
+
+    solver_ret = m_solver->solveStagewiseBack(i, g_lower, x_next, solution);
+    if (!solver_ret) {
+      ret = ReturnCode::ERR_FAIL_CONTROLLABLE;
+      printf("Fail: solveStagewiseBack, lower problem, idx: %d\n", i);
+      break;
+    }
+    // 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]);
+  }
+  en = GetCurrentTimeUsec();
+  printf("time of Back:     %fms\n", (en - be)/1000.0);
+  return ret;
+}
+
 ReturnCode PathParametrizationAlgorithm::computeFeasibleSets() {
   initialize();
   ReturnCode ret = ReturnCode::OK;

cpp/src/toppra/algorithm.hpp

@@ -7,6 +7,8 @@
 #include <toppra/solver.hpp>
 #include <toppra/toppra.hpp>
 
+#include <omp.h>
+
 namespace toppra {
 
 /// Return code for Path Parametrization algorithm.
@@ -91,6 +93,15 @@ class PathParametrizationAlgorithm {
   virtual ReturnCode computePathParametrization(value_type vel_start = 0,
                                                 value_type vel_end = 0);
 
+  /** Compute the time parametrization of the given path in parallel.
+   *
+   * \param vel_start
+   * \param vel_end
+   * \return Return code.
+   */
+  virtual ReturnCode computePathParametrizationParallel(value_type vel_start = 0,
+                                                        value_type vel_end = 0);
+
   /** Compute the sets of feasible squared velocities.
    */
   ReturnCode computeFeasibleSets();
@@ -123,6 +134,10 @@ class PathParametrizationAlgorithm {
    */
   ReturnCode computeControllableSets(const Bound &vel_ends);
 
+  /** Compute the sets of controllable squared path velocities in parallel.
+   */
+  ReturnCode computeControllableSetsParallel(const Bound &vel_ends);
+
   /** To be implemented in child method. */
   LinearConstraintPtrs m_constraints;
   GeometricPathPtr m_path;

cpp/src/toppra/solver.hpp

@@ -81,6 +81,10 @@ class Solver {
         const Bound& x, const Bound& xNext,
         Vector& solution) = 0;
 
+    virtual bool solveStagewiseBatch(int i, const Vector& g){};
+
+    virtual bool solveStagewiseBack(int i, const Vector& g, const Bound& xNext, Vector& solution){};
+
     /// \brief Initialize the solver
     /// \note Child classes should call the parent implementation.
     virtual void initialize (const LinearConstraintPtrs& constraints, const GeometricPathPtr& path,