Add optimizer Graph-ConfigOptimization

include/hpp/manipulation/graph-optimizer.hh

@@ -49,7 +49,7 @@ namespace hpp {
       public:
         typedef core::ProblemSolver::PathOptimizerBuilder_t PathOptimizerBuilder_t;
 
-        template <typename InnerPathOptimizer_t>
+        template <typename TraitsOrInnerType>
           static GraphOptimizerPtr_t create (const core::Problem& problem);
 
         virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
@@ -80,12 +80,12 @@ namespace hpp {
     /// \}
 
     /// Member function definition
-    template <typename InnerPathOptimizer_t>
+    template <typename TraitsOrInnerType>
       GraphOptimizerPtr_t GraphOptimizer::create
       (const core::Problem& problem)
     {
       return GraphOptimizerPtr_t (
-          new GraphOptimizer (problem, InnerPathOptimizer_t::create)
+          new GraphOptimizer (problem, TraitsOrInnerType::create)
           );
     }
 

src/problem-solver.cc

@@ -45,6 +45,15 @@ namespace hpp {
         core::pathOptimization::PartialShortcutTraits {
           static bool removeLockedJoints () { return false; }
       };
+
+      template <typename InnerConfigOptimizationTraits>
+        struct GraphConfigOptimizationTraits {
+          static core::PathOptimizerPtr_t create (const core::Problem& problem)
+          {
+            return core::pathOptimization::ConfigOptimization::
+              createWithTraits <InnerConfigOptimizationTraits> (problem);
+          }
+        };
     }
 
     std::ostream& operator<< (std::ostream& os, const Device& robot)
@@ -67,6 +76,11 @@ namespace hpp {
       addPathOptimizerType ("ConfigOptimization",
           core::pathOptimization::ConfigOptimization::createWithTraits
           <pathOptimization::ConfigOptimizationTraits>);
+      addPathOptimizerType ("Graph-ConfigOptimization",
+          GraphOptimizer::create <
+          GraphConfigOptimizationTraits
+            <pathOptimization::ConfigOptimizationTraits>
+            >);
       pathPlannerType ("M-RRT");
       pathValidationType ("Graph-discretized", 0.05);
     }