Reset rrt tree when replanning for collisions avoidance