Optimize car trajectory in constrained time frame using dynamic/kinematic limits and collision as constraints for non-linear optimization problem. Localize with LiDAR scan matching and feed estimated pose to MPC.
Formulate high-level symbolic planning as a smooth optimization problem and combine with low-level CIO framework to optimize over complex contact planning problems.