From bcad3319b8951d8982d4c606c92f8eece9dbd762 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 14 Dec 2015 17:24:32 +0100
Subject: [PATCH] Add parameter to control the extension length in case of
collision
---
include/hpp/manipulation/manipulation-planner.hh | 2 ++
src/manipulation-planner.cc | 13 ++++++++++---
2 files changed, 12 insertions(+), 3 deletions(-)
diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index a9d0454..429b705 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 7476a7b..1187759 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 ())
{}
--
GitLab