diff --git a/src/graph-optimizer.cc b/src/graph-optimizer.cc
index 852fc9047c9c17ea0cdce4085c2010f033106fbf..36a469e6e2d4107d5ec7ea679556c5b84d406a79 100644
--- a/src/graph-optimizer.cc
+++ b/src/graph-optimizer.cc
@@ -18,6 +18,8 @@
 
 #include <hpp/core/steering-method-straight.hh>
 
+#include <hpp/manipulation/graph/edge.hh>
+
 namespace hpp {
   namespace manipulation {
     PathVectorPtr_t GraphOptimizer::optimize (const PathVectorPtr_t& path)
@@ -51,7 +53,7 @@ namespace hpp {
           current = expanded->pathAtRank (i_e);
           c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
           if (!c && edge) break;
-          if (c && edge != c->edge ()) break;
+          if (c && edge->node() != c->edge ()->node()) break;
           toOpt->appendPath (current);
         }
         pathOptimizer_ = factory_ (this->problem ());