From 2b08f99dd24464c9c5cecf9c5465b2a2867a22d4 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 1 Sep 2014 14:30:22 +0200
Subject: [PATCH] Fix bug in GraphPathValidation. Add assert
---
src/graph-path-validation.cc | 4 +++-
src/graph-steering-method.cc | 1 +
src/manipulation-planner.cc | 1 -
3 files changed, 4 insertions(+), 2 deletions(-)
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
index 84e42ea..e9b5172 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 41b2b07..84ae64b 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 0e18ccc..236d7ed 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)
--
GitLab