adjustments to robot_control for waypoints
Merge branches 'version4' and 'temp2' into version4