fix hungpham2511#244: Seidel LP 1D: incoherent bounds

mskripnik · Jan 15, 2024 · b47400c · b47400c
1 parent 80b3f23
commit b47400c
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 1 deletion.
diff --git a/cpp/src/toppra/solver/seidel-internal.hpp b/cpp/src/toppra/solver/seidel-internal.hpp
@@ -116,6 +116,14 @@ LpSol1d solve_lp1d(const RowVector2& v, const Eigen::MatrixBase<Derived>& A)
     }
   }
 
+  // In case upper bound becomes less than lower bound and their difference is not more than
+  // 2*ABS_TOLERANCE, extend both bounds to not make the problem infeasible due to numerical
+  // errors.
+  if (cur_max < cur_min && cur_min - cur_max < 2 * ABS_TOLERANCE) {
+    cur_min -= ABS_TOLERANCE;
+    cur_max += ABS_TOLERANCE;
+  }
+
   if (   cur_min - cur_max > std::max(std::abs(cur_min),std::abs(cur_max))*REL_TOLERANCE
       || cur_min == infinity
       || cur_max == -infinity) {

diff --git a/cpp/tests/test_solver.cpp b/cpp/tests/test_solver.cpp
@@ -288,7 +288,6 @@ TEST(SeidelFunctions_1D, infeasible_due_to_incoherent_bounds) {
   A << seidel::TINY / 2, 1;
   sol = seidel::solve_lp1d(v, A);
   EXPECT_FALSE(sol.feasible);
-
 }
 
 TEST(SeidelFunctions_1D, feasible_with_a_and_b_small) {
@@ -346,6 +345,25 @@ TEST(SeidelFunctions_1D, feasible_but_with_tolerated_constraint_violation) {
 
   auto sol = seidel::solve_lp1d(v, A);
   EXPECT_TRUE(sol.feasible);
+
+  {
+    // Issue #244
+    v = { -65.960772491990838, 0.0 };
+    A.resize(8, 2);
+    A <<
+     -65.9607724919908,                      -0.0151605259038078,
+      65.9607724919908,                     -0.00782362866419491,
+     0.468679477393087,                      5.6843418860808e-14,
+    -0.468679477393087,                                    -1000,
+                     0, -std::numeric_limits<double>::infinity(),
+                     0, -std::numeric_limits<double>::infinity(),
+     -65.9607724919908,                                        0,
+      65.9607724919908,                      -0.0229841545680027;
+
+    auto sol = seidel::solve_lp1d(v, A);
+    EXPECT_TRUE(sol.feasible);
+    EXPECT_NEAR(0, sol.optvar, TOPPRA_ABS_TOL);
+  }
 }
 
 TEST(SeidelFunctions, seidel_2d) {