From 5b36ad07733e653d2f039066be2921a63c9da243 Mon Sep 17 00:00:00 2001
From: Le Quang Anh <43576719+Toefinder@users.noreply.github.com>
Date: Mon, 28 Mar 2022 17:44:01 +0200
Subject: [PATCH] Fix: analyse each new transition list
BUG: if a certain transition list succeeds to give a configuration list for
the first time, and fails for the second time, a new transition list
will be considered, but skipColAnalysis is NOT reset. This is now fixed
by using the direct comparison with nTryConfigList_ instead of using an
additional data member.
---
.../hpp/manipulation/path-planner/states-path-finder.hh | 2 --
src/path-planner/states-path-finder.cc | 7 +++----
2 files changed, 3 insertions(+), 6 deletions(-)
diff --git a/include/hpp/manipulation/path-planner/states-path-finder.hh b/include/hpp/manipulation/path-planner/states-path-finder.hh
index fa93bf8e..6a6f870b 100644
--- a/include/hpp/manipulation/path-planner/states-path-finder.hh
+++ b/include/hpp/manipulation/path-planner/states-path-finder.hh
@@ -252,8 +252,6 @@ namespace hpp {
std::size_t idxSol_ = 0;
graph::Edges_t lastBuiltTransitions_;
- bool skipColAnalysis_;
-
// Constraints defining the goal
NumericalConstraints_t goalConstraints_;
bool goalDefinedByConstraints_;
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index d5f8a3e7..7a01a9d3 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -104,7 +104,7 @@ namespace hpp {
problem_ (HPP_STATIC_PTR_CAST(const manipulation::Problem, problem)),
constraints_(), index_(), sameRightHandSide_(),
stricterConstraints_(), optData_(0x0), graphData_(0x0),
- idxSol_(0), lastBuiltTransitions_(), skipColAnalysis_(false),
+ idxSol_(0), lastBuiltTransitions_(),
goalConstraints_(), goalDefinedByConstraints_(false),
q1_(0x0), q2_(0x0), configList_(), idxConfigList_(0),
nTryConfigList_(0), solved_(false), interrupt_(false),
@@ -1415,7 +1415,7 @@ namespace hpp {
if (buildOptimizationProblem (transitions)) {
lastBuiltTransitions_ = transitions;
- if (skipColAnalysis_ || analyseOptimizationProblem (transitions)) {
+ if (nTryConfigList_ > 0 || analyseOptimizationProblem (transitions)) {
if (solveOptimizationProblem ()) {
core::Configurations_t path = getConfigList ();
hppDout (warning, " Solution " << idxSol << ": solved configurations list");
@@ -1475,7 +1475,7 @@ namespace hpp {
if (buildOptimizationProblem2 (transitions)) {
lastBuiltTransitions_ = transitions;
- if (skipColAnalysis_ || analyseOptimizationProblem2 (transitions, problem())) {
+ if (nTryConfigList_ > 0 || analyseOptimizationProblem2 (transitions, problem())) {
if (solveOptimizationProblem ()) {
core::Configurations_t path = getConfigList ();
hppDout (warning, " Solution " << idxSol << ": solved configurations list");
@@ -1592,7 +1592,6 @@ namespace hpp {
void StatesPathFinder::oneStep ()
{
if (idxConfigList_ == 0) {
- skipColAnalysis_ = (nTryConfigList_ >= 1); // already passed, don't redo it
// TODO: accommodate when goal is a set of constraints
assert(q1_);
if (!goalDefinedByConstraints_) {
--
GitLab