increase default grab height to 100, set temp waypoints for testing
Merge branches 'version4' and 'qwe123' into version4
added waypoint (bug), tune grab mode for state machine with vision
Merge branches 'version4' and 'oiu' into version4