Integrate GraphSteeringMethod

include/hpp/manipulation/problem.hh

@@ -19,6 +19,7 @@
 #include "hpp/manipulation/robot.hh"
 #include "hpp/manipulation/graph/graph.hh"
 #include "hpp/manipulation/graph-path-validation.hh"
+#include "hpp/manipulation/graph-steering-method.hh"
 #include "hpp/manipulation/fwd.hh"
 
 namespace hpp {
@@ -30,9 +31,10 @@ namespace hpp {
 
         /// Constructor
         Problem (RobotPtr_t robot) : core::Problem (robot),
-          graph_()
+          graph_(), steeringMethod_ (new GraphSteeringMethod (robot))
         {
           Parent::pathValidation (GraphPathValidation::create (Parent::pathValidation(), graph_));
+          steeringMethod (steeringMethod_);
         }
 
         /// Set the graph of constraints
@@ -42,6 +44,7 @@ namespace hpp {
           GraphPathValidationPtr_t graphValidation = pathValidation();
           if (graphValidation)
             graphValidation->constraintGraph(graph);
+          steeringMethod_->constraintGraph (graph);
         }
 
         /// Get the graph of constraints
@@ -66,6 +69,8 @@ namespace hpp {
       private:
         /// The graph of constraints
         graph::GraphPtr_t graph_;
+        /// Steering method
+        GraphSteeringMethodPtr_t steeringMethod_;
     };
   } // namespace manipulation
 } // namespace hpp

src/graph-path-validation.cc

@@ -33,9 +33,10 @@ namespace hpp {
     bool GraphPathValidation::validate (
           const PathPtr_t& path, bool reverse, PathPtr_t& validPart)
     {
+      assert (path);
       PathPtr_t pathGraphValid;
       bool graphValid = impl_validate (path, reverse, pathGraphValid);
-      bool collisionValid = pathValidation_->validate (pathGraphValid, reverse, validPart); 
+      bool collisionValid = pathValidation_->validate (pathGraphValid, reverse, validPart);
       return graphValid && collisionValid;
     }
 
@@ -45,18 +46,20 @@ namespace hpp {
       size_t start = 0,
              end = path->numberPaths ();
       int inc = 1;
+      value_type timeOffset = path->timeRange().first;
       if (reverse) {
         std::swap (start, end);
+        start--;end--;
         inc = -1;
+        timeOffset = path->timeRange ().second;
       }
       PathPtr_t validSubPart;
-      value_type timeOffset = path->timeRange().first; 
       for (size_t index = start; index != end; index += inc) {
         // We should stop at the first non valid subpath.
         if (!impl_validate (path->pathAtRank (index), reverse, validSubPart)) {
           if (reverse)
             validPart = path->extract (
-                std::make_pair (timeOffset + validSubPart->timeRange().first,
+                std::make_pair (timeOffset - validSubPart->timeRange().second,
                                 path->timeRange().second));
           else
             validPart = path->extract (
@@ -64,7 +67,7 @@ namespace hpp {
                                 timeOffset + validSubPart->timeRange().second));
           return false;
         }
-        timeOffset += path->pathAtRank (index)->length(); 
+        timeOffset += inc * path->pathAtRank (index)->length();
       }
       // Here, every subpath is valid.
       validPart = path;

src/manipulation-planner.cc

@@ -107,9 +107,9 @@ namespace hpp {
         return false;
       core::SteeringMethodPtr_t sm (problem().steeringMethod());
       core::PathPtr_t path = (*sm) (*q_near, qProj_);
-      path->constraints (graph->pathConstraint (edges, *q_near));
       if (!path)
         return false;
+      path->constraints (graph->pathConstraint (edges, *q_near));
       core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
       pathValidation->validate (path, false, validPath);
       return true;
@@ -138,8 +138,7 @@ namespace hpp {
           if (possibleEdges.empty())
             continue;
           path = (*sm) (*q1, *q2);
-          path->constraints (graph->pathConstraint (possibleEdges[0], *q1));
-          if (pathValidation->validate (path, false, validPath)) {
+          if (path && pathValidation->validate (path, false, validPath)) {
             roadmap ()->addEdge (*itn1, *itn2, path);
             core::interval_t timeRange = path->timeRange ();
             roadmap ()->addEdge (*itn2, *itn1, path->extract