Add a RRT planning to avoid obstacles