Path circles need z-value set above other things
Created a dubins path intermediate planner