Repository: hungpham2511/toppra (language: python)

Issue:
Spurious out of bounds issue in the const accel parametrizer due to floating point imprecision
**Describe the bug**
When the upper bound of ConstAccel::pathInterval_impl() is passed to ConstAccel::eval_impl(), this can sometimes result in a call to the source path's eval() method with an out-of-bounds value. If this path is a PiecewisePolyPath, this results in an exception being thrown. It appears floating-point imprecision is the issue.

**To Reproduce**

1. Run the following snippet
```cpp
toppra::Vectors positions = {
  toppra::Vector::Zero(1),
  toppra::Vector::Zero(1)
};
toppra::Vectors velocities = {
  toppra::Vector::Zero(1),
  toppra::Vector::Zero(1)
};

double PATH_LEN = 25363210.115759313106536865234375;

toppra::Vector times = toppra::Vector::LinSpaced(2, 0, PATH_LEN);
auto std_times = std::vector<double>(times.data(), times.data() + times.size());

auto path = toppra::PiecewisePolyPath::constructHermite(positions, velocities, std_times);
auto ppath = std::make_shared<toppra::PiecewisePolyPath>(path);

toppra::Vector gridpoints = toppra::Vector::LinSpaced(10, 0, PATH_LEN);
toppra::Vector vsquared{10};
vsquared << 0, 0.1, 0.2, 0.3, 0.5, 0.5, 0.3, 0.2, 0.1, 0.0;
auto p = toppra::parametrizer::ConstAccel(ppath, gridpoints, vsquared);

auto bound = p.pathInterval();
auto qs = p.eval_single(bound[1]);
```

2. This results in an exception being thrown with the following error message.
```
Position 2.53632e+07 is outside of range [ 0, 2.53632e+07]
```
...from piecewise_poly_path.cpp:152

**Expected behavior**
The path should not throw an exception when evaluated with times which are within the path's reported interval.

**Version**
Tip of develop - commit: a70f6a217ddf20c42b62471ab52ce16884a320c1

Note: The spline parametrizer doesn't appear to suffer from the same issue.



Discussion / hints:

Fix patch:
diff --git a/HISTORY.md b/HISTORY.md
index 67554f1e..52bc41f4 100644
--- a/HISTORY.md
+++ b/HISTORY.md
@@ -3,6 +3,7 @@
 ## Unreleased
 
 - [cpp] Minor PR to make the cpp part compiles on windows64 (msvc). Thanks @ahoarau.
+- [cpp] Fixed out of bounds issue in parametrizer::ConstAccel.
 - [python] Fix linting error that causes CI failure
 - [ci] Do integration testing in gh action instead of circle CI
 
diff --git a/cpp/src/toppra/parametrizer/const_accel.cpp b/cpp/src/toppra/parametrizer/const_accel.cpp
index a5e8ad54..53503af3 100644
--- a/cpp/src/toppra/parametrizer/const_accel.cpp
+++ b/cpp/src/toppra/parametrizer/const_accel.cpp
@@ -61,6 +61,25 @@ Vectors ConstAccel::eval_impl(const Vector& times, int order) const {
   Vector ss, vs, us;
   TOPPRA_LOG_DEBUG("eval_impl. order=" << order);
   bool ret = evalParams(times, ss, vs, us);
+
+  auto interval = pathInterval_impl();
+  for (std::size_t i = 0; i < times.size(); ++i)
+  {
+    if ((times[i] < interval[0]) || (times[i] > interval[1]))
+    {
+      std::ostringstream oss;
+      oss << "Time " << times[i] << " is outside of range [ " << interval[0]
+        << ", " << interval[1] << ']';
+      throw std::runtime_error(oss.str());
+    }
+  }
+
+  // Even if the times are within the bounds of the parametrizer, numerical
+  // errors can still push our values outside the bounds of the path, so
+  // we clamp values of ss to the path interval
+  auto path_interval = m_path->pathInterval();
+  ss = ss.cwiseMax(path_interval[0]).cwiseMin(path_interval[1]);
+
   assert(ret);
   switch (order) {
     case 0:


Test patch:
diff --git a/cpp/tests/test_parametrizer.cpp b/cpp/tests/test_parametrizer.cpp
index c1ee0871..91d6e208 100644
--- a/cpp/tests/test_parametrizer.cpp
+++ b/cpp/tests/test_parametrizer.cpp
@@ -100,3 +100,35 @@ TEST_F(ParametrizeConstAccel, Correctness) {
     }
   }
 }
+
+TEST(ParametrizeConstAccelNoFixture, BoundsViolation) {
+    toppra::Vectors positions = {
+      toppra::Vector::Zero(1),
+      toppra::Vector::Zero(1)
+    };
+    toppra::Vectors velocities = {
+      toppra::Vector::Zero(1),
+      toppra::Vector::Zero(1)
+    };
+
+    double PATH_LEN = 3.51363644474459757560680;
+
+    toppra::Vector times = toppra::Vector::LinSpaced(2, 0, PATH_LEN);
+    auto std_times = std::vector<double>(times.data(), times.data() + times.size());
+
+    auto path = toppra::PiecewisePolyPath::constructHermite(positions, velocities, std_times);
+    auto ppath = std::make_shared<toppra::PiecewisePolyPath>(path);
+
+    toppra::Vector gridpoints = toppra::Vector::LinSpaced(10, 0, PATH_LEN);
+    toppra::Vector vsquared{10};
+    vsquared << 0, 0.1, 0.2, 0.3, 0.5, 0.5, 0.3, 0.2, 0.1, 0.0;
+    auto p = toppra::parametrizer::ConstAccel(ppath, gridpoints, vsquared);
+
+    auto bound = p.pathInterval();
+
+    const double EPS = 1e-8;
+    EXPECT_NO_THROW(p.eval_single(bound[0]));
+    EXPECT_THROW(p.eval_single(bound[0] - EPS), std::runtime_error);
+    EXPECT_NO_THROW(p.eval_single(bound[1]));
+    EXPECT_THROW(p.eval_single(bound[1] + EPS), std::runtime_error);
+}