https://www.youtube.com/watch?v=GlYgs6v2YfU
In general, motion planning multi-axis systems is NP-Hard. Have a great day =3