diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 6e4214dc3cae95750ded6b713bd5a4ed4060d1b3..7a66ad06c1484aac2f81435a7a5ed9afa14b1597 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -197,9 +197,12 @@ namespace hpp {
           return false;
         if (!waypoint_.first->build (pathToWaypoint, q1, config_, d))
           return false;
-        core::PathVectorPtr_t pv = core::PathVector::create (graph_.lock ()->robot ()->configSize ());
+        core::PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST (core::PathVector, pathToWaypoint);
+        if (!pv) {
+          pv = core::PathVector::create (graph_.lock ()->robot ()->configSize ());
+          pv->appendPath (pathToWaypoint);
+        }
         path = pv;
-        pv->appendPath (pathToWaypoint);
 
         core::PathPtr_t end;
         if (!Edge::build (end, config_, q2, d))