diff --git a/include/hpp/manipulation/graph-path-validation.hh b/include/hpp/manipulation/graph-path-validation.hh
index 43b3e2ebd79ad30d6e8892f6df485f7dea6db873..402c0074f08b3fdd9b2fc0c65ccda9d76b122ed4 100644
--- a/include/hpp/manipulation/graph-path-validation.hh
+++ b/include/hpp/manipulation/graph-path-validation.hh
@@ -77,11 +77,15 @@ namespace hpp {
         /// \param pathValidation a PathValidation that is responsible for collision
         //         checking.
         //  \param graph A pointer to the constraint graph.
-        static GraphPathValidationPtr_t create (const PathValidationPtr_t& pathValidation, const GraphPtr_t& graph);
+        static GraphPathValidationPtr_t create (const PathValidationPtr_t& pathValidation);
+
+        template <typename T> static GraphPathValidationPtr_t create (const DevicePtr_t& robot, const value_type& stepSize);
+
+        void addObstacle (const hpp::core::CollisionObjectPtr_t&);
 
       protected:
         /// Constructor
-        GraphPathValidation (const PathValidationPtr_t& pathValidation, const GraphPtr_t& graph);
+        GraphPathValidation (const PathValidationPtr_t& pathValidation);
 
       private:
         /// Do validation regarding the constraint graph for PathVector
@@ -93,6 +97,13 @@ namespace hpp {
         /// Pointer to the constraint graph.
         GraphPtr_t constraintGraph_;
     };
+
+    template <typename T>
+      GraphPathValidationPtr_t GraphPathValidation::create
+      (const DevicePtr_t& robot, const value_type& stepSize)
+    {
+      return GraphPathValidation::create (T::create (robot, stepSize));
+    }
   } // namespace manipulation
 } // namespace hpp
 
diff --git a/include/hpp/manipulation/graph-steering-method.hh b/include/hpp/manipulation/graph-steering-method.hh
index 0c4dc27e197b8d2c965c515a7b23a07b00e523fe..6f5634b8829fe28b8ea345dd29f97a3c1b3e6e0b 100644
--- a/include/hpp/manipulation/graph-steering-method.hh
+++ b/include/hpp/manipulation/graph-steering-method.hh
@@ -51,6 +51,8 @@ namespace hpp {
         }
         /// \}
 
+        const core::WeighedDistancePtr_t& distance () const;
+
       protected:
         virtual PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
 
diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index f825dd77ecedc5895c7c3fa77ad61160f4d4fd89..b23507b91c879236f7534e90f4d3046579753557 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -36,17 +36,16 @@ namespace hpp {
         Problem (RobotPtr_t robot) : Parent (robot),
           graph_(), steeringMethod_ (new GraphSteeringMethod (robot))
         {
-          Parent::pathValidation (GraphPathValidation::create (Parent::pathValidation(), graph_));
-          steeringMethod (steeringMethod_);
+          Parent::steeringMethod (steeringMethod_);
         }
 
         /// Set the graph of constraints
         void constraintGraph (const graph::GraphPtr_t& graph)
         {
           graph_ = graph;
-          GraphPathValidationPtr_t gv = pathValidation();
-          if (gv) gv->constraintGraph(graph);
           steeringMethod_->constraintGraph (graph);
+          if (pathValidation ())
+            pathValidation ()->constraintGraph (graph);
         }
 
         /// Get the graph of constraints
@@ -61,14 +60,24 @@ namespace hpp {
           core::Problem::checkProblem ();
           if (!graph_)
             throw std::runtime_error ("No graph in the problem.");
+          if (!pathValidation ())
+            throw std::runtime_error ("No GraphPathValidation in the problem.");
+          if (!steeringMethod ())
+            throw std::runtime_error ("No GraphSteeringMethod in the problem.");
         }
 
         /// Get the path validation as a GraphPathValidation
-        GraphPathValidationPtr_t pathValidation ()
+        GraphPathValidationPtr_t pathValidation () const
         {
           return HPP_DYNAMIC_PTR_CAST (GraphPathValidation, Parent::pathValidation());
         }
 
+        /// Get the steering method as a GraphSteeringMethod
+        GraphSteeringMethodPtr_t steeringMethod () const
+        {
+          return steeringMethod_;
+        }
+
       private:
         /// The graph of constraints
         graph::GraphPtr_t graph_;
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
index 6b92f365a3a44dae234aa66406035145b7c1a53d..251788c31cc79b3350e2fe5eeb71f213e2864437 100644
--- a/src/graph-path-validation.cc
+++ b/src/graph-path-validation.cc
@@ -18,16 +18,14 @@
 
 namespace hpp {
   namespace manipulation {
-    GraphPathValidationPtr_t GraphPathValidation::create (
-        const PathValidationPtr_t& pathValidation, const GraphPtr_t& graph)
+    GraphPathValidationPtr_t GraphPathValidation::create (const PathValidationPtr_t& pathValidation)
     {
-      GraphPathValidation* p = new GraphPathValidation (pathValidation, graph);
+      GraphPathValidation* p = new GraphPathValidation (pathValidation);
       return GraphPathValidationPtr_t (p);
     }
 
-    GraphPathValidation::GraphPathValidation (
-        const PathValidationPtr_t& pathValidation, const GraphPtr_t& graph) :
-      pathValidation_ (pathValidation), constraintGraph_ (graph)
+    GraphPathValidation::GraphPathValidation (const PathValidationPtr_t& pathValidation) :
+      pathValidation_ (pathValidation), constraintGraph_ ()
     {}
 
     bool GraphPathValidation::validate (
@@ -43,6 +41,7 @@ namespace hpp {
       PathPtr_t validSubPart;
       if (reverse) {
         // TODO: This has never been tested.
+        assert (!reverse && "This has never been tested with reverse path");
         for (int i = path->numberPaths () - 1; i >= 0; i--) {
           // We should stop at the first non valid subpath.
           if (!impl_validate (path->pathAtRank (i), true, validSubPart)) {
@@ -130,5 +129,10 @@ namespace hpp {
       validPart = path->extract (std::make_pair (oldTmin,oldTmin));
       return false;
     }
+
+    void GraphPathValidation::addObstacle (const hpp::core::CollisionObjectPtr_t& collisionObject)
+    {
+      pathValidation_->addObstacle (collisionObject);
+    }
   } // namespace manipulation
 } // namespace hpp
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 07bf5f505d9e25273b152c019935caecf646c32a..001501ff079e3438965f3565dcd321387cbac39a 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -110,13 +110,13 @@ namespace hpp {
         addFailure (PROJECTION, edge);
         return false;
       }
-      core::SteeringMethodPtr_t sm (problem().steeringMethod());
-      core::PathPtr_t path = (*sm) (*q_near, qProj_);
-      if (!path) {
+      GraphSteeringMethodPtr_t sm = problem_.steeringMethod();
+      core::PathPtr_t path;
+      if (!edge->build (path, *q_near, qProj_, *(sm->distance ()))) {
         addFailure (STEERING_METHOD, edge);
         return false;
       }
-      core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
+      GraphPathValidationPtr_t pathValidation (problem_.pathValidation ());
       pathValidation->validate (path, false, validPath);
       if (validPath->length () == 0)
         addFailure (PATH_VALIDATION, edge);
@@ -140,7 +140,7 @@ namespace hpp {
       Reasons r = it->second;
       extendStatistics_.addFailure (r.get (t));
       hppDout (info, "Extension failed." << std::endl
-            << extendStatistics_);
+          << extendStatistics_);
     }
 
     inline void ManipulationPlanner::tryConnect (const core::Nodes_t nodes)
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 5bc508f4c0b22412c7cc435e5bacf936fae78bd9..194c5cf5938bfe6b8e89cd710214f64fd0707923 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -91,6 +91,8 @@ namespace hpp {
     {
       problem_ = problem;
       core::ProblemSolver::initializeProblem (problem_);
+      if (problem_->pathValidation ())
+        problem_->pathValidation ()->constraintGraph (constraintGraph_);
     }
 
     void ProblemSolver::constraintGraph (const graph::GraphPtr_t& graph)