Bridging the gap between optimal trajectory planning and safety-critical control with applications to autonomous vehicles.