diff --git a/include/hpp/manipulation/steering-method/cross-state-optimization.hh b/include/hpp/manipulation/steering-method/cross-state-optimization.hh index 14293f6128a0730d85c4a18fe4381c709a94aa81..6563101556b3dd2a6c0b949430619b6fb83bf73c 100644 --- a/include/hpp/manipulation/steering-method/cross-state-optimization.hh +++ b/include/hpp/manipulation/steering-method/cross-state-optimization.hh @@ -35,7 +35,11 @@ namespace hpp { static CrossStateOptimizationPtr_t create (const Problem& problem); /// \warning core::Problem will be casted to Problem - static CrossStateOptimizationPtr_t createFromCore + static CrossStateOptimizationPtr_t create + (const core::Problem& problem); + + template <typename T> + static CrossStateOptimizationPtr_t create (const core::Problem& problem); core::SteeringMethodPtr_t copy () const; @@ -77,6 +81,15 @@ namespace hpp { /// Weak pointer to itself CrossStateOptimizationWkPtr_t weak_; }; // class CrossStateOptimization + + template <typename T> + CrossStateOptimizationPtr_t CrossStateOptimization::create + (const core::Problem& problem) + { + CrossStateOptimizationPtr_t gsm = CrossStateOptimization::create (problem); + gsm->innerSteeringMethod (T::create (problem)); + return gsm; + } } // namespace steeringMethod } // namespace manipulation } // namespace hpp diff --git a/src/problem-solver.cc b/src/problem-solver.cc index 28258129d01964e3ed275d4853e53ac19d7fb7cf..6016b9f50e4e2db209e34505850c606418b3b048 100644 --- a/src/problem-solver.cc +++ b/src/problem-solver.cc @@ -154,14 +154,20 @@ namespace hpp { steeringMethod::Graph::create <core::steeringMethod::Straight>); steeringMethods.add ("Graph-Hermite", steeringMethod::Graph::create <core::steeringMethod::Hermite>); - steeringMethods.add ("CrossStateOptimization", - steeringMethod::CrossStateOptimization::createFromCore); steeringMethods.add ("Graph-ReedsShepp", createSMWithGuess <steeringMethod::Graph, core::steeringMethod::ReedsShepp>); steeringMethods.add ("Graph-Dubins", createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Dubins>); steeringMethods.add ("Graph-Snibud", createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Snibud>); + steeringMethods.add ("CrossStateOptimization-Straight", + steeringMethod::CrossStateOptimization::create<core::steeringMethod::Straight>); + steeringMethods.add ("CrossStateOptimization-ReedsShepp", + createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::ReedsShepp>); + steeringMethods.add ("CrossStateOptimization-Dubins", + createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::Dubins>); + steeringMethods.add ("CrossStateOptimization-Snibud", + createSMWithGuess <steeringMethod::CrossStateOptimization, core::steeringMethod::Snibud>); pathOptimizers.add ("KeypointsShortcut", pathOptimization::Keypoints::create); diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc index 28fbf13f936ed91240bca44bfc88d767442901be..9bd2781f4dd892f99aa138fc56f0cce6f781bf7c 100644 --- a/src/steering-method/cross-state-optimization.cc +++ b/src/steering-method/cross-state-optimization.cc @@ -59,7 +59,7 @@ namespace hpp { return shPtr; } - CrossStateOptimizationPtr_t CrossStateOptimization::createFromCore ( + CrossStateOptimizationPtr_t CrossStateOptimization::create ( const core::Problem& problem) { HPP_STATIC_CAST_REF_CHECK (const Problem, problem);