From 867867a29ac6b668b1400577dcf78ef1ceb3212a Mon Sep 17 00:00:00 2001
From: Le Quang Anh <43576719+Toefinder@users.noreply.github.com>
Date: Mon, 30 May 2022 16:05:44 +0200
Subject: [PATCH] Add to roadmap when inStatePlanner fails
Previously, when the inStatePlanner fails to compute path between two
configurations on the same leaf, the roadmap generated is not added to
the main roadmap of StatesPathFinder. Now it is added so that the
roadmap gets expanded whenever inStatePlanner is used. The good side
effect is fairer comparison between different algorithms when comparing by the number
of nodes in the roadmap.
---
.../path-planner/states-path-finder.hh | 10 ---
src/path-planner/states-path-finder.cc | 69 +++++++++----------
2 files changed, 34 insertions(+), 45 deletions(-)
diff --git a/include/hpp/manipulation/path-planner/states-path-finder.hh b/include/hpp/manipulation/path-planner/states-path-finder.hh
index 3845a56c..49bbfbc1 100644
--- a/include/hpp/manipulation/path-planner/states-path-finder.hh
+++ b/include/hpp/manipulation/path-planner/states-path-finder.hh
@@ -150,16 +150,6 @@ namespace hpp {
SolveStepStatus solveStep(std::size_t wp);
Configuration_t configSolved (std::size_t wp) const;
- /// use inner state planner to plan path between two configurations.
- /// these configs lie on same leaf (same RHS wrt edge constraints)
- /// eg consecutive configs in the solved config list
- /// \param q1 start configuration
- /// \param q2 destination configuration
- /// \param edge the edge connecting the two configurations
- /// \throw core::path_planning_failed if fail to plan path
- void planInState(const ConfigurationPtr_t& q1,
- const ConfigurationPtr_t& q2, const graph::EdgePtr_t& edge);
-
/// deletes from memory the latest working states list, which is used to
/// resume finding solutions from that list in case of failure at a
/// later step.
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index ce7389a4..00d1ece1 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -1446,8 +1446,25 @@ namespace hpp {
reset();
}
- void StatesPathFinder::planInState(const ConfigurationPtr_t& q1,
- const ConfigurationPtr_t& q2, const graph::EdgePtr_t& edge) {
+ void StatesPathFinder::oneStep ()
+ {
+ if (idxConfigList_ == 0) {
+ // TODO: accommodate when goal is a set of constraints
+ assert(q1_);
+ configList_ = computeConfigList(*q1_, q2_);
+ if (configList_.size() <= 1) { // max depth reached
+ reset();
+ throw core::path_planning_failed("Maximal depth reached.");
+ }
+ }
+ size_t & idxSol = graphData_->idxSol;
+ ConfigurationPtr_t q1, q2;
+ const Edges_t& transitions = lastBuiltTransitions_;
+ q1 = ConfigurationPtr_t(new Configuration_t(configSolved
+ (idxConfigList_)));
+ q2 = ConfigurationPtr_t(new Configuration_t(configSolved
+ (idxConfigList_+1)));
+ const graph::EdgePtr_t& edge(transitions[idxConfigList_]);
// Copy edge constraints
core::ConstraintSetPtr_t constraints(HPP_DYNAMIC_PTR_CAST(
core::ConstraintSet, edge->pathConstraint()->copy()));
@@ -1460,6 +1477,9 @@ namespace hpp {
inStateProblem_->resetGoalConfigs();
inStateProblem_->addGoalConfig(q2);
+ /// use inner state planner to plan path between two configurations.
+ /// these configs lie on same leaf (same RHS wrt edge constraints)
+ /// eg consecutive configs in the solved config list
core::PathPlannerPtr_t inStatePlanner
(core::DiffusingPlanner::create(inStateProblem_));
core::PathOptimizerPtr_t inStateOptimizer
@@ -1468,45 +1488,19 @@ namespace hpp {
("StatesPathFinder/innerPlannerMaxIterations").intValue());
inStatePlanner->timeOut(problem_->getParameter
("StatesPathFinder/innerPlannerTimeOut").floatValue());
-
- core::PathVectorPtr_t path = inStatePlanner->solve();
- for (std::size_t r = 0; r < path->numberPaths()-1; r++)
- assert(path->pathAtRank(r)->end() ==
- path->pathAtRank(r+1)->initial());
- roadmap()->merge(inStatePlanner->roadmap());
- // core::PathVectorPtr_t opt = inStateOptimizer->optimize(path);
- // roadmap()->insertPathVector(opt, true);
- }
-
- void StatesPathFinder::oneStep ()
- {
- if (idxConfigList_ == 0) {
- // TODO: accommodate when goal is a set of constraints
- assert(q1_);
- configList_ = computeConfigList(*q1_, q2_);
- if (configList_.size() <= 1) { // max depth reached
- reset();
- throw core::path_planning_failed("Maximal depth reached.");
- }
- }
- size_t & idxSol = graphData_->idxSol;
- ConfigurationPtr_t q1, q2;
+ hppDout(info, "calling InStatePlanner_.solve for transition "
+ << idxConfigList_);
+ core::PathVectorPtr_t path;
try {
- const Edges_t& transitions = lastBuiltTransitions_;
- q1 = ConfigurationPtr_t(new Configuration_t(configSolved
- (idxConfigList_)));
- q2 = ConfigurationPtr_t(new Configuration_t(configSolved
- (idxConfigList_+1)));
- const graph::EdgePtr_t& edge(transitions[idxConfigList_]);
- hppDout(info, "calling InStatePlanner_.solve for transition "
- << idxConfigList_);
- planInState(q1, q2, edge);
+ path = inStatePlanner->solve();
+ for (std::size_t r = 0; r < path->numberPaths()-1; r++)
+ assert(path->pathAtRank(r)->end() ==
+ path->pathAtRank(r+1)->initial());
idxConfigList_++;
if (idxConfigList_ == configList_.size()-1) {
hppDout(warning, "Solution " << idxSol << ": Success"
<< "\n-----------------------------------------------");
}
-
} catch(const core::path_planning_failed& error) {
std::ostringstream oss;
oss << "Error " << error.what() << "\n";
@@ -1523,6 +1517,11 @@ namespace hpp {
idxSol++;
}
}
+ roadmap()->merge(inStatePlanner->roadmap());
+ // if (path) {
+ // core::PathVectorPtr_t opt = inStateOptimizer->optimize(path);
+ // roadmap()->insertPathVector(opt, true);
+ // }
}
void StatesPathFinder::tryConnectInitAndGoals()
--
GitLab