diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc index 464d4c6e0c20382d294b14116c1e439943fa3991..84e42eaecc731e447f0c9375f2a6069bd9ea6324 100644 --- a/src/graph-path-validation.cc +++ b/src/graph-path-validation.cc @@ -92,7 +92,8 @@ namespace hpp { // We check for all of them if both nodes are on the same leaf. ConstraintSetPtr_t constraints; while (!possibleEdges.empty ()) { - constraints = constraintGraph_->pathConstraint (possibleEdges.back(), configAt (tmin)); + constraints = constraintGraph_->pathConstraint (possibleEdges.back()); + constraints->offsetFromConfig(configAt (tmin)); if (constraints->isSatisfied (configAt (tmax))) { validPart = path; return true; diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc index d2691d2fd1132400e84ed5fb0f63404337065461..41b2b07776b2262905a8145cd68e4fff33d4203f 100644 --- a/src/graph-steering-method.cc +++ b/src/graph-steering-method.cc @@ -36,7 +36,8 @@ namespace hpp { graph_->getEdge (graph_->getNode (q1), graph_->getNode (q2)); ConstraintSetPtr_t constraints; while (!possibleEdges.empty()) { - constraints = graph_->pathConstraint (possibleEdges.back(), q1); + constraints = graph_->pathConstraint (possibleEdges.back()); + constraints->offsetFromConfig(q1); if (constraints->isSatisfied (q2)) { path->constraints (constraints); break; diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc index 9c29220a9748059b07af164f887c87929c53a382..24c8dbb30faaecbbd5cf04f627bfcb7341aed0c0 100644 --- a/src/manipulation-planner.cc +++ b/src/manipulation-planner.cc @@ -102,7 +102,8 @@ namespace hpp { // Select next node in the constraint graph. graph::Nodes_t nodes = graph->getNode (*q_near); graph::Edges_t edges = graph->chooseEdge (nodes); - ConstraintPtr_t constraint = graph->configConstraint (edges, *q_near); + ConstraintPtr_t constraint = graph->configConstraint (edges); + constraint->offsetFromConfig (*q_near); qProj_ = *q_rand; if (!constraint->apply (qProj_)) { addFailure (PROJECTION, edges); @@ -114,7 +115,7 @@ namespace hpp { addFailure (STEERING_METHOD, edges); return false; } - path->constraints (graph->pathConstraint (edges, *q_near)); + path->constraints (graph->pathConstraint (edges)); core::PathValidationPtr_t pathValidation (problem ().pathValidation ()); pathValidation->validate (path, false, validPath); if (validPath->length () == 0)