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.
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.
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
double PATH_LEN = 25363210.115759313106536865234375;
toppra::Vector times = toppra::Vector::LinSpaced(2, 0, PATH_LEN); auto std_times = std::vector(times.data(), times.data() + times.size());
auto path = toppra::PiecewisePolyPath::constructHermite(positions, velocities, std_times); auto ppath = std::make_shared(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]);
Position 2.53632e+07 is outside of range [ 0, 2.53632e+07]