diff --git a/src/path-projector/progressive.cc b/src/path-projector/progressive.cc index 8d34fb9a04aa63b16fec6ac75cd6af52f2252be7..906dbb7d0675fda1c155c685d55be47a19f394d7 100644 --- a/src/path-projector/progressive.cc +++ b/src/path-projector/progressive.cc @@ -61,21 +61,20 @@ namespace hpp { timeRange = toSplitRef.timeRange (); const Configuration_t& qb = toSplitRef (timeRange.first); curStep = step_; + curLength = std::numeric_limits <value_type>::max(); bool stop = false; /// Find the good length. /// Here, it would be good to have an upper bound of the Hessian /// of the constraint. do { - toSplitRef (qi, curStep); - if (!constraints->apply (qi)) { - stop = true; - break; - } - curLength = d (qb, qi); if (curStep < 0.02) { stop = true; break; } + toSplitRef (qi, curStep); + if (constraints->apply (qi)) { + curLength = d (qb, qi); + } curStep /= 2; } while (curLength > step_); if (stop) break;