[SW-AI]:  Improve RRT (Removed robot deadlocks by improving the sampler or other logic. Can also change path planner)

### Contact Details

anton

### Describe the issue

robots should be able to twerk

### How long do you estimate to implement this?

None