diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index e101c1c1f4515361323ef00a357e15481df0900a..031ec16a365c939970d31ef9360e2b6174de493a 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -52,7 +52,7 @@ namespace hpp {
     HPP_PREDEF_CLASS (ProblemSolver);
     typedef ProblemSolver* ProblemSolverPtr_t;
     HPP_PREDEF_CLASS (Problem);
-    typedef Problem* ProblemPtr_t;
+    typedef boost::shared_ptr <Problem> ProblemPtr_t;
     HPP_PREDEF_CLASS (Roadmap);
     typedef boost::shared_ptr <Roadmap> RoadmapPtr_t;
     HPP_PREDEF_CLASS (RoadmapNode);
diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index 59d76e409a96e0f05bc27490dc8e5d29ef4ea64b..3225c8bcf8ec49478bb3a36e1f576b60a48ea0f7 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -35,7 +35,7 @@ namespace hpp {
         typedef core::Problem Parent;
 
         /// Constructor
-        Problem (DevicePtr_t robot);
+        static ProblemPtr_t create (DevicePtr_t robot);
 
         /// Set the graph of constraints
         void constraintGraph (const graph::GraphPtr_t& graph);
@@ -76,7 +76,15 @@ namespace hpp {
             const core::PathValidationBuilder_t& factory,
             const value_type& tol);
 
+      protected:
+        /// Constructor
+        Problem (DevicePtr_t robot);
+
+        void init (ProblemWkPtr_t wkPtr);
+
       private:
+        ProblemWkPtr_t wkPtr_;
+
         /// The graph of constraints
         graph::GraphPtr_t graph_;
 
diff --git a/src/graph-optimizer.cc b/src/graph-optimizer.cc
index e80411805b3a280543070106afd2b00022db9474..c9778841f460a5a2bbb47d0448b659d4b1457052 100644
--- a/src/graph-optimizer.cc
+++ b/src/graph-optimizer.cc
@@ -65,18 +65,18 @@ namespace hpp {
         if (isShort)
           toConcat = toOpt;
         else {
-          core::Problem p (problem().robot());
-          p.distance(problem().distance());
+          core::ProblemPtr_t p = core::Problem::create (problem().robot());
+          p->distance(problem().distance());
           // It should be ok to use the path validation of each edge because it
           // corresponds to the global path validation minus the collision pairs
           // disabled using the edge constraint.
           // p.pathValidation(gpv->innerValidation());
-          p.pathProjector(problem().pathProjector());
-          p.steeringMethod(edge->steeringMethod()->copy());
-          p.constraints(p.steeringMethod()->constraints());
-          p.constraints()->configProjector()->rightHandSideFromConfig(toOpt->initial());
-          p.pathValidation(edge->pathValidation());
-          pathOptimizer_ = factory_ (p);
+          p->pathProjector(problem().pathProjector());
+          p->steeringMethod(edge->steeringMethod()->copy());
+          p->constraints(p->steeringMethod()->constraints());
+          p->constraints()->configProjector()->rightHandSideFromConfig(toOpt->initial());
+          p->pathValidation(edge->pathValidation());
+          pathOptimizer_ = factory_ (*p);
           toConcat = pathOptimizer_->optimize (toOpt);
         }
         i_s = i_e;
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index e04aad159477a0061a057f3ce8060316aab50020..43a7c2adc65230a2df9484f18f42542b7d84854d 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -102,7 +102,7 @@ namespace hpp {
     }
 
     ProblemSolver::ProblemSolver () :
-      core::ProblemSolver (), robot_ (), problem_ (0x0)
+      core::ProblemSolver (), robot_ (), problem_ ()
     {
       robots.add ("hpp::manipulation::Device", manipulation::Device::create);
       robotType ("hpp::manipulation::Device");
@@ -168,10 +168,9 @@ namespace hpp {
 
     void ProblemSolver::resetProblem ()
     {
-      ProblemPtr_t p (new Problem (robot_));
+      ProblemPtr_t p (Problem::create(robot_));
       if (problem_) {
         p->parameters = problem_->parameters;
-	delete problem_;
       }
       initializeProblem (p);
     }
@@ -375,7 +374,7 @@ namespace hpp {
 
     void ProblemSolver::setTargetState (const graph::StatePtr_t state)
     {
-      problemTarget::StatePtr_t t =  problemTarget::State::create(NULL);
+      problemTarget::StatePtr_t t =  problemTarget::State::create(ProblemPtr_t());
       t->target(state);
       target_ = t;
     }
diff --git a/src/problem-target/state.cc b/src/problem-target/state.cc
index dcd138a3116aedd8f7cc9db886151b2a05c50b24..978d67c82ce6e35dd9ea150ebea6324d117acc67 100644
--- a/src/problem-target/state.cc
+++ b/src/problem-target/state.cc
@@ -55,7 +55,9 @@ namespace hpp {
 
       core::PathVectorPtr_t State::computePath(const core::RoadmapPtr_t& roadmap) const
       {
-        Astar astar (roadmap, problem_->distance (), state_);
+        core::ProblemPtr_t p = problem_.lock();
+        assert (p);
+        Astar astar (roadmap, p->distance (), state_);
         return astar.solution ();
       }
     } // namespace problemTarget
diff --git a/src/problem.cc b/src/problem.cc
index 0cbc31044cecce0c2ad89c146a06fa58e6fd716f..b7b94c269cf80075eeba3967484135f4322f070b 100644
--- a/src/problem.cc
+++ b/src/problem.cc
@@ -25,20 +25,32 @@
 
 namespace hpp {
   namespace manipulation {
+    ProblemPtr_t Problem::create (DevicePtr_t robot)
+    {
+      ProblemPtr_t p (new Problem (robot));
+      p->init(p);
+      return p;
+    }
+
     Problem::Problem (DevicePtr_t robot)
       : Parent (robot), graph_()
     {
+    }
+
+    void Problem::init (ProblemWkPtr_t wkPtr)
+    {
+      Parent::init (wkPtr);
+      wkPtr_ = wkPtr;
+
       Parent::steeringMethod (steeringMethod::Graph::create (*this));
-      distance (WeighedDistance::create (robot, graph_));
+      distance (WeighedDistance::create (HPP_DYNAMIC_PTR_CAST(Device, robot()), graph_));
       setPathValidationFactory(core::pathValidation::createDiscretizedCollisionChecking, 0.05);
-
-      // add<boost::any>("ManipulationPlanner/ExtendStep", (value_type)1);
     }
 
     void Problem::constraintGraph (const graph::GraphPtr_t& graph)
     {
       graph_ = graph;
-      graph_->problem (this);
+      graph_->problem (wkPtr_.lock());
       if (pathValidation ())
         pathValidation ()->constraintGraph (graph);
       WeighedDistancePtr_t d = HPP_DYNAMIC_PTR_CAST (WeighedDistance,