Refactored MoveToPose, almost ready to make another program that incorporates the sensors.