[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.

src/path-planner/states-path-finder.cc

@@ -685,7 +685,22 @@ namespace hpp {
         (const graph::Edges_t& transitions)
       {
         OptimizationData& d = *optData_;
-        if (d.N == 0) return false;
+        // if no waypoint, check init and goal has same RHS
+        if (d.N == 0) {
+          assert (transitions.size() == 1);
+          assert (!goalDefinedByConstraints_);
+          size_type index = 0;
+          for (NumericalConstraints_t::const_iterator it (constraints_.begin ());
+              it != constraints_.end (); ++it, ++index) {
+            const ImplicitPtr_t& c (*it);
+            // Get transition solver
+            const Solver_t& trSolver
+              (transitions [0]->pathConstraint ()->configProjector ()->solver ());
+            if (!contains (trSolver, c)) continue;
+            if (!checkConstantRightHandSide (index)) return false;
+            }
+          return true;
+        }
         d.M_status.resize (constraints_.size (), d.N);
         d.M_status.fill (OptimizationData::ABSENT);
         d.M_rhs.resize (constraints_.size (), d.N);
@@ -1205,14 +1220,14 @@ namespace hpp {
             }
             if (nBadSolves >= nBadSolvesMax) {
               hppDout (warning, " Solution " << graphData_->idxSol << ": too many bad solve statuses. Resetting back to WP1");
-            }            
+            }
 #endif
 
             continue;
 
           } else if (nTriesDone[wp] >= nTriesMax) {
             // enough tries for a waypoint: backtrack
-            
+
             // update the longest valid sequence of waypoints solved
             if (wp -1 > wp_max) {
               // update the maximum index of valid waypoint
@@ -1539,6 +1554,9 @@ namespace hpp {
         GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front();
         GraphSearchData::state_with_depth_ptr_t _endState = d.addParent (_state, loopEdges[0]);
         d.solutions.push_back(_endState);
+
+        // try connecting initial and final configurations directly
+        if (!goalDefinedByConstraints_) PathPlanner::tryConnectInitAndGoals();
       }
 
       std::vector<std::string> StatesPathFinder::constraintNamesFromSolverAtWaypoint