Done: RRT works with a box(0.4,0.4,[1.22,2])), step_size(0.1)