search

motion-planning / rrt-algorithms

n-dimensional RRT, RRT* (RRT-Star)
MIT License
585 stars 171 forks source link

step size in rrt-connect #33

Closed cengzezou closed 2 months ago

cengzezou commented 3 months ago

Great work! I spot is a general issue in the use of step size in steer function. Maybe your other RRT implementations also suffer from the same issue. Please let me know if the following observations are wrong.

In your code, steer always takes a fixed step size which is fine till q_new gets very close to the target.

Let's think about the case of rrt-connect. The second tree is repeatedly getting closer to a target vertex q_first in the first tree, Eventually it reaches a point where distance between q_second and q_first is below edge length q. If I understand your code correctly, in such a case the next steer will move beyond q_first to a position q_last_steer. This has two consequences:

  1. q_second and q_first are sampled within boundaries. But q_last_steer may not be. I suspect this is the cause of the out-of-bound errors reported by other users (your shouldn't need to define bound_point function).
  2. The two trees at this moment should be very likely to connect since they have two nodes that are less than q-distance apart. But sadly, q_last_steer will not yield REACH unless you increase the threshold 1e-2 to the scale of edge length q.

This explains why rrt-connect does not exhibit superior efficiency against other planners. I suggest a quick fix by simply modifying the step size used in steer to be min(q, norm(q_second-q_first)) .

SZanlongo commented 3 months ago

@cengzezou, appreciate your looking into this! That solution makes sense. If you have time to test out the solution, I'd be happy to merge a PR with the changes.

SZanlongo commented 2 months ago

@cengzezou, I've implemented the changes. Let me know if it reflects what you had in mind.