Dubin's car example working, prints best trajectory