From ffe809718aebc3b49dadbfb43f71c49a3dfdfa35 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Mon, 24 Jun 2024 12:05:06 +0000
Subject: [PATCH] [TransitionPlanner] Use Reeds and Shepp Parameterization when
relevant.
---
.../manipulation/path-planner/transition-planner.hh | 2 ++
src/path-planner/transition-planner.cc | 11 +++++++----
2 files changed, 9 insertions(+), 4 deletions(-)
diff --git a/include/hpp/manipulation/path-planner/transition-planner.hh b/include/hpp/manipulation/path-planner/transition-planner.hh
index 0e6240f3..c7e558f9 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 bbde1950..686fc6d9 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) {
--
GitLab