NLMPC and Hybrid A*

motion planning and control