Closed stevegolton closed 2 years ago
Thank you for the pull request!
I will try to review and merge as soon as I can. Thanks again for the contribution.
May I know what you try to do ?
If 25363210 is in second, it is equal to 0.8 years, which is non-sense for a robot trajectory.
May I know what you try to do ?
If 25363210 is in second, it is equal to 0.8 years, which is non-sense for a robot trajectory.
I've encountered this bug on sensical trajectories - e.g. trajectories of a few seconds in duration - but I didn't manage to capture the conditions. The test here is just a canned example which happened to reproduce the issue.
Edit:
I just updated the test to use a more realistic path length:
double PATH_LEN = 3.51363644474459757560680;
May I know what you try to do ? If 25363210 is in second, it is equal to 0.8 years, which is non-sense for a robot trajectory.
I've encountered this bug on sensical trajectories - e.g. trajectories of a few seconds in duration - but I didn't manage to capture the conditions. The test here is just a canned example which happened to reproduce the issue.
Sounds interesting. Please let us know if you spot another instance of this error again. This can happen if you have a near-zero path velocity, which again can happen if you i) use the basic interpolation scheme and ii) have a tricky path.
May I know what you try to do ? If 25363210 is in second, it is equal to 0.8 years, which is non-sense for a robot trajectory.
I've encountered this bug on sensical trajectories - e.g. trajectories of a few seconds in duration - but I didn't manage to capture the conditions. The test here is just a canned example which happened to reproduce the issue.
Sounds interesting. Please let us know if you spot another instance of this error again. This can happen if you have a near-zero path velocity, which again can happen if you i) use the basic interpolation scheme and ii) have a tricky path.
It's quite easy to reproduce by trying random path lengths. Consider the following test, which tries up to 100 random path lengths on the ConstAccel parametrizer. On my machine, I usually end up with this bug cropping up within a handful of attempts. Note: I didn't include this test in the pull request, as randomness doesn't make for a good unit test.
The bug appears to be an artifact of trying to arrive at the same number via different methods which ends up with slightly different values due to floating point numerical errors. This is fine in the middle of the path but breaks down at the two ends of the path.
TEST(ParametrizeConstAccelNoFixture, BoundsViolationRandom) {
std::random_device rd;
std::mt19937 e2(rd());
std::uniform_real_distribution<> dist(0, 10.0);
for (int i = 0; i < 1e3; ++i)
{
double PATH_LEN = dist(e2);
std::cout << std::setprecision(32) << PATH_LEN << '\n';
toppra::Vectors positions = {
toppra::Vector::Zero(1),
toppra::Vector::Zero(1)
};
toppra::Vectors velocities = {
toppra::Vector::Zero(1),
toppra::Vector::Zero(1)
};
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]);
}
}
May I know what you try to do ? If 25363210 is in second, it is equal to 0.8 years, which is non-sense for a robot trajectory.
I've encountered this bug on sensical trajectories - e.g. trajectories of a few seconds in duration - but I didn't manage to capture the conditions. The test here is just a canned example which happened to reproduce the issue.
Sounds interesting. Please let us know if you spot another instance of this error again. This can happen if you have a near-zero path velocity, which again can happen if you i) use the basic interpolation scheme and ii) have a tricky path.
It's quite easy to reproduce by trying random path lengths. Consider the following test, which tries up to 100 random path lengths on the ConstAccel parametrizer. On my machine, I usually end up with this bug cropping up within a handful of attempts. Note: I didn't include this test in the pull request, as randomness doesn't make for a good unit test.
The bug appears to be an artifact of trying to arrive at the same number via different methods which ends up with slightly different values due to floating point numerical errors. This is fine in the middle of the path but breaks down at the two ends of the path.
TEST(ParametrizeConstAccelNoFixture, BoundsViolationRandom) { std::random_device rd; std::mt19937 e2(rd()); std::uniform_real_distribution<> dist(0, 10.0); for (int i = 0; i < 1e3; ++i) { double PATH_LEN = dist(e2); std::cout << std::setprecision(32) << PATH_LEN << '\n'; toppra::Vectors positions = { toppra::Vector::Zero(1), toppra::Vector::Zero(1) }; toppra::Vectors velocities = { toppra::Vector::Zero(1), toppra::Vector::Zero(1) }; 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]); } }
This certainly sounds like a bug :sweat_smile:
Fixes #184
Changes in this PR:
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 byparametrizer::ConstAccel::pathInterval()
.Checklists:
Description of the PR
Consider the following snippet:
The call to
eval_single(bound[1])
can sometimes throw an error for certain paths, even though the timebound[1]
should be a valid evaluation time. This is due to a bug inside theConstAccel
parametrizer. This PR fixes this bug.The
ConstAccel
object contains a pointer to aGeometricPath
object calledm_path
which is injected in at construction. WhenConstAccel::eval_single()
orConstAccel::eval()
is called, theConstAccel
object translates the time passed in to a value ofs
which is subsequently used in a call to the underlyingm_path->eval()
.As long as the time passed into
ConstAccel::eval()
in the first place is within the bounds defined byConstAccel::pathInterval()
, the resulting call tom_path->eval()
should be within the bounds defined bym_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 ofs
passed tom_path->eval()
can be outside the bounds defined bym_path->pathInterval()
, and ifm_path
is of typePiecewisePolyPath
, astd::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 underlyingm_path
object throws an error, especially with times which are very close to the bounds.Instead, the caller must do something nasty like this:
Note: We assume the following assertions hold true, which appears to be the case for the
LinSpaced()
function.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 bym_path->pathInterval()
. However, this means most values passed toConstAccel::eval()
which are outside the bounds defined byConstAccel::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 toConstAccel::eval()
are within the rangeConstAccel::pathInterval()
otherwise astd::runtime_error
is thrown, which is exactly what thePeicewisePolyPath
class does.This PR does not address any issue which might arise from the caller doing something like the following, either inadvertently or not.
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.