diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 1d636aff8bb5d1b7ea9527904605bed60405278f..7eea322ac597578b8bc803255c205ec307c05b53 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -202,7 +202,7 @@ namespace hpp {
           typedef std::pair < EdgePtr_t, NodePtr_t > Waypoint;
 
           Waypoint waypoint_;
-          mutable Configuration_t config_;
+          mutable Configuration_t config_, result_;
       }; // class WaypointEdge
 
       /// Edge that find intersection of level set.
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 7b27926fa70209974b0193f5d0ff2bc9a69b9794..0757f090ac7dea096e94755b79517c92d2e22640 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -204,9 +204,9 @@ namespace hpp {
       {
         assert (waypoint_.first);
         core::PathPtr_t pathToWaypoint;
-        // TO DO: Many times, this will be called rigth after WaypointEdge::applyConstraints so config_
+        // Many times, this will be called rigth after WaypointEdge::applyConstraints so config_
         // already satisfies the constraints.
-        config_ = q2;
+        if (!result_.isApprox (q2)) config_ = q2;
         if (!waypoint_.first->applyConstraints (q1, config_))
           return false;
         if (!waypoint_.first->build (pathToWaypoint, q1, config_, d))
@@ -230,10 +230,12 @@ namespace hpp {
       bool WaypointEdge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
       {
         assert (waypoint_.first);
-        if (!waypoint_.first->applyConstraints (qoffset, q))
-          return false;
         config_ = q;
-        return Edge::applyConstraints (config_, q);
+        if (!waypoint_.first->applyConstraints (qoffset, config_))
+          return false;
+        bool success = Edge::applyConstraints (config_, q);
+        result_ = q;
+        return success;
       }
 
       void WaypointEdge::createWaypoint (const unsigned d, const std::string& bname)
@@ -258,6 +260,7 @@ namespace hpp {
         edge->name (ss.str ());
         waypoint_ = Waypoint (edge, node);
         config_ = Configuration_t(graph_.lock ()->robot ()->configSize ());
+        result_ = Configuration_t(graph_.lock ()->robot ()->configSize ());
       }
 
       NodePtr_t WaypointEdge::node () const