Catch failed trajectory movement
Sometimes The trajectory fails to start because it is not at the right position.
- catch this error in feedback and stop the print
- print out which trajectory failed (so print can be manually continued from that point)
- if failure is caught, plan and move to start of location and try again to avoid manual stop