Optimization-based trajectory planning

- Optimization-based trajectory planning for urban-highway scenarios
- Design costs, constraints, and collision boundaries
- Trajectory validation considering collision, vehicle dynamics, comfort
- Replanning decision to reduce computing resources