Make GraphPathValidation not mandatory (may be deleted ?)

include/hpp/manipulation/problem.hh

@@ -49,9 +49,11 @@ namespace hpp {
         /// Check whether the problem is well formulated.
         virtual void checkProblem () const;
 
-        /// Get the path validation as a GraphPathValidation
-        GraphPathValidationPtr_t pathValidation () const;
+        /// Expose parent method.
+        PathValidationPtr_t pathValidation () const;
 
+        /// \param pathValidation if of type GraphPathValidation, sets 
+        ///        its constraint graph to Problem::constraintGraph()
         void pathValidation (const PathValidationPtr_t& pathValidation);
 
         /// Get the steering method as a SteeringMethod

src/problem-solver.cc

@@ -32,6 +32,10 @@
 #include <hpp/core/path-projector/dichotomy.hh>
 #include <hpp/core/path-projector/global.hh>
 #include <hpp/core/path-projector/recursive-hermite.hh>
+#include <hpp/core/path-validation/discretized-collision-checking.hh>
+#include <hpp/core/path-validation/discretized-joint-bound.hh>
+#include <hpp/core/continuous-validation/dichotomy.hh>
+#include <hpp/core/continuous-validation/progressive.hh>
 #include <hpp/core/roadmap.hh>
 #include <hpp/core/steering-method/dubins.hh>
 #include <hpp/core/steering-method/hermite.hh>
@@ -74,6 +78,16 @@ namespace hpp {
           static bool removeLockedJoints () { return false; }
       };
 
+#define MAKE_GRAPH_PATH_VALIDATION_BUILDER(name, function)                     \
+      PathValidationPtr_t create ## name ## GraphPathValidation (              \
+          const core::DevicePtr_t& robot, const value_type& stepSize)          \
+      {                                                                        \
+        return GraphPathValidation::create (function (robot, stepSize));       \
+      }
+      MAKE_GRAPH_PATH_VALIDATION_BUILDER(DiscretizedCollision             , core::pathValidation::createDiscretizedCollisionChecking)
+      MAKE_GRAPH_PATH_VALIDATION_BUILDER(DiscretizedJointBound            , core::pathValidation::createDiscretizedJointBound)
+      //MAKE_GRAPH_PATH_VALIDATION_BUILDER(DiscretizedCollisionAndJointBound, createDiscretizedJointBoundAndCollisionChecking)
+
       template <typename ParentSM_t, typename ChildSM_t>
       core::SteeringMethodPtr_t createSMWithGuess
       (const core::Problem& problem)
@@ -110,6 +124,15 @@ namespace hpp {
       pathPlanners.add ("M-RRT", ManipulationPlanner::create);
       pathPlanners.add ("SymbolicPlanner", SymbolicPlanner::create);
 
+      pathValidations.add ("Graph-Discretized"                      , createDiscretizedCollisionGraphPathValidation);
+      pathValidations.add ("Graph-DiscretizedCollision"             , createDiscretizedCollisionGraphPathValidation);
+      pathValidations.add ("Graph-DiscretizedJointBound"            , createDiscretizedJointBoundGraphPathValidation);
+      //pathValidations.add ("Graph-DiscretizedCollisionAndJointBound", createDiscretizedCollisionAndJointBoundGraphPathValidation);
+      pathValidations.add ("Graph-Dichotomy"  , GraphPathValidation::create<core::continuousValidation::Dichotomy  >);
+      pathValidations.add ("Graph-Progressive", GraphPathValidation::create<core::continuousValidation::Progressive>);
+
+      pathValidationType ("Graph-Discretized", 0.05);
+
       pathOptimizers.add ("RandomShortcut",
           pathOptimization::RandomShortcut::create);
       pathOptimizers.add ("Graph-RandomShortcut",
@@ -179,11 +202,8 @@ namespace hpp {
     {
       problem_ = problem;
       core::ProblemSolver::initializeProblem (problem_);
-      if (constraintGraph_) {
+      if (constraintGraph_)
         problem_->constraintGraph (constraintGraph_);
-        if (problem_->pathValidation ())
-          problem_->pathValidation ()->constraintGraph (constraintGraph_);
-      }
     }
 
     void ProblemSolver::constraintGraph (const std::string& graphName)

src/problem.cc

@@ -51,24 +51,24 @@ namespace hpp {
     {
       graph_ = graph;
       graph_->problem (wkPtr_.lock());
-      if (pathValidation ())
-        pathValidation ()->constraintGraph (graph);
+
+      GraphPathValidationPtr_t pv = HPP_DYNAMIC_PTR_CAST(GraphPathValidation, pathValidation());
+      if (pv) pv->constraintGraph (graph_);
       WeighedDistancePtr_t d = HPP_DYNAMIC_PTR_CAST (WeighedDistance,
           distance ());
       if (d) d->constraintGraph (graph);
     }
 
-    GraphPathValidationPtr_t Problem::pathValidation () const
+    PathValidationPtr_t Problem::pathValidation () const
     {
-      return HPP_DYNAMIC_PTR_CAST (GraphPathValidation,
-          Parent::pathValidation());
+      return Parent::pathValidation();
     }
 
     void Problem::pathValidation (const PathValidationPtr_t& pathValidation)
     {
-      GraphPathValidationPtr_t pv (GraphPathValidation::create (pathValidation));
-      pv->constraintGraph (graph_);
-      Parent::pathValidation (pv);
+      GraphPathValidationPtr_t pv = HPP_DYNAMIC_PTR_CAST(GraphPathValidation, pathValidation);
+      if (pv) pv->constraintGraph (graph_);
+      Parent::pathValidation (pathValidation);
     }
 
     PathValidationPtr_t Problem::pathValidationFactory () const
@@ -79,6 +79,8 @@ namespace hpp {
       for (core::ObjectStdVector_t::const_iterator _obs = obstacles.begin ();
 	   _obs != obstacles.end (); ++_obs)
 	pv->addObstacle (*_obs);
+      GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST(GraphPathValidation, pv);
+      if (gpv) return gpv->innerValidation();
       return pv;
     }