From 208126a7206ac7dd01f257bafc3a1f0d8f6d9a50 Mon Sep 17 00:00:00 2001
From: Le Quang Anh <43576719+Toefinder@users.noreply.github.com>
Date: Tue, 14 Jun 2022 13:45:26 +0200
Subject: [PATCH] Set path projector instateplanner same as problem
Note that we need to set it **after** the problem's path projector has
been initialized.
---
src/path-planner/states-path-finder.cc | 10 ++++------
1 file changed, 4 insertions(+), 6 deletions(-)
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index 07e177b..69f52b4 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -111,10 +111,6 @@ namespace hpp {
{
gatherGraphConstraints ();
inStateProblem_ = core::Problem::create(problem_->robot());
- core::PathProjectorPtr_t pathProjector
- (core::pathProjector::Progressive::create
- (inStateProblem_, 1e-2));
- inStateProblem_->pathProjector(pathProjector);
}
StatesPathFinder::StatesPathFinder (const StatesPathFinder& other) :
@@ -1374,6 +1370,7 @@ namespace hpp {
q1_ = problem_->initConfig();
assert(q1_);
+ inStateProblem_->pathProjector(problem_->pathProjector());
const graph::GraphPtr_t& graph(problem_->constraintGraph ());
graphData_.reset(new GraphSearchData());
GraphSearchData& d = *graphData_;
@@ -1478,6 +1475,7 @@ namespace hpp {
assert(constraints->isSatisfied(*q2));
inStateProblem_->constraints(constraints);
inStateProblem_->pathValidation(edge->pathValidation());
+ inStateProblem_->steeringMethod(edge->steeringMethod());
inStateProblem_->initConfig(q1);
inStateProblem_->resetGoalConfigs();
inStateProblem_->addGoalConfig(q2);
@@ -1487,8 +1485,6 @@ namespace hpp {
/// eg consecutive configs in the solved config list
core::PathPlannerPtr_t inStatePlanner
(core::DiffusingPlanner::create(inStateProblem_));
- core::PathOptimizerPtr_t inStateOptimizer
- (core::pathOptimization::RandomShortcut::create(inStateProblem_));
inStatePlanner->maxIterations(problem_->getParameter
("StatesPathFinder/innerPlannerMaxIterations").intValue());
inStatePlanner->timeOut(problem_->getParameter
@@ -1524,6 +1520,8 @@ namespace hpp {
}
roadmap()->merge(inStatePlanner->roadmap());
// if (path) {
+ // core::PathOptimizerPtr_t inStateOptimizer
+ // (core::pathOptimization::RandomShortcut::create(inStateProblem_));
// core::PathVectorPtr_t opt = inStateOptimizer->optimize(path);
// roadmap()->insertPathVector(opt, true);
// }
--
GitLab