Optimization-Based Solutions for Planning and Control