From b1ae3085055f95c9d810cf173fc29f0054b19852 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 27 Nov 2015 15:31:21 +0100
Subject: [PATCH] Do not add the full valid part path when path is only
partially valid.
---
src/manipulation-planner.cc | 17 ++++++++++++++---
1 file changed, 14 insertions(+), 3 deletions(-)
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 38db831..d9aa4ed 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_);
--
GitLab