From c3a7ef4b4977f74ae09343c476fe865d1bb6e9d3 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 29 Aug 2017 14:44:24 +0200
Subject: [PATCH] Fix GraphOptimizer: it was modify the constraint of an edge
steering method.
---
src/graph-optimizer.cc | 18 +++++++++---------
1 file changed, 9 insertions(+), 9 deletions(-)
diff --git a/src/graph-optimizer.cc b/src/graph-optimizer.cc
index 66ec3303..e8041180 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);
--
GitLab