Update GraphOptimizer

include/hpp/manipulation/graph-optimizer.hh

@@ -22,6 +22,7 @@
 # include <hpp/core/path-vector.hh>
 # include <hpp/core/path-optimizer.hh>
 # include <hpp/core/problem.hh>
+# include <hpp/core/problem-solver.hh>
 
 # include <hpp/manipulation/fwd.hh>
 # include <hpp/manipulation/graph/fwd.hh>
@@ -46,6 +47,8 @@ namespace hpp {
     class HPP_MANIPULATION_DLLAPI GraphOptimizer : public PathOptimizer
     {
       public:
+        typedef core::ProblemSolver::PathOptimizerBuilder_t PathOptimizerBuilder_t;
+
         template <typename InnerPathOptimizer_t>
           static GraphOptimizerPtr_t create (const core::Problem& problem);
 
@@ -59,11 +62,13 @@ namespace hpp {
 
       protected:
         /// Constructor
-        GraphOptimizer (const core::Problem& problem, PathOptimizerPtr_t inner) :
-          PathOptimizer (problem), pathOptimizer_ (inner)
+        GraphOptimizer (const core::Problem& problem, PathOptimizerBuilder_t factory) :
+          PathOptimizer (problem), factory_ (factory), pathOptimizer_ ()
         {}
 
       private:
+        PathOptimizerBuilder_t factory_;
+
         /// The encapsulated PathOptimizer
         PathOptimizerPtr_t pathOptimizer_;
 
@@ -80,7 +85,7 @@ namespace hpp {
       (const core::Problem& problem)
     {
       return GraphOptimizerPtr_t (
-          new GraphOptimizer (problem, InnerPathOptimizer_t::create (problem))
+          new GraphOptimizer (problem, InnerPathOptimizer_t::create)
           );
     }
 

src/graph-optimizer.cc

@@ -16,10 +16,20 @@
 
 #include <hpp/manipulation/graph-optimizer.hh>
 
+#include <hpp/core/steering-method-straight.hh>
+
 namespace hpp {
   namespace manipulation {
     PathVectorPtr_t GraphOptimizer::optimize (const PathVectorPtr_t& path)
     {
+      core::Problem& p = const_cast <core::Problem&> (this->problem ());
+      core::SteeringMethodPtr_t sm = p.steeringMethod ();
+
+      core::ConstraintSetPtr_t oldC = p.constraints ();
+      core::SteeringMethodStraightPtr_t smS = core::SteeringMethodStraight::
+        create (p.robot ());
+      p.steeringMethod (smS);
+
       PathVectorPtr_t opted = PathVector::create
         (path->outputSize(), path->outputDerivativeSize()),
         expanded = PathVector::create
@@ -34,6 +44,7 @@ namespace hpp {
         toOpt->appendPath (current);
         graph::EdgePtr_t edge;
         c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
+        p.constraints(c);
         if (c) edge = c->edge ();
         std::size_t i_e = i_s + 1;
         for (; i_e < expanded->numberPaths (); ++i_e) {
@@ -43,10 +54,14 @@ namespace hpp {
           if (c && edge != c->edge ()) break;
           toOpt->appendPath (current);
         }
+        pathOptimizer_ = factory_ (this->problem ());
         toConcat = pathOptimizer_->optimize (toOpt);
         i_s = i_e;
         opted->concatenate (*toConcat);
       }
+      p.steeringMethod (sm);
+      p.constraints (oldC);
+      pathOptimizer_.reset ();
       return opted;
     }