Added more debug and error information

Needed while I was debugging and setting up the trajectory controller.