Safe nonlinear trajectory generation for parallel autonomy with a dynamic vehicle model