Sandwich Approach for Motion Planning and Control