Merge remote-tracking branch 'florent/devel' into devel

include/hpp/manipulation/fwd.hh

@@ -53,6 +53,7 @@ namespace hpp {
     typedef ProblemSolver* ProblemSolverPtr_t;
     HPP_PREDEF_CLASS (Problem);
     typedef boost::shared_ptr <Problem> ProblemPtr_t;
+    typedef boost::shared_ptr <const Problem> ProblemConstPtr_t;
     HPP_PREDEF_CLASS (Roadmap);
     typedef boost::shared_ptr <Roadmap> RoadmapPtr_t;
     HPP_PREDEF_CLASS (RoadmapNode);

include/hpp/manipulation/graph-node-optimizer.hh

@@ -46,13 +46,14 @@ namespace hpp {
     class HPP_MANIPULATION_DLLAPI GraphNodeOptimizer : public PathOptimizer
     {
       public:
-        static GraphNodeOptimizerPtr_t create (const core::Problem& problem);
+        static GraphNodeOptimizerPtr_t create
+	  (const core::ProblemConstPtr_t& problem);
 
         virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
 
       protected:
         /// Constructor
-        GraphNodeOptimizer (const core::Problem& problem) :
+        GraphNodeOptimizer (const core::ProblemConstPtr_t& problem) :
           PathOptimizer (problem)
         {}
 

include/hpp/manipulation/graph-optimizer.hh

@@ -46,7 +46,8 @@ namespace hpp {
         typedef core::PathOptimizerBuilder_t PathOptimizerBuilder_t;
 
         template <typename TraitsOrInnerType>
-          static GraphOptimizerPtr_t create (const core::Problem& problem);
+          static GraphOptimizerPtr_t create
+	  (const core::ProblemConstPtr_t& problem);
 
         virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
 
@@ -58,7 +59,8 @@ namespace hpp {
 
       protected:
         /// Constructor
-        GraphOptimizer (const core::Problem& problem, PathOptimizerBuilder_t factory) :
+        GraphOptimizer (const core::ProblemConstPtr_t& problem,
+			PathOptimizerBuilder_t factory) :
           PathOptimizer (problem), factory_ (factory), pathOptimizer_ ()
         {}
 
@@ -73,7 +75,7 @@ namespace hpp {
     /// Member function definition
     template <typename TraitsOrInnerType>
       GraphOptimizerPtr_t GraphOptimizer::create
-      (const core::Problem& problem)
+      (const core::ProblemConstPtr_t& problem)
     {
       return GraphOptimizerPtr_t (
           new GraphOptimizer (problem, TraitsOrInnerType::create)

include/hpp/manipulation/manipulation-planner.hh

@@ -40,7 +40,8 @@ namespace hpp {
         typedef std::list<std::size_t> ErrorFreqs_t;
 
         /// Create an instance and return a shared pointer to the instance
-        static ManipulationPlannerPtr_t create (const core::Problem& problem,
+        static ManipulationPlannerPtr_t create
+	  (const core::ProblemConstPtr_t& problem,
             const core::RoadmapPtr_t& roadmap);
 
         /// One step of extension.
@@ -71,7 +72,7 @@ namespace hpp {
 
       protected:
         /// Protected constructor
-        ManipulationPlanner (const Problem& problem,
+        ManipulationPlanner (const ProblemConstPtr_t& problem,
             const RoadmapPtr_t& roadmap);
 
         /// Store weak pointer to itself
@@ -88,7 +89,7 @@ namespace hpp {
         /// Configuration shooter
         ConfigurationShooterPtr_t shooter_;
         /// Pointer to the problem
-        const Problem& problem_;
+        ProblemConstPtr_t problem_;
         /// Pointer to the roadmap
         RoadmapPtr_t roadmap_;
         /// weak pointer to itself

include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh

@@ -36,8 +36,8 @@ namespace hpp {
           typedef hpp::core::PathVectorPtr_t PathVectorPtr_t;
           typedef boost::shared_ptr<EnforceTransitionSemantic> Ptr_t;
 
-          static Ptr_t create (const core::Problem& problem) {
-            const Problem& p = dynamic_cast <const Problem&> (problem);
+          static Ptr_t create (const core::ProblemConstPtr_t& problem) {
+            ProblemConstPtr_t p (HPP_DYNAMIC_PTR_CAST(const Problem, problem));
             return Ptr_t (new EnforceTransitionSemantic (p));
           }
 
@@ -45,11 +45,11 @@ namespace hpp {
 
         protected:
           /// Constructor
-          EnforceTransitionSemantic (const Problem& problem) :
+          EnforceTransitionSemantic (const ProblemConstPtr_t& problem) :
             PathOptimizer (problem), problem_ (problem) {}
 
         private:
-          const Problem& problem_;
+          ProblemConstPtr_t problem_;
       };
 
       /// \}

include/hpp/manipulation/path-optimization/random-shortcut.hh

@@ -35,13 +35,14 @@ namespace hpp {
       {
         public:
           /// Return shared pointer to new object.
-          static RandomShortcutPtr_t create (const core::Problem& problem)
+          static RandomShortcutPtr_t create
+	    (const core::ProblemConstPtr_t problem)
           {
             return RandomShortcutPtr_t (new RandomShortcut (problem));
           }
 
         protected:
-          RandomShortcut (const core::Problem& problem)
+          RandomShortcut (const core::ProblemConstPtr_t& problem)
             : core::pathOptimization::RandomShortcut (problem)
           {}
 

include/hpp/manipulation/path-optimization/small-steps.hh

@@ -39,7 +39,7 @@ namespace hpp {
       class HPP_MANIPULATION_DLLAPI SmallSteps : public PathOptimizer
       {
         public:
-          static SmallStepsPtr_t create (const core::Problem& problem)
+          static SmallStepsPtr_t create (const core::ProblemConstPtr_t& problem)
           {
             SmallSteps* ptr (new SmallSteps (problem));
             return SmallStepsPtr_t (ptr);
@@ -49,7 +49,7 @@ namespace hpp {
 
         protected:
           /// Constructor
-          SmallSteps (const core::Problem& problem) :
+          SmallSteps (const core::ProblemConstPtr_t& problem) :
             PathOptimizer (problem)
         {}
       };

include/hpp/manipulation/path-optimization/spline-gradient-based.hh

@@ -45,19 +45,19 @@ namespace hpp {
           using typename Parent_t::Splines_t;
 
           /// Return shared pointer to new object.
-          static Ptr_t create (const Problem& problem);
+          static Ptr_t create (const ProblemConstPtr_t& problem);
 
           /// This is only for compatibility purpose (with ProblemSolver).
           /// problem is statically casted to an object of type
           /// const manipulation::Problem& and method create(const Problem&)
           /// is called.
-          static Ptr_t createFromCore (const core::Problem& problem);
+          static Ptr_t createFromCore (const core::ProblemConstPtr_t& problem);
 
         protected:
           typedef typename hpp::core::pathOptimization::LinearConstraint LinearConstraint;
           using typename Parent_t::SplineOptimizationDatas_t;
 
-          SplineGradientBased (const Problem& problem);
+          SplineGradientBased(const ProblemConstPtr_t& problem);
 
           /// Get path validation for each spline
           ///

include/hpp/manipulation/path-planner/end-effector-trajectory.hh

@@ -53,12 +53,14 @@ namespace hpp {
       public:
         /// Return shared pointer to new instance
         /// \param problem the path planning problem
-        static EndEffectorTrajectoryPtr_t create (const core::Problem& problem);
+        static EndEffectorTrajectoryPtr_t create
+	  (const core::ProblemConstPtr_t& problem);
         /// Return shared pointer to new instance
         /// \param problem the path planning problem
         /// \param roadmap previously built roadmap
         static EndEffectorTrajectoryPtr_t createWithRoadmap
-          (const core::Problem& problem, const core::RoadmapPtr_t& roadmap);
+          (const core::ProblemConstPtr_t& problem,
+	   const core::RoadmapPtr_t& roadmap);
 
         /// Initialize the problem resolution
         ///  \li call parent implementation
@@ -108,11 +110,12 @@ namespace hpp {
       protected:
         /// Protected constructor
         /// \param problem the path planning problem
-        EndEffectorTrajectory (const core::Problem& problem);
+        EndEffectorTrajectory (const core::ProblemConstPtr_t& problem);
         /// Protected constructor
         /// \param problem the path planning problem
         /// \param roadmap previously built roadmap
-        EndEffectorTrajectory (const core::Problem& problem, const core::RoadmapPtr_t& roadmap);
+        EndEffectorTrajectory (const core::ProblemConstPtr_t& problem,
+			       const core::RoadmapPtr_t& roadmap);
         /// Store weak pointer to itself
         void init (const EndEffectorTrajectoryWkPtr_t& weak);
 

include/hpp/manipulation/steering-method/cross-state-optimization.hh

@@ -89,20 +89,21 @@ namespace hpp {
         public:
           struct OptimizationData;
 
-          static CrossStateOptimizationPtr_t create (const Problem& problem);
+          static CrossStateOptimizationPtr_t create
+	    (const ProblemConstPtr_t& problem);
 
           /// \warning core::Problem will be casted to Problem
           static CrossStateOptimizationPtr_t create
-            (const core::Problem& problem);
+            (const core::ProblemConstPtr_t& problem);
 
           template <typename T>
             static CrossStateOptimizationPtr_t create
-            (const core::Problem& problem);
+            (const core::ProblemConstPtr_t& problem);
 
           core::SteeringMethodPtr_t copy () const;
 
         protected:
-          CrossStateOptimization (const Problem& problem) :
+          CrossStateOptimization (const ProblemConstPtr_t& problem) :
             SteeringMethod (problem),
             sameRightHandSide_ ()
           {
@@ -167,9 +168,10 @@ namespace hpp {
 
       template <typename T>
         CrossStateOptimizationPtr_t CrossStateOptimization::create
-        (const core::Problem& problem)
+        (const core::ProblemConstPtr_t& problem)
       {
-        CrossStateOptimizationPtr_t gsm = CrossStateOptimization::create (problem);
+        CrossStateOptimizationPtr_t gsm = CrossStateOptimization::create
+	  (problem);
         gsm->innerSteeringMethod (T::create (problem));
         return gsm;
       }

include/hpp/manipulation/steering-method/end-effector-trajectory.hh

@@ -39,9 +39,10 @@ namespace hpp {
         public:
           typedef core::interval_t interval_t;
 
-          static EndEffectorTrajectoryPtr_t create (const core::Problem& problem)
+          static EndEffectorTrajectoryPtr_t create
+	    (const core::ProblemConstPtr_t& problem)
           {
-            EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory (problem));
+            EndEffectorTrajectoryPtr_t ptr(new EndEffectorTrajectory (problem));
             ptr->init(ptr);
             return ptr;
           }
@@ -95,7 +96,7 @@ namespace hpp {
           PathPtr_t projectedPath (vectorIn_t times, matrixIn_t configs) const;
 
         protected:
-          EndEffectorTrajectory (const core::Problem& problem)
+          EndEffectorTrajectory (const core::ProblemConstPtr_t& problem)
             : core::SteeringMethod (problem)
           {}
 

include/hpp/manipulation/steering-method/graph.hh

@@ -45,7 +45,7 @@ namespace hpp {
 
       protected:
         /// Constructor
-        SteeringMethod (const Problem& problem);
+        SteeringMethod (const ProblemConstPtr_t& problem);
 
         /// Copy constructor
         SteeringMethod (const SteeringMethod& other);
@@ -56,7 +56,7 @@ namespace hpp {
         }
 
         /// A pointer to the manipulation problem
-        const Problem& problem_;
+        ProblemConstPtr_t problem_;
         /// The encapsulated steering method
         core::SteeringMethodPtr_t steeringMethod_;
     };
@@ -72,14 +72,11 @@ namespace hpp {
           /// Create instance and return shared pointer
           /// \warning core::Problem will be casted to Problem
           static GraphPtr_t create
-            (const core::Problem& problem);
+            (const core::ProblemConstPtr_t& problem);
 
           template <typename T>
             static GraphPtr_t create
-            (const core::Problem& problem);
-
-          /// Create instance and return shared pointer
-          static GraphPtr_t create (const Problem& problem);
+            (const core::ProblemConstPtr_t& problem);
 
           /// Create copy and return shared pointer
           static GraphPtr_t createCopy
@@ -93,7 +90,7 @@ namespace hpp {
 
         protected:
           /// Constructor
-          Graph (const Problem& problem);
+          Graph (const ProblemConstPtr_t& problem);
 
           /// Copy constructor
           Graph (const Graph&);
@@ -113,7 +110,7 @@ namespace hpp {
 
       template <typename T>
         GraphPtr_t Graph::create
-        (const core::Problem& problem)
+        (const core::ProblemConstPtr_t& problem)
       {
         GraphPtr_t gsm = Graph::create (problem);
         gsm->innerSteeringMethod (T::create (problem));

src/graph-node-optimizer.cc

@@ -21,7 +21,7 @@
 namespace hpp {
   namespace manipulation {
     GraphNodeOptimizerPtr_t GraphNodeOptimizer::create
-      (const core::Problem& problem)
+      (const core::ProblemConstPtr_t& problem)
     {
       GraphNodeOptimizer* ptr = new GraphNodeOptimizer (problem);
       return GraphNodeOptimizerPtr_t (ptr);
@@ -29,7 +29,7 @@ namespace hpp {
 
     PathVectorPtr_t GraphNodeOptimizer::optimize (const PathVectorPtr_t& path)
     {
-      core::Problem& p = const_cast <core::Problem&> (this->problem ());
+      core::ProblemPtr_t p = const_cast <core::ProblemPtr_t> (this->problem ());
       core::SteeringMethodPtr_t sm = p.steeringMethod ();
 
       /// Start by flattening the path

src/graph-optimizer.cc

@@ -35,7 +35,7 @@ namespace hpp {
           (path->outputSize(), path->outputDerivativeSize()),
         toConcat;
       GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST (GraphPathValidation,
-              this->problem().pathValidation ());
+              this->problem()->pathValidation ());
 
       path->flatten (expanded);
       ConstraintSetPtr_t c;
@@ -65,18 +65,18 @@ namespace hpp {
         if (isShort)
           toConcat = toOpt;
         else {
-          core::ProblemPtr_t p = core::Problem::create (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->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);
+          pathOptimizer_ = factory_ (p);
           toConcat = pathOptimizer_->optimize (toOpt);
         }
         i_s = i_e;

src/manipulation-planner.cc

@@ -109,22 +109,22 @@ namespace hpp {
       (SuccessBin::createReason ("--Path validation returned length 0"))        // PATH_VALIDATION_ZERO = 6,    
       (SuccessBin::createReason ("--Path could not be projected at all"));      // PATH_PROJECTION_ZERO = 7     
 
-    ManipulationPlannerPtr_t ManipulationPlanner::create (const core::Problem& problem,
-        const core::RoadmapPtr_t& roadmap)
+    ManipulationPlannerPtr_t ManipulationPlanner::create
+    (const core::ProblemConstPtr_t& problem,
+     const core::RoadmapPtr_t& roadmap)
     {
       ManipulationPlanner* ptr;
       core::RoadmapPtr_t r2 = roadmap;
-      RoadmapPtr_t r;
-      try {
-        const Problem& p = dynamic_cast < const Problem& > (problem);
-        RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST (Roadmap, r2);
-        ptr = new ManipulationPlanner (p, r);
-      } catch (std::exception&) {
-        if (!r)
-          throw std::invalid_argument ("The roadmap must be of type hpp::manipulation::Roadmap.");
-        else
-        throw std::invalid_argument ("The problem must be of type hpp::manipulation::Problem.");
-      }
+      ProblemConstPtr_t p = HPP_DYNAMIC_PTR_CAST (const Problem, problem);
+      RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST (Roadmap, r2);
+      if (!r)
+	throw std::invalid_argument
+	  ("The roadmap must be of type hpp::manipulation::Roadmap.");
+      if (!p)
+        throw std::invalid_argument
+	  ("The problem must be of type hpp::manipulation::Problem.");
+
+      ptr = new ManipulationPlanner (p, r);
       ManipulationPlannerPtr_t shPtr (ptr);
       ptr->init (shPtr);
       return shPtr;
@@ -160,9 +160,9 @@ namespace hpp {
     {
       HPP_START_TIMECOUNTER(oneStep);
 
-      DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(Device, problem ().robot ());
+      DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(Device, problem()->robot ());
       HPP_ASSERT(robot);
-      const graph::States_t& graphStates = problem_.constraintGraph ()
+      const graph::States_t& graphStates = problem_->constraintGraph ()
         ->stateSelector ()->getStates ();
       graph::States_t::const_iterator itState;
       core::Nodes_t newNodes;
@@ -252,9 +252,9 @@ namespace hpp {
         const ConfigurationPtr_t& q_rand,
         core::PathPtr_t& validPath)
     {
-      graph::GraphPtr_t graph = problem_.constraintGraph ();
-      PathProjectorPtr_t pathProjector = problem_.pathProjector ();
-      pinocchio::DevicePtr_t robot (problem_.robot ());
+      graph::GraphPtr_t graph = problem_->constraintGraph ();
+      PathProjectorPtr_t pathProjector = problem_->pathProjector ();
+      pinocchio::DevicePtr_t robot (problem_->robot ());
       value_type eps (graph->errorThreshold ());
       // Select next node in the constraint graph.
       const ConfigurationPtr_t q_near = n_near->configuration ();
@@ -303,7 +303,7 @@ namespace hpp {
         }
         HPP_STOP_TIMECOUNTER (projectPath);
       } else projPath = path;
-      PathValidationPtr_t pathValidation (problem_.pathValidation ());
+      PathValidationPtr_t pathValidation (problem_->pathValidation ());
       PathValidationReportPtr_t report;
       core::PathPtr_t fullValidPath;
       HPP_START_TIMECOUNTER (validatePath);
@@ -373,9 +373,9 @@ namespace hpp {
 
     inline std::size_t ManipulationPlanner::tryConnectToRoadmap (const core::Nodes_t nodes)
     {
-      PathProjectorPtr_t pathProjector (problem().pathProjector ());
+      PathProjectorPtr_t pathProjector (problem()->pathProjector ());
       core::PathPtr_t path;
-      graph::GraphPtr_t graph = problem_.constraintGraph ();
+      graph::GraphPtr_t graph = problem_->constraintGraph ();
       graph::Edges_t possibleEdges;
 
       bool connectSucceed = false;
@@ -404,7 +404,8 @@ namespace hpp {
             graph::StatePtr_t s2 = getState (graph, *itn2);
             assert (q1 != q2);
 
-            path = connect (q1, q2, s1, s2, graph, pathProjector, problem_.pathValidation());
+            path = connect (q1, q2, s1, s2, graph, pathProjector,
+			    problem_->pathValidation());
 
             if (path) {
               nbConnection++;
@@ -427,9 +428,9 @@ namespace hpp {
 
     inline std::size_t ManipulationPlanner::tryConnectNewNodes (const core::Nodes_t nodes)
     {
-      PathProjectorPtr_t pathProjector (problem().pathProjector ());
+      PathProjectorPtr_t pathProjector (problem()->pathProjector ());
       core::PathPtr_t path;
-      graph::GraphPtr_t graph = problem_.constraintGraph ();
+      graph::GraphPtr_t graph = problem_->constraintGraph ();
       std::size_t nbConnection = 0;
       for (core::Nodes_t::const_iterator itn1 = nodes.begin ();
           itn1 != nodes.end (); ++itn1) {
@@ -447,7 +448,8 @@ namespace hpp {
           graph::StatePtr_t s2 = getState (graph, *itn2);
           assert (q1 != q2);
 
-          path = connect (q1, q2, s1, s2, graph, pathProjector, problem_.pathValidation());
+          path = connect (q1, q2, s1, s2, graph, pathProjector,
+			  problem_->pathValidation());
           if (path) {
             nbConnection++;
             if (!_1to2) roadmap ()->addEdge (*itn1, *itn2, path);
@@ -463,13 +465,14 @@ namespace hpp {
       return nbConnection;
     }
 
-    ManipulationPlanner::ManipulationPlanner (const Problem& problem,
+    ManipulationPlanner::ManipulationPlanner (const ProblemConstPtr_t& problem,
         const RoadmapPtr_t& roadmap) :
       core::PathPlanner (problem, roadmap),
-      shooter_ (problem.configurationShooter()),
+      shooter_ (problem->configurationShooter()),
       problem_ (problem), roadmap_ (roadmap),
-      extendStep_ (problem.getParameter("ManipulationPlanner/extendStep").floatValue()),
-      qProj_ (problem.robot ()->configSize ())
+      extendStep_ (problem->getParameter
+		   ("ManipulationPlanner/extendStep").floatValue()),
+      qProj_ (problem->robot ()->configSize ())
     {}
 
     void ManipulationPlanner::init (const ManipulationPlannerWkPtr_t& weak)

src/path-optimization/small-steps.cc

@@ -38,9 +38,10 @@ namespace hpp {
           toConcat;
         path->flatten (flat);
 
-        GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST (GraphPathValidation,
-            this->problem().pathValidation ());
-        const_cast <core::Problem&>(this->problem ()).pathValidation (gpv->innerValidation());
+        GraphPathValidationPtr_t gpv(HPP_DYNAMIC_PTR_CAST(GraphPathValidation,
+					    this->problem()->pathValidation()));
+        const_cast<core::Problem&>(*this->problem()).pathValidation
+	  (gpv->innerValidation());
 
         wholebodyStep::SmallStepsPtr_t stepPtr
           (wholebodyStep::SmallSteps::create(problem()));
@@ -69,7 +70,7 @@ namespace hpp {
           opted->concatenate (toConcat);
         }
 
-        const_cast <core::Problem&>(this->problem ()).pathValidation (gpv);
+        const_cast<core::Problem&>(*this->problem ()).pathValidation (gpv);
         return opted;
       }
     } // namespace pathOptimization

src/path-optimization/spline-gradient-based.cc

@@ -27,8 +27,8 @@ namespace hpp {
     namespace pathOptimization {
 
       template <int _PB, int _SO>
-      SplineGradientBased<_PB, _SO>::SplineGradientBased (const Problem& problem)
-        : Parent_t (problem)
+      SplineGradientBased<_PB, _SO>::SplineGradientBased
+      (const ProblemConstPtr_t& problem) : Parent_t (problem)
       {
         this->checkOptimum_ = true;
       }
@@ -39,7 +39,7 @@ namespace hpp {
 
       template <int _PB, int _SO>
       typename SplineGradientBased<_PB, _SO>::Ptr_t SplineGradientBased<_PB, _SO>::create
-      (const Problem& problem)
+      (const ProblemConstPtr_t& problem)
       {
 	SplineGradientBased* ptr = new SplineGradientBased (problem);
 	Ptr_t shPtr (ptr);
@@ -48,10 +48,10 @@ namespace hpp {
 
       template <int _PB, int _SO>
       typename SplineGradientBased<_PB, _SO>::Ptr_t SplineGradientBased<_PB, _SO>::createFromCore
-      (const core::Problem& problem)
+      (const core::ProblemConstPtr_t& problem)
       {
-        HPP_STATIC_CAST_REF_CHECK(const Problem, problem);
-        return create (static_cast<const Problem&>(problem));
+        assert(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+        return create (HPP_STATIC_PTR_CAST(const Problem, problem));
       }
 
       template <int _PB, int _SO>
@@ -64,7 +64,7 @@ namespace hpp {
           if (set && set->edge())
             this->validations_[i] = set->edge()->pathValidation();
           else
-            this->validations_[i] = this->problem().pathValidation();
+            this->validations_[i] = this->problem()->pathValidation();
         }
       }
 
@@ -74,7 +74,9 @@ namespace hpp {
       {
         assert (init->numberPaths() == splines.size() && sods.size() == splines.size());
 
-        bool zeroDerivative = this->problem().getParameter ("SplineGradientBased/zeroDerivativesAtStateIntersection").boolValue();
+        bool zeroDerivative = this->problem()->getParameter
+	  ("SplineGradientBased/zeroDerivativesAtStateIntersection").
+	  boolValue();
 
         const std::size_t last = splines.size() - 1;
         graph::StatePtr_t stateOfStart;
@@ -179,7 +181,8 @@ namespace hpp {
         // TODO Should we add zero velocity sometimes ?
 
         ConstraintSetPtr_t set = state->configConstraint();
-        value_type guessThreshold = this->problem().getParameter ("SplineGradientBased/guessThreshold").floatValue();
+        value_type guessThreshold = this->problem()->getParameter
+	  ("SplineGradientBased/guessThreshold").floatValue();
         Eigen::RowBlockIndices select =
           this->computeActiveParameters (path, set->configProjector()->solver(), guessThreshold, true);
         hppDout (info, "End of path " << idxSpline << ": do not change this dof " << select);

src/path-planner/end-effector-trajectory.cc

@@ -41,7 +41,8 @@ namespace hpp {
       typedef manipulation::steeringMethod::EndEffectorTrajectory      SM_t;
       typedef manipulation::steeringMethod::EndEffectorTrajectoryPtr_t SMPtr_t;
 
-      EndEffectorTrajectoryPtr_t EndEffectorTrajectory::create (const core::Problem& problem)
+      EndEffectorTrajectoryPtr_t EndEffectorTrajectory::create
+      (const core::ProblemConstPtr_t& problem)
       {
         EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem));
         ptr->init(ptr);
@@ -49,9 +50,11 @@ namespace hpp {
       }
 
       EndEffectorTrajectoryPtr_t EndEffectorTrajectory::createWithRoadmap (
-          const core::Problem& problem, const core::RoadmapPtr_t& roadmap)
+          const core::ProblemConstPtr_t& problem,
+	  const core::RoadmapPtr_t& roadmap)
       {
-        EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem, roadmap));
+        EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem,
+								  roadmap));
         ptr->init(ptr);
         return ptr;
       }
@@ -63,13 +66,13 @@ namespace hpp {
       {
         //core::PathPlanner::startSolve();
         //problem().checkProblem ();
-        if (!problem().robot ()) {
+        if (!problem()->robot ()) {
           std::string msg ("No device in problem.");
           hppDout (error, msg);
           throw std::runtime_error (msg);
         }
 
-        if (!problem().initConfig ()) {
+        if (!problem()->initConfig ()) {
           std::string msg ("No init config in problem.");
           hppDout (error, msg);
           throw std::runtime_error (msg);
@@ -78,7 +81,7 @@ namespace hpp {
         // Tag init and goal configurations in the roadmap
         roadmap()->resetGoalNodes ();
 
-        SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem().steeringMethod()));
+        SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem()->steeringMethod()));
         if (!sm)
           throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
 
@@ -119,7 +122,7 @@ namespace hpp {
 
       void EndEffectorTrajectory::oneStep ()
       {
-        SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem().steeringMethod()));
+        SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem()->steeringMethod()));
         if (!sm)
           throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
         if (!sm->trajectoryConstraint ())
@@ -131,14 +134,14 @@ namespace hpp {
           throw std::invalid_argument ("Steering method constraint has no ConfigProjector.");
         core::ConfigProjectorPtr_t constraints (sm->constraints()->configProjector());
 
-        core::ConfigValidationPtr_t  cfgValidation (problem().configValidations());
-        core::  PathValidationPtr_t pathValidation (problem().pathValidation());
+        core::ConfigValidationPtr_t  cfgValidation (problem()->configValidations());
+        core::  PathValidationPtr_t pathValidation (problem()->pathValidation());
         core::    ValidationReportPtr_t cfgReport;
         core::PathValidationReportPtr_t pathReport;
 
         core::interval_t timeRange (sm->timeRange());
 
-        std::vector<core::Configuration_t> qs (configurations(*problem().initConfig()));
+        std::vector<core::Configuration_t> qs (configurations(*problem()->initConfig()));
         if (qs.empty()) {
           hppDout (info, "Failed to generate initial configs.");
           return;
@@ -150,7 +153,7 @@ namespace hpp {
         std::size_t i;
 
         vector_t times (nDiscreteSteps_+1);
-        matrix_t steps (problem().robot()->configSize(), nDiscreteSteps_+1);
+        matrix_t steps (problem()->robot()->configSize(), nDiscreteSteps_+1);
 
         times[0] = timeRange.first;
         for (int j = 1; j < nDiscreteSteps_; ++j)
@@ -221,7 +224,7 @@ namespace hpp {
           std::vector<core::Configuration_t> configs(nRandomConfig_ + 1);
           configs[0] = q_init;
           for (int i = 1; i < nRandomConfig_ + 1; ++i)
-            problem().configurationShooter()->shoot(configs[i]);
+            problem()->configurationShooter()->shoot(configs[i]);
           return configs;
         }
 
@@ -231,11 +234,13 @@ namespace hpp {
         throw std::runtime_error ("Using an IkSolverInitialization is not implemented yet");
       }
 
-      EndEffectorTrajectory::EndEffectorTrajectory (const core::Problem& problem)
-        : core::PathPlanner (problem)
+      EndEffectorTrajectory::EndEffectorTrajectory
+      (const core::ProblemConstPtr_t& problem) : core::PathPlanner (problem)
       {}
 
-      EndEffectorTrajectory::EndEffectorTrajectory (const core::Problem& problem, const core::RoadmapPtr_t& roadmap)
+      EndEffectorTrajectory::EndEffectorTrajectory
+      (const core::ProblemConstPtr_t& problem,
+       const core::RoadmapPtr_t& roadmap)
         : core::PathPlanner (problem, roadmap)
       {}
 

src/problem-solver.cc

@@ -91,7 +91,7 @@ namespace hpp {
 
       template <typename ParentSM_t, typename ChildSM_t>
       core::SteeringMethodPtr_t createSMWithGuess
-      (const core::Problem& problem)
+      (const core::ProblemConstPtr_t& problem)
       {
         boost::shared_ptr<ParentSM_t> sm = ParentSM_t::create (problem);
         sm->innerSteeringMethod (ChildSM_t::createWithGuess (problem));
@@ -100,13 +100,15 @@ namespace hpp {
 
       template <typename PathProjectorType>
       core::PathProjectorPtr_t createPathProjector
-      (const core::Problem& problem, const value_type& step)
+      (const core::ProblemConstPtr_t& problem, const value_type& step)
       {
         steeringMethod::GraphPtr_t gsm =
-          HPP_DYNAMIC_PTR_CAST (steeringMethod::Graph, problem.steeringMethod());
-        if (!gsm) throw std::logic_error ("The steering method should be of type"
-            " steeringMethod::Graph");
-        return PathProjectorType::create (problem.distance(),
+          HPP_DYNAMIC_PTR_CAST
+	  (steeringMethod::Graph, problem->steeringMethod());
+        if (!gsm) throw std::logic_error
+		    ("The steering method should be of type"
+		     " steeringMethod::Graph");
+        return PathProjectorType::create (problem->distance(),
             gsm->innerSteeringMethod(), step);
       }
     }

src/problem.cc

@@ -42,7 +42,7 @@ namespace hpp {
       Parent::init (wkPtr);
       wkPtr_ = wkPtr;
 
-      Parent::steeringMethod (steeringMethod::Graph::create (*this));
+      Parent::steeringMethod (steeringMethod::Graph::create (wkPtr_.lock()));
       distance (WeighedDistance::create (HPP_DYNAMIC_PTR_CAST(Device, robot()), graph_));
       setPathValidationFactory(core::pathValidation::createDiscretizedCollisionChecking, 0.05);
     }