Python Simulation: Formation Flight RRT* Algorithm