Fix bug in GraphPathValidation. Add assert

src/graph-path-validation.cc

@@ -94,7 +94,9 @@ namespace hpp {
       while (!possibleEdges.empty ()) {
         constraints = constraintGraph_->pathConstraint (possibleEdges.back());
         constraints->offsetFromConfig(configAt (tmin));
-        if (constraints->isSatisfied (configAt (tmax))) {
+        assert (constraints->isSatisfied (configAt (tmin)));
+        if (constraints == path->constraints ()
+            && constraints->isSatisfied (configAt (tmax))) {
           validPart = path;
           return true;
         }

src/graph-steering-method.cc

@@ -38,6 +38,7 @@ namespace hpp {
       while (!possibleEdges.empty()) {
         constraints = graph_->pathConstraint (possibleEdges.back());
         constraints->offsetFromConfig(q1);
+        assert (constraints->isSatisfied (q1));
         if (constraints->isSatisfied (q2)) {
           path->constraints (constraints);
           break;

src/manipulation-planner.cc

@@ -117,7 +117,6 @@ namespace hpp {
         addFailure (STEERING_METHOD, edges);
         return false;
       }
-      path->constraints (graph->pathConstraint (edges));
       core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
       pathValidation->validate (path, false, validPath);
       if (validPath->length () == 0)