diff --git a/src/graph-optimizer.cc b/src/graph-optimizer.cc index 66ec330368be6eeee158d4e242c5fd25df1fbfc3..e80411805b3a280543070106afd2b00022db9474 100644 --- a/src/graph-optimizer.cc +++ b/src/graph-optimizer.cc @@ -36,13 +36,6 @@ namespace hpp { toConcat; GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST (GraphPathValidation, this->problem().pathValidation ()); - core::Problem p (problem().robot()); - p.distance(problem().distance()); - // It should be ok to use the path validation of each edge because it - // corresponds to the global path validation minus the collision pairs - // disabled using the edge constraint. - // p.pathValidation(gpv->innerValidation()); - p.pathProjector(problem().pathProjector()); path->flatten (expanded); ConstraintSetPtr_t c; @@ -72,9 +65,16 @@ namespace hpp { if (isShort) toConcat = toOpt; else { - p.constraints(edge->steeringMethod()->constraints()); + core::Problem p (problem().robot()); + p.distance(problem().distance()); + // It should be ok to use the path validation of each edge because it + // corresponds to the global path validation minus the collision pairs + // disabled using the edge constraint. + // p.pathValidation(gpv->innerValidation()); + p.pathProjector(problem().pathProjector()); + p.steeringMethod(edge->steeringMethod()->copy()); + p.constraints(p.steeringMethod()->constraints()); p.constraints()->configProjector()->rightHandSideFromConfig(toOpt->initial()); - p.steeringMethod(edge->steeringMethod()); p.pathValidation(edge->pathValidation()); pathOptimizer_ = factory_ (p); toConcat = pathOptimizer_->optimize (toOpt);