diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 3e7869d473242fe5e6fea72bfbd68be8a890c9f9..b8594b3c6cd667aed57465b12acf9bca4a3b0fdd 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -23,6 +23,7 @@
 # include <hpp/model/device.hh>
 # include "hpp/manipulation/object.hh"
 # include "hpp/manipulation/robot.hh"
+# include "hpp/manipulation/fwd.hh"
 # include "hpp/manipulation/graph/fwd.hh"
 
 namespace hpp {
@@ -95,19 +96,6 @@ namespace hpp {
       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,
@@ -160,9 +148,21 @@ namespace hpp {
       /// Objects are detected by dynamic cast.
       void buildCompositeRobot (const std::string& robotName,
 				const Names_t& robotNames);
+
+      /// Create new problem.
+      virtual void resetProblem ();
+
+      /// Get pointer to problem
+      ProblemPtr_t problem ()
+      {
+	return problem_;
+      }
+
     private:
       typedef std::map <const std::string, DevicePtr_t> RobotsandObjects_t;
       RobotPtr_t robot_;
+      /// The pointer should point to the same object as core::Problem.
+      ProblemPtr_t problem_;
       graph::GraphPtr_t constraintGraph_;
       /// Map of single robots to store before building a composite robot.
       RobotsandObjects_t robotsAndObjects_;
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index b0f552866872b367d4794402b169ef35cd020218..fdc68109a4cc647b7712f164d47bc78dc295dd2a 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -18,11 +18,13 @@
 #include <hpp/util/pointer.hh>
 #include <hpp/util/debug.hh>
 #include <hpp/model/gripper.hh>
+#include <hpp/core/roadmap.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.hh"
 
 #include "hpp/manipulation/problem-solver.hh"
 
@@ -72,6 +74,17 @@ namespace hpp {
       return object;
     }
 
+    void ProblemSolver::resetProblem ()
+    {
+      if (problem_)
+        delete (problem_);
+      problem_ = new Problem (robot_);
+      roadmap (core::Roadmap::create (problem_->distance (), problem_->robot()));
+      problem_->constraints ();
+      problem_->constraintGraph (constraintGraph_);
+      core::ProblemSolver::problem (problem_);
+    }
+
     void ProblemSolver::constraintGraph (const graph::GraphPtr_t& graph)
     {
       constraintGraph_ = graph;
@@ -145,27 +158,5 @@ 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