diff --git a/include/hpp/manipulation/path-planner/states-path-finder.hh b/include/hpp/manipulation/path-planner/states-path-finder.hh index 9fd0db91a3d2a044e6d3a1125e1e9b0fb167cbda..e0fad8a9a2a40a00ef8545253a1f6c33255ad5e2 100644 --- a/include/hpp/manipulation/path-planner/states-path-finder.hh +++ b/include/hpp/manipulation/path-planner/states-path-finder.hh @@ -214,6 +214,8 @@ namespace hpp { bool skipColAnalysis_; + // Constraints defining the goal + NumericalConstraints_t goalConstraints_; // Variables used across several calls to oneStep ConfigurationPtr_t q1_, q2_; core::Configurations_t configList_; diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc index 4a807c5396ec7acddddafd4b228ef77f77d8a02d..e2dd79fbd15f88531d66a9a3d626bf812e3dcf75 100644 --- a/src/path-planner/states-path-finder.cc +++ b/src/path-planner/states-path-finder.cc @@ -42,7 +42,8 @@ #include <hpp/core/configuration-shooter.hh> #include <hpp/core/collision-validation.hh> #include <hpp/core/collision-validation-report.hh> -#include <hpp/core/problem-target.hh> +#include <hpp/core/problem-target/task-target.hh> +#include <hpp/core/problem-target/goal-configurations.hh> #include <hpp/core/path-optimization/random-shortcut.hh> #include <hpp/manipulation/constraint-set.hh> @@ -69,6 +70,12 @@ namespace hpp { using graph::LockedJoints_t; using graph::segments_t; + using core::ProblemTargetPtr_t; + using core::problemTarget::GoalConfigurations; + using core::problemTarget::GoalConfigurationsPtr_t; + using core::problemTarget::TaskTarget; + using core::problemTarget::TaskTargetPtr_t; + static void displayRoadmap(const core::RoadmapPtr_t& #ifdef HPP_DEBUG roadmap @@ -1126,15 +1133,26 @@ namespace hpp { assert(problem_); q1_ = problem_->initConfig(); assert(q1_); - core::Configurations_t q2s = problem_->goalConfigs(); - if (q2s.size() != 1) { - std::ostringstream os; - os << "StatesPathFinder accept one and only one goal configuration, "; - os << q2s.size() << " provided."; - throw std::logic_error(os.str().c_str()); + // Detect whether the goal is defined by a configuration or by a + // set of constraints + ProblemTargetPtr_t target(problem()->target()); + GoalConfigurationsPtr_t goalConfigs + (HPP_DYNAMIC_PTR_CAST(GoalConfigurations, target)); + if (goalConfigs){ + core::Configurations_t q2s = goalConfigs->configurations(); + if (q2s.size() != 1) { + std::ostringstream os; + os << "StatesPathFinder accept one and only one goal " + "configuration, "; + os << q2s.size() << " provided."; + throw std::logic_error(os.str().c_str()); + } + q2_ = q2s[0]; + } + TaskTargetPtr_t taskTarget(HPP_DYNAMIC_PTR_CAST(TaskTarget,target)); + if(taskTarget){ + goalConstraints_ = taskTarget->constraints(); } - q2_ = q2s[0]; - reset(); }