The choice of heuristic function in RRT Star

[sosoeeee]

Thank you for your excellent work! It has been incredibly helpful to me.

While reviewing your code, I encountered some confusion regarding the design of the "Heuristic function" within the following functions in rrt_star.py:

pythonCopy codedef connect_shortest_valid(self, tree, x_new, L_near):
    """
    Connect to the nearest vertex that has an unobstructed path
    :param tree: int, tree being added to
    :param x_new: tuple, vertex being added
    :param L_near: list of nearby vertices
    """
    # Check nearby vertices for total cost and connect shortest valid edge
    for c_near, x_near in L_near:
        if c_near + cost_to_go(x_near, self.x_goal) < self.c_best and self.connect_to_point(tree, x_near, x_new):
            break

I perceive the expression "c_near + cost_to_go(x_near, self.x_goal)" as a heuristic function. However, I'm not quite sure why this particular heuristic function was chosen.

Additionally, when running the RRT star algorithm alone, the value of self.c_best remains inf. Consequently, the function above consistently chooses the first collision-free nearby vertex as the parent node of the new vertex.

[SZanlongo]

@sosoeeee, I've pushed a commit that cleans up what you were seeing. The heuristic is meant to only add edges that would result in a shorter path than the current best solution. The heuristic was consolidated into the bidirectional approach, and the stale code from rrt_star was removed. A possible future improvement could be to make the heuristic accessible to all rrt approaches.