diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc index 3fe36ae9914fd7e0f91ebfa16f0af81984488802..c54e82225d4890b865beb89bee3ea6f40f129228 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_)) {