-Add the inital setup for RRT planning