diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc index 84e42eaecc731e447f0c9375f2a6069bd9ea6324..e9b51721b1567f359d368e8401ad69677352801a 100644 --- a/src/graph-path-validation.cc +++ b/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; } diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc index 41b2b07776b2262905a8145cd68e4fff33d4203f..84ae64b2b6dc3d2087f6bef672ae3e3591ed6193 100644 --- a/src/graph-steering-method.cc +++ b/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; diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc index 0e18ccc1b240ec16a6ad7ca115e6ae1a185edb72..236d7edddb481ec129f18a8cd60db55476884323 100644 --- a/src/manipulation-planner.cc +++ b/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)