search

sea-bass / pyrobosim

ROS 2 enabled 2D mobile robot simulator for behavior prototyping.
https://pyrobosim.readthedocs.io/
MIT License
232 stars 38 forks source link

[Planning] RRT Implementation Improvements #15

Closed sea-bass closed 1 year ago

sea-bass commented 2 years ago

I found 2 annoying things about the RRT implementation in testing other stuff:

We don't check if we can immediately connect to the goal over short distances. So some trees will look silly like this:

image

Furthermore, in a bidirectional RRT* implementation we don't try rewire the tree once we connect the start and end nodes. You can see that in the kink here, for example:

image

This second one might be a little trickier to do in that in may actually require the solution path to be added to the start tree and then running a rewire on all the newly connected nodes.

ibrahiminfinite commented 1 year ago

Are there any specific examples of where the second issue happened?

ibrahiminfinite commented 1 year ago

I am able to replicate the first issue 

start = Pose(x=-2.2, y=2.8)
goal = Pose(x=-2.2, y=3.0)

zoomed_bug_image

sea-bass commented 1 year ago

I suspect this is a combination of using bidirectional RRT and RRT for path shortening, in that the RRT step isn't shortening the connection between start and goal trees.