Rewire never occurs due to inaccurate nearby vertices cost calculation.

[tahsinkose]

Hey @SZanlongo. I would like to first thank you for your great work. It was extremely useful in my own C++ RRT library implementation.

One thing I have noticed during my experiments is that the tent_cost is always more than curr_cost. Its only reason is the following line: path_cost(self.trees[tree].E, x_init, x_near) + segment_cost(x_near, x_new). You are computing all the cost from start when you are searching for nearby vertices. But then in rewire(), you add this cost to tentative cost one more time unnecessarily. That is, you simply double the tentative cost and it becomes impossible for it to be better than current cost. Despite this, your RRT behaves very different than RRT, because you sort the nearby vertices according to their cost and connect with the minimal one. If you use connect_to_point() of RRT in RRT, the path you will get will be the exact same of the former one without any rewiring.

I'm preparing to submit a PR for that, that's why I would like to note the problem. Again, many thanks for your effort.

EDIT: After printing costs of the path, I found out that current version outputs a much shorter path. However, rewire function is never called since get_nearby_vertices already determines the optimum nearest node due to different metric for cost. In classical RRT implementations, that cost is computed with segment_cost(x_near, x_new) only. In terms of efficiency, this is much better, but I would like to know whether this is RRT or has another name.

For educational purposes, I'm still planning to submit the PR. Perhaps we can discuss in more detail.

[SZanlongo]

@tahsinkose, thanks for taking the time to explain the issue so thoroughly. I haven't looked at this project in a while, but it looks like you are correct regarding the doubling of the cost-to-come; it should only add the cost-to-go to the existing value. I'll gladly take a PR fixing that.

Regarding your second observation, I'm not sure; I'll have to spend some time to go over the code and see what is going on.

[tahsinkose]

Actually a mathematical approach on the current metric should finalize the ambiguity in here. It takes the argmin nearest node according to the tree, which points to greedy property. So basically, this is a greedy RRT. I made a brief literature review and didn't encounter this metric. In every study, the metric that I've described seems to be used. I also checked OMPL implementation and it uses the classical method too.

A PR is a 2-min issue in this case, but I think current solution is also valuable and you might choose to name it differently like I proposed so that it is not lost by my adjustment. If you want, I can also provide a stochastic world generation (I have already done it in C++, just need to translate to Python) with meaningful gaps in it so that we can compare classical RRT* and this version extensively in different configurations.

EDIT: Further search reveals out Greedy RRT as a different algorithm. One final question: Have you written the current cost computation accidently as is or using a paper? If it is the latter one, we can enlighten the issue :)

[SZanlongo]

I couldn't find the paper I remember reading, but Algorithm 6 here should be the correct one. Based on that, I think the solution is simply replacing tent_cost with tent_cost = path_cost(self.trees[tree].E, self.x_init, x_new) + segment_cost(x_new, x_near). This metric makes more sense without doubling the cost, and allows rewiring to take place when a nearby vertex has a shorter path through the new vertex. Was this the solution you had in mind? If not, we can define it as a different solution as you suggested.

The world generation sounds good, it would be a nice demo for new users to play with.

[tahsinkose]

Hey I have just submitted #16 .Could you please inspect that? I think you should change current RRT with the classical method and name the old one with a new name. Greedy RRT is a different algorithm, but it can be Greedy RRT. I think it really needs consideration, since it might be a new RRT-variant :smile:

[SZanlongo]

Merged and changed name of original to rrt_star_rewire.

Thanks for taking the time to reach out and improve this project!