diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 932042a4ffd793c4268f4d590e940e7ea8cd0ebc..3e7869d473242fe5e6fea72bfbd68be8a890c9f9 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -21,8 +21,9 @@
 # include <map>
 # include <hpp/core/problem-solver.hh>
 # include <hpp/model/device.hh>
-# include <hpp/manipulation/object.hh>
-# include <hpp/manipulation/robot.hh>
+# include "hpp/manipulation/object.hh"
+# include "hpp/manipulation/robot.hh"
+# include "hpp/manipulation/graph/fwd.hh"
 
 namespace hpp {
   namespace manipulation {
@@ -77,29 +78,52 @@ namespace hpp {
       ///
       /// throw if no robot is registered with this name.
       DevicePtr_t robot (const std::string& name) const;
-      
+
       /// Get object with given name
       ///
       /// throw if no object is registered with this name.
       ObjectPtr_t object (const std::string& name) const;
       /// \}
 
+      /// \name Constraint graph
+      /// \{
+
+      /// Set the constraint graph
+      void constraintGraph (const graph::GraphPtr_t& graph);
+
+      /// Get the constraint graph
+      graph::GraphPtr_t constraintGraph () const;
+      /// \}
+
+      /// \name Solve problem
+      /// \{
+
+      /// Prepare the solver for a step by step planning.
+      /// and try to make direct connections (call PathPlanner::tryDirectPath)
+      /// \return the return value of PathPlanner::pathExists
+      virtual bool prepareSolveStepByStep ();
+
+      /// Set and solve the problem
+      virtual void solve ();
+
+      /// \}
+
       /// Add grasp
       void addGrasp( const DifferentiableFunctionPtr_t& constraint,
                      const model::GripperPtr_t& gripper,
-                     const HandlePtr_t& handle) 
+                     const HandlePtr_t& handle)
       {
         Grasp_t* ptr = new Grasp_t (gripper, handle);
 	GraspPtr_t shPtr (ptr);
         graspsMap_[constraint] = shPtr;
       }
-   
+
       /// get grapsMap
       GraspsMap_t& grasps()
       {
         return graspsMap_;
       }
- 
+
       /// get graps by name
       ///
       /// return NULL if no grasp named graspName
@@ -139,6 +163,7 @@ namespace hpp {
     private:
       typedef std::map <const std::string, DevicePtr_t> RobotsandObjects_t;
       RobotPtr_t robot_;
+      graph::GraphPtr_t constraintGraph_;
       /// Map of single robots to store before building a composite robot.
       RobotsandObjects_t robotsAndObjects_;
       GraspsMap_t graspsMap_;
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 2e48ae6216334a1d04417f96c4f752023fb0468b..b0f552866872b367d4794402b169ef35cd020218 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -17,11 +17,14 @@
 
 #include <hpp/util/pointer.hh>
 #include <hpp/util/debug.hh>
-#include <hpp/manipulation/object.hh>
-#include <hpp/manipulation/problem-solver.hh>
-#include <hpp/manipulation/robot.hh>
 #include <hpp/model/gripper.hh>
 
+#include "hpp/manipulation/object.hh"
+#include "hpp/manipulation/robot.hh"
+#include "hpp/manipulation/graph/graph.hh"
+#include "hpp/manipulation/manipulation-planner.hh"
+
+#include "hpp/manipulation/problem-solver.hh"
 
 namespace hpp {
   namespace manipulation {
@@ -69,6 +72,16 @@ namespace hpp {
       return object;
     }
 
+    void ProblemSolver::constraintGraph (const graph::GraphPtr_t& graph)
+    {
+      constraintGraph_ = graph;
+    }
+
+    graph::GraphPtr_t ProblemSolver::constraintGraph () const
+    {
+      return constraintGraph_;
+    }
+
     LockedDofPtr_t ProblemSolver::lockedDofConstraint (const std::string& name) const
     {
       LockedDofConstraintMap_t::const_iterator it =
@@ -94,10 +107,10 @@ namespace hpp {
     void ProblemSolver::resetConstraints ()
     {
       if (robot_)
-	constraints_ = ConstraintSet::create (robot_, 
-                                              "Default constraint set");    
+	constraints_ = ConstraintSet::create (robot_,
+                                              "Default constraint set");
       GraspsMap_t graspsMap = grasps();
-      for (GraspsMap_t::const_iterator itGrasp = graspsMap.begin() ;  
+      for (GraspsMap_t::const_iterator itGrasp = graspsMap.begin();
              itGrasp != graspsMap.end() ; itGrasp++) {
         GraspPtr_t grasp = itGrasp->second;
         GripperPtr_t gripper = grasp->first;
@@ -111,7 +124,7 @@ namespace hpp {
         }
       }
     }
-   
+
     void ProblemSolver::addConstraintToConfigProjector (
                           const std::string& constraintName,
                           const DifferentiableFunctionPtr_t& constraint)
@@ -132,5 +145,27 @@ namespace hpp {
         }
       }
     }
+
+    bool ProblemSolver::prepareSolveStepByStep ()
+    {
+      ManipulationPlannerPtr_t manipPlanner =
+        HPP_DYNAMIC_PTR_CAST (ManipulationPlanner, pathPlanner ());
+      if (manipPlanner)
+        manipPlanner->constraintGraph (constraintGraph_);
+      else
+        hppDout (warning, "The planner is not a manipulation planner.");
+      return core::ProblemSolver::prepareSolveStepByStep ();
+    }
+
+    void ProblemSolver::solve ()
+    {
+      ManipulationPlannerPtr_t manipPlanner =
+        HPP_DYNAMIC_PTR_CAST (ManipulationPlanner, pathPlanner ());
+      if (manipPlanner)
+        manipPlanner->constraintGraph (constraintGraph_);
+      else
+        hppDout (warning, "The planner is not a manipulation planner.");
+      core::ProblemSolver::solve ();
+    }
   } // namespace manipulation
 } // namespace hpp