diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 90d8cb135b3cd71c85f625e0723fb9434f753906..5ea948be020cb87421d7e12b684c2337606651db 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -58,6 +58,7 @@ namespace hpp {
       /// configuration in the start state by an admissible path.
       class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent
       {
+        friend class WaypointEdge;
         public:
           typedef core::RelativeMotion RelativeMotion;
 
@@ -363,6 +364,8 @@ namespace hpp {
           void init (const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const StateWkPtr_t& from,
               const StateWkPtr_t& to);
 
+          /// Initialize each of the internal edges
+          virtual void initialize ();
           /// Print the object in a stream.
           virtual std::ostream& print (std::ostream& os) const;
 
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index f7adef5ffd7c5f30e6dc8bbbf45fa64b50166123..548a26cc456959c022a198ff3f9c1cd958dcfde2 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -447,6 +447,22 @@ namespace hpp {
         wkPtr_ = weak;
       }
 
+      void WaypointEdge::initialize ()
+      {
+        Edge::initialize();
+        // Set error threshold of internal edge to error threshold of
+        // waypoint edge divided by number of edges.
+        assert(targetConstraint()->configProjector());
+        value_type eps(targetConstraint()->configProjector()
+                       ->errorThreshold()/(value_type)edges_.size());
+        for(Edges_t::iterator it(edges_.begin()); it != edges_.end(); ++it){
+          (*it)->initialize();
+          assert ((*it)->targetConstraint());
+          assert ((*it)->targetConstraint()->configProjector());
+          (*it)->targetConstraint()->configProjector()->errorThreshold(eps);
+        }
+      }
+
       bool WaypointEdge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const
       {
         /// TODO: This is not correct
@@ -533,6 +549,7 @@ namespace hpp {
         states_.back() = stateTo();
         const size_type nbDof = graph_.lock ()->robot ()->configSize ();
         configs_ = matrix_t (nbDof, number + 2);
+        invalidate();
       }
 
       void WaypointEdge::setWaypoint (const std::size_t index,