Fix GraphOptimizer: it was modify the constraint of an edge steering method.

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);