diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 313b437402b87daa411ed73740edf15a65557aa0..4780d34ce320e55dce57496275516f2ca0fb51de 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 {