diff --git a/include/hpp/manipulation/path-planner/states-path-finder.hh b/include/hpp/manipulation/path-planner/states-path-finder.hh
index 3845a56ccd155093dad689ebee3136bec4e58b76..49bbfbc17c886121d8e8ae4172a2bad0f4b724db 100644
--- a/include/hpp/manipulation/path-planner/states-path-finder.hh
+++ b/include/hpp/manipulation/path-planner/states-path-finder.hh
@@ -150,16 +150,6 @@ namespace hpp {
           SolveStepStatus solveStep(std::size_t wp);
           Configuration_t configSolved (std::size_t wp) const;
 
-          /// use inner state planner to plan path between two configurations.
-          /// these configs lie on same leaf (same RHS wrt edge constraints)
-          /// eg consecutive configs in the solved config list
-          /// \param q1 start configuration
-          /// \param q2 destination configuration
-          /// \param edge the edge connecting the two configurations
-          /// \throw core::path_planning_failed if fail to plan path
-          void planInState(const ConfigurationPtr_t& q1,
-              const ConfigurationPtr_t& q2, const graph::EdgePtr_t& edge);
-
           /// deletes from memory the latest working states list, which is used to
           /// resume finding solutions from that list in case of failure at a
           /// later step.
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index ce7389a432a2aeeb76708a144fe6948d9f442de3..00d1ece1fc34888a5d5756719e65d931bc778d9a 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -1446,8 +1446,25 @@ namespace hpp {
         reset();
       }
 
-      void StatesPathFinder::planInState(const ConfigurationPtr_t& q1,
-          const ConfigurationPtr_t& q2, const graph::EdgePtr_t& edge) {
+      void StatesPathFinder::oneStep ()
+      {
+        if (idxConfigList_ == 0) {
+          // TODO: accommodate when goal is a set of constraints
+          assert(q1_);
+          configList_ = computeConfigList(*q1_, q2_);
+          if (configList_.size() <= 1) { // max depth reached
+            reset();
+            throw core::path_planning_failed("Maximal depth reached.");
+          }
+        }
+        size_t & idxSol = graphData_->idxSol;
+        ConfigurationPtr_t q1, q2;
+        const Edges_t& transitions = lastBuiltTransitions_;
+        q1 = ConfigurationPtr_t(new Configuration_t(configSolved
+                                                    (idxConfigList_)));
+        q2 = ConfigurationPtr_t(new Configuration_t(configSolved
+                                                    (idxConfigList_+1)));
+        const graph::EdgePtr_t& edge(transitions[idxConfigList_]);
         // Copy edge constraints
         core::ConstraintSetPtr_t constraints(HPP_DYNAMIC_PTR_CAST(
             core::ConstraintSet, edge->pathConstraint()->copy()));
@@ -1460,6 +1477,9 @@ namespace hpp {
         inStateProblem_->resetGoalConfigs();
         inStateProblem_->addGoalConfig(q2);
 
+        /// use inner state planner to plan path between two configurations.
+        /// these configs lie on same leaf (same RHS wrt edge constraints)
+        /// eg consecutive configs in the solved config list
         core::PathPlannerPtr_t inStatePlanner
           (core::DiffusingPlanner::create(inStateProblem_));
         core::PathOptimizerPtr_t inStateOptimizer
@@ -1468,45 +1488,19 @@ namespace hpp {
             ("StatesPathFinder/innerPlannerMaxIterations").intValue());
         inStatePlanner->timeOut(problem_->getParameter
             ("StatesPathFinder/innerPlannerTimeOut").floatValue());
-
-        core::PathVectorPtr_t path = inStatePlanner->solve();
-        for (std::size_t r = 0; r < path->numberPaths()-1; r++)
-          assert(path->pathAtRank(r)->end() ==
-                  path->pathAtRank(r+1)->initial());
-        roadmap()->merge(inStatePlanner->roadmap());
-        // core::PathVectorPtr_t opt = inStateOptimizer->optimize(path);
-        // roadmap()->insertPathVector(opt, true);
-      }
-
-      void StatesPathFinder::oneStep ()
-      {
-        if (idxConfigList_ == 0) {
-          // TODO: accommodate when goal is a set of constraints
-          assert(q1_);
-          configList_ = computeConfigList(*q1_, q2_);
-          if (configList_.size() <= 1) { // max depth reached
-            reset();
-            throw core::path_planning_failed("Maximal depth reached.");
-          }
-        }
-        size_t & idxSol = graphData_->idxSol;
-        ConfigurationPtr_t q1, q2;
+        hppDout(info, "calling InStatePlanner_.solve for transition "
+                << idxConfigList_);
+        core::PathVectorPtr_t path;
         try {
-          const Edges_t& transitions = lastBuiltTransitions_;
-          q1 = ConfigurationPtr_t(new Configuration_t(configSolved
-                                                      (idxConfigList_)));
-          q2 = ConfigurationPtr_t(new Configuration_t(configSolved
-                                                      (idxConfigList_+1)));
-          const graph::EdgePtr_t& edge(transitions[idxConfigList_]);
-          hppDout(info, "calling InStatePlanner_.solve for transition "
-                  << idxConfigList_);
-          planInState(q1, q2, edge);
+          path = inStatePlanner->solve();
+          for (std::size_t r = 0; r < path->numberPaths()-1; r++)
+            assert(path->pathAtRank(r)->end() ==
+                   path->pathAtRank(r+1)->initial());
           idxConfigList_++;
           if (idxConfigList_ == configList_.size()-1) {
             hppDout(warning, "Solution " << idxSol << ": Success"
                     << "\n-----------------------------------------------");
           }
-
         } catch(const core::path_planning_failed& error) {
           std::ostringstream oss;
           oss << "Error " << error.what() << "\n";
@@ -1523,6 +1517,11 @@ namespace hpp {
             idxSol++;
           }
         }
+        roadmap()->merge(inStatePlanner->roadmap());
+        // if (path) {
+        //   core::PathVectorPtr_t opt = inStateOptimizer->optimize(path);
+        //   roadmap()->insertPathVector(opt, true);
+        // }
       }
 
       void StatesPathFinder::tryConnectInitAndGoals()