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
Merge remote-tracking branch 'origin/pvt_segment_mode' into sbpl
Reduced the number of intermediate poses per motion primitive