diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh index 5a524c44ff3105703b1d0a2f88d13f52a31953ff..b827071c65c56b911ce476ae5c965ab5cb4e3691 100644 --- a/include/hpp/manipulation/problem.hh +++ b/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 diff --git a/src/problem-solver.cc b/src/problem-solver.cc index 43a7c2adc65230a2df9484f18f42542b7d84854d..337a5818e0b7196eb165d9f9e8a79ff8e453d482 100644 --- a/src/problem-solver.cc +++ b/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) diff --git a/src/problem.cc b/src/problem.cc index b0bf9a9080ce5968e9c393fa20c86e8184e98826..6cb988c8d53d35048300c5da0d78fb8935c30601 100644 --- a/src/problem.cc +++ b/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; }