diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh index a9d04541c12e79a77f3e3b228cb189135ec8aeec..429b7059f22358cdccf59b56d15ff7e7de06dca6 100644 --- a/include/hpp/manipulation/manipulation-planner.hh +++ b/include/hpp/manipulation/manipulation-planner.hh @@ -128,6 +128,8 @@ namespace hpp { void addFailure (TypeOfFailure t, const graph::EdgePtr_t& edge); + const value_type extendStep_; + mutable Configuration_t qProj_; }; /// \} diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc index 7476a7b371995d32fe3f86987943263212fc49da..11877598270f8f149a2109fb39031c672dcd5111 100644 --- a/src/manipulation-planner.cc +++ b/src/manipulation-planner.cc @@ -233,12 +233,18 @@ namespace hpp { addFailure (PATH_VALIDATION, edge); validPath = fullValidPath; } else { - if (fullyValid) validPath = fullValidPath; + if (extendStep_ == 1 || fullyValid) validPath = fullValidPath; else { const value_type& length = fullValidPath->length(); const value_type& t_init = fullValidPath->timeRange ().first; - validPath = fullValidPath->extract - (core::interval_t(t_init, t_init + length * 0.5)); + try { + validPath = fullValidPath->extract + (core::interval_t(t_init, t_init + length * extendStep_)); + } catch (const core::projection_error& e) { + hppDout (error, e.what()); + addFailure (PATH_PROJECTION_SHORTER, edge); + return false; + } } extendStatistics_.addSuccess (); hppDout (info, "Extension:" << std::endl @@ -372,6 +378,7 @@ namespace hpp { core::PathPlanner (problem, roadmap), shooter_ (problem.configurationShooter()), problem_ (problem), roadmap_ (roadmap), + extendStep_ (1), qProj_ (problem.robot ()->configSize ()) {}