Repository: hungpham2511/toppra (language: python)

Issue:
[CPP] Limit on the sd variable
Dear @hungpham2511,

is there a rationale for the hardcoded values
```c++
  x << 0, 100;
  x_next << 0, 1;
```
at line https://github.com/hungpham2511/toppra/blob/ec723091a717373413d5747d6c1e56d57a646fa1/cpp/src/toppra/algorithm.cpp#L40 ?

I couldn't find such a limit in the Python implementation.
Implement Seidel LP solver in C++
The Cython implementation of Seidel LP solver 

https://github.com/hungpham2511/toppra/blob/ba9328cd2f4e0ecb1eb71bba5c98fab78a8b4be6/toppra/solverwrapper/cy_seidel_solverwrapper.pyx#L65

is still the fastest solver for toppra. This solver was very important to achieve sub-millisecond solution time, or, a reasonable time for large and difficult problem.

It will be ideal to implement this in C++, then provide a binding back to the Python API.
[CPP] Limit on the sd variable
Dear @hungpham2511,

is there a rationale for the hardcoded values
```c++
  x << 0, 100;
  x_next << 0, 1;
```
at line https://github.com/hungpham2511/toppra/blob/ec723091a717373413d5747d6c1e56d57a646fa1/cpp/src/toppra/algorithm.cpp#L40 ?

I couldn't find such a limit in the Python implementation.


Discussion / hints:
You are right, there is no reason particular reason for that upper limit to be there.

However, we have had problems in the past when x is too large, the optimizer behaves quite badly. In the Python implementation, there is a scaling step to address this problem. See https://github.com/hungpham2511/toppra/blob/644e263269e45be8fc595bae59e959b9570edaa3/toppra/solverwrapper/hot_qpoases_solverwrapper.py#L120

This step should, however, be implemented by the solver wrappers like in the Python implementation because solvers are generally different when it comes to numerical instability.

 
How to call  Cython implementation in C++?   
Hi, I have no idea how to do that, and even if it's possible I doubt it's a good idea.

Bests,

Hung

________________________________
From: yy16808 <notifications@github.com>
Sent: Wednesday, July 15, 2020 9:33:14 PM
To: hungpham2511/toppra <toppra@noreply.github.com>
Cc: Hung Pham (Phạm Tiến Hùng) <hungpham2511@gmail.com>; Author <author@noreply.github.com>
Subject: Re: [hungpham2511/toppra] Implement Seidel LP solver in C++ (#109)


How to call Cython implementation in C++?

—
You are receiving this because you authored the thread.
Reply to this email directly, view it on GitHub<https://github.com/hungpham2511/toppra/issues/109#issuecomment-658770281>, or unsubscribe<https://github.com/notifications/unsubscribe-auth/ACKD6TAXZMDIFATDPN2DIWLR3WVZVANCNFSM4NAZL3SA>.

Maybe a re-implementation with cpp is fast enough. Actually I've already done this part after trying several third-party solvers (which are more generous but not fast enough for 2D LP). 
You are right, there is no reason particular reason for that upper limit to be there.

However, we have had problems in the past when x is too large, the optimizer behaves quite badly. In the Python implementation, there is a scaling step to address this problem. See https://github.com/hungpham2511/toppra/blob/644e263269e45be8fc595bae59e959b9570edaa3/toppra/solverwrapper/hot_qpoases_solverwrapper.py#L120

This step should, however, be implemented by the solver wrappers like in the Python implementation because solvers are generally different when it comes to numerical instability.

 

Fix patch:
diff --git a/HISTORY.md b/HISTORY.md
index 89c978ae..cc0b0282 100644
--- a/HISTORY.md
+++ b/HISTORY.md
@@ -12,6 +12,7 @@
 - [cpp] [#129][#129] Implement constant-acceleration trajectory parametrizer
 - [cpp] [#143][#143] Add PathParametrizationAlgorithm::setGridpoints
 - [cpp] [#146][#146] Add constraint::CartesianVelocityNorm
+- [cpp] [#158][#158] Add Seidel solver.
 
 ### Changed
 - [cpp] [#153] Fix variable mismatch in constraint::CartesianVelocityNorm
diff --git a/cpp/src/CMakeLists.txt b/cpp/src/CMakeLists.txt
index 07cd5d49..4ebffe7c 100644
--- a/cpp/src/CMakeLists.txt
+++ b/cpp/src/CMakeLists.txt
@@ -8,6 +8,7 @@ set(SOURCE_FILES
   toppra/constraint/cartesian_velocity_norm.cpp
 
   toppra/solver.cpp
+  toppra/solver/seidel.cpp
 
   toppra/geometric_path.cpp
   toppra/geometric_path/piecewise_poly_path.cpp
@@ -98,4 +99,5 @@ install(FILES
 install(FILES
   toppra/solver/qpOASES-wrapper.hpp
   toppra/solver/glpk-wrapper.hpp
+  toppra/solver/seidel.hpp
   DESTINATION include/toppra/solver)
diff --git a/cpp/src/toppra/algorithm.cpp b/cpp/src/toppra/algorithm.cpp
index 1907df9c..fe183afe 100644
--- a/cpp/src/toppra/algorithm.cpp
+++ b/cpp/src/toppra/algorithm.cpp
@@ -37,6 +37,11 @@ ReturnCode PathParametrizationAlgorithm::computeControllableSets(
 
   Matrix H;
   Bound x, x_next;
+  /// \todo The hard-coded bound below avoids numerical issues in LP / QP solvers
+  /// when \f$ x \f$ becomes too big. This issue should be addressed in the
+  /// solver wrapper themselfves as numerical behaviors is proper to each
+  /// individual solver.
+  /// See https://github.com/hungpham2511/toppra/issues/156
   x << 0, 100;
   x_next << 0, 1;
   for (std::size_t i = m_N - 1; i != (std::size_t)-1; i--) {
diff --git a/cpp/src/toppra/algorithm.hpp b/cpp/src/toppra/algorithm.hpp
index 538222df..ac77c65a 100644
--- a/cpp/src/toppra/algorithm.hpp
+++ b/cpp/src/toppra/algorithm.hpp
@@ -80,7 +80,7 @@ class PathParametrizationAlgorithm {
 
   /** \brief Get output or result of algorithm.
    */
-  ParametrizationData getParameterizationData() const { return m_data; };
+  const ParametrizationData& getParameterizationData() const { return m_data; };
 
   /** Compute the time parametrization of the given path.
    *
diff --git a/cpp/src/toppra/solver.cpp b/cpp/src/toppra/solver.cpp
index 28c698bf..c757dcd5 100644
--- a/cpp/src/toppra/solver.cpp
+++ b/cpp/src/toppra/solver.cpp
@@ -9,6 +9,7 @@
 #ifdef BUILD_WITH_GLPK
 #include <toppra/solver/glpk-wrapper.hpp>
 #endif
+#include <toppra/solver/seidel.hpp>
 
 namespace toppra {
 
@@ -18,7 +19,7 @@ SolverPtr Solver::createDefault() {
 #elif BUILD_WITH_GLPK
     return std::make_shared<solver::GLPKWrapper>();
 #else
-    return SolverPtr();
+    return std::make_shared<solver::Seidel>();
 #endif
 }
 
diff --git a/cpp/src/toppra/solver/qpOASES-wrapper.cpp b/cpp/src/toppra/solver/qpOASES-wrapper.cpp
index c1780fbe..3325f054 100644
--- a/cpp/src/toppra/solver/qpOASES-wrapper.cpp
+++ b/cpp/src/toppra/solver/qpOASES-wrapper.cpp
@@ -107,11 +107,13 @@ bool qpOASESWrapper::solveStagewiseOptim(std::size_t i,
     }
   }
 
-  TOPPRA_LOG_DEBUG("lA: " << std::endl << m_lA);
-  TOPPRA_LOG_DEBUG("hA: " << std::endl << m_hA);
-  TOPPRA_LOG_DEBUG(" A: " << std::endl << m_A);
-  TOPPRA_LOG_DEBUG("l : " << std::endl << l);
-  TOPPRA_LOG_DEBUG("h : " << std::endl << h);
+  TOPPRA_LOG_DEBUG("qpOASES QP:\n"
+      << "g: " << g.transpose() << '\n'
+      << "lA: " << m_lA.transpose() << '\n'
+      << "hA: " << m_hA.transpose() << '\n'
+      << "l: " << l << '\n'
+      << "h: " << h << '\n'
+      << "A:\n" << m_A);
 
   qpOASES::returnValue res;
   // TODO I assumed 1000 is the argument nWSR of the SQProblem.init function.
@@ -151,7 +153,14 @@ bool qpOASESWrapper::solveStagewiseOptim(std::size_t i,
   if (res == qpOASES::SUCCESSFUL_RETURN) {
     solution.resize(nbVars());
     m_impl->qp.getPrimalSolution(solution.data());
-    TOPPRA_LOG_DEBUG("solution: " << solution);
+    TOPPRA_LOG_DEBUG("solution: " << solution.transpose());
+#ifdef TOPPRA_DEBUG_ON
+    {
+      Vector lambdas (m_impl->qp.getNV() + m_impl->qp.getNC());
+      m_impl->qp.getDualSolution(lambdas.data());
+      TOPPRA_LOG_DEBUG("dual solution: " << lambdas.transpose());
+    }
+#endif
     solution = solution.cwiseMax(l.transpose());
     solution = solution.cwiseMin(h.transpose());
     assert((solution.transpose().array() <= h.array()).all());
diff --git a/cpp/src/toppra/solver/seidel.cpp b/cpp/src/toppra/solver/seidel.cpp
new file mode 100644
index 00000000..960d5056
--- /dev/null
+++ b/cpp/src/toppra/solver/seidel.cpp
@@ -0,0 +1,465 @@
+#include <toppra/solver/seidel.hpp>
+
+namespace toppra {
+namespace solver {
+
+typedef Eigen::Matrix<value_type, 2, 1> Vector2;
+typedef Eigen::Matrix<value_type, 1, 2> RowVector2;
+typedef Eigen::Matrix<value_type, 3, 1> Vector3;
+typedef Eigen::Matrix<value_type, 1, 3> RowVector3;
+typedef Eigen::Matrix<value_type, Eigen::Dynamic, 2> MatrixX2;
+typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3> MatrixX3;
+
+namespace seidel {
+struct LpSol {
+  bool feasible;
+  value_type optval;
+  Vector2 optvar;
+  std::array<int, 2> active_c;
+};
+
+std::ostream& operator<< (std::ostream& os, const LpSol& sol)
+{
+  if (!sol.feasible) return os << "infeasible";
+  os << "feasible: " << sol.optval
+    << "\n\toptvar: " << sol.optvar.transpose()
+    //<< "\n\tactive_c: " << sol.active_c
+    ;
+}
+
+const LpSol INFEASIBLE { false };
+
+constexpr double TINY = 1e-10;
+constexpr double SMALL = 1e-8;
+
+// bounds on variable used in seidel solver wrapper. u and x at every
+// stage is constrained to stay within this range.
+constexpr double VAR_MIN = -1e8;
+constexpr double VAR_MAX =  1e8;
+
+// absolute largest value that a variable can have. This bound should
+// be never be reached, however, in order for the code to work properly.
+constexpr double INF = 1e10;
+
+template<class Coeffs, class Vars>
+typename Coeffs::Scalar value(const Eigen::MatrixBase<Coeffs>& coeffs,
+    const Eigen::MatrixBase<Vars>& vars)
+{
+  static_assert(Coeffs::RowsAtCompileTime == 1
+      && Vars::ColsAtCompileTime == 1
+      && Coeffs::ColsAtCompileTime == Vars::RowsAtCompileTime + 1,
+      "Size mismatch between coefficient (1x(N+1)) and vars (Nx1)");
+  return coeffs.template head<Vars::RowsAtCompileTime>() * vars + coeffs[Coeffs::ColsAtCompileTime-1];
+}
+
+/**
+Solve a Linear Program with 1 variable.
+
+max   v[0] x + v[1]
+s.t.  A [ x 1 ] <= 0
+**/
+template<typename Derived>
+LpSol solve_lp1d(const RowVector2& v, const Eigen::MatrixBase<Derived>& A)
+{
+  value_type cur_min = -INF,
+             cur_max = INF;
+  int active_c_min = -1,
+      active_c_max = -2;
+  auto a (A.col(0)), b (A.col(1));
+
+  TOPPRA_LOG_DEBUG("Seidel LP 1D:\n"
+      << "v: " << v << '\n'
+      << "A:\n" << A << '\n'
+      );
+
+  for (int i = 0; i < a.size(); ++i) {
+    if (a[i] > TINY) {
+      value_type cur_x = - b[i] / a[i];
+      if (cur_x < cur_max) {
+        cur_max = cur_x;
+        active_c_max = i;
+      }
+    } else if (a[i] < -TINY) {
+      value_type cur_x = - b[i] / a[i];
+      if (cur_x > cur_min) {
+        cur_min = cur_x;
+        active_c_min = i;
+      }
+    } else if (b[i] > SMALL) {
+      TOPPRA_LOG_DEBUG("Seidel LP 1D: constraint " << i << " infeasible.");
+      return INFEASIBLE;
+    }
+    // else a[i] is approximately zero. do nothing.
+  }
+
+  if (cur_min - cur_max > SMALL)
+    return INFEASIBLE;
+
+  if (abs(v[0]) < TINY || v[0] < 0) {
+    // optimizing direction is perpencicular to the line, or is
+    // pointing toward the negative direction.
+    return LpSol{ true,
+      v[0] * cur_min + v[1],
+      { cur_min, 0. },
+      { active_c_min, false },
+    };
+  } else {
+    return LpSol{ true,
+      v[0] * cur_max + v[1],
+      { cur_max, 0. },
+      { active_c_max, 0 },
+    };
+  }
+}
+
+namespace internal {
+  // projective coefficients to the line
+  // add respective coefficients to A_1d
+  template<typename Derived>
+  void compute_denom_and_t_limit (const Eigen::MatrixBase<Derived>& Aj,
+      const Vector2& d_tan, const Vector2& zero_prj,
+      value_type& denom, value_type& t_limit)
+  {
+    denom = Aj.template head<2>() * d_tan;
+    t_limit = value(Aj, zero_prj);
+  }
+}
+
+/**
+Solve a LP with two variables.
+
+The LP is specified as follow:
+        max     v^T x
+        s.t.    A [ x^T 1 ]^T <= 0
+                low <= x <= high
+
+NOTE: A possible optimization for this function is pruning linear
+constraints that are clearly infeasible. This is not implemented
+because in my current code, the bottleneck is not in solving
+TOPP-RA but in setting up the parameters.
+
+Parameters
+----------
+\param v
+\param a
+\param b
+\param c
+\param low
+\param high
+\param active_c Contains (2) indicies of rows in a, b, c that are likely the
+    active constraints at the optimal solution.
+\param use_cache: bool
+\param index_map A view to a pre-allocated integer array, to map from
+    [1,...,nrows] to the considered entries. This array is created
+    to avoid the cost of initializing a new array.
+\param A_1d A view to an initialized array. This array is created to avoid
+    the cost of initializing a new array.
+
+\return A LpSol instance that contains the ouput. When LpSol.feasible is false,
+        other fields are not meaningful.
+**/
+LpSol solve_lp2d(const RowVector2& v,
+    const MatrixX3& A,
+    const Vector2& low, const Vector2& high,
+    std::array<int, 2> active_c, bool use_cache,
+    std::vector<int> index_map,
+    MatrixX2& A_1d)
+{
+  // number of working set recomputation
+  unsigned int nrows = A.rows();
+  Vector2 cur_optvar;
+
+  // absolute bounds used in solving the 1 dimensional
+  // optimization sub-problems. These bounds needs to be very
+  // large, so that they are never active at the optimization
+  // solution of these 1D subproblem..
+  LpSol sol;
+
+  // print all input to the algorithm
+  TOPPRA_LOG_DEBUG("Seidel LP 2D:\n"
+      << "v: " << v << '\n'
+      << "A:\n" << A << '\n'
+      << "lo: " << low .transpose() << '\n'
+      << "hi: " << high.transpose() << '\n'
+      );
+
+  if (use_cache) {
+    assert(index_map.size() == nrows);
+  } else {
+    index_map.resize(nrows);
+    for (int i = 0; i < nrows; ++i) index_map[i] = i;
+    A_1d = MatrixX2::Zero(nrows+4, 2);
+  }
+
+  // handle fixed bounds (low, high). The following convention is
+  // adhered to: fixed bounds are assigned the numbers: -1, -2, -3,
+  // -4 according to the following order: low[0], high[0], low[1],
+  // high[1].
+  if ((low.array() > high.array()).any()) {
+    TOPPRA_LOG_DEBUG("Seidel: incoherent bounds.");
+    return INFEASIBLE;
+  }
+  for (int i = 0; i < 2; ++i) {
+    if (v[i] > TINY) {
+      cur_optvar[i] = high[i];
+      sol.active_c[i] = (i == 0) ? -2 : -4;
+    } else {
+      cur_optvar[i] = low[i];
+      sol.active_c[i] = (i == 0) ? -1 : -3;
+    }
+  }
+  TOPPRA_LOG_DEBUG("cur_optvar = " << cur_optvar.transpose());
+
+  // If active_c contains valid entries, swap the first two indices
+  // in index_map to these values.
+  unsigned int cur_row = 2;
+  if (   active_c[0] >= 0 && active_c[0] < nrows
+      && active_c[1] >= 0 && active_c[1] < nrows
+      && active_c[0] != active_c[1]) {
+    // active_c contains valid indices
+    index_map[0] = active_c[1];
+    index_map[1] = active_c[0];
+    for (int i = 0; i < nrows; ++i)
+      if (i != active_c[0] && i != active_c[1])
+        index_map[cur_row++] = i;
+  } else
+    for (int i = 0; i < nrows; ++i)
+      index_map[i] = i;
+
+  // pre-process the inequalities, remove those that are redundant
+
+  // handle other constraints in a, b, c
+  for (int k = 0; k < nrows; ++k) {
+    int i = index_map[k];
+    // if current optimal variable satisfies the i-th constraint, continue
+    if (value(A.row(i), cur_optvar) < TINY)
+      continue;
+    // otherwise, project all constraints on the line defined by (a[i], b[i], c[i])
+    sol.active_c[0] = i;
+    // project the origin (0, 0) onto the new constraint
+    // let ax + by + c=0 b the new constraint
+    // let zero_prj be the projected point, one has
+    //     zero_prj =  1 / (a^2 + b^2) [a  -b] [-c]
+    //                                 [b   a] [ 0]
+    // this can be derived using perpendicularity
+    Vector2 zero_prj (A.row(i).head<2>());
+    zero_prj *= - A(i,2) / zero_prj.squaredNorm();
+
+    // Let x = zero_prj + d_tan * t
+    // where t is the variable of the future 1D LP.
+    Vector2 d_tan { -A(i,1), A(i,0) }; // vector parallel to the line
+    Vector2 v_1d { v * d_tan, 0 };
+
+    // project 4 + k constraints onto the parallel line. each
+    // constraint occupies a row of A_1d.
+    for (int j = 0; j < 4 + k; ++j) { // nb rows 1d.
+      value_type denom, t_limit;
+      switch (j-k) {
+        case 0: // j == k: handle low <= x
+          internal::compute_denom_and_t_limit(RowVector3 { -1., 0., low[0] },
+              d_tan, zero_prj, denom, t_limit);
+          break;
+        case 1: // j == k + 1: handle x <= high
+          internal::compute_denom_and_t_limit(RowVector3 { 1., 0., -high[0] },
+              d_tan, zero_prj, denom, t_limit);
+          break;
+        case 2: // j == k + 2: handle low <= y
+          internal::compute_denom_and_t_limit(RowVector3 { 0., -1., low[1] },
+              d_tan, zero_prj, denom, t_limit);
+          break;
+        case 3: // j == k + 3: handle y <= high
+          internal::compute_denom_and_t_limit(RowVector3 { 0., 1., -high[1] },
+              d_tan, zero_prj, denom, t_limit);
+          break;
+        default: // handle other constraint
+          internal::compute_denom_and_t_limit(A.row(index_map[j]),
+              d_tan, zero_prj, denom, t_limit);
+      }
+
+      if (denom > TINY)
+        A_1d.row(j) <<  1.,  t_limit / denom;
+      else if (denom < -TINY)
+        A_1d.row(j) << -1., -t_limit / denom;
+      else {
+        // Current constraint is parallel to the base one.
+        // if infeasible, return failure.
+        if (t_limit > SMALL) {
+          TOPPRA_LOG_DEBUG("Seidel: infeasible constraint. t_limit = " << t_limit
+              << ", denom = " << denom);
+          return INFEASIBLE;
+        }
+        // Otherwise, specify 0 <= 1
+        A_1d.row(j) << 0, -1.;
+      }
+    }
+
+    // solve the projected, 1 dimensional LP
+    TOPPRA_LOG_DEBUG("Seidel LP 2D activate constraint " << i);
+    LpSol sol_1d = solve_lp1d(v_1d, A_1d.topRows(4+k));
+    TOPPRA_LOG_DEBUG("Seidel LP 1D solution:\n" << sol_1d);
+
+    // 1d lp infeasible
+    if (!sol_1d.feasible) return INFEASIBLE;
+
+    // feasible, wrapping up
+    // compute the current optimal solution
+    cur_optvar = zero_prj + sol_1d.optvar[0] * d_tan;
+    TOPPRA_LOG_DEBUG("cur_optvar = " << cur_optvar.transpose());
+    // record the active constraint's index
+    switch (sol_1d.active_c[0] - k) {
+      case 0: sol.active_c[1] = -1; break;
+      case 1: sol.active_c[1] = -2; break;
+      case 2: sol.active_c[1] = -3; break;
+      case 3: sol.active_c[1] = -4; break;
+      default:
+              if (sol_1d.active_c[0] < k)
+                sol.active_c[1] = index_map[sol_1d.active_c[0]];
+              else
+                // the algorithm should not reach this point. If it
+                // does, this means the active constraint at the
+                // optimal solution is the fixed bound used in the 1
+                // dimensional subproblem. This should not happen
+                // though.
+                return INFEASIBLE;
+    }
+  }
+
+  for (int i = 0; i < nrows; ++i) {
+    value_type v = value(A.row(i), cur_optvar);
+    if (v > 0) {
+      TOPPRA_LOG_DEBUG("Seidel 2D: contraint " << i << " violated (" << v << " should be <= 0).");
+    }
+  }
+
+  TOPPRA_LOG_DEBUG("Seidel solution: " << cur_optvar.transpose()
+      << "\n active constraints " << sol.active_c[0] << " " << sol.active_c[1]);
+
+  // Fill the solution struct
+  sol.feasible = true;
+  sol.optvar = cur_optvar;
+  sol.optval = v * sol.optvar;
+  return sol;
+}
+
+}
+
+void Seidel::initialize (const LinearConstraintPtrs& constraints, const GeometricPathPtr& path,
+        const Vector& times)
+{
+  Solver::initialize (constraints, path, times);
+
+  // Currently only support Canonical Linear Constraint
+  assert(nbVars() == 2);
+
+  // structural variables
+  int nC = 2;
+  for (const Solver::LinearConstraintParams& linParam : m_constraintsParams.lin)
+    nC += linParam.F[0].rows();
+
+  int N (nbStages());
+
+  // init constraint coefficients for the 2d lps, which are
+  // m_A, m_low, m_high.
+  // The first dimensions of these array all equal N + 1.
+  m_A.assign(N+1, MatrixX3::Zero(nC, 3));
+  m_low  = MatrixX2::Constant(N + 1, 2, seidel::VAR_MIN);
+  m_high = MatrixX2::Constant(N + 1, 2, seidel::VAR_MAX);
+  int cur_index = 2;
+
+  for (const Solver::LinearConstraintParams& p : m_constraintsParams.lin) {
+    assert(N+1 == p.a.size());
+
+    auto nC_ = p.F[0].rows();
+    const bool identical = (p.F.size() == 1);
+    for (int i = 0; i < N+1; ++i) {
+      int k = (identical ? 0 : i);
+      m_A[i].middleRows(cur_index, nC_) <<
+        p.F[k] * p.a[i], p.F[k] * p.b[i], p.F[k] * p.c[i] - p.g[k];
+    }
+    cur_index += nC_;
+  }
+
+  for (const Solver::BoxConstraintParams& p : m_constraintsParams.box) {
+    if (!p.u.empty()) {
+      assert(p.u.size() == N+1);
+      for (int i = 0; i < N+1; ++i) {
+        m_low (i, 0) = std::max(m_low (i, 0), p.u[i][0]);
+        m_high(i, 0) = std::min(m_high(i, 0), p.u[i][1]);
+      }
+    }
+    if (!p.x.empty()) {
+      assert(p.x.size() == N+1);
+      for (int i = 0; i < N+1; ++i) {
+        m_low (i, 1) = std::max(m_low (i, 1), p.x[i][0]);
+        m_high(i, 1) = std::min(m_high(i, 1), p.x[i][1]);
+      }
+    }
+  }
+
+  // init constraint coefficients for the 1d LPs
+  m_A_1d = MatrixX2::Zero(nC + 4, 2);
+  m_index_map.resize(nC, 0);
+  m_active_c_up.fill(0);
+  m_active_c_down.fill(0);
+}
+
+bool Seidel::solveStagewiseOptim(std::size_t i,
+        const Matrix& H, const Vector& g,
+        const Bound& x, const Bound& xNext,
+        Vector& solution)
+{
+  if (H.size() > 0)
+    throw std::invalid_argument("Seidel can only solve LPs.");
+
+  int N (nbStages());
+  assert (i <= N && 0 <= i);
+
+  // fill coefficient
+  Vector2 low (m_low.row(i)),
+          high (m_high.row(i));
+
+  // handle x_min <= x_i <= x_max
+  low[1] = std::max(low[1], x[0]);
+  high[1] = std::min(high[1], x[1]);
+
+  // handle x_next_min <= 2 delta u + x_i <= x_next_max
+  if (i < N) {
+    value_type delta = deltas()[i];
+    if (xNext[0] < -seidel::INF) // TODO isnan(x_next_min)
+      m_A[i].row(0) << 0, 0, -1;
+    else
+      m_A[i].row(0) << -2*delta, -1, xNext[0];
+    if (xNext[1] > seidel::INF) // TODO isnan(x_next_max)
+      m_A[i].row(1) << 0, 0, -1;
+    else
+      m_A[i].row(1) << 2*delta, 1, -xNext[1];
+  } else {
+    // at the last stage, neglect this constraint
+    m_A[i].topLeftCorner<2,2>().setZero();
+    m_A[i].topRightCorner<2,1>().setConstant(-1);
+  }
+
+  // objective function (because seidel solver does max)
+  RowVector2 v (-g.head<2>());
+
+  // warmstarting feature: one in two solvers, upper and lower,
+  // is be selected depending on the sign of g[1]
+  bool upper (g[1] > 0);
+  auto& active_c = (upper ? m_active_c_up : m_active_c_down);
+  // solver selected:
+  // - upper: when computing the lower bound of the controllable set.
+  // - lower: when computing the lower bound of the controllable set,
+  //          or computing the parametrization in the forward pass.
+  seidel::LpSol lpsol = seidel::solve_lp2d(v, m_A[i],
+      low, high, active_c, true, m_index_map, m_A_1d);
+  if (lpsol.feasible) {
+    solution = lpsol.optvar;
+    active_c = lpsol.active_c;
+    return true;
+  }
+  TOPPRA_LOG_DEBUG("Seidel: solver fails (upper ? " << upper << ')');
+  return false;
+}
+
+} // namespace solver
+} // namespace toppra
diff --git a/cpp/src/toppra/solver/seidel.hpp b/cpp/src/toppra/solver/seidel.hpp
new file mode 100644
index 00000000..a78518ca
--- /dev/null
+++ b/cpp/src/toppra/solver/seidel.hpp
@@ -0,0 +1,44 @@
+#ifndef TOPPRA_SOLVER_SEIDEL_HPP
+#define TOPPRA_SOLVER_SEIDEL_HPP
+
+#include <memory.h>
+#include <toppra/solver.hpp>
+
+// Forward declare glpk solver.
+struct glp_prob;
+
+namespace toppra {
+namespace solver {
+
+/** Implementation of Seidel algorithm.
+ *
+ * */
+class Seidel : public Solver {
+  public:
+    Seidel () = default;
+
+    void initialize (const LinearConstraintPtrs& constraints, const GeometricPathPtr& path,
+        const Vector& times);
+
+    bool solveStagewiseOptim(std::size_t i,
+        const Matrix& H, const Vector& g,
+        const Bound& x, const Bound& xNext,
+        Vector& solution);
+
+  private:
+    typedef Eigen::Matrix<value_type, Eigen::Dynamic, 2> MatrixX2;
+    typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3> MatrixX3;
+    typedef std::vector<MatrixX3, Eigen::aligned_allocator<MatrixX3> > MatricesX3;
+
+    MatricesX3 m_A;
+    MatrixX2 m_low, m_high;
+
+    MatrixX2 m_A_1d;
+    std::vector<int> m_index_map;
+    std::array<int, 2> m_active_c_up, m_active_c_down;
+}; // class Seidel
+
+} // namespace solver
+} // namespace toppra
+
+#endif


Test patch:
diff --git a/cpp/tests/test_algorithm.cpp b/cpp/tests/test_algorithm.cpp
index 45862632..3df1a3bf 100644
--- a/cpp/tests/test_algorithm.cpp
+++ b/cpp/tests/test_algorithm.cpp
@@ -7,6 +7,13 @@
 #include <toppra/geometric_path.hpp>
 #include <toppra/geometric_path/piecewise_poly_path.hpp>
 #include <toppra/parametrizer/const_accel.hpp>
+#ifdef BUILD_WITH_qpOASES
+#include <toppra/solver/qpOASES-wrapper.hpp>
+#endif
+#ifdef BUILD_WITH_GLPK
+#include <toppra/solver/glpk-wrapper.hpp>
+#endif
+#include <toppra/solver/seidel.hpp>
 #include <toppra/toppra.hpp>
 
 #include "gtest/gtest.h"
@@ -50,6 +57,7 @@ feasible_sets = instance.compute_feasible_sets().
  */
 // clang-format on
 
+template<typename Solver>
 class ProblemInstance : public testing::Test {
  public:
   ProblemInstance() {
@@ -74,21 +82,26 @@ class ProblemInstance : public testing::Test {
   int nDof = 2;
 };
 
-TEST_F(ProblemInstance, GridpointsHasCorrectShape) {
-  toppra::algorithm::TOPPRA problem{v, path};
+using SolverTypes = ::testing::Types<
+#ifdef BUILD_WITH_qpOASES
+  toppra::solver::qpOASESWrapper,
+#endif
+#ifdef BUILD_WITH_GLPK
+  // toppra::solver::GLPKWrapper, // Failure due to numerical issue.
+#endif
+  toppra::solver::Seidel>;
+TYPED_TEST_SUITE(ProblemInstance, SolverTypes);
+
+TYPED_TEST(ProblemInstance, ControllableSets) {
+  toppra::algorithm::TOPPRA problem{this->v, this->path};
   problem.setN(50);
-  problem.computePathParametrization();
-  auto data = problem.getParameterizationData();
+  problem.solver(std::make_shared<TypeParam>());
+  auto ret_code = problem.computePathParametrization();
+  const auto& data = problem.getParameterizationData();
 
   ASSERT_EQ(data.gridpoints.size(), 51);
   ASSERT_TRUE(data.gridpoints.isApprox(toppra::Vector::LinSpaced(51, 0, 3)));
-}
 
-TEST_F(ProblemInstance, ControllableSets) {
-  toppra::algorithm::TOPPRA problem{v, path};
-  problem.setN(50);
-  auto ret_code = problem.computePathParametrization();
-  auto data = problem.getParameterizationData();
   toppra::Vector e_K_max(51);
   e_K_max << 0.06666667, 0.07624309, 0.08631706, 0.09690258, 0.1005511, 0.09982804,
       0.09979021, 0.1004364, 0.10178673, 0.10184412, 0.09655088, 0.09173679, 0.08734254,
@@ -106,11 +119,12 @@ TEST_F(ProblemInstance, ControllableSets) {
         << "idx: " << i;
 }
 
-TEST_F(ProblemInstance, OutputParmetrization) {
-  toppra::algorithm::TOPPRA problem{v, path};
+TYPED_TEST(ProblemInstance, OutputParmetrization) {
+  toppra::algorithm::TOPPRA problem{this->v, this->path};
   problem.setN(50);
+  problem.solver(std::make_shared<TypeParam>());
   auto ret_code = problem.computePathParametrization();
-  auto data = problem.getParameterizationData();
+  const auto& data = problem.getParameterizationData();
   toppra::Vector expected_parametrization(51);
   expected_parametrization << 0., 0.00799999, 0.01559927, 0.02295854, 0.03021812,
       0.0375065, 0.04494723, 0.05266502, 0.06079176, 0.06947278, 0.07887417, 0.08890758,
@@ -134,11 +148,12 @@ TEST_F(ProblemInstance, OutputParmetrization) {
   EXPECT_DOUBLE_EQ(data.parametrization(50), 0);
 }
 
-TEST_F(ProblemInstance, FeasibleSets) {
-  toppra::algorithm::TOPPRA problem{v, path};
+TYPED_TEST(ProblemInstance, FeasibleSets) {
+  toppra::algorithm::TOPPRA problem{this->v, this->path};
   problem.setN(50);
+  problem.solver(std::make_shared<TypeParam>());
   auto ret_code = problem.computeFeasibleSets();
-  auto data = problem.getParameterizationData();
+  const auto& data = problem.getParameterizationData();
   toppra::Vector expected_feasible_max(51);
   expected_feasible_max << 0.06666667, 0.07624309, 0.08631706, 0.09690258, 0.1005511,
       0.09982804, 0.09979021, 0.1004364, 0.10178673, 0.10388394, 0.10679654, 0.11062383,
@@ -158,14 +173,15 @@ TEST_F(ProblemInstance, FeasibleSets) {
         << ", abs diff=" << data.parametrization(i) - expected_feasible_max(i);
 }
 
-TEST_F(ProblemInstance, ParametrizeOutputTrajectory) {
-  toppra::algorithm::TOPPRA problem{v, path};
+TYPED_TEST(ProblemInstance, ParametrizeOutputTrajectory) {
+  toppra::algorithm::TOPPRA problem{this->v, this->path};
   problem.setN(50);
+  problem.solver(std::make_shared<TypeParam>());
   auto ret_code = problem.computePathParametrization();
 
   TOPPRA_LOG_DEBUG("Pre constructed");
   toppra::parametrizer::ConstAccel output_traj{
-      path, problem.getParameterizationData().gridpoints,
+      this->path, problem.getParameterizationData().gridpoints,
       problem.getParameterizationData().parametrization};
 
   // Qualitative assertion
@@ -175,9 +191,9 @@ TEST_F(ProblemInstance, ParametrizeOutputTrajectory) {
   auto interval = output_traj.pathInterval();
 
   // Qualitative assertion
-  for (int i = 0; i < path->dof(); i++) {
-    ASSERT_EQ(output_traj.eval_single(0)[i], path->eval_single(0)[i]);
+  for (int i = 0; i < this->path->dof(); i++) {
+    ASSERT_EQ(output_traj.eval_single(0)[i], this->path->eval_single(0)[i]);
     ASSERT_EQ(output_traj.eval_single(output_traj.pathInterval()[1])[i],
-              path->eval_single(path->pathInterval()[1])[i]);
+              this->path->eval_single(this->path->pathInterval()[1])[i]);
   }
 }
diff --git a/cpp/tests/test_solver.cpp b/cpp/tests/test_solver.cpp
index 104734fb..3f88b899 100644
--- a/cpp/tests/test_solver.cpp
+++ b/cpp/tests/test_solver.cpp
@@ -4,6 +4,7 @@
 #ifdef BUILD_WITH_GLPK
 #include <toppra/solver/glpk-wrapper.hpp>
 #endif
+#include <toppra/solver/seidel.hpp>
 
 #include <toppra/constraint/linear_joint_acceleration.hpp>
 #include <toppra/constraint/linear_joint_velocity.hpp>
@@ -12,9 +13,13 @@
 
 #include "gtest/gtest.h"
 
+constexpr int Ntrials = 10;
+
+std::map<std::string, toppra::Vectors> solutions;
+
 class Solver : public testing::Test {
 public:
-  Solver() : path(constructPath()) {
+  Solver() : path(constructPath()), g(Eigen::Vector2d{0.5, 2.}) {
 
     // path has the equation: 0 * x ^ 3 + 1 * x ^ 2 + 2 x ^ 1 + 3
   }
@@ -34,88 +39,67 @@ class Solver : public testing::Test {
   }
 
   std::shared_ptr<toppra::PiecewisePolyPath> path;
-};
+  toppra::Vector g;
 
-std::map<std::string, toppra::Vectors> solutions;
+  void testSolver(toppra::Solver& solver, const char* name)
+  {
+    using namespace toppra;
+
+    int nDof = path->dof();
+    Vector lb (-Vector::Ones(nDof)),
+           ub ( Vector::Ones(nDof));
+    LinearConstraintPtr ljv (new constraint::LinearJointVelocity (lb, ub));
+    LinearConstraintPtr lja (new constraint::LinearJointAcceleration (lb, ub));
+
+    int N = Ntrials;
+    Vector times (getTimes(N));
+
+    solver.initialize ({ ljv, lja }, path, times);
+
+    EXPECT_EQ(solver.nbStages(), N-1);
+    EXPECT_EQ(solver.nbVars(), 2);
+    ASSERT_EQ(solver.deltas().size(), N-1);
+    for (int i = 0; i < N-1; ++i)
+      EXPECT_NEAR(solver.deltas()[i], times[i+1] - times[i], 1e-10);
+
+    solver.setupSolver();
+    Vector solution;
+    Matrix H;
+    const value_type infty (std::numeric_limits<value_type>::infinity());
+    Bound x, xNext;
+    x << -infty, infty;
+    xNext << -infty, infty;
+    Vectors sols;
+    for (int i = 0; i < N; ++i) {
+      EXPECT_TRUE(solver.solveStagewiseOptim(i, H, g, x, xNext, solution));
+      EXPECT_EQ(solution.size(), solver.nbVars());
+      sols.emplace_back(solution);
+    }
+    solver.closeSolver();
 
-#ifdef BUILD_WITH_qpOASES
-TEST_F(Solver, qpOASESWrapper) {
-  using namespace toppra;
-  int nDof = path->dof();
-  Vector lb (-Vector::Ones(nDof)),
-         ub ( Vector::Ones(nDof));
-  LinearConstraintPtr ljv (new constraint::LinearJointVelocity (lb, ub));
-  LinearConstraintPtr lja (new constraint::LinearJointAcceleration (lb, ub));
-
-  int N = 10;
-  Vector times (getTimes(N));
-  solver::qpOASESWrapper solver;
-  solver.initialize ({ ljv, lja }, path, times);
-
-  EXPECT_EQ(solver.nbStages(), N-1);
-  EXPECT_EQ(solver.nbVars(), 2);
-  ASSERT_EQ(solver.deltas().size(), N-1);
-  for (int i = 0; i < N-1; ++i)
-    EXPECT_NEAR(solver.deltas()[i], times[i+1] - times[i], 1e-10);
-
-  solver.setupSolver();
-  Vector g (Vector::Ones(2)), solution;
-  Matrix H;
-  const value_type infty (std::numeric_limits<value_type>::infinity());
-  Bound x, xNext;
-  x << -infty, infty;
-  xNext << -infty, infty;
-  Vectors sols;
-  for (int i = 0; i < N; ++i) {
-    EXPECT_TRUE(solver.solveStagewiseOptim(i, H, g, x, xNext, solution));
-    EXPECT_EQ(solution.size(), solver.nbVars());
-    sols.emplace_back(solution);
+    solutions.emplace(name, sols);
   }
-  solver.closeSolver();
+};
 
-  solutions.emplace("qpOASES", sols);
+#ifdef BUILD_WITH_qpOASES
+TEST_F(Solver, qpOASESWrapper) {
+  toppra::solver::qpOASESWrapper solver;
+  testSolver(solver, "qpOASES");
 }
 #endif
 
 #ifdef BUILD_WITH_GLPK
 TEST_F(Solver, GLPKWrapper) {
-  using namespace toppra;
-  int nDof = path->dof();
-  Vector lb (-Vector::Ones(nDof)),
-         ub ( Vector::Ones(nDof));
-  LinearConstraintPtr ljv (new constraint::LinearJointVelocity (lb, ub));
-  LinearConstraintPtr lja (new constraint::LinearJointAcceleration (lb, ub));
-
-  int N = 10;
-  Vector times (getTimes(N));
-  solver::GLPKWrapper solver;
-  solver.initialize ({ ljv, lja }, path, times);
-
-  EXPECT_EQ(solver.nbStages(), N-1);
-  EXPECT_EQ(solver.nbVars(), 2);
-  ASSERT_EQ(solver.deltas().size(), N-1);
-  for (int i = 0; i < N-1; ++i)
-    EXPECT_NEAR(solver.deltas()[i], times[i+1] - times[i], 1e-10);
-
-  solver.setupSolver();
-  Vector g (Vector::Ones(2)), solution;
-  Matrix H;
-  const value_type infty (std::numeric_limits<value_type>::infinity());
-  Bound x, xNext;
-  x << -infty, infty;
-  xNext << -infty, infty;
-  Vectors sols;
-  for (int i = 0; i < N; ++i) {
-    EXPECT_TRUE(solver.solveStagewiseOptim(i, H, g, x, xNext, solution));
-    EXPECT_EQ(solution.size(), solver.nbVars());
-    sols.emplace_back(solution);
-  }
-  solver.closeSolver();
-
-  solutions.emplace("GLPK", sols);
+  toppra::solver::GLPKWrapper solver;
+  testSolver(solver, "GLPK");
 }
 #endif
 
+TEST_F(Solver, Seidel) {
+  toppra::solver::Seidel solver;
+  testSolver(solver, "Seidel");
+}
+
 // Check that all the solvers returns the same solution.
 // TODO each solver is expected to be tested on the same inputs. It should be
 // templated, so that we know the same problem is setup (with a template hook to