diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 4a1cc5bf582fd7fdabf34f8b19fb5c4200816479..9ce1e1090dd23f38864be0de7853c0512965f45b 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -258,7 +258,8 @@ namespace hpp {
 
         protected:
 	  WaypointEdge (const std::string& name) :
-	    Edge (name)
+	    Edge (name),
+            lastSucceeded_ (false)
 	    {
 	    }
           /// Initialization of the object.
@@ -273,6 +274,7 @@ namespace hpp {
           States_t states_;
 
           mutable matrix_t configs_;
+          mutable bool lastSucceeded_;
 
           WaypointEdgeWkPtr_t wkPtr_;
       }; // class WaypointEdge
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index edfa06bb249c8086e2c42470d941c3f2fa5c05a5..2066b85c927fa0ec15ee08e22b4fd722f464f86d 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -336,32 +336,35 @@ namespace hpp {
            graph_.lock ()->robot ()->numberDof ());
         // Many times, this will be called rigth after WaypointEdge::applyConstraints so config_
         // already satisfies the constraints.
-        size_type n = edges_.size() - 1;
-        bool useCache = configs_.col(0).isApprox (q1)
-          &&            configs_.col(n).isApprox (q2);
-        if (!useCache) {
-          configs_.col(0) = q1;
-          configs_.col(n) = q2;
-        }
-
-        for (std::size_t i = 0; i < edges_.size (); ++i) {
-          if (!useCache) configs_.col (i+1) = q2;
-          if (!edges_[i]->applyConstraints (configs_.col(i), configs_.col (i+1))) {
+        size_type n = edges_.size();
+        assert (configs_.cols() == n + 1);
+        bool useCache = lastSucceeded_
+          && configs_.col(0).isApprox (q1)
+          && configs_.col(n).isApprox (q2);
+        configs_.col(0) = q1;
+        configs_.col(n) = q2;
+
+        for (size_type i = 0; i < n; ++i) {
+          if (i < (n-1) && !useCache) configs_.col (i+1) = q2;
+          if (i < (n-1) && !edges_[i]->applyConstraints (configs_.col(i), configs_.col (i+1))) {
             hppDout (info, "Waypoint edge " << name() << ": applyConstraints failed at waypoint " << i << "."
                 << "\nUse cache: " << useCache
                 );
+            lastSucceeded_ = false;
             return false;
           }
           if (!edges_[i]->build (p, configs_.col(i), configs_.col (i+1))) {
             hppDout (info, "Waypoint edge " << name() << ": build failed at waypoint " << i << "."
                 << "\nUse cache: " << useCache
                 );
+            lastSucceeded_ = false;
             return false;
           }
           pv->appendPath (p);
         }
 
         path = pv;
+        lastSucceeded_ = true;
         return true;
       }
 
@@ -373,10 +376,12 @@ namespace hpp {
           configs_.col (i+1) = q;
           if (!edges_[i]->applyConstraints (configs_.col(i), configs_.col (i+1))) {
             q = configs_.col(i+1);
+            lastSucceeded_ = false;
             return false;
           }
         }
         q = configs_.col(edges_.size());
+        lastSucceeded_ = true;
         return true;
       }