diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index 566f43c807f399e0a463a637b91a3c9576566cd3..2cc6eaf48ae5a60199d4a728a6185660262ca531 100644
--- a/include/hpp/manipulation/manipulation-planner.hh
+++ b/include/hpp/manipulation/manipulation-planner.hh
@@ -20,7 +20,6 @@
 #include <hpp/model/configuration.hh>
 #include <hpp/core/basic-configuration-shooter.hh>
 #include <hpp/core/path-planner.hh>
-#include <hpp/core/problem.hh>
 #include <hpp/core/roadmap.hh>
 
 #include "hpp/manipulation/config.hh"
@@ -37,12 +36,6 @@ namespace hpp {
         static ManipulationPlannerPtr_t create (const core::Problem& problem,
             const core::RoadmapPtr_t& roadmap);
 
-        /// Set the constraint graph
-        void constraintGraph (const graph::GraphPtr_t& graph)
-        {
-          constraintGraph_ = graph;
-        }
-
         /// One step of extension.
         /// 
         /// A set of constraints is choosen using the graph of constraints.
@@ -50,14 +43,6 @@ namespace hpp {
         ///
         virtual void oneStep ();
 
-      protected:
-        /// Protected constructor
-        ManipulationPlanner (const core::Problem& problem,
-            const core::RoadmapPtr_t& roadmap);
-
-        /// Store weak pointer to itself
-        void init (const ManipulationPlannerWkPtr_t& weak);
-
         /// Extend a the configuration q_near toward q_rand.
         /// \param q_near the configuration to be extended.
         /// \param q_rand the configuration toward extension is performed.
@@ -67,18 +52,19 @@ namespace hpp {
         bool extend (const ConfigurationPtr_t &q_near,
             const ConfigurationPtr_t &q_rand, core::PathPtr_t& validPath);
 
-        /// Extend a node of the roadmap towards a configuration.
-        /// near The node of the roadmap to be extended.
-        /// target The configuration towards which extension is performed.
-        core::PathPtr_t extend(const core::NodePtr_t& near,
-            const ConfigurationPtr_t& target,
-            const graph::Edges_t& edge);
+      protected:
+        /// Protected constructor
+        ManipulationPlanner (const Problem& problem,
+            const core::RoadmapPtr_t& roadmap);
+
+        /// Store weak pointer to itself
+        void init (const ManipulationPlannerWkPtr_t& weak);
 
       private:
         /// Configuration shooter
         ConfigurationShooterPtr_t shooter_;
-        /// The graph of constraints
-        graph::GraphPtr_t constraintGraph_;
+        /// Pointer to the problem
+        const Problem& problem_;
         /// weak pointer to itself
         ManipulationPlannerWkPtr_t weakPtr_;
 
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 4612b0f9391d0ef061c5855d8331096d8d1f0b3f..07f2cd9feb75a7f512c6f9bdc9e15da05b154d17 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -18,6 +18,7 @@
 #include <hpp/core/path-validation.hh>
 
 #include "hpp/manipulation/robot.hh"
+#include "hpp/manipulation/problem.hh"
 #include "hpp/manipulation/manipulation-planner.hh"
 
 namespace hpp {
@@ -25,7 +26,13 @@ namespace hpp {
     ManipulationPlannerPtr_t ManipulationPlanner::create (const core::Problem& problem,
         const core::RoadmapPtr_t& roadmap)
     {
-      ManipulationPlanner* ptr = new ManipulationPlanner (problem, roadmap);
+      ManipulationPlanner* ptr;
+      try {
+        const Problem& p = dynamic_cast < const Problem& > (problem);
+        ptr = new ManipulationPlanner (p, roadmap);
+      } catch (std::exception&) {
+        throw std::invalid_argument ("The problem must be of type hpp::manipulation::Problem.");
+      }
       ManipulationPlannerPtr_t shPtr (ptr);
       ptr->init (shPtr);
       return shPtr;
@@ -107,10 +114,11 @@ namespace hpp {
         const ConfigurationPtr_t& q_rand,
         core::PathPtr_t& validPath)
     {
+      graph::GraphPtr_t graph = problem_.constraintGraph ();
       // Select next node in the constraint graph.
-      graph::Nodes_t nodes = constraintGraph_->getNode (*q_near);
-      graph::Edges_t edges = constraintGraph_->chooseEdge (nodes);
-      ConstraintPtr_t constraint = constraintGraph_->configConstraint (edges, *q_near);
+      graph::Nodes_t nodes = graph->getNode (*q_near);
+      graph::Edges_t edges = graph->chooseEdge (nodes);
+      ConstraintPtr_t constraint = graph->configConstraint (edges, *q_near);
       qProj_ = *q_rand;
       if (!constraint->apply (qProj_))
         return core::PathPtr_t();
@@ -122,10 +130,11 @@ namespace hpp {
       return pathValidation->validate (path, false, validPath);
     }
 
-    ManipulationPlanner::ManipulationPlanner (const core::Problem& problem,
+    ManipulationPlanner::ManipulationPlanner (const Problem& problem,
         const core::RoadmapPtr_t& roadmap) :
       core::PathPlanner (problem, roadmap),
       shooter_ (new core::BasicConfigurationShooter (problem.robot ())),
+      problem_ (problem),
       qProj_ (problem.robot ()->configSize ())
     {}