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