[TransitionPlanner] Add a method to validate a configuration with selected edge.

include/hpp/manipulation/path-planner/transition-planner.hh

@@ -31,6 +31,7 @@
 
 #include <hpp/core/path-planner.hh>
 #include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/graph/fwd.hh>
 
 namespace hpp {
 namespace manipulation {
@@ -110,6 +111,11 @@ class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner {
   /// continuity.
   PathPtr_t directPath(ConfigurationIn_t q1, ConfigurationIn_t q2,
                        bool validate, bool& success, std::string& status);
+  /// Validate a configuration with the path validation of an edge.
+  /// \param q configuration to validate,
+  /// \param id index of the edge in the constraint graph.
+  bool validateConfiguration(ConfigurationIn_t q, std::size_t id,
+			     core::ValidationReportPtr_t& report) const;
   /// Optimize path using the selected path optimizers
   /// \param path input path
   /// \return optimized path
@@ -156,6 +162,8 @@ class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner {
   void init(TransitionPlannerWkPtr_t weak);
 
  private:
+  /// Get pointer to edge from an id
+  graph::EdgePtr_t getEdgeOrThrow(std::size_t id) const;
   /// Pointer to the problem of the inner planner
   core::ProblemPtr_t innerProblem_;
   /// Pointer to the inner path planner

src/path-planner/transition-planner.cc

@@ -145,6 +145,13 @@ core::PathPtr_t TransitionPlanner::directPath(ConfigurationIn_t q1,
   return validPart;
 }
 
+bool TransitionPlanner::validateConfiguration(ConfigurationIn_t q, std::size_t id,
+					      core::ValidationReportPtr_t& report) const
+{
+  graph::EdgePtr_t edge(getEdgeOrThrow(id));
+  return edge->pathValidation()->validate(q, report);
+}
+
 core::PathVectorPtr_t TransitionPlanner::optimizePath(const PathPtr_t& path) {
   PathVectorPtr_t pv(HPP_DYNAMIC_PTR_CAST(PathVector, path));
   if (!pv) {
@@ -164,16 +171,7 @@ core::PathVectorPtr_t TransitionPlanner::timeParameterization(
 }
 
 void TransitionPlanner::setEdge(std::size_t id) {
-  ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem()));
-  assert(p);
-  graph::GraphComponentPtr_t comp(p->constraintGraph()->get(id).lock());
-  graph::EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(graph::Edge, comp));
-  if (!edge) {
-    std::ostringstream os;
-    os << "hpp::manipulation::pathPlanner::TransitionPlanner::setEdge: index "
-       << id << " does not correspond to any edge of the constraint graph.";
-    throw std::logic_error(os.str().c_str());
-  }
+  graph::EdgePtr_t edge(getEdgeOrThrow(id));
   innerProblem_->constraints(edge->pathConstraint());
   innerProblem_->pathValidation(edge->pathValidation());
   innerProblem_->steeringMethod(edge->steeringMethod());
@@ -243,6 +241,21 @@ void TransitionPlanner::init(TransitionPlannerWkPtr_t weak) {
   weakPtr_ = weak;
 }
 
+graph::EdgePtr_t TransitionPlanner::getEdgeOrThrow(std::size_t id) const
+{
+  ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem()));
+  assert(p);
+  graph::GraphComponentPtr_t comp(p->constraintGraph()->get(id).lock());
+  graph::EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(graph::Edge, comp));
+  if (!edge) {
+    std::ostringstream os;
+    os << "hpp::manipulation::pathPlanner::TransitionPlanner::setEdge: index "
+       << id << " does not correspond to any edge of the constraint graph.";
+    throw std::logic_error(os.str().c_str());
+  }
+  return edge;
+}
+
 }  // namespace pathPlanner
 }  // namespace manipulation
 }  // namespace hpp