diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index ace1836c37725c39a0bee90ea300397392f6d204..4b5f32867a8e20d01064887f135d3ba5e49ead3f 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -73,6 +73,8 @@ namespace hpp {
 
           virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
 
+          virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
           virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1,
               ConfigurationIn_t q2) const;
 
@@ -204,6 +206,8 @@ namespace hpp {
 
           virtual bool direction (const core::PathPtr_t& path) const;
 
+          virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
           virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2) const;
 
           virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 402857b7158042c323a70e29d44c4d9288ce84f0..1c2089f94d6c27c18424205f52b4539407ee7a15 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -256,6 +256,17 @@ namespace hpp {
         return constraint;
       }
 
+      bool Edge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2)
+	const
+      {
+        ConstraintSetPtr_t constraints = pathConstraint ();
+        constraints->configProjector ()->rightHandSideFromConfig(q1);
+        if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
+          return false;
+        }
+        return true;
+      }
+
       bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
 			ConfigurationIn_t q2)
 	const
@@ -311,6 +322,11 @@ namespace hpp {
         wkPtr_ = weak;
       }
 
+      bool WaypointEdge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const
+      {
+        return waypoint_.first->canConnect (q1, q2) && Edge::canConnect (q1, q2);
+      }
+
       bool WaypointEdge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
           ConfigurationIn_t q2) const
       {