Motion Planning and Control using Trajectory Optimization