diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index bfe54800572eea1d11dd416d88e55398af78a54c..06f485aa20fbbeadc85b0b206a2326f27ad713e0 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -311,6 +311,7 @@ namespace hpp {
 
           Waypoints_t waypoints_;
 
+          mutable Configuration_t init_;
           mutable matrix_t configs_;
           mutable Configuration_t result_;
 
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 44b2b6defc8f9fdaf83267c8391bb0f923ab6111..711f686aa41de8e3f4dad5be8463a462a330f2b1 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -364,7 +364,7 @@ namespace hpp {
            graph_.lock ()->robot ()->numberDof ());
         // Many times, this will be called rigth after WaypointEdge::applyConstraints so config_
         // already satisfies the constraints.
-        bool useCache = result_.isApprox (q2);
+        bool useCache = init_.isApprox (q1) && result_.isApprox (q2);
         if (!useCache) configs_.col (0) = q2;
 
         assert (waypoints_[0].first);
@@ -425,6 +425,7 @@ namespace hpp {
           }
         }
         bool success = Edge::applyConstraints (configs_.col (configs_.cols()-1), q);
+        init_ = qoffset;
         result_ = q;
         return success;
       }
@@ -434,6 +435,7 @@ namespace hpp {
         waypoints_.resize (number);
         const size_type nbDof = graph_.lock ()->robot ()->configSize ();
         configs_ = matrix_t (nbDof, number);
+        init_   = Configuration_t (nbDof);
         result_ = Configuration_t (nbDof);
       }