[SPF] Fix case: init goal configs from same state

buildOptProblem should allow the case when there is no waypoint to be
computed (only 1 loop transition from init config to the goal).

PathPlanner::tryConnectInitAndGoals() should be called when the goal
configuration is given.