I was producing metrics with these algorithms and figured out this problem which results in an infinite loop in Heuristicized Bidirectional RRT*. Whenever it shortens the path, the best cost should be adapted accordingly after the line:
self.sigma_best = self.sigma_best[:min(a, b) + 1] + self.sigma_best[max(a, b):]
So we need to simply iterate over the sigma_best and compute new cost after the line above. I'm planning to submit a minor PR for that in this evening.
Hi there,
I was producing metrics with these algorithms and figured out this problem which results in an infinite loop in Heuristicized Bidirectional RRT*. Whenever it shortens the path, the best cost should be adapted accordingly after the line:
self.sigma_best = self.sigma_best[:min(a, b) + 1] + self.sigma_best[max(a, b):]
So we need to simply iterate over the sigma_best and compute new cost after the line above. I'm planning to submit a minor PR for that in this evening.