[StatesPathFinder] Avoid loops in state sequence

Currently for most of the problems we look at, loops (an edge going into
the same state) are not useful. However, these loops appear in a lot of
state sequences considered by the algorithm and slow it down
significantly. Until we encounter a problem where loops are necessary to
accomplish the task, we should just ignore them.

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

@@ -237,6 +237,10 @@ namespace hpp {
         return false;
       }
 
+      static bool isLoopTransition (const graph::EdgePtr_t& transition) {
+        return transition->stateTo() == transition->stateFrom();
+      }
+
       void StatesPathFinder::gatherGraphConstraints ()
       {
         typedef graph::Edge Edge;
@@ -315,6 +319,9 @@ namespace hpp {
             // Avoid identical consecutive transition
             if (transition == parent.e) continue;
 
+            // Avoid loop transitions
+            if (isLoopTransition(transition)) continue;
+
             // Insert parent
             d.queue1.push (
               d.addParent (_state, transition)