From 1656dea789a3435f774b278a5087c81e5647b975 Mon Sep 17 00:00:00 2001
From: Toefinder <43576719+Toefinder@users.noreply.github.com>
Date: Sat, 2 Jul 2022 20:15:57 +0200
Subject: [PATCH] [SPF] Fix case: init goal configs from same state
buildOptProblem should allow the case when there is no waypoint to be
computed (only 1 loop transition from init config to the goal).
PathPlanner::tryConnectInitAndGoals() should be called when the goal
configuration is given.
---
src/path-planner/states-path-finder.cc | 24 +++++++++++++++++++++---
1 file changed, 21 insertions(+), 3 deletions(-)
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index 0c846d55..6e8d4bf4 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -685,7 +685,22 @@ namespace hpp {
(const graph::Edges_t& transitions)
{
OptimizationData& d = *optData_;
- if (d.N == 0) return false;
+ // if no waypoint, check init and goal has same RHS
+ if (d.N == 0) {
+ assert (transitions.size() == 1);
+ assert (!goalDefinedByConstraints_);
+ size_type index = 0;
+ for (NumericalConstraints_t::const_iterator it (constraints_.begin ());
+ it != constraints_.end (); ++it, ++index) {
+ const ImplicitPtr_t& c (*it);
+ // Get transition solver
+ const Solver_t& trSolver
+ (transitions [0]->pathConstraint ()->configProjector ()->solver ());
+ if (!contains (trSolver, c)) continue;
+ if (!checkConstantRightHandSide (index)) return false;
+ }
+ return true;
+ }
d.M_status.resize (constraints_.size (), d.N);
d.M_status.fill (OptimizationData::ABSENT);
d.M_rhs.resize (constraints_.size (), d.N);
@@ -1205,14 +1220,14 @@ namespace hpp {
}
if (nBadSolves >= nBadSolvesMax) {
hppDout (warning, " Solution " << graphData_->idxSol << ": too many bad solve statuses. Resetting back to WP1");
- }
+ }
#endif
continue;
} else if (nTriesDone[wp] >= nTriesMax) {
// enough tries for a waypoint: backtrack
-
+
// update the longest valid sequence of waypoints solved
if (wp -1 > wp_max) {
// update the maximum index of valid waypoint
@@ -1539,6 +1554,9 @@ namespace hpp {
GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front();
GraphSearchData::state_with_depth_ptr_t _endState = d.addParent (_state, loopEdges[0]);
d.solutions.push_back(_endState);
+
+ // try connecting initial and final configurations directly
+ if (!goalDefinedByConstraints_) PathPlanner::tryConnectInitAndGoals();
}
std::vector<std::string> StatesPathFinder::constraintNamesFromSolverAtWaypoint
--
GitLab