diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index 9a59b99a5d2d50c590efc3d573efcf85366e3d4b..ca82c632dce082ce158c6b6095daa41b6baa1e09 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -424,7 +424,7 @@ namespace hpp {
       {
         assert (goalDefinedByConstraints_);
         Edges_t transitions;
-        while (d.queueIt != d.queue1.size()) {
+        while (d.queueIt != d.queue1.size() && transitions.empty()) {
           GraphSearchData::state_with_depth_ptr_t _state = d.queue1.at(d.queueIt);
           ++d.queueIt;
           // check that the state is one of the goal states