From bf9e71dd0e98790f403a283855d48598610bc041 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 1 Dec 2014 17:12:12 +0100
Subject: [PATCH] Fix bug regarding Nodes without neighbors in
ManipulationPlanner::extend.
---
src/manipulation-planner.cc | 3 +++
1 file changed, 3 insertions(+)
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 3fe36ae9..c54e8222 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -104,6 +104,9 @@ namespace hpp {
// Select next node in the constraint graph.
const ConfigurationPtr_t q_near = n_near->configuration ();
graph::NodePtr_t node = graph->getNode (*q_near);
+ if (node->neighbors ().totalWeight () == 0) {
+ return false;
+ }
graph::EdgePtr_t edge = graph->chooseEdge (node);
qProj_ = *q_rand;
if (!edge->applyConstraints (n_near, qProj_)) {
--
GitLab