fixed constraint planner error when initial traj is bad