diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index ec6ec64fde4916112c0a23f1d4cf8735960ccd7b..f7384192ae5d18935f89a34d44737c86acd3c0c2 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -38,12 +38,7 @@ namespace hpp {
         Problem (DevicePtr_t robot);
 
         /// Set the graph of constraints
-        void constraintGraph (const graph::GraphPtr_t& graph)
-        {
-          graph_ = graph;
-          if (pathValidation ())
-            pathValidation ()->constraintGraph (graph);
-        }
+        void constraintGraph (const graph::GraphPtr_t& graph);
 
         /// Get the graph of constraints
         graph::GraphPtr_t constraintGraph () const
diff --git a/src/problem.cc b/src/problem.cc
index ba00d23c82ec18675462efd4d674517478062f0a..7758f09ceeb9da50dd09f0bd1e2247d46469bd04 100644
--- a/src/problem.cc
+++ b/src/problem.cc
@@ -15,6 +15,7 @@
 // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
 
 #include <hpp/manipulation/problem.hh>
+#include <hpp/manipulation/weighed-distance.hh>
 #include <hpp/manipulation/graph-steering-method.hh>
 
 namespace hpp {
@@ -23,6 +24,17 @@ namespace hpp {
       : Parent (robot), graph_()
     {
       Parent::steeringMethod (GraphSteeringMethod::create (this));
+      distance (WeighedDistance::create (robot, graph_));
+    }
+
+    void Problem::constraintGraph (const graph::GraphPtr_t& graph)
+    {
+      graph_ = graph;
+      if (pathValidation ())
+        pathValidation ()->constraintGraph (graph);
+      WeighedDistancePtr_t d = HPP_DYNAMIC_PTR_CAST (WeighedDistance,
+          distance ());
+      if (d) d->constraintGraph (graph);
     }
 
     GraphPathValidationPtr_t Problem::pathValidation () const