Repository: hungpham2511/toppra (language: python)

Issue:
Fixed bug in ConstAccel parametrizer where evaluating valid times can sometimes result in an exception being thrown.
Fixes #184

Changes in this PR:
- Fixed bug where the `parametrizer::ConstAccel::eval_single()` can sometimes throw an out-of-bounds error, even when the time passed to it is supposedly within bounds as reported by `parametrizer::ConstAccel::pathInterval()`.

Checklists:
- [x] Update HISTORY.md with a single line describing this PR


## Description of the PR

Consider the following snippet:
```cpp
auto p = toppra::parametrizer::ConstAccel(path, gridpoints, vsquared);
auto bound = p.pathInterval();
p.eval_single(bound[1]));
```
The call to `eval_single(bound[1])` can sometimes throw an error for certain paths, even though the time `bound[1]` should be a valid evaluation time. This is due to a bug inside the `ConstAccel` parametrizer. This PR fixes this bug.

The `ConstAccel` object contains a pointer to a `GeometricPath` object called `m_path` which is injected in at construction. When `ConstAccel::eval_single()` or `ConstAccel::eval()` is called, the `ConstAccel` object translates the time passed in to a value of `s` which is subsequently used in a call to the underlying `m_path->eval()`.

As long as the time passed into `ConstAccel::eval()` in the first place is within the bounds defined by `ConstAccel::pathInterval()`, the resulting call to `m_path->eval()` should be within the bounds defined by `m_path->pathInterval()`, but due to floating point numerical errors and the way the translation works, the isn't always the case. Sometimes the resulting value of `s` passed to `m_path->eval()` can be outside the bounds defined by `m_path->pathInterval()`, and if `m_path` is of type `PiecewisePolyPath`, a `std::runtime_error` is thrown:

https://github.com/hungpham2511/toppra/blob/a70f6a217ddf20c42b62471ab52ce16884a320c1/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp#L148-L153

In practice, this means it's dangerous to sample arbitrary trajectories using e.g. Eigen's `LinSpaced()` function, as sometimes this will fail when the underlying `m_path` object throws an error, especially with times which are very close to the bounds.
```cpp
auto bound = p.pathInterval();
toppra::Vector times = toppra::Vector::LinSpaced(N, bound[0], bound[1]);
p.eval(times); // <-- can sometimes fail
```

Instead, the caller must do something nasty like this:
```cpp
auto bound = p.pathInterval();
toppra::Vector times = toppra::Vector::LinSpaced(N, bound[0], bound[1]);
const double EPS = 1e-5;
times[0] += EPS;
times[N-1] -= EPS;
p.eval(times); // <-- will hopefully never fail..??!
```

Note: We assume the following assertions hold true, which appears to be the case for the `LinSpaced()` function.
```cpp
toppra::Vector times = toppra::Vector::LinSpaced(N, A, B);
assert(times[0] == A);
assert(times[N-1] == B);
```

The most important thing this PR adds is a clamp on any value before it is passed to the underlying `m_path->eval()` in order to move it within the bounds defined by `m_path->pathInterval()`. However, this means most values passed to `ConstAccel::eval()` which are outside the bounds defined by `ConstAccel::pathInterval()` get silently clamped, which is not friendly for debugging. Thus, the second thing this PR adds is a specific check that all times passed to `ConstAccel::eval()` are within the range `ConstAccel::pathInterval()` otherwise a `std::runtime_error` is thrown, which is exactly what the `PeicewisePolyPath` class does.

This PR does not address any issue which might arise from the caller doing something like the following, either inadvertently or not.
```cpp
auto p = toppra::parametrizer::ConstAccel(path, gridpoints, vsquared);
auto bound = p.pathInterval();
p.eval_single(bound[1] + SOME_TINY_VALUE));
```

This is because the caller can check for this case themselves, whereas in the bug discussed above it is not possible to do so from the outside. Also, this is an obvious violation of the `ConstAccel` object's interface.
Fixed bug in ConstAccel parametrizer where evaluating valid times can sometimes result in an exception being thrown.
Fixes #184

Changes in this PR:
- Fixed bug where the `parametrizer::ConstAccel::eval_single()` can sometimes throw an out-of-bounds error, even when the time passed to it is supposedly within bounds as reported by `parametrizer::ConstAccel::pathInterval()`.

Checklists:
- [x] Update HISTORY.md with a single line describing this PR


## Description of the PR

Consider the following snippet:
```cpp
auto p = toppra::parametrizer::ConstAccel(path, gridpoints, vsquared);
auto bound = p.pathInterval();
p.eval_single(bound[1]));
```
The call to `eval_single(bound[1])` can sometimes throw an error for certain paths, even though the time `bound[1]` should be a valid evaluation time. This is due to a bug inside the `ConstAccel` parametrizer. This PR fixes this bug.

The `ConstAccel` object contains a pointer to a `GeometricPath` object called `m_path` which is injected in at construction. When `ConstAccel::eval_single()` or `ConstAccel::eval()` is called, the `ConstAccel` object translates the time passed in to a value of `s` which is subsequently used in a call to the underlying `m_path->eval()`.

As long as the time passed into `ConstAccel::eval()` in the first place is within the bounds defined by `ConstAccel::pathInterval()`, the resulting call to `m_path->eval()` should be within the bounds defined by `m_path->pathInterval()`, but due to floating point numerical errors and the way the translation works, the isn't always the case. Sometimes the resulting value of `s` passed to `m_path->eval()` can be outside the bounds defined by `m_path->pathInterval()`, and if `m_path` is of type `PiecewisePolyPath`, a `std::runtime_error` is thrown:

https://github.com/hungpham2511/toppra/blob/a70f6a217ddf20c42b62471ab52ce16884a320c1/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp#L148-L153

In practice, this means it's dangerous to sample arbitrary trajectories using e.g. Eigen's `LinSpaced()` function, as sometimes this will fail when the underlying `m_path` object throws an error, especially with times which are very close to the bounds.
```cpp
auto bound = p.pathInterval();
toppra::Vector times = toppra::Vector::LinSpaced(N, bound[0], bound[1]);
p.eval(times); // <-- can sometimes fail
```

Instead, the caller must do something nasty like this:
```cpp
auto bound = p.pathInterval();
toppra::Vector times = toppra::Vector::LinSpaced(N, bound[0], bound[1]);
const double EPS = 1e-5;
times[0] += EPS;
times[N-1] -= EPS;
p.eval(times); // <-- will hopefully never fail..??!
```

Note: We assume the following assertions hold true, which appears to be the case for the `LinSpaced()` function.
```cpp
toppra::Vector times = toppra::Vector::LinSpaced(N, A, B);
assert(times[0] == A);
assert(times[N-1] == B);
```

The most important thing this PR adds is a clamp on any value before it is passed to the underlying `m_path->eval()` in order to move it within the bounds defined by `m_path->pathInterval()`. However, this means most values passed to `ConstAccel::eval()` which are outside the bounds defined by `ConstAccel::pathInterval()` get silently clamped, which is not friendly for debugging. Thus, the second thing this PR adds is a specific check that all times passed to `ConstAccel::eval()` are within the range `ConstAccel::pathInterval()` otherwise a `std::runtime_error` is thrown, which is exactly what the `PeicewisePolyPath` class does.

This PR does not address any issue which might arise from the caller doing something like the following, either inadvertently or not.
```cpp
auto p = toppra::parametrizer::ConstAccel(path, gridpoints, vsquared);
auto bound = p.pathInterval();
p.eval_single(bound[1] + SOME_TINY_VALUE));
```

This is because the caller can check for this case themselves, whereas in the bug discussed above it is not possible to do so from the outside. Also, this is an obvious violation of the `ConstAccel` object's interface.
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 4cab36bc..3e405d0f 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.
 - [cpp] Fixed incorrect variable bug in seidel solver.
 - [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..4a08bef3 100644
--- a/cpp/src/toppra/parametrizer/const_accel.cpp
+++ b/cpp/src/toppra/parametrizer/const_accel.cpp
@@ -61,6 +61,26 @@ 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);
+
+  // Check that the requested evaluation times are within
+  // bounds. Small tolerances (based on TOPPRA_ABS_TOL and
+  // TOPPRA_REL_TOL) are allowed. This quantity is small (~1e-8 sec)
+  // so it has almost nil effect on the physical trajectory quality.
+  auto interval = pathInterval_impl();
+  if ((times.array() < (interval[0] * (1 - TOPPRA_REL_TOL) - TOPPRA_ABS_TOL)).any()) {
+    throw std::runtime_error("Request time instances out of numerical lower bounds.");
+  }
+  else if ((times.array() > (interval[1] * (1 + TOPPRA_REL_TOL) + TOPPRA_ABS_TOL)).any())
+  {
+    throw std::runtime_error("Request time instances out of numerical upper bounds.");
+  }
+
+  // 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:
diff --git a/cpp/src/toppra/toppra.hpp b/cpp/src/toppra/toppra.hpp
index 7f873c78..a3763550 100644
--- a/cpp/src/toppra/toppra.hpp
+++ b/cpp/src/toppra/toppra.hpp
@@ -24,9 +24,10 @@
 #endif
 
 // Use for checking if a quantity is very close to zero
-#ifndef TOPPRA_NEARLY_ZERO
 #define TOPPRA_NEARLY_ZERO 1e-8
-#endif
+#define TOPPRA_REL_TOL 1e-6
+#define TOPPRA_ABS_TOL 1e-8
+
 
 /// The TOPP-RA namespace
 namespace toppra {


Test patch:
diff --git a/cpp/tests/test_parametrizer.cpp b/cpp/tests/test_parametrizer.cpp
index c1ee0871..9afae5a4 100644
--- a/cpp/tests/test_parametrizer.cpp
+++ b/cpp/tests/test_parametrizer.cpp
@@ -62,13 +62,13 @@ TEST_F(ParametrizeConstAccel, Correctness) {
 
   // evaluation
   toppra::Vector ts = toppra::Vector{4};
-  ts << -1, 0, 3, 9;
+  ts << 1, 0, 3, 8;
   auto qs = p.eval(ts);
   auto qds = p.eval(ts, 1);
   auto qdds = p.eval(ts, 2);
   toppra::Vector q0, qd0, qdd0;
 
-  // test a few data points
+  // test for t = 0
   {
     // t = 0
     toppra::value_type t = 0, u = 0.15, v = 0, s = 0;
@@ -100,3 +100,38 @@ 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();
+
+    // Assert different ways that the evaluation attempt can fail
+    const double EPS = 1.5e-8;
+    const double REL_EPS = 1e-6;
+    EXPECT_NO_THROW(p.eval_single(bound[0]));
+    EXPECT_NO_THROW(p.eval_single(bound[0] - EPS / 2));
+    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] * (1 + REL_EPS) + 2 * EPS), std::runtime_error);
+}