diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc index 38db831c33bc8e830ce73dbbaff2faa272bea5e3..d9aa4ed8c2142934765472828cea56dcdd8e77db 100644 --- a/src/manipulation-planner.cc +++ b/src/manipulation-planner.cc @@ -209,18 +209,29 @@ namespace hpp { } else projPath = path; GraphPathValidationPtr_t pathValidation (problem_.pathValidation ()); PathValidationReportPtr_t report; + core::PathPtr_t fullValidPath; HPP_START_TIMECOUNTER (validatePath); + bool fullyValid = false; try { - pathValidation->validate (projPath, false, validPath, report); + fullyValid = pathValidation->validate + (projPath, false, fullValidPath, report); } catch (const core::projection_error& e) { hppDout (error, e.what ()); addFailure (PATH_VALIDATION, edge); return false; } HPP_STOP_TIMECOUNTER (validatePath); - if (validPath->length () == 0) + if (fullValidPath->length () == 0) { addFailure (PATH_VALIDATION, edge); - else { + validPath = fullValidPath; + } else { + if (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)); + } extendStatistics_.addSuccess (); hppDout (info, "Extension:" << std::endl << extendStatistics_);