From ecf9e6209e011a4aeb060eeb8b588fd243088ddb Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 6 Aug 2014 17:49:36 +0200
Subject: [PATCH] Update ManipulationPlanner::oneStep to follow commit
b159534683f7a6670333b14ab7e1e7202f95ad36
---
src/manipulation-planner.cc | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 313b4374..4780d34c 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -69,10 +69,10 @@ namespace hpp {
bool pathIsValid = extend (near->configuration (), q_rand, path);
// Insert new path to q_near in roadmap
value_type t_final = path->timeRange ().second;
- if (t_final != path->timeRange ().first) {
+ if (pathIsValid && t_final != path->timeRange ().first) {
ConfigurationPtr_t q_new (new Configuration_t
((*path) (t_final)));
- if (!pathIsValid || !belongs (q_new, newNodes)) {
+ if (!belongs (q_new, newNodes)) {
newNodes.push_back (roadmap ()->addNodeAndEdges
(near, q_new, path));
} else {
--
GitLab