diff --git a/include/hpp/manipulation/path-planner/transition-planner.hh b/include/hpp/manipulation/path-planner/transition-planner.hh index 0e6240f3f30f5d09147bbe7d2d02610e348b3b0f..c7e558f92a9fde0be4205a2aaa28d68e26098031 100644 --- a/include/hpp/manipulation/path-planner/transition-planner.hh +++ b/include/hpp/manipulation/path-planner/transition-planner.hh @@ -162,6 +162,8 @@ class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner { PathPlannerPtr_t innerPlanner_; /// Vector of optimizers to call after solve std::vector<PathOptimizerPtr_t> pathOptimizers_; + /// Time parameterization instance + core::PathOptimizerPtr_t timeParameterization_; /// weak pointer to itself TransitionPlannerWkPtr_t weakPtr_; }; // class TransitionPlanner diff --git a/src/path-planner/transition-planner.cc b/src/path-planner/transition-planner.cc index bbde195044749ba32541cdf4aadbe6c1e04ae701..686fc6d9ce01e02c2d89aa76502c379e6b83b12e 100644 --- a/src/path-planner/transition-planner.cc +++ b/src/path-planner/transition-planner.cc @@ -32,6 +32,7 @@ #include <hpp/core/constraint-set.hh> #include <hpp/core/distance/reeds-shepp.hh> #include <hpp/core/joint-bound-validation.hh> +#include <hpp/core/path-optimization/rs-time-parameterization.hh> #include <hpp/core/path-optimization/simple-time-parameterization.hh> #include <hpp/core/path-optimizer.hh> #include <hpp/core/path-planner/bi-rrt-star.hh> @@ -159,10 +160,7 @@ core::PathVectorPtr_t TransitionPlanner::optimizePath(const PathPtr_t& path) { core::PathVectorPtr_t TransitionPlanner::timeParameterization( const PathVectorPtr_t& path) { - core::PathOptimizerPtr_t tp( - core::pathOptimization::SimpleTimeParameterization::create( - innerProblem_)); - return tp->optimize(path); + return timeParameterization_->optimize(path); } void TransitionPlanner::setEdge(std::size_t id) { @@ -188,6 +186,8 @@ void TransitionPlanner::setReedsAndSheppSteeringMethod(double turningRadius) { core::DistancePtr_t dist(core::distance::ReedsShepp::create(innerProblem_)); innerProblem_->steeringMethod(sm); innerProblem_->distance(dist); + timeParameterization_ = core::pathOptimization::RSTimeParameterization::create( + innerProblem_); } void TransitionPlanner::pathProjector(const PathProjectorPtr_t pathProjector) { @@ -233,6 +233,9 @@ TransitionPlanner::TransitionPlanner(const core::ProblemConstPtr_t& problem, // Create default path planner innerPlanner_ = hpp::core::pathPlanner::BiRrtStar::createWithRoadmap( innerProblem_, roadmap); + // Create default time parameterization + timeParameterization_ = core::pathOptimization::SimpleTimeParameterization::create( + innerProblem_); } void TransitionPlanner::init(TransitionPlannerWkPtr_t weak) {