From 3a915cccd04899853db07eb22e93a32af18a8ac4 Mon Sep 17 00:00:00 2001
From: Le Quang Anh <43576719+Toefinder@users.noreply.github.com>
Date: Mon, 7 Mar 2022 18:14:11 +0100
Subject: [PATCH] Keep longest VALID config seq if cannot solve full
The expected behavior of solveOptimizationProblem () function is that,
if it manages to compute configurations for all waypoints, it will
return that sequence of configurations. In case when it fails, it
should return the longest sequence of valid configurations.
The previous implementation saves each configuration computed, even if
it is invalid. Also, when backtracking to a previous waypoint, the new
points computed will overwrite some of the waypoint in the longest
sequence. So indeed, the final sequence kept is not valid, and not
consistent (not generated in successful consecutive solves).
This has now been fixed by saving the longest sequence of valid
configurations in a separate variable, and only update it when a longer
sequence is found.
---
src/path-planner/states-path-finder.cc | 16 +++++++++++++++-
1 file changed, 15 insertions(+), 1 deletion(-)
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index d31d14ca..39fb070d 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -1140,6 +1140,7 @@ namespace hpp {
matrix_t secmat1 = edges[j]->securityMargins();
matrix_t secmat2;
if (j == edges.size()-1) {
+ // if j is the goal node, there is no edge after this node
secmat2 = secmat1;
} else {
secmat2 = edges[j+1]->securityMargins();
@@ -1199,16 +1200,27 @@ namespace hpp {
std::vector<std::size_t> nTriesDone(d.solvers.size()+1, 0);
std::size_t nFails = 0;
std::size_t wp = 1; // waypoint index starting at 1 (wp 0 = q1)
-
+ std::size_t wp_max = 0; // all waypoints up to this index are valid solved
+ matrix_t longestSolved(d.nq, d.N);
+ longestSolved.setZero();
while (wp <= d.solvers.size()) {
// enough tries for a waypoint: backtrack or stop
while (nTriesDone[wp] >= nTriesMax) {
if (wp == 1) {
if (nTriesDone[wp] < nTriesMax1)
break;
+ // if cannot solve all the way, return longest VALID sequence
+ d.waypoint = longestSolved;
displayConfigsSolved();
return false; // too many tries that need to reset the entire solution
}
+ // update the longest valid sequence of waypoints solved
+ if (wp -1 > wp_max) {
+ // update the maximum index of valid waypoint
+ wp_max = wp - 1;
+ // save the sequence
+ longestSolved.leftCols(wp_max) = d.waypoint.leftCols(wp_max);
+ }
do {
nTriesDone[wp] = 0;
wp--; // backtrack: try a new solution for the latest waypoint
@@ -1221,6 +1233,8 @@ namespace hpp {
nTriesDone[k] = 0;
wp = 1;
if (nTriesDone[1] >= nTriesMax1) {
+ // if cannot solve all the way, return longest VALID sequence
+ d.waypoint = longestSolved;
displayConfigsSolved();
return false;
}
--
GitLab