More robust manual_control

Add sensible options and warning messages to all select_motion_mode fail paths
Query present motion mode before trying to change
Actually check all servos have reached their target
Log current motion mode on query
More sensible header on stitched_path