A Nonlinear Planner with Loop Avoidance