Implement InStatePathPlanner

CMakeLists.txt

@@ -81,6 +81,8 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/manipulation/path-optimization/spline-gradient-based.hh
 
   include/hpp/manipulation/path-planner/end-effector-trajectory.hh
+  include/hpp/manipulation/path-planner/states-path-finder.hh
+  include/hpp/manipulation/path-planner/in-state-path.hh
 
   include/hpp/manipulation/problem-target/state.hh
 
@@ -123,6 +125,8 @@ SET(${PROJECT_NAME}_SOURCES
   src/path-optimization/enforce-transition-semantic.cc
 
   src/path-planner/end-effector-trajectory.cc
+  src/path-planner/states-path-finder.cc
+  src/path-planner/in-state-path.cc
 
   src/problem-target/state.cc
 

NEWS

@@ -24,7 +24,7 @@ New in 4.10.0
   - call the problem PathValidation instead of the edge one.
 * In class ProblemSolver
   - register contact constraints and complement in constraint graph.
-  
+
 New in 4.8.0
 * Rewrite steering method CrossStateOptimization.
 * In graph component classes (State, Edge, Graph) locked joints are handled as other numerical constraints.

TODO.txt

+- InStatePath::solve
+    BiRRT* does not work properly at all:
+        - discontinuities due to an algorithmic mistake involving qProj_
+        - not using path projectors, while it should
+    DiffusingPlanner does not work properly sometimes
+        - Some collisions were detected on paths solves for romeo-placard
+          both with discrete and continuous (Progressive, 0.2) path validations
+
+
+- InStatePath::mergeLeafRoadmapWith
+    this is inefficient because the roadmap recomputes the connected
+    component at every step. A better merge function should be added to roadmap.cc
+
+
+- path optimizations after solving once :
+    They are done "locally" (for each of the leafs separately) in C++ with a
+    hard-coded optimizer (Graph-RandomShortcut) and then globally with the
+    command ps.addPathOptimizer("Graph-RandomShortcut"). Due to how this works,
+    this is like doing two times the same thing so the first should be removed.
+    However bugs have been observed when the global optimization is used (Core 
+    dumped). For unknown reasons, in GraphOptimizer::optimize, the dynamic
+    cast of current->constraints() may fail on *some* 0-length paths. Ignoring
+    empty paths (the "if" I have added) seems to make the problem disappear
+    but the reason why some 0-length paths don't have the right Constraint type
+    is still to be found.

include/hpp/manipulation/fwd.hh

@@ -21,6 +21,7 @@
 # define HPP_MANIPULATION_FWD_HH
 
 # include <map>
+# include <hpp/manipulation/config.hh>
 # include <hpp/core/fwd.hh>
 
 namespace hpp {
@@ -87,6 +88,16 @@ namespace hpp {
     typedef core::vectorOut_t vectorOut_t;
     HPP_PREDEF_CLASS (ManipulationPlanner);
     typedef shared_ptr < ManipulationPlanner > ManipulationPlannerPtr_t;
+    namespace pathPlanner {
+      HPP_PREDEF_CLASS (RMRStar);
+      typedef shared_ptr < RMRStar > RMRStarPtr_t;
+      HPP_PREDEF_CLASS (StatesPathFinder);
+      typedef shared_ptr < StatesPathFinder > StatesPathFinderPtr_t;
+      HPP_PREDEF_CLASS (InStatePath);
+      typedef shared_ptr < InStatePath > InStatePathPtr_t;
+      HPP_PREDEF_CLASS (StateShooter);
+      typedef shared_ptr < StateShooter > StateShooterPtr_t;
+    } // namespace pathPlanner
     HPP_PREDEF_CLASS (GraphPathValidation);
     typedef shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
     HPP_PREDEF_CLASS (SteeringMethod);

include/hpp/manipulation/path-planner/in-state-path.hh

+// Copyright (c) 2021, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
+//          Florent Lamiraux (florent.lamiraux@laas.fr)
+//          Alexandre Thiault (athiault@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_MANIPULATION_PATH_PLANNER_IN_STATE_PATH_HH
+# define HPP_MANIPULATION_PATH_PLANNER_IN_STATE_PATH_HH
+
+# include <hpp/core/path-planner.hh>
+# include <hpp/core/config-projector.hh>
+# include <hpp/core/collision-validation.hh>
+# include <hpp/core/joint-bound-validation.hh>
+
+# include <hpp/manipulation/config.hh>
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/problem.hh>
+# include <hpp/manipulation/steering-method/graph.hh>
+
+namespace hpp {
+  namespace manipulation {
+    namespace pathPlanner {
+      /// \addtogroup path_planner
+      /// \{
+
+      /// 
+      /// Path planner designed to build a path between two configurations
+      /// on the same leaf of a given state graph edge
+      class HPP_MANIPULATION_DLLAPI InStatePath : public core::PathPlanner
+      {
+        public:
+          virtual ~InStatePath()
+          {}
+
+          static InStatePathPtr_t create (
+            const core::ProblemConstPtr_t& problem);
+            
+          static InStatePathPtr_t createWithRoadmap (
+            const core::ProblemConstPtr_t& problem,
+            const core::RoadmapPtr_t& roadmap);
+
+          InStatePathPtr_t copy () const;
+
+          void maxIterPlanner(const unsigned long& maxiter);
+          void timeOutPlanner(const double& timeout);
+          void resetRoadmap(const bool& resetroadmap);
+          void plannerType(const std::string& plannertype);
+          void addOptimizerType(const std::string& opttype);
+          void resetOptimizerTypes();
+
+          /// Set the edge along which q_init and q_goal will be linked.
+          /// Use setEdge before setInit and setGoal.
+          void setEdge (const graph::EdgePtr_t& edge);
+          void setInit (const ConfigurationPtr_t& q);
+          void setGoal (const ConfigurationPtr_t& q);
+
+          virtual void oneStep()
+          {}
+          virtual core::PathVectorPtr_t solve();
+
+          const core::RoadmapPtr_t& leafRoadmap() const;
+          void mergeLeafRoadmapWith(const core::RoadmapPtr_t& roadmap) const;
+
+        protected:
+          InStatePath (const core::ProblemConstPtr_t& problem,
+                const core::RoadmapPtr_t& roadmap) :
+            PathPlanner(problem),
+            problem_ (HPP_STATIC_PTR_CAST(const manipulation::Problem, problem)),
+            leafRoadmap_(roadmap),
+            constraints_(), weak_()
+          {
+            const core::DevicePtr_t& robot = problem_->robot();
+            leafProblem_ = core::Problem::create(robot);
+            leafProblem_->setParameter
+              ("kPRM*/numberOfNodes", core::Parameter((size_type) 2000));
+            leafProblem_->clearConfigValidations();
+            leafProblem_->addConfigValidation(core::CollisionValidation::create(robot));
+            leafProblem_->addConfigValidation(core::JointBoundValidation::create(robot));
+            for (const core::CollisionObjectPtr_t & obs: problem_->collisionObstacles()) {
+              leafProblem_->addObstacle(obs);
+            }
+            leafProblem_->pathProjector(problem->pathProjector());
+          }
+
+          InStatePath (const InStatePath& other) :
+            PathPlanner(other.problem_),
+            problem_(other.problem_), leafProblem_(other.leafProblem_),
+            leafRoadmap_ (other.leafRoadmap_),
+            constraints_(other.constraints_),
+            weak_ ()
+          {}
+
+          void init (InStatePathWkPtr_t weak)
+          {
+            weak_ = weak;
+          }
+
+        private:
+
+          // a pointer to the problem used to create the InStatePath instance
+          ProblemConstPtr_t problem_;
+          // a new problem created for this InStatePath instance
+          core::ProblemPtr_t leafProblem_;
+          core::RoadmapPtr_t leafRoadmap_;
+          ConstraintSetPtr_t constraints_;
+
+          double timeOutPathPlanning_ = 0;
+          unsigned long maxIterPathPlanning_ = 0;
+          bool resetRoadmap_ = true;
+          std::string plannerType_ = "BiRRT*";
+          std::vector<std::string> optimizerTypes_;
+
+          /// Weak pointer to itself
+          InStatePathWkPtr_t weak_;
+
+      }; // class InStatePath
+      /// \}
+
+    } // namespace pathPlanner
+  } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH

include/hpp/manipulation/path-planner/states-path-finder.hh

+// Copyright (c) 2017, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
+//          Florent Lamiraux (florent.lamiraux@laas.fr)
+//          Alexandre Thiault (athiault@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
+# define HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
+
+# include <hpp/core/fwd.hh>
+# include <hpp/core/path.hh>
+
+# include <hpp/core/projection-error.hh>
+# include <hpp/core/config-projector.hh>
+
+# include <hpp/core/validation-report.hh>
+# include <hpp/core/config-validations.hh>
+
+# include <hpp/core/path-planner.hh>
+
+# include <hpp/manipulation/config.hh>
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/problem.hh>
+
+namespace hpp {
+  namespace manipulation {
+    namespace pathPlanner {
+
+      /// \addtogroup path_planner
+      /// \{
+
+      /// Optimization-based path planning method.
+      ///
+      /// #### Sketch of the method
+      ///
+      /// Given two configuration \f$ (q_1,q_2) \f$, this class formulates and
+      /// solves the problem as follows.
+      /// - Compute the corresponding states \f$ (s_1, s_2) \f$.
+      /// - For a each path \f$ (e_0, ... e_n) \f$ of length not more than
+      ///   parameter "StatesPathFinder/maxDepth" between
+      ///   \f$ (s_1, s_2)\f$ in the constraint graph, do:
+      ///   - define \f$ n-1 \f$ intermediate configuration \f$ p_i \f$,
+      ///   - initialize the optimization problem, as explained below,
+      ///   - solve the optimization problem, which gives \f$ p^*_i \f$,
+      ///   - in case of failure, continue the loop.
+      ///
+      /// #### Problem formulation
+      /// Find \f$ (p_i) \f$ such that:
+      /// - \f$ p_0 = q_1 \f$,
+      /// - \f$ p_{n+1} = q_2 \f$,
+      /// - \f$ p_i \f$ is in state between \f$ (e_{i-1}, e_i) \f$, (\ref StateFunction)
+      /// - \f$ (p_i, p_{i+1}) \f$ are reachable with transition \f$ e_i \f$ (\ref EdgeFunction).
+      ///
+      /// #### Problem resolution
+      ///
+      /// One solver (hpp::constraints::solver::BySubstitution) is created
+      /// for each waypoint \f$p_i\f$.
+      /// - method buildOptimizationProblem builds a matrix the rows of which
+      ///   are the parameterizable numerical constraints present in the
+      ///   graph, and the columns of which are the waypoints. Each value in the
+      ///   matrix defines the status of each constraint right hand side for
+      ///   this waypoint, among {absent from the solver,
+      ///                         equal to value for previous waypoint,
+      ///                         equal to value for start configuration,
+      ///                         equal to value for end configuration}.
+      /// - method analyseOptimizationProblem loops over the waypoint solvers,
+      ///   tests what happens when solving each waypoint after initializing
+      ///   only the right hand sides that are equal to the initial or goal
+      ///   configuraion, and detects if a collision is certain to block any attemps
+      ///   to solve the problem in the solveOptimizationProblem step.
+      /// - method solveOptimizationProblem tries to solve for each waypoint after
+      ///   initializing the right hand sides with the proper values, backtracking
+      ///   to the previous waypoint if the solving failed or a collision is
+      ///   detected a number of times set from the parameter
+      ///   "StatesPathFinder/nTriesUntilBacktrack". If too much backtracking
+      ///   occurs, the method can eventually return false.
+      /// - eventually method buildPath build paths between waypoints with
+      ///   the constraints of the transition in which the path lies.
+      ///
+      /// #### Current status
+      ///
+      /// The method has been successfully tested with romeo holding a placard
+      /// and the construction set benchmarks. The result is satisfactory
+      /// except between pregrasp and grasp waypoints that may be far
+      /// away from each other if the transition between those state does
+      /// not contain the grasp complement constraint. The same holds
+      /// between placement and pre-placement.
+      class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner
+      {
+
+        public:
+          struct OptimizationData;
+
+        virtual ~StatesPathFinder () {};
+
+          static StatesPathFinderPtr_t create (
+            const core::ProblemConstPtr_t& problem);
+            
+          static StatesPathFinderPtr_t createWithRoadmap (
+            const core::ProblemConstPtr_t& problem,
+            const core::RoadmapPtr_t& roadmap);
+
+          StatesPathFinderPtr_t copy () const;
+          
+          core::ProblemConstPtr_t problem() const
+          {
+            return problem_;
+          }
+
+          /// create a vector of configurations between two configurations
+          /// \return a Configurations_t from q1 to q2 if found. An empty
+          /// vector if a path could not be built.
+          core::Configurations_t computeConfigList (ConfigurationIn_t q1,
+                                          ConfigurationIn_t q2);
+
+          // access functions for Python interface
+          std::vector<std::string> constraintNamesFromSolverAtWaypoint
+            (std::size_t wp);
+          std::vector<std::string> lastBuiltTransitions() const;
+          std::string displayConfigsSolved () const;
+          bool buildOptimizationProblemFromNames(std::vector<std::string> names);
+
+          // Substeps of method solveOptimizationProblem
+          void initWPRandom(std::size_t wp);
+          void initWPNear(std::size_t wp);
+          void initWP(std::size_t wp, ConfigurationIn_t q);
+          int solveStep(std::size_t wp);
+          Configuration_t configSolved (std::size_t wp) const;
+
+          /// Step 7 of the algorithm
+          core::PathVectorPtr_t pathFromConfigList (std::size_t i) const;
+
+          /// 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.
+          void reset();
+          core::PathVectorPtr_t buildPath (ConfigurationIn_t q1, ConfigurationIn_t q2);
+
+          virtual void startSolve();
+          virtual void oneStep();
+          virtual core::PathVectorPtr_t solve ();
+
+        protected:
+          StatesPathFinder (const core::ProblemConstPtr_t& problem,
+                const core::RoadmapPtr_t&) :
+            PathPlanner(problem),
+            problem_ (HPP_STATIC_PTR_CAST(const manipulation::Problem, problem)),
+            sameRightHandSide_ (), weak_ ()
+          {
+            gatherGraphConstraints ();
+          }
+
+          StatesPathFinder (const StatesPathFinder& other) :
+            PathPlanner(other.problem_),
+            problem_ (other.problem_), constraints_ (), index_ (other.index_),
+            sameRightHandSide_ (other.sameRightHandSide_),  weak_ ()
+          {}
+
+          void init (StatesPathFinderWkPtr_t weak)
+          {
+            weak_ = weak;
+          }
+
+        private:
+          typedef constraints::solver::BySubstitution Solver_t;
+          struct GraphSearchData;
+
+          /// Gather constraints of all edges
+          void gatherGraphConstraints ();
+
+          /// Step 1 of the algorithm
+          /// \return whether the max depth was reached.
+          bool findTransitions (GraphSearchData& data) const;
+
+          /// Step 2 of the algorithm
+          graph::Edges_t getTransitionList (const GraphSearchData& data, const std::size_t& i) const;
+
+          /// Step 3 of the algorithm
+          bool contains (const Solver_t& solver, const ImplicitPtr_t& c) const;
+          bool checkConstantRightHandSide (size_type index);
+          bool buildOptimizationProblem (const graph::Edges_t& transitions);
+
+          /// Step 4 of the algorithm
+          void preInitializeRHS(std::size_t j, Configuration_t& q);
+          bool analyseOptimizationProblem (const graph::Edges_t& transitions);
+
+          /// Step 5 of the algorithm
+          void initializeRHS (std::size_t j);
+          bool solveOptimizationProblem ();
+
+          /// Step 6 of the algorithm
+          core::Configurations_t buildConfigList () const;
+
+          /// Functions used in assert statements
+          bool checkWaypointRightHandSide (std::size_t ictr, std::size_t jslv) const;
+          bool checkSolverRightHandSide (std::size_t ictr, std::size_t jslv) const;
+          bool checkWaypointRightHandSide (std::size_t jslv) const;
+          bool checkSolverRightHandSide (std::size_t jslv) const;
+
+          void displayRhsMatrix ();
+          void displayStatusMatrix (const graph::Edges_t& transitions);
+
+          /// A pointer to the manipulation problem
+          ProblemConstPtr_t problem_;
+
+          /// Vector of parameterizable edge numerical constraints
+          NumericalConstraints_t constraints_;
+          /// Map of indexes in constraints_
+          std::map < std::string, std::size_t > index_;
+
+          /// associative map that stores pairs of constraints of the form
+          /// (constraint, constraint/hold)
+          std::map <ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
+
+          mutable OptimizationData* optData_ = nullptr;
+          std::size_t idxSol_ = 0;
+          graph::Edges_t lastBuiltTransitions_;
+
+          bool skipColAnalysis_ = false;
+
+          // Variables used across several calls to oneStep
+          ConfigurationPtr_t q1_, q2_;
+          core::Configurations_t configList_;
+          std::size_t idxConfigList_ = 0;
+          size_type nTryConfigList_ = 0;
+          InStatePathPtr_t planner_;
+          core::PathVectorPtr_t solution_;
+          bool solved_ = false, interrupt_ = false;
+
+          /// Weak pointer to itself
+          StatesPathFinderWkPtr_t weak_;
+
+      }; // class StatesPathFinder
+      /// \}
+      
+    } // namespace pathPlanner
+  } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH

include/hpp/manipulation/steering-method/cross-state-optimization.hh

@@ -136,7 +136,11 @@ namespace hpp {
           bool findTransitions (GraphSearchData& data) const;
 
           /// Step 2 of the algorithm
-          graph::Edges_t getTransitionList (GraphSearchData& data, const std::size_t& i) const;
+          graph::Edges_t getTransitionList (const GraphSearchData& data, const std::size_t& i) const;
+
+          // These two lines should be removed once we get rid of level set edges
+          #define LSE_GET_TRANSITION_LISTS
+          std::vector<graph::Edges_t> getTransitionLists (const GraphSearchData& data, const std::size_t& i) const;
 
           /// Step 3 of the algorithm
           bool buildOptimizationProblem

src/graph-optimizer.cc

@@ -43,6 +43,10 @@ namespace hpp {
         PathVectorPtr_t toOpt = PathVector::create (
             path->outputSize(), path->outputDerivativeSize()); 
         PathPtr_t current = expanded->pathAtRank (i_s);
+        if (current->length() == 0) {
+          i_s++;
+          continue;
+        }
         toOpt->appendPath (current);
         graph::EdgePtr_t edge;
         c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());

src/graph-path-validation.cc

@@ -26,9 +26,6 @@
 #include <hpp/manipulation/graph/graph.hh>
 #include <hpp/manipulation/constraint-set.hh>
 
-#ifdef HPP_DEBUG
-# include <hpp/manipulation/graph/state.hh>
-#endif
 
 namespace hpp {
   namespace manipulation {

src/graph/state.cc

@@ -52,6 +52,7 @@ namespace hpp {
 			     const size_type& w, EdgeFactory create)
       {
         EdgePtr_t newEdge = create(name, graph_, wkPtr_, to);
+        assert (newEdge);
         if (w >= 0) neighbors_.insert (newEdge, (Weight_t)w);
         else hiddenNeighbors_.push_back (newEdge);
         return newEdge;
@@ -125,6 +126,7 @@ namespace hpp {
             it != neighbors_.end(); ++it) {
           if (it->second == e) {
             /// Update the weights
+            assert (e);
             neighbors_.insert (e, w);
           }
         }

src/path-planner/in-state-path.cc

+// Copyright (c) 2021, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
+//          Florent Lamiraux (florent.lamiraux@laas.fr)
+//          Alexandre Thiault (athiault@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#include <hpp/manipulation/path-planner/in-state-path.hh>
+
+#include <hpp/util/exception-factory.hh>
+
+#include <hpp/pinocchio/configuration.hh>
+
+#include <hpp/core/path-vector.hh>
+#include <hpp/core/roadmap.hh>
+#include <hpp/core/edge.hh>
+
+#include <hpp/core/bi-rrt-planner.hh>
+#include <hpp/core/path-planner/k-prm-star.hh>
+#include <hpp/core/diffusing-planner.hh>
+
+#include <hpp/core/path-optimizer.hh>
+#include <hpp/core/path-optimization/random-shortcut.hh>
+#include <hpp/core/path-optimization/simple-shortcut.hh>
+#include <hpp/core/path-optimization/partial-shortcut.hh>
+#include <hpp/core/path-optimization/simple-time-parameterization.hh>
+#include <hpp/manipulation/path-optimization/random-shortcut.hh>
+
+#include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/roadmap.hh>
+
+
+namespace hpp {
+  namespace manipulation {
+    namespace pathPlanner {
+
+      InStatePathPtr_t InStatePath::create (
+          const core::ProblemConstPtr_t& problem)
+      {
+        InStatePath* ptr;
+        RoadmapPtr_t r = Roadmap::create(problem->distance(), problem->robot());
+        try {
+          ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+          ptr = new InStatePath (p, r);
+        } catch (std::exception&) {
+          throw std::invalid_argument
+            ("The problem must be of type hpp::manipulation::Problem.");
+        }
+        InStatePathPtr_t shPtr (ptr);
+        ptr->init (shPtr);
+        return shPtr;
+      }
+
+      InStatePathPtr_t InStatePath::createWithRoadmap (
+          const core::ProblemConstPtr_t& problem,
+          const core::RoadmapPtr_t& roadmap)
+      {
+        InStatePath* ptr;
+        core::RoadmapPtr_t r2 = roadmap;
+        RoadmapPtr_t r;
+        try {
+          ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+          r = HPP_DYNAMIC_PTR_CAST (Roadmap, r2);
+          ptr = new InStatePath (p, r);
+        } catch (std::exception&) {
+          if (!r)
+            throw std::invalid_argument
+              ("The roadmap must be of type hpp::manipulation::Roadmap.");
+          else
+            throw std::invalid_argument
+              ("The problem must be of type hpp::manipulation::Problem.");
+        }
+        InStatePathPtr_t shPtr (ptr);
+        ptr->init (shPtr);
+        return shPtr;
+      }
+
+      InStatePathPtr_t InStatePath::copy () const
+      {
+        InStatePath *ptr = new InStatePath(*this);
+        InStatePathPtr_t shPtr(ptr);
+        ptr->init(shPtr);
+        return shPtr;
+      }
+
+      void InStatePath::maxIterPlanner(const unsigned long& maxiter)
+      {
+        maxIterPathPlanning_ = maxiter;
+      }
+
+      void InStatePath::timeOutPlanner(const double& timeout)
+      {
+        timeOutPathPlanning_ = timeout;
+      }
+
+      void InStatePath::resetRoadmap(const bool& resetroadmap)
+      {
+        resetRoadmap_ = resetroadmap;
+      }
+
+      void InStatePath::plannerType(const std::string& plannertype)
+      {
+        plannerType_ = plannertype;
+      }
+      
+      void InStatePath::addOptimizerType(const std::string& opttype)
+      {
+        optimizerTypes_.push_back(opttype);
+      }
+      
+      void InStatePath::resetOptimizerTypes()
+      {
+        optimizerTypes_.clear();
+      }
+
+      void InStatePath::setEdge (const graph::EdgePtr_t& edge)
+      {
+        constraints_ = edge->pathConstraint();
+        leafProblem_->pathValidation(edge->pathValidation());
+        leafProblem_->constraints(constraints_);
+        leafProblem_->steeringMethod(edge->steeringMethod());
+      }
+
+      void InStatePath::setInit (const ConfigurationPtr_t& qinit)
+      {
+        if (!constraints_)
+          throw std::logic_error("Use setEdge before setInit and setGoal");
+        constraints_->configProjector()->rightHandSideFromConfig(*qinit);
+        leafProblem_->initConfig(qinit);
+        leafProblem_->resetGoalConfigs();
+      }
+
+      void InStatePath::setGoal (const ConfigurationPtr_t& qgoal)
+      {
+        if (!constraints_)
+          throw std::logic_error("Use setEdge before setInit and setGoal");
+        ConfigurationPtr_t qgoalc (new Configuration_t (*qgoal));
+        constraints_->apply(*qgoalc);
+        assert((*qgoal-*qgoalc).isZero());
+        leafProblem_->resetGoalConfigs();
+        leafProblem_->addGoalConfig(qgoal);
+      }
+
+      core::PathVectorPtr_t InStatePath::solve()
+      {
+        if (!constraints_)
+          throw std::logic_error("Use setEdge, setInit and setGoal before solve");
+        if (resetRoadmap_ || !leafRoadmap_)
+          leafRoadmap_ = core::Roadmap::create (leafProblem_->distance(), problem_->robot());
+        
+        core::PathPlannerPtr_t planner;
+        // TODO: BiRRT* does not work properly:
+        //     - discontinuities due to an algorithmic mistake involving qProj_
+        //     - not using path projectors, it should
+        if (plannerType_ == "kPRM*")
+          planner = core::pathPlanner::kPrmStar::createWithRoadmap(leafProblem_, leafRoadmap_);
+        else if (plannerType_ == "DiffusingPlanner")
+          planner = core::DiffusingPlanner::createWithRoadmap(leafProblem_, leafRoadmap_);
+        else if (plannerType_ == "BiRRT*")
+          planner = core::BiRRTPlanner::createWithRoadmap(leafProblem_, leafRoadmap_);
+        else {
+          hppDout(warning, "Unknown planner type specified. Setting to default DiffusingPlanner");
+          planner = core::DiffusingPlanner::createWithRoadmap(leafProblem_, leafRoadmap_);
+        }
+        if (maxIterPathPlanning_)
+            planner->maxIterations(maxIterPathPlanning_);
+        if (timeOutPathPlanning_)
+          planner->timeOut(timeOutPathPlanning_);
+        if (!maxIterPathPlanning_ && !timeOutPathPlanning_)
+            planner->stopWhenProblemIsSolved(true);
+        core::PathVectorPtr_t path = planner->solve();
+        
+        for (const std::string& optType: optimizerTypes_) {
+          namespace manipOpt = pathOptimization;
+          namespace coreOpt = core::pathOptimization;
+          PathOptimizerPtr_t optimizer;
+          if (optType == "RandomShortcut")
+            optimizer = coreOpt::RandomShortcut::create(problem_);
+          else if (optType == "SimpleShortcut")
+            optimizer = coreOpt::SimpleShortcut::create(problem_);
+          else if (optType == "PartialShortcut")
+            optimizer = coreOpt::PartialShortcut::create(problem_);
+          else if (optType == "SimpleTimeParameterization")
+            optimizer = coreOpt::SimpleTimeParameterization::create(problem_);
+          else if (optType == "Graph-RandomShortcut")
+            optimizer = manipOpt::RandomShortcut::create(problem_);
+          else
+            continue;
+          try {
+            path = optimizer->optimize(path);
+          } catch (const hpp::Exception& e) {
+            hppDout(info, "could not optimize " << e.what());
+          }
+        }
+        return path;
+      }
+      
+      const core::RoadmapPtr_t& InStatePath::leafRoadmap() const
+      {
+        return leafRoadmap_;
+      }
+
+      void InStatePath::mergeLeafRoadmapWith(const core::RoadmapPtr_t& roadmap) const {
+        std::map<core::NodePtr_t, core::NodePtr_t> cNode;
+        for (const core::NodePtr_t& node: leafRoadmap_->nodes()) {
+          cNode[node] = roadmap->addNode(node->configuration());
+        }
+        for (const core::EdgePtr_t& edge: leafRoadmap_->edges()) {
+          if (edge->path()->length() == 0)
+            assert (edge->from() == edge->to());
+          else
+            roadmap->addEdges(
+                cNode[edge->from()], cNode[edge->to()], edge->path());
+        }
+        // TODO this is inefficient because the roadmap recomputes the connected
+        // component at every step. A merge function should be added in roadmap.cc
+      }
+      
+    } // namespace pathPlanner
+  } // namespace manipulation
+} // namespace hpp
\ No newline at end of file

src/problem-solver.cc

@@ -57,6 +57,8 @@
 #include "hpp/manipulation/path-optimization/random-shortcut.hh"
 #include "hpp/manipulation/path-optimization/enforce-transition-semantic.hh"
 #include "hpp/manipulation/path-planner/end-effector-trajectory.hh"
+#include "hpp/manipulation/path-planner/states-path-finder.hh"
+#include "hpp/manipulation/path-planner/in-state-path.hh"
 #include "hpp/manipulation/problem-target/state.hh"
 #include "hpp/manipulation/steering-method/cross-state-optimization.hh"
 #include "hpp/manipulation/steering-method/graph.hh"
@@ -119,6 +121,8 @@ namespace hpp {
 
       pathPlanners.add ("M-RRT", ManipulationPlanner::create);
       pathPlanners.add ("EndEffectorTrajectory", pathPlanner::EndEffectorTrajectory::createWithRoadmap);
+      pathPlanners.add ("StatesPathFinder", pathPlanner::StatesPathFinder::createWithRoadmap);
+      pathPlanners.add ("InStatePath", pathPlanner::InStatePath::createWithRoadmap);
 
       pathValidations.add ("Graph-Discretized"                      , createDiscretizedCollisionGraphPathValidation);
       pathValidations.add ("Graph-DiscretizedCollision"             , createDiscretizedCollisionGraphPathValidation);
@@ -162,6 +166,7 @@ namespace hpp {
           createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Dubins>);
       steeringMethods.add ("Graph-Snibud",
           createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Snibud>);
+
       steeringMethods.add ("CrossStateOptimization-Straight",
           steeringMethod::CrossStateOptimization::create<core::steeringMethod::Straight>);
       steeringMethods.add ("CrossStateOptimization-ReedsShepp",

src/steering-method/cross-state-optimization.cc

@@ -193,6 +193,104 @@ namespace hpp {
         }
       }
 
+      static bool containsLevelSet(const graph::EdgePtr_t& e) {
+        // First case, in case given edge e is already a sub edge inside a WaypointEdge
+        graph::LevelSetEdgePtr_t lse =
+          HPP_DYNAMIC_PTR_CAST(graph::LevelSetEdge, e);
+        if (lse)
+          return true;
+        // Second case, given edge e links two non-waypoint states
+        graph::WaypointEdgePtr_t we =
+          HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, e);
+        if (!we)
+          return false;
+        for (std::size_t i = 0; i <= we->nbWaypoints(); i++) {
+          graph::LevelSetEdgePtr_t lse =
+            HPP_DYNAMIC_PTR_CAST(graph::LevelSetEdge, we->waypoint(i));
+          if (lse)
+            return true;
+        }
+        return false;
+      }
+
+      static bool containsLevelSet(const graph::Edges_t& transitions) {
+        for (std::size_t i = 0; i < transitions.size(); i++)
+          if (HPP_DYNAMIC_PTR_CAST(graph::LevelSetEdge, transitions[i]))
+            return true;
+        return false;
+      }
+
+#ifdef LSE_GET_TRANSITION_LISTS
+      /// Given an edge path "transitions", return a vector of 2^i edge paths
+      /// where i is the number of edges in transitions which have a LSE alter ego
+      static std::vector<graph::Edges_t> transitionsWithLSE(
+          const graph::GraphPtr_t& graph, const graph::Edges_t& transitions) {
+        std::vector<graph::Edges_t> altEdges(transitions.size());
+        std::vector<std::size_t> indexWithAlt;
+
+        for (std::size_t i = 0; i < transitions.size(); i++)
+        {
+          graph::StatePtr_t from = transitions[i]->stateFrom ();
+          graph::StatePtr_t to = transitions[i]->stateTo ();
+          altEdges[i] = graph->getEdges (from, to);
+          if (altEdges[i].size() == 2)
+            indexWithAlt.push_back (i);
+        }
+        std::size_t nbEdgesWithAlt = indexWithAlt.size();
+        std::size_t nbAlternatives = ((std::size_t) 1) << nbEdgesWithAlt;
+
+        std::vector<graph::Edges_t> alternativePaths(nbAlternatives);
+        if (nbAlternatives == 1) {
+          alternativePaths[0] = transitions;
+          return alternativePaths;
+        }
+
+        for (std::size_t i = 0; i < nbAlternatives; i++)
+        {
+          std::size_t alti = 0;
+          EdgePtr_t edge;
+          graph::WaypointEdgePtr_t we;
+          for (std::size_t j = 0; j < transitions.size(); j++)
+          {
+            if (j == indexWithAlt[alti])
+            {
+              edge = altEdges[j][(i >> alti) & 1];
+              if (alti+1 != nbEdgesWithAlt)
+                alti++;
+            } else {
+              edge = transitions[j];
+            }
+            we = HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, edge);
+            if (we) {
+              for (std::size_t k = 0; k <= we->nbWaypoints(); k++)
+                alternativePaths[i].push_back(we->waypoint(k));
+            } else {
+              alternativePaths[i].push_back(edge);
+            }
+          }
+        }
+        return alternativePaths;
+      }
+
+      std::vector<Edges_t> CrossStateOptimization::getTransitionLists (
+          const GraphSearchData& d, const std::size_t& i) const
+      {
+        assert (d.parent1.find (d.s2) != d.parent1.end());
+        const GraphSearchData::state_with_depths_t& roots = d.parent1.at(d.s2);
+        if (i >= roots.size()) return std::vector<Edges_t>();
+        const GraphSearchData::state_with_depth* current = &roots[i];
+
+        Edges_t transitions;
+        transitions.reserve (current->l);
+        while (current->e) {
+          assert (current->l > 0);
+          transitions.push_back(current->e);
+          current = &d.parent1.at(current->s)[current->i];
+        }
+        std::reverse (transitions.begin(), transitions.end());
+        return transitionsWithLSE(problem_->constraintGraph(), transitions);
+      }
+#endif
       bool CrossStateOptimization::findTransitions (GraphSearchData& d) const
       {
         while (! d.queue1.empty())
@@ -210,6 +308,10 @@ namespace hpp {
               _n != neighbors.end(); ++_n) {
             EdgePtr_t transition = _n->second;
 
+#ifdef LSE_GET_TRANSITION_LISTS
+            // Don't even consider level set edges
+            if (containsLevelSet(transition)) continue;
+#endif
             // Avoid identical consecutive transition
             if (transition == parent.e) continue;
 
@@ -235,10 +337,10 @@ namespace hpp {
       }
 
       Edges_t CrossStateOptimization::getTransitionList (
-          GraphSearchData& d, const std::size_t& i) const
+          const GraphSearchData& d, const std::size_t& i) const
       {
         assert (d.parent1.find (d.s2) != d.parent1.end());
-        const GraphSearchData::state_with_depths_t& roots = d.parent1[d.s2];
+        const GraphSearchData::state_with_depths_t& roots = d.parent1.at(d.s2);
         Edges_t transitions;
         if (i >= roots.size()) return transitions;
 
@@ -254,7 +356,7 @@ namespace hpp {
           } else {
             transitions.push_back(current->e);
           }
-          current = &d.parent1[current->s][current->i];
+          current = &d.parent1.at(current->s)[current->i];
         }
         std::reverse (transitions.begin(), transitions.end());
         return transitions;
@@ -387,26 +489,36 @@ namespace hpp {
         oss << "\\paragraph{Edges}" << std::endl;
         oss << "\\begin{enumerate}" << std::endl;
         for (auto edge : transitions) {
-          oss << "\\item " << edge->name() << std::endl;
+          oss << "\\item \\texttt{" << edge->name() << "}" << std::endl;
         }
         oss << "\\end{enumerate}" << std::endl;
-        oss << "\\begin {tabular}{";
-        for (size_type j=0; j<m.cols () + 1; ++j)
+        oss << "\\begin {tabular}{l";
+        for (size_type j=0; j<m.cols (); ++j)
           oss << "c";
         oss << "}" << std::endl;
+        oss << "Solver index";
+        for (size_type j=0; j<m.cols (); ++j)
+          oss << " & " << j+1;
+        oss << "\\\\" << std::endl;
         for (size_type i=0; i<m.rows (); ++i) {
-          oss << constraints [i]->function ().name () << " & ";
+          oss << "\\texttt{" << constraints [i]->function ().name () << "} & " << std::endl;
           for (size_type j=0; j<m.cols (); ++j) {
             oss << m (i,j);
-            if (j < m.cols () - 1) {
-              oss << " & " << std::endl;
-            }
+            if (j < m.cols () - 1)
+              oss << " & ";
           }
           oss << "\\\\" << std::endl;
         }
         oss << "\\end{tabular}" << std::endl;
         oss << "\\end {document}" << std::endl;
-        hppDout (info, oss.str ());
+
+        std::string s = oss.str ();
+        std::string s2 = "";
+        for (int i=0; i < s.size(); i++) {
+          if (s[i] == '_') s2 += "\\_";
+          else s2.push_back(s[i]);
+        }
+        hppDout (info, s2);
       }
 
       bool CrossStateOptimization::contains
@@ -425,7 +537,7 @@ namespace hpp {
       bool CrossStateOptimization::buildOptimizationProblem
       (OptimizationData& d, const graph::Edges_t& transitions) const
       {
-        if (d.N == 0) return true;
+        if (d.N == 0) return true; // TODO: false when there is only a "loop | f"
         d.M_status.resize (constraints_.size (), d.N);
         d.M_status.fill (OptimizationData::ABSENT);
         d.M_rhs.resize (constraints_.size (), d.N);
@@ -551,25 +663,26 @@ namespace hpp {
 
           while(status != Solver_t::SUCCESS && nbTry < nRandomConfigs){
             d.waypoint.col (j) = *(problem()->configurationShooter()->shoot());
-            status = solver.solve
-            (d.waypoint.col (j),
-             constraints::solver::lineSearch::Backtracking ());
+            status = solver.solve (d.waypoint.col (j),
+              constraints::solver::lineSearch::Backtracking ());
             ++nbTry;
           }
           switch (status) {
           case Solver_t::ERROR_INCREASED:
-            hppDout (info, "error increased.");
+            hppDout (info, "  error increased at step " << j);
             return false;
           case Solver_t::MAX_ITERATION_REACHED:
-            hppDout (info, "max iteration reached.");
+            hppDout (info, "  max iteration reached at step " << j);
             return false;
           case Solver_t::INFEASIBLE:
-            hppDout (info, "infeasible.");
+            hppDout (info, "  infeasible at step " << j);
             return false;
           case Solver_t::SUCCESS:
-            hppDout (info, "success.");
+            hppDout(info,  "  config solved at transition " << j << ": " << pinocchio::displayConfig(d.waypoint.col(j)));
+            ;
           }
         }
+        hppDout (info, "  success");
         return true;
       }
 
@@ -602,9 +715,8 @@ namespace hpp {
           else {
             status = t->build (path, d.waypoint.col (i-1), d.waypoint.col (i));
           }
-
+          // This might fail when last argument constraint error is slightly above the threshold
           if (!status || !path) {
-            hppDout (warning, "Could not build path from solution ");
             return PathVectorPtr_t();
           }
           pv->appendPath(path);
@@ -621,41 +733,81 @@ namespace hpp {
         GraphSearchData d;
         d.s1 = graph->getState (q1);
         d.s2 = graph->getState (q2);
-        // d.maxDepth = 2;
+
         d.maxDepth = problem_->getParameter
-	  ("CrossStateOptimization/maxDepth").intValue();
+	        ("CrossStateOptimization/maxDepth").intValue();
+        int cnt = 0;
+        std::size_t nTriesForEachPath = problem_->getParameter
+	        ("CrossStateOptimization/nTriesForEachPath").intValue();
 
         // Find
         d.queue1.push (d.addInitState());
         std::size_t idxSol = (d.s1 == d.s2 ? 1 : 0);
-        bool maxDepthReached = findTransitions (d);
 
-        while (!maxDepthReached) {
-          Edges_t transitions = getTransitionList (d, idxSol);
+        bool maxDepthReached;
+        while (!(maxDepthReached = findTransitions (d))) { // mut
+#ifdef LSE_GET_TRANSITION_LISTS
+          std::vector<Edges_t> transitionss = getTransitionLists (d, idxSol); // const, const
+          cnt += transitionss.size();
+          while (! transitionss.empty()) {
+            for (std::size_t nTry = 0; nTry < nTriesForEachPath; nTry++) {
+              for (std::size_t idySol = 0; idySol < transitionss.size(); idySol++) {
+                const Edges_t& transitions = transitionss[idySol];
+#else
+          Edges_t transitions = getTransitionList (d, idxSol); // const, const
           while (! transitions.empty()) {
+            for (std::size_t nTry = 0; nTry < nTriesForEachPath; nTry++) {
+                std::size_t idySol = 0;
+#endif
 #ifdef HPP_DEBUG
-            std::ostringstream ss;
-            ss << "Trying solution " << idxSol << ": ";
-            for (std::size_t j = 0; j < transitions.size(); ++j)
-              ss << transitions[j]->name() << ", ";
-            hppDout (info, ss.str());
+                std::ostringstream ss;
+                ss << " Trying solution " << idxSol << "-" << idySol <<
+                      ", try " << nTry << ": \n\t";
+                for (std::size_t j = 0; j < transitions.size(); ++j)
+                  ss << transitions[j]->name() << ", \n\t";
+                hppDout (info, ss.str());
 #endif // HPP_DEBUG
-
-            OptimizationData optData (problem(), q1, q2, transitions);
-            if (buildOptimizationProblem (optData, transitions)) {
-              if (solveOptimizationProblem (optData)) {
-                core::PathPtr_t path = buildPath (optData, transitions);
-                if (path) return path;
-                hppDout (info, "Failed to build path from solution: ");
-              } else {
-                hppDout (info, "Failed to solve");
+                OptimizationData optData (problem(), q1, q2, transitions);
+                if (buildOptimizationProblem (optData, transitions)) {
+                  if (solveOptimizationProblem (optData)) {
+                    core::PathPtr_t path = buildPath (optData, transitions);
+                    if (path) {
+                      hppDout (info, " Success for solution " << idxSol <<
+                        "-" << idySol << ", return path, try" << nTry+1);
+                      return path; // comment to see other transitions which would have worked
+                      idySol = SIZE_MAX-1;
+                      nTry = SIZE_MAX-1;
+                      // we already know this path works so let's move on to the next
+                    } else {
+                      hppDout (info, " Failed solution " << idxSol <<
+                        "-" << idySol << " at step 5 (build path)");
+                    }
+                  } else {
+                    hppDout (info, " Failed solution " << idxSol <<
+                      "-" << idySol << " at step 4 (solve opt pb)");
+                  }
+                } else {
+                  hppDout (info, " Failed solution " << idxSol <<
+                    "-" << idySol << " at step 3 (build opt pb)");
+                  idySol = SIZE_MAX-1;
+                  nTry = SIZE_MAX-1;
+                  // no other LSE alter ego will go further than step 3
+                }
               }
+#ifdef LSE_GET_TRANSITION_LISTS
             }
+            ++idxSol;
+            transitionss = getTransitionLists(d, idxSol);
+#else
             ++idxSol;
             transitions = getTransitionList(d, idxSol);
+#endif
           }
-          maxDepthReached = findTransitions (d);
         }
+        hppDout (warning, " Max depth reached");
+#ifdef LSE_GET_TRANSITION_LISTS
+        hppDout (warning, cnt << " transitions in total");
+#endif
 
         return core::PathPtr_t ();
       }
@@ -680,6 +832,10 @@ namespace hpp {
             "CrossStateOptimization/nRandomConfigs",
             "Number of random configurations to sample to initialize each "
             "solver.", Parameter((size_type)0)));
+      core::Problem::declareParameter(ParameterDescription(Parameter::INT,
+            "CrossStateOptimization/nTriesForEachPath",
+            "Number of tries to be done for each state path "
+            "solver.", Parameter((size_type)1)));
       HPP_END_PARAMETER_DECLARATION(CrossStateOptimization)
     } // namespace steeringMethod
   } // namespace manipulation