RRT Motion Planning & Control Module Written by Aron Klevansky, Yotam Granov, and Alex Furman - April 2023